mani-skill-nightly 2025.5.23.1843__py3-none-any.whl → 2025.5.26.2237__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- mani_skill/agents/controllers/pd_ee_pose.py +15 -6
- mani_skill/agents/controllers/utils/kinematics.py +84 -20
- mani_skill/agents/robots/fetch/fetch.py +5 -2
- mani_skill/assets/robots/fetch/fetch_torso_up.urdf +435 -0
- mani_skill/examples/demo_manual_control.py +4 -4
- mani_skill/examples/demo_manual_control_continuous.py +260 -0
- mani_skill/utils/visualization/renderer.py +12 -2
- {mani_skill_nightly-2025.5.23.1843.dist-info → mani_skill_nightly-2025.5.26.2237.dist-info}/METADATA +1 -1
- {mani_skill_nightly-2025.5.23.1843.dist-info → mani_skill_nightly-2025.5.26.2237.dist-info}/RECORD +12 -10
- {mani_skill_nightly-2025.5.23.1843.dist-info → mani_skill_nightly-2025.5.26.2237.dist-info}/LICENSE +0 -0
- {mani_skill_nightly-2025.5.23.1843.dist-info → mani_skill_nightly-2025.5.26.2237.dist-info}/WHEEL +0 -0
- {mani_skill_nightly-2025.5.23.1843.dist-info → mani_skill_nightly-2025.5.26.2237.dist-info}/top_level.txt +0 -0
|
@@ -1,12 +1,12 @@
|
|
|
1
1
|
from dataclasses import dataclass
|
|
2
|
-
from typing import Literal, Sequence, Union
|
|
2
|
+
from typing import Literal, Optional, Sequence, Union
|
|
3
3
|
|
|
4
4
|
import numpy as np
|
|
5
5
|
import torch
|
|
6
6
|
from gymnasium import spaces
|
|
7
7
|
|
|
8
8
|
from mani_skill.agents.controllers.utils.kinematics import Kinematics
|
|
9
|
-
from mani_skill.utils import gym_utils
|
|
9
|
+
from mani_skill.utils import gym_utils, sapien_utils
|
|
10
10
|
from mani_skill.utils.geometry.rotation_conversions import (
|
|
11
11
|
euler_angles_to_matrix,
|
|
12
12
|
matrix_to_quaternion,
|
|
@@ -45,6 +45,13 @@ class PDEEPosController(PDJointPosController):
|
|
|
45
45
|
|
|
46
46
|
self.ee_link = self.kinematics.end_link
|
|
47
47
|
|
|
48
|
+
if self.config.root_link_name is not None:
|
|
49
|
+
self.root_link = sapien_utils.get_obj_by_name(
|
|
50
|
+
self.articulation.get_links(), self.config.root_link_name
|
|
51
|
+
)
|
|
52
|
+
else:
|
|
53
|
+
self.root_link = self.articulation.root
|
|
54
|
+
|
|
48
55
|
def _initialize_action_space(self):
|
|
49
56
|
low = np.float32(np.broadcast_to(self.config.pos_lower, 3))
|
|
50
57
|
high = np.float32(np.broadcast_to(self.config.pos_upper, 3))
|
|
@@ -60,7 +67,7 @@ class PDEEPosController(PDJointPosController):
|
|
|
60
67
|
|
|
61
68
|
@property
|
|
62
69
|
def ee_pose_at_base(self):
|
|
63
|
-
to_base = self.
|
|
70
|
+
to_base = self.root_link.pose.inv()
|
|
64
71
|
return to_base * (self.ee_pose)
|
|
65
72
|
|
|
66
73
|
def reset(self):
|
|
@@ -69,9 +76,9 @@ class PDEEPosController(PDJointPosController):
|
|
|
69
76
|
self._target_pose = self.ee_pose_at_base
|
|
70
77
|
else:
|
|
71
78
|
# TODO (stao): this is a strange way to mask setting individual batched pose parts
|
|
72
|
-
self._target_pose.raw_pose[
|
|
73
|
-
self.scene._reset_mask
|
|
74
|
-
|
|
79
|
+
self._target_pose.raw_pose[self.scene._reset_mask] = (
|
|
80
|
+
self.ee_pose_at_base.raw_pose[self.scene._reset_mask]
|
|
81
|
+
)
|
|
75
82
|
|
|
76
83
|
def compute_target_pose(self, prev_ee_pose_at_base, action):
|
|
77
84
|
# Keep the current rotation and change the position
|
|
@@ -156,6 +163,8 @@ class PDEEPosControllerConfig(ControllerConfig):
|
|
|
156
163
|
] = "root_translation"
|
|
157
164
|
"""Choice of frame to use for translational and rotational control of the end-effector. To learn how these work explicitly
|
|
158
165
|
with videos of each one's behavior see https://maniskill.readthedocs.io/en/latest/user_guide/concepts/controllers.html#pd-ee-end-effector-pose"""
|
|
166
|
+
root_link_name: Optional[str] = None
|
|
167
|
+
"""Optionally set different root link for root translation control (e.g. if root is different than base)"""
|
|
159
168
|
use_delta: bool = True
|
|
160
169
|
"""Whether to use delta-action control. If true then actions indicate the delta/change in position via translation and orientation via
|
|
161
170
|
rotation. If false, then actions indicate in the base frame (typically wherever the root link of the robot is) what pose the end effector
|
|
@@ -1,6 +1,7 @@
|
|
|
1
1
|
"""
|
|
2
2
|
Code for kinematics utilities on CPU/GPU
|
|
3
3
|
"""
|
|
4
|
+
|
|
4
5
|
from contextlib import contextmanager, redirect_stderr, redirect_stdout
|
|
5
6
|
from os import devnull
|
|
6
7
|
from typing import List
|
|
@@ -12,6 +13,7 @@ except ImportError:
|
|
|
12
13
|
"pytorch_kinematics_ms not installed. Install with pip install pytorch_kinematics_ms"
|
|
13
14
|
)
|
|
14
15
|
import torch
|
|
16
|
+
from lxml import etree as ET
|
|
15
17
|
from sapien.wrapper.pinocchio_model import PinocchioModel
|
|
16
18
|
|
|
17
19
|
from mani_skill.utils import common
|
|
@@ -44,12 +46,18 @@ class Kinematics:
|
|
|
44
46
|
articulation (Articulation): the articulation object
|
|
45
47
|
active_joint_indices (torch.Tensor): indices of the active joints that can be controlled
|
|
46
48
|
"""
|
|
49
|
+
|
|
50
|
+
# NOTE (arth): urdf path with feasible kinematic chain. may not be same urdf used to
|
|
51
|
+
# build the sapien articulation (e.g. sapien articulation may have added joints for
|
|
52
|
+
# mobile base which should not be used in IK)
|
|
47
53
|
self.urdf_path = urdf_path
|
|
48
54
|
self.end_link = articulation.links_map[end_link_name]
|
|
49
|
-
|
|
50
|
-
self.active_joint_indices = active_joint_indices
|
|
55
|
+
|
|
51
56
|
self.articulation = articulation
|
|
52
57
|
self.device = articulation.device
|
|
58
|
+
|
|
59
|
+
self.active_joint_indices = active_joint_indices
|
|
60
|
+
|
|
53
61
|
# note that everything past the end-link is ignored. Any joint whose ancestor is self.end_link is ignored
|
|
54
62
|
cur_link = self.end_link
|
|
55
63
|
active_ancestor_joints: List[ArticulationJoint] = []
|
|
@@ -58,17 +66,25 @@ class Kinematics:
|
|
|
58
66
|
active_ancestor_joints.append(cur_link.joint)
|
|
59
67
|
cur_link = cur_link.joint.parent_link
|
|
60
68
|
active_ancestor_joints = active_ancestor_joints[::-1]
|
|
61
|
-
self.active_ancestor_joints = active_ancestor_joints
|
|
62
69
|
|
|
63
|
-
#
|
|
64
|
-
#
|
|
65
|
-
|
|
66
|
-
|
|
67
|
-
|
|
68
|
-
|
|
69
|
-
|
|
70
|
-
for
|
|
70
|
+
# NOTE (arth): some robots, like Fetch, have dummy joints that can mess with IK solver.
|
|
71
|
+
# we assume that the urdf_path provides a valid kinematic chain, and prune joints
|
|
72
|
+
# which are in the ManiSkill articulation but not in the kinematic chain
|
|
73
|
+
with open(self.urdf_path, "r") as f:
|
|
74
|
+
urdf_string = f.read()
|
|
75
|
+
xml = ET.fromstring(urdf_string.encode("utf-8"))
|
|
76
|
+
self._kinematic_chain_joint_names = set(
|
|
77
|
+
node.get("name") for node in xml if node.tag == "joint"
|
|
78
|
+
)
|
|
79
|
+
self._kinematic_chain_link_names = set(
|
|
80
|
+
node.get("name") for node in xml if node.tag == "link"
|
|
81
|
+
)
|
|
82
|
+
self.active_ancestor_joints = [
|
|
83
|
+
x
|
|
84
|
+
for x in active_ancestor_joints
|
|
85
|
+
if x.name in self._kinematic_chain_joint_names
|
|
71
86
|
]
|
|
87
|
+
|
|
72
88
|
if self.device.type == "cuda":
|
|
73
89
|
self.use_gpu_ik = True
|
|
74
90
|
self._setup_gpu()
|
|
@@ -79,14 +95,47 @@ class Kinematics:
|
|
|
79
95
|
def _setup_cpu(self):
|
|
80
96
|
"""setup the kinematics solvers on the CPU"""
|
|
81
97
|
self.use_gpu_ik = False
|
|
82
|
-
|
|
83
|
-
self.
|
|
84
|
-
|
|
98
|
+
|
|
99
|
+
with open(self.urdf_path, "r") as f:
|
|
100
|
+
xml = f.read()
|
|
101
|
+
|
|
102
|
+
joint_order = [
|
|
103
|
+
j.name
|
|
104
|
+
for j in self.articulation.active_joints
|
|
105
|
+
if j.name in self._kinematic_chain_joint_names
|
|
106
|
+
]
|
|
107
|
+
link_order = [
|
|
108
|
+
l.name
|
|
109
|
+
for l in self.articulation.links
|
|
110
|
+
if l.name in self._kinematic_chain_link_names
|
|
111
|
+
]
|
|
112
|
+
|
|
113
|
+
self.pmodel = PinocchioModel(xml, [0, 0, -9.81])
|
|
114
|
+
self.pmodel.set_joint_order(joint_order)
|
|
115
|
+
self.pmodel.set_link_order(link_order)
|
|
116
|
+
|
|
117
|
+
controlled_joint_names = [
|
|
118
|
+
self.articulation.active_joints[i].name for i in self.active_joint_indices
|
|
119
|
+
]
|
|
120
|
+
self.pmodel_controlled_joint_indices = torch.tensor(
|
|
121
|
+
[joint_order.index(cj) for cj in controlled_joint_names],
|
|
122
|
+
dtype=torch.int,
|
|
123
|
+
device=self.device,
|
|
85
124
|
)
|
|
86
|
-
|
|
87
|
-
|
|
88
|
-
|
|
89
|
-
|
|
125
|
+
|
|
126
|
+
articulation_active_joint_names_to_idx = dict(
|
|
127
|
+
(j.name, i) for i, j in enumerate(self.articulation.active_joints)
|
|
128
|
+
)
|
|
129
|
+
self.pmodel_active_joint_indices = torch.tensor(
|
|
130
|
+
[articulation_active_joint_names_to_idx[jn] for jn in joint_order],
|
|
131
|
+
dtype=torch.int,
|
|
132
|
+
device=self.device,
|
|
133
|
+
)
|
|
134
|
+
|
|
135
|
+
# NOTE (arth): pmodel will use urdf_path, set values based on this xml
|
|
136
|
+
self.end_link_idx = link_order.index(self.end_link.name)
|
|
137
|
+
self.qmask = torch.zeros(len(joint_order), dtype=bool, device=self.device)
|
|
138
|
+
self.qmask[self.pmodel_controlled_joint_indices] = 1
|
|
90
139
|
|
|
91
140
|
def _setup_gpu(self):
|
|
92
141
|
"""setup the kinematics solvers on the GPU"""
|
|
@@ -116,6 +165,16 @@ class Kinematics:
|
|
|
116
165
|
num_retries=1,
|
|
117
166
|
)
|
|
118
167
|
|
|
168
|
+
# initially self.active_joint_indices references active joints that are controlled.
|
|
169
|
+
# we also make the assumption that the active index is the same across all parallel managed joints
|
|
170
|
+
self.active_ancestor_joint_idxs = [
|
|
171
|
+
(x.active_index[0]).cpu().item() for x in self.active_ancestor_joints
|
|
172
|
+
]
|
|
173
|
+
self.controlled_joints_idx_in_qmask = [
|
|
174
|
+
self.active_ancestor_joint_idxs.index(idx)
|
|
175
|
+
for idx in self.active_joint_indices
|
|
176
|
+
]
|
|
177
|
+
|
|
119
178
|
self.qmask = torch.zeros(
|
|
120
179
|
len(self.active_ancestor_joints), dtype=bool, device=self.device
|
|
121
180
|
)
|
|
@@ -167,10 +226,15 @@ class Kinematics:
|
|
|
167
226
|
if pos_only:
|
|
168
227
|
jacobian = jacobian[:, 0:3]
|
|
169
228
|
|
|
229
|
+
# NOTE (arth): use only the parts of the jacobian that correspond to the active joints
|
|
230
|
+
jacobian = jacobian[:, :, self.qmask]
|
|
231
|
+
|
|
170
232
|
# NOTE (stao): this method of IK is from https://mathweb.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf by Samuel R. Buss
|
|
171
233
|
delta_joint_pos = torch.linalg.pinv(jacobian) @ action.unsqueeze(-1)
|
|
172
|
-
|
|
234
|
+
|
|
235
|
+
return q0[:, self.qmask] + delta_joint_pos.squeeze(-1)
|
|
173
236
|
else:
|
|
237
|
+
q0 = q0[:, self.pmodel_active_joint_indices]
|
|
174
238
|
result, success, error = self.pmodel.compute_inverse_kinematics(
|
|
175
239
|
self.end_link_idx,
|
|
176
240
|
target_pose.sp,
|
|
@@ -180,7 +244,7 @@ class Kinematics:
|
|
|
180
244
|
)
|
|
181
245
|
if success:
|
|
182
246
|
return common.to_tensor(
|
|
183
|
-
[result[self.
|
|
247
|
+
[result[self.pmodel_controlled_joint_indices]], device=self.device
|
|
184
248
|
)
|
|
185
249
|
else:
|
|
186
250
|
return None
|
|
@@ -27,6 +27,7 @@ FETCH_BASE_COLLISION_BIT = 31
|
|
|
27
27
|
class Fetch(BaseAgent):
|
|
28
28
|
uid = "fetch"
|
|
29
29
|
urdf_path = f"{PACKAGE_ASSET_DIR}/robots/fetch/fetch.urdf"
|
|
30
|
+
urdf_arm_ik_path = f"{PACKAGE_ASSET_DIR}/robots/fetch/fetch_torso_up.urdf"
|
|
30
31
|
urdf_config = dict(
|
|
31
32
|
_materials=dict(
|
|
32
33
|
gripper=dict(static_friction=2.0, dynamic_friction=2.0, restitution=0.0)
|
|
@@ -149,7 +150,8 @@ class Fetch(BaseAgent):
|
|
|
149
150
|
damping=self.arm_damping,
|
|
150
151
|
force_limit=self.arm_force_limit,
|
|
151
152
|
ee_link=self.ee_link_name,
|
|
152
|
-
urdf_path=self.
|
|
153
|
+
urdf_path=self.urdf_arm_ik_path,
|
|
154
|
+
root_link_name="torso_lift_link",
|
|
153
155
|
)
|
|
154
156
|
arm_pd_ee_delta_pose = PDEEPoseControllerConfig(
|
|
155
157
|
joint_names=self.arm_joint_names,
|
|
@@ -161,7 +163,8 @@ class Fetch(BaseAgent):
|
|
|
161
163
|
damping=self.arm_damping,
|
|
162
164
|
force_limit=self.arm_force_limit,
|
|
163
165
|
ee_link=self.ee_link_name,
|
|
164
|
-
urdf_path=self.
|
|
166
|
+
urdf_path=self.urdf_arm_ik_path,
|
|
167
|
+
root_link_name="torso_lift_link",
|
|
165
168
|
)
|
|
166
169
|
|
|
167
170
|
arm_pd_ee_target_delta_pos = deepcopy(arm_pd_ee_delta_pos)
|
|
@@ -0,0 +1,435 @@
|
|
|
1
|
+
<robot name="fetch">
|
|
2
|
+
<link name="torso_lift_link">
|
|
3
|
+
<inertial>
|
|
4
|
+
<origin rpy="0 0 0" xyz="-0.0013 -0.0009 0.2935"/>
|
|
5
|
+
<mass value="10.7796"/>
|
|
6
|
+
<inertia ixx="0.3354" ixy="0.0" ixz="-0.0162" iyy="0.3354" iyz="-0.0006" izz="0.0954"/>
|
|
7
|
+
</inertial>
|
|
8
|
+
<visual>
|
|
9
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
10
|
+
<geometry>
|
|
11
|
+
<mesh filename="fetch_description/meshes/torso_lift_link.dae"/>
|
|
12
|
+
</geometry>
|
|
13
|
+
</visual>
|
|
14
|
+
<collision>
|
|
15
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
16
|
+
<geometry>
|
|
17
|
+
<mesh filename="fetch_description/meshes/torso_lift_link_collision.STL"/>
|
|
18
|
+
</geometry>
|
|
19
|
+
</collision>
|
|
20
|
+
</link>
|
|
21
|
+
<link name="head_pan_link">
|
|
22
|
+
<inertial>
|
|
23
|
+
<origin rpy="0 0 0" xyz="0.0321 0.0161 0.0390"/>
|
|
24
|
+
<mass value="2.2556"/>
|
|
25
|
+
<inertia ixx="0.0129" ixy="0.0002" ixz="0.0007" iyy="0.0095" iyz="-0.0" izz="0.0184"/>
|
|
26
|
+
</inertial>
|
|
27
|
+
<visual>
|
|
28
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
29
|
+
<geometry>
|
|
30
|
+
<mesh filename="fetch_description/meshes/head_pan_link.dae"/>
|
|
31
|
+
</geometry>
|
|
32
|
+
</visual>
|
|
33
|
+
<collision>
|
|
34
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
35
|
+
<geometry>
|
|
36
|
+
<mesh filename="fetch_description/meshes/head_pan_link_collision.STL"/>
|
|
37
|
+
</geometry>
|
|
38
|
+
</collision>
|
|
39
|
+
</link>
|
|
40
|
+
<joint name="head_pan_joint" type="revolute">
|
|
41
|
+
<origin rpy="0 0 0" xyz="0.053125 0 0.603001417713939"/>
|
|
42
|
+
<parent link="torso_lift_link"/>
|
|
43
|
+
<child link="head_pan_link"/>
|
|
44
|
+
<axis xyz="0 0 1"/>
|
|
45
|
+
<limit effort="0.32" lower="-1.57" upper="1.57" velocity="1.57"/>
|
|
46
|
+
</joint>
|
|
47
|
+
<link name="head_tilt_link">
|
|
48
|
+
<inertial>
|
|
49
|
+
<origin rpy="0 0 0" xyz="0.0081 0.0025 0.0113"/>
|
|
50
|
+
<mass value="0.9087"/>
|
|
51
|
+
<inertia ixx="0.0061" ixy="-0.0" ixz="0.0002" iyy="0.0014" iyz="-0.0001" izz="0.0061"/>
|
|
52
|
+
</inertial>
|
|
53
|
+
<visual>
|
|
54
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
55
|
+
<geometry>
|
|
56
|
+
<mesh filename="fetch_description/meshes/head_tilt_link.dae"/>
|
|
57
|
+
</geometry>
|
|
58
|
+
</visual>
|
|
59
|
+
<collision>
|
|
60
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
61
|
+
<geometry>
|
|
62
|
+
<mesh filename="fetch_description/meshes/head_tilt_link_collision.STL"/>
|
|
63
|
+
</geometry>
|
|
64
|
+
</collision>
|
|
65
|
+
</link>
|
|
66
|
+
<joint name="head_tilt_joint" type="revolute">
|
|
67
|
+
<origin rpy="0 0 0" xyz="0.14253 0 0.057999"/>
|
|
68
|
+
<parent link="head_pan_link"/>
|
|
69
|
+
<child link="head_tilt_link"/>
|
|
70
|
+
<axis xyz="0 1 0"/>
|
|
71
|
+
<limit effort="0.68" lower="-0.76" upper="1.45" velocity="1.57"/>
|
|
72
|
+
</joint>
|
|
73
|
+
<link name="shoulder_pan_link">
|
|
74
|
+
<inertial>
|
|
75
|
+
<origin rpy="0 0 0" xyz="0.0927 -0.0056 0.0564"/>
|
|
76
|
+
<mass value="2.5587"/>
|
|
77
|
+
<inertia ixx="0.0043" ixy="-0.0001" ixz="0.001" iyy="0.0087" iyz="-0.0001" izz="0.0087"/>
|
|
78
|
+
</inertial>
|
|
79
|
+
<visual>
|
|
80
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
81
|
+
<geometry>
|
|
82
|
+
<mesh filename="fetch_description/meshes/shoulder_pan_link.dae"/>
|
|
83
|
+
</geometry>
|
|
84
|
+
</visual>
|
|
85
|
+
<collision>
|
|
86
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
87
|
+
<geometry>
|
|
88
|
+
<mesh filename="fetch_description/meshes/shoulder_pan_link_collision.STL"/>
|
|
89
|
+
</geometry>
|
|
90
|
+
</collision>
|
|
91
|
+
</link>
|
|
92
|
+
<joint name="shoulder_pan_joint" type="revolute">
|
|
93
|
+
<origin rpy="0 0 0" xyz="0.119525 0 0.34858"/>
|
|
94
|
+
<parent link="torso_lift_link"/>
|
|
95
|
+
<child link="shoulder_pan_link"/>
|
|
96
|
+
<axis xyz="0 0 1"/>
|
|
97
|
+
<dynamics damping="1.0"/>
|
|
98
|
+
<limit effort="33.82" lower="-1.6056" upper="1.6056" velocity="1.256"/>
|
|
99
|
+
</joint>
|
|
100
|
+
<link name="shoulder_lift_link">
|
|
101
|
+
<inertial>
|
|
102
|
+
<origin rpy="0 0 0" xyz="0.1432 0.0072 -0.0001"/>
|
|
103
|
+
<mass value="2.6615"/>
|
|
104
|
+
<inertia ixx="0.0028" ixy="-0.0021" ixz="-0.0" iyy="0.0111" iyz="-0.0" izz="0.0112"/>
|
|
105
|
+
</inertial>
|
|
106
|
+
<visual>
|
|
107
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
108
|
+
<geometry>
|
|
109
|
+
<mesh filename="fetch_description/meshes/shoulder_lift_link.dae"/>
|
|
110
|
+
</geometry>
|
|
111
|
+
</visual>
|
|
112
|
+
<collision>
|
|
113
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
114
|
+
<geometry>
|
|
115
|
+
<mesh filename="fetch_description/meshes/shoulder_lift_link_collision.STL"/>
|
|
116
|
+
</geometry>
|
|
117
|
+
</collision>
|
|
118
|
+
</link>
|
|
119
|
+
<joint name="shoulder_lift_joint" type="revolute">
|
|
120
|
+
<origin rpy="0 0 0" xyz="0.117 0 0.0599999999999999"/>
|
|
121
|
+
<parent link="shoulder_pan_link"/>
|
|
122
|
+
<child link="shoulder_lift_link"/>
|
|
123
|
+
<axis xyz="0 1 0"/>
|
|
124
|
+
<dynamics damping="1.0"/>
|
|
125
|
+
<limit effort="131.76" lower="-1.221" upper="1.518" velocity="1.454"/>
|
|
126
|
+
</joint>
|
|
127
|
+
<link name="upperarm_roll_link">
|
|
128
|
+
<inertial>
|
|
129
|
+
<origin rpy="0 0 0" xyz="0.1165 0.0014 0.0000"/>
|
|
130
|
+
<mass value="2.3311"/>
|
|
131
|
+
<inertia ixx="0.0019" ixy="-0.0001" ixz="0.0" iyy="0.0045" iyz="0.0" izz="0.0047"/>
|
|
132
|
+
</inertial>
|
|
133
|
+
<visual>
|
|
134
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
135
|
+
<geometry>
|
|
136
|
+
<mesh filename="fetch_description/meshes/upperarm_roll_link.dae"/>
|
|
137
|
+
</geometry>
|
|
138
|
+
</visual>
|
|
139
|
+
<collision>
|
|
140
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
141
|
+
<geometry>
|
|
142
|
+
<mesh filename="fetch_description/meshes/upperarm_roll_link_collision.STL"/>
|
|
143
|
+
</geometry>
|
|
144
|
+
</collision>
|
|
145
|
+
</link>
|
|
146
|
+
<joint name="upperarm_roll_joint" type="continuous">
|
|
147
|
+
<origin rpy="0 0 0" xyz="0.219 0 0"/>
|
|
148
|
+
<parent link="shoulder_lift_link"/>
|
|
149
|
+
<child link="upperarm_roll_link"/>
|
|
150
|
+
<axis xyz="1 0 0"/>
|
|
151
|
+
<dynamics damping="5.0"/>
|
|
152
|
+
<limit effort="76.94" velocity="1.571"/>
|
|
153
|
+
</joint>
|
|
154
|
+
<link name="elbow_flex_link">
|
|
155
|
+
<inertial>
|
|
156
|
+
<origin rpy="0 0 0" xyz="0.1279 0.0073 0.0000"/>
|
|
157
|
+
<mass value="2.1299"/>
|
|
158
|
+
<inertia ixx="0.0024" ixy="-0.0016" ixz="0.0" iyy="0.0082" iyz="-0.0" izz="0.0084"/>
|
|
159
|
+
</inertial>
|
|
160
|
+
<visual>
|
|
161
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
162
|
+
<geometry>
|
|
163
|
+
<mesh filename="fetch_description/meshes/elbow_flex_link.dae"/>
|
|
164
|
+
</geometry>
|
|
165
|
+
</visual>
|
|
166
|
+
<collision>
|
|
167
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
168
|
+
<geometry>
|
|
169
|
+
<mesh filename="fetch_description/meshes/elbow_flex_link_collision.STL"/>
|
|
170
|
+
</geometry>
|
|
171
|
+
</collision>
|
|
172
|
+
</link>
|
|
173
|
+
<joint name="elbow_flex_joint" type="revolute">
|
|
174
|
+
<origin rpy="0 0 0" xyz="0.133 0 0"/>
|
|
175
|
+
<parent link="upperarm_roll_link"/>
|
|
176
|
+
<child link="elbow_flex_link"/>
|
|
177
|
+
<axis xyz="0 1 0"/>
|
|
178
|
+
<dynamics damping="1.0"/>
|
|
179
|
+
<limit effort="66.18" lower="-2.251" upper="2.251" velocity="1.521"/>
|
|
180
|
+
</joint>
|
|
181
|
+
<link name="forearm_roll_link">
|
|
182
|
+
<inertial>
|
|
183
|
+
<origin rpy="0 0 0" xyz="0.1097 -0.0266 0.0000"/>
|
|
184
|
+
<mass value="1.6563"/>
|
|
185
|
+
<inertia ixx="0.0016" ixy="-0.0003" ixz="0.0" iyy="0.003" iyz="-0.0" izz="0.0035"/>
|
|
186
|
+
</inertial>
|
|
187
|
+
<visual>
|
|
188
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
189
|
+
<geometry>
|
|
190
|
+
<mesh filename="fetch_description/meshes/forearm_roll_link.dae"/>
|
|
191
|
+
</geometry>
|
|
192
|
+
</visual>
|
|
193
|
+
<collision>
|
|
194
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
195
|
+
<geometry>
|
|
196
|
+
<mesh filename="fetch_description/meshes/forearm_roll_link_collision.STL"/>
|
|
197
|
+
</geometry>
|
|
198
|
+
</collision>
|
|
199
|
+
</link>
|
|
200
|
+
<joint name="forearm_roll_joint" type="continuous">
|
|
201
|
+
<origin rpy="0 0 0" xyz="0.197 0 0"/>
|
|
202
|
+
<parent link="elbow_flex_link"/>
|
|
203
|
+
<child link="forearm_roll_link"/>
|
|
204
|
+
<axis xyz="1 0 0"/>
|
|
205
|
+
<dynamics damping="5.0"/>
|
|
206
|
+
<limit effort="29.35" velocity="1.571"/>
|
|
207
|
+
</joint>
|
|
208
|
+
<link name="wrist_flex_link">
|
|
209
|
+
<inertial>
|
|
210
|
+
<origin rpy="0 0 0" xyz="0.0882 0.0009 -0.0001"/>
|
|
211
|
+
<mass value="1.725"/>
|
|
212
|
+
<inertia ixx="0.0018" ixy="-0.0001" ixz="-0.0" iyy="0.0042" iyz="0.0" izz="0.0042"/>
|
|
213
|
+
</inertial>
|
|
214
|
+
<visual>
|
|
215
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
216
|
+
<geometry>
|
|
217
|
+
<mesh filename="fetch_description/meshes/wrist_flex_link.dae"/>
|
|
218
|
+
</geometry>
|
|
219
|
+
</visual>
|
|
220
|
+
<collision>
|
|
221
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
222
|
+
<geometry>
|
|
223
|
+
<mesh filename="fetch_description/meshes/wrist_flex_link_collision.STL"/>
|
|
224
|
+
</geometry>
|
|
225
|
+
</collision>
|
|
226
|
+
</link>
|
|
227
|
+
<joint name="wrist_flex_joint" type="revolute">
|
|
228
|
+
<origin rpy="0 0 0" xyz="0.1245 0 0"/>
|
|
229
|
+
<parent link="forearm_roll_link"/>
|
|
230
|
+
<child link="wrist_flex_link"/>
|
|
231
|
+
<axis xyz="0 1 0"/>
|
|
232
|
+
<dynamics damping="1.0"/>
|
|
233
|
+
<limit effort="25.7" lower="-2.16" upper="2.16" velocity="2.268"/>
|
|
234
|
+
</joint>
|
|
235
|
+
<link name="wrist_roll_link">
|
|
236
|
+
<inertial>
|
|
237
|
+
<origin rpy="0 0 0" xyz="0.0095 0.0004 -0.0002"/>
|
|
238
|
+
<mass value="0.1354"/>
|
|
239
|
+
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
|
240
|
+
</inertial>
|
|
241
|
+
<visual>
|
|
242
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
243
|
+
<geometry>
|
|
244
|
+
<mesh filename="fetch_description/meshes/wrist_roll_link.dae"/>
|
|
245
|
+
</geometry>
|
|
246
|
+
</visual>
|
|
247
|
+
<collision>
|
|
248
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
249
|
+
<geometry>
|
|
250
|
+
<mesh filename="fetch_description/meshes/wrist_roll_link_collision.STL"/>
|
|
251
|
+
</geometry>
|
|
252
|
+
</collision>
|
|
253
|
+
</link>
|
|
254
|
+
<joint name="wrist_roll_joint" type="continuous">
|
|
255
|
+
<origin rpy="0 0 0" xyz="0.1385 0 0"/>
|
|
256
|
+
<parent link="wrist_flex_link"/>
|
|
257
|
+
<child link="wrist_roll_link"/>
|
|
258
|
+
<axis xyz="1 0 0"/>
|
|
259
|
+
<limit effort="7.36" velocity="2.268"/>
|
|
260
|
+
<dynamics damping="5.0"/>
|
|
261
|
+
</joint>
|
|
262
|
+
<link name="gripper_link">
|
|
263
|
+
<inertial>
|
|
264
|
+
<origin rpy="0 0 0" xyz="-0.0900 -0.0001 -0.0017"/>
|
|
265
|
+
<mass value="1.5175"/>
|
|
266
|
+
<inertia ixx="0.0013" ixy="-0.0" ixz="0.0" iyy="0.0019" iyz="-0.0" izz="0.0024"/>
|
|
267
|
+
</inertial>
|
|
268
|
+
<visual>
|
|
269
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
270
|
+
<geometry>
|
|
271
|
+
<mesh filename="fetch_description/meshes/gripper_link.dae"/>
|
|
272
|
+
</geometry>
|
|
273
|
+
</visual>
|
|
274
|
+
<collision>
|
|
275
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
276
|
+
<geometry>
|
|
277
|
+
<mesh filename="fetch_description/meshes/gripper_link.STL"/>
|
|
278
|
+
</geometry>
|
|
279
|
+
</collision>
|
|
280
|
+
</link>
|
|
281
|
+
<joint name="gripper_axis" type="fixed">
|
|
282
|
+
<origin rpy="0 0 0" xyz="0.16645 0 0"/>
|
|
283
|
+
<parent link="wrist_roll_link"/>
|
|
284
|
+
<child link="gripper_link"/>
|
|
285
|
+
<axis xyz="0 1 0"/>
|
|
286
|
+
</joint>
|
|
287
|
+
<link name="r_gripper_finger_link">
|
|
288
|
+
<inertial>
|
|
289
|
+
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
|
|
290
|
+
<mass value="0.0798"/>
|
|
291
|
+
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.0002" iyz="0" izz="0.0002"/>
|
|
292
|
+
</inertial>
|
|
293
|
+
<visual>
|
|
294
|
+
<origin rpy="0 0 0" xyz="0 0.101425 0"/>
|
|
295
|
+
<geometry>
|
|
296
|
+
<mesh filename="fetch_description/meshes/r_gripper_finger_link.STL"/>
|
|
297
|
+
</geometry>
|
|
298
|
+
<material name="">
|
|
299
|
+
<color rgba="0.356 0.361 0.376 1"/>
|
|
300
|
+
</material>
|
|
301
|
+
</visual>
|
|
302
|
+
<collision>
|
|
303
|
+
<origin rpy="0 0 0" xyz="0 0.101425 0"/>
|
|
304
|
+
<geometry>
|
|
305
|
+
<mesh filename="fetch_description/meshes/r_gripper_finger_link.STL"/>
|
|
306
|
+
</geometry>
|
|
307
|
+
</collision>
|
|
308
|
+
</link>
|
|
309
|
+
<joint name="r_gripper_finger_joint" type="prismatic">
|
|
310
|
+
<origin rpy="0 0 0" xyz="0 0.015425 0"/>
|
|
311
|
+
<parent link="gripper_link"/>
|
|
312
|
+
<child link="r_gripper_finger_link"/>
|
|
313
|
+
<axis xyz="0 1 0"/>
|
|
314
|
+
<limit effort="60" lower="0.0" upper="0.05" velocity="0.05"/>
|
|
315
|
+
<dynamics damping="100.0"/>
|
|
316
|
+
</joint>
|
|
317
|
+
<link name="l_gripper_finger_link">
|
|
318
|
+
<inertial>
|
|
319
|
+
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
|
|
320
|
+
<mass value="0.0798"/>
|
|
321
|
+
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.0002" iyz="0" izz="0.0002"/>
|
|
322
|
+
</inertial>
|
|
323
|
+
<visual>
|
|
324
|
+
<origin rpy="0 0 0" xyz="0 -0.101425 0"/>
|
|
325
|
+
<geometry>
|
|
326
|
+
<mesh filename="fetch_description/meshes/l_gripper_finger_link.STL"/>
|
|
327
|
+
</geometry>
|
|
328
|
+
<material name="">
|
|
329
|
+
<color rgba="0.356 0.361 0.376 1"/>
|
|
330
|
+
</material>
|
|
331
|
+
</visual>
|
|
332
|
+
<collision>
|
|
333
|
+
<origin rpy="0 0 0" xyz="0 -0.101425 0"/>
|
|
334
|
+
<geometry>
|
|
335
|
+
<mesh filename="fetch_description/meshes/l_gripper_finger_link.STL"/>
|
|
336
|
+
</geometry>
|
|
337
|
+
</collision>
|
|
338
|
+
</link>
|
|
339
|
+
<joint name="l_gripper_finger_joint" type="prismatic">
|
|
340
|
+
<origin rpy="0 0 0" xyz="0 -0.015425 0"/>
|
|
341
|
+
<parent link="gripper_link"/>
|
|
342
|
+
<child link="l_gripper_finger_link"/>
|
|
343
|
+
<axis xyz="0 -1 0"/>
|
|
344
|
+
<limit effort="60" lower="0.0" upper="0.05" velocity="0.05"/>
|
|
345
|
+
<dynamics damping="100.0"/>
|
|
346
|
+
</joint>
|
|
347
|
+
<link name="bellows_link">
|
|
348
|
+
<inertial>
|
|
349
|
+
<origin rpy="0 0 0" xyz="0.0191320000286228 -1.67219873481315E-16 -0.134861625443387"/>
|
|
350
|
+
<mass value="0.169374038216602"/>
|
|
351
|
+
<inertia ixx="0.00331159128762117" ixy="-8.9230964321455E-18" ixz="-5.38622201018293E-08"
|
|
352
|
+
iyy="0.00174447292786627" iyz="-8.06964979976371E-17" izz="0.00169417568121457"/>
|
|
353
|
+
</inertial>
|
|
354
|
+
<!-- <visual>-->
|
|
355
|
+
<!-- <origin rpy="0 0 0" xyz="0 0 0" />-->
|
|
356
|
+
<!-- <geometry>-->
|
|
357
|
+
<!-- <mesh filename="fetch_description/meshes/bellows_link.STL" />-->
|
|
358
|
+
<!-- </geometry>-->
|
|
359
|
+
<!-- <material name="">-->
|
|
360
|
+
<!-- <color rgba="0 0 0 1" />-->
|
|
361
|
+
<!-- </material>-->
|
|
362
|
+
<!-- </visual>-->
|
|
363
|
+
<!-- <collision>-->
|
|
364
|
+
<!-- <origin rpy="0 0 0" xyz="0 0 0" />-->
|
|
365
|
+
<!-- <geometry>-->
|
|
366
|
+
<!-- <mesh filename="fetch_description/meshes/bellows_link_collision.STL" />-->
|
|
367
|
+
<!-- </geometry>-->
|
|
368
|
+
<!-- </collision>-->
|
|
369
|
+
</link>
|
|
370
|
+
<joint name="bellows_joint" type="fixed">
|
|
371
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
372
|
+
<parent link="torso_lift_link"/>
|
|
373
|
+
<child link="bellows_link"/>
|
|
374
|
+
<axis xyz="0 0 -1"/>
|
|
375
|
+
<limit effort="5.0" lower="0" upper="0.4" velocity="0.1"/>
|
|
376
|
+
</joint>
|
|
377
|
+
<link name="bellows_link2">
|
|
378
|
+
<inertial>
|
|
379
|
+
<origin rpy="0 0 0" xyz="0.0191320000286228 -1.67219873481315E-16 -0.134861625443387"/>
|
|
380
|
+
<mass value="0.169374038216602"/>
|
|
381
|
+
<inertia ixx="0.00331159128762117" ixy="-8.9230964321455E-18" ixz="-5.38622201018293E-08"
|
|
382
|
+
iyy="0.00174447292786627" iyz="-8.06964979976371E-17" izz="0.00169417568121457"/>
|
|
383
|
+
</inertial>
|
|
384
|
+
<visual>
|
|
385
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
386
|
+
<geometry>
|
|
387
|
+
<mesh filename="fetch_description/meshes/bellows_link.STL"/>
|
|
388
|
+
</geometry>
|
|
389
|
+
<material name="">
|
|
390
|
+
<color rgba="0 0 0 1"/>
|
|
391
|
+
</material>
|
|
392
|
+
</visual>
|
|
393
|
+
<collision>
|
|
394
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
395
|
+
<geometry>
|
|
396
|
+
<mesh filename="fetch_description/meshes/bellows_link_collision.STL"/>
|
|
397
|
+
</geometry>
|
|
398
|
+
</collision>
|
|
399
|
+
</link>
|
|
400
|
+
<joint name="bellows_joint2" type="fixed">
|
|
401
|
+
<origin rpy="0 0 0" xyz="0 0 0"/>
|
|
402
|
+
<parent link="torso_lift_link"/>
|
|
403
|
+
<child link="bellows_link2"/>
|
|
404
|
+
</joint>
|
|
405
|
+
<link name="head_camera_link"/>
|
|
406
|
+
<joint name="head_camera_joint" type="fixed">
|
|
407
|
+
<origin rpy="0 0 0" xyz="0.055 0 0.0225"/>
|
|
408
|
+
<parent link="head_tilt_link"/>
|
|
409
|
+
<child link="head_camera_link"/>
|
|
410
|
+
</joint>
|
|
411
|
+
<link name="head_camera_rgb_frame"/>
|
|
412
|
+
<joint name="head_camera_rgb_joint" type="fixed">
|
|
413
|
+
<origin rpy="0 0 0" xyz="0 0.02 0"/>
|
|
414
|
+
<parent link="head_camera_link"/>
|
|
415
|
+
<child link="head_camera_rgb_frame"/>
|
|
416
|
+
</joint>
|
|
417
|
+
<link name="head_camera_rgb_optical_frame"/>
|
|
418
|
+
<joint name="head_camera_rgb_optical_joint" type="fixed">
|
|
419
|
+
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
|
|
420
|
+
<parent link="head_camera_rgb_frame"/>
|
|
421
|
+
<child link="head_camera_rgb_optical_frame"/>
|
|
422
|
+
</joint>
|
|
423
|
+
<link name="head_camera_depth_frame"/>
|
|
424
|
+
<joint name="head_camera_depth_joint" type="fixed">
|
|
425
|
+
<origin rpy="0 0 0" xyz="0 0.045 0"/>
|
|
426
|
+
<parent link="head_camera_link"/>
|
|
427
|
+
<child link="head_camera_depth_frame"/>
|
|
428
|
+
</joint>
|
|
429
|
+
<link name="head_camera_depth_optical_frame"/>
|
|
430
|
+
<joint name="head_camera_depth_optical_joint" type="fixed">
|
|
431
|
+
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
|
|
432
|
+
<parent link="head_camera_depth_frame"/>
|
|
433
|
+
<child link="head_camera_depth_optical_frame"/>
|
|
434
|
+
</joint>
|
|
435
|
+
</robot>
|
|
@@ -137,10 +137,10 @@ def main():
|
|
|
137
137
|
base_action[0] = 1
|
|
138
138
|
elif key == "s": # backward
|
|
139
139
|
base_action[0] = -1
|
|
140
|
-
elif key == "
|
|
141
|
-
base_action[
|
|
142
|
-
elif key == "
|
|
143
|
-
base_action[
|
|
140
|
+
elif key == "a": # rotate counter
|
|
141
|
+
base_action[1] = 1
|
|
142
|
+
elif key == "d": # rotate clockwise
|
|
143
|
+
base_action[1] = -1
|
|
144
144
|
elif key == "z": # lift
|
|
145
145
|
body_action[2] = 1
|
|
146
146
|
elif key == "x": # lower
|
|
@@ -0,0 +1,260 @@
|
|
|
1
|
+
import argparse
|
|
2
|
+
import signal
|
|
3
|
+
|
|
4
|
+
import gymnasium as gym
|
|
5
|
+
import numpy as np
|
|
6
|
+
from matplotlib import pyplot as plt
|
|
7
|
+
|
|
8
|
+
signal.signal(signal.SIGINT, signal.SIG_DFL) # allow ctrl+c
|
|
9
|
+
from mani_skill.envs.sapien_env import BaseEnv
|
|
10
|
+
from mani_skill.utils import common, visualization
|
|
11
|
+
from mani_skill.utils.wrappers import RecordEpisode
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
def parse_args():
|
|
15
|
+
parser = argparse.ArgumentParser()
|
|
16
|
+
parser.add_argument("-e", "--env-id", type=str, required=True)
|
|
17
|
+
parser.add_argument("-o", "--obs-mode", type=str)
|
|
18
|
+
parser.add_argument("--reward-mode", type=str)
|
|
19
|
+
parser.add_argument("-c", "--control-mode", type=str, default="pd_ee_delta_pose")
|
|
20
|
+
parser.add_argument("--render-mode", type=str, default="sensors")
|
|
21
|
+
parser.add_argument("--enable-sapien-viewer", action="store_true")
|
|
22
|
+
parser.add_argument("--record-dir", type=str)
|
|
23
|
+
args, opts = parser.parse_known_args()
|
|
24
|
+
|
|
25
|
+
# Parse env kwargs
|
|
26
|
+
print("opts:", opts)
|
|
27
|
+
eval_str = lambda x: eval(x[1:]) if x.startswith("@") else x
|
|
28
|
+
env_kwargs = dict((x, eval_str(y)) for x, y in zip(opts[0::2], opts[1::2]))
|
|
29
|
+
print("env_kwargs:", env_kwargs)
|
|
30
|
+
args.env_kwargs = env_kwargs
|
|
31
|
+
|
|
32
|
+
return args
|
|
33
|
+
|
|
34
|
+
|
|
35
|
+
def main():
|
|
36
|
+
np.set_printoptions(suppress=True, precision=3)
|
|
37
|
+
args = parse_args()
|
|
38
|
+
|
|
39
|
+
env: BaseEnv = gym.make(
|
|
40
|
+
args.env_id,
|
|
41
|
+
obs_mode=args.obs_mode,
|
|
42
|
+
reward_mode=args.reward_mode,
|
|
43
|
+
control_mode=args.control_mode,
|
|
44
|
+
render_mode=args.render_mode,
|
|
45
|
+
**args.env_kwargs
|
|
46
|
+
)
|
|
47
|
+
|
|
48
|
+
record_dir = args.record_dir
|
|
49
|
+
if record_dir:
|
|
50
|
+
record_dir = record_dir.format(env_id=args.env_id)
|
|
51
|
+
env = RecordEpisode(env, record_dir, render_mode=args.render_mode)
|
|
52
|
+
|
|
53
|
+
print("Observation space", env.observation_space)
|
|
54
|
+
print("Action space", env.action_space)
|
|
55
|
+
print("Control mode", env.control_mode)
|
|
56
|
+
print("Reward mode", env.reward_mode)
|
|
57
|
+
|
|
58
|
+
obs, _ = env.reset()
|
|
59
|
+
after_reset = True
|
|
60
|
+
|
|
61
|
+
# Viewer
|
|
62
|
+
if args.enable_sapien_viewer:
|
|
63
|
+
env.render_human()
|
|
64
|
+
renderer = visualization.ImageRenderer(wait_for_button_press=False)
|
|
65
|
+
# disable all default plt shortcuts that are lowercase letters
|
|
66
|
+
plt.rcParams["keymap.fullscreen"].remove("f")
|
|
67
|
+
plt.rcParams["keymap.home"].remove("h")
|
|
68
|
+
plt.rcParams["keymap.home"].remove("r")
|
|
69
|
+
plt.rcParams["keymap.back"].remove("c")
|
|
70
|
+
plt.rcParams["keymap.forward"].remove("v")
|
|
71
|
+
plt.rcParams["keymap.pan"].remove("p")
|
|
72
|
+
plt.rcParams["keymap.zoom"].remove("o")
|
|
73
|
+
plt.rcParams["keymap.save"].remove("s")
|
|
74
|
+
plt.rcParams["keymap.grid"].remove("g")
|
|
75
|
+
plt.rcParams["keymap.yscale"].remove("l")
|
|
76
|
+
plt.rcParams["keymap.xscale"].remove("k")
|
|
77
|
+
|
|
78
|
+
def render_wait():
|
|
79
|
+
if not args.enable_sapien_viewer:
|
|
80
|
+
return
|
|
81
|
+
while True:
|
|
82
|
+
env.render_human()
|
|
83
|
+
sapien_viewer = env.viewer
|
|
84
|
+
if sapien_viewer.window.key_down("0"):
|
|
85
|
+
break
|
|
86
|
+
|
|
87
|
+
# Embodiment
|
|
88
|
+
has_base = "base" in env.agent.controller.configs
|
|
89
|
+
num_arms = sum("arm" in x for x in env.agent.controller.configs)
|
|
90
|
+
has_gripper = any("gripper" in x for x in env.agent.controller.configs)
|
|
91
|
+
gripper_action = 1
|
|
92
|
+
EE_ACTION = 0.1
|
|
93
|
+
|
|
94
|
+
# For real-time simulation
|
|
95
|
+
import time
|
|
96
|
+
|
|
97
|
+
control_timestep = env.unwrapped.control_timestep
|
|
98
|
+
last_update_time = time.time()
|
|
99
|
+
|
|
100
|
+
while True:
|
|
101
|
+
current_time = time.time()
|
|
102
|
+
elapsed_time = current_time - last_update_time
|
|
103
|
+
|
|
104
|
+
if elapsed_time >= control_timestep:
|
|
105
|
+
last_update_time = current_time
|
|
106
|
+
|
|
107
|
+
# -------------------------------------------------------------------------- #
|
|
108
|
+
# Visualization
|
|
109
|
+
# -------------------------------------------------------------------------- #
|
|
110
|
+
if args.enable_sapien_viewer:
|
|
111
|
+
env.render_human()
|
|
112
|
+
|
|
113
|
+
render_frame = env.render().cpu().numpy()[0]
|
|
114
|
+
|
|
115
|
+
if after_reset:
|
|
116
|
+
after_reset = False
|
|
117
|
+
if args.enable_sapien_viewer:
|
|
118
|
+
renderer.close()
|
|
119
|
+
renderer = visualization.ImageRenderer(wait_for_button_press=False)
|
|
120
|
+
|
|
121
|
+
renderer(render_frame)
|
|
122
|
+
|
|
123
|
+
# -------------------------------------------------------------------------- #
|
|
124
|
+
# Interaction
|
|
125
|
+
# -------------------------------------------------------------------------- #
|
|
126
|
+
# Get the set of currently pressed keys
|
|
127
|
+
pressed_keys = renderer.pressed_keys
|
|
128
|
+
|
|
129
|
+
body_action = np.zeros([3])
|
|
130
|
+
base_action = np.zeros([2]) # hardcoded for fetch robot
|
|
131
|
+
|
|
132
|
+
# Parse end-effector action
|
|
133
|
+
if (
|
|
134
|
+
"pd_ee_delta_pose" in args.control_mode
|
|
135
|
+
or "pd_ee_target_delta_pose" in args.control_mode
|
|
136
|
+
):
|
|
137
|
+
ee_action = np.zeros([6])
|
|
138
|
+
elif (
|
|
139
|
+
"pd_ee_delta_pos" in args.control_mode
|
|
140
|
+
or "pd_ee_target_delta_pos" in args.control_mode
|
|
141
|
+
):
|
|
142
|
+
ee_action = np.zeros([3])
|
|
143
|
+
else:
|
|
144
|
+
raise NotImplementedError(args.control_mode)
|
|
145
|
+
|
|
146
|
+
if has_base:
|
|
147
|
+
if "w" in pressed_keys: # forward
|
|
148
|
+
base_action[0] = 1
|
|
149
|
+
if "s" in pressed_keys: # backward
|
|
150
|
+
base_action[0] = -1
|
|
151
|
+
if "a" in pressed_keys: # rotate counter
|
|
152
|
+
base_action[1] = 1
|
|
153
|
+
if "d" in pressed_keys: # rotate clockwise
|
|
154
|
+
base_action[1] = -1
|
|
155
|
+
if "z" in pressed_keys: # lift
|
|
156
|
+
body_action[2] = 1
|
|
157
|
+
if "x" in pressed_keys: # lower
|
|
158
|
+
body_action[2] = -1
|
|
159
|
+
if "v" in pressed_keys: # rotate head left
|
|
160
|
+
body_action[0] = 1
|
|
161
|
+
if "b" in pressed_keys: # rotate head right
|
|
162
|
+
body_action[0] = -1
|
|
163
|
+
if "n" in pressed_keys: # tilt head down
|
|
164
|
+
body_action[1] = 1
|
|
165
|
+
if "m" in pressed_keys: # rotate head up
|
|
166
|
+
body_action[1] = -1
|
|
167
|
+
|
|
168
|
+
# End-effector
|
|
169
|
+
if num_arms > 0:
|
|
170
|
+
# Position
|
|
171
|
+
if "i" in pressed_keys: # +x
|
|
172
|
+
ee_action[0] = EE_ACTION
|
|
173
|
+
if "k" in pressed_keys: # -x
|
|
174
|
+
ee_action[0] = -EE_ACTION
|
|
175
|
+
if "j" in pressed_keys: # +y
|
|
176
|
+
ee_action[1] = EE_ACTION
|
|
177
|
+
if "l" in pressed_keys: # -y
|
|
178
|
+
ee_action[1] = -EE_ACTION
|
|
179
|
+
if "u" in pressed_keys: # +z
|
|
180
|
+
ee_action[2] = EE_ACTION
|
|
181
|
+
if "o" in pressed_keys: # -z
|
|
182
|
+
ee_action[2] = -EE_ACTION
|
|
183
|
+
|
|
184
|
+
# Rotation (axis-angle)
|
|
185
|
+
if "1" in pressed_keys:
|
|
186
|
+
ee_action[3:6] = (1, 0, 0)
|
|
187
|
+
elif "2" in pressed_keys:
|
|
188
|
+
ee_action[3:6] = (-1, 0, 0)
|
|
189
|
+
elif "3" in pressed_keys:
|
|
190
|
+
ee_action[3:6] = (0, 1, 0)
|
|
191
|
+
elif "4" in pressed_keys:
|
|
192
|
+
ee_action[3:6] = (0, -1, 0)
|
|
193
|
+
elif "5" in pressed_keys:
|
|
194
|
+
ee_action[3:6] = (0, 0, 1)
|
|
195
|
+
elif "6" in pressed_keys:
|
|
196
|
+
ee_action[3:6] = (0, 0, -1)
|
|
197
|
+
|
|
198
|
+
# Gripper
|
|
199
|
+
if has_gripper:
|
|
200
|
+
if "f" in pressed_keys: # open gripper
|
|
201
|
+
gripper_action = 1
|
|
202
|
+
if "g" in pressed_keys: # close gripper
|
|
203
|
+
gripper_action = -1
|
|
204
|
+
|
|
205
|
+
# Other functions
|
|
206
|
+
if "0" in pressed_keys: # switch to SAPIEN viewer
|
|
207
|
+
render_wait()
|
|
208
|
+
if "0" in renderer.pressed_keys:
|
|
209
|
+
renderer.pressed_keys.remove("0")
|
|
210
|
+
elif "r" in pressed_keys: # reset env
|
|
211
|
+
obs, _ = env.reset()
|
|
212
|
+
gripper_action = 1
|
|
213
|
+
after_reset = True
|
|
214
|
+
if "r" in renderer.pressed_keys:
|
|
215
|
+
renderer.pressed_keys.remove("r")
|
|
216
|
+
continue
|
|
217
|
+
elif "q" in pressed_keys or "escape" in pressed_keys: # exit
|
|
218
|
+
break
|
|
219
|
+
|
|
220
|
+
# Visualize observation
|
|
221
|
+
if "v" in pressed_keys and "pointcloud" in env.obs_mode:
|
|
222
|
+
import trimesh
|
|
223
|
+
|
|
224
|
+
xyzw = obs["pointcloud"]["xyzw"]
|
|
225
|
+
mask = xyzw[..., 3] > 0
|
|
226
|
+
rgb = obs["pointcloud"]["rgb"]
|
|
227
|
+
if "robot_seg" in obs["pointcloud"]:
|
|
228
|
+
robot_seg = obs["pointcloud"]["robot_seg"]
|
|
229
|
+
rgb = np.uint8(robot_seg * [11, 61, 127])
|
|
230
|
+
trimesh.PointCloud(xyzw[mask, :3], rgb[mask]).show()
|
|
231
|
+
if "v" in renderer.pressed_keys:
|
|
232
|
+
renderer.pressed_keys.remove("v")
|
|
233
|
+
|
|
234
|
+
# -------------------------------------------------------------------------- #
|
|
235
|
+
# Post-process action
|
|
236
|
+
# -------------------------------------------------------------------------- #
|
|
237
|
+
action_dict = dict(
|
|
238
|
+
base=base_action,
|
|
239
|
+
arm=ee_action,
|
|
240
|
+
body=body_action,
|
|
241
|
+
gripper=gripper_action,
|
|
242
|
+
)
|
|
243
|
+
action_dict = common.to_tensor(action_dict)
|
|
244
|
+
action = env.agent.controller.from_action_dict(action_dict)
|
|
245
|
+
|
|
246
|
+
obs, reward, terminated, truncated, info = env.step(action)
|
|
247
|
+
print("reward", reward)
|
|
248
|
+
print("terminated", terminated, "truncated", truncated)
|
|
249
|
+
print("info", info)
|
|
250
|
+
else:
|
|
251
|
+
# Small sleep to prevent CPU hogging when waiting for the next timestep
|
|
252
|
+
time.sleep(0.001)
|
|
253
|
+
# Update the renderer to keep the display responsive
|
|
254
|
+
plt.pause(0.001)
|
|
255
|
+
|
|
256
|
+
env.close()
|
|
257
|
+
|
|
258
|
+
|
|
259
|
+
if __name__ == "__main__":
|
|
260
|
+
main()
|
|
@@ -15,22 +15,32 @@ class ImageRenderer:
|
|
|
15
15
|
self._image = None
|
|
16
16
|
self.last_event = None
|
|
17
17
|
self.wait_for_button_press = wait_for_button_press
|
|
18
|
+
self.pressed_keys = set()
|
|
18
19
|
|
|
19
|
-
def
|
|
20
|
+
def key_press_handler(self, event):
|
|
20
21
|
self.last_event = event
|
|
22
|
+
self.pressed_keys.add(event.key)
|
|
21
23
|
if event.key in ["q", "escape"]:
|
|
22
24
|
sys.exit(0)
|
|
23
25
|
|
|
26
|
+
def key_release_handler(self, event):
|
|
27
|
+
if event.key in self.pressed_keys:
|
|
28
|
+
self.pressed_keys.remove(event.key)
|
|
29
|
+
|
|
24
30
|
def __call__(self, buffer):
|
|
25
31
|
if not self._image:
|
|
26
32
|
plt.ion()
|
|
27
33
|
self.fig = plt.figure()
|
|
28
34
|
self._image = plt.imshow(buffer, animated=True)
|
|
29
|
-
self.fig.canvas.mpl_connect("key_press_event", self.
|
|
35
|
+
self.fig.canvas.mpl_connect("key_press_event", self.key_press_handler)
|
|
36
|
+
self.fig.canvas.mpl_connect("key_release_event", self.key_release_handler)
|
|
30
37
|
else:
|
|
31
38
|
self._image.set_data(buffer)
|
|
32
39
|
if self.wait_for_button_press:
|
|
33
40
|
plt.waitforbuttonpress()
|
|
41
|
+
else:
|
|
42
|
+
self.fig.canvas.draw_idle()
|
|
43
|
+
self.fig.canvas.flush_events()
|
|
34
44
|
plt.draw()
|
|
35
45
|
|
|
36
46
|
def __del__(self):
|
{mani_skill_nightly-2025.5.23.1843.dist-info → mani_skill_nightly-2025.5.26.2237.dist-info}/RECORD
RENAMED
|
@@ -9,12 +9,12 @@ mani_skill/agents/controllers/__init__.py,sha256=tsd27dW6brQBm563CddUGtFMBNAxe_M
|
|
|
9
9
|
mani_skill/agents/controllers/base_controller.py,sha256=Q-HBiweRyqvUwmHV8WPJw96BKrmnlH9h5kSUhIwy4eQ,13334
|
|
10
10
|
mani_skill/agents/controllers/passive_controller.py,sha256=1cZp758AVuABvgWOzCeguBbbB8wJ6hF_cqN-k_vx5Kw,1322
|
|
11
11
|
mani_skill/agents/controllers/pd_base_vel.py,sha256=-3ZzLZ9nRDMGHWnFE7c4reEbC3jHtQJCOx3LmArz95g,2868
|
|
12
|
-
mani_skill/agents/controllers/pd_ee_pose.py,sha256=
|
|
12
|
+
mani_skill/agents/controllers/pd_ee_pose.py,sha256=iCqK8q1ds7VTH4tkQleyryiBTIts70yS4SEQ0jzpMYI,12134
|
|
13
13
|
mani_skill/agents/controllers/pd_joint_pos.py,sha256=R0085LrFTMOwBomQcHlZVhNaWVtVc7WvuEqj5zKhA9U,11109
|
|
14
14
|
mani_skill/agents/controllers/pd_joint_pos_vel.py,sha256=wgiXmenTVIao1Tm1vtdJWTZRFJMB_yQBNd5vQaP8WXw,2489
|
|
15
15
|
mani_skill/agents/controllers/pd_joint_vel.py,sha256=VZF06ISCkdKBX_fUHxb7mdl9GN1Lob5dhrFGlwCx16Q,1957
|
|
16
16
|
mani_skill/agents/controllers/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
17
|
-
mani_skill/agents/controllers/utils/kinematics.py,sha256=
|
|
17
|
+
mani_skill/agents/controllers/utils/kinematics.py,sha256=FiSE1nfZejShEqvua9buzA_SCBuEP9OiAd7LpLbSp44,10415
|
|
18
18
|
mani_skill/agents/robots/__init__.py,sha256=Wva1cg21zTnZWtLCsHvInCmUEFmkJDvj1k2Vt42ov9M,713
|
|
19
19
|
mani_skill/agents/robots/allegro_hand/__init__.py,sha256=PWH6MMv2i5g1xi76_E-YJLtKFLVKxkJgQ96GD_YeQN4,104
|
|
20
20
|
mani_skill/agents/robots/allegro_hand/allegro.py,sha256=MnU04fCcvppTGJHdhgvs2OqUKToQtNMXbs2bFM5sXiU,4896
|
|
@@ -24,7 +24,7 @@ mani_skill/agents/robots/anymal/anymal_c.py,sha256=p7DuLDI7yx2_C1Eg2VXpNTAKCQrEP
|
|
|
24
24
|
mani_skill/agents/robots/dclaw/__init__.py,sha256=t1VSGN3WYw9f3mR7_M08-VhCBQPWOi7vKz7Rz3T8KJQ,25
|
|
25
25
|
mani_skill/agents/robots/dclaw/dclaw.py,sha256=G5DqqRl2R8NroNyTaStdofEFW23wWqla2qy6mqkVOG8,4082
|
|
26
26
|
mani_skill/agents/robots/fetch/__init__.py,sha256=q3QA2oGTx-LgdmCbpe3wpj1ifoqhhFDdrMMXC43Nsuc,79
|
|
27
|
-
mani_skill/agents/robots/fetch/fetch.py,sha256=
|
|
27
|
+
mani_skill/agents/robots/fetch/fetch.py,sha256=akai_C0PZZ6KflfzyI7gPzNyeUPMYU_Su6h-lglCPmM,15416
|
|
28
28
|
mani_skill/agents/robots/floating_panda_gripper/__init__.py,sha256=AwV0Sml7DmQb6hk4FqbxHdO7_XXHHMhrOZtZRk6d-Po,57
|
|
29
29
|
mani_skill/agents/robots/floating_panda_gripper/floating_panda_gripper.py,sha256=sUzYnpEGDW_afwldcerl679wC-6HDEq-APNBSY6-WAo,5071
|
|
30
30
|
mani_skill/agents/robots/floating_robotiq_2f_85_gripper/__init__.py,sha256=bBXRyK4ncX9Zv6R5NXEHD9f6hT9A2PZEsG0n4K76f00,71
|
|
@@ -184,6 +184,7 @@ mani_skill/assets/robots/dclaw/meshes/visual/link_3.mtl,sha256=YY3Yq9p39oJNj9O69
|
|
|
184
184
|
mani_skill/assets/robots/dclaw/meshes/visual/link_3.obj,sha256=R7x-y4Vw6HVSECVlNJrZqzOR9ksX7xWkSd8YUWelu5s,839825
|
|
185
185
|
mani_skill/assets/robots/fetch/fetch.srdf,sha256=v3j8A0oTX4Cb4-6W2Wexp5vPr6F3ahFzIun58qNSVUY,928
|
|
186
186
|
mani_skill/assets/robots/fetch/fetch.urdf,sha256=dQPqLclus9ETDIH3_x45fzDfRkWbPb8Qz10NJHxbElY,21990
|
|
187
|
+
mani_skill/assets/robots/fetch/fetch_torso_up.urdf,sha256=0I8c0SGYJsnbn3fL8gxb5HIlV0rHO7OfxNduXr-w4u8,15127
|
|
187
188
|
mani_skill/assets/robots/fetch/fetch_description/meshes/base_link.dae,sha256=W84mYbBy-UAvSvibQLEHJxBU48dwXi-fI_-vKG5KO3w,4771104
|
|
188
189
|
mani_skill/assets/robots/fetch/fetch_description/meshes/base_link_collision.STL,sha256=3bvsqpTuu5I6FTk1uf8CiTyhWTYc5PtANj_mTuZzO4E,236084
|
|
189
190
|
mani_skill/assets/robots/fetch/fetch_description/meshes/base_link_uv.png,sha256=xX0KWSNrRB-pH54Eiay0mWCueldgPDNyfEXN23Aliqw,442058
|
|
@@ -634,7 +635,8 @@ mani_skill/evaluation/evaluator.py,sha256=1EN6qAiGx3taB4vCeArUp33UZjLvBt1Ke6iUW8
|
|
|
634
635
|
mani_skill/evaluation/run_evaluation.py,sha256=yorphrlJKEGycHfQS8equnJHRsyjDuv77ZGNpg9wvCs,4780
|
|
635
636
|
mani_skill/evaluation/solution.py,sha256=e_Aa0f4sSQ56KXL7tVDPUKf7WTjcuFc5X4J76p884Zs,1269
|
|
636
637
|
mani_skill/examples/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
637
|
-
mani_skill/examples/demo_manual_control.py,sha256=
|
|
638
|
+
mani_skill/examples/demo_manual_control.py,sha256=Z17ER37oehS8VgtDO_4dwiy5jDgL93nT9IdCsNDf0Es,8275
|
|
639
|
+
mani_skill/examples/demo_manual_control_continuous.py,sha256=tnCnKX2v1iIhtXwvWR2NzXgpf3e0y2-qAO91jJBLIO0,9679
|
|
638
640
|
mani_skill/examples/demo_random_action.py,sha256=qdpndV31mWxRK_340TGDXYQAV4CAkKc4DaFHmPM_7Jw,5226
|
|
639
641
|
mani_skill/examples/demo_reset_distribution.py,sha256=qkg9TlGjL13WfYgnoimKN5XZr2bK1WvJGvi2Lj3Tmq8,2987
|
|
640
642
|
mani_skill/examples/demo_robot.py,sha256=bIeHztjM0R6yJT699WQ6jkhv6LjsiP4GWa3Whyom_qM,4881
|
|
@@ -802,7 +804,7 @@ mani_skill/utils/visualization/UbuntuSansMono-Regular.ttf,sha256=y_4ls2KucNhIA6C
|
|
|
802
804
|
mani_skill/utils/visualization/__init__.py,sha256=0QF97UR8d7poMHo6m52DsAUXAmUb3SDr1B21bx33EEU,163
|
|
803
805
|
mani_skill/utils/visualization/jupyter_utils.py,sha256=dXXUQz-rFTOV_Xq5yA6YE6cXg7DPw15YStw37NgB5Qc,1322
|
|
804
806
|
mani_skill/utils/visualization/misc.py,sha256=KrDCef7F5GmGOdiBQ4qFUnmUTe-7-nNBz2DVBGFD8YU,5041
|
|
805
|
-
mani_skill/utils/visualization/renderer.py,sha256
|
|
807
|
+
mani_skill/utils/visualization/renderer.py,sha256=afFWwSQEeL-9c5CsBT1uug-zugGjOr1FDzmvd45-9dk,1646
|
|
806
808
|
mani_skill/utils/wrappers/__init__.py,sha256=f6HDHHoM8gyNgX5RwTr2u3oGlAeHqawRvVNQiWXEJfI,229
|
|
807
809
|
mani_skill/utils/wrappers/action_repeat.py,sha256=RhCtzt3fYCtD-CClIOhAzdycGwVTXP_FG61yEf-QLqY,3542
|
|
808
810
|
mani_skill/utils/wrappers/flatten.py,sha256=GuHJ3fCOdj9G_jm--XgG8k0p2G1eJx4LY1tesQQjnkg,4913
|
|
@@ -815,8 +817,8 @@ mani_skill/vector/wrappers/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJ
|
|
|
815
817
|
mani_skill/vector/wrappers/gymnasium.py,sha256=v1MDPIrVACBKCulrpdXBK2jDZQI7LKYFZgGgaCC5avY,7408
|
|
816
818
|
mani_skill/vector/wrappers/sb3.py,sha256=SlXdiEPqcNHYMhJCzA29kBU6zK7DKTe1nc0L6Z3QQtY,4722
|
|
817
819
|
mani_skill/viewer/__init__.py,sha256=srvDBsk4LQU75K2VIttrhiQ68p_ro7PSDqQRls2PY5c,1722
|
|
818
|
-
mani_skill_nightly-2025.5.
|
|
819
|
-
mani_skill_nightly-2025.5.
|
|
820
|
-
mani_skill_nightly-2025.5.
|
|
821
|
-
mani_skill_nightly-2025.5.
|
|
822
|
-
mani_skill_nightly-2025.5.
|
|
820
|
+
mani_skill_nightly-2025.5.26.2237.dist-info/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
|
|
821
|
+
mani_skill_nightly-2025.5.26.2237.dist-info/METADATA,sha256=v_3iMf2ZWCfz1wN0tLioX1AtDDutK30UYH9v7HhgOHI,9411
|
|
822
|
+
mani_skill_nightly-2025.5.26.2237.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
|
|
823
|
+
mani_skill_nightly-2025.5.26.2237.dist-info/top_level.txt,sha256=bkBgOVl_MZMoQx2aRFsSFEYlZLxjWlip5vtJ39FB3jA,11
|
|
824
|
+
mani_skill_nightly-2025.5.26.2237.dist-info/RECORD,,
|
{mani_skill_nightly-2025.5.23.1843.dist-info → mani_skill_nightly-2025.5.26.2237.dist-info}/LICENSE
RENAMED
|
File without changes
|
{mani_skill_nightly-2025.5.23.1843.dist-info → mani_skill_nightly-2025.5.26.2237.dist-info}/WHEEL
RENAMED
|
File without changes
|
|
File without changes
|