Skip to content
This repository has been archived by the owner on Dec 13, 2024. It is now read-only.

Add Admittance Control to Open Lever Handle Door #22

Merged
merged 10 commits into from
Aug 1, 2023
Prev Previous commit
Next Next commit
Renamed open_door_affordance to open_cabinet_door_affordance and adde…
…d ForceSuccess to more instances so they will work in sim
  • Loading branch information
Chance Cardona committed Aug 1, 2023
commit 917d6740173b5deb790e1b56cdb76ca1c95efa60
Original file line number Diff line number Diff line change
@@ -3,13 +3,17 @@
<BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle">
<Control ID="Sequence" name="Setup">
<!-- Tare the force torque sensor -->
<!-- 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>
<!-- Set admittance control parameters at the start of the Objective -->
<Action ID="UpdateAdmittanceController" config_file_name="open_cabinet_door_admittance_controller_config.yaml"/>
<Decorator ID="ForceSuccess">
chancecardona marked this conversation as resolved.
Show resolved Hide resolved
<Action ID="UpdateAdmittanceController" config_file_name="open_cabinet_door_admittance_controller_config.yaml"/>
</Decorator>
<!-- Execute the Objective -->
<Action ID="LoadObjectiveParameters" config_file_name="open_door_affordance_config.yaml" parameters="{parameters}" />
<Action ID="LoadObjectiveParameters" config_file_name="open_cabinet_door_affordance_config.yaml" parameters="{parameters}" />
<Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_end" parameter_value="{hinge_axis_pose_end}" />
<Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_start" parameter_value="{hinge_axis_pose_start}" />
<Action ID="GetPoseFromUser" parameter_name="process_door_selection.grasp_pose" parameter_value="{grasp_pose}" />
Original file line number Diff line number Diff line change
@@ -4,12 +4,17 @@
<BehaviorTree ID="Open Cabinet Door ML"
_description="Move to Home waypoint, and then open a cabinet door detected by a network">
<Control ID="Sequence" name="Setup">
<!-- Wrap in a ForceSuccess as Sim doesn't have the zero_ftsensor service -->
<!-- This should eventually get overridden for non sim site configs to not ForceSuccess -->
<!-- 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="UpdateAdmittanceController"
config_file_name="open_cabinet_door_admittance_controller_config.yaml" />
<Decorator ID="ForceSuccess">
chancecardona marked this conversation as resolved.
Show resolved Hide resolved
<Action ID="UpdateAdmittanceController"
config_file_name="open_cabinet_door_admittance_controller_config.yaml" />
</Decorator>
<Action ID="LoadObjectiveParameters" config_file_name="open_door_affordance_config.yaml"
parameters="{parameters}" />
<Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
Original file line number Diff line number Diff line change
@@ -5,6 +5,9 @@
<Control ID="Sequence" name="Setup">
<Action ID="LoadObjectiveParameters"
config_file_name="open_lever_handle_door_config.yaml" parameters="{parameters}" />
<!-- Wrap in a ForceSuccess as Sim doesn't have the admittance controllers -->
<!-- This should eventually get overridden for non sim site configs to not ForceSuccess -->
<!-- See https://github.com/PickNikRobotics/moveit_studio/issues/3406 -->
<Decorator ID="ForceSuccess">
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this so the Objective still runs in Gazebo where there is no admittance controller? If so, might be worth dropping a comment.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah this is why. It also has some of the odd behavior (same as the Tare FTS service) where if planning fails then it also complains it can't find the correct controller. In the future we should try to fix that for both of these. In the meantime I'll just update the comments.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated comments. I think it should be included so it works on sim (even if the behavior otherwise fails for different reasons such as our scene not having what it needs). Issue so we can fix this properly.

<Action ID="UpdateAdmittanceController"
config_file_name="open_lever_handle_door_admittance_controller_config.yaml" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
<?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 these actions in ForceSuccess so they work in Sim -->
<!-- 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>
<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>
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