Skip to content

Commit 171414f

Browse files
juelgjuelg
committed
docs(examples): improve grasping demo
- clean up scenes - fixed ee rotation and tcp
1 parent 656470c commit 171414f

4 files changed

Lines changed: 34 additions & 36 deletions

File tree

‎assets/scenes/fr3_empty_world/scene.xml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -20,9 +20,9 @@
2020
<worldbody>
2121
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
2222
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
23-
<body>
24-
<geom type="box" name="isocube" size="0.2 0.2 0.2" rgba="0.153 0.961 0.329 0.2" pos="0.498 0.0 0.226" contype="0" conaffinity="0" group="2"/>
23+
<!-- <body>
24+
<geom type="box" name="isocube" size="0.2 0.2 0.2" rgba="0.153 0.961 0.329 0.2" pos="0.498 0.0 0.226" contype="0" conaffinity="0" group="2"/>
25+
</body> -->
2526
<camera name="bird_eye_cam" pos="0.271 -0.000 2.080" xyaxes="0.001 -1.000 -0.000 1.000 0.001 0.017" />
26-
</body>
2727
</worldbody>
2828
</mujoco>

‎assets/scenes/fr3_simple_pick_up/scene.xml‎

Lines changed: 8 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
<mujoco model="simple pick up task">
1+
<mujoco model="fr3 simple pick up">
22
<include file="fr3_common.xml" />
33
<include file="robot.xml" />
44

@@ -12,29 +12,24 @@
1212

1313
<asset>
1414
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
15-
16-
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0 0 0" rgb2="0 0 0" markrgb="0.8 0.8 0.8" width="300" height="300" />
17-
18-
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0" />
15+
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
16+
markrgb="0.8 0.8 0.8" width="300" height="300" />
17+
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
1918
</asset>
2019

2120
<worldbody>
2221
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
23-
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" friction="1 0.005 0.0001" />
24-
<!-- <body>
25-
<geom type="box" name="isocube" size="0.2 0.2 0.2" rgba="0.153 0.961 0.329 0.2" pos="0.498 0.0 0.226" contype="0" conaffinity="0" group="2"/>
26-
</body> -->
27-
22+
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
2823
<camera name="bird_eye_cam" pos="0.271 -0.000 2.080" xyaxes="0.001 -1.000 -0.000 1.000 0.001 0.017" />
2924
<camera name="openvla_view" pos="1.535 -0.406 0.757" xyaxes="0.246 0.969 0.000 -0.468 0.119 0.876" />
3025
<camera name="right_side" pos="0.217 -1.738 0.583" xyaxes="1.000 -0.010 0.000 0.001 0.143 0.990"/>
3126
<camera name="front" pos="2.278 -0.020 0.931" xyaxes="0.003 1.000 0.000 -0.274 0.001 0.962"/>
3227
<camera name="left_side" pos="0.266 1.832 0.434" xyaxes="-1.000 -0.021 0.000 0.002 -0.092 0.996"/>
3328
<camera name="side_view" pos="1.760 -1.205 0.621" xyaxes="0.681 0.733 0.000 -0.105 0.097 0.990"/>
3429

35-
<body name="box_body" pos="0.44 0.1 0.03">
36-
<geom type="box" size="0.03 0.03 0.03" rgba="1 1 0 1" name="box_geom" friction="1 0.3 0.1" density="50" />
37-
<joint type="free" name="box_joint" />
30+
<body name="box_geom" pos="0.44 0.1 0.03" quat="0 0 0 1">
31+
<geom type="box" size="0.032 0.016 0.0288" rgba="0 0.984 0.373 1" name="box_geom" friction="1 0.3 0.1" density="50" />
32+
<joint type="free" name="box_joint" />
3833
</body>
3934
</worldbody>
4035
</mujoco>

‎examples/fr3/grasp_demo.py‎

Lines changed: 19 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,13 @@
11
import logging
2+
from time import sleep
23
from typing import Any, cast
34

45
import gymnasium as gym
56
import mujoco
67
import numpy as np
78
from rcs._core.common import Pose
89
from rcs.envs.base import GripperWrapper, RobotEnv
10+
from rcs.envs.creators import FR3SimplePickUpSimEnvCreator
911

1012
logger = logging.getLogger(__name__)
1113
logger.setLevel(logging.INFO)
@@ -25,54 +27,49 @@ def get_object_pose(self, geom_name) -> Pose:
2527
data = self.env.get_wrapper_attr("sim").data
2628

2729
geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
28-
return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3))
30+
obj_pose_world_coordinates = Pose(
31+
translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3)
32+
) * Pose(rpy_vector=np.array([0, 0, np.pi]), translation=[0, 0, 0]) # type: ignore
33+
return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates)
2934

3035
def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]:
3136
waypoints = []
3237
for i in range(num_waypoints + 1):
33-
t = i / (num_waypoints + 1)
38+
t = i / (num_waypoints)
3439
waypoints.append(start_pose.interpolate(end_pose, t))
35-
waypoints.append(end_pose)
3640
return waypoints
3741

3842
def step(self, action: dict) -> dict:
3943
return self.env.step(action)[0]
4044

4145
def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]:
4246
end_eff_pose = self.unwrapped.robot.get_cartesian_position()
43-
4447
goal_pose = self.get_object_pose(geom_name=geom_name)
45-
# goal pose is above the object and gripper coordinate must flip z-axis (end effector base rotation is [1, 0, 0, 0])
46-
# be careful we define identity quaternion as as [0, 0, 0, 1]
47-
# this does not work if the object is flipped
4848
goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore
49-
5049
return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints)
5150

5251
def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
5352
for i in range(len(waypoints)):
54-
# calculate delta action
55-
# delta_action = waypoints[i] * waypoints[i - 1].inverse()
5653
obs = self.step(self._action(waypoints[i], gripper))
5754
return obs
5855

5956
def approach(self, geom_name: str):
60-
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=5)
57+
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60)
6158
self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
6259

6360
def grasp(self, geom_name: str):
6461

65-
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=15)
62+
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.01, num_waypoints=60)
6663
self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
6764

6865
self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED))
6966

70-
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=20)
67+
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60)
7168
self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)
7269

7370
def move_home(self):
7471
end_eff_pose = self.unwrapped.robot.get_cartesian_position()
75-
waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=10)
72+
waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=60)
7673
self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)
7774

7875
def pickup(self, geom_name: str):
@@ -82,15 +79,17 @@ def pickup(self, geom_name: str):
8279

8380

8481
def main():
85-
env = gym.make(
86-
"rcs/FR3SimplePickUpSim-v0",
82+
env = FR3SimplePickUpSimEnvCreator()(
8783
render_mode="human",
8884
delta_actions=False,
8985
)
90-
env.reset()
91-
print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore
92-
controller = PickUpDemo(env)
93-
controller.pickup("box_geom")
86+
# wait for gui to open
87+
sleep(3)
88+
for _ in range(100):
89+
env.reset()
90+
print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore
91+
controller = PickUpDemo(env)
92+
controller.pickup("box_geom")
9493

9594

9695
if __name__ == "__main__":

‎python/rcs/envs/creators.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -209,6 +209,10 @@ def __call__( # type: ignore
209209
for cam in cam_list
210210
}
211211
robot_cfg = default_sim_robot_cfg(scene="fr3_simple_pick_up")
212+
robot_cfg.tcp_offset = rcs.common.Pose(
213+
translation=np.array([0.0, 0.0, 0.1034]), # type: ignore
214+
rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]), # type: ignore
215+
)
212216

213217
return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras)
214218

0 commit comments

Comments
 (0)