Merge branch 'feat/xarm-sim' into feat/XArm7

juelg · juelg · commit 9e7dde80aebc · 2025-08-07T10:14:47.000+02:00
diff --git a/.gitattributes b/.gitattributes
@@ -1,2 +1 @@
-.stl filter=lfs diff=lfs merge=lfs -text
-.obj filter=lfs diff=lfs merge=lfs -text
+*.stl filter=lfs diff=lfs merge=lfs -text
diff --git a/assets/scenes/fr3_empty_world/robot.urdf b/assets/scenes/fr3_empty_world/robot.urdf
diff --git a/assets/scenes/fr3_empty_world/robot.xml b/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
@@ -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"/>
 
diff --git a/assets/scenes/fr3_simple_pick_up/robot.urdf b/assets/scenes/fr3_simple_pick_up/robot.urdf
diff --git a/assets/scenes/fr3_simple_pick_up/robot.xml b/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
@@ -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" />
 
diff --git a/assets/scenes/xarm7_empty_world/assets b/assets/scenes/xarm7_empty_world/assets
@@ -0,0 +1 @@
+../../xarm7/stl
diff --git a/assets/scenes/xarm7_empty_world/robot.xml b/assets/scenes/xarm7_empty_world/robot.xml
@@ -0,0 +1 @@
+../../xarm7/mjcf/xarm7.xml
diff --git a/assets/scenes/xarm7_empty_world/scene.xml b/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>
diff --git a/assets/xarm7/mjcf/xarm7.xml b/assets/xarm7/mjcf/xarm7.xml
@@ -0,0 +1,106 @@
+<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"/>
+
+    <mesh file="link_base.stl"/>
+    <mesh file="link1.stl"/>
+    <mesh file="link2.stl"/>
+    <mesh file="link3.stl"/>
+    <mesh file="link4.stl"/>
+    <mesh file="link5.stl"/>
+    <mesh file="link6.stl"/>
+    <mesh file="link7.stl"/>
+    <mesh file="end_tool.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>
+      <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"/>
+      <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"/>
+        <geom mesh="link1"/>
+        <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"/>
+          <geom mesh="link2"/>
+          <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"/>
+            <geom mesh="link3"/>
+            <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"/>
+              <geom mesh="link4"/>
+              <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"/>
+                <geom mesh="link5"/>
+                <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"/>
+                  <geom mesh="link6"/>
+                  <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"/>
+                    <geom material="gray" mesh="end_tool"/>
+                    <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>
diff --git a/assets/xarm7/stl/base_link.stl b/assets/xarm7/stl/base_link.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cdaa4cff22f7c9cff05c6a8ed32f94fd2b11a69d37dd97a159ccee2d8dd32f13
+size 1211434
diff --git a/assets/xarm7/stl/end_tool.stl b/assets/xarm7/stl/end_tool.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e06112781cdd615b7df536569f50addb445a6f4b4acced6d53b0742018b6c768
+size 13084
diff --git a/assets/xarm7/stl/left_finger.stl b/assets/xarm7/stl/left_finger.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a44756eb72f9c214cb37e61dc209cd7073fdff3e4271a7423476ef6fd090d2d4
+size 242684
diff --git a/assets/xarm7/stl/left_inner_knuckle.stl b/assets/xarm7/stl/left_inner_knuckle.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e8e48692ad26837bb3d6a97582c89784d09948fc09bfe4e5a59017859ff04dac
+size 366284
diff --git a/assets/xarm7/stl/left_outer_knuckle.stl b/assets/xarm7/stl/left_outer_knuckle.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:501665812b08d67e764390db781e839adc6896a9540301d60adf606f57648921
+size 22284
diff --git a/assets/xarm7/stl/link1.stl b/assets/xarm7/stl/link1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:34b541122df84d2ef5fcb91b715eb19659dc15ad8d44a191dde481f780265636
+size 184184
diff --git a/assets/xarm7/stl/link2.stl b/assets/xarm7/stl/link2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:61e641cd47c169ecef779683332e00e4914db729bf02dfb61bfbe69351827455
+size 225584
diff --git a/assets/xarm7/stl/link3.stl b/assets/xarm7/stl/link3.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9e2798e7946dd70046c95455d5ba96392d0b54a6069caba91dc4ca66e1379b42
+size 237084
diff --git a/assets/xarm7/stl/link4.stl b/assets/xarm7/stl/link4.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c757fee95f873191a0633c355c07a360032960771cabbd7593a6cdb0f1ffb089
+size 243684
diff --git a/assets/xarm7/stl/link5.stl b/assets/xarm7/stl/link5.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:715ad5787c5dab57589937fd47289882707b5e1eb997e340d567785b02f4ec90
+size 229084
diff --git a/assets/xarm7/stl/link6.stl b/assets/xarm7/stl/link6.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:85b320aa420497827223d16d492bba8de091173374e361396fc7a5dad7bdb0cb
+size 399384
diff --git a/assets/xarm7/stl/link7.stl b/assets/xarm7/stl/link7.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:97115d848fbf802cb770cd9be639ae2af993103b9d9bbb0c50c943c738a36f18
+size 231684
diff --git a/assets/xarm7/stl/link_base.stl b/assets/xarm7/stl/link_base.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cf52275a913a0977595f5d12373f06583513e26fe18b8cf2827c74da94a4f0be
+size 304784
diff --git a/assets/xarm7/stl/right_finger.stl b/assets/xarm7/stl/right_finger.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c5dee87c7f37baf554b8456ebfe0b3e8ed0b22b8938bd1add6505c2ad6d32c7d
+size 242684
diff --git a/assets/xarm7/stl/right_inner_knuckle.stl b/assets/xarm7/stl/right_inner_knuckle.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b41dd2c2c550281bf78d7cc6fa117b14786700e5c453560a0cb5fd6dfa0ffb3e
+size 366284
diff --git a/assets/xarm7/stl/right_outer_knuckle.stl b/assets/xarm7/stl/right_outer_knuckle.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:75ca1107d0a42a0f03802a9a49cab48419b31851ee8935f8f1ca06be1c1c91e8
+size 22284
diff --git 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_env_cartesian_control.py b/examples/fr3_env_cartesian_control.py
@@ -12,6 +12,8 @@
 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
 
+import rcs
+
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
 
@@ -64,19 +66,22 @@ def main():
         else:
             env_rel = SimEnvCreator()(
                 control_mode=ControlMode.CARTESIAN_TQuat,
-                robot_cfg=default_sim_robot_cfg(),
+                robot_cfg=default_sim_robot_cfg(mjcf=rcs.scenes["fr3_empty_world"]["mjcf_scene"]),
                 collision_guard=False,
                 gripper_cfg=default_sim_gripper_cfg(),
                 cameras=default_mujoco_cameraset_cfg(),
                 max_relative_movement=0.5,
                 relative_to=RelativeTo.LAST_STEP,
+                mjcf=rcs.scenes["fr3_empty_world"]["mjb"],
+                robot_kinematics_path=rcs.scenes["fr3_empty_world"]["mjcf_robot"],
+                kinematics_frame_id="attachment_site_0",
             )
             env_rel.get_wrapper_attr("sim").open_gui()
 
         env_rel.reset()
         print(env_rel.unwrapped.robot.get_cartesian_position())  # type: ignore
 
-        for _ in range(10):
+        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}
diff --git a/examples/fr3_env_joint_control.py b/examples/fr3_env_joint_control.py
@@ -13,6 +13,8 @@
 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
 
+import rcs
+
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
 
@@ -66,15 +68,17 @@ def main():
             env_rel = SimEnvCreator()(
                 control_mode=ControlMode.JOINTS,
                 collision_guard=False,
-                robot_cfg=default_sim_robot_cfg(),
+                robot_cfg=default_sim_robot_cfg(mjcf=rcs.scenes["fr3_empty_world"]["mjcf_scene"]),
                 gripper_cfg=default_sim_gripper_cfg(),
                 cameras=default_mujoco_cameraset_cfg(),
                 max_relative_movement=np.deg2rad(5),
                 relative_to=RelativeTo.LAST_STEP,
+                mjcf=rcs.scenes["fr3_empty_world"]["mjb"],
+                robot_kinematics_path=rcs.scenes["fr3_empty_world"]["mjcf_robot"],
             )
             env_rel.get_wrapper_attr("sim").open_gui()
 
-        for _ in range(10):
+        for _ in range(100):
             obs, info = env_rel.reset()
             for _ in range(10):
                 # sample random relative action and execute it
diff --git a/examples/xarm7_env_cartesian_control.py b/examples/xarm7_env_cartesian_control.py
@@ -0,0 +1,74 @@
+import logging
+from time import sleep
+
+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_robot"])
+    env_rel = SimEnvCreator()(
+        control_mode=ControlMode.CARTESIAN_TQuat,
+        collision_guard=False,
+        robot_cfg=robot_cfg,
+        gripper_cfg=None,
+        # cameras=default_mujoco_cameraset_cfg(),
+        max_relative_movement=0.5,
+        relative_to=RelativeTo.LAST_STEP,
+        mjcf=rcs.scenes["xarm7_empty_world"]["mjb"],
+        robot_kinematics_path=rcs.scenes["xarm7_empty_world"]["mjcf_robot"],
+    )
+    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
diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py
diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py
diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py
diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py

Original file line number	Diff line number	Diff line change
`@@ -1,2 +1 @@`
`1`		`-.stl filter=lfs diff=lfs merge=lfs -text`
`2`		`-.obj filter=lfs diff=lfs merge=lfs -text`
	`1`	`+*.stl filter=lfs diff=lfs merge=lfs -text`