Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 10 additions & 7 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,12 @@ from rcs.camera.sim import SimCameraSet
from rcs.envs.base import (
CameraSetWrapper,
ControlMode,
CoverWrapper,
GripperWrapper,
RelativeActionSpace,
RelativeTo,
RobotEnv,
RobotWrapper,
SimEnv,
)
from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper
from rcs.envs.utils import (
Expand Down Expand Up @@ -82,36 +84,37 @@ if __name__ == "__main__":

# base env
robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
env: gym.Env = RobotEnv(robot, ControlMode.CARTESIAN_TQuat)
env: gym.Env = SimEnv(simulation)
env = RobotWrapper(env, robot, ControlMode.CARTESIAN_TQuat)

# gripper
gripper = sim.SimGripper(simulation, gripper_cfg)
env = GripperWrapper(env, gripper, binary=True)

env = RobotSimWrapper(env, simulation)
env = GripperWrapperSim(env, gripper)
env = RobotSimWrapper(env)
env = GripperWrapperSim(env)

# camera
camera_set = SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True)
env = CameraSetWrapper(env, camera_set, include_depth=True)
env = CameraSetWrapper(env, camera_set, include_depth=True) # type: ignore

# relative actions bounded by 10cm translation and 10 degree rotation
env = RelativeActionSpace(env, max_mov=(0.1, np.deg2rad(10)), relative_to=RelativeTo.LAST_STEP)
env = CoverWrapper(env)

env.get_wrapper_attr("sim").open_gui()
# wait for gui to open
sleep(1)
env.reset()

# access low level robot api to get current cartesian position
print(env.unwrapped.robot.get_cartesian_position())
print(env.get_wrapper_attr("robot").get_cartesian_position())

for _ in range(10):
# move 1cm in x direction (forward) and close gripper
act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": [0]}
obs, reward, terminated, truncated, info = env.step(act)
print(obs)

```

> **Note:** This and other examples can be found in the [`examples/`]() folder.
Expand Down
154 changes: 154 additions & 0 deletions assets/fr3/mjcf/fr3_1.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,154 @@
<mujoco>
<worldbody>
<frame quat="0 0 0 1" pos="1 0 0">
<body name="base_1" childclass="fr3" gravcomp="1">
<body name="fr3_link0_1" gravcomp="1">
<geom mesh="fr3_link0_0" material="black" class="visual"/>
<geom mesh="fr3_link0_1" material="white" class="visual"/>
<geom mesh="fr3_link0_2" material="white" class="visual"/>
<geom mesh="fr3_link0_3" material="white" class="visual"/>
<geom mesh="fr3_link0_4" material="white" class="visual"/>
<geom mesh="fr3_link0_5" material="red" class="visual"/>
<geom mesh="fr3_link0_6" material="black" class="visual"/>
<geom name="fr3_link0_collision_1" mesh="fr3_link0_coll" class="collision"/>
<body name="fr3_link1_1" pos="0 0 0.333" gravcomp="1">
<inertial pos="4.128e-07 -0.0181251 -0.0386036" quat="0.999901 0.0021545 0.00429041 0.0132325" mass="2.92747"
diaginertia="0.0186043 0.0181194 0.00538716"/>
<joint name="fr3_joint1_1" axis="0 0 1" range="-2.7437 2.7437" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
<geom name="fr3_link1_collision_1" class="collision" mesh="fr3_link1_coll"/>
<geom material="white" mesh="fr3_link1" class="visual"/>
<body name="fr3_link2_1" quat="1 -1 0 0" gravcomp="1">
<inertial pos="0.00318289 -0.0743222 0.00881461" quat="0.356855 0.680818 -0.434812 0.469126" mass="2.93554"
diaginertia="0.0299291 0.0299291 0.0299291"/>
<joint name="fr3_joint2_1" axis="0 0 1" range="-1.7837 1.7837" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
<geom material="white" mesh="fr3_link2" class="visual"/>
<geom name="fr3_link2_collision_1" class="collision" mesh="fr3_link2_coll"/>
<body name="fr3_link3_1" pos="0 -0.316 0" quat="1 1 0 0" gravcomp="1">
<inertial pos="0.0407016 -0.00482006 -0.0289731" quat="0.950032 -0.148357 0.229935 0.150201" mass="2.2449"
diaginertia="0.0140109 0.0140109 0.0140109"/>
<joint name="fr3_joint3_1" axis="0 0 1" range="-2.9007 2.9007" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
<geom mesh="fr3_link3_0" material="white" class="visual"/>
<geom mesh="fr3_link3_1" material="black" class="visual"/>
<geom name="fr3_link3_collision_1" class="collision" mesh="fr3_link3_coll"/>
<body name="fr3_link4_1" pos="0.0825 0 0" quat="1 1 0 0" gravcomp="1">
<inertial pos="-0.0459101 0.0630493 -0.00851879" quat="0.23761 0.892458 -0.00078702 0.383484"
mass="2.6156" diaginertia="0.0206104 0.0206104 0.0206104"/>
<joint name="fr3_joint4_1" axis="0 0 1" range="-3.0421 -0.1518" actuatorfrcrange="-87 87" actuatorgravcomp="true"/>
<geom mesh="fr3_link4_0" material="white" class="visual"/>
<geom mesh="fr3_link4_1" material="black" class="visual"/>
<geom name="fr3_link4_collision_1" class="collision" mesh="fr3_link4_coll"/>
<body name="fr3_link5_1" pos="-0.0825 0.384 0" quat="1 -1 0 0" gravcomp="1">
<inertial pos="-0.00160396 0.0292536 -0.0972966" quat="0.922285 0.098826 0.0982562 -0.360514"
mass="2.32712" diaginertia="0.0182879 0.0182879 0.0182879"/>
<joint name="fr3_joint5_1" axis="0 0 1" range="-2.8065 2.8065" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
<geom mesh="fr3_link5_0" material="white" class="visual"/>
<geom mesh="fr3_link5_1" material="white" class="visual"/>
<geom mesh="fr3_link5_2" material="black" class="visual"/>
<geom name="fr3_link5_collision_1" class="collision" mesh="fr3_link5_coll"/>
<body name="fr3_link6_1" quat="1 1 0 0" gravcomp="1">
<inertial pos="0.0597131 -0.0410295 -0.0101693" quat="0.593933 0.525442 0.520644 0.316361"
mass="1.81704" diaginertia="0.00483538 0.00483538 0.00483538"/>
<joint name="fr3_joint6_1" axis="0 0 1" range="0.5445 4.5169" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
<geom mesh="fr3_link6_0" material="button_green" class="visual"/>
<geom mesh="fr3_link6_1" material="white" class="visual"/>
<geom mesh="fr3_link6_2" material="white" class="visual"/>
<geom mesh="fr3_link6_3" material="gray" class="visual"/>
<geom mesh="fr3_link6_4" material="button_red" class="visual"/>
<geom mesh="fr3_link6_5" material="white" class="visual"/>
<geom mesh="fr3_link6_6" material="black" class="visual"/>
<geom mesh="fr3_link6_7" material="button_blue" class="visual"/>
<geom name="fr3_link6_collision_1" class="collision" mesh="fr3_link6_coll"/>
<body name="fr3_link7_1" pos="0.088 0 0" quat="1 1 0 0" gravcomp="1">
<inertial pos="0.00452258 0.00862619 -0.0161633" quat="0.120255 0.394761 -0.799132 0.437139"
mass="0.627143" diaginertia="3.076e-07 3.076e-07 3.076e-07"/>
<joint name="fr3_joint7_1" axis="0 0 1" range="-3.0159 3.0159" actuatorfrcrange="-12 12" actuatorgravcomp="true"/>
<geom mesh="fr3_link7_0" material="black" class="visual"/>
<geom mesh="fr3_link7_1" material="white" class="visual"/>
<geom mesh="fr3_link7_2" material="white" class="visual"/>
<geom mesh="fr3_link7_3" material="black" class="visual"/>
<geom name="fr3_link7_collision_1" class="collision" mesh="fr3_link7_coll"/>
<site name="attachment_site_1" pos="0 0 0.107"/>
<body name="hand_1" pos="0 0 0.107" quat="0.9238795 0 0 -0.3826834" gravcomp="1">
<geom mesh="Panda_RealSenseD435_Camera_Mount" class="visual" pos="0.065 0.03 0" euler="1.57 3.142 0"/>
<geom mesh="Panda_RealSenseD435_Camera_Mount" class="collision"/>
<body name="d435i_1" childclass="d435i" pos="0.051 0 0.065" euler="0 0 -1.5707" gravcomp="1">
<geom mesh="d435i_0" material="IR_Lens" class="visual_d435i"/>
<geom mesh="d435i_1" material="IR_Emitter_Lens" class="visual_d435i"/>
<geom mesh="d435i_2" material="IR_Rim" class="visual_d435i"/>
<geom mesh="d435i_3" material="IR_Lens" class="visual_d435i"/>
<geom mesh="d435i_4" material="Cameras_Gray" class="visual_d435i"/>
<geom mesh="d435i_5" material="Black_Acrylic" class="visual_d435i"/>
<geom mesh="d435i_6" material="Black_Acrylic" class="visual_d435i"/>
<geom mesh="d435i_7" material="RGB_Pupil" class="visual_d435i"/>
<geom mesh="d435i_8" mass="0.072" material="Metal_Casing" class="visual_d435i"/>
<geom class="collision_d435i" type="capsule" mesh="d435i_8" name="d435i_collision_1"/>
<camera name="wrist_1" mode="fixed" pos="0.1 0 0" quat="0 1 0 0" resolution="1920 1080"/>
</body>
<inertial mass="0.73" pos="-0.01 0 0.03" diaginertia="0.001 0.0025 0.0017"/>
<geom mesh="franka_hand_0" material="white" class="visual"/>
<geom mesh="franka_hand_1" material="black" class="visual"/>
<geom mesh="franka_hand_2" material="black" class="visual"/>
<geom mesh="franka_hand_3" material="white" class="visual"/>
<geom mesh="franka_hand_4" material="white" class="visual"/>
<geom mesh="franka_hand_coll" class="collision" name="hand_c_1"/>
<site name="tcp_1" pos="0 0 0.1034" rgba="0 0 0 0.25"/>
<body name="left_finger_1" pos="0 0 0.0584" gravcomp="1">
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
<joint name="finger_joint1_1" class="finger"/>
<geom mesh="finger_0" material="white" class="visual"/>
<geom mesh="finger_1" material="black" class="visual"/>
<geom mesh="finger_0" class="collision" name="finger_0_left_1"/>
<geom class="fingertip_pad_collision_1" name="fingertip_pad_collision_1_left_1" friction="2 0.05 0.0001"/>
<geom class="fingertip_pad_collision_2" name="fingertip_pad_collision_2_left_1" friction="2 0.05 0.0001"/>
<geom class="fingertip_pad_collision_3" name="fingertip_pad_collision_3_left_1" friction="2 0.05 0.0001"/>
<geom class="fingertip_pad_collision_4" name="fingertip_pad_collision_4_left_1" friction="2 0.05 0.0001"/>
<geom class="fingertip_pad_collision_5" name="fingertip_pad_collision_5_left_1" friction="2 0.05 0.0001"/>
</body>
<body name="right_finger_1" pos="0 0 0.0584" quat="0 0 0 1" gravcomp="1">
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
<joint name="finger_joint2_1" class="finger"/>
<geom mesh="finger_0" material="white" class="visual"/>
<geom mesh="finger_1" material="black" class="visual"/>
<geom mesh="finger_0" class="collision" name="finger_0_right_1"/>
<geom class="fingertip_pad_collision_1" name="fingertip_pad_collision_1_right_1" friction="2 0.05 0.0001"/>
<geom class="fingertip_pad_collision_2" name="fingertip_pad_collision_2_right_1" friction="2 0.05 0.0001"/>
<geom class="fingertip_pad_collision_3" name="fingertip_pad_collision_3_right_1" friction="2 0.05 0.0001"/>
<geom class="fingertip_pad_collision_4" name="fingertip_pad_collision_4_right_1" friction="2 0.05 0.0001"/>
<geom class="fingertip_pad_collision_5" name="fingertip_pad_collision_5_right_1" friction="2 0.05 0.0001"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</frame>
</worldbody>

<tendon>
<fixed name="split_1">
<joint joint="finger_joint1_1" coef="0.5"/>
<joint joint="finger_joint2_1" coef="0.5"/>
</fixed>
</tendon>

<equality>
<joint joint1="finger_joint1_1" joint2="finger_joint2_1" solimp="0.95 0.99 0.001" solref="0.005 1"/>
</equality>
<actuator>
<position class="fr3" name="fr3_joint1_1" joint="fr3_joint1_1" kp="4500" kv="450"/>
<position class="fr3" name="fr3_joint2_1" joint="fr3_joint2_1" kp="4500" kv="450"/>
<position class="fr3" name="fr3_joint3_1" joint="fr3_joint3_1" kp="3500" kv="350"/>
<position class="fr3" name="fr3_joint4_1" joint="fr3_joint4_1" kp="3500" kv="350"/>
<position class="fr3" name="fr3_joint5_1" joint="fr3_joint5_1" kp="2000" kv="200"/>
<position class="fr3" name="fr3_joint6_1" joint="fr3_joint6_1" kp="2000" kv="200"/>
<position class="fr3" name="fr3_joint7_1" joint="fr3_joint7_1" kp="2000" kv="200"/>
<!-- Remap original ctrlrange (0, 0.04) to (0, 255): 0.04 * 100 / 255 = 0.01568627451 -->
<general class="fr3" name="actuator8_1" tendon="split_1" forcerange="-100 100" ctrlrange="0 255"
gainprm="0.01568627451 0 0" biasprm="0 -100 -10"/>
</actuator>
</mujoco>
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_2.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_3.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_4.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_5.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_6.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_7.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/d435i_8.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/finger_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/finger_1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link0.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link0_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link0_1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link0_2.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link0_3.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link0_4.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link0_5.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link0_6.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link1.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link2.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link2.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link3.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link3_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link3_1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link4.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link4_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link4_1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link5.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link5_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link5_1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link5_2.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6_1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6_2.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6_3.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6_4.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6_5.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6_6.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link6_7.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link7.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link7_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link7_1.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link7_2.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/fr3_link7_3.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/franka_hand.stl
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/franka_hand_0.obj
1 change: 1 addition & 0 deletions assets/scenes/fr3_dual_arm/assets/franka_hand_1.obj
Loading
Loading