Merge pull request #20 from PickNikRobotics/update-objectives-remove-…

…manual-grasp-link

Update objectives and remove manual_grasp_link

src/kinova_gen3_base_config/config/config.yaml

@@ -131,6 +131,13 @@ moveit_params:
     allowed_goal_duration_margin: 5.0
     allowed_start_tolerance: 0.01
 
+# Additional configurable parameters for the MoveIt Studio user interface.
+# [Required]
+ui_params:
+  # By default, we use a frame named "grasp_link" for tool grasp pose rendering and planning.
+  # [Required]
+  servo_endpoint_frame_id: "grasp_link"
+
 # This configures what controllers gets run at startup
 ros2_control:
   config:

src/kinova_gen3_base_config/config/moveit/picknik_kinova_gen3_base.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">
         <joint name="robotiq_85_base_joint"/>

src/kinova_gen3_base_config/description/gripper_and_camera.xacro

@@ -11,15 +11,15 @@
   <xacro:include filename="$(find picknik_accessories)/descriptions/sensors/realsense_d415.urdf.xacro"/>
 
   <!-- Create parent 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 built-in Objectives such as inspect surface without modification. -->
+  <link name="grasp_link" />
 
   <!-- Create bracelet link -->
   <link name="bracelet_link" />
 
   <joint name="bracelet_joint" type="fixed">
-    <origin xyz="0 -0.031521 -0.1787672" rpy="${pi / 180.0 * 170} 0 0" />
-    <parent link="manual_grasp_link" />
+    <origin xyz="0 -0.031521 -0.1787672" rpy="0 0 0" />
+    <parent link="grasp_link" />
     <child link="bracelet_link" />
   </joint>
 

src/kinova_gen3_base_config/description/picknik_kinova_gen3.xacro

@@ -133,14 +133,13 @@
       <origin xyz="0 0 0.16" rpy="0 0 0"/>
     </joint>
 
-    <!-- Moveit Studio requires a link name called manual_grasp_link to perform quick tasks such as inspect surface -->
-    <!-- we add this link here and make it coincident with the manual_grasp_link but rotate it to put the camera normal to the surface -->
-    <link name="manual_grasp_link"/>
+    <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
+    <link name="grasp_link"/>
 
-    <joint name="manual_grasp_joint" type="fixed">
+    <joint name="grasp_link_joint" type="fixed">
       <parent link="$(arg prefix)end_effector_link"/>
-      <child link = "manual_grasp_link"/>
-      <origin xyz="0 0 0.15" rpy="${3.14 / 180.0 * 10} 0 0"/>
+      <child link = "grasp_link"/>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
     </joint>
 
     <!-- Camera External -->

src/kinova_gen3_base_config/objectives/3_waypoint_pick_and_place.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
+    <!-- ////////// -->
+    <BehaviorTree ID="3 Waypoints Pick and Place" _description="Repeatedly grab a small object, place it at desired destination, and then navigate to specified waypoint until failure" _favorite="true">
+        <Control ID="Sequence">
+            <!-- Clear snapshot, move to center pose, and open gripper -->
+            <Action ID="ClearSnapshot" />
+            <Action ID="MoveToWaypoint" waypoint_name="Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
+            <!-- Keep executing the pick and place sequence until failure -->
+            <Decorator ID="KeepRunningUntilFailure">
+                <Control ID="Sequence">
+                    <!-- Pick object from "Pick", put it down at "Place", and go back to center pose -->
+                    <Control ID="Sequence">
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                    </Control>
+                    <!-- Pick object from "Place", put it down at "Pick", and go back to center pose -->
+                    <Control ID="Sequence">
+                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                    </Control>
+                </Control>
+            </Decorator>
+        </Control>
+    </BehaviorTree>
+</root>

src/kinova_gen3_base_config/objectives/close_gripper.xml

@@ -2,7 +2,7 @@
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
     <BehaviorTree ID="Close Gripper" _description="Close the gripper">
         <Control ID="Sequence" name="root">
-            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.8"/>
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
         </Control>
     </BehaviorTree>
 </root>

src/kinova_gen3_base_config/objectives/inspect_surface.xml

@@ -1,18 +1,13 @@
 <?xml version="1.0"?>
-<root BTCPP_format="4" main_tree_to_execute="Inspect Surface">
-    <BehaviorTree ID="Inspect Surface" _description="Move the wrist camera to point towards a selected surface">
+<root BTCPP_format="4" main_tree_to_execute="Inspect Surface" _favorite="true">
+    <BehaviorTree ID="Inspect Surface" _description="Move the wrist camera to point towards a selected surface" _favorite="true">
         <Control ID="Sequence">
             <Action ID="InitializeMTCTask" task_id="inspect_surface" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
             <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
             <Action ID="GetPoseFromUser" parameter_name="inspect_surface.target_pose" parameter_value="{target_pose}" />
-            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}"/>
+            <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}"/>
-            <Fallback name="wait_for_approval_if_user_available">
-                <Inverter>
-                    <Action ID="IsUserAvailable" />
-                </Inverter>
-                <Action ID="WaitForUserTrajectoryApproval" solution="{move_to_pose_solution}" />
-            </Fallback>
+            <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
             <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
         </Control>
     </BehaviorTree>

src/kinova_gen3_base_config/objectives/move_to_named_pose.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Move to Named Pose">
+    <BehaviorTree ID="Move to Named Pose" _description="This Objective is used when moving to one of the saved waypoints in your site configuration">
+        <Control ID="Sequence" name="root">
+            <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="Home"/>
+            <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
+            <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
+            <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="manipulator" task="{move_to_waypoint_task}"/>
+            <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
+            <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
+        </Control>
+    </BehaviorTree>
+</root>

src/kinova_gen3_base_config/objectives/move_to_pose.xml

@@ -2,17 +2,12 @@
 <root BTCPP_format="4" main_tree_to_execute="Move to Pose">
     <BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
         <Control ID="Sequence">
-            <Action ID="RetrievePoseParameter" timeout_sec="" pose="{target_pose}" />
+            <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}"/>
+            <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}"/>
-            <Fallback name="wait_for_approval_if_user_available">
-                <Inverter>
-                    <Action ID="IsUserAvailable" />
-                </Inverter>
-                <Action ID="WaitForUserTrajectoryApproval" solution="{move_to_pose_solution}" />
-            </Fallback>
+            <SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
             <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
         </Control>
     </BehaviorTree>

src/kinova_gen3_base_config/objectives/open_cabinet_door_affordance.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Open Cabinet Door">
+    <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle">
+        <Control ID="Sequence" name="Setup">
+            <SubTree ID="Re-Zero Force-Torque Sensors" />
+            <!-- Set admittance control parameters at the start of the Objective -->
+            <!-- Wrap in a ForceSuccess as Sim doesn't have the admittance controller loaded -->
+            <!-- See https://github.com/PickNikRobotics/moveit_studio/issues/3406 -->
+            <Decorator ID="ForceSuccess">
+                <Action ID="UpdateAdmittanceController" config_file_name="open_cabinet_door_admittance_controller_config.yaml"/>
+            </Decorator>
+            <!-- Execute the Objective -->
+            <Action ID="LoadObjectiveParameters" config_file_name="open_cabinet_door_affordance_config.yaml" parameters="{parameters}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_end" parameter_value="{hinge_axis_pose_end}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_start" parameter_value="{hinge_axis_pose_start}" />
+            <Action ID="GetPoseFromUser" parameter_name="process_door_selection.grasp_pose" parameter_value="{grasp_pose}" />
+            <Action ID="GetMoveAlongArcSubframes" grasp_pose="{grasp_pose}" hinge_axis_pose_end="{hinge_axis_pose_end}" hinge_axis_pose_start="{hinge_axis_pose_start}" screw_axis_pose="{screw_axis_pose}" screw_origin_pose="{screw_origin_pose}" target_grasp_pose="{grasp_pose}" />
+            <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0" />
+            <Control ID="Sequence" name="OpenDoorAffordanceMain">
+                <Action ID="InitializeMTCTask" task_id="open_door_affordance" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_door_affordance_task}" />
+                <Action ID="SetupMTCCurrentState" task="{open_door_affordance_task}" />
+                <Action ID="SetupMTCGraspThenMoveAlongArcPull" grasp_pose="{grasp_pose}" name="SetupMTCOpenDoorAffordance" parameters="{parameters}" screw_axis_pose="{screw_axis_pose}" screw_origin_pose="{screw_origin_pose}" task="{open_door_affordance_task}" />
+                <Action ID="PlanMTCTask" solution="{open_door_affordance_solution}" task="{open_door_affordance_task}" />
+            	<SubTree ID="Wait for Trajectory Approval if User Available" solution="{open_door_affordance_solution}"/>
+                <!-- Whether task execution succeeds or fails, reset the controllers to disable admittance control. -->
+                <Control ID="IfThenElse" name="execute_and_reset_controllers">
+                    <!-- IF task execution succeeds -->
+                    <Action ID="ExecuteMTCTask" solution="{open_door_affordance_solution}" />
+                    <!-- THEN reset the controllers -->
+                    <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
+                    <!-- ELSE reset the controllers and return failure -->
+                    <Control ID="Sequence">
+                        <Action ID="ActivateControllers" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" />
+                        <Action ID="AlwaysFailure" />
+                    </Control>
+                </Control>
+            </Control>
+        </Control>
+    </BehaviorTree>
+</root>

src/kinova_gen3_base_config/objectives/open_cabinet_door_affordance_config.yaml

@@ -0,0 +1,16 @@
+SetupMTCOpenDoorAffordance:
+  arm_group_name: "manipulator"
+  end_effector_group_name: "gripper"
+  end_effector_name: "moveit_ee"
+  ik_frame_name: "grasp_link"
+  end_effector_closed_pose_name: "close"
+  end_effector_open_pose_name: "open"
+
+  # List of controllers to use during the stages that interact with the door.
+  # For this objective, we want to enable admittance control.
+  door_opening_controllers: "/joint_trajectory_controller_chained_open_door /admittance_controller_open_door /robotiq_gripper_controller"
+
+  approach_distance: 0.05
+  translation_distance: 0.0
+  rotation_distance: 60.0
+  use_circular_arc: true

src/kinova_gen3_base_config/objectives/open_gripper.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<root BTCPP_format="4" main_tree_to_execute="Open Gripper" _description="Open the gripper">
+<root BTCPP_format="4" main_tree_to_execute="Open Gripper">
     <!-- ////////// -->
-    <BehaviorTree ID="Open Gripper">
+    <BehaviorTree ID="Open Gripper" _description="Open the gripper">
         <Control ID="Sequence" name="root">
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
         </Control>

src/kinova_gen3_base_config/objectives/open_lever_handle_door_config.yaml

@@ -4,10 +4,13 @@ GetDoorHandle:
   # e.g. "world", is typically the right choice.
   door_handle_pose_frame_name: world
 
-SetupMTCOpenLeverHandleDoor:
+SetupMTCGraspAndTwistThenMoveAlongArcPush:
   arm_group_name: "manipulator"
   end_effector_group_name: "gripper"
-  ik_frame_name: "manual_grasp_link"
+  ik_frame_name: "grasp_link"
+
+  # List of controllers to use during the stages that interact with the door.
+  door_opening_controllers: "/joint_trajectory_controller_chained_open_door /admittance_controller_open_door /robotiq_gripper_controller"
 
   # The hand should be placed 75% of the way down the handle length when
   # manipulating the handle. Valid value range [0, 1.0].