From 49455d51082552acbd6da1ac5327eb1201ada78e Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 4 Sep 2020 17:49:11 +0200 Subject: [PATCH 1/5] [gripper_controller] Added fns for impact detection along z (useful for placing) I want to do impact detection as change detection over force measurements, so I've added two functions: * one for initialising the measurements * one for checking if the difference between the initial measurements and the current measurements is above a threshold --- .../gripper_controller_base.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py b/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py index 69050a9b6..ebe3e3375 100644 --- a/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py +++ b/mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py @@ -2,17 +2,25 @@ class GripperControllerBase(object): def open(self): - rospy.loginfo('[OPEN_GRIPPER] Ignoring request') + rospy.loginfo('[open] Ignoring request') raise NotImplementedError() def close(self): - rospy.loginfo('[CLOSE_GRIPPER] Ignoring request') + rospy.loginfo('[close] Ignoring request') + raise NotImplementedError() + + def init_impact_detection_z(self): + rospy.loginfo('[init_impact_detection_z] Ignoring request') + raise NotImplementedError() + + def detect_impact_z(self): + rospy.loginfo('[detect_impact_z] Ignoring request') raise NotImplementedError() def init_grasp_verification(self): - rospy.loginfo('[INIT_GRASP_VERIFICATION] Ignoring request') + rospy.loginfo('[init_grasp_verification] Ignoring request') raise NotImplementedError() def verify_grasp(self): - rospy.loginfo('[VERIFY_GRASP] Ignoring request') + rospy.loginfo('[verify_grasp] Ignoring request') raise NotImplementedError() From 81fcdb28bfe0df62af172a396e91f5b8b51138d1 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Fri, 4 Sep 2020 19:04:16 +0200 Subject: [PATCH 2/5] [place_action] Added downward arm movement for placing by detecting surface impact --- .../mdr_place_action/README.md | 1 + .../ros/launch/place_action.launch | 1 + .../mdr_place_action/ros/scripts/place_action | 6 +++++- .../ros/src/mdr_place_action/action_states.py | 19 +++++++++++++++---- 4 files changed, 22 insertions(+), 5 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md index 1495a078d..a356c5a53 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md @@ -61,6 +61,7 @@ The following parameters may be passed when launching the action server: * ``base_elbow_offset``: An optional offset between `base_link` and the manipulator's elbow; used for aligning the base with the placing pose so that the manipulator can easily reach it (default: -1) * ``placing_dmp``: Path to a YAML file containing the weights of a dynamic motion primitive used for placing (default: '') * ``dmp_tau``: The value of the temporal dynamic motion primitive parameter (default: 1) +* ``downward_placing_vel``: Velocity with which the arm moves downwards to detect impact with the placing surface (default: -0.02) * ``placing_orientation``: For more constrained manipulators, it might make sense to use a fixed placing orientation (expressed as an (x, y, z, w) quaternion) to ensure easier reachability; for instance, we might want to keep the orientation with which an object was grasped instead of allowing arbitrary orientations (default: [], in which case the argument is ignored) ### Action client diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch index c9e0a1651..806ecb4fd 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch @@ -9,6 +9,7 @@ + [0, 0, 0, 1] diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action index 38d267601..661a4c515 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action @@ -22,6 +22,8 @@ class PlaceServer(object): * placing_dmp: Path to a YAML file containing the weights of a dynamic motion primitive used for placing (default: '') * dmp_tau: The value of the temporal dynamic motion primitive parameter (default: 1) + * downward_placing_vel: Velocity with which the arm moves downwards to detect impact + with the placing surface (default -0.02rad/s) * placing_orientation: For more constrained manipulators, it might make sense to use a fixed placing orientation (expressed as an (x, y, z, w) quaternion) to ensure easier reachability; for instance, we might want to keep @@ -43,6 +45,7 @@ class PlaceServer(object): placing_orientation = rospy.get_param('~placing_orientation', list()) placing_dmp = rospy.get_param('~placing_dmp', '') dmp_tau = float(rospy.get_param('~dmp_tau', 1.)) + downward_placing_vel = float(rospy.get_param('~downward_placing_vel', -0.02)) rospy.loginfo('[place] Initialising state machine') self.action_sm = PlaceSM(move_arm_server=move_arm_server, @@ -53,7 +56,8 @@ class PlaceServer(object): base_elbow_offset=base_elbow_offset, placing_orientation=placing_orientation, placing_dmp=placing_dmp, - dmp_tau=dmp_tau) + dmp_tau=dmp_tau, + downward_placing_vel=downward_placing_vel) rospy.loginfo('[place] State machine initialised') self.action_server = actionlib.SimpleActionServer('place_server', diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py index 993d3ec84..0227ad30a 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py @@ -22,9 +22,10 @@ def __init__(self, timeout=120.0, move_arm_server='move_arm_server', move_base_server='move_base_server', base_elbow_offset=-1., - placing_orientation=list(), + placing_orientation=None, placing_dmp='', dmp_tau=1., + downward_placing_vel=-0.02, max_recovery_attempts=1): super(PlaceSM, self).__init__('Place', [], max_recovery_attempts) self.timeout = timeout @@ -32,7 +33,7 @@ def __init__(self, timeout=120.0, gripper_controller_module_name = '{0}.gripper_controller'.format(gripper_controller_pkg_name) GripperControllerClass = getattr(import_module(gripper_controller_module_name), 'GripperController') - self.gripper = GripperControllerClass() + self.gripper = GripperControllerClass(joint_vel=downward_placing_vel) self.preplace_config_name = preplace_config_name self.preplace_low_config_name = preplace_low_config_name @@ -71,7 +72,7 @@ def running(self): pose = self.goal.pose pose.header.stamp = rospy.Time(0) pose_base_link = self.tf_listener.transformPose('base_link', pose) - if self.placing_orientation: + if self.placing_orientation is not None: pose_base_link.pose.orientation.x = self.placing_orientation[0] pose_base_link.pose.orientation.y = self.placing_orientation[1] pose_base_link.pose.orientation.z = self.placing_orientation[2] @@ -97,8 +98,18 @@ def running(self): rospy.logerr('[place] Arm motion unsuccessful') self.result = self.set_result(False) return FTSMTransitions.DONE - rospy.loginfo('[place] Arm motion successful') + + # the arm is moved down until it makes an impact with the placing surface + rospy.loginfo('[place] Moving arm down until surface impact is detected...') + self.gripper.init_impact_detection_z() + while not self.gripper.detect_impact_z(): + self.gripper.move_down() + rospy.sleep(0.1) + self.gripper.stop_arm() + rospy.loginfo('[place] Impact detected') + + # the object can be released now that impact with the surface has been made rospy.loginfo('[place] Opening the gripper...') self.gripper.open() From 6624b166b2924f2c73e9fc79a169ee9b7efe1945 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 1 Nov 2020 15:20:02 +0100 Subject: [PATCH 3/5] [place_action] Added a new goal field to the action definition The field is a Boolean "release_on_impact", which specifies whether the object should be released by detecting impact with the surface or by simply releasing the object (the latter is used for throwing an object) --- .../mdr_place_action/ros/action/Place.action | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/action/Place.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/action/Place.action index d6001c17a..f112cf64d 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/action/Place.action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/action/Place.action @@ -1,5 +1,7 @@ # goal -geometry_msgs/PoseStamped pose +geometry_msgs/PoseStamped pose # pose at which the object should be placed +bool release_on_impact # whether the object should be released only if + # impact with a placing surface is detected --- # result bool success From d09fb49fce4fbb3726664623e9d975c7f83e69b9 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 1 Nov 2020 15:23:13 +0100 Subject: [PATCH 4/5] [place_action/action_states] The release_on_impact goal field is now used by the action --- .../ros/src/mdr_place_action/action_states.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py index 0227ad30a..33d058960 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py @@ -101,13 +101,14 @@ def running(self): rospy.loginfo('[place] Arm motion successful') # the arm is moved down until it makes an impact with the placing surface - rospy.loginfo('[place] Moving arm down until surface impact is detected...') - self.gripper.init_impact_detection_z() - while not self.gripper.detect_impact_z(): - self.gripper.move_down() - rospy.sleep(0.1) - self.gripper.stop_arm() - rospy.loginfo('[place] Impact detected') + if self.goal.release_on_impact: + rospy.loginfo('[place] Moving arm down until surface impact is detected...') + self.gripper.init_impact_detection_z() + while not self.gripper.detect_impact_z(): + self.gripper.move_down() + rospy.sleep(0.1) + self.gripper.stop_arm() + rospy.loginfo('[place] Impact detected') # the object can be released now that impact with the surface has been made rospy.loginfo('[place] Opening the gripper...') From 8e348726f67e3c80cf65b966c4caa73ec9e34858 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sun, 1 Nov 2020 15:59:24 +0100 Subject: [PATCH 5/5] [place_action/clients] Setting 'release_on_impact' in the place and throw clients The field is set to true and false respectively (we want to detect surface impact while placing, but we don't want to wait for impact while throwing an object) --- .../mdr_place_action/ros/scripts/place_client | 4 ++++ .../mdr_place_action/ros/scripts/throw_client | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client index d62bfd2ed..bc317ad72 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client @@ -75,6 +75,10 @@ class PlaceClient(ActionClientBase): self.robot_name = param.value goal.pose = self.get_placing_pose() + + # we want to place the object by detecting impact with the placing surface + goal.release_on_impact = True + return goal def get_placing_pose(self): diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client index 21ea56e9c..58d5d6ccf 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client @@ -72,6 +72,10 @@ class ThrowClient(ActionClientBase): self.robot_name = param.value goal.pose = self.get_throwing_pose() + + # we don't want to wait for impact when throwing the object + goal.release_on_impact = False + return goal def get_throwing_pose(self):