Skip to content

Commit cae6cc2

Browse files
juelgjuelg
authored
Merge pull request #216 from RobotControlStack/juelg/examples-restructure
- restructure examples into folder structure - moved examples from extensions to examples folder - added example explanation - fixed pick cube example - added pybind for sim config - added support for async mode and realtime mode in sim
2 parents 77fabe0 + 719cd1e commit cae6cc2

33 files changed

Lines changed: 369 additions & 434 deletions

‎.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,3 +17,4 @@ config.h
1717
wheels
1818
.env
1919
settings.json
20+
*.egg-info

‎README.md‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,6 @@ from rcs.envs.utils import (
7272
from rcs.envs.base import ControlMode, RelativeTo
7373
env_rel = SimEnvCreator()(
7474
control_mode=ControlMode.JOINTS,
75-
collision_guard=False,
7675
robot_cfg=default_sim_robot_cfg(),
7776
gripper_cfg=default_sim_gripper_cfg(),
7877
cameras=default_mujoco_cameraset_cfg(),
@@ -93,10 +92,11 @@ for _ in range(100):
9392

9493

9594
### Examples
96-
Checkout the python examples in the [examples](examples) folder:
97-
- [fr3_direct_control.py](examples/fr3_direct_control.py) shows direct robot control with RCS's python bindings
98-
- [fr3_env_joint_control.py](examples/env_joint_control.py) and [fr3_env_cartesian_control.py](examples/env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control
99-
All of these examples work both in the MuJoCo simulation as well as on your hardware FR3.
95+
Checkout the python examples in the [examples](examples) folder. For example
96+
[fr3_direct_control.py](examples/fr3/fr3_direct_control.py) shows direct robot control with RCS's python bindings.
97+
And [fr3_env_joint_control.py](examples/fr3/fr3_env_joint_control.py) and [fr3_env_cartesian_control.py](examples/fr3/fr3_env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control
98+
Checkout the other sub folders for other robot-specific examples.
99+
Most of these examples work both in the MuJoCo simulation as well as on hardware.
100100

101101

102102
### Hardware Extensions

‎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/__init__.py‎

Whitespace-only changes.

examples/fr3_direct_control.py renamed to examples/fr3/fr3_direct_control.py

File renamed without changes.

‎examples/fr3/fr3_env_cartesian_control.py‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
import logging
2+
3+
from rcs._core.common import RobotPlatform
4+
from rcs.envs.base import ControlMode, RelativeTo
5+
from rcs.envs.creators import SimEnvCreator
6+
from rcs.envs.utils import (
7+
default_mujoco_cameraset_cfg,
8+
default_sim_gripper_cfg,
9+
default_sim_robot_cfg,
10+
)
11+
12+
logger = logging.getLogger(__name__)
13+
logger.setLevel(logging.INFO)
14+
15+
"""
16+
This script demonstrates how to control the FR3 robot in Cartesian position control mode
17+
using relative movements. The robot (or its simulation) moves 1cm forward and then 1cm backward
18+
in a loop while opening and closing the gripper.
19+
20+
To control a real FR3 robot, install the rcs_fr3 extension (`pip install extensions/rcs_fr3`),
21+
and set the FR3_IP variable to the robot's IP address. Make sure to unlock the robot's joints and
22+
put it into FCI mode before running this script. For a scripted way of unlocking and guiding mode see the
23+
fr3_direct_control.py example which uses the FCI context manager.
24+
"""
25+
26+
ROBOT_INSTANCE = RobotPlatform.SIMULATION
27+
FR3_IP = "192.168.101.1"
28+
29+
30+
def main():
31+
if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
32+
env_rel = SimEnvCreator()(
33+
control_mode=ControlMode.CARTESIAN_TQuat,
34+
robot_cfg=default_sim_robot_cfg(scene="fr3_empty_world"),
35+
gripper_cfg=default_sim_gripper_cfg(),
36+
cameras=default_mujoco_cameraset_cfg(),
37+
max_relative_movement=0.5,
38+
relative_to=RelativeTo.LAST_STEP,
39+
)
40+
env_rel.get_wrapper_attr("sim").open_gui()
41+
else:
42+
from rcs_fr3.creators import RCSFR3EnvCreator
43+
from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
44+
45+
env_rel = RCSFR3EnvCreator()(
46+
ip=FR3_IP,
47+
control_mode=ControlMode.CARTESIAN_TQuat,
48+
robot_cfg=default_fr3_hw_robot_cfg(),
49+
gripper_cfg=default_fr3_hw_gripper_cfg(),
50+
camera_set=None,
51+
max_relative_movement=0.5,
52+
relative_to=RelativeTo.LAST_STEP,
53+
)
54+
input("the robot is going to move, press enter whenever you are ready")
55+
56+
env_rel.reset()
57+
58+
# access low level robot api to get current cartesian position
59+
print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore
60+
61+
for _ in range(100):
62+
for _ in range(10):
63+
# move 1cm in x direction (forward) and close gripper
64+
act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0}
65+
obs, reward, terminated, truncated, info = env_rel.step(act)
66+
for _ in range(10):
67+
# move 1cm in negative x direction (backward) and open gripper
68+
act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1}
69+
obs, reward, terminated, truncated, info = env_rel.step(act)
70+
71+
72+
if __name__ == "__main__":
73+
main()

‎examples/fr3/fr3_env_joint_control.py‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
import logging
2+
3+
import numpy as np
4+
from rcs._core.common import RobotPlatform
5+
from rcs.envs.base import ControlMode, RelativeTo
6+
from rcs.envs.creators import SimEnvCreator
7+
from rcs.envs.utils import (
8+
default_mujoco_cameraset_cfg,
9+
default_sim_gripper_cfg,
10+
default_sim_robot_cfg,
11+
)
12+
13+
logger = logging.getLogger(__name__)
14+
logger.setLevel(logging.INFO)
15+
16+
"""
17+
This script demonstrates how to control the FR3 robot in joint position control mode
18+
using relative movements. The robot (or its simulation) samples random relative joint movements
19+
in a loop.
20+
21+
To control a real FR3 robot, install the rcs_fr3 extension (`pip install extensions/rcs_fr3`),
22+
change the ROBOT_INSTANCE variable to RobotPlatform.HARDWARE
23+
and set the FR3_IP variable to the robot's IP address. Make sure to unlock the robot's joints and
24+
put it into FCI mode before running this script. For a scripted way of unlocking and guiding mode see the
25+
fr3_direct_control.py example which uses the FCI context manager.
26+
"""
27+
28+
ROBOT_INSTANCE = RobotPlatform.SIMULATION
29+
FR3_IP = "192.168.101.1"
30+
31+
32+
def main():
33+
if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
34+
env_rel = SimEnvCreator()(
35+
control_mode=ControlMode.JOINTS,
36+
robot_cfg=default_sim_robot_cfg("fr3_empty_world"),
37+
gripper_cfg=default_sim_gripper_cfg(),
38+
cameras=default_mujoco_cameraset_cfg(),
39+
max_relative_movement=np.deg2rad(5),
40+
relative_to=RelativeTo.LAST_STEP,
41+
)
42+
env_rel.get_wrapper_attr("sim").open_gui()
43+
else:
44+
from rcs_fr3.creators import RCSFR3EnvCreator
45+
from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
46+
47+
env_rel = RCSFR3EnvCreator()(
48+
ip=FR3_IP,
49+
control_mode=ControlMode.JOINTS,
50+
robot_cfg=default_fr3_hw_robot_cfg(),
51+
gripper_cfg=default_fr3_hw_gripper_cfg(),
52+
camera_set=None,
53+
max_relative_movement=np.deg2rad(5),
54+
relative_to=RelativeTo.LAST_STEP,
55+
)
56+
input("the robot is going to move, press enter whenever you are ready")
57+
58+
# access low level robot api to get current cartesian position
59+
print(env_rel.unwrapped.robot.get_joint_position()) # type: ignore
60+
61+
for _ in range(100):
62+
obs, info = env_rel.reset()
63+
for _ in range(10):
64+
# sample random relative action and execute it
65+
act = env_rel.action_space.sample()
66+
obs, reward, terminated, truncated, info = env_rel.step(act)
67+
68+
69+
if __name__ == "__main__":
70+
main()

examples/grasp_demo.py renamed to examples/fr3/grasp_demo.py

Lines changed: 21 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,51 @@ 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(
33+
rpy_vector=np.array([0, 0, np.pi]), translation=[0, 0, 0] # type: ignore
34+
)
35+
return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates)
2936

3037
def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]:
3138
waypoints = []
3239
for i in range(num_waypoints + 1):
33-
t = i / (num_waypoints + 1)
40+
t = i / (num_waypoints)
3441
waypoints.append(start_pose.interpolate(end_pose, t))
35-
waypoints.append(end_pose)
3642
return waypoints
3743

3844
def step(self, action: dict) -> dict:
3945
return self.env.step(action)[0]
4046

4147
def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]:
4248
end_eff_pose = self.unwrapped.robot.get_cartesian_position()
43-
4449
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
4850
goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore
49-
5051
return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints)
5152

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

5958
def approach(self, geom_name: str):
60-
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=5)
59+
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60)
6160
self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
6261

6362
def grasp(self, geom_name: str):
6463

65-
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=15)
64+
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.01, num_waypoints=60)
6665
self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
6766

6867
self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED))
6968

70-
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=20)
69+
waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60)
7170
self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)
7271

7372
def move_home(self):
7473
end_eff_pose = self.unwrapped.robot.get_cartesian_position()
75-
waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=10)
74+
waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=60)
7675
self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)
7776

7877
def pickup(self, geom_name: str):
@@ -82,15 +81,17 @@ def pickup(self, geom_name: str):
8281

8382

8483
def main():
85-
env = gym.make(
86-
"rcs/FR3SimplePickUpSim-v0",
84+
env = FR3SimplePickUpSimEnvCreator()(
8785
render_mode="human",
8886
delta_actions=False,
8987
)
90-
env.reset()
91-
print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore
92-
controller = PickUpDemo(env)
93-
controller.pickup("box_geom")
88+
# wait for gui to open
89+
sleep(3)
90+
for _ in range(100):
91+
env.reset()
92+
print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore
93+
controller = PickUpDemo(env)
94+
controller.pickup("box_geom")
9495

9596

9697
if __name__ == "__main__":

‎examples/fr3_env_cartesian_control.py‎

Lines changed: 0 additions & 49 deletions
This file was deleted.

0 commit comments

Comments
 (0)