Added pedestal, for reference.

kineticsystem · kineticsystem · commit 0d061d9e5d2f · 2025-09-09T19:28:53.000+02:00
diff --git a/src/moveit_pro_franka_configs/franka_arm_sim/description/franka.urdf.xacro b/src/moveit_pro_franka_configs/franka_arm_sim/description/franka.urdf.xacro
@@ -26,6 +26,45 @@
                       xyz="0 0 0.9">
   </xacro:franka_robot>
   
+  <!-- Pedestal the robot stands on -->
+  <link name="pedestal">
+    <!-- simple mass/inertia for a solid cylinder -->
+    <inertial>
+      <mass value="30.0"/> <!-- adjust as needed -->
+      <!-- Inertia for solid cylinder: Ixx=Iyy=(1/12)m(3r^2+h^2), Izz=(1/2)m r^2 -->
+      <!-- For r=0.2 m, h=0.9 m, m=30 kg: Ixx=Iyy≈2.325, Izz≈0.6 -->
+      <origin xyz="0 0 0.45"/> <!-- center of mass at half height -->
+      <inertia ixx="2.325" ixy="0.0" ixz="0.0"
+               iyy="2.325" iyz="0.0"
+               izz="0.6"/>
+    </inertial>
+
+    <visual>
+      <!-- place the cylinder so its base sits at z=0 and top at z=0.9 -->
+      <origin xyz="0 0 0.45"/>
+      <geometry>
+        <cylinder radius="0.2" length="0.9"/>
+      </geometry>
+      <material name="pedestal_grey">
+        <color rgba="0.7 0.7 0.7 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0.45"/>
+      <geometry>
+        <cylinder radius="0.2" length="0.9"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Fix pedestal to the world at the origin -->
+  <joint name="pedestal_to_world" type="fixed">
+    <parent link="world"/>
+    <child link="pedestal"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
     <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
    <link name="grasp_link" />
    <joint name="grasp_link_joint" type="fixed">
diff --git a/src/moveit_pro_franka_configs/franka_arm_sim/description/mujoco/scene.xml b/src/moveit_pro_franka_configs/franka_arm_sim/description/mujoco/scene.xml
@@ -67,6 +67,18 @@
       xyaxes="0.510 -0.860 0.000 0.666 0.395 0.633"
     />
 
+    <!-- Cylindrical pedestal: radius=0.2, length=0.9 (size takes half-length=0.45) -->
+    <body name="pedestal" pos="0 0 0">
+      <!-- place geom so the cylinder stands from z=0 to z=0.9 -->
+      <geom name="pedestal_geom"
+            type="cylinder"
+            size="0.2 0.45"
+            pos="0 0 0.45"
+            rgba="0.7 0.7 0.7 1"/>
+      <!-- useful marker at the top of the pedestal -->
+      <site name="pedestal_top" pos="0 0 0.9" size="0.01" type="sphere" rgba="0.1 0.8 0.1 1"/>
+    </body>
+
     <body name="desk" pos="0.8 0.1 0" euler="0 0 -1.5708">
       <body name="counters1" pos="0 0 0">
         <include file="assets/kitchen_assets/item_assets/counters_chain.xml" />
@@ -120,8 +132,7 @@
          fr3_joint5,
          fr3_joint6,
          fr3_joint7,
-         fr3_finger1,
-         fr3_finger2
+         fr3_finger1 - We control only one finger.
     -->
     <key
       name="home"
diff --git a/src/moveit_pro_franka_configs/franka_arm_sim/objectives/take_scene_camera_snapshot.xml b/src/moveit_pro_franka_configs/franka_arm_sim/objectives/take_scene_camera_snapshot.xml
@@ -14,7 +14,7 @@
         acceleration_scale_factor="1.0"
         controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
         controller_names="joint_trajectory_controller"
-        joint_group_name="arm"
+        joint_group_name="manipulator"
         keep_orientation="false"
         keep_orientation_tolerance="0.05"
         link_padding="0.01"
@@ -93,7 +93,7 @@
         seed="0"
         velocity_scale_factor="1.0"
         waypoint_name="Home"
-        joint_group_name="arm"
+        joint_group_name="manipulator"
       />
     </Control>
   </BehaviorTree>
diff --git a/src/moveit_pro_franka_configs/franka_arm_sim/waypoints/waypoints.yaml b/src/moveit_pro_franka_configs/franka_arm_sim/waypoints/waypoints.yaml
@@ -1,7 +1,7 @@
 - description: ''
   favorite: false
   joint_group_names:
-  - arm
+  - manipulator
   joint_state:
     effort: []
     header:
@@ -40,8 +40,6 @@
 - description: ''
   favorite: false
   joint_group_names:
-  - arm
-  - hand
   - manipulator
   joint_state:
     effort: []
@@ -60,14 +58,14 @@
     - fr3_joint7
     - fr3_finger_joint1
     position:
-    - -2.2161396085498604
-    - 1.5466183464436773
-    - -0.22145440706027172
-    - -1.6205947051727179
-    - -0.004699868288038443
-    - 1.581359852803185
-    - 0.7845073181465004
-    - 0.034967833251422394
+    - -2.2283963696816342
+    - 1.563234326437979
+    - -0.21066213082094074
+    - -1.4035234551463138
+    - -0.0034359985486580383
+    - 1.6199226560931328
+    - 0.7839630999178752
+    - 0.035000000000000066
     velocity: []
   multi_dof_joint_state:
     header:
@@ -83,8 +81,6 @@
 - description: ''
   favorite: false
   joint_group_names:
-  - arm
-  - hand
   - manipulator
   joint_state:
     effort: []
diff --git a/src/moveit_pro_franka_configs/franka_base_config/config/moveit/franka.srdf b/src/moveit_pro_franka_configs/franka_base_config/config/moveit/franka.srdf
@@ -1,23 +1,28 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot name="franka">
+
   <!-- Planning groups -->
-  <group name="arm">
-    <chain base_link="fr3_link0" tip_link="fr3_link8"/>
-  </group>
-  <group name="hand">
-    <link name="fr3_hand"/>
+
+  <!-- All gripper links, excluding arm. -->
+  <group name="gripper">
+    <link name="fr3_hand_tcp"/> <!-- root of gripper -->
     <link name="fr3_leftfinger"/>
     <link name="fr3_rightfinger"/>
-    <link name="fr3_hand_tcp"/>
     <link name="grasp_link"/>
     <joint name="fr3_finger_joint1"/>
-    <passive_joint name="$fr3_finger_joint2"/>
+    <passive_joint name="fr3_finger_joint2"/>
   </group>
+
+  <!-- All robot links, including gripper. -->
   <group name="manipulator">
     <chain base_link="fr3_link0" tip_link="grasp_link"/>
   </group>
+
+  <!-- End effectors -->
+  <end_effector name="hand_tcp" parent_link="grasp_link" group="gripper" parent_group="manipulator"/>
+
   <!-- Arm group states -->
-  <group_state name="ready" group="arm">
+  <group_state name="ready" group="manipulator">
     <joint name="fr3_joint1" value="0"/>
     <joint name="fr3_joint2" value="-0.7853"/>
     <joint name="fr3_joint3" value="0"/>
@@ -34,8 +39,6 @@
     <joint name="fr3_finger_joint1" value="0"/>
     <joint name="fr3_finger_joint2" value="0"/>
   </group_state>
-  <!-- End effectors -->
-  <end_effector name="hand_tcp" parent_link="fr3_link8" group="hand" parent_group="arm"/>
 
   <!-- Arm collisions -->
   <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="fr3_link0"/>
