make imarker work in multi_arm_sim, for just the first arm

Author: marioprats

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

@@ -28,6 +28,7 @@
         <group name="fourth_manipulator"/>
     </group>
     <group name="gripper">
+        <link name="first_grasp_link"/>
         <joint name="first_robotiq_85_left_finger_tip_joint"/>
         <joint name="first_robotiq_85_left_inner_knuckle_joint"/>
         <joint name="first_robotiq_85_left_knuckle_joint"/>
@@ -36,6 +37,7 @@
         <joint name="first_robotiq_85_right_knuckle_joint"/>
     </group>
     <group name="second_gripper">
+        <link name="second_grasp_link"/>
         <joint name="second_robotiq_85_left_finger_tip_joint"/>
         <joint name="second_robotiq_85_left_inner_knuckle_joint"/>
         <joint name="second_robotiq_85_left_knuckle_joint"/>
@@ -44,6 +46,7 @@
         <joint name="second_robotiq_85_right_knuckle_joint"/>
     </group>
     <group name="third_gripper">
+        <link name="third_grasp_link"/>
         <joint name="third_robotiq_85_left_finger_tip_joint"/>
         <joint name="third_robotiq_85_left_inner_knuckle_joint"/>
         <joint name="third_robotiq_85_left_knuckle_joint"/>
@@ -52,6 +55,7 @@
         <joint name="third_robotiq_85_right_knuckle_joint"/>
     </group>
     <group name="fourth_gripper">
+        <link name="fourth_grasp_link"/>
         <joint name="fourth_robotiq_85_left_finger_tip_joint"/>
         <joint name="fourth_robotiq_85_left_inner_knuckle_joint"/>
         <joint name="fourth_robotiq_85_left_knuckle_joint"/>

src/moveit_pro_ur_configs/multi_arm_sim/objectives/move_to_pose.xml

@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Move to Pose">
+  <BehaviorTree
+    ID="Move to Pose"
+    _description="Plan and execute motion to a Cartesian coordinate pose in a ROS message type, using inverse kinematics."
+  >
+    <Control ID="Sequence">
+      <Action
+        ID="InitializeMTCTask"
+        task_id="move_to_pose"
+        controller_names="{controller_names}"
+        task="{move_to_pose_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
+      <Action
+        ID="SetupMTCPlanToPose"
+        ik_frame="first_grasp_link"
+        planning_group_name="first_manipulator"
+        target_pose="{target_pose}"
+        task="{move_to_pose_task}"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{move_to_pose_solution}"
+        task="{move_to_pose_task}"
+      />
+      <SubTree
+        ID="Wait for Trajectory Approval if User Available"
+        solution="{move_to_pose_solution}"
+      />
+      <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Pose">
+      <input_port name="target_pose" default="{target_pose}" />
+      <input_port
+        name="controller_names"
+        default="/joint_trajectory_controller"
+      />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+        <Metadata runnable="false" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>