diff --git a/.gitattributes b/.gitattributes deleted file mode 100644 index ab855174..00000000 --- a/.gitattributes +++ /dev/null @@ -1,2 +0,0 @@ -.stl filter=lfs diff=lfs merge=lfs -text -.obj filter=lfs diff=lfs merge=lfs -text diff --git a/assets/scenes/fr3_empty_world/fr3.urdf b/assets/scenes/fr3_empty_world/robot.urdf similarity index 100% rename from assets/scenes/fr3_empty_world/fr3.urdf rename to assets/scenes/fr3_empty_world/robot.urdf diff --git a/assets/scenes/fr3_empty_world/fr3_0.xml b/assets/scenes/fr3_empty_world/robot.xml similarity index 100% rename from assets/scenes/fr3_empty_world/fr3_0.xml rename to assets/scenes/fr3_empty_world/robot.xml diff --git a/assets/scenes/fr3_empty_world/scene.xml b/assets/scenes/fr3_empty_world/scene.xml index 1199a97b..128dc17b 100644 --- a/assets/scenes/fr3_empty_world/scene.xml +++ b/assets/scenes/fr3_empty_world/scene.xml @@ -1,6 +1,6 @@ - + diff --git a/assets/scenes/fr3_simple_pick_up/fr3.urdf b/assets/scenes/fr3_simple_pick_up/robot.urdf similarity index 100% rename from assets/scenes/fr3_simple_pick_up/fr3.urdf rename to assets/scenes/fr3_simple_pick_up/robot.urdf diff --git a/assets/scenes/fr3_simple_pick_up/fr3_0.xml b/assets/scenes/fr3_simple_pick_up/robot.xml similarity index 100% rename from assets/scenes/fr3_simple_pick_up/fr3_0.xml rename to assets/scenes/fr3_simple_pick_up/robot.xml diff --git a/assets/scenes/fr3_simple_pick_up/scene.xml b/assets/scenes/fr3_simple_pick_up/scene.xml index 274e2089..716bfce5 100644 --- a/assets/scenes/fr3_simple_pick_up/scene.xml +++ b/assets/scenes/fr3_simple_pick_up/scene.xml @@ -1,6 +1,6 @@ - + diff --git a/assets/scenes/so101_empty_world/assets b/assets/scenes/so101_empty_world/assets new file mode 120000 index 00000000..bfad3f68 --- /dev/null +++ b/assets/scenes/so101_empty_world/assets @@ -0,0 +1 @@ +../../so101/stl \ No newline at end of file diff --git a/assets/scenes/so101_empty_world/robot.urdf b/assets/scenes/so101_empty_world/robot.urdf new file mode 100644 index 00000000..5dd02d24 --- /dev/null +++ b/assets/scenes/so101_empty_world/robot.urdf @@ -0,0 +1,460 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + \ No newline at end of file diff --git a/assets/scenes/so101_empty_world/robot.xml b/assets/scenes/so101_empty_world/robot.xml new file mode 120000 index 00000000..1c042fd7 --- /dev/null +++ b/assets/scenes/so101_empty_world/robot.xml @@ -0,0 +1 @@ +../../so101/mjcf/so101.xml \ No newline at end of file diff --git a/assets/scenes/so101_empty_world/scene.xml b/assets/scenes/so101_empty_world/scene.xml new file mode 100644 index 00000000..6dd1be24 --- /dev/null +++ b/assets/scenes/so101_empty_world/scene.xml @@ -0,0 +1,25 @@ + + + \ No newline at end of file diff --git a/assets/scenes/xarm7_empty_world/assets b/assets/scenes/xarm7_empty_world/assets new file mode 120000 index 00000000..800d8259 --- /dev/null +++ b/assets/scenes/xarm7_empty_world/assets @@ -0,0 +1 @@ +../../xarm7/stl \ No newline at end of file diff --git a/assets/scenes/xarm7_empty_world/robot.xml b/assets/scenes/xarm7_empty_world/robot.xml new file mode 120000 index 00000000..421709b9 --- /dev/null +++ b/assets/scenes/xarm7_empty_world/robot.xml @@ -0,0 +1 @@ +../../xarm7/mjcf/xarm7.xml \ No newline at end of file diff --git a/assets/scenes/xarm7_empty_world/scene.xml b/assets/scenes/xarm7_empty_world/scene.xml new file mode 100644 index 00000000..0d9d22f3 --- /dev/null +++ b/assets/scenes/xarm7_empty_world/scene.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/assets/so101/mjcf/so101.xml b/assets/so101/mjcf/so101.xml new file mode 100644 index 00000000..0b0f4f88 --- /dev/null +++ b/assets/so101/mjcf/so101.xml @@ -0,0 +1,158 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/assets/so101/stl/base_motor_holder_so101_v1.stl b/assets/so101/stl/base_motor_holder_so101_v1.stl new file mode 100644 index 00000000..cac2b8dd Binary files /dev/null and b/assets/so101/stl/base_motor_holder_so101_v1.stl differ diff --git a/assets/so101/stl/base_so101_v2.stl b/assets/so101/stl/base_so101_v2.stl new file mode 100644 index 00000000..ed10aa61 Binary files /dev/null and b/assets/so101/stl/base_so101_v2.stl differ diff --git a/assets/so101/stl/motor_holder_so101_base_v1.stl b/assets/so101/stl/motor_holder_so101_base_v1.stl new file mode 100644 index 00000000..9eaf6724 Binary files /dev/null and b/assets/so101/stl/motor_holder_so101_base_v1.stl differ diff --git a/assets/so101/stl/motor_holder_so101_wrist_v1.stl b/assets/so101/stl/motor_holder_so101_wrist_v1.stl new file mode 100644 index 00000000..d0aa18b1 Binary files /dev/null and b/assets/so101/stl/motor_holder_so101_wrist_v1.stl differ diff --git a/assets/so101/stl/moving_jaw_so101_v1.stl b/assets/so101/stl/moving_jaw_so101_v1.stl new file mode 100644 index 00000000..7e873668 Binary files /dev/null and b/assets/so101/stl/moving_jaw_so101_v1.stl differ diff --git a/assets/so101/stl/rotation_pitch_so101_v1.stl b/assets/so101/stl/rotation_pitch_so101_v1.stl new file mode 100644 index 00000000..5c05dee5 Binary files /dev/null and b/assets/so101/stl/rotation_pitch_so101_v1.stl differ diff --git a/assets/so101/stl/sts3215_03a_no_horn_v1.stl b/assets/so101/stl/sts3215_03a_no_horn_v1.stl new file mode 100644 index 00000000..2a895977 Binary files /dev/null and b/assets/so101/stl/sts3215_03a_no_horn_v1.stl differ diff --git a/assets/so101/stl/sts3215_03a_v1.stl b/assets/so101/stl/sts3215_03a_v1.stl new file mode 100644 index 00000000..fe05403d Binary files /dev/null and b/assets/so101/stl/sts3215_03a_v1.stl differ diff --git a/assets/so101/stl/under_arm_so101_v1.stl b/assets/so101/stl/under_arm_so101_v1.stl new file mode 100644 index 00000000..480e0583 Binary files /dev/null and b/assets/so101/stl/under_arm_so101_v1.stl differ diff --git a/assets/so101/stl/upper_arm_so101_v1.stl b/assets/so101/stl/upper_arm_so101_v1.stl new file mode 100644 index 00000000..9c0f9b27 Binary files /dev/null and b/assets/so101/stl/upper_arm_so101_v1.stl differ diff --git a/assets/so101/stl/waveshare_mounting_plate_so101_v2.stl b/assets/so101/stl/waveshare_mounting_plate_so101_v2.stl new file mode 100644 index 00000000..4dba1f22 Binary files /dev/null and b/assets/so101/stl/waveshare_mounting_plate_so101_v2.stl differ diff --git a/assets/so101/stl/wrist_roll_follower_so101_v1.stl b/assets/so101/stl/wrist_roll_follower_so101_v1.stl new file mode 100644 index 00000000..123ce374 Binary files /dev/null and b/assets/so101/stl/wrist_roll_follower_so101_v1.stl differ diff --git a/assets/so101/stl/wrist_roll_pitch_so101_v2.stl b/assets/so101/stl/wrist_roll_pitch_so101_v2.stl new file mode 100644 index 00000000..ac5172a3 Binary files /dev/null and b/assets/so101/stl/wrist_roll_pitch_so101_v2.stl differ diff --git a/assets/so101/urdf/so101.urdf b/assets/so101/urdf/so101.urdf new file mode 100644 index 00000000..5dd02d24 --- /dev/null +++ b/assets/so101/urdf/so101.urdf @@ -0,0 +1,460 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + \ No newline at end of file diff --git a/assets/xarm7/mjcf/xarm7.xml b/assets/xarm7/mjcf/xarm7.xml new file mode 100644 index 00000000..6b4ed3d4 --- /dev/null +++ b/assets/xarm7/mjcf/xarm7.xml @@ -0,0 +1,174 @@ + + + + \ No newline at end of file diff --git a/assets/xarm7/stl/base_link.stl b/assets/xarm7/stl/base_link.stl new file mode 100644 index 00000000..6a3263b5 Binary files /dev/null and b/assets/xarm7/stl/base_link.stl differ diff --git a/assets/xarm7/stl/end_tool.stl b/assets/xarm7/stl/end_tool.stl new file mode 100644 index 00000000..e2324f41 Binary files /dev/null and b/assets/xarm7/stl/end_tool.stl differ diff --git a/assets/xarm7/stl/end_tool_convex.stl b/assets/xarm7/stl/end_tool_convex.stl new file mode 100644 index 00000000..06323303 Binary files /dev/null and b/assets/xarm7/stl/end_tool_convex.stl differ diff --git a/assets/xarm7/stl/left_finger.stl b/assets/xarm7/stl/left_finger.stl new file mode 100644 index 00000000..41f2c0d7 Binary files /dev/null and b/assets/xarm7/stl/left_finger.stl differ diff --git a/assets/xarm7/stl/left_inner_knuckle.stl b/assets/xarm7/stl/left_inner_knuckle.stl new file mode 100644 index 00000000..abeff0ce Binary files /dev/null and b/assets/xarm7/stl/left_inner_knuckle.stl differ diff --git a/assets/xarm7/stl/left_outer_knuckle.stl b/assets/xarm7/stl/left_outer_knuckle.stl new file mode 100644 index 00000000..639add9e Binary files /dev/null and b/assets/xarm7/stl/left_outer_knuckle.stl differ diff --git a/assets/xarm7/stl/link1.stl b/assets/xarm7/stl/link1.stl new file mode 100644 index 00000000..a5ca309d Binary files /dev/null and b/assets/xarm7/stl/link1.stl differ diff --git a/assets/xarm7/stl/link1_convex.stl b/assets/xarm7/stl/link1_convex.stl new file mode 100644 index 00000000..fc55f522 Binary files /dev/null and b/assets/xarm7/stl/link1_convex.stl differ diff --git a/assets/xarm7/stl/link1_ring.stl b/assets/xarm7/stl/link1_ring.stl new file mode 100644 index 00000000..c772fe3e Binary files /dev/null and b/assets/xarm7/stl/link1_ring.stl differ diff --git a/assets/xarm7/stl/link1_trunk.stl b/assets/xarm7/stl/link1_trunk.stl new file mode 100644 index 00000000..23874227 Binary files /dev/null and b/assets/xarm7/stl/link1_trunk.stl differ diff --git a/assets/xarm7/stl/link2.stl b/assets/xarm7/stl/link2.stl new file mode 100644 index 00000000..1539adb6 Binary files /dev/null and b/assets/xarm7/stl/link2.stl differ diff --git a/assets/xarm7/stl/link2_convex.stl b/assets/xarm7/stl/link2_convex.stl new file mode 100644 index 00000000..ceab9340 Binary files /dev/null and b/assets/xarm7/stl/link2_convex.stl differ diff --git a/assets/xarm7/stl/link2_ring.stl b/assets/xarm7/stl/link2_ring.stl new file mode 100644 index 00000000..13c3783b Binary files /dev/null and b/assets/xarm7/stl/link2_ring.stl differ diff --git a/assets/xarm7/stl/link2_trunk.stl b/assets/xarm7/stl/link2_trunk.stl new file mode 100644 index 00000000..776bdbf8 Binary files /dev/null and b/assets/xarm7/stl/link2_trunk.stl differ diff --git a/assets/xarm7/stl/link3.stl b/assets/xarm7/stl/link3.stl new file mode 100644 index 00000000..23926351 Binary files /dev/null and b/assets/xarm7/stl/link3.stl differ diff --git a/assets/xarm7/stl/link3_convex.stl b/assets/xarm7/stl/link3_convex.stl new file mode 100644 index 00000000..a3e3dbba Binary files /dev/null and b/assets/xarm7/stl/link3_convex.stl differ diff --git a/assets/xarm7/stl/link3_ring.stl b/assets/xarm7/stl/link3_ring.stl new file mode 100644 index 00000000..2f1cecfa Binary files /dev/null and b/assets/xarm7/stl/link3_ring.stl differ diff --git a/assets/xarm7/stl/link3_trunk.stl b/assets/xarm7/stl/link3_trunk.stl new file mode 100644 index 00000000..15fc1827 Binary files /dev/null and b/assets/xarm7/stl/link3_trunk.stl differ diff --git a/assets/xarm7/stl/link4.stl b/assets/xarm7/stl/link4.stl new file mode 100644 index 00000000..a24cc91d Binary files /dev/null and b/assets/xarm7/stl/link4.stl differ diff --git a/assets/xarm7/stl/link4_convex.stl b/assets/xarm7/stl/link4_convex.stl new file mode 100644 index 00000000..93d6999b Binary files /dev/null and b/assets/xarm7/stl/link4_convex.stl differ diff --git a/assets/xarm7/stl/link4_ring.stl b/assets/xarm7/stl/link4_ring.stl new file mode 100644 index 00000000..cb18e3b3 Binary files /dev/null and b/assets/xarm7/stl/link4_ring.stl differ diff --git a/assets/xarm7/stl/link4_trunk.stl b/assets/xarm7/stl/link4_trunk.stl new file mode 100644 index 00000000..2a4418e7 Binary files /dev/null and b/assets/xarm7/stl/link4_trunk.stl differ diff --git a/assets/xarm7/stl/link5.stl b/assets/xarm7/stl/link5.stl new file mode 100644 index 00000000..7f91e37c Binary files /dev/null and b/assets/xarm7/stl/link5.stl differ diff --git a/assets/xarm7/stl/link5_convex.stl b/assets/xarm7/stl/link5_convex.stl new file mode 100644 index 00000000..ea87d065 Binary files /dev/null and b/assets/xarm7/stl/link5_convex.stl differ diff --git a/assets/xarm7/stl/link5_ring.stl b/assets/xarm7/stl/link5_ring.stl new file mode 100644 index 00000000..c88d24d3 Binary files /dev/null and b/assets/xarm7/stl/link5_ring.stl differ diff --git a/assets/xarm7/stl/link5_trunk.stl b/assets/xarm7/stl/link5_trunk.stl new file mode 100644 index 00000000..a27d05a3 Binary files /dev/null and b/assets/xarm7/stl/link5_trunk.stl differ diff --git a/assets/xarm7/stl/link6.stl b/assets/xarm7/stl/link6.stl new file mode 100644 index 00000000..04fc4805 Binary files /dev/null and b/assets/xarm7/stl/link6.stl differ diff --git a/assets/xarm7/stl/link6_convex.stl b/assets/xarm7/stl/link6_convex.stl new file mode 100644 index 00000000..d23d2846 Binary files /dev/null and b/assets/xarm7/stl/link6_convex.stl differ diff --git a/assets/xarm7/stl/link6_ring.stl b/assets/xarm7/stl/link6_ring.stl new file mode 100644 index 00000000..e7d6959b Binary files /dev/null and b/assets/xarm7/stl/link6_ring.stl differ diff --git a/assets/xarm7/stl/link6_trunk.stl b/assets/xarm7/stl/link6_trunk.stl new file mode 100644 index 00000000..da070667 Binary files /dev/null and b/assets/xarm7/stl/link6_trunk.stl differ diff --git a/assets/xarm7/stl/link7.stl b/assets/xarm7/stl/link7.stl new file mode 100644 index 00000000..0e9963c3 Binary files /dev/null and b/assets/xarm7/stl/link7.stl differ diff --git a/assets/xarm7/stl/link7_convex.stl b/assets/xarm7/stl/link7_convex.stl new file mode 100644 index 00000000..1b785e2a Binary files /dev/null and b/assets/xarm7/stl/link7_convex.stl differ diff --git a/assets/xarm7/stl/link_base.stl b/assets/xarm7/stl/link_base.stl new file mode 100644 index 00000000..555d6f2e Binary files /dev/null and b/assets/xarm7/stl/link_base.stl differ diff --git a/assets/xarm7/stl/link_base_ring.stl b/assets/xarm7/stl/link_base_ring.stl new file mode 100644 index 00000000..91b5221b Binary files /dev/null and b/assets/xarm7/stl/link_base_ring.stl differ diff --git a/assets/xarm7/stl/link_base_root.stl b/assets/xarm7/stl/link_base_root.stl new file mode 100644 index 00000000..d7af084c Binary files /dev/null and b/assets/xarm7/stl/link_base_root.stl differ diff --git a/assets/xarm7/stl/link_base_trunk.stl b/assets/xarm7/stl/link_base_trunk.stl new file mode 100644 index 00000000..b3698602 Binary files /dev/null and b/assets/xarm7/stl/link_base_trunk.stl differ diff --git a/assets/xarm7/stl/link_base_trunk_whole.stl b/assets/xarm7/stl/link_base_trunk_whole.stl new file mode 100644 index 00000000..d85a315c Binary files /dev/null and b/assets/xarm7/stl/link_base_trunk_whole.stl differ diff --git a/assets/xarm7/stl/right_finger.stl b/assets/xarm7/stl/right_finger.stl new file mode 100644 index 00000000..53f0eeca Binary files /dev/null and b/assets/xarm7/stl/right_finger.stl differ diff --git a/assets/xarm7/stl/right_inner_knuckle.stl b/assets/xarm7/stl/right_inner_knuckle.stl new file mode 100644 index 00000000..f1a486da Binary files /dev/null and b/assets/xarm7/stl/right_inner_knuckle.stl differ diff --git a/assets/xarm7/stl/right_outer_knuckle.stl b/assets/xarm7/stl/right_outer_knuckle.stl new file mode 100644 index 00000000..3099dfde Binary files /dev/null and b/assets/xarm7/stl/right_outer_knuckle.stl differ diff --git a/cmake/compile_scenes.cmake b/cmake/compile_scenes.cmake index f30e15c5..d13a2e2f 100644 --- a/cmake/compile_scenes.cmake +++ b/cmake/compile_scenes.cmake @@ -30,11 +30,6 @@ foreach(scene_dir ${scene_dirs}) # Install scene files install(FILES ${scene_mjb} DESTINATION rcs/scenes/${scene_name} COMPONENT python_package) - # Install URDF files - file(GLOB urdfs ${scene_dir}/*.urdf) - foreach(urdf ${urdfs}) - install(FILES ${urdf} DESTINATION rcs/scenes/${scene_name} COMPONENT python_package) - endforeach() endforeach() # Create a custom target that depends on all generated .mjb files diff --git a/examples/fr3_direct_control.py b/examples/fr3_direct_control.py index a428e5c8..b7be3c04 100644 --- a/examples/fr3_direct_control.py +++ b/examples/fr3_direct_control.py @@ -59,8 +59,8 @@ def main(): robot: rcs.common.Robot gripper: rcs.common.Gripper if ROBOT_INSTANCE == RobotPlatform.SIMULATION: - simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"]) - urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] + simulation = sim.Sim(rcs.scenes["fr3_empty_world"].mjb) + urdf_path = rcs.scenes["fr3_empty_world"].urdf ik = rcs.common.RL(str(urdf_path)) cfg = sim.SimRobotConfig() cfg.add_id("0") @@ -92,7 +92,7 @@ def main(): simulation.open_gui() else: - urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] + urdf_path = rcs.scenes["fr3_empty_world"].urdf ik = rcs.common.RL(str(urdf_path)) robot = hw.FR3(ROBOT_IP, ik) robot_cfg = hw.FR3Config() @@ -116,7 +116,7 @@ def main(): # 5cm in x direction robot.set_cartesian_position( - robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.05, 0, 0])) + robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.05, 0, 0])) # type: ignore ) if ROBOT_INSTANCE == RobotPlatform.SIMULATION: simulation.step_until_convergence() @@ -125,7 +125,7 @@ def main(): # 5cm in y direction robot.set_cartesian_position( - robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0, 0.05, 0])) + robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0, 0.05, 0])) # type: ignore ) if ROBOT_INSTANCE == RobotPlatform.SIMULATION: simulation.step_until_convergence() @@ -134,7 +134,7 @@ def main(): # 5cm in z direction robot.set_cartesian_position( - robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0, 0, 0.05])) + robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0, 0, 0.05])) # type: ignore ) if ROBOT_INSTANCE == RobotPlatform.SIMULATION: simulation.step_until_convergence() @@ -143,7 +143,7 @@ def main(): # rotate the arm 90 degrees around the inverted y and z axis new_pose = robot.get_cartesian_position() * rcs.common.Pose( - translation=np.array([0, 0, 0]), rpy=rcs.common.RPY(roll=0, pitch=-np.deg2rad(90), yaw=-np.deg2rad(90)) + translation=np.array([0, 0, 0]), rpy=rcs.common.RPY(roll=0, pitch=-np.deg2rad(90), yaw=-np.deg2rad(90)) # type: ignore ) robot.set_cartesian_position(new_pose) if ROBOT_INSTANCE == RobotPlatform.SIMULATION: @@ -158,7 +158,7 @@ def main(): # move 25cm towards the gripper direction robot.set_cartesian_position( - robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0, 0, 0.25])) + robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0, 0, 0.25])) # type: ignore ) if ROBOT_INSTANCE == RobotPlatform.SIMULATION: simulation.step_until_convergence() @@ -173,7 +173,7 @@ def main(): # move 25cm backward robot.set_cartesian_position( - robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0, 0, -0.25])) + robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0, 0, -0.25])) # type: ignore ) if ROBOT_INSTANCE == RobotPlatform.SIMULATION: simulation.step_until_convergence() diff --git a/examples/fr3_env_cartesian_control.py b/examples/fr3_env_cartesian_control.py index 9a460923..fecb2a16 100644 --- a/examples/fr3_env_cartesian_control.py +++ b/examples/fr3_env_cartesian_control.py @@ -1,6 +1,5 @@ import logging -from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import SimEnvCreator from rcs.envs.utils import ( @@ -8,89 +7,42 @@ default_sim_gripper_cfg, default_sim_robot_cfg, ) -from rcs_fr3.creators import RCSFR3EnvCreator -from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk -from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) -ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotPlatform.SIMULATION - - -""" -Create a .env file in the same directory as this file with the following content: -FR3_USERNAME= -FR3_PASSWORD= - -When you use a real FR3 you first need to unlock its joints using the following cli script: - -python -m rcs_fr3 unlock - -or put it into guiding mode using: - -python -m rcs_fr3 guiding-mode - -When you are done you lock it again using: - -python -m rcs_fr3 lock - -or even shut it down using: - -python -m rcs_fr3 shutdown -""" - def main(): - context_manger: ContextManager - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - user, pw = load_creds_fr3_desk() - context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) - else: - context_manger = ContextManager() - - with context_manger: - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - env_rel = RCSFR3EnvCreator()( - ip=ROBOT_IP, - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard=None, - gripper_cfg=default_fr3_hw_gripper_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - else: - env_rel = SimEnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_sim_robot_cfg(), - collision_guard=False, - gripper_cfg=default_sim_gripper_cfg(), - cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - env_rel.reset() - print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore + env_rel = SimEnvCreator()( + control_mode=ControlMode.CARTESIAN_TQuat, + robot_cfg=default_sim_robot_cfg(scene="fr3_empty_world"), + collision_guard=False, + gripper_cfg=default_sim_gripper_cfg(), + cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + env_rel.reset() + print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore + + for _ in range(100): + 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_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return for _ in range(10): - 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_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - for _ in range(10): - # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return + # move 1cm in negative x direction (backward) and open gripper + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return if __name__ == "__main__": diff --git a/examples/fr3_env_joint_control.py b/examples/fr3_env_joint_control.py index a1dd2cce..ff19708b 100644 --- a/examples/fr3_env_joint_control.py +++ b/examples/fr3_env_joint_control.py @@ -1,7 +1,6 @@ import logging import numpy as np -from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import SimEnvCreator from rcs.envs.utils import ( @@ -9,80 +8,32 @@ default_sim_gripper_cfg, default_sim_robot_cfg, ) -from rcs_fr3.creators import RCSFR3EnvCreator -from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk -from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) -ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotPlatform.SIMULATION - - -""" -Create a .env file in the same directory as this file with the following content: -FR3_USERNAME= -FR3_PASSWORD= - -When you use a real FR3 you first need to unlock its joints using the following cli script: - -python -m rcs_fr3 unlock - -or put it into guiding mode using: - -python -m rcs_fr3 guiding-mode - -When you are done you lock it again using: - -python -m rcs_fr3 lock - -or even shut it down using: - -python -m rcs_fr3 shutdown -""" - def main(): - context_manger: FCI | ContextManager - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - user, pw = load_creds_fr3_desk() - context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) - else: - context_manger = ContextManager() - with context_manger: - - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - env_rel = RCSFR3EnvCreator()( - ip=ROBOT_IP, - control_mode=ControlMode.JOINTS, - robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard=None, - gripper_cfg=default_fr3_hw_gripper_cfg(), - max_relative_movement=np.deg2rad(5), - relative_to=RelativeTo.LAST_STEP, - ) - else: - env_rel = SimEnvCreator()( - control_mode=ControlMode.JOINTS, - collision_guard=False, - robot_cfg=default_sim_robot_cfg(), - gripper_cfg=default_sim_gripper_cfg(), - cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=np.deg2rad(5), - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - + env_rel = SimEnvCreator()( + control_mode=ControlMode.JOINTS, + collision_guard=False, + robot_cfg=default_sim_robot_cfg("fr3_empty_world"), + gripper_cfg=default_sim_gripper_cfg(), + cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + for _ in range(100): + obs, info = env_rel.reset() for _ in range(10): - obs, info = env_rel.reset() - for _ in range(10): - # sample random relative action and execute it - act = env_rel.action_space.sample() - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return + # sample random relative action and execute it + act = env_rel.action_space.sample() + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return if __name__ == "__main__": diff --git a/examples/grasp_demo.py b/examples/grasp_demo.py index 113d2f80..d741597c 100644 --- a/examples/grasp_demo.py +++ b/examples/grasp_demo.py @@ -45,7 +45,7 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int # goal pose is above the object and gripper coordinate must flip z-axis (end effector base rotation is [1, 0, 0, 0]) # be careful we define identity quaternion as as [0, 0, 0, 1] # this does not work if the object is flipped - goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) + goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) diff --git a/examples/so101_env_cartesian_control.py b/examples/so101_env_cartesian_control.py new file mode 100644 index 00000000..d571bb22 --- /dev/null +++ b/examples/so101_env_cartesian_control.py @@ -0,0 +1,74 @@ +import logging + +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +def main(): + + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = ["1", "2", "3", "4", "5"] + robot_cfg.joints = [ + "1", + "2", + "3", + "4", + "5", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.SO101 + robot_cfg.attachment_site = "gripper" + robot_cfg.arm_collision_geoms = [] + robot_cfg.mjcf_scene_path = rcs.scenes["so101_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot + + gripper_cfg = sim.SimGripperConfig() + gripper_cfg.min_actuator_width = -0.17453292519943295 + gripper_cfg.max_actuator_width = 1.7453292519943295 + gripper_cfg.min_joint_width = -0.17453292519943295 + gripper_cfg.max_joint_width = 1.7453292519943295 + gripper_cfg.actuator = "6" + gripper_cfg.joint = "6" + gripper_cfg.collision_geoms = [] + gripper_cfg.collision_geoms_fingers = [] + + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + collision_guard=False, + gripper_cfg=gripper_cfg, + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + obs, info = env_rel.reset() + + act = {"tquat": [0.03, 0, 0, 0, 0, 0, 1], "gripper": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + + for _ in range(100): + for _ in range(5): + # 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_rel.step(act) + print(info, obs) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + for _ in range(5): + # move 1cm in negative x direction (backward) and open gripper + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + + +if __name__ == "__main__": + main() diff --git a/examples/so101_env_joint_control.py b/examples/so101_env_joint_control.py new file mode 100644 index 00000000..1f60ea75 --- /dev/null +++ b/examples/so101_env_joint_control.py @@ -0,0 +1,65 @@ +import logging + +import numpy as np +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +def main(): + + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = ["1", "2", "3", "4", "5"] + robot_cfg.joints = [ + "1", + "2", + "3", + "4", + "5", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.SO101 + robot_cfg.attachment_site = "gripper" + robot_cfg.arm_collision_geoms = [] + robot_cfg.mjcf_scene_path = rcs.scenes["so101_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot + + gripper_cfg = sim.SimGripperConfig() + gripper_cfg.min_actuator_width = -0.17453292519943295 + gripper_cfg.max_actuator_width = 1.7453292519943295 + gripper_cfg.min_joint_width = -0.17453292519943295 + gripper_cfg.max_joint_width = 1.7453292519943295 + gripper_cfg.actuator = "6" + gripper_cfg.joint = "6" + gripper_cfg.collision_geoms = [] + gripper_cfg.collision_geoms_fingers = [] + + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, + collision_guard=False, + gripper_cfg=gripper_cfg, + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + for _ in range(100): + obs, info = env_rel.reset() + for _ in range(10): + # sample random relative action and execute it + act = env_rel.action_space.sample() + # print(act) + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + + +if __name__ == "__main__": + main() diff --git a/examples/xarm7_env_cartesian_control.py b/examples/xarm7_env_cartesian_control.py new file mode 100644 index 00000000..c0b2e933 --- /dev/null +++ b/examples/xarm7_env_cartesian_control.py @@ -0,0 +1,72 @@ +import logging + +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import get_tcp_offset + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +def main(): + + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = [ + "act1", + "act2", + "act3", + "act4", + "act5", + "act6", + "act7", + ] + robot_cfg.joints = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_cfg.attachment_site = "attachment_site" + robot_cfg.arm_collision_geoms = [] + robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene) + robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + collision_guard=False, + gripper_cfg=None, + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + obs, info = env_rel.reset() + + for _ in range(100): + 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_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + for _ in range(10): + # move 1cm in negative x direction (backward) and open gripper + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + + +if __name__ == "__main__": + main() diff --git a/examples/xarm7_env_joint_control.py b/examples/xarm7_env_joint_control.py new file mode 100644 index 00000000..818bbc53 --- /dev/null +++ b/examples/xarm7_env_joint_control.py @@ -0,0 +1,65 @@ +import logging + +import numpy as np +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import get_tcp_offset + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +def main(): + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = [ + "act1", + "act2", + "act3", + "act4", + "act5", + "act6", + "act7", + ] + robot_cfg.joints = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_cfg.attachment_site = "attachment_site" + robot_cfg.arm_collision_geoms = [] + robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene) + robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, + collision_guard=False, + gripper_cfg=None, + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + for _ in range(100): + obs, info = env_rel.reset() + for _ in range(10): + # sample random relative action and execute it + act = env_rel.action_space.sample() + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_fr3/src/hw/FR3.h b/extensions/rcs_fr3/src/hw/FR3.h index 3cc2a799..7197f569 100644 --- a/extensions/rcs_fr3/src/hw/FR3.h +++ b/extensions/rcs_fr3/src/hw/FR3.h @@ -39,7 +39,6 @@ struct FR3Config : common::RobotConfig { std::optional load_parameters = std::nullopt; std::optional nominal_end_effector_frame = std::nullopt; std::optional world_to_robot = std::nullopt; - common::Pose tcp_offset = common::Pose::Identity(); bool async_control = false; }; @@ -113,6 +112,7 @@ class FR3 : public common::Robot { common::Pose get_base_pose_in_world_coordinates() override; void reset() override; + void close() override {}; }; } // namespace hw } // namespace rcs diff --git a/extensions/rcs_fr3/src/hw/FrankaHand.h b/extensions/rcs_fr3/src/hw/FrankaHand.h index 40895160..7de4753d 100644 --- a/extensions/rcs_fr3/src/hw/FrankaHand.h +++ b/extensions/rcs_fr3/src/hw/FrankaHand.h @@ -76,6 +76,7 @@ class FrankaHand : public common::Gripper { void grasp() override; void open() override; void shut() override; + void close() override {}; }; } // namespace hw } // namespace rcs diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 5fcb37c7..e5fefcfa 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -66,7 +66,6 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("nominal_end_effector_frame", &rcs::hw::FR3Config::nominal_end_effector_frame) .def_readwrite("world_to_robot", &rcs::hw::FR3Config::world_to_robot) - .def_readwrite("tcp_offset", &rcs::hw::FR3Config::tcp_offset) .def_readwrite("async_control", &rcs::hw::FR3Config::async_control); py::object gripper_config = diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index 8e8a8062..c2311b46 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -86,7 +86,6 @@ class FR3Config(rcs._core.common.RobotConfig): load_parameters: FR3Load | None nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float - tcp_offset: rcs._core.common.Pose world_to_robot: rcs._core.common.Pose | None def __init__(self) -> None: ... diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 68df13db..76369ef1 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -16,7 +16,6 @@ RobotEnv, ) from rcs.envs.creators import RCSHardwareEnvCreator -from rcs.envs.sim import CollisionGuard from rcs.hand.tilburg_hand import TilburgHand from rcs_fr3 import hw from rcs_fr3.envs import FR3HW @@ -64,7 +63,7 @@ def __call__( # type: ignore gym.Env: The configured hardware environment for the FR3 robot. """ if urdf_path is None: - urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] + urdf_path = rcs.scenes["fr3_empty_world"].urdf ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None robot = hw.FR3(ip, ik) robot.set_parameters(robot_cfg) @@ -85,19 +84,19 @@ def __call__( # type: ignore logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) - if collision_guard is not None: - assert urdf_path is not None - env = CollisionGuard.env_from_xml_paths( - env, - str(rcs.scenes.get(str(collision_guard), collision_guard)), - str(urdf_path), - gripper=True, - check_home_collision=False, - control_mode=control_mode, - tcp_offset=rcs.common.Pose(rcs.common.FrankaHandTCPOffset()), - sim_gui=True, - truncate_on_collision=False, - ) + # if collision_guard is not None: + # assert urdf_path is not None + # env = CollisionGuard.env_from_xml_paths( + # env, + # str(rcs.scenes.get(str(collision_guard), collision_guard)), + # str(urdf_path), + # gripper=True, + # check_home_collision=False, + # control_mode=control_mode, + # tcp_offset=rcs.common.Pose(rcs.common.FrankaHandTCPOffset()), + # sim_gui=True, + # truncate_on_collision=False, + # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) @@ -116,7 +115,7 @@ def __call__( # type: ignore urdf_path: str | PathLike | None = None, ) -> gym.Env: - urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] + urdf_path = rcs.scenes["fr3_empty_world"].urdf ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None robots: dict[str, hw.FR3] = {} for ip in ips: diff --git a/extensions/rcs_realsense/src/rcs_realsense/calibration.py b/extensions/rcs_realsense/src/rcs_realsense/calibration.py index f532a2ba..647419bb 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/calibration.py +++ b/extensions/rcs_realsense/src/rcs_realsense/calibration.py @@ -27,7 +27,7 @@ def __init__(self, camera_name: str): ) # None self.camera_name = camera_name self.tag_to_world = common.Pose( - rpy_vector=np.array([np.pi, 0, -np.pi / 2]), translation=np.array([0.145, 0, 0]) + rpy_vector=np.array([np.pi, 0, -np.pi / 2]), translation=np.array([0.145, 0, 0]) # type: ignore ).pose_matrix() def calibrate( diff --git a/extensions/rcs_realsense/src/rcs_realsense/camera.py b/extensions/rcs_realsense/src/rcs_realsense/camera.py index 01d36e4c..bae408e5 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/camera.py +++ b/extensions/rcs_realsense/src/rcs_realsense/camera.py @@ -196,8 +196,8 @@ def enable_device(self, camera_name: str, device_info: RealSenseDeviceInfo, enab pipeline_profile, device_info, depth_scale=sensor.get_depth_scale(), - color_intrinsics=color_intrinsics, - depth_intrinsics=depth_intrinsics, + color_intrinsics=color_intrinsics, # type: ignore + depth_intrinsics=depth_intrinsics, # type: ignore depth_to_color=common.Pose( translation=depth_to_color.translation, rotation=np.array(depth_to_color.rotation).reshape(3, 3) # type: ignore ), @@ -283,7 +283,7 @@ def to_ts(frame: rs.frame) -> float: ), timestamp=to_ts(frame), intrinsics=device.depth_intrinsics, - extrinsics=depth_extrinsics, + extrinsics=depth_extrinsics, # type: ignore ) elif rs.stream.accel == stream.stream_type(): frame = frameset.first(stream.stream_index()) diff --git a/extensions/rcs_realsense/src/rcs_realsense/utils.py b/extensions/rcs_realsense/src/rcs_realsense/utils.py index b63842bb..2797fd29 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/utils.py +++ b/extensions/rcs_realsense/src/rcs_realsense/utils.py @@ -16,3 +16,13 @@ def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | No } calibration_strategy = {name: typing.cast(CalibrationStrategy, FR3BaseArucoCalibration(name)) for name in name2id} return RealSenseCameraSet(cameras=cameras, calibration_strategy=calibration_strategy) + + +def default_realsense_dummy_calibration(name2id: dict[str, str] | None) -> RealSenseCameraSet | None: + if name2id is None: + return None + cameras = { + name: common.BaseCameraConfig(identifier=id, resolution_width=640, resolution_height=480, frame_rate=30) + for name, id in name2id.items() + } + return RealSenseCameraSet(cameras=cameras) diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py index 98ae5404..b48ee3c4 100644 --- a/extensions/rcs_so101/src/rcs_so101/creators.py +++ b/extensions/rcs_so101/src/rcs_so101/creators.py @@ -1,23 +1,7 @@ import logging -import typing -from os import PathLike -from pathlib import Path -from typing import Type import gymnasium as gym -from gymnasium.envs.registration import EnvCreator -from lerobot.common.robots import make_robot_from_config -from lerobot.common.robots.so101_follower.config_so101_follower import ( - SO101FollowerConfig, -) -from lerobot.common.teleoperators.so101_leader.config_so101_leader import ( - SO101LeaderConfig, -) -from lerobot.common.teleoperators.so101_leader.so101_leader import SO101Leader -from lerobot.common.teleoperators.utils import make_teleoperator_from_config from rcs.camera.hw import HardwareCameraSet -from rcs.camera.interface import BaseCameraSet -from rcs.camera.sim import SimCameraSet from rcs.envs.base import ( CameraSetWrapper, ControlMode, @@ -27,12 +11,9 @@ RobotEnv, ) from rcs.envs.creators import RCSHardwareEnvCreator -from rcs.envs.sim import CollisionGuard, GripperWrapperSim, RobotSimWrapper, SimWrapper -from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig -from rcs_so101.hw import SO101, S0101Gripper +from rcs_so101.hw import SO101, SO101Config, SO101Gripper import rcs -from rcs import common, sim logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -41,115 +22,43 @@ class RCSSO101EnvCreator(RCSHardwareEnvCreator): def __call__( # type: ignore self, - id: str, - port: str, - urdf_path: str, - calibration_dir: PathLike | str | None = None, + robot_cfg: SO101Config, + control_mode: ControlMode, camera_set: HardwareCameraSet | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, ) -> gym.Env: - if isinstance(calibration_dir, str): - calibration_dir = Path(calibration_dir) - cfg = SO101FollowerConfig(id=id, calibration_dir=calibration_dir, port=port) - hf_robot = make_robot_from_config(cfg) - robot = SO101(hf_robot, urdf_path=urdf_path) - env: gym.Env = RobotEnv(robot, ControlMode.JOINTS) - # env = FR3HW(env) - - gripper = S0101Gripper(hf_robot) + ik = rcs.common.Pin( + robot_cfg.kinematic_model_path, + robot_cfg.attachment_site, + urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), + ) + robot = SO101(robot_cfg=robot_cfg, ik=ik) + env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True) + + gripper = SO101Gripper(robot.hf_robot, robot) env = GripperWrapper(env, gripper, binary=False) if camera_set is not None: camera_set.start() camera_set.wait_for_frames() logger.info("CameraSet started") - env = CameraSetWrapper(env, camera_set) - - if max_relative_movement is not None: - env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - - return env - - @staticmethod - def teleoperator( - id: str, - port: str, - calibration_dir: PathLike | str | None = None, - ) -> SO101Leader: - if isinstance(calibration_dir, str): - calibration_dir = Path(calibration_dir) - cfg = SO101LeaderConfig(id=id, calibration_dir=calibration_dir, port=port) - teleop = make_teleoperator_from_config(cfg) - teleop.connect() - return teleop - - -class SO101SimEnvCreator(EnvCreator): - def __call__( # type: ignore - self, - control_mode: ControlMode, - robot_cfg: SimRobotConfig, - urdf_path: str, - mjcf: str, - collision_guard: bool = False, - gripper_cfg: SimGripperConfig | None = None, - cameras: dict[str, SimCameraConfig] | None = None, - max_relative_movement: float | tuple[float, float] | None = None, - relative_to: RelativeTo = RelativeTo.LAST_STEP, - sim_wrapper: Type[SimWrapper] | None = None, - ) -> gym.Env: - """ - Creates a simulation environment for the FR3 robot. - Args: - control_mode (ControlMode): Control mode for the robot. - robot_cfg (rcs.sim.SimRobotConfig): Configuration for the FR3 robot. - collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding. - gripper_cfg (rcs.sim.SimGripperConfig | None): Configuration for the gripper. If None, no gripper is used. - camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used. - max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts - translational movement in meters. If tuple, it restricts both translational (in meters) and rotational - (in radians) movements. If None, no restriction is applied. - relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced - which requires a UTN compatible lab scene to be present. - mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world". - sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful - for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. - Returns: - gym.Env: The configured simulation environment for the FR3 robot. - """ - simulation = sim.Sim(mjcf) - - ik = rcs.common.RL(str(urdf_path)) - robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) - env: gym.Env = RobotEnv(robot, control_mode) - env = RobotSimWrapper(env, simulation, sim_wrapper) - - if cameras is not None: - camera_set = typing.cast( - BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) - ) env = CameraSetWrapper(env, camera_set, include_depth=True) - if gripper_cfg is not None and isinstance(gripper_cfg, SimGripperConfig): - gripper = sim.SimGripper(simulation, gripper_cfg) - env = GripperWrapper(env, gripper, binary=False) - env = GripperWrapperSim(env, gripper) - - if collision_guard: - env = CollisionGuard.env_from_xml_paths( - env, - str(rcs.scenes.get(str(mjcf), mjcf)), - str(urdf_path), - gripper=gripper_cfg is not None, - check_home_collision=False, - control_mode=control_mode, - tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), - sim_gui=True, - truncate_on_collision=True, - ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) return env + + # @staticmethod + # def teleoperator( + # id: str, + # port: str, + # calibration_dir: PathLike | str | None = None, + # ) -> SO101Leader: + # if isinstance(calibration_dir, str): + # calibration_dir = Path(calibration_dir) + # cfg = SO101LeaderConfig(id=id, calibration_dir=calibration_dir, port=port) + # teleop = make_teleoperator_from_config(cfg) + # teleop.connect() + # return teleop diff --git a/extensions/rcs_so101/src/rcs_so101/env_cartesian_control.py b/extensions/rcs_so101/src/rcs_so101/env_cartesian_control.py new file mode 100644 index 00000000..5626c200 --- /dev/null +++ b/extensions/rcs_so101/src/rcs_so101/env_cartesian_control.py @@ -0,0 +1,99 @@ +import logging +from time import sleep + +import numpy as np +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs_so101.creators import RCSSO101EnvCreator +from rcs_so101.hw import SO101Config + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) +ROBOT_INSTANCE = RobotPlatform.HARDWARE + + +def main(): + + robot_cfg: rcs._core.common.RobotConfig + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + robot_cfg = SO101Config() + robot_cfg.id = "jobi_follower" + robot_cfg.port = "/dev/ttyACM0" + robot_cfg.calibration_dir = "/home/tobi/coding/lerobot" + robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot + robot_cfg.attachment_site = "gripper" + env_rel = RCSSO101EnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + relative_to=RelativeTo.LAST_STEP, + max_relative_movement=(0.05, np.deg2rad(3)), + ) + + else: + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = ["1", "2", "3", "4", "5"] + robot_cfg.joints = [ + "1", + "2", + "3", + "4", + "5", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.SO101 + robot_cfg.attachment_site = "gripper" + robot_cfg.arm_collision_geoms = [] + robot_cfg.mjcf_scene_path = rcs.scenes["so101_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot + + gripper_cfg = sim.SimGripperConfig() + gripper_cfg.min_actuator_width = -0.17453292519943295 + gripper_cfg.max_actuator_width = 1.7453292519943295 + gripper_cfg.min_joint_width = -0.17453292519943295 + gripper_cfg.max_joint_width = 1.7453292519943295 + gripper_cfg.actuator = "6" + gripper_cfg.joint = "6" + gripper_cfg.collision_geoms = [] + gripper_cfg.collision_geoms_fingers = [] + + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + collision_guard=False, + gripper_cfg=gripper_cfg, + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + obs, info = env_rel.reset() + + act = {"tquat": [0.03, 0, 0, 0, 0, 0, 1], "gripper": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + sleep(1) + + for _ in range(100): + for _ in range(5): + # 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_rel.step(act) + print(info, obs) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + sleep(1) + for _ in range(5): + # move 1cm in negative x direction (backward) and open gripper + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + sleep(1) + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py index d7cbbb80..fc041a71 100644 --- a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py +++ b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py @@ -1,9 +1,12 @@ import logging from time import sleep +import numpy as np from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo -from rcs_so101.creators import RCSSO101EnvCreator, SO101SimEnvCreator +from rcs.envs.creators import SimEnvCreator +from rcs_so101.creators import RCSSO101EnvCreator +from rcs_so101.hw import SO101Config import rcs from rcs import sim @@ -11,52 +14,67 @@ logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) -ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE def main(): + robot_cfg: rcs._core.common.RobotConfig if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + robot_cfg = SO101Config() + robot_cfg.id = "jobi_follower" + robot_cfg.port = "/dev/ttyACM0" + robot_cfg.calibration_dir = "/home/tobi/coding/lerobot" + robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot + robot_cfg.attachment_site = "gripper" + env_rel = RCSSO101EnvCreator()( - id="so101_follower", - urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf", - port="/dev/ttyACM0", - calibration_dir="/home/tobi/coding/lerobot-container/calibration/robots/so101_follower/ninja_follower.json", - # max_relative_movement=(0.5, np.deg2rad(90)), + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, relative_to=RelativeTo.LAST_STEP, + max_relative_movement=np.deg2rad(10), ) else: - cfg = sim.SimRobotConfig() - cfg.robot_type = rcs.common.RobotType.SO101 - cfg.actuators = ["1", "2", "3", "4", "5"] - cfg.joints = ["1", "2", "3", "4", "5"] - cfg.arm_collision_geoms = [] - cfg.attachment_site = "gripper" + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = ["1", "2", "3", "4", "5"] + robot_cfg.joints = [ + "1", + "2", + "3", + "4", + "5", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.SO101 + robot_cfg.attachment_site = "gripper" + robot_cfg.arm_collision_geoms = [] + robot_cfg.mjcf_scene_path = rcs.scenes["so101_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot - grp_cfg = sim.SimGripperConfig() - grp_cfg.actuator = "6" - grp_cfg.joint = "6" - grp_cfg.collision_geoms = [] - grp_cfg.collision_geoms_fingers = [] + gripper_cfg = sim.SimGripperConfig() + gripper_cfg.min_actuator_width = -0.17453292519943295 + gripper_cfg.max_actuator_width = 1.7453292519943295 + gripper_cfg.min_joint_width = -0.17453292519943295 + gripper_cfg.max_joint_width = 1.7453292519943295 + gripper_cfg.actuator = "6" + gripper_cfg.joint = "6" + gripper_cfg.collision_geoms = [] + gripper_cfg.collision_geoms_fingers = [] - env_rel = SO101SimEnvCreator()( + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, control_mode=ControlMode.JOINTS, - urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf", - robot_cfg=cfg, collision_guard=False, - mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/SO101/scene.xml", - gripper_cfg=grp_cfg, - # camera_set_cfg=default_mujoco_cameraset_cfg(), - max_relative_movement=None, - # max_relative_movement=10.0, - # max_relative_movement=0.5, + gripper_cfg=gripper_cfg, + max_relative_movement=np.deg2rad(5), relative_to=RelativeTo.LAST_STEP, ) env_rel.get_wrapper_attr("sim").open_gui() for _ in range(10): obs, info = env_rel.reset() - for _ in range(100): + sleep(1.0) + for _ in range(3): # sample random relative action and execute it act = env_rel.action_space.sample() print(act) diff --git a/extensions/rcs_so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py index 1ca20b64..a0f7cc68 100644 --- a/extensions/rcs_so101/src/rcs_so101/hw.py +++ b/extensions/rcs_so101/src/rcs_so101/hw.py @@ -1,40 +1,78 @@ +import threading import typing +from pathlib import Path import numpy as np -from lerobot.common.robots.so101_follower.so101_follower import SO101Follower +from lerobot.robots import make_robot_from_config +from lerobot.robots.so101_follower.config_so101_follower import SO101FollowerConfig +from lerobot.robots.so101_follower.so101_follower import SO101Follower +from rcs.utils import SimpleFrameRate from rcs import common +class SO101Config(common.RobotConfig): + id: str = "follower" + port: str = "/dev/ttyACM0" + calibration_dir: str = "." + + class SO101(common.Robot): - def __init__(self, hf_robot: SO101Follower, urdf_path: str): - self.ik = common.RL(urdf_path=urdf_path) - self._hf_robot = hf_robot - self._hf_robot.connect() + def __init__(self, robot_cfg: SO101Config, ik: common.IK): + super().__init__() + self.ik = ik + cfg = SO101FollowerConfig(id=robot_cfg.id, calibration_dir=Path(robot_cfg.calibration_dir), port=robot_cfg.port) + self.hf_robot = make_robot_from_config(cfg) + self.hf_robot.connect() + self._thread: threading.Thread | None = None + self._running = False + self._goal = None + self._goal_lock = threading.Lock() + self._rate_limiter = SimpleFrameRate(30, "teleop readout") + self.obs = None + self._last_joint = self._get_joint_position() - # def get_base_pose_in_world_coordinates(self) -> Pose: ... def get_cartesian_position(self) -> common.Pose: return self.ik.forward(self.get_joint_position()) + def _get_cartesian_position(self) -> common.Pose: + return self._last_cart + def get_ik(self) -> common.IK | None: return self.ik - def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore - obs = self._hf_robot.get_observation() - return np.array( + def _get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore + obs = self.hf_robot.get_observation() + self.obs = obs + joints_hf = np.array( [ obs["shoulder_pan.pos"], obs["shoulder_lift.pos"], obs["elbow_flex.pos"], obs["wrist_flex.pos"], obs["wrist_roll.pos"], - obs["gripper.pos"], ], dtype=np.float64, ) + # print(obs) + joints_normalized = (joints_hf + 100) / 200 + joints_in_rad = ( + joints_normalized + * ( + common.robots_meta_config(common.RobotType.SO101).joint_limits[1] + - common.robots_meta_config(common.RobotType.SO101).joint_limits[0] + ) + + common.robots_meta_config(common.RobotType.SO101).joint_limits[0] + ) + self._last_joint = joints_in_rad + return joints_in_rad + + def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore + # return self._last_joint + return self._get_joint_position() - def get_parameters(self): - a = common.RobotConfig() + def get_parameters(self) -> SO101Config: + a = SO101Config() a.robot_platform = common.RobotPlatform.HARDWARE a.robot_type = common.RobotType.SO101 return a @@ -43,42 +81,111 @@ def get_state(self) -> common.RobotState: return common.RobotState() def move_home(self) -> None: - home = typing.cast( - np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]], - common.robots_meta_config(common.RobotType.SO101).q_home, - ) - self.set_joint_position(home) + # home = typing.cast( + # np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]], + # common.robots_meta_config(common.RobotType.SO101).q_home, + # ) + home = np.array([-0.01914898, -1.90521916, 1.56476701, 1.04783839, -1.40323926]) + self.set_joint_position(home) # type: ignore def reset(self) -> None: pass def set_cartesian_position(self, pose: common.Pose) -> None: - joints = self.ik.ik(pose, q0=self.get_joint_position()) + q0 = self.get_joint_position() + joints = self.ik.ik(pose, q0=q0) if joints is not None: self.set_joint_position(joints) + self._last_cart = pose - def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore - self._hf_robot.send_action( + def _set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore + self._last_joint = q + q_normalized = (q - common.robots_meta_config(common.RobotType.SO101).joint_limits[0]) / ( + common.robots_meta_config(common.RobotType.SO101).joint_limits[1] + - common.robots_meta_config(common.RobotType.SO101).joint_limits[0] + ) + q_hf = (q_normalized * 200) - 100 + self.hf_robot.send_action( { - "shoulder_pan.pos": q[0], - "shoulder_lift.pos": q[1], - "elbow_flex.pos": q[2], - "wrist_flex.pos": q[3], - "wrist_roll.pos": q[4], + "shoulder_pan.pos": q_hf[0], + "shoulder_lift.pos": q_hf[1], + "elbow_flex.pos": q_hf[2], + "wrist_flex.pos": q_hf[3], + "wrist_roll.pos": q_hf[4], } ) + def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore + self._set_joint_position(q) + + def _controller(self): + print("Controller thread started") + while self._running: + + with self._goal_lock: + goal = self._goal + if goal is None: + self._rate_limiter() + continue + current_pos = self._get_joint_position() + if np.allclose(current_pos, goal, atol=np.deg2rad(5)): + # print("Goal reached, continuing...") + self._rate_limiter() + continue + # interpolate with max 10 degree / s + max_step = np.deg2rad(90) * self._rate_limiter.get_frame_time() + delta = goal - current_pos + # how many steps are needed to reach the goal + steps_needed = np.ceil(np.max(np.abs(delta)) / max_step) + for i in range(int(steps_needed)): + if not self._running: + # print("Controller thread stopped") + return + # calculate the next position + step = delta / steps_needed * (i + 1) + new_pos = current_pos + step + self._set_joint_position(new_pos) + + self._rate_limiter() + # check if new goal is set + with self._goal_lock: + if self._goal is None or not np.allclose(goal, self._goal, atol=np.deg2rad(1)): + break + + def start_controller_thread(self): + self._running = True + self._thread = threading.Thread(target=self._controller, daemon=True) + self._thread.start() + + def stop_controller_thread(self): + print("Stopping controller thread") + self._running = False + with self._goal_lock: + self._goal = None + if self._thread is not None and self._thread.is_alive(): + self._thread.join() + # def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ... # def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ... + def close(self): + self.move_home() + self.stop_controller_thread() + self.hf_robot.disconnect() + # TODO: problem when we inherit from gripper then we also need to call init which doesnt exist -class S0101Gripper(common.Gripper): - def __init__(self, hf_robot: SO101Follower): - self._hf_robot = hf_robot +class SO101Gripper(common.Gripper): + def __init__(self, hf_robot: SO101Follower, robot: SO101): + super().__init__() + self.hf_robot = hf_robot + self.robot = robot def get_normalized_width(self) -> float: - obs = self._hf_robot.get_observation() + # obs = self.hf_robot.get_observation() + obs = self.robot.obs + if obs is None: + return 0.0 return obs["gripper.pos"] / 100.0 # def get_parameters(self) -> GripperConfig: ... @@ -110,7 +217,7 @@ def set_normalized_width(self, width: float, _: float = 0) -> None: raise ValueError(msg) # Convert normalized width to absolute position abs_width = width * 100.0 - self._hf_robot.send_action({"gripper.pos": abs_width}) + self.hf_robot.send_action({"gripper.pos": abs_width}) def shut(self) -> None: """ diff --git a/extensions/rcs_usb_cam/pyproject.toml b/extensions/rcs_usb_cam/pyproject.toml new file mode 100644 index 00000000..c72bbe6a --- /dev/null +++ b/extensions/rcs_usb_cam/pyproject.toml @@ -0,0 +1,26 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "rcs_usb_cam" +version = "0.4.0" +description="RCS USB Camera module" +dependencies = [ + "rcs==0.4.0", + "opencv-python~=4.10.0" +] +readme = "README.md" +maintainers = [ + { name = "Seongjin Bien", email = "seongjin.bien@utn.de" }, +] +authors = [ + { name = "Seongjin Bien", email = "seongjin.bien@utn.de" }, +] +requires-python = ">=3.10" +classifiers = [ + "Development Status :: 3 - Alpha", + "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)", + "Programming Language :: Python :: 3", +] + diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py new file mode 100644 index 00000000..7900278b --- /dev/null +++ b/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py @@ -0,0 +1,126 @@ +import copy +import datetime +import logging +import threading +import typing + +import cv2 +import numpy as np +from rcs.camera.hw import CalibrationStrategy, DummyCalibrationStrategy, HardwareCamera +from rcs.camera.interface import CameraFrame, DataFrame, Frame + +from rcs import common + +""" +A generic extension class for handling USB-connected cameras. +Uses OpenCV to interface with the camera hardware, specifically using cv2.VideoCapture(id). +The ID can be both a single integer passed as a string, i.e. str(0), str(1), or the full /dev/ path, like /dev/video0. + +""" + + +class USBCameraConfig(common.BaseCameraConfig): + color_intrinsics: np.ndarray[tuple[typing.Literal[3], typing.Literal[4]], np.dtype[np.float64]] | None = None + distortion_coeffs: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]] | None = None + + +class USBCameraSet(HardwareCamera): + def __init__( + self, + cameras: dict[str, USBCameraConfig], + calibration_strategy: dict[str, CalibrationStrategy] | None = None, + ): + self.cameras = cameras + self.CALIBRATION_FRAME_SIZE = 30 + if calibration_strategy is None: + calibration_strategy = {camera_name: DummyCalibrationStrategy() for camera_name in cameras} + for cam in self.cameras.values(): + if cam.color_intrinsics is None: + cam.color_intrinsics = np.zeros((3, 4), dtype=np.float64) # type: ignore + if cam.distortion_coeffs is None: + cam.distortion_coeffs = np.zeros((5,), dtype=np.float64) # type: ignore + if cam.resolution_height is None: + cam.resolution_height = 480 + if cam.resolution_width is None: + cam.resolution_width = 640 + if cam.frame_rate is None: + cam.frame_rate = 30 + self.calibration_strategy = calibration_strategy + self._camera_names = list(self.cameras.keys()) + self._captures: dict[str, cv2.VideoCapture] = {} + self._logger = logging.getLogger(__name__) + self._logger.info("USBCamera initialized with cameras: %s", self._camera_names) + self._logger.info( + "If the camera streams are not correct, try v4l2-ctl --list-devices to see the available cameras." + ) + self._frame_buffer_lock: dict[str, threading.Lock] = {} + self._frame_buffer: dict[str, list] = {} + + def open(self): + for name, camera in self.cameras.items(): + self._frame_buffer_lock[name] = threading.Lock() + self._frame_buffer[name] = [] + cap = cv2.VideoCapture(camera.identifier) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, camera.resolution_width) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, camera.resolution_height) + cap.set(cv2.CAP_PROP_FPS, camera.frame_rate) + + if not cap.isOpened(): + msg = f"Could not open camera {name} with id {camera.identifier}" + raise RuntimeError(msg) + self._captures[name] = cap + + @property + def camera_names(self) -> list[str]: + return self._camera_names + + def poll_frame(self, camera_name: str) -> Frame: + cap = self._captures[camera_name] + timestamp = datetime.datetime.now().timestamp() + ret, color_frame = cap.read() + if not ret: + msg = f"Failed to read frame from camera {camera_name}" + raise RuntimeError(msg) + with self._frame_buffer_lock[camera_name]: + if len(self._frame_buffer[camera_name]) >= self.CALIBRATION_FRAME_SIZE: + self._frame_buffer[camera_name].pop(0) + self._frame_buffer[camera_name].append(copy.deepcopy(color_frame)) + color = DataFrame( + data=color_frame, + timestamp=timestamp, + intrinsics=self.cameras[camera_name].color_intrinsics, + extrinsics=self.calibration_strategy[camera_name].get_extrinsics(), + ) + depth_frame = np.zeros( + (self.cameras[camera_name].resolution_height, self.cameras[camera_name].resolution_width), dtype=np.uint16 + ) + depth = DataFrame( + data=depth_frame, + timestamp=timestamp, + intrinsics=self.cameras[camera_name].color_intrinsics, + extrinsics=self.calibration_strategy[camera_name].get_extrinsics(), + ) + cf = CameraFrame(color=color, depth=depth) + return Frame(camera=cf, avg_timestamp=timestamp) + + def close(self): + for cap in self._captures.values(): + cap.release() + self._captures = {} + + def config(self, camera_name: str) -> USBCameraConfig: + return self.cameras[camera_name] + + def calibrate(self) -> bool: + for camera_name in self.cameras: + color_intrinsics = self.cameras[camera_name].color_intrinsics + assert color_intrinsics is not None, f"Color intrinsics for camera {camera_name} not found" + if not self.calibration_strategy[camera_name].calibrate( + intrinsics=color_intrinsics, + samples=self._frame_buffer[camera_name], + lock=self._frame_buffer_lock[camera_name], + ): + self._logger.warning(f"Calibration of camera {camera_name} failed.") + return False + self._logger.info("Calibration successful.") + return True diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/webcam.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/webcam.py new file mode 100644 index 00000000..785e3eee --- /dev/null +++ b/extensions/rcs_usb_cam/src/rcs_usb_cam/webcam.py @@ -0,0 +1,41 @@ +import cv2 + +""" +Simple script for opening a webcam using OpenCV and displaying the video feed. +""" + + +def open_webcam(): + # Open the default webcam (0). + # If you have multiple cameras, you can try 1, 2, etc. + cap = cv2.VideoCapture("/dev/video8") + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) + cap.set(cv2.CAP_PROP_FPS, 30) + + if not cap.isOpened(): + print("Error: Could not open webcam.") + return + + # Loop to continuously get frames + while True: + ret, frame = cap.read() + + if not ret: + print("Failed to grab frame") + break + + # Show the frame in a window + cv2.imshow("Webcam", frame) + + # Press 'q' to quit + if cv2.waitKey(1) & 0xFF == ord("q"): + break + + # Release the camera and close windows + cap.release() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + open_webcam() diff --git a/extensions/rcs_xarm7/pyproject.toml b/extensions/rcs_xarm7/pyproject.toml new file mode 100644 index 00000000..2dc42f8e --- /dev/null +++ b/extensions/rcs_xarm7/pyproject.toml @@ -0,0 +1,28 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "rcs_xarm7" +version = "0.4.0" +description="RCS xArm7 module" +dependencies = [ + "rcs==0.4.0", + "xarm-python-sdk==1.17.0", +] +readme = "README.md" +maintainers = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, + { name = "Ken Nakahara", email = "knakahara@lasr.org" }, +] +authors = [ + { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, + { name = "Ken Nakahara", email = "knakahara@lasr.org" }, +] +requires-python = ">=3.10" +license = "AGPL-3.0-or-later" +classifiers = [ + "Development Status :: 3 - Alpha", + "Programming Language :: Python :: 3", +] + diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/__init__.py b/extensions/rcs_xarm7/src/rcs_xarm7/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py new file mode 100644 index 00000000..9406b0a4 --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -0,0 +1,149 @@ +import logging +import typing +from os import PathLike +from pathlib import Path +from typing import Type + +import gymnasium as gym +from gymnasium.envs.registration import EnvCreator +from rcs.camera.hw import HardwareCameraSet +from rcs.camera.interface import BaseCameraSet +from rcs.camera.sim import SimCameraSet +from rcs.envs.base import ( + CameraSetWrapper, + ControlMode, + GripperWrapper, + HandWrapper, + RelativeActionSpace, + RelativeTo, + RobotEnv, +) +from rcs.envs.creators import RCSHardwareEnvCreator +from rcs.envs.sim import CollisionGuard, GripperWrapperSim, RobotSimWrapper, SimWrapper +from rcs.hand.tilburg_hand import THConfig, TilburgHand +from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig +from rcs_xarm7.hw import XArm7 + +import rcs +from rcs import common, sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +class RCSXArm7EnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + control_mode: ControlMode, + ip: str, + calibration_dir: PathLike | str | None = None, + collision_guard: str | PathLike | None = None, + cg_kinematics_path: str | PathLike | None = None, + camera_set: HardwareCameraSet | None = None, + hand_cfg: THConfig | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + ) -> gym.Env: + if isinstance(calibration_dir, str): + calibration_dir = Path(calibration_dir) + robot = XArm7(ip=ip) + # env: gym.Env = RobotEnv(robot, ControlMode.JOINTS, home_on_reset=True) + env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True) + + if camera_set is not None: + camera_set.start() + camera_set.wait_for_frames() + logger.info("CameraSet started") + env = CameraSetWrapper(env, camera_set, include_depth=True) + if hand_cfg is not None and isinstance(hand_cfg, THConfig): + hand = TilburgHand(cfg=hand_cfg, verbose=True) + env = HandWrapper(env, hand, True) + + # if collision_guard: + # env = CollisionGuard.env_from_xml_paths( + # env=env, + # cg_kinematics_path=cg_kinematics_path, + # hand=True, + # gripper=False, + # check_home_collision=False, + # control_mode=control_mode, + # tcp_offset=rcs.common.Pose(), + # sim_gui=True, + # truncate_on_collision=True, + # id="", + # ) + + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + + return env + + +class XArm7SimEnvCreator(EnvCreator): + def __call__( # type: ignore + self, + control_mode: ControlMode, + robot_cfg: SimRobotConfig, + urdf_path: str, + mjcf: str, + collision_guard: bool = False, + gripper_cfg: SimGripperConfig | None = None, + cameras: dict[str, SimCameraConfig] | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + sim_wrapper: Type[SimWrapper] | None = None, + ) -> gym.Env: + """ + Creates a simulation environment for the FR3 robot. + Args: + control_mode (ControlMode): Control mode for the robot. + robot_cfg (rcs.sim.SimRobotConfig): Configuration for the FR3 robot. + collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding. + gripper_cfg (rcs.sim.SimGripperConfig | None): Configuration for the gripper. If None, no gripper is used. + camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used. + max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts + translational movement in meters. If tuple, it restricts both translational (in meters) and rotational + (in radians) movements. If None, no restriction is applied. + relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. + urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced + which requires a UTN compatible lab scene to be present. + mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world". + sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful + for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. + Returns: + gym.Env: The configured simulation environment for the FR3 robot. + """ + simulation = sim.Sim(mjcf) + + ik = rcs.common.RL(str(urdf_path)) + robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) + env: gym.Env = RobotEnv(robot, control_mode) + env = RobotSimWrapper(env, simulation, sim_wrapper) + + if cameras is not None: + camera_set = typing.cast( + BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True) + ) + env = CameraSetWrapper(env, camera_set, include_depth=True) + + if gripper_cfg is not None and isinstance(gripper_cfg, SimGripperConfig): + gripper = sim.SimGripper(simulation, gripper_cfg) + env = GripperWrapper(env, gripper, binary=False) + env = GripperWrapperSim(env, gripper) + + if collision_guard: + env = CollisionGuard.env_from_xml_paths( + env, + str(rcs.scenes.get(str(mjcf), mjcf)), + str(urdf_path), + gripper=gripper_cfg is not None, + check_home_collision=False, + control_mode=control_mode, + tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), + sim_gui=True, + truncate_on_collision=True, + ) + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + + return env diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py new file mode 100644 index 00000000..a23c5d58 --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py @@ -0,0 +1,111 @@ +import logging + +import numpy as np +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import default_sim_tilburg_hand_cfg, get_tcp_offset +from rcs.hand.tilburg_hand import THConfig +from rcs_xarm7.creators import RCSXArm7EnvCreator + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_IP = "192.168.1.245" +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE + + +def sim_env(): + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = [ + "act1", + "act2", + "act3", + "act4", + "act5", + "act6", + "act7", + ] + robot_cfg.joints = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_cfg.attachment_site = "attachment_site" + robot_cfg.arm_collision_geoms = [] + robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene) + robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + collision_guard=False, + gripper_cfg=None, + hand_cfg=default_sim_tilburg_hand_cfg(), + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + return env_rel + + +def main(): + + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + hand_cfg = THConfig( + calibration_file="/home/ken/tilburg_hand/calibration.json", grasp_percentage=1, hand_orientation="right" + ) + env_rel = RCSXArm7EnvCreator()( + control_mode=ControlMode.CARTESIAN_TQuat, + ip=ROBOT_IP, + hand_cfg=hand_cfg, + relative_to=RelativeTo.LAST_STEP, + max_relative_movement=np.deg2rad(3), + # collision_guard=rcs.scenes["xarm7_empty_world"]["mjcf_scene"], + # cg_kinematics_path=rcs.scenes["xarm7_empty_world"]["mjcf_robot"], + ) + else: + env_rel = sim_env() + + twin_env = sim_env() + twin_robot = twin_env.unwrapped.robot + twin_sim = twin_env.get_wrapper_attr("sim") + + env_rel.reset() + # act = {"tquat": [0.1, 0, 0, 0, 0, 0, 1], "gripper": 0} + # obs, reward, terminated, truncated, info = env_rel.step(act) + + with env_rel: + for _ in range(10): + for _ in range(10): + # move 1cm in x direction (forward) and close gripper + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "hand": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + joints = env_rel.unwrapped.robot.get_joint_position() # type: ignore + twin_robot.set_joints_hard(joints) + twin_sim.step(50) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + for _ in range(10): + # move 1cm in negative x direction (backward) and open gripper + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "hand": 0} + obs, reward, terminated, truncated, info = env_rel.step(act) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py new file mode 100644 index 00000000..ca4b3144 --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py @@ -0,0 +1,115 @@ +import logging +import math +from time import sleep + +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import default_sim_tilburg_hand_cfg, get_tcp_offset +from rcs.hand.tilburg_hand import THConfig +from rcs_xarm7.creators import RCSXArm7EnvCreator + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_IP = "192.168.1.245" +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE + + +def sim_env(): + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = [ + "act1", + "act2", + "act3", + "act4", + "act5", + "act6", + "act7", + ] + robot_cfg.joints = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_cfg.attachment_site = "attachment_site" + robot_cfg.arm_collision_geoms = [] + robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene) + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, + collision_guard=False, + gripper_cfg=None, + hand_cfg=default_sim_tilburg_hand_cfg(), + # cameras=default_mujoco_cameraset_cfg(), + # max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + return env_rel + + +def main(): + + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + hand_cfg = THConfig( + calibration_file="/home/ken/tilburg_hand/calibration.json", grasp_percentage=1, hand_orientation="right" + ) + env_rel = RCSXArm7EnvCreator()( + control_mode=ControlMode.JOINTS, + ip=ROBOT_IP, + hand_cfg=hand_cfg, + relative_to=RelativeTo.LAST_STEP, + max_relative_movement=None, + ) + else: + env_rel = sim_env() + + twin_env = sim_env() + + env_rel.reset() + + actions = [ + # open hand + ([0, math.radians(-45), 0, math.radians(15), 0, math.radians(-25), 0], 1, 2.0), + # approach + ([0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 1, 2.0), + # close hand + ([0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 0, 2.0), + # lift + ([0, math.radians(15), 0, math.radians(30), 0, math.radians(-75), 0], 0, 4.0), + # put back + ([0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 0, 2.0), + # open hand + ([0, math.radians(45), 0, math.radians(40), 0, math.radians(-95), 0], 1, 2.0), + # back to home + ([0, math.radians(-45), 0, math.radians(15), 0, math.radians(-25), 0], 1, 0.0), + ] + + with env_rel: + for joints, hand, delay in actions: + act = {"joints": joints, "hand": hand} + twin_env.step(act) + obs, reward, terminated, truncated, info = env_rel.step(act) + # twin_robot.set_joint_position(joints) + # twin_sim.step(50) + sleep(1) + if truncated or terminated: + logger.info("Truncated or terminated!") + break + if delay > 0: + sleep(delay) + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py new file mode 100644 index 00000000..93c5c39d --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py @@ -0,0 +1,87 @@ +import logging +from time import sleep + +import numpy as np +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import get_tcp_offset +from rcs_xarm7.creators import RCSXArm7EnvCreator + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +ROBOT_IP = "192.168.1.245" +# ROBOT_INSTANCE = RobotPlatform.SIMULATION +ROBOT_INSTANCE = RobotPlatform.HARDWARE + + +def main(): + + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSXArm7EnvCreator()( + control_mode=ControlMode.JOINTS, + ip=ROBOT_IP, + relative_to=RelativeTo.LAST_STEP, + max_relative_movement=np.deg2rad(3), + ) + else: + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = [ + "act1", + "act2", + "act3", + "act4", + "act5", + "act6", + "act7", + ] + robot_cfg.joints = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_cfg.attachment_site = "attachment_site" + robot_cfg.arm_collision_geoms = [] + robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes["xarm7_empty_world"].mjcf_scene) + robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, + collision_guard=False, + gripper_cfg=None, + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + + with env_rel: + for _ in range(10): + obs, info = env_rel.reset() + for _ in range(3): + # sample random relative action and execute it + act = env_rel.action_space.sample() + print(act) + # act["gripper"] = 1.0 + obs, reward, terminated, truncated, info = env_rel.step(act) + print(obs) + if truncated or terminated: + logger.info("Truncated or terminated!") + return + # logger.info(act["gripper"]) + sleep(2.0) + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py new file mode 100644 index 00000000..e818e9c2 --- /dev/null +++ b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py @@ -0,0 +1,101 @@ +import typing +from dataclasses import dataclass, field +from typing import List + +import numpy as np +from xarm.wrapper import XArmAPI + +from rcs import common + + +@dataclass(kw_only=True) +class XArm7Config(common.RobotConfig): + # some_custom_config: str = "default_value" + payload_weight: float = 0.624 + payload_tcp: List[float] = field(default_factory=lambda: [-4.15, 5.24, 76.38]) + async_control: bool = False + + def __post_init__(self): + super().__init__() + + +class XArm7(common.Robot): + def __init__(self, ip: str): + super().__init__() + + self.ik = None # common.RL(urdf_path=urdf_path) + self._config = XArm7Config() + self._config.robot_platform = common.RobotPlatform.HARDWARE + self._config.robot_type = common.RobotType.XArm7 + + self._xarm = XArmAPI(ip) + self._xarm.set_mode(0) + self._xarm.clean_error() + self._xarm.clean_warn() + self._xarm.motion_enable(enable=True) + self._xarm.set_state(state=0) + self._xarm.set_tcp_load( + weight=self._config.payload_weight, + center_of_gravity=self._config.payload_tcp, + wait=True, + ) + + def get_cartesian_position(self) -> common.Pose: + code, xyzrpy = self._xarm.get_position(is_radian=True) + if code != 0: + msg = "couldn't get cartesian position from xarm" + raise RuntimeError(msg) + + x_mm, y_mm, z_mm = xyzrpy[:3] + translation_meter = [x_mm * 0.001, y_mm * 0.001, z_mm * 0.001] + rpy = xyzrpy[3:] + + return common.Pose(rpy_vector=rpy, translation=translation_meter) # type: ignore + + def get_ik(self) -> common.IK | None: + return self.ik + + def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]: # type: ignore + return typing.cast( + np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]], + np.array(self._xarm.get_servo_angle(is_radian=True)[1]), + ) + + def get_parameters(self) -> XArm7Config: + return self._config + + def set_parameters(self, robot_cfg: XArm7Config) -> None: + self._config = robot_cfg + + def get_state(self) -> common.RobotState: + return common.RobotState() + + def move_home(self) -> None: + home = typing.cast( + np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]], + common.robots_meta_config(common.RobotType.XArm7).q_home, + ) + # self.set_joint_position(home) + self._xarm.set_mode(0) + self._xarm.set_state(0) + self._xarm.set_servo_angle(angle=home, is_radian=True, wait=True) + + def reset(self) -> None: + pass + + def set_cartesian_position(self, pose: common.Pose) -> None: + if self._config.async_control: + self._xarm.set_mode(7) + self._xarm.set_state(0) + x, y, z, roll, pitch, yaw = pose.xyzrpy() + x_mm, y_mm, z_mm = 1000 * x, 1000 * y, 1000 * z + self._xarm.set_position(x_mm, y_mm, z_mm, roll, pitch, yaw, is_radian=True, wait=not self._config.async_control) + + def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]]) -> None: # type: ignore + if self._config.async_control: + self._xarm.set_mode(6) + self._xarm.set_state(0) + self._xarm.set_servo_angle(angle=q, is_radian=True, wait=not self._config.async_control) + + def close(self): + self._xarm.disconnect() diff --git a/include/rcs/IK.h b/include/rcs/IK.h index 5c13529a..0fc7b2b1 100644 --- a/include/rcs/IK.h +++ b/include/rcs/IK.h @@ -53,6 +53,8 @@ class Pin : public IK { const int IT_MAX = 1000; const double DT = 1e-1; const double damp = 1e-6; + const double eps_pos = 0.01; + const double eps_ori = 10 * (M_PI / 180.0); int FRAME_ID; pinocchio::Model model; diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h index 793ca6e5..f927ff67 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -18,7 +18,7 @@ struct RobotMetaConfig { Eigen::Matrix joint_limits; }; -enum RobotType { FR3 = 0, UR5e, SO101 }; +enum RobotType { FR3 = 0, UR5e, SO101, XArm7 }; enum RobotPlatform { SIMULATION = 0, HARDWARE }; static const std::unordered_map robots_meta_config = @@ -57,27 +57,55 @@ static const std::unordered_map robots_meta_config = // high 6‐tuple 2 * M_PI, 2 * M_PI, 1 * M_PI, 2 * M_PI, 2 * M_PI, 2 * M_PI) .finished()}}, + // -------------- XArm7 -------------- + {XArm7, RobotMetaConfig{ + // q_home (7‐vector): + (VectorXd(7) << 0, + -45. / 180. * M_PI, + 0, + 15. / 180. * M_PI, + 0, + -25. / 180. * M_PI, + 0 + ).finished(), + // dof: + 7, + // joint_limits (2×7): + (Eigen::Matrix(2, 7) << + // low 7‐tuple + -2 * M_PI, -2.094395, -2 * M_PI, -3.92699, -2 * M_PI, -M_PI, -2 * M_PI, + // high 7‐tuple + 2 * M_PI, 2.059488, 2 * M_PI, 0.191986, 2 * M_PI, 1.692969, 2 * M_PI).finished()}}, // -------------- SO101 -------------- {SO101, RobotMetaConfig{ // q_home (5‐vector): - (VectorXd(5) << -9.40612320177057, -99.66130397967824, - 99.9124726477024, 69.96996996996998, -9.095744680851055) + // (VectorXd(5) << -9.40612320177057, -99.66130397967824, + // 99.9124726477024, 69.96996996996998, -9.095744680851055) + // .finished(), + (VectorXd(5) << 0, 0, 0, 1.5707963267948966, + 0) .finished(), // dof: 5, // joint_limits (2×5): (Eigen::Matrix(2, 5) << - // low 5‐tuple - -100.0, - -100.0, -100.0, -100.0, -100.0, + // low 5‐tuple + -1.9198621771937616, + -1.9198621771937634, -1.7453292519943295, -1.6580627969561903, + -2.7925268969992407, + // high 5‐tuple - 100.0, 100.0, 100.0, 100.0, 100.0) + 1.9198621771937616, 1.9198621771937634, 1.5707963267948966, + 1.6580627969561903, 2.7925268969992407) .finished()}}}}; struct RobotConfig { RobotType robot_type = RobotType::FR3; RobotPlatform robot_platform = RobotPlatform::SIMULATION; + rcs::common::Pose tcp_offset = rcs::common::Pose::Identity(); + std::string attachment_site = "attachment_site"; + std::string kinematic_model_path = "assets/scenes/fr3_empty_world/robot.xml"; virtual ~RobotConfig(){}; }; struct RobotState { @@ -91,6 +119,17 @@ struct GripperState { virtual ~GripperState(){}; }; +enum GraspType { POWER_GRASP = 0, + PRECISION_GRASP, + LATERAL_GRASP, + TRIPOD_GRASP }; +struct HandConfig { + virtual ~HandConfig(){}; +}; +struct HandState { + virtual ~HandState(){}; +}; + class Robot { public: virtual ~Robot(){}; @@ -115,6 +154,8 @@ class Robot { virtual void reset() = 0; + virtual void close() = 0; + virtual void set_cartesian_position(const Pose& pose) = 0; virtual std::optional> get_ik() = 0; @@ -156,6 +197,42 @@ class Gripper { // puts the gripper to max position virtual void reset() = 0; + + // closes connection to gripper + virtual void close() = 0; +}; + +class Hand { + public: + virtual ~Hand(){}; + // TODO: Add low-level control interface for the hand with internal state updates + // Also add an implementation specific set_parameters function that takes + // a deduced config type + // bool set_parameters(const GConfig& cfg); + + virtual HandConfig* get_parameters() = 0; + virtual HandState* get_state() = 0; + + // set width of the hand, 0 is closed, 1 is open + virtual void set_normalized_joint_poses(const VectorXd& q) = 0; + virtual VectorXd get_normalized_joint_poses() = 0; + + virtual bool is_grasped() = 0; + + // close hand with force, return true if the object is grasped successfully + virtual void grasp() = 0; + + // open hand + virtual void open() = 0; + + // close hand without applying force + virtual void shut() = 0; + + // puts the hand to max position + virtual void reset() = 0; + + // closes connection + virtual void close() = 0; }; } // namespace common diff --git a/include/rcs/utils.h b/include/rcs/utils.h index 53889bcd..472c54de 100644 --- a/include/rcs/utils.h +++ b/include/rcs/utils.h @@ -9,6 +9,7 @@ namespace common { typedef Eigen::Matrix Vector6d; typedef Eigen::Matrix Vector7d; +typedef Eigen::Matrix Vector16d; typedef Eigen::Matrix VectorXd; typedef Eigen::Matrix Vector7i; diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index 413ddfa5..4ebbc948 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -1,21 +1,83 @@ """Robot control stack python bindings.""" -import pathlib +import os import site +from dataclasses import dataclass from gymnasium import register from rcs._core import __version__, common -from rcs.envs.creators import FR3SimplePickUpSimEnvCreator +from rcs.envs.creators import ( + FR3LabDigitGripperPickUpSimEnvCreator, + FR3SimplePickUpSimEnvCreator, +) from rcs import camera, envs, hand, sim + +@dataclass(kw_only=True) +class Scene: + """Scene configuration.""" + + mjb: str + """Path to the Mujoco binary scene file.""" + mjcf_scene: str + """Path to the Mujoco scene XML file.""" + mjcf_robot: str + """Path to the Mujoco robot XML file for IK.""" + urdf: str | None = None + """Path to the URDF robot file for IK, if available.""" + robot_type: common.RobotType + """Type of the robot in the scene.""" + + # available mujoco scenes -scenes: dict[str, dict[str, pathlib.Path]] = { - path.stem: { - "mjb": path / "scene.mjb", - "urdf": path / "fr3.urdf", - } - for path in (pathlib.Path(site.getsitepackages()[0]) / "rcs" / "scenes").glob("*") +scenes: dict[str, Scene] = { + "fr3_empty_world": Scene( + mjb=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "fr3_empty_world", "scene.mjb"), + mjcf_scene=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "fr3_empty_world", "scene.xml" + ), + mjcf_robot=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "fr3_empty_world", "robot.xml" + ), + urdf=os.path.join(os.path.dirname(__file__), "..", "..", "assets", "scenes", "fr3_empty_world", "robot.urdf"), + robot_type=common.RobotType.FR3, + ), + "fr3_simple_pick_up": Scene( + mjb=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "fr3_simple_pick_up", "scene.mjb"), + mjcf_scene=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "fr3_simple_pick_up", "scene.xml" + ), + mjcf_robot=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "fr3_simple_pick_up", "robot.xml" + ), + urdf=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "fr3_simple_pick_up", "robot.urdf" + ), + robot_type=common.RobotType.FR3, + ), + "xarm7_empty_world": Scene( + mjb=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "xarm7_empty_world", "scene.mjb"), + mjcf_scene=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "xarm7_empty_world", "scene.xml" + ), + mjcf_robot=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "xarm7_empty_world", "robot.xml" + ), + urdf=os.path.join(os.path.dirname(__file__), "..", "..", "assets", "scenes", "xarm7_empty_world", "robot.urdf"), + robot_type=common.RobotType.FR3, + ), + "so101_empty_world": Scene( + mjb=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "so101_empty_world", "scene.mjb"), + mjcf_scene=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "so101_empty_world", "scene.xml" + ), + mjcf_robot=os.path.join( + os.path.dirname(__file__), "..", "..", "assets", "scenes", "so101_empty_world", "robot.xml" + ), + urdf=os.path.join(os.path.dirname(__file__), "..", "..", "assets", "scenes", "so101_empty_world", "robot.urdf"), + robot_type=common.RobotType.FR3, + ), } # make submodules available @@ -26,6 +88,12 @@ id="rcs/FR3SimplePickUpSim-v0", entry_point=FR3SimplePickUpSimEnvCreator(), ) +register( + id="rcs/FR3LabDigitGripperPickUpSim-v0", + entry_point=FR3LabDigitGripperPickUpSimEnvCreator(), +) + +# Genius TODO: Add the tacto version of the SimEnvCreator # TODO: gym.make("rcs/FR3SimEnv-v0") results in a pickling error: # TypeError: cannot pickle 'rcs._core.sim.SimRobotConfig' object # cf. https://pybind11.readthedocs.io/en/stable/advanced/classes.html#deepcopy-support diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 3238eb55..8da2e256 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -13,14 +13,21 @@ __all__ = [ "BaseCameraConfig", "FR3", "FrankaHandTCPOffset", + "GraspType", "Gripper", "GripperConfig", "GripperState", "HARDWARE", + "Hand", + "HandConfig", + "HandState", "IK", "IdentityRotMatrix", "IdentityRotQuatVec", "IdentityTranslation", + "LATERAL_GRASP", + "POWER_GRASP", + "PRECISION_GRASP", "Pin", "Pose", "RL", @@ -33,7 +40,9 @@ __all__ = [ "RobotType", "SIMULATION", "SO101", + "TRIPOD_GRASP", "UR5e", + "XArm7", "robots_meta_config", ] M = typing.TypeVar("M", bound=int) @@ -46,7 +55,44 @@ class BaseCameraConfig: resolution_width: int def __init__(self, identifier: str, frame_rate: int, resolution_width: int, resolution_height: int) -> None: ... +class GraspType: + """ + Members: + + POWER_GRASP + + PRECISION_GRASP + + LATERAL_GRASP + + TRIPOD_GRASP + """ + + LATERAL_GRASP: typing.ClassVar[GraspType] # value = + POWER_GRASP: typing.ClassVar[GraspType] # value = + PRECISION_GRASP: typing.ClassVar[GraspType] # value = + TRIPOD_GRASP: typing.ClassVar[GraspType] # value = + __members__: typing.ClassVar[ + dict[str, GraspType] + ] # value = {'POWER_GRASP': , 'PRECISION_GRASP': , 'LATERAL_GRASP': , 'TRIPOD_GRASP': } + def __eq__(self, other: typing.Any) -> bool: ... + def __getstate__(self) -> int: ... + def __hash__(self) -> int: ... + def __index__(self) -> int: ... + def __init__(self, value: int) -> None: ... + def __int__(self) -> int: ... + def __ne__(self, other: typing.Any) -> bool: ... + def __repr__(self) -> str: ... + def __setstate__(self, state: int) -> None: ... + def __str__(self) -> str: ... + @property + def name(self) -> str: ... + @property + def value(self) -> int: ... + class Gripper: + def __init__(self) -> None: ... + def close(self) -> None: ... def get_normalized_width(self) -> float: ... def get_parameters(self) -> GripperConfig: ... def get_state(self) -> GripperState: ... @@ -63,6 +109,25 @@ class GripperConfig: class GripperState: pass +class Hand: + def __init__(self) -> None: ... + def close(self) -> None: ... + def get_normalized_joint_poses(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... + def get_parameters(self) -> HandConfig: ... + def get_state(self) -> HandState: ... + def grasp(self) -> None: ... + def is_grasped(self) -> bool: ... + def open(self) -> None: ... + def reset(self) -> None: ... + def set_normalized_joint_poses(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ... + def shut(self) -> None: ... + +class HandConfig: + def __init__(self) -> None: ... + +class HandState: + def __init__(self) -> None: ... + class IK: def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... def ik( @@ -153,6 +218,8 @@ class RPY: ) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ... class Robot: + def __init__(self) -> None: ... + def close(self) -> None: ... def get_base_pose_in_world_coordinates(self) -> Pose: ... def get_cartesian_position(self) -> Pose: ... def get_ik(self) -> IK | None: ... @@ -167,8 +234,11 @@ class Robot: def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ... class RobotConfig: + attachment_site: str + kinematic_model_path: str robot_platform: RobotPlatform robot_type: RobotType + tcp_offset: Pose def __init__(self) -> None: ... class RobotMetaConfig: @@ -220,14 +290,17 @@ class RobotType: UR5e SO101 + + XArm7 """ FR3: typing.ClassVar[RobotType] # value = SO101: typing.ClassVar[RobotType] # value = UR5e: typing.ClassVar[RobotType] # value = + XArm7: typing.ClassVar[RobotType] # value = __members__: typing.ClassVar[ dict[str, RobotType] - ] # value = {'FR3': , 'UR5e': , 'SO101': } + ] # value = {'FR3': , 'UR5e': , 'SO101': , 'XArm7': } def __eq__(self, other: typing.Any) -> bool: ... def __getstate__(self) -> int: ... def __hash__(self) -> int: ... @@ -252,6 +325,11 @@ def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ... FR3: RobotType # value = HARDWARE: RobotPlatform # value = +LATERAL_GRASP: GraspType # value = +POWER_GRASP: GraspType # value = +PRECISION_GRASP: GraspType # value = SIMULATION: RobotPlatform # value = SO101: RobotType # value = +TRIPOD_GRASP: GraspType # value = UR5e: RobotType # value = +XArm7: RobotType # value = diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 6c6e34fe..070a0e0e 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -22,6 +22,9 @@ __all__ = [ "SimRobot", "SimRobotConfig", "SimRobotState", + "SimTilburgHand", + "SimTilburgHandConfig", + "SimTilburgHandState", "default_free", "fixed", "free", @@ -148,14 +151,12 @@ class SimRobot(rcs._core.common.Robot): class SimRobotConfig(rcs._core.common.RobotConfig): actuators: list[str] arm_collision_geoms: list[str] - attachment_site: str base: str joint_rotational_tolerance: float joints: list[str] + mjcf_scene_path: str realtime: bool - robot_type: rcs._core.common.RobotType seconds_between_callbacks: float - tcp_offset: rcs._core.common.Pose trajectory_trace: bool def __init__(self) -> None: ... def add_id(self, id: str) -> None: ... @@ -177,6 +178,36 @@ class SimRobotState(rcs._core.common.RobotState): @property def target_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... +class SimTilburgHand(rcs._core.common.Hand): + def __init__(self, sim: Sim, cfg: SimTilburgHandConfig) -> None: ... + def get_parameters(self) -> SimTilburgHandConfig: ... + def get_state(self) -> SimTilburgHandState: ... + def set_parameters(self, cfg: SimTilburgHandConfig) -> bool: ... + +class SimTilburgHandConfig(rcs._core.common.HandConfig): + actuators: list[str] + collision_geoms: list[str] + collision_geoms_fingers: list[str] + grasp_type: rcs._core.common.GraspType + ignored_collision_geoms: list[str] + joints: list[str] + max_joint_position: numpy.ndarray[tuple[typing.Literal[16]], numpy.dtype[numpy.float64]] + min_joint_position: numpy.ndarray[tuple[typing.Literal[16]], numpy.dtype[numpy.float64]] + seconds_between_callbacks: float + def __init__(self) -> None: ... + def add_id(self, id: str) -> None: ... + +class SimTilburgHandState(rcs._core.common.HandState): + def __init__(self) -> None: ... + @property + def collision(self) -> bool: ... + @property + def is_moving(self) -> bool: ... + @property + def last_commanded_qpos(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... + @property + def last_qpos(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... + default_free: CameraType # value = fixed: CameraType # value = free: CameraType # value = diff --git a/python/rcs/camera/hw.py b/python/rcs/camera/hw.py index a04e5289..460f26f7 100644 --- a/python/rcs/camera/hw.py +++ b/python/rcs/camera/hw.py @@ -83,7 +83,7 @@ def calibrate( return True def get_extrinsics(self) -> np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None: - return np.eye(4) + return np.eye(4) # type: ignore[return-value] class HardwareCameraSet(BaseCameraSet): diff --git a/python/rcs/camera/sim.py b/python/rcs/camera/sim.py index 1ec1ee94..a374bc97 100644 --- a/python/rcs/camera/sim.py +++ b/python/rcs/camera/sim.py @@ -109,7 +109,7 @@ def _extrinsics(self, camera_name) -> np.ndarray[tuple[Literal[4], Literal[4]], cam = common.Pose(rotation=xmat, translation=xpos) # put z axis infront - rotation_p = common.Pose(rpy_vector=np.array([np.pi, 0, 0]), translation=np.array([0, 0, 0])) + rotation_p = common.Pose(rpy_vector=np.array([np.pi, 0, 0]), translation=np.array([0, 0, 0])) # type: ignore cam = cam * rotation_p return cam.inverse().pose_matrix() diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index 50c17c91..288438f6 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -7,6 +7,7 @@ import gymnasium as gym import numpy as np +from rcs._core.common import Hand from rcs.camera.interface import BaseCameraSet from rcs.envs.space_utils import ( ActObsInfoWrapper, @@ -18,7 +19,6 @@ get_space, get_space_keys, ) -from rcs.hand.interface import BaseHand from rcs import common @@ -104,7 +104,7 @@ class GripperDictType(RCSpaceType): class HandBinDictType(RCSpaceType): # 0 for closed, 1 for open (>=0.5 for open) - hand: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] + gripper: Annotated[float, gym.spaces.Box(low=0, high=1, dtype=np.float32)] class HandVecDictType(RCSpaceType): @@ -246,7 +246,7 @@ def override_control_mode(self, control_mode: ControlMode): def get_obs(self) -> ArmObsType: return ArmObsType( tquat=np.concatenate( - [self.robot.get_cartesian_position().translation(), self.robot.get_cartesian_position().rotation_q()] + [self.robot.get_cartesian_position().translation(), self.robot.get_cartesian_position().rotation_q()] # type: ignore ), joints=self.robot.get_joint_position(), xyzrpy=self.robot.get_cartesian_position().xyzrpy(), @@ -291,18 +291,20 @@ def reset( self, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[ArmObsType, dict[str, Any]]: if seed is not None: - msg = "seeding not implemented yet" - raise NotImplementedError(msg) + msg = "seeding not implemented yet. Ignoring seed." + # raise NotImplementedError(msg) + _logger.error(msg) if options is not None: - msg = "options not implemented yet" - raise NotImplementedError(msg) + msg = "options not implemented yet. Ignoring options." + # raise NotImplementedError(msg) + _logger.error(msg) self.robot.reset() if self.home_on_reset: self.robot.move_home() return self.get_obs(), {} def close(self): - super().close() + self.robot.close() class MultiRobotWrapper(gym.Env): @@ -516,12 +518,12 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: self._last_action = clipped_pose_offset unclipped_pose = common.Pose( - translation=self._origin.translation() + clipped_pose_offset.translation(), + translation=self._origin.translation() + clipped_pose_offset.translation(), # type: ignore rpy_vector=(clipped_pose_offset * self._origin).rotation_rpy().as_vector(), ) action.update( TRPYDictType( - xyzrpy=np.concatenate( + xyzrpy=np.concatenate( # type: ignore [ np.clip(unclipped_pose.translation(), pose_space.low[:3], pose_space.high[:3]), unclipped_pose.rotation_rpy().as_vector(), @@ -560,13 +562,13 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: self._last_action = clipped_pose_offset unclipped_pose = common.Pose( - translation=self._origin.translation() + clipped_pose_offset.translation(), + translation=self._origin.translation() + clipped_pose_offset.translation(), # type: ignore quaternion=(clipped_pose_offset * self._origin).rotation_q(), ) action.update( TQuatDictType( - tquat=np.concatenate( + tquat=np.concatenate( # type: ignore [ np.clip(unclipped_pose.translation(), pose_space.low[:3], pose_space.high[:3]), unclipped_pose.rotation_q(), @@ -749,7 +751,7 @@ class HandWrapper(ActObsInfoWrapper): BINARY_HAND_CLOSED = 0 BINARY_HAND_OPEN = 1 - def __init__(self, env, hand: BaseHand, binary: bool = True): + def __init__(self, env, hand: Hand, binary: bool = True): super().__init__(env) self.unwrapped: RobotEnv self.observation_space: gym.spaces.Dict @@ -778,7 +780,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl self._last_hand_cmd if self._last_hand_cmd is not None else self.BINARY_HAND_OPEN ) else: - observation[self.hand_key] = self._hand.get_normalized_joints_poses() + observation[self.hand_key] = self._hand.get_normalized_joint_poses() info = {} return observation, info @@ -798,7 +800,10 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: else: self._hand.open() else: - self._hand.set_normalized_joints_poses(hand_action) + self._hand.set_normalized_joint_poses(hand_action) self._last_hand_cmd = hand_action del action[self.hand_key] return action + + def close(self): + self._hand.close() diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index cde695ae..865b604b 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -1,6 +1,6 @@ import logging import typing -from os import PathLike +from functools import partial from typing import Type import gymnasium as gym @@ -13,15 +13,17 @@ CameraSetWrapper, ControlMode, GripperWrapper, + HandWrapper, RelativeActionSpace, RelativeTo, RobotEnv, ) from rcs.envs.sim import ( - CollisionGuard, GripperWrapperSim, + HandWrapperSim, PickCubeSuccessWrapper, RandomCubePos, + RandomObjectPos, RobotSimWrapper, SimWrapper, ) @@ -45,11 +47,10 @@ def __call__( # type: ignore robot_cfg: rcs.sim.SimRobotConfig, collision_guard: bool = False, gripper_cfg: rcs.sim.SimGripperConfig | None = None, + hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, cameras: dict[str, SimCameraConfig] | None = None, max_relative_movement: float | tuple[float, float] | None = None, relative_to: RelativeTo = RelativeTo.LAST_STEP, - urdf_path: str | PathLike | None = None, - mjcf: str | PathLike = "fr3_empty_world", sim_wrapper: Type[SimWrapper] | None = None, ) -> gym.Env: """ @@ -60,32 +61,28 @@ def __call__( # type: ignore robot_cfg (rcs.sim.SimRobotConfig): Configuration for the FR3 robot. collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding. gripper_cfg (rcs.sim.SimGripperConfig | None): Configuration for the gripper. If None, no gripper is used. + Cannot be used together with hand_cfg. + hand_cfg (rcs.sim.SimHandConfig | None): Configuration for the hand. If None, no hand is used. + Cannot be used together with gripper_cfg. camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used. max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts translational movement in meters. If tuple, it restricts both translational (in meters) and rotational (in radians) movements. If None, no restriction is applied. relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced - which requires a UTN compatible lab scene to be present. - mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world". sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. Returns: gym.Env: The configured simulation environment for the FR3 robot. """ - if urdf_path is None: - urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - if mjcf not in rcs.scenes: - logger.info("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") - if mjcf in rcs.scenes: - assert isinstance(mjcf, str) - mjcf = rcs.scenes[mjcf]["mjb"] - simulation = sim.Sim(mjcf) - - # ik = rcs.common.Pin("/home/juelg/code/internal_rcs/assets/scenes/fr3_empty_world/fr3_0.xml", "attachment_site_0", urdf=False) - # ik = rcs.common.Pin(str(urdf_path)) - ik = rcs.common.RL(str(urdf_path)) + simulation = sim.Sim(robot_cfg.mjcf_scene_path) + + ik = rcs.common.Pin( + robot_cfg.kinematic_model_path, + robot_cfg.attachment_site, + urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), + ) + # ik = rcs.common.RL(robot_cfg.kinematic_model_path) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) env: gym.Env = RobotEnv(robot, control_mode) @@ -97,23 +94,33 @@ def __call__( # type: ignore ) env = CameraSetWrapper(env, camera_set, include_depth=True) + assert not ( + hand_cfg is not None and gripper_cfg is not None + ), "Hand and gripper configurations cannot be used together." + + if hand_cfg is not None and isinstance(hand_cfg, rcs.sim.SimTilburgHandConfig): + hand = sim.SimTilburgHand(simulation, hand_cfg) + env = HandWrapper(env, hand, binary=True) + env = HandWrapperSim(env, hand) + if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig): gripper = sim.SimGripper(simulation, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) env = GripperWrapperSim(env, gripper) - if collision_guard: - env = CollisionGuard.env_from_xml_paths( - env, - str(rcs.scenes.get(str(mjcf), mjcf)), - str(urdf_path), - gripper=gripper_cfg is not None, - check_home_collision=False, - control_mode=control_mode, - tcp_offset=rcs.common.Pose(rcs.common.FrankaHandTCPOffset()), - sim_gui=True, - truncate_on_collision=True, - ) + # TODO: collision guard not working atm + # if collision_guard: + # env = CollisionGuard.env_from_xml_paths( + # env, + # mjcf, + # robot_kinematics, + # gripper=gripper_cfg is not None, + # check_home_collision=False, + # control_mode=control_mode, + # tcp_offset=rcs.common.Pose(rcs.common.FrankaHandTCPOffset()), + # sim_gui=True, + # truncate_on_collision=True, + # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) @@ -123,25 +130,54 @@ def __call__( # type: ignore class SimTaskEnvCreator(EnvCreator): def __call__( # type: ignore self, - mjcf: str, + robot_cfg: rcs.sim.SimRobotConfig, render_mode: str = "human", control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, delta_actions: bool = True, cameras: dict[str, SimCameraConfig] | None = None, + hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, + gripper_cfg: rcs.sim.SimGripperConfig | None = None, + random_pos_args: dict | None = None, ) -> gym.Env: + mode = "gripper" + if gripper_cfg is None and hand_cfg is None: + _gripper_cfg = default_sim_gripper_cfg() + _hand_cfg = None + logger.info("Using default gripper configuration.") + elif hand_cfg is not None: + _gripper_cfg = None + _hand_cfg = hand_cfg + mode = "hand" + logger.info("Using hand configuration.") + else: + # Either both cfgs are set, or only gripper_cfg is set + _gripper_cfg = gripper_cfg + _hand_cfg = None + logger.info("Using gripper configuration.") + + random_env = RandomCubePos + if random_pos_args is not None: + # check that all the keys are there + required_keys = ["joint_name", "init_object_pose"] + if not all(key in random_pos_args for key in required_keys): + missing_keys = [key for key in required_keys if key not in random_pos_args] + logger.warning(f"Missing random position arguments: {missing_keys}; Defaulting to RandomCubePos") + random_env = partial(RandomObjectPos, **random_pos_args) # type: ignore env_rel = SimEnvCreator()( control_mode=control_mode, - robot_cfg=default_sim_robot_cfg(mjcf), + robot_cfg=robot_cfg, collision_guard=False, - gripper_cfg=default_sim_gripper_cfg(), + gripper_cfg=_gripper_cfg, + hand_cfg=_hand_cfg, cameras=cameras, max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, relative_to=RelativeTo.LAST_STEP, - mjcf=mjcf, - sim_wrapper=RandomCubePos, + sim_wrapper=random_env, ) - env_rel = PickCubeSuccessWrapper(env_rel) + if mode == "gripper": + env_rel = PickCubeSuccessWrapper(env_rel) + if render_mode == "human": env_rel.get_wrapper_attr("sim").open_gui() @@ -156,53 +192,74 @@ def __call__( # type: ignore resolution: tuple[int, int] | None = None, frame_rate: int = 0, delta_actions: bool = True, + cam_list: list[str] | None = None, ) -> gym.Env: + if cam_list is None: + cam_list = [] if resolution is None: resolution = (256, 256) - cameras = { - "wrist": SimCameraConfig( - identifier="wrist_0", + cam: SimCameraConfig( + identifier=cam, type=CameraType.fixed, resolution_height=resolution[1], resolution_width=resolution[0], frame_rate=frame_rate, - ), - "bird_eye": SimCameraConfig( - identifier="bird_eye_cam", - type=CameraType.fixed, - resolution_height=resolution[1], - resolution_width=resolution[0], - frame_rate=frame_rate, - ), - "side": SimCameraConfig( - identifier="side_view", - type=CameraType.fixed, - resolution_height=resolution[1], - resolution_width=resolution[0], - frame_rate=frame_rate, - ), - "right_side": SimCameraConfig( - identifier="right_side", - type=CameraType.fixed, - resolution_height=resolution[1], - resolution_width=resolution[0], - frame_rate=frame_rate, - ), - "left_side": SimCameraConfig( - identifier="left_side", - type=CameraType.fixed, - resolution_height=resolution[1], - resolution_width=resolution[0], - frame_rate=frame_rate, - ), - "front": SimCameraConfig( - identifier="front", + ) + for cam in cam_list + } + robot_cfg = default_sim_robot_cfg(scene="fr3_simple_pick_up") + + return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras) + + +class FR3LabDigitGripperPickUpSimEnvCreator(EnvCreator): + def __call__( # type: ignore + self, + render_mode: str = "human", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + resolution: tuple[int, int] | None = None, + frame_rate: int = 0, + delta_actions: bool = True, + cam_list: list[str] | None = None, + mjcf_path: str = "", + ) -> gym.Env: + if cam_list is None: + cam_list = [] + if resolution is None: + resolution = (256, 256) + if cam_list is None or len(cam_list) == 0: + error_msg = "cam_list must contain at least one camera name." + raise ValueError(error_msg) + cameras = { + cam: SimCameraConfig( + identifier=cam, type=CameraType.fixed, resolution_height=resolution[1], resolution_width=resolution[0], frame_rate=frame_rate, - ), + ) + for cam in cam_list } + robot_cfg = rcs.sim.SimRobotConfig() + robot_cfg.tcp_offset = rcs.common.Pose( + translation=np.array([0.0, 0.0, 0.15]), # type: ignore + rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]), # type: ignore + ) + robot_cfg.robot_type = rcs.common.RobotType.FR3 + robot_cfg.realtime = False + robot_cfg.add_id("0") # only required for fr3 + robot_cfg.mjcf_scene_path = mjcf_path + robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot # .urdf (in case for urdf) + print( + f"Creating FR3LabDigitGripperPickUpSim with the following parameters: \n" + f" render_mode: {render_mode}\n" + f" control_mode: {control_mode}\n" + f" resolution: {resolution}\n" + f" frame_rate: {frame_rate}\n" + f" delta_actions: {delta_actions}\n" + f" cameras: {cameras}\n" + f" mjcf_path: {mjcf_path}\n" + ) - return SimTaskEnvCreator()("fr3_simple_pick_up", render_mode, control_mode, delta_actions, cameras) + return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 283b246b..ee36ab60 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -3,9 +3,15 @@ import gymnasium as gym import numpy as np -from rcs.envs.base import ControlMode, GripperWrapper, MultiRobotWrapper, RobotEnv +from rcs.envs.base import ( + ControlMode, + GripperWrapper, + HandWrapper, + MultiRobotWrapper, + RobotEnv, +) from rcs.envs.space_utils import ActObsInfoWrapper -from rcs.envs.utils import default_sim_robot_cfg +from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg import rcs from rcs import sim @@ -114,6 +120,28 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl return observation, info +class HandWrapperSim(ActObsInfoWrapper): + def __init__(self, env, hand: sim.SimTilburgHand): + super().__init__(env) + self._hand = hand + + def action(self, action: dict[str, Any]) -> dict[str, Any]: + if isinstance(action["hand"], int | float): + return action + if len(action["hand"]) == 18: + action["hand"] = action["hand"][:16] + assert len(action["hand"]) == 16 or len(action["hand"]) == 1, "Hand action must be of length 16 or 1" + return action + + def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: + state = self._hand.get_state() + if "collision" not in info or not info["collision"]: + info["collision"] = state.collision + info["hand_position"] = self._hand.get_normalized_joint_poses() + # info["is_grasped"] = self._hand.get_normalized_joint_poses() > 0.01 and self._hand.get_normalized_joint_poses() < 0.99 + return observation, info + + class CollisionGuard(gym.Wrapper[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]): """ - Gripper Wrapper has to be added before this as it removes the gripper action @@ -193,20 +221,22 @@ def env_from_xml_paths( cls, env: gym.Env, mjmld: str, - urdf: str, + cg_kinematics_path: str, id: str = "0", gripper: bool = True, + hand: bool = False, check_home_collision: bool = True, tcp_offset: rcs.common.Pose | None = None, control_mode: ControlMode | None = None, sim_gui: bool = True, truncate_on_collision: bool = True, ) -> "CollisionGuard": + # TODO: remove urdf and use mjcf # TODO: this needs to support non FR3 robots assert isinstance(env.unwrapped, RobotEnv) simulation = sim.Sim(mjmld) - ik = rcs.common.RL(urdf, max_duration_ms=300) cfg = default_sim_robot_cfg(mjmld, id) + ik = rcs.common.Pin(cg_kinematics_path, cfg.attachment_site, False) cfg.realtime = False if tcp_offset is not None: cfg.tcp_offset = tcp_offset @@ -229,6 +259,13 @@ def env_from_xml_paths( fh = sim.SimGripper(simulation, gripper_cfg) c_env = GripperWrapper(c_env, fh) c_env = GripperWrapperSim(c_env, fh) + if hand: + hand_cfg = default_sim_tilburg_hand_cfg() + # hand_cfg.add_id(id) + th = sim.SimTilburgHand(simulation, hand_cfg) + c_env = HandWrapper(c_env, th) + c_env = HandWrapperSim(c_env, th) + return cls( env=env, simulation=simulation, @@ -240,6 +277,74 @@ def env_from_xml_paths( ) +class RandomObjectPos(SimWrapper): + """ + Wrapper to randomly re-place an object in the lab environments. + Given the object's joint name and initial pose, its x, y coordinates are randomized, while z remains fixed. + If include_rotation is true, the object's z-axis rotation (yaw) is also randomized. + + Args: + env (gym.Env): The environment to wrap. + simulation (sim.Sim): The simulation instance. + joint_name (str): The name of the free joint attached to the object to manipulate. + init_object_pose (rcs.common.Pose): The initial pose of the object. + include_rotation (bool): Whether to include rotation in the randomization. + """ + + def __init__( + self, + env: gym.Env, + simulation: sim.Sim, + joint_name: str, + init_object_pose: rcs.common.Pose, + include_position: bool = True, + include_rotation: bool = False, + ): + super().__init__(env, simulation) + self.joint_name = joint_name + self.init_object_pose = init_object_pose + self.include_position = include_position + self.include_rotation = include_rotation + + def reset( + self, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict[str, Any], dict[str, Any]]: + if options is not None and "RandomObjectPos.init_object_pose" in options: + assert isinstance( + options["RandomObjectPos.init_object_pose"], rcs.common.Pose + ), "RandomObjectPos.init_object_pose must be a rcs.common.Pose" + + self.init_object_pose = options["RandomObjectPos.init_object_pose"] + print("Got random object pos!\n", self.init_object_pose) + del options["RandomObjectPos.init_object_pose"] + obs, info = super().reset(seed=seed, options=options) + self.sim.step(1) + + pos_z = self.init_object_pose.translation()[2] + if self.include_position: + pos_x = self.init_object_pose.translation()[0] + np.random.random() * 0.2 - 0.1 + pos_y = self.init_object_pose.translation()[1] + np.random.random() * 0.2 - 0.1 + else: + pos_x = self.init_object_pose.translation()[0] + pos_y = self.init_object_pose.translation()[1] + + quat = self.init_object_pose.rotation_q() # xyzw format + if self.include_rotation: + self.sim.data.joint(self.joint_name).qpos = [ + pos_x, + pos_y, + pos_z, + 2 * np.random.random() - quat[3], + quat[0], + quat[1], + quat[2], + ] + else: + self.sim.data.joint(self.joint_name).qpos = [pos_x, pos_y, pos_z, quat[3], quat[0], quat[1], quat[2]] + + return obs, info + + class RandomCubePos(SimWrapper): """Wrapper to randomly place cube in the lab environments.""" @@ -254,7 +359,7 @@ def reset( self.sim.step(1) iso_cube = np.array([0.498, 0.0, 0.226]) - iso_cube_pose = rcs.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) + iso_cube_pose = rcs.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) # type: ignore iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() pos_z = 0.826 pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 0287e3ae..d4fed97c 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -17,13 +17,16 @@ logger.setLevel(logging.INFO) -def default_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig: - cfg = sim.SimRobotConfig() - cfg.tcp_offset = get_tcp_offset(mjcf) - cfg.realtime = False - cfg.robot_type = rcs.common.RobotType.FR3 - cfg.add_id(idx) - return cfg +def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig: + robot_cfg = rcs.sim.SimRobotConfig() + robot_cfg.tcp_offset = get_tcp_offset(rcs.scenes[scene].mjcf_scene) + robot_cfg.realtime = False + robot_cfg.robot_type = rcs.scenes[scene].robot_type + robot_cfg.add_id(idx) + robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjb + robot_cfg.kinematic_model_path = rcs.scenes[scene].mjcf_robot + # robot_cfg.kinematic_model_path = rcs.scenes[scene].urdf + return robot_cfg def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig: @@ -39,6 +42,10 @@ def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: return cfg +def default_sim_tilburg_hand_cfg() -> sim.SimTilburgHandConfig: + return sim.SimTilburgHandConfig() + + def default_digit(name2id: dict[str, str] | None, stream_name: str = "QVGA") -> DigitCam | None: if name2id is None: return None @@ -79,24 +86,21 @@ def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose: Returns: rcs.common.Pose: The tcp offset. """ - if mjcf in rcs.scenes: - model = mj.MjModel.from_binary_path(str(rcs.scenes[str(mjcf)]["mjb"])) + mjcf = Path(mjcf) + if mjcf.suffix in (".xml", ".mjcf"): + model = mj.MjModel.from_xml_path(str(mjcf)) + elif mjcf.suffix == ".mjb": + model = mj.MjModel.from_binary_path(str(mjcf)) else: - mjcf = Path(mjcf) - if mjcf.suffix in (".xml", ".mjcf"): - model = mj.MjModel.from_xml_path(str(mjcf)) - elif mjcf.suffix == ".mjb": - model = mj.MjModel.from_binary_path(str(mjcf)) - else: - msg = f"Expected .mjb, .mjcf or.xml, got {mjcf.suffix} and {mjcf}" - raise AssertionError(msg) + msg = f"Expected .mjb, .mjcf or.xml, got {mjcf.suffix} and {mjcf}" + raise AssertionError(msg) try: tcp_offset_translation = np.array(model.numeric("tcp_offset_translation").data) tcp_offset_rotation_matrix = np.array(model.numeric("tcp_offset_rotation_matrix").data) return rcs.common.Pose( - translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3)) # type: ignore + translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3)) # type: ignore ) except KeyError: - msg = "No tcp offset found in the model. Using the default tcp offset." - logging.info(msg) + msg = "No tcp offset found in the model. Using an identity transform as tcp offset." + logging.warning(msg) return rcs.common.Pose() diff --git a/python/rcs/hand/interface.py b/python/rcs/hand/interface.py deleted file mode 100644 index 2ce5b700..00000000 --- a/python/rcs/hand/interface.py +++ /dev/null @@ -1,31 +0,0 @@ -from typing import Protocol - -import numpy as np - - -class BaseHand(Protocol): - """ - Hand Class - This class provides an interface for hand control. - """ - - def grasp(self): - pass - - def open(self): - pass - - def reset(self): - pass - - def close(self): - pass - - def get_state(self) -> np.ndarray: - pass - - def get_normalized_joints_poses(self) -> np.ndarray: - pass - - def set_normalized_joints_poses(self, values: np.ndarray): - pass diff --git a/python/rcs/hand/tilburg_hand.py b/python/rcs/hand/tilburg_hand.py index 7c0a9b6b..30b44a08 100644 --- a/python/rcs/hand/tilburg_hand.py +++ b/python/rcs/hand/tilburg_hand.py @@ -1,11 +1,12 @@ import copy import logging +import typing from dataclasses import dataclass from time import sleep import numpy as np +from rcs._core import common from rcs.envs.space_utils import Vec18Type -from rcs.hand.interface import BaseHand from tilburg_hand import Finger, TilburgHandMotorInterface, Unit # Setup logger @@ -15,16 +16,29 @@ @dataclass(kw_only=True) -class THConfig: +class THConfig(common.HandConfig): """Config for the Tilburg hand""" calibration_file: str | None = None grasp_percentage: float = 1.0 control_unit: Unit = Unit.NORMALIZED hand_orientation: str = "right" + grasp_type: common.GraspType = common.GraspType.POWER_GRASP + def __post_init__(self): + # 👇 satisfy pybind11 by actually calling the C++ constructor + super().__init__() -class TilburgHand(BaseHand): + +@dataclass +class TilburgHandState(common.HandState): + joint_positions: Vec18Type + + def __post_init__(self): + super().__init__() + + +class TilburgHand(common.Hand): """ Tilburg Hand Class This class provides an interface for controlling the Tilburg Hand. @@ -35,10 +49,60 @@ class TilburgHand(BaseHand): [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0] ) + # TODO: Control mode for position control and pos+effort control + + POWER_GRASP_VALUES = np.array( + [ + 0.5, + 0.5, + 0.5, + 1.4, # THUMB_(IP, MCP, ABD, CMC) + 0.5, + 0.5, + 1.0, + 0.7, # INDEX_(DIP, PIP, MCP, ABD) + 0.5, + 0.5, + 1.0, + 0.3, + 0.5, + 0.5, + 1.0, + 0.0, + 0, + 0, + ], + dtype=np.float32, + ) + OPEN_VALUES = np.array( + [ + 0.0, + 0.0, + 0.5, + 1.4, # THUMB_(IP, MCP, ABD, CMC) + 0.2, + 0.2, + 0.2, + 0.7, # INDEX_(DIP, PIP, MCP, ABD) + 0.2, + 0.2, + 0.2, + 0.3, + 0.2, + 0.2, + 0.2, + 0.0, + 0, + 0, + ], + dtype=np.float32, + ) + def __init__(self, cfg: THConfig, verbose: bool = False): """ Initializes the Tilburg Hand interface. """ + super().__init__() self._cfg = cfg self._motors = TilburgHandMotorInterface( @@ -68,25 +132,24 @@ def set_pos_vector(self, pos_vector: Vec18Type): """ Sets the position vector for the motors. """ - assert len(pos_vector) == len( + assert len(pos_vector) == ( self._motors.n_motors - ), f"Invalid position vector length: {len(pos_vector)}. Expected: {len(self._motors.n_motors)}" + ), f"Invalid position vector length: {len(pos_vector)}. Expected: {self._motors.n_motors}" self._motors.set_pos_vector(copy.deepcopy(pos_vector), unit=self._cfg.control_unit) - logger.info(f"Set pose vector: {pos_vector}") def set_zero_pos(self): """ Sets all finger joint positions to zero. """ - pos_normalized = 0 * self.MAX_GRASP_JOINTS_VALS - self._motors.set_pos_vector(pos_normalized, unit=self._cfg.control_unit) + pos_normalized = typing.cast(Vec18Type, 0 * self.MAX_GRASP_JOINTS_VALS) + self.set_pos_vector(pos_normalized) logger.info("All joints reset to zero position.") def set_joint_pos(self, finger_joint: Finger, pos_value: float): """ Sets a single joint to a specific normalized position. """ - self._motors.set_pos_single(finger_joint, pos_value, unit=self._cfg.control_unit) + self._motors.set_pos_single(finger_joint, copy.deepcopy(pos_value), unit=self._cfg.control_unit) def reset_joint_pos(self, finger_joint: Finger): """ @@ -115,9 +178,13 @@ def get_pos_single(self, finger_joint: Finger) -> float: return self._motors.get_encoder_single(finger_joint, self._cfg.control_unit) def _grasp(self): - pos_normalized = self._cfg.grasp_percentage * self.MAX_GRASP_JOINTS_VALS - self._motors.set_pos_vector(pos_normalized, unit=self._cfg.control_unit) - logger.info(f"Grasp command sent with value: {self._cfg.grasp_percentage:.2f}") + if self._cfg.grasp_type == common.GraspType.POWER_GRASP: + pos_normalized = self.POWER_GRASP_VALUES * self._cfg.grasp_percentage + else: + logger.warning(f"Grasp type {self._cfg.grasp_type} is not implemented. Defaulting to power grasp.") + pos_normalized = self.POWER_GRASP_VALUES * self._cfg.grasp_percentage + pos_normalized = typing.cast(Vec18Type, pos_normalized) + self.set_pos_vector(pos_normalized) def auto_recovery(self): if not np.array(self._motors.check_enabled_motors()).all(): @@ -127,6 +194,30 @@ def auto_recovery(self): re = self._motors.connect() assert re >= 0, "Failed to reconnect to the motors' board." + def set_grasp_type(self, grasp_type: common.GraspType): + """ + Sets the grasp type for the hand. + """ + if not isinstance(grasp_type, common.GraspType): + error_msg = f"Invalid grasp type: {grasp_type}. Must be an instance of common.GraspType." + raise ValueError(error_msg) + if grasp_type == common.GraspType.POWER_GRASP: + self._cfg.grasp_type = common.GraspType.POWER_GRASP + elif grasp_type == common.GraspType.PRECISION_GRASP: + logger.warning("Precision grasp is not implemented yet. Defaulting to power grasp.") + self._cfg.grasp_type = common.GraspType.POWER_GRASP + elif grasp_type == common.GraspType.LATERAL_GRASP: + logger.warning("Lateral grasp is not implemented yet. Defaulting to power grasp.") + self._cfg.grasp_type = common.GraspType.POWER_GRASP + elif grasp_type == common.GraspType.TRIPOD_GRASP: + logger.warning("Tripod grasp is not implemented yet. Defaulting to power grasp.") + self._cfg.grasp_type = common.GraspType.POWER_GRASP + else: + error_msg = f"Unknown grasp type: {grasp_type}." + raise ValueError(error_msg) + + logger.info(f"Grasp type set to: {self._cfg.grasp_type}") + #### BaseHandControl Interface methods #### def grasp(self): @@ -136,21 +227,21 @@ def grasp(self): self._grasp() def open(self): - self.set_zero_pos() + self.set_pos_vector(typing.cast(Vec18Type, self.OPEN_VALUES)) def reset(self): """ Resets the hand to its initial state. """ self.auto_recovery() - self.set_zero_pos() + self.open() logger.info("Hand reset to initial state.") - def get_state(self) -> np.ndarray: + def get_state(self) -> TilburgHandState: """ Returns the current state of the hand. """ - return self.get_pos_vector() + return TilburgHandState(joint_positions=self.get_pos_vector()) def close(self): """ @@ -159,8 +250,8 @@ def close(self): self.disconnect() logger.info("Hand control interface closed.") - def get_normalized_joints_poses(self) -> np.ndarray: + def get_normalized_joint_poses(self) -> np.ndarray: return self.get_pos_vector() - def set_normalized_joints_poses(self, values: np.ndarray): + def set_normalized_joint_poses(self, values: np.ndarray): self.set_pos_vector(values) diff --git a/python/rcs/ompl/mj_ompl.py b/python/rcs/ompl/mj_ompl.py index a8d7a459..8ec5f233 100644 --- a/python/rcs/ompl/mj_ompl.py +++ b/python/rcs/ompl/mj_ompl.py @@ -50,11 +50,11 @@ def get_collision_bodies(xml_file, robot_name): class MjORobot: franka_hand_tcp = Pose( - pose_matrix=np.array([[0.707, 0.707, 0, 0], [-0.707, 0.707, 0, 0], [0, 0, 1, 0.1034], [0, 0, 0, 1]]) + pose_matrix=np.array([[0.707, 0.707, 0, 0], [-0.707, 0.707, 0, 0], [0, 0, 1, 0.1034], [0, 0, 0, 1]]) # type: ignore ) digit_hand_tcp = Pose( - pose_matrix=np.array([[0.707, 0.707, 0, 0], [-0.707, 0.707, 0, 0], [0, 0, 1, 0.15], [0, 0, 0, 1]]) + pose_matrix=np.array([[0.707, 0.707, 0, 0], [-0.707, 0.707, 0, 0], [0, 0, 1, 0.15], [0, 0, 0, 1]]) # type: ignore ) def __init__( diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py index c9476131..036f5bb5 100644 --- a/python/rcs/sim/__init__.py +++ b/python/rcs/sim/__init__.py @@ -6,6 +6,9 @@ SimRobot, SimRobotConfig, SimRobotState, + SimTilburgHand, + SimTilburgHandConfig, + SimTilburgHandState, ) from rcs.sim.sim import Sim, gui_loop @@ -17,6 +20,9 @@ "SimGripper", "SimGripperConfig", "SimGripperState", + "SimTilburgHand", + "SimTilburgHandConfig", + "SimTilburgHandState", "gui_loop", "SimCameraConfig", ] diff --git a/python/rcs/utils.py b/python/rcs/utils.py index 38773580..65e5be3e 100644 --- a/python/rcs/utils.py +++ b/python/rcs/utils.py @@ -39,3 +39,14 @@ def __call__(self): logger.info(f"FPS {self.loop_name}: {1 / (perf_counter() - self.t)}") self.t = perf_counter() + + def get_frame_time(self) -> float: + return 1 / self.frame_rate if isinstance(self.frame_rate, int) else self.frame_rate + + +class ContextManager: + def __enter__(self): + pass + + def __exit__(self, *args): + pass diff --git a/python/tests/test_common.py b/python/tests/test_common.py index 52996ff3..aaa39080 100644 --- a/python/tests/test_common.py +++ b/python/tests/test_common.py @@ -193,7 +193,7 @@ def test_rotation_rpy(self, pose_m: np.ndarray, expected_rpy: common.RPY): def test_rpy_conversion(self): # equal to identity - assert common.Pose(translation=np.array([0, 0, 0]), rpy_vector=np.array([0, 0, 0])).is_close(common.Pose()) + assert common.Pose(translation=np.array([0, 0, 0]), rpy_vector=np.array([0, 0, 0])).is_close(common.Pose()) # type: ignore home_m = np.array( [ @@ -204,14 +204,14 @@ def test_rpy_conversion(self): ] ) - home_pose = common.Pose(pose_matrix=home_m) + home_pose = common.Pose(pose_matrix=home_m) # type: ignore assert np.allclose(home_pose.pose_matrix(), home_m) trpy = home_pose.xyzrpy() assert np.allclose(trpy[:3], home_pose.translation()) - home_pose2 = common.Pose(translation=trpy[:3], rpy_vector=trpy[3:]) + home_pose2 = common.Pose(translation=trpy[:3], rpy_vector=trpy[3:]) # type: ignore home_m2 = home_pose2.pose_matrix() assert home_pose.is_close(home_pose2) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index e2495f8e..5c95c9d4 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -1,12 +1,9 @@ -from typing import cast - import numpy as np import pytest from rcs.envs.base import ( ControlMode, GripperDictType, JointsDictType, - RobotEnv, TQuatDictType, TRPYDictType, ) @@ -40,7 +37,6 @@ class TestSimEnvs: def assert_no_pose_change(self, info: dict, initial_obs: dict, final_obs: dict): assert info["ik_success"] - assert info["is_sim_converged"] out_pose = rcs.common.Pose( translation=np.array(final_obs["tquat"][:3]), quaternion=np.array(final_obs["tquat"][3:]) ) @@ -51,7 +47,6 @@ def assert_no_pose_change(self, info: dict, initial_obs: dict, final_obs: dict): def assert_collision(self, info: dict): assert info["ik_success"] - assert info["is_sim_converged"] assert info["collision"] @@ -117,7 +112,7 @@ def test_relative_zero_action_trpy(self, cfg, gripper_cfg): ) obs_initial, _ = env.reset() # action to be performed - zero_action = TRPYDictType(xyzrpy=np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)) + zero_action = TRPYDictType(xyzrpy=np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)) # type: ignore zero_action.update(GripperDictType(gripper=0)) obs, _, _, _, info = env.step(zero_action) self.assert_no_pose_change(info, obs_initial, obs) @@ -131,7 +126,7 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg): obs_initial, _ = env.reset() # action to be performed x_pos_change = 0.2 - non_zero_action = TRPYDictType(xyzrpy=np.array([x_pos_change, 0, 0, 0, 0, 0])) + non_zero_action = TRPYDictType(xyzrpy=np.array([x_pos_change, 0, 0, 0, 0, 0])) # type: ignore non_zero_action.update(GripperDictType(gripper=0)) expected_obs = obs_initial.copy() expected_obs["tquat"][0] += x_pos_change @@ -155,32 +150,32 @@ def test_collision_trpy(self, cfg, gripper_cfg): obs, _, _, _, info = env.step(collision_action) self.assert_collision(info) - def test_collision_guard_trpy(self, cfg, gripper_cfg): - """ - Check that an obvious collision is detected by the CollisionGuard - """ - # env creation - env = SimEnvCreator()( - ControlMode.CARTESIAN_TRPY, - cfg, - gripper_cfg=gripper_cfg, - collision_guard=True, - cameras=None, - max_relative_movement=None, - ) - obs, _ = env.reset() - unwrapped = cast(RobotEnv, env.unwrapped) - p1: np.ndarray = unwrapped.robot.get_joint_position() - # an obvious below ground collision action - obs["xyzrpy"][0] = 0.4 - obs["xyzrpy"][2] = -0.05 - collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) - collision_action.update(GripperDictType(gripper=0)) - _, _, _, _, info = env.step(collision_action) - p2: np.ndarray = unwrapped.robot.get_joint_position() - self.assert_collision(info) - # assure that the robot did not move - assert np.allclose(p1, p2) + # def test_collision_guard_trpy(self, cfg, gripper_cfg): + # """ + # Check that an obvious collision is detected by the CollisionGuard + # """ + # # env creation + # env = SimEnvCreator()( + # ControlMode.CARTESIAN_TRPY, + # cfg, + # gripper_cfg=gripper_cfg, + # collision_guard=True, + # cameras=None, + # max_relative_movement=None, + # ) + # obs, _ = env.reset() + # unwrapped = cast(RobotEnv, env.unwrapped) + # p1: np.ndarray = unwrapped.robot.get_joint_position() + # # an obvious below ground collision action + # obs["xyzrpy"][0] = 0.4 + # obs["xyzrpy"][2] = -0.05 + # collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) + # collision_action.update(GripperDictType(gripper=0)) + # _, _, _, _, info = env.step(collision_action) + # p2: np.ndarray = unwrapped.robot.get_joint_position() + # self.assert_collision(info) + # # assure that the robot did not move + # assert np.allclose(p1, p2) class TestSimEnvsTquat(TestSimEnvs): @@ -249,7 +244,7 @@ def test_relative_zero_action_tquat(self, cfg, gripper_cfg): max_relative_movement=0.5, ) obs_initial, _ = env_rel.reset() - zero_rel_action = TQuatDictType(tquat=np.array([0, 0, 0, 0, 0, 0, 1.0], dtype=np.float32)) + zero_rel_action = TQuatDictType(tquat=np.array([0, 0, 0, 0, 0, 0, 1.0], dtype=np.float32)) # type: ignore zero_rel_action.update(GripperDictType(gripper=0)) obs, _, _, _, info = env_rel.step(zero_rel_action) self.assert_no_pose_change(info, obs_initial, obs) @@ -275,32 +270,32 @@ def test_collision_tquat(self, cfg, gripper_cfg): _, _, _, _, info = env.step(collision_action) self.assert_collision(info) - def test_collision_guard_tquat(self, cfg, gripper_cfg): - """ - Check that an obvious collision is detected by the CollisionGuard - """ - # env creation - env = SimEnvCreator()( - ControlMode.CARTESIAN_TQuat, - cfg, - gripper_cfg=gripper_cfg, - collision_guard=True, - cameras=None, - max_relative_movement=None, - ) - obs, _ = env.reset() - unwrapped = cast(RobotEnv, env.unwrapped) - p1: np.ndarray = unwrapped.robot.get_joint_position() - # an obvious below ground collision action - obs["tquat"][0] = 0.4 - obs["tquat"][2] = -0.05 - collision_action = TQuatDictType(tquat=obs["tquat"]) - collision_action.update(GripperDictType(gripper=0)) - _, _, _, _, info = env.step(collision_action) - p2: np.ndarray = unwrapped.robot.get_joint_position() - self.assert_collision(info) - # assure that the robot did not move - assert np.allclose(p1, p2) + # def test_collision_guard_tquat(self, cfg, gripper_cfg): + # """ + # Check that an obvious collision is detected by the CollisionGuard + # """ + # # env creation + # env = SimEnvCreator()( + # ControlMode.CARTESIAN_TQuat, + # cfg, + # gripper_cfg=gripper_cfg, + # collision_guard=True, + # cameras=None, + # max_relative_movement=None, + # ) + # obs, _ = env.reset() + # unwrapped = cast(RobotEnv, env.unwrapped) + # p1: np.ndarray = unwrapped.robot.get_joint_position() + # # an obvious below ground collision action + # obs["tquat"][0] = 0.4 + # obs["tquat"][2] = -0.05 + # collision_action = TQuatDictType(tquat=obs["tquat"]) + # collision_action.update(GripperDictType(gripper=0)) + # _, _, _, _, info = env.step(collision_action) + # p2: np.ndarray = unwrapped.robot.get_joint_position() + # self.assert_collision(info) + # # assure that the robot did not move + # assert np.allclose(p1, p2) class TestSimEnvsJoints(TestSimEnvs): @@ -364,31 +359,31 @@ def test_collision_joints(self, cfg, gripper_cfg): _, _, _, _, info = env.step(collision_act) self.assert_collision(info) - def test_collision_guard_joints(self, cfg, gripper_cfg): - """ - Check that an obvious collision is detected by sim - """ - # env creation - env = SimEnvCreator()( - ControlMode.JOINTS, - cfg, - gripper_cfg=gripper_cfg, - collision_guard=True, - cameras=None, - max_relative_movement=None, - ) - env.reset() - unwrapped = cast(RobotEnv, env.unwrapped) - p1: np.ndarray = unwrapped.robot.get_joint_position() - # the below action is a test_case where there is an obvious collision regardless of the gripper action - collision_act = JointsDictType(joints=np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)) - collision_act.update(GripperDictType(gripper=1)) - _, _, _, _, info = env.step(collision_act) - p2: np.ndarray = unwrapped.robot.get_joint_position() - - self.assert_collision(info) - # assure that the robot did not move - assert np.allclose(p1, p2) + # def test_collision_guard_joints(self, cfg, gripper_cfg): + # """ + # Check that an obvious collision is detected by sim + # """ + # # env creation + # env = SimEnvCreator()( + # ControlMode.JOINTS, + # cfg, + # gripper_cfg=gripper_cfg, + # collision_guard=True, + # cameras=None, + # max_relative_movement=None, + # ) + # env.reset() + # unwrapped = cast(RobotEnv, env.unwrapped) + # p1: np.ndarray = unwrapped.robot.get_joint_position() + # # the below action is a test_case where there is an obvious collision regardless of the gripper action + # collision_act = JointsDictType(joints=np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)) + # collision_act.update(GripperDictType(gripper=1)) + # _, _, _, _, info = env.step(collision_act) + # p2: np.ndarray = unwrapped.robot.get_joint_position() + + # self.assert_collision(info) + # # assure that the robot did not move + # assert np.allclose(p1, p2) def test_relative_zero_action_joints(self, cfg, gripper_cfg): """ diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 2c55ed72..63ea9e45 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,18 @@ class PyRobot : public rcs::common::Robot { PYBIND11_OVERRIDE_PURE(void, rcs::common::Robot, reset, ); } + void close() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Robot, close, ); + } + std::optional> get_ik() override { + PYBIND11_OVERRIDE_PURE(std::optional>, + rcs::common::Robot, get_ik, ); + } + rcs::common::Pose get_base_pose_in_world_coordinates() override { + PYBIND11_OVERRIDE_PURE(rcs::common::Pose, rcs::common::Robot, + get_base_pose_in_world_coordinates, ); + } + void set_cartesian_position(const rcs::common::Pose &pose) override { PYBIND11_OVERRIDE_PURE(void, rcs::common::Robot, set_cartesian_position, pose); @@ -115,6 +128,59 @@ class PyGripper : public rcs::common::Gripper { void reset() override { PYBIND11_OVERRIDE_PURE(void, rcs::common::Gripper, reset, ); } + void close() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Gripper, close, ); + } +}; + +/** + * @brief Hand trampoline class for python bindings + */ +class PyHand : public rcs::common::Hand { + public: + using rcs::common::Hand::Hand; // Inherit constructors + + rcs::common::HandConfig *get_parameters() override { + PYBIND11_OVERRIDE_PURE(rcs::common::HandConfig *, rcs::common::Hand, + get_parameters, ); + } + + rcs::common::HandState *get_state() override { + PYBIND11_OVERRIDE_PURE(rcs::common::HandState *, rcs::common::Hand, + get_state, ); + } + + void set_normalized_joint_poses(const rcs::common::VectorXd &q) override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, set_normalized_joint_poses, + q); + } + + rcs::common::VectorXd get_normalized_joint_poses() override { + PYBIND11_OVERRIDE_PURE(rcs::common::VectorXd, rcs::common::Hand, + get_normalized_joint_poses, ); + } + + bool is_grasped() override { + PYBIND11_OVERRIDE_PURE(bool, rcs::common::Hand, is_grasped, ); + } + + void grasp() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, grasp, ); + } + + void open() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, open, ); + } + + void shut() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, shut, ); + } + void reset() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, reset, ); + } + void close() override { + PYBIND11_OVERRIDE_PURE(void, rcs::common::Hand, close, ); + } }; PYBIND11_MODULE(_core, m) { @@ -241,6 +307,7 @@ PYBIND11_MODULE(_core, m) { .value("FR3", rcs::common::RobotType::FR3) .value("UR5e", rcs::common::RobotType::UR5e) .value("SO101", rcs::common::RobotType::SO101) + .value("XArm7", rcs::common::RobotType::XArm7) .export_values(); py::enum_(common, "RobotPlatform") @@ -264,16 +331,30 @@ PYBIND11_MODULE(_core, m) { py::class_(common, "RobotConfig") .def(py::init<>()) .def_readwrite("robot_type", &rcs::common::RobotConfig::robot_type) + .def_readwrite("kinematic_model_path", + &rcs::common::RobotConfig::kinematic_model_path) + .def_readwrite("attachment_site", + &rcs::sim::SimRobotConfig::attachment_site) + .def_readwrite("tcp_offset", &rcs::sim::SimRobotConfig::tcp_offset) .def_readwrite("robot_platform", &rcs::common::RobotConfig::robot_platform); py::class_(common, "RobotState"); py::class_(common, "GripperConfig"); py::class_(common, "GripperState"); + py::enum_(common, "GraspType") + .value("POWER_GRASP", rcs::common::GraspType::POWER_GRASP) + .value("PRECISION_GRASP", rcs::common::GraspType::PRECISION_GRASP) + .value("LATERAL_GRASP", rcs::common::GraspType::LATERAL_GRASP) + .value("TRIPOD_GRASP", rcs::common::GraspType::TRIPOD_GRASP) + .export_values(); + py::class_(common, "HandConfig").def(py::init<>()); + py::class_(common, "HandState").def(py::init<>()); // holder type should be smart pointer as we deal with smart pointer // instances of this class py::class_>( common, "Robot") + .def(py::init<>()) .def("get_parameters", &rcs::common::Robot::get_parameters) .def("get_state", &rcs::common::Robot::get_state) .def("get_cartesian_position", @@ -284,6 +365,7 @@ PYBIND11_MODULE(_core, m) { .def("move_home", &rcs::common::Robot::move_home, py::call_guard()) .def("reset", &rcs::common::Robot::reset) + .def("close", &rcs::common::Robot::close) .def("set_cartesian_position", &rcs::common::Robot::set_cartesian_position, py::arg("pose"), py::call_guard()) @@ -299,6 +381,7 @@ PYBIND11_MODULE(_core, m) { py::class_>(common, "Gripper") + .def(py::init<>()) .def("get_parameters", &rcs::common::Gripper::get_parameters) .def("get_state", &rcs::common::Gripper::get_state) .def("set_normalized_width", &rcs::common::Gripper::set_normalized_width, @@ -311,28 +394,49 @@ PYBIND11_MODULE(_core, m) { py::call_guard()) .def("shut", &rcs::common::Gripper::shut, py::call_guard()) + .def("close", &rcs::common::Gripper::close, + py::call_guard()) .def("reset", &rcs::common::Gripper::reset, py::call_guard()); + py::class_>( + common, "Hand") + .def(py::init<>()) + .def("get_parameters", &rcs::common::Hand::get_parameters) + .def("get_state", &rcs::common::Hand::get_state) + .def("set_normalized_joint_poses", + &rcs::common::Hand::set_normalized_joint_poses, py::arg("q")) + .def("get_normalized_joint_poses", + &rcs::common::Hand::get_normalized_joint_poses) + .def("grasp", &rcs::common::Hand::grasp, + py::call_guard()) + .def("is_grasped", &rcs::common::Hand::is_grasped) + .def("open", &rcs::common::Hand::open, + py::call_guard()) + .def("shut", &rcs::common::Hand::shut, + py::call_guard()) + .def("close", &rcs::common::Hand::close, + py::call_guard()) + .def("reset", &rcs::common::Hand::reset, + py::call_guard()); + // SIM MODULE auto sim = m.def_submodule("sim", "sim module"); py::class_( sim, "SimRobotConfig") .def(py::init<>()) - .def_readwrite("tcp_offset", &rcs::sim::SimRobotConfig::tcp_offset) .def_readwrite("joint_rotational_tolerance", &rcs::sim::SimRobotConfig::joint_rotational_tolerance) .def_readwrite("seconds_between_callbacks", &rcs::sim::SimRobotConfig::seconds_between_callbacks) .def_readwrite("realtime", &rcs::sim::SimRobotConfig::realtime) + .def_readwrite("mjcf_scene_path", + &rcs::sim::SimRobotConfig::mjcf_scene_path) .def_readwrite("trajectory_trace", &rcs::sim::SimRobotConfig::trajectory_trace) - .def_readwrite("robot_type", &rcs::sim::SimRobotConfig::robot_type) .def_readwrite("arm_collision_geoms", &rcs::sim::SimRobotConfig::arm_collision_geoms) .def_readwrite("joints", &rcs::sim::SimRobotConfig::joints) - .def_readwrite("attachment_site", - &rcs::sim::SimRobotConfig::attachment_site) .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators) .def_readwrite("base", &rcs::sim::SimRobotConfig::base) .def("add_id", &rcs::sim::SimRobotConfig::add_id, py::arg("id")); @@ -417,6 +521,47 @@ PYBIND11_MODULE(_core, m) { .def("set_joints_hard", &rcs::sim::SimRobot::set_joints_hard, py::arg("q")) .def("get_state", &rcs::sim::SimRobot::get_state); + + // SimTilburgHandState + py::class_( + sim, "SimTilburgHandState") + .def(py::init<>()) + .def_readonly("last_commanded_qpos", + &rcs::sim::SimTilburgHandState::last_commanded_qpos) + .def_readonly("is_moving", &rcs::sim::SimTilburgHandState::is_moving) + .def_readonly("last_qpos", &rcs::sim::SimTilburgHandState::last_qpos) + .def_readonly("collision", &rcs::sim::SimTilburgHandState::collision); + // SimTilburgHandConfig + py::class_( + sim, "SimTilburgHandConfig") + .def(py::init<>()) + .def_readwrite("max_joint_position", + &rcs::sim::SimTilburgHandConfig::max_joint_position) + .def_readwrite("min_joint_position", + &rcs::sim::SimTilburgHandConfig::min_joint_position) + .def_readwrite("ignored_collision_geoms", + &rcs::sim::SimTilburgHandConfig::ignored_collision_geoms) + .def_readwrite("collision_geoms", + &rcs::sim::SimTilburgHandConfig::collision_geoms) + .def_readwrite("collision_geoms_fingers", + &rcs::sim::SimTilburgHandConfig::collision_geoms_fingers) + .def_readwrite("joints", &rcs::sim::SimTilburgHandConfig::joints) + .def_readwrite("actuators", &rcs::sim::SimTilburgHandConfig::actuators) + .def_readwrite("grasp_type", &rcs::sim::SimTilburgHandConfig::grasp_type) + .def_readwrite("seconds_between_callbacks", + &rcs::sim::SimTilburgHandConfig::seconds_between_callbacks) + .def("add_id", &rcs::sim::SimTilburgHandConfig::add_id, py::arg("id")); + // SimTilburgHand + py::class_>(sim, "SimTilburgHand") + .def(py::init, + const rcs::sim::SimTilburgHandConfig &>(), + py::arg("sim"), py::arg("cfg")) + .def("get_parameters", &rcs::sim::SimTilburgHand::get_parameters) + .def("get_state", &rcs::sim::SimTilburgHand::get_state) + .def("set_parameters", &rcs::sim::SimTilburgHand::set_parameters, + py::arg("cfg")); + py::enum_(sim, "CameraType") .value("free", rcs::sim::CameraType::free) .value("tracking", rcs::sim::CameraType::tracking) diff --git a/src/rcs/IK.cpp b/src/rcs/IK.cpp index 1643c4db..40a38590 100644 --- a/src/rcs/IK.cpp +++ b/src/rcs/IK.cpp @@ -75,24 +75,128 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf = true) } } +// std::optional Pin::ik(const Pose& pose, const VectorXd& q0, +// const Pose& tcp_offset) { +// rcs::common::Pose new_pose = pose * tcp_offset.inverse(); +// VectorXd q(model.nq); +// q.setZero(); +// q.head(q0.size()) = q0; +// const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); +// pinocchio::Data::Matrix6x J(6, model.nv); +// J.setZero(); +// bool success = false; +// Vector6d err; +// Eigen::VectorXd v(model.nv); +// for (int i = 0;; i++) { +// pinocchio::forwardKinematics(model, data, q); +// pinocchio::updateFramePlacements(model, data); +// const pinocchio::SE3 iMd = data.oMf[this->FRAME_ID].actInv(oMdes); +// err = pinocchio::log6(iMd).toVector(); +// // if (err.norm() < this->eps) { +// // check if all error components are below their respective thresholds +// Vector6d thresholds; +// thresholds << 0.03, 0.03, 0.03, 60.0 * (M_PI/180.0), 60.0 * +// (M_PI/180.0), 60.0 * (M_PI/180.0); if ((err.array().abs() <= +// thresholds.array()).all()) { +// success = true; +// break; +// } +// if (i >= this->IT_MAX) { +// std::cout << "err: " << err.transpose() << std::endl; +// success = false; +// break; +// } +// pinocchio::computeFrameJacobian(model, data, q, this->FRAME_ID, J); +// pinocchio::Data::Matrix6 Jlog; +// pinocchio::Jlog6(iMd.inverse(), Jlog); +// J = -Jlog * J; +// pinocchio::Data::Matrix6 JJt; +// JJt.noalias() = J * J.transpose(); +// JJt.diagonal().array() += this->damp; +// v.noalias() = -J.transpose() * JJt.ldlt().solve(err); +// q = pinocchio::integrate(model, q, v * this->DT); +// } +// if (success) { +// // return q; +// // retrun q, but only the first n elements, where n is the number of +// return q.head(q0.size()); +// } else { +// return std::nullopt; +// } +// } std::optional Pin::ik(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset) { + // --- Tunables (could be class members) ------------------------------------ + const double WP = 1.0; // position weight + const double WO = 0.12; // orientation (pitch+yaw) weight + const double WO_ROLL = 0.03; // orientation roll weight (let it "float" more) + const double ORI_TOL = 5.0 * M_PI / 180.0; // 5 deg tolerance (dead-zone) + const double ORI_CAP = + 0.6; // cap on |eo| scaling (rad/s-equivalent per iter) + const double STEP_CAP = + 0.15; // cap on ||J * v|| (meters/rad) per iter to avoid jumps + // If you already adapt damping elsewhere, keep using this->damp. Otherwise: + const double DAMP_BASE = this->damp; // your existing scalar + // -------------------------------------------------------------------------- + rcs::common::Pose new_pose = pose * tcp_offset.inverse(); VectorXd q(model.nq); q.setZero(); q.head(q0.size()) = q0; + const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); + pinocchio::Data::Matrix6x J(6, model.nv); J.setZero(); bool success = false; - Vector6d err; - Eigen::VectorXd v(model.nv); - for (int i = 0;; i++) { + + // Pre-allocations + Vector6d err, err_w; // 6x1 + Eigen::Vector3d ep, eo; // position/orientation parts + Eigen::VectorXd v(model.nv); // nv x 1 + pinocchio::Data::Matrix6 JJt; // 6x6 + Eigen::Matrix Wsqrt; // sqrt of weights + + // Build constant sqrt-weights (diag): [WP, WP, WP, WO, WO, WO_ROLL] + Wsqrt.setZero(); + Wsqrt(0, 0) = std::sqrt(WP); + Wsqrt(1, 1) = std::sqrt(WP); + Wsqrt(2, 2) = std::sqrt(WP); + Wsqrt(3, 3) = std::sqrt(WO); + Wsqrt(4, 4) = std::sqrt(WO); + Wsqrt(5, 5) = std::sqrt(WO_ROLL); + + for (int i = 0;; ++i) { + // FK + current frame pose pinocchio::forwardKinematics(model, data, q); pinocchio::updateFramePlacements(model, data); + + // Error in frame tangent space (twist): [trans; rot] const pinocchio::SE3 iMd = data.oMf[this->FRAME_ID].actInv(oMdes); err = pinocchio::log6(iMd).toVector(); - if (err.norm() < this->eps) { + + // Split and shape errors + ep = err.head<3>(); + eo = err.tail<3>(); + + // ---- Orientation tolerance (dead-zone + soft cap) ---------------------- + // If |eo| is small, ignore it completely (prevents twitch). + const double eo_norm = eo.norm(); + if (eo_norm < ORI_TOL) { + eo.setZero(); + } else { + // shrink just the "excess" beyond tolerance (preserve direction) + const double scaled = std::min(eo_norm - ORI_TOL, ORI_CAP); + eo *= (scaled / eo_norm); + } + // ------------------------------------------------------------------------ + + // Re-pack the shaped error + err.head<3>() = ep; + err.tail<3>() = eo; + + // Convergence test emphasizes position because eo may be zeroed + if (ep.norm() < this->eps_pos && eo.norm() < this->eps_ori) { success = true; break; } @@ -100,21 +204,65 @@ std::optional Pin::ik(const Pose& pose, const VectorXd& q0, success = false; break; } + + // 6xnv body Jacobian for the frame pinocchio::computeFrameJacobian(model, data, q, this->FRAME_ID, J); + + // Map to log space (same as your original) pinocchio::Data::Matrix6 Jlog; pinocchio::Jlog6(iMd.inverse(), Jlog); - J = -Jlog * J; - pinocchio::Data::Matrix6 JJt; - JJt.noalias() = J * J.transpose(); - JJt.diagonal().array() += this->damp; - v.noalias() = -J.transpose() * JJt.ldlt().solve(err); + J = -Jlog * J; // tangent-space Jacobian for the error you’re minimizing + + // ---- Weighted damped least-squares ------------------------------------- + // Apply sqrt-weights on rows: this prioritizes translation > orientation, + // and de-weights tool roll further. + const auto Jw = Wsqrt * J; // 6xnv + err_w = Wsqrt * err; // 6x1 + + // Optional: adapt damping using position manipulability (safer near + // singularities) + double damp = DAMP_BASE; + { + // Use position block for manipulability (3xnv) + Eigen::Matrix Jp = J.topRows<3>(); + Eigen::JacobiSVD svd( + Jp, Eigen::ComputeThinU | Eigen::ComputeThinV); + const double sigma_min = + std::max(1e-9, svd.singularValues().tail<1>()(0)); + // Increase damping when near singular (simple schedule) + damp = DAMP_BASE + 0.01 / sigma_min; + } + + // Normal equations on weighted system + JJt.noalias() = Jw * Jw.transpose(); + JJt.diagonal().array() += damp; + + // Solve for v (joint velocity increment) + v.noalias() = -Jw.transpose() * JJt.ldlt().solve(err_w); + // ------------------------------------------------------------------------ + + // ---- Step-size limiter in task space to avoid sudden jumps ------------- + // Predict task step and scale if too large + Eigen::Matrix task_step = J * v * this->DT; + double step_norm = task_step.head<3>().norm() + task_step.tail<3>().norm(); + if (step_norm > STEP_CAP && step_norm > 1e-9) { + const double scale = STEP_CAP / step_norm; + v *= scale; + } + // Also respect per-joint velocity limits if you have them (not shown) + // ------------------------------------------------------------------------ + + // Integrate q = pinocchio::integrate(model, q, v * this->DT); } + if (success) { - return q; - } else { - return std::nullopt; + return q.head(q0.size()); } + // std::cout << "IK failed after " << this->IT_MAX << " iterations." << + // std::endl; + std::cout << "ep: " << ep.norm() << " eo: " << eo.norm() << std::endl; + return std::nullopt; } Pose Pin::forward(const VectorXd& q0, const Pose& tcp_offset) { diff --git a/src/sim/CMakeLists.txt b/src/sim/CMakeLists.txt index 0768d92f..5c57a61c 100644 --- a/src/sim/CMakeLists.txt +++ b/src/sim/CMakeLists.txt @@ -1,6 +1,6 @@ add_library(sim) target_sources(sim - PRIVATE sim.cpp SimRobot.cpp renderer.cpp camera.cpp SimGripper.cpp gui_server.cpp gui_client.cpp + PRIVATE sim.cpp SimRobot.cpp renderer.cpp camera.cpp SimGripper.cpp SimTilburgHand.cpp gui_server.cpp gui_client.cpp ) target_link_libraries(sim PUBLIC rcs MuJoCo::MuJoCo) diff --git a/src/sim/SimGripper.cpp b/src/sim/SimGripper.cpp index ae331cd9..f431e488 100644 --- a/src/sim/SimGripper.cpp +++ b/src/sim/SimGripper.cpp @@ -94,8 +94,8 @@ double SimGripper::get_normalized_width() { // TODO: maybe we should use the mujoco sensors? Not sure what the difference // is between reading out from qpos and reading from the sensors. double width = - (this->sim->d->qpos[this->joint_id] - this->cfg.min_actuator_width) / - (this->cfg.max_joint_width - this->cfg.min_actuator_width); + (this->sim->d->qpos[this->joint_id] - this->cfg.min_joint_width) / + (this->cfg.max_joint_width - this->cfg.min_joint_width); // sometimes the joint is slightly outside of the bounds if (width < 0) { width = 0; @@ -159,7 +159,7 @@ void SimGripper::m_reset() { this->state = SimGripperState(); // reset state hard this->sim->d->qpos[this->joint_id] = this->cfg.max_joint_width; - this->sim->d->ctrl[this->actuator_id] = this->cfg.max_joint_width; + this->sim->d->ctrl[this->actuator_id] = this->cfg.max_actuator_width; } void SimGripper::reset() { this->m_reset(); } diff --git a/src/sim/SimGripper.h b/src/sim/SimGripper.h index f48acbb8..6d4cae79 100644 --- a/src/sim/SimGripper.h +++ b/src/sim/SimGripper.h @@ -88,6 +88,7 @@ class SimGripper : public common::Gripper { void grasp() override; void open() override; void shut() override; + void close() override {}; }; } // namespace sim } // namespace rcs diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index f1b8dccd..cd13fe59 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -75,7 +75,8 @@ void SimRobot::init_ids() { // Joints for (size_t i = 0; i < std::size(this->cfg.joints); ++i) { name = this->cfg.joints[i]; - this->ids.joints[i] = mj_name2id(this->sim->m, mjOBJ_JOINT, name.c_str()); + this->ids.joints.push_back( + mj_name2id(this->sim->m, mjOBJ_JOINT, name.c_str())); if (this->ids.joints[i] == -1) { throw std::runtime_error(std::string("No joint named " + name)); } @@ -83,8 +84,8 @@ void SimRobot::init_ids() { // Actuators for (size_t i = 0; i < std::size(this->cfg.actuators); ++i) { name = this->cfg.actuators[i]; - this->ids.actuators[i] = - mj_name2id(this->sim->m, mjOBJ_ACTUATOR, name.c_str()); + this->ids.actuators.push_back( + mj_name2id(this->sim->m, mjOBJ_ACTUATOR, name.c_str())); if (this->ids.actuators[i] == -1) { throw std::runtime_error(std::string("No actuator named " + name)); } diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index 04cef8aa..86724611 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -12,13 +12,11 @@ namespace rcs { namespace sim { struct SimRobotConfig : common::RobotConfig { - rcs::common::Pose tcp_offset = rcs::common::Pose::Identity(); double joint_rotational_tolerance = .05 * (std::numbers::pi / 180.0); // 0.05 degree double seconds_between_callbacks = 0.1; // 10 Hz bool realtime = false; bool trajectory_trace = false; - common::RobotType robot_type = common::RobotType::FR3; std::vector arm_collision_geoms{ "fr3_link0_collision", "fr3_link1_collision", "fr3_link2_collision", "fr3_link3_collision", "fr3_link4_collision", "fr3_link5_collision", @@ -31,8 +29,8 @@ struct SimRobotConfig : common::RobotConfig { "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4", "fr3_joint5", "fr3_joint6", "fr3_joint7", }; - std::string attachment_site = "attachment_site"; std::string base = "base"; + std::string mjcf_scene_path = "assets/scenes/fr3_empty_world/scene.xml"; void add_id(const std::string &id) { for (auto &s : this->arm_collision_geoms) { @@ -76,6 +74,7 @@ class SimRobot : public common::Robot { std::optional> get_ik() override; void reset() override; void set_joints_hard(const common::VectorXd &q); + void close() override {}; private: SimRobotConfig cfg; @@ -85,9 +84,8 @@ class SimRobot : public common::Robot { struct { std::set cgeom; int attachment_site; - std::array joints; - std::array ctrl; - std::array actuators; + std::vector joints; + std::vector actuators; int base; } ids; void is_moving_callback(); diff --git a/src/sim/SimTilburgHand.cpp b/src/sim/SimTilburgHand.cpp new file mode 100644 index 00000000..45cc110c --- /dev/null +++ b/src/sim/SimTilburgHand.cpp @@ -0,0 +1,208 @@ + +#include "SimTilburgHand.h" + +#include +#include +#include +#include +#include + +namespace rcs { +namespace sim { + +SimTilburgHand::SimTilburgHand(std::shared_ptr sim, + const SimTilburgHandConfig &cfg) + : sim{sim}, cfg{cfg} { + this->state = SimTilburgHandState(); + + // Initialize actuator and joint IDs + for (int i = 0; i < this->n_joints; ++i) { + // actuators + this->actuator_ids[i] = mj_name2id(this->sim->m, mjOBJ_ACTUATOR, + this->cfg.actuators[i].c_str()); + if (this->actuator_ids[i] == -1) { + throw std::runtime_error( + std::string("No actuator named " + this->cfg.actuators[i])); + } + // joints + this->joint_ids[i] = this->sim->m->jnt_qposadr[mj_name2id( + this->sim->m, mjOBJ_JOINT, this->cfg.joints[i].c_str())]; + if (this->joint_ids[i] == -1) { + throw std::runtime_error( + std::string("No joint named " + this->cfg.joints[i])); + } + } + + // Collision geoms + // this->add_collision_geoms(this->cfg.collision_geoms, this->cgeom, false); + // this->add_collision_geoms(this->cfg.collision_geoms_fingers, this->cfgeom, + // false); + + this->sim->register_all_cb( + std::bind(&SimTilburgHand::convergence_callback, this), + this->cfg.seconds_between_callbacks); + this->sim->register_any_cb( + std::bind(&SimTilburgHand::collision_callback, this), + this->cfg.seconds_between_callbacks); + this->m_reset(); +} + +SimTilburgHand::~SimTilburgHand() {} + +// void SimTilburgHand::add_collision_geoms( +// const std::vector &cgeoms_str, std::set &cgeoms_set, +// bool clear_before) { +// if (clear_before) { +// cgeoms_set.clear(); +// } +// for (size_t i = 0; i < std::size(cgeoms_str); ++i) { +// std::string name = cgeoms_str[i]; + +// int coll_id = mj_name2id(this->sim->m, mjOBJ_GEOM, name.c_str()); +// if (coll_id == -1) { +// throw std::runtime_error(std::string("No geom named " + name)); +// } +// cgeoms_set.insert(coll_id); +// } +// } + +bool SimTilburgHand::set_parameters(const SimTilburgHandConfig &cfg) { + auto current_grasp_type = this->cfg.grasp_type; + this->cfg = cfg; + if (!this->set_grasp_type(current_grasp_type)) { + std::cerr << "Provided grasp type is not supported." << std::endl; + this->cfg.grasp_type = current_grasp_type; + } + // this->add_collision_geoms(cfg.ignored_collision_geoms, + // this->ignored_collision_geoms, true); + return true; +} + +bool SimTilburgHand::set_grasp_type(common::GraspType grasp_type) { + switch (grasp_type) { + case common::GraspType::POWER_GRASP: + this->cfg.grasp_type = common::GraspType::POWER_GRASP; + break; + default: + return false; + } + return true; +} + +SimTilburgHandConfig *SimTilburgHand::get_parameters() { + // copy config to heap + SimTilburgHandConfig *cfg = new SimTilburgHandConfig(); + *cfg = this->cfg; + return cfg; +} + +SimTilburgHandState *SimTilburgHand::get_state() { + SimTilburgHandState *state = new SimTilburgHandState(); + *state = this->state; + return state; +} +void SimTilburgHand::set_normalized_joint_poses( + const rcs::common::VectorXd &q) { + if (q.size() != this->n_joints) { + throw std::invalid_argument("Invalid joint pose vector size, expected 16."); + } + this->state.last_commanded_qpos = q; + for (int i = 0; i < this->n_joints; ++i) { + this->sim->d->ctrl[this->actuator_ids[i]] = + (mjtNum)q[i] * (this->cfg.max_joint_position[i] - + this->cfg.min_joint_position[i]) + + this->cfg.min_joint_position[i]; + } + + // we ignore force for now + // this->sim->d->actuator_force[this->gripper_id] = 0; +} +rcs::common::VectorXd SimTilburgHand::get_normalized_joint_poses() { + // TODO: maybe we should use the mujoco sensors? Not sure what the difference + // is between reading out from qpos and reading from the sensors. + rcs::common::VectorXd q(this->n_joints); + for (int i = 0; i < this->n_joints; ++i) { + q[i] = (this->sim->d->qpos[this->joint_ids[i]] - + this->cfg.min_joint_position[i]) / + (this->cfg.max_joint_position[i] - this->cfg.min_joint_position[i]); + } + return q; +} + +bool SimTilburgHand::collision_callback() { + this->state.collision = false; + // for (size_t i = 0; i < this->sim->d->ncon; ++i) { + // if (this->cfgeom.contains(this->sim->d->contact[i].geom[0]) && + // this->cfgeom.contains(this->sim->d->contact[i].geom[1])) { + // // ignore collisions between fingers + // continue; + // } + + // if ((this->cgeom.contains(this->sim->d->contact[i].geom[0]) or + // this->cgeom.contains(this->sim->d->contact[i].geom[1])) and + // // ignore all collision with ignored objects with frankahand + // // not just fingers + // not(this->ignored_collision_geoms.contains( + // this->sim->d->contact[i].geom[1]) or + // this->ignored_collision_geoms.contains( + // this->sim->d->contact[i].geom[1]))) { + // this->state.collision = true; + // break; + // } + // } + return this->state.collision; +} + +bool SimTilburgHand::is_grasped() { + // double width = this->get_normalized_width(); + + // if (this->state.last_commanded_width - this->cfg.epsilon_inner < width && + // width < this->state.last_commanded_width + this->cfg.epsilon_outer) { + // // this is the libfranka logic to decide whether an object is grasped + // return true; + // } + return false; +} + +bool SimTilburgHand::convergence_callback() { + auto qpos = get_normalized_joint_poses(); + this->state.is_moving = + (this->state.last_qpos - qpos).norm() > + 0.001 * (this->cfg.max_joint_position - this->cfg.min_joint_position) + .norm(); // 0.1% tolerance + this->state.last_qpos = qpos; + return not this->state.is_moving; +} + +void SimTilburgHand::grasp() { + switch (this->cfg.grasp_type) { + case common::GraspType::POWER_GRASP: + this->set_normalized_joint_poses(this->power_grasp_pose); + break; + default: + std::cerr << "Grasp type not implemented, using power grasp as default." + << std::endl; + this->set_normalized_joint_poses(this->power_grasp_pose); + break; + } +} + +void SimTilburgHand::open() { + this->set_normalized_joint_poses(this->open_pose); +} +void SimTilburgHand::shut() { + this->set_normalized_joint_poses(rcs::common::VectorXd::Ones(16)); +} + +void SimTilburgHand::m_reset() { + this->state = SimTilburgHandState(); + // reset state hard + for (int i = 0; i < this->n_joints; ++i) { + this->sim->d->qpos[this->joint_ids[i]] = this->cfg.min_joint_position[i]; + this->sim->d->ctrl[this->actuator_ids[i]] = this->cfg.min_joint_position[i]; + } +} + +void SimTilburgHand::reset() { this->m_reset(); } +} // namespace sim +} // namespace rcs diff --git a/src/sim/SimTilburgHand.h b/src/sim/SimTilburgHand.h new file mode 100644 index 00000000..9ab36064 --- /dev/null +++ b/src/sim/SimTilburgHand.h @@ -0,0 +1,127 @@ +#ifndef RCS_TILBURG_HAND_SIM_H +#define RCS_TILBURG_HAND_SIM_H + +#include +#include +#include +#include + +#include "rcs/Robot.h" +#include "sim/sim.h" + +namespace rcs { +namespace sim { + +struct SimTilburgHandConfig : common::HandConfig { + rcs::common::Vector16d max_joint_position = + (rcs::common::VectorXd(16) << 1.6581, 1.5708, 0.0000, 1.5708, 1.6581, + 1.6581, 1.6581, 0.4363, 1.6581, 1.6581, 1.6581, 0.4363, 1.6581, 1.6581, + 1.6581, 0.4363) + .finished(); + rcs::common::Vector16d min_joint_position = + (rcs::common::VectorXd(16) << 0.0000, 0.0000, -1.7453, 0.0000, -0.0873, + -0.0873, -0.0873, -0.4363, -0.0873, -0.0873, -0.0873, -0.4363, -0.0873, + -0.0873, -0.0873, -0.4363) + .finished(); + std::vector ignored_collision_geoms = {}; + std::vector collision_geoms = + {}; //{"hand_c", "d435i_collision", + // "finger_0_left", "finger_0_right"}; + + std::vector collision_geoms_fingers = {}; //{"finger_0_left", + //"finger_0_right"}; + // following the real robot motor order convention + std::vector joints = { + "thumb_ip", "thumb_mcp", "thumb_mcp_rot", "thumb_cmc", + "index_dip", "index_pip", "index_mcp", "index_mcp_abadd", + "middle_dip", "middle_pip", "middle_mcp", "middle_mcp_abadd", + "ring_dip", "ring_pip", "ring_mcp", "ring_mcp_abadd"}; + std::vector actuators = { + // following the real robot motor order convention + "thumb_ip", "thumb_mcp", "thumb_mcp_rot", "thumb_cmc", + "index_dip", "index_pip", "index_mcp", "index_mcp_abadd", + "middle_dip", "middle_pip", "middle_mcp", "middle_mcp_abadd", + "ring_dip", "ring_pip", "ring_mcp", "ring_mcp_abadd"}; + + common::GraspType grasp_type = common::GraspType::POWER_GRASP; + + double seconds_between_callbacks = 0.0167; // 60 Hz + void add_id(const std::string &id) { + for (auto &s : this->collision_geoms) { + s = s + "_" + id; + } + for (auto &s : this->collision_geoms_fingers) { + s = s + "_" + id; + } + for (auto &s : this->ignored_collision_geoms) { + s = s + "_" + id; + } + for (auto &s : this->joints) { + s = s + "_" + id; + } + for (auto &s : this->actuators) { + s = s + "_" + id; + } + } +}; + +struct SimTilburgHandState : common::HandState { + rcs::common::VectorXd last_commanded_qpos = rcs::common::VectorXd::Zero(16); + bool is_moving = false; + rcs::common::VectorXd last_qpos = rcs::common::VectorXd::Zero(16); + bool collision = false; +}; + +class SimTilburgHand : public common::Hand { + private: + SimTilburgHandConfig cfg; + std::shared_ptr sim; + const int n_joints = 16; + std::vector actuator_ids = std::vector(n_joints); + std::vector joint_ids = std::vector(n_joints); + SimTilburgHandState state; + bool convergence_callback(); + bool collision_callback(); + std::set cgeom; + std::set cfgeom; + std::set ignored_collision_geoms; + void add_collision_geoms(const std::vector &cgeoms_str, + std::set &cgeoms_set, bool clear_before); + void m_reset(); + + const rcs::common::VectorXd open_pose = + (rcs::common::VectorXd(16) << 0.0, 0.0, 0.5, 1.4, 0.2, 0.2, 0.2, 0.7, 0.2, + 0.2, 0.2, 0.3, 0.2, 0.2, 0.2, 0.0) + .finished(); + const rcs::common::VectorXd power_grasp_pose = + (rcs::common::VectorXd(16) << 0.5, 0.5, 0.5, 1.4, 0.5, 0.5, 1.0, 0.7, 0.5, + 0.5, 1.0, 0.3, 0.5, 0.5, 1.0, 0.0) + .finished(); + bool set_grasp_type(common::GraspType grasp_type); + + public: + SimTilburgHand(std::shared_ptr sim, const SimTilburgHandConfig &cfg); + ~SimTilburgHand() override; + + bool set_parameters(const SimTilburgHandConfig &cfg); + + SimTilburgHandConfig *get_parameters() override; + + SimTilburgHandState *get_state() override; + + // normalized joints of the hand, 0 is closed, 1 is open + void set_normalized_joint_poses(const rcs::common::VectorXd &q) override; + rcs::common::VectorXd get_normalized_joint_poses() override; + + void reset() override; + + bool is_grasped() override; + + void grasp() override; + void open() override; + void shut() override; + void close() override {}; +}; +} // namespace sim +} // namespace rcs +#endif // RCS_TILBURG_HAND_SIM_H \ No newline at end of file diff --git a/src/sim/sim.h b/src/sim/sim.h index ac48d603..677be8e1 100644 --- a/src/sim/sim.h +++ b/src/sim/sim.h @@ -29,7 +29,7 @@ class Renderer { struct Config { bool async = false; bool realtime = false; - int max_convergence_steps = 5000; + int max_convergence_steps = 300; }; struct Callback {