Add visual servo teach / move Objectives (#197)

* Add visual servo teach / move Objectives * add exit conditions * add end of line * fix end of line * remove unnecessary Objectives * average poses in fixed frame * collapse subtree * Update src/picknik_ur_gazebo_config/objectives/visual_servo_to_reference.xml Co-authored-by: Paul Gesel <[email protected]> --------- Co-authored-by: Paul Gesel <[email protected]>
PickNikRobotics · Feb 5, 2024 · 099f0b9 · 099f0b9
1 parent eb10242
commit 099f0b9
Show file tree

Hide file tree

Showing 3 changed files with 53 additions and 0 deletions.
diff --git a/src/picknik_ur_gazebo_config/objectives/visual_servo_reference.yaml b/src/picknik_ur_gazebo_config/objectives/visual_servo_reference.yaml
@@ -0,0 +1,10 @@
+ServoReference:
+  position:
+    x: 0.045306888032763615
+    y: 0.053867148569981871
+    z: 0.1720067433901151
+  orientation:
+    x: -0.69868796363054175
+    y: 0.71517864722160651
+    z: 0.00054314097295501279
+    w: -0.018823842167928755
diff --git a/src/picknik_ur_gazebo_config/objectives/visual_servo_teach_reference.xml b/src/picknik_ur_gazebo_config/objectives/visual_servo_teach_reference.xml
@@ -0,0 +1,13 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Visual Servo Teach Reference">
+  <BehaviorTree ID="Visual Servo Teach Reference" _description="Teach the pose reference used in `Visual Servo To Reference'" _favorite="true">
+    <Control ID="Sequence" name="root">
+      <SubTree ID="Sample April Tag" _collapsed="true" num_samples="5" tag_id="2" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="CreateStampedPose" reference_frame="grasp_link" position_xyz="0.0;0.0;0.0" orientation_xyzw="0.0;0.0;0.0;1.0" stamped_pose="{grasp_link_pose}"/>
+      <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
+      <Action ID="TransformPoseFrame" input_pose="{grasp_link_pose}" target_frame_id="world" output_pose="{grasp_link_pose_world}"/>
+      <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{grasp_link_pose_world}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+      <Action ID="SavePoseToYaml" yaml_filename="visual_servo_reference" namespace="ServoReference" message="{tag_to_user_grasp_pose}"/>
+    </Control>
+  </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_gazebo_config/objectives/visual_servo_to_reference.xml b/src/picknik_ur_gazebo_config/objectives/visual_servo_to_reference.xml
@@ -0,0 +1,30 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Visual Servo To Reference">
+  <!-- ////////// -->
+  <BehaviorTree ID="Visual Servo To Reference" _description="Move in closed loop to a pose relative to an AprilTag" _favorite="true" _hardcoded="false">
+    <Control ID="Sequence">
+      <Action ID="LoadObjectiveParameters" config_file_name="visual_servo_reference.yaml" parameters="{servo_reference_parameters}"/>
+      <Action ID="ActivateControllers" controller_names="servo_controller"/>
+      <Control ID="Sequence">
+        <SubTree ID="Sample April Tag" _collapsed="true" num_samples="1" tag_id="2" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+        <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="base_link" output_pose="{tag_pose_base}"/>
+        <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_base}" parameter_namespace="ServoReference" pose_parameters="{servo_reference_parameters}" output_pose="{reference_pose}"/>
+      </Control>
+      <Control ID="Parallel" success_count="1" failure_count="1">
+        <Decorator ID="KeepRunningUntilFailure">
+          <Control ID="Sequence">
+            <SubTree ID="Sample April Tag" _collapsed="true" num_samples="1" tag_id="2" apriltag_config="51mm_apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+            <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="base_link" output_pose="{tag_pose_base}"/>
+          </Control>
+        </Decorator>
+        <Action ID="AveragePoseStamped" run_continuously="true" num_samples="5" max_distance="0.04" max_rotation="0.4" pose_sample="{tag_pose_base}" avg_pose="{tag_pose_avg}"/>
+        <Decorator ID="KeepRunningUntilFailure">
+          <Control ID="Sequence">
+            <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_avg}" parameter_namespace="ServoReference" pose_parameters="{servo_reference_parameters}" output_pose="{reference_pose}"/>
+          </Control>
+        </Decorator>
+        <Action ID="ServoTowardsPose" planning_group_name="manipulator" target_pose="{reference_pose}" translational_gain="1.0" rotational_gain="1.0" max_translational_vel="0.1" max_rotational_vel="0.1" publish_rate="20" exit_threshold_translation="0.01" exit_threshold_rotation="0.01" exit_threshold_time="0.1"/>
+      </Control>
+    </Control>
+  </BehaviorTree>
+</root>