Merge pull request #543 from PickNikRobotics/add-mujoco-objectives-to-all-configs

Add MujocoBehaviorsLoader and mujoco_objectives to all robot configs

Author: marioprats

src/april_tag_sim/config/config.yaml

@@ -23,6 +23,9 @@ objectives:
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
+    mujoco_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/mujoco"
     sim_objectives:
       package_name: "april_tag_sim"
       relative_path: "objectives"

src/april_tag_sim/description/scene.xml

@@ -141,11 +141,28 @@
     <include file="ur5e/ur5e_linear_rail.xml" />
   </worldbody>
 
-  <!--  <keyframe>-->
-  <!--    <key-->
-  <!--      qpos="-0.886764 0.727551 0.472375 -0.834559 -6.95805e-05 -6.1375e-05 -0.550919 -0.499647 0.689928 0.470942 -0.749838 3.91843e-05 -5.08119e-05 -0.661622 -0.3609 0.609537 0.470941 -0.757741 2.96011e-05 4.00136e-05 -0.652556 -0.303591 0.696467 0.470989 -0.7788 -5.32927e-05 2.99347e-06 -0.627272 -0.000373058 0.88828 0.473534 -0.838137 -1.0566e-05 -3.07868e-06 0.54546 -0.0963647 0.840186 0.473528 0.281469 -1.36042e-05 6.60035e-06 -0.95957 0.0484372 0.842182 0.473688 -0.758342 6.07647e-06 2.73022e-05 -0.651856 0.2 0.75 0.499376 1 0 0 0 1.70629e-05 0.749992 0.499377 1 0.00016562 0.000360588 1.95345e-10 -0.2 0.75 0.499376 1 0 0 0 0 0 -1.88491 1.10028 -1.25653 -1.57075 -1.08677e-12 0.0208231 0.000536851 0.0221006 -0.022866 0.020837 0.000547055 0.0221413 -0.0229382"-->
-  <!--      qvel="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 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 -1.8849 1.0997 -1.2566 -1.57075 0 0 0"-->
-  <!--    />-->
-  <!--  </keyframe>-->
+  <keyframe>
+    <!-- qpos layout: -->
+    <!-- block1..block6 freejoints [x y z qw qx qy qz] (6 x 7 = 42) -->
+    <!-- linear_rail_joint slide (1) -->
+    <!-- shoulder_pan, shoulder_lift, elbow, wrist_1, wrist_2, wrist_3 (6) -->
+    <!-- robotiq_85_right_knuckle_joint (1) -->
+    <!-- 7 gripper mimic joints (7) -->
+    <!-- total: 42 + 1 + 6 + 1 + 7 = 57 -->
+    <key
+      name="default"
+      qpos="
+        1.75 0.75 0.575 1 0 0 0
+        1.0  0.75 0.575 1 0 0 0
+        0.25 0.75 0.575 1 0 0 0
+        -0.5  0.75 0.575 1 0 0 0
+        -1.25 0.75 0.575 1 0 0 0
+        -2.0  0.75 0.575 1 0 0 0
+        0
+        -1.88491 1.10028 -1.25653 -1.57075 0 0
+        0
+        0 0 0 0 0 0 0"
+      ctrl="0 -1.8849 1.0997 -1.2566 -1.57075 0 0 0"
+    />
+  </keyframe>
 </mujoco>

src/dual_arm_sim/config/config.yaml

@@ -32,6 +32,7 @@ hardware:
     # Many of these are specific to the UR descriptions packages, and can be customized as needed.
     # [Optional]
     urdf_params:
+      - mujoco_keyframe: "default"
       - mujoco_model: "description/mujoco/scene.xml"
       - mujoco_viewer: false
     simulated_hardware_launch_file:
@@ -86,6 +87,7 @@ objectives:
       - "moveit_pro::behaviors::MTCCoreBehaviorsLoader"
       - "moveit_pro::behaviors::VisionBehaviorsLoader"
       - "moveit_pro::behaviors::ConverterBehaviorsLoader"
+      - "moveit_pro::behaviors::MujocoBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
@@ -101,6 +103,9 @@ objectives:
     visualization_objectives:
       package_name: "moveit_pro_objectives"
       relative_path: "objectives/visualization"
+    mujoco_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/mujoco"
     custom_objectives:
       package_name: "dual_arm_sim"
       relative_path: "objectives"

src/factory_sim/config/config.yaml

@@ -41,7 +41,7 @@ hardware:
     # Many of these are specific to the descriptions packages, and can be customized as needed.
     # [Optional]
     urdf_params:
-        # Use "mock", "mujoco", or "real"
+      # Use "mock", "mujoco", or "real"
       - hardware_interface: "mujoco"
       - mujoco_viewer: "false"
 
@@ -124,6 +124,7 @@ objectives:
       - "moveit_pro::behaviors::MTCCoreBehaviorsLoader"
       - "moveit_pro::behaviors::VisionBehaviorsLoader"
       - "moveit_pro::behaviors::ConverterBehaviorsLoader"
+      - "moveit_pro::behaviors::MujocoBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
@@ -139,6 +140,9 @@ objectives:
     visualization_objectives:
       package_name: "moveit_pro_objectives"
       relative_path: "objectives/visualization"
+    mujoco_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/mujoco"
     sim_objectives:
       package_name: "factory_sim"
       relative_path: "objectives"

src/factory_sim/description/scene.xml

@@ -412,4 +412,25 @@
 
   <!-- Add welds for suction gripping of parts -->
   <include file="box-welds.xml" />
+
+  <keyframe>
+    <!-- qpos layout: -->
+    <!-- robot [joint_1..joint_6] (6) -->
+    <!-- shelf freejoint [x y z qw qx qy qz] (7) -->
+    <!-- bracket-0 freejoint (7), bracket-1 freejoint (7), bracket-2 freejoint (7) -->
+    <!-- suction_tool (gripper_base) freejoint (7) -->
+    <!-- inspection_tool freejoint (7) -->
+    <key
+      name="default"
+      qpos="
+        0 0 0 0 0 0
+        0.25 -0.4 0.2301 1 0 0 0
+        -0.42 0.5 0.35 1 0 0 0
+        -0.42 0.56 0.40 1 0 0 0
+        -0.42 0.62 0.45 1 0 0 0
+        0.25 0.08 0.3 0 1 0 0
+        0.25 -0.08 0.3 0 1 0 0"
+      ctrl="0 0 0 0 0 0"
+    />
+  </keyframe>
 </mujoco>

src/grinding_sim/config/config.yaml

@@ -41,7 +41,8 @@ hardware:
     # Many of these are specific to the descriptions packages, and can be customized as needed.
     # [Optional]
     urdf_params:
-        # Use "mock", "mujoco", or "real"
+      - mujoco_keyframe: "default"
+      # Use "mock", "mujoco", or "real"
       - hardware_interface: "mujoco"
 
 # Sets ROS global params for launch.
@@ -115,6 +116,7 @@ objectives:
       - "moveit_pro::behaviors::MTCCoreBehaviorsLoader"
       - "moveit_pro::behaviors::VisionBehaviorsLoader"
       - "moveit_pro::behaviors::ConverterBehaviorsLoader"
+      - "moveit_pro::behaviors::MujocoBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
@@ -130,6 +132,9 @@ objectives:
     visualization_objectives:
       package_name: "moveit_pro_objectives"
       relative_path: "objectives/visualization"
+    mujoco_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/mujoco"
     sim_objectives:
       package_name: "grinding_sim"
       relative_path: "objectives"

src/kitchen_sim/config/config.yaml

@@ -33,6 +33,7 @@ hardware:
     # Many of these are specific to the UR descriptions packages, and can be customized as needed.
     # [Optional]
     urdf_params:
+      - mujoco_keyframe: "default"
       - mujoco_model: "description/mujoco/scene.xml"
       - mujoco_viewer: false
     simulated_hardware_launch_file:
@@ -52,6 +53,7 @@ objectives:
       - "moveit_pro::behaviors::MTCCoreBehaviorsLoader"
       - "moveit_pro::behaviors::VisionBehaviorsLoader"
       - "moveit_pro::behaviors::ConverterBehaviorsLoader"
+      - "moveit_pro::behaviors::MujocoBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
@@ -67,6 +69,9 @@ objectives:
     visualization_objectives:
       package_name: "moveit_pro_objectives"
       relative_path: "objectives/visualization"
+    mujoco_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/mujoco"
     sim_objectives:
       package_name: "kitchen_sim"
       relative_path: "objectives"

src/kitchen_sim/description/mujoco/scene.xml

@@ -157,7 +157,7 @@
          fr3_finger1 - We control only one finger.
     -->
     <key
-      name="home"
+      name="default"
       qpos="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0.035 0.035 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.2 0.6 0.96 0 0 0 0"
       ctrl="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0.035"
     />

src/lab_sim/config/config.yaml

@@ -16,6 +16,7 @@ hardware:
       package: "lab_sim"
       path: "config/moveit/picknik_ur.srdf"
     urdf_params:
+      - mujoco_keyframe: "default"
       - mujoco_model: "description/scene.xml"
       - mujoco_viewer: false
 

src/moveit_pro_kinova_configs/kinova_sim/config/config.yaml

@@ -51,6 +51,7 @@ hardware:
     # Many of these are specific to the UR descriptions packages, and can be customized as needed.
     # [Optional]
     urdf_params:
+      - mujoco_keyframe: "default"
       - arm: "gen3"
       - dof: "7"
       - vision: "false"