Merge pull request #409 from PickNikRobotics/keyframe_from_behavior

Update space_satellite_sim to use new reset keyframe behavior

Author: rlpratt12

src/grinding_sim/description/scene.xml

@@ -398,7 +398,7 @@
 
   <keyframe>
     <key
-      name="retract"
+      name="default"
       qpos="1.57055 -0.999904 -1.80019 -2.45625 -1.57086 -3.14161 0.0256934"
       ctrl="1.57055 -0.999904 -1.80019 -2.45625 -1.57086 -3.14161 115"
     />

src/lab_sim/config/config.yaml

@@ -47,9 +47,13 @@ objectives:
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
       - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+      - "moveit_studio::behaviors::MujocoBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
+    mujoco_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/mujoco"
     sim_objectives:
       package_name: "lab_sim"
       relative_path: "objectives"

src/lab_sim/description/scene.xml

@@ -73,6 +73,7 @@
 
   <keyframe>
     <key
+      name="default"
       qpos="-1.168 0.776711 0.469754 1 5.72696e-06 -4.26533e-08 -9.38821e-05 -1.34591 1.07777 0.902167 1 -2.70778e-08 7.60222e-07 -8.18971e-05 -1.1503 1.07777 0.902163 -0.873311 -1.32611e-06 -2.31428e-06 -0.487163 -0.871046 0.729603 0.473006 -0.839083 -3.76738e-06 -5.23752e-06 -0.544003 -0.699998 0.85 0.472753 -0.759699 6.25044e-07 1.49157e-06 -0.650275 -0.5 0.69 0.472779 -0.75966 -8.69884e-06 3.09906e-06 -0.65032 -0.360001 0.609999 0.472778 -0.759663 -6.29279e-06 3.61581e-06 -0.650317 -0.299999 0.7 0.472779 -0.75966 -5.34077e-06 1.33814e-06 -0.65032 -3.70361e-09 0.9 0.47375 -0.839072 -1.82055e-05 -5.31435e-05 0.544021 -0.1 0.85 0.47375 0.283662 4.83891e-05 2.85336e-05 -0.958924 0.0952783 0.840677 0.47375 -0.852936 4.00776e-05 -3.93632e-05 -0.522015 0.2 0.75 0.498669 1 3.49711e-17 2.9976e-18 1.51884e-30 1.73912e-05 0.749992 0.498669 1 0.00016562 0.000360588 4.6626e-11 -0.2 0.75 0.498669 1 2.94315e-17 -7.92697e-18 -4.14434e-30 1.56909e-06 -3.71262e-07 -1.88491 1.10086 -1.25647 -1.57075 -3.63345e-07 0.0525545 0.000140183 0.0525603 -0.050625 0.0525545 0.000140184 0.0525567 -0.0506181"
       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 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"

src/moveit_pro_franka_configs/franka_dual_arm_sim/description/mujoco/scene.xml

@@ -116,7 +116,7 @@
 
     -->
     <key
-      name="home"
+      name="default"
       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"
       qvel="0 0 0 0 0 0 0 0 0

src/moveit_pro_kinova_configs/kinova_sim/config/config.yaml

@@ -154,6 +154,7 @@ objectives:
       - "moveit_studio::behaviors::ServoBehaviorsLoader"
       - "moveit_studio::behaviors::VisionBehaviorsLoader"
       - "moveit_studio::behaviors::ConverterBehaviorsLoader"
+      - "moveit_studio::behaviors::MujocoBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:
@@ -166,6 +167,9 @@ objectives:
     perception_objectives:
       package_name: "moveit_pro_objectives"
       relative_path: "objectives/perception"
+    mujoco_objectives:
+      package_name: "moveit_pro_objectives"
+      relative_path: "objectives/mujoco"
     sim_objectives:
       package_name: "kinova_sim"
       relative_path: "objectives"

src/moveit_pro_kinova_configs/space_satellite_sim/objectives/grapple_moving_satellite_with_fuse.xml

@@ -11,9 +11,29 @@
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Open Gripper" _collapsed="true" />
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        acceleration_scale_factor="1.0"
+        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+        controller_names="joint_trajectory_controller"
+        joint_group_name="manipulator"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        seed="0"
+        velocity_scale_factor="1.0"
+        waypoint_name="View Tag"
+      />
       <Action
         ID="SwitchController"
         activate_controllers="velocity_force_controller"
+        activate_asap="true"
+        automatic_deactivation="true"
+        controller_list_action_name="/controller_manager/list_controllers"
+        controller_switch_action_name="/controller_manager/switch_controller"
+        strictness="1"
+        timeout="-1.000000"
       />
       <Action
         ID="CreateStampedPose"
@@ -39,6 +59,12 @@
         stamped_twist="{desired_twist}"
         reference_frame="world"
       />
+      <Action
+        ID="ResetMujocoKeyframe"
+        response_timeout="10.000000"
+        wait_for_server_available_timeout="3.000000"
+        keyframe_name="moving_target_satellite"
+      />
       <Control ID="Parallel" failure_count="1" success_count="1">
         <Decorator ID="KeepRunningUntilFailure">
           <Action
@@ -96,6 +122,7 @@
             link_padding="0.010000"
             planning_group_names="manipulator"
             stop_safety_factor="1.200000"
+            include_octomap="false"
           />
         </Decorator>
         <Control ID="Sequence" name="TopLevelSequence">