Merge pull request #309 from PickNikRobotics/imarker-group-support

update multi_arm_sim to use UI-provided planning group and tip link

Author: shaur-k

src/hangar_sim/config/control/picknik_ur.ros2_control.yaml

@@ -203,7 +203,7 @@ joint_trajectory_controller:
       - platform_velocity_controller/rotational_yaw_joint
     state_publish_rate: 100.0
     action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
+    allow_partial_joints_goal: true
     open_loop_control: false # this is closed loop
     gains:
       shoulder_pan_joint:

src/hangar_sim/objectives/request_teleoperation.xml

@@ -18,6 +18,7 @@
           current_teleop_mode="{teleop_mode}"
           planning_groups="{planning_groups}"
           controllers="{controllers}"
+          tip_links="{tip_links}"
         />
         <Decorator ID="KeepRunningUntilFailure">
           <Control ID="Sequence">
@@ -73,11 +74,29 @@
                       timeout_sec="-1"
                       pose="{target_pose}"
                     />
+                    <Decorator
+                      ID="ForEach"
+                      index="{index}"
+                      out="{planning_group}"
+                      vector_in="{planning_groups}"
+                    >
+                      <Action ID="AlwaysSuccess" />
+                    </Decorator>
+                    <Decorator
+                      ID="ForEach"
+                      index="{index}"
+                      out="{tip_link}"
+                      vector_in="{tip_links}"
+                    >
+                      <Action ID="AlwaysSuccess" />
+                    </Decorator>
                     <SubTree
                       ID="Move to Pose"
                       _collapsed="false"
                       target_pose="{target_pose}"
                       link_padding="0.0"
+                      planning_group_name="{planning_group}"
+                      ik_frame="{tip_link}"
                       controller_names="/joint_trajectory_controller"
                     />
                     <Action

src/lab_sim/test/objectives_integration_test.py

@@ -40,7 +40,6 @@
 cancel_objectives = {
     "3 Waypoints Pick and Place",
     "Cycle Between Waypoints",
-    "Grasp Object from Text Prompt",
     "Grasp Planning",
     "Grasp Pose Tuning With April Tag",
     "Grasp Pose Using Yaml",
@@ -56,6 +55,7 @@
 }
 
 skip_objectives = {
+    "Grasp Object from Text Prompt",  # https://github.com/PickNikRobotics/moveit_pro/issues/13236
     "Grasp Planning",
     "Joint Diagnostic",
     "Fuse Multiple Views",

src/moveit_pro_ur_configs/multi_arm_sim/config/moveit/multi_arm_ur.srdf

@@ -29,35 +29,47 @@
     </group>
     <group name="gripper">
         <joint name="first_robotiq_85_left_finger_tip_joint"/>
+        <joint name="first_robotiq_85_left_finger_joint"/>
         <joint name="first_robotiq_85_left_inner_knuckle_joint"/>
         <joint name="first_robotiq_85_left_knuckle_joint"/>
         <joint name="first_robotiq_85_right_finger_tip_joint"/>
+        <joint name="first_robotiq_85_right_finger_joint"/>
         <joint name="first_robotiq_85_right_inner_knuckle_joint"/>
         <joint name="first_robotiq_85_right_knuckle_joint"/>
+        <link name="first_grasp_link"/>
     </group>
     <group name="second_gripper">
         <joint name="second_robotiq_85_left_finger_tip_joint"/>
+        <joint name="second_robotiq_85_left_finger_joint"/>
         <joint name="second_robotiq_85_left_inner_knuckle_joint"/>
         <joint name="second_robotiq_85_left_knuckle_joint"/>
         <joint name="second_robotiq_85_right_finger_tip_joint"/>
+        <joint name="second_robotiq_85_right_finger_joint"/>
         <joint name="second_robotiq_85_right_inner_knuckle_joint"/>
         <joint name="second_robotiq_85_right_knuckle_joint"/>
+        <link name="second_grasp_link"/>
     </group>
     <group name="third_gripper">
         <joint name="third_robotiq_85_left_finger_tip_joint"/>
+        <joint name="third_robotiq_85_left_finger_joint"/>
         <joint name="third_robotiq_85_left_inner_knuckle_joint"/>
         <joint name="third_robotiq_85_left_knuckle_joint"/>
         <joint name="third_robotiq_85_right_finger_tip_joint"/>
+        <joint name="third_robotiq_85_right_finger_joint"/>
         <joint name="third_robotiq_85_right_inner_knuckle_joint"/>
         <joint name="third_robotiq_85_right_knuckle_joint"/>
+        <link name="third_grasp_link"/>
     </group>
     <group name="fourth_gripper">
         <joint name="fourth_robotiq_85_left_finger_tip_joint"/>
+        <joint name="fourth_robotiq_85_left_finger_joint"/>
         <joint name="fourth_robotiq_85_left_inner_knuckle_joint"/>
         <joint name="fourth_robotiq_85_left_knuckle_joint"/>
         <joint name="fourth_robotiq_85_right_finger_tip_joint"/>
+        <joint name="fourth_robotiq_85_right_finger_joint"/>
         <joint name="fourth_robotiq_85_right_inner_knuckle_joint"/>
         <joint name="fourth_robotiq_85_right_knuckle_joint"/>
+        <link name="fourth_grasp_link"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="open_right" group="gripper">
@@ -126,6 +138,9 @@
     </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="first_ee" parent_link="first_tool0" group="gripper"/>
+    <end_effector name="second_ee" parent_link="second_tool0" group="second_gripper"/>
+    <end_effector name="third_ee" parent_link="third_tool0" group="third_gripper"/>
+    <end_effector name="fourth_ee" parent_link="fourth_tool0" group="fourth_gripper"/>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="first_fixed_base" type="fixed" parent_frame="world" child_link="first_base_link"/>
     <virtual_joint name="second_fixed_base" type="fixed" parent_frame="world" child_link="second_base_link"/>