Remove manual_grasp_link (#178)

PickNikRobotics · Jan 22, 2024 · 57e4fc7 · 57e4fc7
1 parent 26dfbd6
commit 57e4fc7
Show file tree

Hide file tree

Showing 26 changed files with 56 additions and 81 deletions.
diff --git a/src/external_dependencies/picknik_accessories b/src/external_dependencies/picknik_accessories
diff --git a/src/picknik_ur_base_config/config/config.yaml b/src/picknik_ur_base_config/config/config.yaml
@@ -166,10 +166,9 @@ moveit_params:
 # Additional configurable parameters for the MoveIt Studio user interface.
 # [Required]
 ui_params:
-  # By default, MoveIt Studio uses a frame called "manual_grasp_link" for tool grasp pose rendering
-  # and planning.
+  # By default, we use a frame named "grasp_link" for tool grasp pose rendering and planning.
   # [Required]
-  servo_endpoint_frame_id: "manual_grasp_link"
+  servo_endpoint_frame_id: "grasp_link"
 
 # Configuration for launching ros2_control processes.
 # [Required, if using ros2_control]

diff --git a/src/picknik_ur_base_config/config/moveit/picknik_ur.srdf b/src/picknik_ur_base_config/config/moveit/picknik_ur.srdf
@@ -10,7 +10,7 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="manipulator">
-        <chain base_link="base_link" tip_link="manual_grasp_link"/>
+        <chain base_link="base_link" tip_link="grasp_link"/>
     </group>
     <group name="gripper">
         <link name="robotiq_85_base_link"/>

diff --git a/src/picknik_ur_base_config/description/gripper_and_camera.xacro b/src/picknik_ur_base_config/description/gripper_and_camera.xacro
@@ -8,14 +8,13 @@
   <xacro:include filename="$(find picknik_accessories)/descriptions/sensors/realsense_d415.urdf.xacro"/>
 
   <!-- Create base link -->
-  <!-- Moveit Studio requires a link name called manual_grasp_link to perform built-in Objectives such as inspect surface without modification. -->
-  <link name="manual_grasp_link" />
-
+  <!-- We use a link name called grasp_link to perform built-in Objectives such as inspect surface without modification. -->
+  <link name="grasp_link"/>
   <link name="tool0" />
   <joint name="flange-tool0" type="fixed">
-    <parent link="manual_grasp_link" />
+    <parent link="grasp_link" />
     <child link="tool0" />
-    <origin xyz="0.0 -0.016124 -0.18430" rpy="${pi / 180.0 * 5} 0 -${pi}" />
+    <origin xyz="0 -0.016124 -0.18430" rpy="0 0 -${pi}" />
   </joint>
 
   <link name="tool_changer_tool0" />

diff --git a/src/picknik_ur_base_config/description/picknik_ur.xacro b/src/picknik_ur_base_config/description/picknik_ur.xacro
@@ -91,21 +91,14 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:robotiq_gripper>
 
+  <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
   <link name="grasp_link" />
   <joint name="grasp_link_joint" type="fixed">
     <parent link="gripper_mount_link" />
     <child link="grasp_link" />
     <origin xyz="0.0 0.0 0.134" rpy="0.0 0.0 ${pi}" />
   </joint>
 
-  <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
-  <link name="manual_grasp_link" />
-  <joint name="manual_grasp_joint" type="fixed">
-    <parent link="grasp_link" />
-    <child link="manual_grasp_link" />
-    <origin xyz="0 0 0" rpy="${pi / 180.0 * 5} 0 0" />
-  </joint>
-
   <!-- Environment -->
   <link name="environment">
     <visual>

diff --git a/src/picknik_ur_base_config/description/robotiq_2f_85_gripper.urdf b/src/picknik_ur_base_config/description/robotiq_2f_85_gripper.urdf
@@ -11,13 +11,13 @@
     <color rgba="0.1 0.1 0.1 1"/>
   </material>
   <!-- Create base link -->
-  <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
-  <link name="manual_grasp_link"/>
+  <!-- We use a link name called grasp_link to perform quick tasks such as inspect surface -->
+  <link name="grasp_link"/>
   <link name="tool0"/>
   <joint name="flange-tool0" type="fixed">
-    <parent link="manual_grasp_link"/>
+    <parent link="grasp_link"/>
     <child link="tool0"/>
-    <origin rpy="0.08726646259971647 0 -3.141592653589793" xyz="0.0 -0.016124 -0.18430"/>
+    <origin rpy="0 0 -3.141592653589793" xyz="0.0 -0.016124 -0.18430"/>
   </joint>
   <link name="tool_changer_tool0"/>
   <joint name="tool_side_joint" type="fixed">

diff --git a/src/picknik_ur_base_config/objectives/move_to_pose.xml b/src/picknik_ur_base_config/objectives/move_to_pose.xml
@@ -5,7 +5,7 @@
             <Action ID="RetrievePoseParameter" timeout_sec="" pose="{target_pose}"/>
             <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
             <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
-            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" use_all_planners="false"/>
+            <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" use_all_planners="false"/>
             <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}" />

diff --git a/src/picknik_ur_gazebo_config/config/moveit/picknik_ur_gazebo.srdf.xacro b/src/picknik_ur_gazebo_config/config/moveit/picknik_ur_gazebo.srdf.xacro
@@ -13,7 +13,7 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="manipulator">
-        <chain base_link="base_link" tip_link="manual_grasp_link"/>
+        <chain base_link="base_link" tip_link="grasp_link"/>
     </group>
     <group name="gripper">
         <link name="$(arg gripper_name)_base_link"/>

diff --git a/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro b/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro
@@ -70,14 +70,6 @@
     <origin xyz="0.0 0.0 0.134" rpy="0.0 0.0 ${pi}" />
   </joint>
 
-  <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
-  <link name="manual_grasp_link" />
-  <joint name="manual_grasp_joint" type="fixed">
-    <parent link="grasp_link" />
-    <child link="manual_grasp_link" />
-    <origin xyz="0 0 0" rpy="${pi / 180.0 * 5} 0 0" />
-  </joint>
-
   <!-- Environment -->
   <link name="environment">
     <!-- Table -->

diff --git a/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml b/src/picknik_ur_gazebo_config/objectives/close_cabinet_door.xml
@@ -10,17 +10,17 @@
                 <Action ID="InitializeMTCTask" task_id="close_cabinet_door" controller_names="/joint_trajectory_controller" task="{close_cabinet_door_task}"/>
                 <Action ID="SetupMTCCurrentState" task="{close_cabinet_door_task}"/>
                 <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
-                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
+                <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
                 <Action ID="WaitForUserTrajectoryApproval" solution="{full_close_cabinet_door_solution}"/>
                 <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}"/>
                 <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}" />
                 <Control ID="Parallel" success_count="1" failure_count="1">
-                    <Action ID="ForceExceedsThreshold" wrench_topic_name="/force_torque_sensor_broadcaster/wrench" hand_frame_name="manual_grasp_link" wrench_frame_name="tool0" minimum_consecutive_wrench_values="10" force_threshold="10.0"/>
+                    <Action ID="ForceExceedsThreshold" wrench_topic_name="/force_torque_sensor_broadcaster/wrench" hand_frame_name="grasp_link" wrench_frame_name="tool0" minimum_consecutive_wrench_values="10" force_threshold="10.0"/>
                     <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
                 </Control>
             </Control>

diff --git a/src/picknik_ur_gazebo_config/objectives/looping_pick_and_place_object_config.yaml b/src/picknik_ur_gazebo_config/objectives/looping_pick_and_place_object_config.yaml
@@ -27,7 +27,7 @@ SetupMTCApproachGrasp:
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
   end_effector_name: "moveit_ee"
-  hand_frame_name: "manual_grasp_link"
+  hand_frame_name: "grasp_link"
 
   approach_distance: 0.15
 
@@ -41,9 +41,9 @@ SetupMTCApproachGrasp:
 SetupMTCGenerateCuboidGrasps:
   end_effector_group_name: "gripper"
   end_effector_name: "moveit_ee"
-  hand_frame_name: "manual_grasp_link"
+  hand_frame_name: "grasp_link"
   end_effector_closed_pose_name: "close"
-  ui_grasp_link: "manual_grasp_link"
+  ui_grasp_link: "grasp_link"
 
   # Grasp candidate configs
   grasp_candidate_config:
@@ -85,7 +85,7 @@ SetupMTCRetractFromGrasp:
   world_frame_name: "world"
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
-  hand_frame_name: "manual_grasp_link"
+  hand_frame_name: "grasp_link"
   end_effector_closed_pose_name: "close"
 
   # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.

diff --git a/src/picknik_ur_gazebo_config/objectives/pick_place_object_config.yaml b/src/picknik_ur_gazebo_config/objectives/pick_place_object_config.yaml
@@ -27,7 +27,7 @@ SetupMTCApproachGrasp:
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
   end_effector_name: "moveit_ee"
-  hand_frame_name: "manual_grasp_link"
+  hand_frame_name: "grasp_link"
 
   approach_distance: 0.15
 
@@ -41,9 +41,9 @@ SetupMTCApproachGrasp:
 SetupMTCGenerateCuboidGrasps:
   end_effector_group_name: "gripper"
   end_effector_name: "moveit_ee"
-  hand_frame_name: "manual_grasp_link"
+  hand_frame_name: "grasp_link"
   end_effector_closed_pose_name: "close"
-  ui_grasp_link: "manual_grasp_link"
+  ui_grasp_link: "grasp_link"
 
   # Grasp candidate configs
   grasp_candidate_config:
@@ -85,7 +85,7 @@ SetupMTCRetractFromGrasp:
   world_frame_name: "world"
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
-  hand_frame_name: "manual_grasp_link"
+  hand_frame_name: "grasp_link"
   end_effector_closed_pose_name: "close"
 
   # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.

diff --git a/src/picknik_ur_gazebo_config/objectives/push_button.xml b/src/picknik_ur_gazebo_config/objectives/push_button.xml
@@ -10,17 +10,17 @@
                 <Action ID="InitializeMTCTask" task_id="push_button" controller_names="/joint_trajectory_controller" task="{push_button_task}"/>
                 <Action ID="SetupMTCCurrentState" task="{push_button_task}"/>
                 <Action ID="GetPoseFromUser" parameter_name="push_button.target_pose" parameter_value="{target_pose}" />
-                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}" use_all_planners="false"/>
+                <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
                 <Action ID="PlanMTCTask" solution="{full_push_button_solution}" task="{push_button_task}"/>
                 <Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
                 <Action ID="WaitForUserTrajectoryApproval" solution="{full_push_button_solution}"/>
                 <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}"/>
                 <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}" />
                 <Control ID="Parallel" success_count="1" failure_count="1">
-                    <Action ID="ForceExceedsThreshold" wrench_topic_name="/force_torque_sensor_broadcaster/wrench" hand_frame_name="manual_grasp_link" wrench_frame_name="tool0" minimum_consecutive_wrench_values="10" force_threshold="10.0"/>
+                    <Action ID="ForceExceedsThreshold" wrench_topic_name="/force_torque_sensor_broadcaster/wrench" hand_frame_name="grasp_link" wrench_frame_name="tool0" minimum_consecutive_wrench_values="10" force_threshold="10.0"/>
                     <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
                 </Control>
             </Control>

diff --git a/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml b/src/picknik_ur_gazebo_config/objectives/push_button_ml.xml
@@ -37,13 +37,13 @@
                 <Action ID="InitializeMTCTask" task_id="push_button_ml" controller_names="/joint_trajectory_controller"
                     task="{push_along_axis_task}" />
                 <Action ID="SetupMTCCurrentState" task="{push_along_axis_task}" />
-                <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link"
+                <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link"
                     planning_group_name="manipulator" target_pose="{button_pose}"
                     task="{push_along_axis_task}" use_all_planners="false"/>
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="AllowGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
-                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="manual_grasp_link" axis_frame="manual_grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
+                <Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
                 <Action ID="SetupMTCUpdateGroupCollisionRule"
                     name="ForbidGripperCollisionWithOctomap" parameters="{parameters}"
                     task="{push_along_axis_task}" />
@@ -57,7 +57,7 @@
                 <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}" />
                 <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}" />
                 <Control ID="Parallel" success_count="1" failure_count="1">
-                    <Action ID="ForceExceedsThreshold" wrench_topic_name="/force_torque_sensor_broadcaster/wrench" hand_frame_name="manual_grasp_link" wrench_frame_name="tool0" minimum_consecutive_wrench_values="10" force_threshold="10.0"/>
+                    <Action ID="ForceExceedsThreshold" wrench_topic_name="/force_torque_sensor_broadcaster/wrench" hand_frame_name="grasp_link" wrench_frame_name="tool0" minimum_consecutive_wrench_values="10" force_threshold="10.0"/>
                     <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
                 </Control>
             </Control>

diff --git a/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml b/src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml
@@ -26,7 +26,7 @@
     <Control ID="Sequence">
       <Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
       <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
-      <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" use_all_planners="false" task="{move_to_pose_task}"/>
+      <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" use_all_planners="false" task="{move_to_pose_task}"/>
       <Action ID="PlanMTCTask" task="{move_to_pose_task}" solution="{move_to_pose_solution}"/>
       <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}"/>
     </Control>

diff --git a/src/picknik_ur_mock_hw_config/config/moveit/picknik_ur_machine_tending.srdf b/src/picknik_ur_mock_hw_config/config/moveit/picknik_ur_machine_tending.srdf
@@ -10,7 +10,7 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="manipulator">
-        <chain base_link="base_link" tip_link="manual_grasp_link"/>
+        <chain base_link="base_link" tip_link="grasp_link"/>
     </group>
     <group name="gripper">
         <link name="robotiq_85_base_link"/>

diff --git a/src/picknik_ur_mock_hw_config/description/picknik_ur_machine_tending.xacro b/src/picknik_ur_mock_hw_config/description/picknik_ur_machine_tending.xacro
@@ -87,14 +87,6 @@
     <origin xyz="0.0 0.0 0.134" rpy="0.0 0.0 ${pi}" />
   </joint>
 
-  <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
-  <link name="manual_grasp_link" />
-  <joint name="manual_grasp_joint" type="fixed">
-    <parent link="grasp_link" />
-    <child link="manual_grasp_link" />
-    <origin xyz="0 0 0" rpy="${pi / 180.0 * 5} 0 0" />
-  </joint>
-
   <!-- CNC machine -->
   <link name="cnc_machine">
     <visual>

diff --git a/src/picknik_ur_mock_hw_config/objectives/my_constraints.yaml b/src/picknik_ur_mock_hw_config/objectives/my_constraints.yaml
@@ -1,6 +1,6 @@
 AppendOrientationConstraint:
   constraint_frame: "world"
-  link_frame: "manual_grasp_link"
+  link_frame: "grasp_link"
   orientation:  # radians
     x: -3.14
     y: 0.0