pick_place_demo.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2015, Fetch Robotics Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 #     * Redistributions of source code must retain the above copyright
00010 #       notice, this list of conditions and the following disclaimer.
00011 #     * Redistributions in binary form must reproduce the above copyright
00012 #       notice, this list of conditions and the following disclaimer in the
00013 #       documentation and/or other materials provided with the distribution.
00014 #     * Neither the name of the Fetch Robotics Inc. nor the names of its
00015 #       contributors may be used to endorse or promote products derived from
00016 #       this software without specific prior written permission.
00017 # 
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY
00022 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00027 # THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 
00029 # Author: Michael Ferguson
00030 # Author: Di Sun
00031 
00032 import copy
00033 import actionlib
00034 import rospy
00035 
00036 from math import sin, cos
00037 from moveit_python import (MoveGroupInterface,
00038                            PlanningSceneInterface,
00039                            PickPlaceInterface)
00040 from moveit_python.geometry import rotate_pose_msg_by_euler_angles
00041 
00042 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
00043 from control_msgs.msg import PointHeadAction, PointHeadGoal
00044 from grasping_msgs.msg import FindGraspableObjectsAction, FindGraspableObjectsGoal
00045 from geometry_msgs.msg import PoseStamped
00046 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00047 from moveit_msgs.msg import PlaceLocation, MoveItErrorCodes
00048 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00049 
00050 # Move base using navigation stack
00051 class MoveBaseClient(object):
00052 
00053     def __init__(self):
00054         self.client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
00055         rospy.loginfo("Waiting for move_base...")
00056         self.client.wait_for_server()
00057 
00058     def goto(self, x, y, theta, frame="map"):
00059         move_goal = MoveBaseGoal()
00060         move_goal.target_pose.pose.position.x = x
00061         move_goal.target_pose.pose.position.y = y
00062         move_goal.target_pose.pose.orientation.z = sin(theta/2.0)
00063         move_goal.target_pose.pose.orientation.w = cos(theta/2.0)
00064         move_goal.target_pose.header.frame_id = frame
00065         move_goal.target_pose.header.stamp = rospy.Time.now()
00066 
00067         # TODO wait for things to work
00068         self.client.send_goal(move_goal)
00069         self.client.wait_for_result()
00070 
00071 # Send a trajectory to controller
00072 class FollowTrajectoryClient(object):
00073 
00074     def __init__(self, name, joint_names):
00075         self.client = actionlib.SimpleActionClient("%s/follow_joint_trajectory" % name,
00076                                                    FollowJointTrajectoryAction)
00077         rospy.loginfo("Waiting for %s..." % name)
00078         self.client.wait_for_server()
00079         self.joint_names = joint_names
00080 
00081     def move_to(self, positions, duration=5.0):
00082         if len(self.joint_names) != len(positions):
00083             print("Invalid trajectory position")
00084             return False
00085         trajectory = JointTrajectory()
00086         trajectory.joint_names = self.joint_names
00087         trajectory.points.append(JointTrajectoryPoint())
00088         trajectory.points[0].positions = positions
00089         trajectory.points[0].velocities = [0.0 for _ in positions]
00090         trajectory.points[0].accelerations = [0.0 for _ in positions]
00091         trajectory.points[0].time_from_start = rospy.Duration(duration)
00092         follow_goal = FollowJointTrajectoryGoal()
00093         follow_goal.trajectory = trajectory
00094 
00095         self.client.send_goal(follow_goal)
00096         self.client.wait_for_result()
00097 
00098 # Point the head using controller
00099 class PointHeadClient(object):
00100 
00101     def __init__(self):
00102         self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
00103         rospy.loginfo("Waiting for head_controller...")
00104         self.client.wait_for_server()
00105 
00106     def look_at(self, x, y, z, frame, duration=1.0):
00107         goal = PointHeadGoal()
00108         goal.target.header.stamp = rospy.Time.now()
00109         goal.target.header.frame_id = frame
00110         goal.target.point.x = x
00111         goal.target.point.y = y
00112         goal.target.point.z = z
00113         goal.min_duration = rospy.Duration(duration)
00114         self.client.send_goal(goal)
00115         self.client.wait_for_result()
00116 
00117 # Tools for grasping
00118 class GraspingClient(object):
00119 
00120     def __init__(self):
00121         self.scene = PlanningSceneInterface("base_link")
00122         self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
00123         self.move_group = MoveGroupInterface("arm", "base_link")
00124 
00125         find_topic = "basic_grasping_perception/find_objects"
00126         rospy.loginfo("Waiting for %s..." % find_topic)
00127         self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
00128         self.find_client.wait_for_server()
00129 
00130     def updateScene(self):
00131         # find objects
00132         goal = FindGraspableObjectsGoal()
00133         goal.plan_grasps = True
00134         self.find_client.send_goal(goal)
00135         self.find_client.wait_for_result(rospy.Duration(5.0))
00136         find_result = self.find_client.get_result()
00137 
00138         # remove previous objects
00139         for name in self.scene.getKnownCollisionObjects():
00140             self.scene.removeCollisionObject(name, False)
00141         for name in self.scene.getKnownAttachedObjects():
00142             self.scene.removeAttachedObject(name, False)
00143         self.scene.waitForSync()
00144 
00145         # insert objects to scene
00146         objects = list()
00147         idx = -1
00148         for obj in find_result.objects:
00149             idx += 1
00150             obj.object.name = "object%d"%idx
00151             self.scene.addSolidPrimitive(obj.object.name,
00152                                          obj.object.primitives[0],
00153                                          obj.object.primitive_poses[0],
00154                                          wait = False)
00155             if obj.object.primitive_poses[0].position.x < 0.85:
00156                 objects.append([obj, obj.object.primitive_poses[0].position.z])
00157 
00158         for obj in find_result.support_surfaces:
00159             # extend surface to floor, and make wider since we have narrow field of view
00160             height = obj.primitive_poses[0].position.z
00161             obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
00162                                             1.5,  # wider
00163                                             obj.primitives[0].dimensions[2] + height]
00164             obj.primitive_poses[0].position.z += -height/2.0
00165 
00166             # add to scene
00167             self.scene.addSolidPrimitive(obj.name,
00168                                          obj.primitives[0],
00169                                          obj.primitive_poses[0],
00170                                          wait = False)
00171 
00172         self.scene.waitForSync()
00173 
00174         # store for grasping
00175         #self.objects = find_result.objects
00176         self.surfaces = find_result.support_surfaces
00177 
00178         # store graspable objects by Z
00179         objects.sort(key=lambda object: object[1])
00180         objects.reverse()
00181         self.objects = [object[0] for object in objects]
00182         #for object in objects:
00183         #    print(object[0].object.name, object[1])
00184         #exit(-1)
00185 
00186     def getGraspableObject(self):
00187         graspable = None
00188         for obj in self.objects:
00189             # need grasps
00190             if len(obj.grasps) < 1:
00191                 continue
00192             # check size
00193             if obj.object.primitives[0].dimensions[0] < 0.03 or \
00194                obj.object.primitives[0].dimensions[0] > 0.25 or \
00195                obj.object.primitives[0].dimensions[0] < 0.03 or \
00196                obj.object.primitives[0].dimensions[0] > 0.25 or \
00197                obj.object.primitives[0].dimensions[0] < 0.03 or \
00198                obj.object.primitives[0].dimensions[0] > 0.25:
00199                 continue
00200             # has to be on table
00201             if obj.object.primitive_poses[0].position.z < 0.5:
00202                 continue
00203             print obj.object.primitive_poses[0], obj.object.primitives[0]
00204             return obj.object, obj.grasps
00205         # nothing detected
00206         return None, None
00207 
00208     def getSupportSurface(self, name):
00209         for surface in self.support_surfaces:
00210             if surface.name == name:
00211                 return surface
00212         return None
00213 
00214     def getPlaceLocation(self):
00215         pass
00216 
00217     def pick(self, block, grasps):
00218         success, pick_result = self.pickplace.pick_with_retry(block.name,
00219                                                               grasps,
00220                                                               support_name=block.support_surface,
00221                                                               scene=self.scene)
00222         self.pick_result = pick_result
00223         return success
00224 
00225     def place(self, block, pose_stamped):
00226         places = list()
00227         l = PlaceLocation()
00228         l.place_pose.pose = pose_stamped.pose
00229         l.place_pose.header.frame_id = pose_stamped.header.frame_id
00230 
00231         # copy the posture, approach and retreat from the grasp used
00232         l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
00233         l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
00234         l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
00235         places.append(copy.deepcopy(l))
00236         # create another several places, rotate each by 360/m degrees in yaw direction
00237         m = 16 # number of possible place poses
00238         pi = 3.141592653589
00239         for i in range(0, m-1):
00240             l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
00241             places.append(copy.deepcopy(l))
00242 
00243         success, place_result = self.pickplace.place_with_retry(block.name,
00244                                                                 places,
00245                                                                 scene=self.scene)
00246         return success
00247 
00248     def tuck(self):
00249         joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
00250                   "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
00251         pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
00252         while not rospy.is_shutdown():
00253             result = self.move_group.moveToJointPosition(joints, pose, 0.02)
00254             if result.error_code.val == MoveItErrorCodes.SUCCESS:
00255                 return
00256 
00257     def stow(self):
00258         joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
00259                   "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
00260         pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
00261         while not rospy.is_shutdown():
00262             result = self.move_group.moveToJointPosition(joints, pose, 0.02)
00263             if result.error_code.val == MoveItErrorCodes.SUCCESS:
00264                 return
00265 
00266     def intermediate_stow(self):
00267         joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
00268                   "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
00269         pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
00270         while not rospy.is_shutdown():
00271             result = self.move_group.moveToJointPosition(joints, pose, 0.02)
00272             if result.error_code.val == MoveItErrorCodes.SUCCESS:
00273                 return
00274 
00275 if __name__ == "__main__":
00276     # Create a node
00277     rospy.init_node("demo")
00278 
00279     # Make sure sim time is working
00280     while not rospy.Time.now():
00281         pass
00282 
00283     # Setup clients
00284     #move_base = MoveBaseClient()
00285     #torso_action = FollowTrajectoryClient("torso_controller", ["torso_lift_joint"])
00286     head_action = PointHeadClient()
00287     grasping_client = GraspingClient()
00288 
00289     # Move the base to be in front of the table
00290     # Demonstrates the use of the navigation stack
00291     #rospy.loginfo("Moving to table...")
00292     #move_base.goto(2.250, 3.118, 0.0)
00293     #move_base.goto(2.750, 3.118, 0.0)
00294 
00295     # Raise the torso using just a controller
00296     #rospy.loginfo("Raising torso...")
00297     #torso_action.move_to([0.4, ])
00298 
00299     # Point the head at the cube we want to pick
00300     # head_action.look_at(3.7, 3.18, 0.0, "map")
00301     cube_in_grapper = False
00302     grasping_client.stow()
00303 
00304     while not rospy.is_shutdown():
00305         head_action.look_at(1.2, 0.0, 0.0, "base_link")
00306 
00307         # Get block to pick
00308         fail_ct = 0
00309         while not rospy.is_shutdown() and not cube_in_grapper:
00310             rospy.loginfo("Picking object...")
00311             grasping_client.updateScene()
00312             cube, grasps = grasping_client.getGraspableObject()
00313             if cube == None:
00314                 rospy.logwarn("Perception failed.")
00315                 # grasping_client.intermediate_stow()
00316                 grasping_client.stow()
00317                 head_action.look_at(1.2, 0.0, 0.0, "base_link")
00318                 continue
00319 
00320             # Pick the block
00321             if grasping_client.pick(cube, grasps):
00322                 cube_in_grapper = True
00323                 break
00324             rospy.logwarn("Grasping failed.")
00325             grasping_client.stow()
00326             if fail_ct > 15:
00327                 fail_ct = 0
00328                 break
00329             fail_ct += 1
00330 
00331         # Tuck the arm
00332         #grasping_client.tuck()
00333 
00334         # Lower torso
00335         #rospy.loginfo("Lowering torso...")
00336         #torso_action.move_to([0.0, ])
00337 
00338         # Move to second table
00339         #rospy.loginfo("Moving to second table...")
00340         #move_base.goto(-3.53, 3.75, 1.57)
00341         #move_base.goto(-3.53, 4.15, 1.57)
00342 
00343         # Raise the torso using just a controller
00344         #rospy.loginfo("Raising torso...")
00345         #torso_action.move_to([0.4, ])
00346 
00347         # Place the block
00348         while not rospy.is_shutdown() and cube_in_grapper:
00349             rospy.loginfo("Placing object...")
00350             pose = PoseStamped()
00351             pose.pose = cube.primitive_poses[0]
00352             pose.pose.position.y *= -1.0
00353             pose.pose.position.z += 0.02
00354             pose.header.frame_id = cube.header.frame_id
00355             if grasping_client.place(cube, pose):
00356                 cube_in_grapper = False
00357                 break
00358             rospy.logwarn("Placing failed.")
00359             grasping_client.intermediate_stow()
00360             grasping_client.stow()
00361             if fail_ct > 15:
00362                 fail_ct = 0
00363                 break
00364             fail_ct += 1
00365         # Tuck the arm, lower the torso
00366         grasping_client.intermediate_stow()
00367         grasping_client.stow()
00368         rospy.loginfo("Finished")
00369         #torso_action.move_to([0.0, ])


fetch_gazebo_demo
Author(s): Michael Ferguson
autogenerated on Mon Jan 2 2017 03:53:16