mujoco model fixes and tweaks

marioprats · marioprats · commit 0f31687b6754 · 2025-09-17T16:03:10.000-06:00
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/franka.urdf.xacro b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/franka.urdf.xacro
@@ -34,7 +34,7 @@
   <joint name="right_grasp_link_joint" type="fixed">
      <parent link="right_fr3_link8"/>
      <child link = "right_grasp_link"/>
-     <origin xyz="0 0 0.15" rpy="0 0 -1.5708"/>
+     <origin xyz="0 0 0" rpy="0 0 -1.5708"/>
   </joint>
 
   <joint name="left_mount_joint" type="fixed">
@@ -64,9 +64,9 @@
     <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
    <link name="left_grasp_link" />
    <joint name="left_grasp_link_joint" type="fixed">
-      <parent link="left_fr3_link8"/>
+      <parent link="world"/>
       <child link = "left_grasp_link"/>
-      <origin xyz="0 0 0.15" rpy="0 0 -1.5708"/>
+      <origin xyz="0 0 0" rpy="0 0 -1.5708"/>
     </joint>
 
     <ros2_control name="franka_mujoco_simulation" type="system">
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/franka3.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/franka3.xml
@@ -100,7 +100,7 @@
       <body
         name="left_arm"
         pos="0.035 0.050681 0.356"
-        quat="0.8730 -0.4380 -0.0229 -0.2215"
+        quat="0.8657172 -0.4369879 0.0223569 -0.2430545"
       >
         <body name="left_base" childclass="fr3">
           <body name="left_fr3_link0">
@@ -411,7 +411,7 @@
       <body
         name="right_arm"
         pos="0.035 -0.050681 0.356"
-        quat="0.8730 0.4380 0.0229 0.2215"
+        quat="0.8657172 0.4369879 0.0223569 0.2430545"
       >
         <body name="right_base" childclass="fr3">
           <body name="right_fr3_link0">
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/scene.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/scene.xml
@@ -2,8 +2,8 @@
   <include file="franka3.xml" />
 
   <statistic center="0.2 0.2 0.5" extent="1.8" />
-  <size nkey="1"/>
-  <size nuser_jnt="0"/>
+  <size nkey="1" />
+  <size nuser_jnt="0" />
 
   <visual>
     <global azimuth="120" elevation="-21" fovy="58" />
@@ -68,32 +68,32 @@
       type="box"
       size="0.25 0.5 0.01"
       pos="0 0.9 0.0"
-      rgba="0.7 0.3 0.3 1"
+      rgba="0.3 0.7 0.3 1"
     />
     <geom
       name="box_right"
       type="box"
       size="0.25 0.5 0.01"
       pos="0 -0.9 0.0"
-      rgba="0.3 0.7 0.3 1"
+      rgba="0.7 0.3 0.3 1"
     />
 
     <body name="block_green_1" pos="0.5 0 0.2">
       <freejoint name="block_green_1" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0 0.8 0 1" />
     </body>
-    <body name="block_green_2" pos="0.5 0.1 0.2">
+    <!-- <body name="block_green_2" pos="0.5 0.1 0.2">
       <freejoint name="block_green_2" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0 0.8 0 1" />
-    </body>
+    </body> -->
     <body name="block_green_3" pos="0.5 0.2 0.2">
       <freejoint name="block_green_3" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0 0.8 0 1" />
     </body>
-    <body name="block_green_4" pos="0.5 -0.1 0.2">
+    <!-- <body name="block_green_4" pos="0.5 -0.1 0.2">
       <freejoint name="block_green_4" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0 0.8 0 1" />
-    </body>
+    </body> -->
     <body name="block_green_5" pos="0.5 -0.2 0.2">
       <freejoint name="block_green_5" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0 0.8 0 1" />
@@ -103,18 +103,18 @@
       <freejoint name="block_red_1" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0.8 0 0 1" />
     </body>
-    <body name="block_red_2" pos="0.6 0.1 0.2">
+    <!-- <body name="block_red_2" pos="0.6 0.1 0.2">
       <freejoint name="block_red_2" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0.8 0 0 1" />
-    </body>
+    </body> -->
     <body name="block_red_3" pos="0.6 0.2 0.2">
       <freejoint name="block_red_3" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0.8 0 0 1" />
     </body>
-    <body name="block_red_4" pos="0.6 -0.1 0.2">
+    <!-- <body name="block_red_4" pos="0.6 -0.1 0.2">
       <freejoint name="block_red_4" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0.8 0 0 1" />
-    </body>
+    </body> -->
     <body name="block_red_5" pos="0.6 -0.2 0.2">
       <freejoint name="block_red_5" />
       <geom type="box" size="0.025 0.025 0.025" rgba="0.8 0 0 1" />
@@ -127,14 +127,10 @@
       qpos="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0
             0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0
             0.5 0 0.2 1 0 0 0
-            0.5 0.1 0.2 1 0 0 0
             0.5 0.2 0.2 1 0 0 0
-            0.5 -0.1 0.2 1 0 0 0
             0.5 -0.2 0.2 1 0 0 0
             0.6 0 0.2 1 0 0 0
-            0.6 0.1 0.2 1 0 0 0
             0.6 0.2 0.2 1 0 0 0
-            0.6 -0.1 0.2 1 0 0 0
             0.6 -0.2 0.2 1 0 0 0"
       qvel="0 0 0 0 0 0 0 0 0
             0 0 0 0 0 0 0 0 0
@@ -143,14 +139,9 @@
             0 0 0 0 0 0
             0 0 0 0 0 0
             0 0 0 0 0 0
-            0 0 0 0 0 0
-            0 0 0 0 0 0
-            0 0 0 0 0 0
-            0 0 0 0 0 0
             0 0 0 0 0 0"
       ctrl="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0
             0 -0.7854 0 -2.3562 0 1.5707 0.7853 0 0"
     />
   </keyframe>
-
 </mujoco>
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/find_red_block.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/find_red_block.xml
@@ -5,7 +5,7 @@
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree
         ID="Find with prompt"
-        _collapsed="true"
+        _collapsed="false"
         prompt="a red block"
         object_centroid="{object_centroid}"
       />
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/find_with_prompt.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/find_with_prompt.xml
@@ -15,7 +15,7 @@
         points_topic_name="/scene_camera/points"
         clip_model_path="models/clip.onnx"
         clipseg_model_path="models/clipseg.onnx"
-        erosion_size="2"
+        erosion_size="5"
         prompts="{prompt}"
         threshold="0.28"
         masks3d="{masks3d}"
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/sequential_sort.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/sequential_sort.xml
diff --git a/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/take_snap.xml b/src/moveit_pro_franka_configs/franka_dual_arm_sim/objectives/take_snap.xml
