Use keyframe to initialize state (#323)

---- Co-authored-by: Michael Wrock <[email protected]> Co-authored-by: Mario Prats <[email protected]>
PickNikRobotics · Aug 8, 2024 · 1ff1238 · 1ff1238
1 parent eb278e4
commit 1ff1238
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 13 deletions.
diff --git a/src/picknik_ur_mujoco_config/description/scene.xml b/src/picknik_ur_mujoco_config/description/scene.xml
@@ -47,18 +47,40 @@
         <geom type="box" size="0.5 0.5 0.025" material="wood"/>
     </body>
     <!-- Define the blocks with AprilTags on each face -->
-    <body name="block1" pos="0.75 0.2 0.51">
+    <body name="block1">
         <freejoint name="block1"/>
         <geom type="box" size="0.025 0.025 0.025" rgba="1 0 0 1" material="apriltag_material1"/>
     </body>
-    <body name="block2" pos="0.75 0.0 0.51">
+    <body name="block2">
         <freejoint name="block2"/>
         <geom type="box" size="0.025 0.025 0.025" rgba="0 1 0 1" material="apriltag_material2"/>
     </body>
-    <body name="block3" pos="0.75 -0.2 0.51">
+    <body name="block3">
         <freejoint name="block3"/>
         <geom type="box" size="0.025 0.025 0.025" rgba="0 0 1 1" material="apriltag_material3"/>
     </body>
 
   </worldbody>
+
+  <keyframe>
+    <!-- qpos -->
+    <!-- linear_rail, shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3 -->
+    <!-- right_knuckle, right_finger, right_inner_knuckle, right_finger_tip, left_knuckle, left_finger, left_inner_knuckle, left_finger_tip -->
+    <!-- block1 [x, y, z, qw, qx, qy, qz] -->
+    <!-- block2 [x, y, z, qw, qx, qy, qz] -->
+    <!-- block3 [x, y, z, qw, qx, qy, qz] -->
+    <!-- ctrl -->
+    <!-- linear_rail, shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3, robotiq_85_left_knuckle_joint -->
+    <key qpos="
+      0 0 -1.5707 1.5707 -1.8 -1.5707 0
+      0 0 0 0 0 0 0 0
+      0.75 0.2 0.51 1 0 0 0
+      0.75 0.0 0.51 1 0 0 0
+      0.75 -0.2 0.51 1 0 0 0
+    "
+    ctrl="
+      0 0 -1.5707 1.5707 -1.8 -1.5707 0 0
+    "
+    />
+  </keyframe>
 </mujoco>
diff --git a/src/picknik_ur_mujoco_config/description/ur5e.xml b/src/picknik_ur_mujoco_config/description/ur5e.xml
@@ -280,6 +280,10 @@
     </body>
   </worldbody>
 
+  <actuator>
+    <position class="robot" name="linear_rail_joint" joint="linear_rail_joint" kp="75000" dampratio="1.0"/>
+  </actuator>
+
   <actuator>
     <general class="size3" name="shoulder_pan_joint" joint="shoulder_pan_joint"/>
     <general class="size3" name="shoulder_lift_joint" joint="shoulder_lift_joint"/>
@@ -291,15 +295,10 @@
              gainprm="100 0 0" biasprm="0 -100 -150"/>
   </actuator>
 
-
   <contact>
     <exclude body1="base_platform" body2="base"/>
   </contact>
 
-  <actuator>
-    <position class="robot" name="linear_rail_joint" joint="linear_rail_joint" kp="75000" dampratio="1.0"/>
-  </actuator>
-
     <!-- Robotiq gripper contact -->
   <contact>
     <exclude body1="robotiq_85_base_link" body2="robotiq_85_left_knuckle_link"/>
@@ -327,9 +326,4 @@
     <force name="ee_force_sensor" site="robotiq_ft_sensor"/>
     <torque name="ee_torque_sensor" site="robotiq_ft_sensor" />
   </sensor>
-
-  <keyframe>
-    <!-- ur_base_pan ur_shoulder ur_elbow ur_wrist_1 ur_wrist_2 ur_wrist_3 gripper linear_rail-->
-    <key qpos='0 -2.1 1.2 -1.9 -1.9 0 0 1.5 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'/>
-  </keyframe>
 </mujoco>