feat: XArm7 support

README.md

@@ -65,8 +65,8 @@ from rcs import sim
 from rcs._core.sim import CameraType
 from rcs.camera.sim import SimCameraConfig, SimCameraSet
 from time import sleep
-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")

assets/scenes/fr3_empty_world/scene.xml

@@ -1,6 +1,6 @@
 <mujoco model="fr3 scene">
   <include file="fr3_common.xml"/>
-  <include file="fr3_0.xml"/>
+  <include file="robot.xml"/>
 
   <statistic center="0.3 0 0.4" extent="1"/>
 

assets/scenes/fr3_simple_pick_up/scene.xml

@@ -1,6 +1,6 @@
 <mujoco model="simple pick up task">
   <include file="fr3_common.xml" />
-  <include file="fr3_0.xml" />
+  <include file="robot.xml" />
 
   <statistic center="0.3 0 0.4" extent="1" />
 

assets/scenes/xarm7_empty_world/assets

@@ -0,0 +1 @@
+../../xarm7/stl

assets/scenes/xarm7_empty_world/robot.xml

@@ -0,0 +1 @@
+../../xarm7/mjcf/xarm7.xml

assets/scenes/xarm7_empty_world/scene.xml

@@ -0,0 +1,24 @@
+<mujoco model="xarm7 scene">
+  <include file="robot.xml"/>
+
+  <statistic center="0.2 0 0.4" extent=".65"/>
+
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="150" elevation="-20"/>
+  </visual>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
+      markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
+    <geom type="cylinder" size=".06 .06" pos="0 0 .06" rgba="1 1 1 1"/>
+  </worldbody>
+</mujoco>

assets/xarm7/mjcf/xarm7.xml

@@ -0,0 +1,174 @@
+<mujoco model="xarm7 nohand">
+  <compiler angle="radian" autolimits="true" meshdir="assets" />
+
+  <option integrator="implicitfast" />
+
+  <asset>
+    <material name="white" rgba="1 1 1 1" />
+    <material name="gray" rgba="0.753 0.753 0.753 1" />
+    <material name="brushed_aluminum_gray"
+          specular="0.5"
+          shininess="0.9"
+          reflectance="0.3"
+          metallic="1.0"
+          rgba="0.75 0.75 0.75 1"/>
+
+    <mesh file="link_base_root.stl" />
+    <mesh file="link_base_trunk.stl" />
+    <mesh file="link1_trunk.stl" />
+    <mesh file="link2_trunk.stl" />
+    <mesh file="link3_trunk.stl" />
+    <mesh file="link4_trunk.stl" />
+    <mesh file="link5_trunk.stl" />
+    <mesh file="link6_trunk.stl" />
+    <mesh file="link7.stl" />
+
+    <mesh file="link_base_ring.stl" />
+    <mesh file="link1_ring.stl" />
+    <mesh file="link2_ring.stl" />
+    <mesh file="link3_ring.stl" />
+    <mesh file="link4_ring.stl" />
+    <mesh file="link5_ring.stl" />
+    <mesh file="link6_ring.stl" />
+
+    <mesh file="end_tool.stl" />
+    <mesh file="link1_convex.stl" />
+    <mesh file="link2_convex.stl" />
+    <mesh file="link3_convex.stl" />
+    <mesh file="link4_convex.stl" />
+    <mesh file="link5_convex.stl" />
+    <mesh file="link6_convex.stl" />
+    <mesh file="link7_convex.stl" />
+    <mesh file="end_tool_convex.stl" />
+  </asset>
+
+  <default>
+    <default class="xarm7">
+      <geom type="mesh" material="white" />
+      <joint axis="0 0 1" armature="0.1" range="-6.28319 6.28319" frictionloss="1" />
+      <general biastype="affine" ctrlrange="-6.28319 6.28319" />
+      <default class="size1">
+        <joint damping="10" actuatorgravcomp="true" />
+        <general gainprm="1500" biasprm="0 -1500 -150" forcerange="-50 50" />
+      </default>
+      <default class="size2">
+        <joint damping="5" actuatorgravcomp="true" />
+        <general gainprm="1000" biasprm="0 -1000 -100" forcerange="-30 30" />
+      </default>
+      <default class="size3">
+        <joint damping="2" actuatorgravcomp="true" />
+        <general gainprm="800" biasprm="0 -800 -80" forcerange="-20 20" />
+      </default>
+      <default class="visual">
+        <geom type="mesh" group="2" contype="0" conaffinity="0" />
+      </default>
+      <default class="collision">
+        <geom type="mesh" group="3" mass="0" density="0" />
+        <default class="fingertip_pad_collision_1">
+          <geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445" />
+        </default>
+        <default class="fingertip_pad_collision_2">
+          <geom type="box" size="0.003 0.002 0.003" pos="0.0055 0.002 0.05" />
+        </default>
+        <default class="fingertip_pad_collision_3">
+          <geom type="box" size="0.003 0.002 0.003" pos="-0.0055 0.002 0.05" />
+        </default>
+        <default class="fingertip_pad_collision_4">
+          <geom type="box" size="0.003 0.002 0.0035" pos="0.0055 0.002 0.0395" />
+        </default>
+        <default class="fingertip_pad_collision_5">
+          <geom type="box" size="0.003 0.002 0.0035" pos="-0.0055 0.002 0.0395" />
+        </default>
+      </default>
+      <site size="0.001" rgba="1 0 0 1" group="4" />
+    </default>
+  </default>
+
+  <worldbody>
+    <body name="base" pos="0 0 .12" childclass="xarm7" gravcomp="1">
+      <inertial pos="-0.021131 -0.0016302 0.056488" quat="0.696843 0.20176 0.10388 0.680376"
+        mass="0.88556"
+        diaginertia="0.00382023 0.00335282 0.00167725" />
+      <geom mesh="link_base_root" material="brushed_aluminum_gray" class="visual" />
+      <geom mesh="link_base_trunk" class="visual" />
+      <geom mesh="link_base_ring" material="brushed_aluminum_gray" class="visual" />
+      <body name="link1" pos="0 0 0.267" gravcomp="1">
+        <inertial pos="-0.0002 0.02905 -0.01233" quat="0.978953 -0.202769 -0.00441617 -0.0227264"
+          mass="2.382"
+          diaginertia="0.00569127 0.00533384 0.00293865" />
+        <joint name="joint1" class="size1" actuatorgravcomp="true" />
+        <geom mesh="link1_ring" material="brushed_aluminum_gray" class="visual" />
+        <geom mesh="link1_trunk" class="visual" />
+        <geom mesh="link1_convex" class="collision"/>
+        <body name="link2" quat="1 -1 0 0" gravcomp="1">
+          <inertial pos="0.00022 -0.12856 0.01735" quat="0.50198 0.86483 -0.00778841 0.00483285"
+            mass="1.869"
+            diaginertia="0.00959898 0.00937717 0.00201315" />
+          <joint name="joint2" range="-2.059 2.0944" class="size1" actuatorgravcomp="true" />
+          <geom mesh="link2_ring" material="brushed_aluminum_gray" class="visual"/>
+          <geom mesh="link2_trunk" class="visual"/>
+          <geom mesh="link2_convex" class="collision"/>
+          <body name="link3" pos="0 -0.293 0" quat="1 1 0 0" gravcomp="1">
+            <inertial pos="0.0466 -0.02463 -0.00768" quat="0.913819 0.289775 0.281481 -0.0416455"
+              mass="1.6383"
+              diaginertia="0.00351721 0.00294089 0.00195868" />
+            <joint name="joint3" class="size2" actuatorgravcomp="true" />
+            <geom mesh="link3_ring" material="brushed_aluminum_gray" class="visual"/>
+            <geom mesh="link3_trunk" class="visual"/>
+            <geom mesh="link3_convex" class="collision"/>
+            <body name="link4" pos="0.0525 0 0" quat="1 1 0 0" gravcomp="1">
+              <inertial pos="0.07047 -0.11575 0.012" quat="0.422108 0.852026 -0.126025 0.282832"
+                mass="1.7269"
+                diaginertia="0.00657137 0.00647948 0.00186763" />
+              <joint name="joint4" range="-0.19198 3.927" class="size2" actuatorgravcomp="true" />
+            <geom mesh="link4_ring" material="brushed_aluminum_gray" class="visual"/>
+            <geom mesh="link4_trunk" class="visual"/>
+              <geom mesh="link4_convex" class="collision"/>
+              <body name="link5" pos="0.0775 -0.3425 0" quat="1 1 0 0" gravcomp="1">
+                <inertial pos="-0.00032 0.01604 -0.026"
+                  quat="0.999311 -0.0304457 0.000577067 0.0212082" mass="1.3203"
+                  diaginertia="0.00534729 0.00499076 0.0013489" />
+                <joint name="joint5" class="size2" actuatorgravcomp="true" />
+                <geom mesh="link5_ring" material="brushed_aluminum_gray" class="visual"/>
+                <geom mesh="link5_trunk" class="visual"/>
+                <geom mesh="link5_convex" class="collision"/>
+                <body name="link6" quat="1 1 0 0" gravcomp="1">
+                  <inertial pos="0.06469 0.03278 0.02141" quat="-0.217672 0.772419 0.16258 0.574069"
+                    mass="1.325"
+                    diaginertia="0.00245421 0.00221646 0.00107273" />
+                  <joint name="joint6" range="-1.69297 3.14159" class="size3" actuatorgravcomp="true" />
+                  <geom mesh="link6_ring" material="brushed_aluminum_gray" class="visual"/>
+                  <geom mesh="link6_trunk" class="visual"/>
+                  <geom mesh="link6_convex" class="collision"/>
+                  <body name="link7" pos="0.076 0.097 0" quat="1 -1 0 0" gravcomp="1">
+                    <inertial pos="0 -0.00677 -0.01098" quat="0.487612 0.512088 -0.512088 0.487612"
+                      mass="0.17"
+                      diaginertia="0.000132176 9.3e-05 5.85236e-05" />
+                    <joint name="joint7" class="size3" actuatorgravcomp="true" />
+                    <geom material="brushed_aluminum_gray" mesh="link7" class="visual"/>
+                    <geom mesh="end_tool_convex" class="collision"/>
+                    <site name="attachment_site" />
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <general name="act1" joint="joint1" class="size1" />
+    <general name="act2" joint="joint2" class="size1" ctrlrange="-2.059 2.0944" />
+    <general name="act3" joint="joint3" class="size2" />
+    <general name="act4" joint="joint4" class="size2" ctrlrange="-0.19198 3.927" />
+    <general name="act5" joint="joint5" class="size2" />
+    <general name="act6" joint="joint6" class="size3" ctrlrange="-1.69297 3.14159" />
+    <general name="act7" joint="joint7" class="size3" />
+  </actuator>
+
+  <keyframe>
+    <key name="home" qpos="0 -.247 0 .909 0 1.15644 0" ctrl="0 -.247 0 .909 0 1.15644 0" />
+  </keyframe>
+</mujoco>

cmake/compile_scenes.cmake

@@ -30,11 +30,26 @@ 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)
+    # Copy all .xml files (dereferencing symlinks) and install them
+    file(GLOB scene_xml_files ${scene_dir}/*.xml)
+    foreach(xml_file ${scene_xml_files})
+        get_filename_component(xml_name ${xml_file} NAME)
+        configure_file(${xml_file} ${CMAKE_BINARY_DIR}/assets/scenes/${scene_name}/${xml_name} COPYONLY)
+        install(FILES ${CMAKE_BINARY_DIR}/assets/scenes/${scene_name}/${xml_name} DESTINATION rcs/scenes/${scene_name} COMPONENT python_package)
     endforeach()
+
+    # Always copy robot.xml to build dir, dereferencing symlinks
+    if(EXISTS ${scene_dir}/robot.xml)
+        configure_file(${scene_dir}/robot.xml ${CMAKE_BINARY_DIR}/assets/scenes/${scene_name}/robot.xml COPYONLY)
+        install(FILES ${CMAKE_BINARY_DIR}/assets/scenes/${scene_name}/robot.xml DESTINATION rcs/scenes/${scene_name} COMPONENT python_package)
+    endif()
+    if(EXISTS ${scene_dir}/robot.urdf)
+        install(FILES ${scene_dir}/robot.urdf DESTINATION rcs/scenes/${scene_name} COMPONENT python_package)
+    endif()
+    if(EXISTS ${scene_dir}/scene.xml)
+        install(FILES ${scene_dir}/scene.xml DESTINATION rcs/scenes/${scene_name} COMPONENT python_package)
+    endif()
+
 endforeach()
 
 # Create a custom target that depends on all generated .mjb files

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()