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 Objective to ur_site_configs
- Loading branch information
1 parent
eb10242
commit d4520fa
Showing
3 changed files
with
51 additions
and
0 deletions.
There are no files selected for viewing
10 changes: 10 additions & 0 deletions
10
src/picknik_ur_site_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_site_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> |
28 changes: 28 additions & 0 deletions
28
src/picknik_ur_site_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,28 @@ | ||
| <?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 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}"/> | ||
| </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}"/> | ||
| </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"/> | ||
| </Control> | ||
| </Control> | ||
| </BehaviorTree> | ||
| </root> |