Merge pull request #239 from b-it-bots/features/surface-impact-while-…

…placing

Impact-based object placing

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()

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

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

mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_action.launch

@@ -9,6 +9,7 @@
     <arg name="placing_dmp" default="$(find mdr_place_action)/config/trajectory_weights/weights_table_place.yaml" />
     <arg name="dmp_tau" default="30" />
     <arg name="placing_orientation" default="[0, 0, 0, 1]" />
+    <arg name="downward_placing_vel" default="-0.02" />
 
     <node pkg="mdr_place_action" type="place_action" name="place_server" output="screen">
         <param name="move_arm_server" value="$(arg move_arm_server)" />
@@ -19,6 +20,7 @@
         <param name="base_elbow_offset" value="$(arg base_elbow_offset)" />
         <param name="placing_dmp" value="$(arg placing_dmp)" />
         <param name="dmp_tau" value="$(arg dmp_tau)" />
+        <param name="downward_placing_vel" value="$(arg downward_placing_vel)" />
         <rosparam param="placing_orientation" subst_value="True">$(arg placing_orientation)</rosparam>
     </node>
 </launch>

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',

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):

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):

mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py

@@ -22,17 +22,18 @@ 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
 
         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,19 @@ 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
+        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...')
         self.gripper.open()