This repository has been archived by the owner on Dec 13, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 6
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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]>
- Loading branch information
1 parent
eb10242
commit 099f0b9
Showing
3 changed files
with
53 additions
and
0 deletions.
There are no files selected for viewing
10 changes: 10 additions & 0 deletions
10
src/picknik_ur_gazebo_config/objectives/visual_servo_reference.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
13 changes: 13 additions & 0 deletions
13
src/picknik_ur_gazebo_config/objectives/visual_servo_teach_reference.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> |
30 changes: 30 additions & 0 deletions
30
src/picknik_ur_gazebo_config/objectives/visual_servo_to_reference.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> |