apply averaging in common frame

PickNikRobotics · Feb 5, 2024 · cdaac90 · cdaac90
1 parent d4520fa
commit cdaac90
Showing 1 changed file with 12 additions and 10 deletions.
diff --git a/src/picknik_ur_site_config/objectives/visual_servo_to_reference.xml b/src/picknik_ur_site_config/objectives/visual_servo_to_reference.xml
@@ -4,24 +4,26 @@
   <BehaviorTree ID="Visual Servo To Reference" _description="Move in closed loop to a pose relative to an AR Tag" _favorite="true" _hardcoded="false">
     <Control ID="Sequence">
       <Action ID="LoadObjectiveParameters" config_file_name="visual_servo_reference.yaml" parameters="{servo_reference_parameters}"/>
-      <Action ID="LoadObjectiveParameters" config_file_name="51mm_apriltag_detection_config.yaml" parameters="{tag_parameters}"/>
-      <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
       <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="TransformPoseFromYaml" input_pose="{tag_pose}" parameter_namespace="ServoReference" pose_parameters="{servo_reference_parameters}" output_pose="{reference_pose}"/>
-        <Action ID="TransformPoseFrame" input_pose="{reference_pose}" target_frame_id="base_link" output_pose="{target_pose}"/>
+        <SubTree ID="Sample April Tag" _collapsed="true" num_samples="1" tag_id="5" 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="TransformPoseFromYaml" input_pose="{tag_pose}" parameter_namespace="ServoReference" pose_parameters="{servo_reference_parameters}" output_pose="{reference_pose}"/>
-            <Action ID="TransformPoseFrame" input_pose="{reference_pose}" target_frame_id="base_link" output_pose="{target_pose}"/>
+            <SubTree ID="Sample April Tag" _collapsed="true" num_samples="1" tag_id="5" 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="10" max_distance="0.08" max_rotation="0.4" pose_sample="{target_pose}" avg_pose="{target_pose_avg}"/>
-        <Action ID="ServoTowardsPose" planning_group_name="manipulator" target_pose="{target_pose_avg}" 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"/>
+        <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>