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 Admittance Control to Open Lever Handle Door (#22)
* Moved admittance controller configs from base package to site package * Renamed open_door_affordance to open_cabinet_door_affordance * Added ForceSuccess to more open_door objectives so they will work in sim
- Loading branch information
1 parent
c2bcffc
commit baef76e
Showing
9 changed files
with
170 additions
and
6 deletions.
There are no files selected for viewing
File renamed without changes.
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
File renamed without changes.
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
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
52 changes: 52 additions & 0 deletions
52
...icknik_ur_site_config/objectives/open_lever_handle_door_admittance_controller_config.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,52 @@ | ||
| UpdateAdmittanceController: | ||
| controller_name: "admittance_controller_open_door" | ||
| end_effector_frame: "grasp_link" | ||
| control_frame: "grasp_link" | ||
| admittance: | ||
| selected_axes: | ||
| x: True | ||
| y: True | ||
| z: True | ||
| rx: True | ||
| ry: True | ||
| rz: True | ||
| mass_position: # kg | ||
| x: 10.0 | ||
| y: 10.0 | ||
| z: 10.0 | ||
| min: 0.001 | ||
| max: 100.0 | ||
| mass_rotation: # kg*m^2 | ||
| x: 5.0 | ||
| y: 5.0 | ||
| z: 5.0 | ||
| min: 0.001 | ||
| max: 100.0 | ||
| stiffness_position: # N/m | ||
| x: 500.0 | ||
| y: 500.0 | ||
| z: 500.0 | ||
| min: 0.0 | ||
| max: 1000.0 | ||
| stiffness_rotation: # N*m/rad | ||
| x: 100.0 | ||
| y: 100.0 | ||
| z: 100.0 | ||
| min: 0.0 | ||
| max: 1000.0 | ||
| damping_ratio_position: # unitless | ||
| x: 5.0 | ||
| y: 5.0 | ||
| z: 5.0 | ||
| min: 0.0 | ||
| max: 100.0 | ||
| damping_ratio_rotation: # unitless | ||
| x: 5.0 | ||
| y: 5.0 | ||
| z: 5.0 | ||
| min: 0.0 | ||
| max: 100.0 | ||
| joint_damping: # 1/s | ||
| value: 5.0 | ||
| min: 0.0 | ||
| max: 100.0 |
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
72 changes: 72 additions & 0 deletions
72
src/picknik_ur_site_config/objectives/open_lever_handle_door_ml.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,72 @@ | ||
| <?xml version="1.0"?> | ||
| <root BTCPP_format="4" main_tree_to_execute="Open Lever Handle Door ML"> | ||
| <!-- ////////// --> | ||
| <BehaviorTree ID="Open Lever Handle Door ML" _description="Move to Door waypoint, and then open a lever handle door detected by a network"> | ||
| <Control ID="Sequence" name="Setup"> | ||
| <Action ID="LoadObjectiveParameters" config_file_name="open_lever_handle_door_ml_config.yaml" parameters="{parameters}"/> | ||
| <!-- 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_lever_handle_door_admittance_controller_config.yaml" /> | ||
| </Decorator> | ||
| <!-- Wrap in a ForceSuccess as Sim doesn't have the zero_ftsensor service --> | ||
| <!-- See https://github.com/PickNikRobotics/moveit_studio/issues/3406 --> | ||
| <Decorator ID="ForceSuccess"> | ||
| <Action ID="CallTriggerService" | ||
| service_name="/io_and_status_controller/zero_ftsensor" /> | ||
| </Decorator> | ||
| <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" /> | ||
| <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" | ||
| point_cloud_service="/point_cloud_service" /> | ||
| <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" | ||
| sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" | ||
| point_cloud_uuid="" /> | ||
| <Action ID="MoveToJointState" | ||
| waypoint_name="Door" | ||
| planning_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| use_all_planners="true" /> | ||
| <Action ID="DetectObjects" | ||
| rgb_image_topic="/wrist_mounted_camera/color/image_raw" | ||
| depth_image_topic="/wrist_mounted_camera/depth/image_rect_raw" | ||
| camera_info_topic="/wrist_mounted_camera/color/camera_info" | ||
| detect_objects_action_name="/detect_objects" | ||
| object_detections_goal="{object_detections_goal}" | ||
| object_detections_result="{object_detections_result}" | ||
| min_probability="0.1"/> | ||
| <Action ID="GetLeverHandleFromDetections" | ||
| object_detections_goal="{object_detections_goal}" | ||
| object_detections_result="{object_detections_result}" | ||
| fit_object_3d_service_name="fit_object_3d" | ||
| handle_pose="{handle_pose}" | ||
| length="{length}" | ||
| height="{height}"/> | ||
| <SubTree ID="CloseGripper"/> | ||
| <Control ID="Sequence" name="OpenLeverHandleDoor"> | ||
| <Action ID="InitializeMTCTask" task_id="open_lever_handle_door_ml" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{open_lever_handle_door_task}"/> | ||
| <Action ID="SetupMTCCurrentState" task="{open_lever_handle_door_task}"/> | ||
| <Action ID="SetupMTCOpenLeverHandleDoor" | ||
| handle_pose="{handle_pose}" | ||
| handle_length="{length}" | ||
| handle_z_offset="{height}" | ||
| parameters="{parameters}" | ||
| task="{open_lever_handle_door_task}"/> | ||
| <Action ID="PlanMTCTask" solution="{open_lever_handle_door_solution}" task="{open_lever_handle_door_task}"/> | ||
| <Fallback> | ||
| <Inverter> | ||
| <Action ID="IsUserAvailable"/> | ||
| </Inverter> | ||
| <Action ID="WaitForUserTrajectoryApproval" solution="{open_lever_handle_door_solution}"/> | ||
| </Fallback> | ||
| <Action ID="ExecuteMTCTask" solution="{open_lever_handle_door_solution}"/> | ||
| </Control> | ||
| </Control> | ||
| </BehaviorTree> | ||
| <!-- ////////// --> | ||
| <BehaviorTree ID="CloseGripper"> | ||
| <Control ID="Sequence" name="CloseGripper"> | ||
| <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/> | ||
| </Control> | ||
| </BehaviorTree> | ||
| </root> |
22 changes: 22 additions & 0 deletions
22
src/picknik_ur_site_config/objectives/open_lever_handle_door_ml_config.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,22 @@ | ||
| SetupMTCOpenLeverHandleDoor: | ||
| arm_group_name: "manipulator" | ||
| end_effector_group_name: "gripper" | ||
| 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]. | ||
| handle_grasp_offset_percent: 0.75 | ||
| # The distance above the handle the robot should approach with before | ||
| # manipulating it. | ||
| above_handle_approach_distance: 0.03 # meters | ||
| # The distance the hand should move past the end of the handle to release it. | ||
| release_handle_distance: 0.03 # meters | ||
| # How many degrees to rotate the lever handle (use positive value for | ||
| # counter-clockwise rotation and negative value for clockwise rotation). | ||
| handle_twist_angle: 65.0 # degrees | ||
| approach_distance: 0.15 # meters | ||
| push_open_distance: 0.05 # meters | ||
| use_circular_arc: true |