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 
00031 import copy
00032 import actionlib
00033 import rospy
00034 
00035 from math import sin, cos
00036 from moveit_python import (MoveGroupInterface,
00037                            PlanningSceneInterface,
00038                            PickPlaceInterface)
00039 from moveit_python.geometry import rotate_pose_msg_by_euler_angles
00040 
00041 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
00042 from control_msgs.msg import PointHeadAction, PointHeadGoal
00043 from grasping_msgs.msg import FindGraspableObjectsAction, FindGraspableObjectsGoal
00044 from geometry_msgs.msg import PoseStamped
00045 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00046 from moveit_msgs.msg import PlaceLocation, MoveItErrorCodes
00047 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00048 
00049 # Move base using navigation stack
00050 class MoveBaseClient(object):
00051 
00052     def __init__(self):
00053         self.client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
00054         rospy.loginfo("Waiting for move_base...")
00055         self.client.wait_for_server()
00056 
00057     def goto(self, x, y, theta, frame="map"):
00058         move_goal = MoveBaseGoal()
00059         move_goal.target_pose.pose.position.x = x
00060         move_goal.target_pose.pose.position.y = y
00061         move_goal.target_pose.pose.orientation.z = sin(theta/2.0)
00062         move_goal.target_pose.pose.orientation.w = cos(theta/2.0)
00063         move_goal.target_pose.header.frame_id = frame
00064         move_goal.target_pose.header.stamp = rospy.Time.now()
00065 
00066         # TODO wait for things to work
00067         self.client.send_goal(move_goal)
00068         self.client.wait_for_result()
00069 
00070 # Send a trajectory to controller
00071 class FollowTrajectoryClient(object):
00072 
00073     def __init__(self, name, joint_names):
00074         self.client = actionlib.SimpleActionClient("%s/follow_joint_trajectory" % name,
00075                                                    FollowJointTrajectoryAction)
00076         rospy.loginfo("Waiting for %s..." % name)
00077         self.client.wait_for_server()
00078         self.joint_names = joint_names
00079 
00080     def move_to(self, positions, duration=5.0):
00081         if len(self.joint_names) != len(positions):
00082             print("Invalid trajectory position")
00083             return False
00084         trajectory = JointTrajectory()
00085         trajectory.joint_names = self.joint_names
00086         trajectory.points.append(JointTrajectoryPoint())
00087         trajectory.points[0].positions = positions
00088         trajectory.points[0].velocities = [0.0 for _ in positions]
00089         trajectory.points[0].accelerations = [0.0 for _ in positions]
00090         trajectory.points[0].time_from_start = rospy.Duration(duration)
00091         follow_goal = FollowJointTrajectoryGoal()
00092         follow_goal.trajectory = trajectory
00093 
00094         self.client.send_goal(follow_goal)
00095         self.client.wait_for_result()
00096 
00097 # Point the head using controller
00098 class PointHeadClient(object):
00099 
00100     def __init__(self):
00101         self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
00102         rospy.loginfo("Waiting for head_controller...")
00103         self.client.wait_for_server()
00104 
00105     def look_at(self, x, y, z, frame, duration=1.0):
00106         goal = PointHeadGoal()
00107         goal.target.header.stamp = rospy.Time.now()
00108         goal.target.header.frame_id = frame
00109         goal.target.point.x = x
00110         goal.target.point.y = y
00111         goal.target.point.z = z
00112         goal.min_duration = rospy.Duration(duration)
00113         self.client.send_goal(goal)
00114         self.client.wait_for_result()
00115 
00116 # Tools for grasping
00117 class GraspingClient(object):
00118 
00119     def __init__(self):
00120         self.scene = PlanningSceneInterface("base_link")
00121         self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
00122         self.move_group = MoveGroupInterface("arm", "base_link")
00123 
00124         find_topic = "basic_grasping_perception/find_objects"
00125         rospy.loginfo("Waiting for %s..." % find_topic)
00126         self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
00127         self.find_client.wait_for_server()
00128 
00129     def updateScene(self):
00130         # find objects
00131         goal = FindGraspableObjectsGoal()
00132         goal.plan_grasps = True
00133         self.find_client.send_goal(goal)
00134         self.find_client.wait_for_result(rospy.Duration(5.0))
00135         find_result = self.find_client.get_result()
00136 
00137         # remove previous objects
00138         for name in self.scene.getKnownCollisionObjects():
00139             self.scene.removeCollisionObject(name, False)
00140         for name in self.scene.getKnownAttachedObjects():
00141             self.scene.removeAttachedObject(name, False)
00142         self.scene.waitForSync()
00143 
00144         # insert objects to scene
00145         idx = -1
00146         for obj in find_result.objects:
00147             idx += 1
00148             obj.object.name = "object%d"%idx
00149             self.scene.addSolidPrimitive(obj.object.name,
00150                                          obj.object.primitives[0],
00151                                          obj.object.primitive_poses[0],
00152                                          wait = False)
00153 
00154         for obj in find_result.support_surfaces:
00155             # extend surface to floor, and make wider since we have narrow field of view
00156             height = obj.primitive_poses[0].position.z
00157             obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
00158                                             1.5,  # wider
00159                                             obj.primitives[0].dimensions[2] + height]
00160             obj.primitive_poses[0].position.z += -height/2.0
00161 
00162             # add to scene
00163             self.scene.addSolidPrimitive(obj.name,
00164                                          obj.primitives[0],
00165                                          obj.primitive_poses[0],
00166                                          wait = False)
00167 
00168         self.scene.waitForSync()
00169 
00170         # store for grasping
00171         self.objects = find_result.objects
00172         self.surfaces = find_result.support_surfaces
00173 
00174     def getGraspableCube(self):
00175         graspable = None
00176         for obj in self.objects:
00177             # need grasps
00178             if len(obj.grasps) < 1:
00179                 continue
00180             # check size
00181             if obj.object.primitives[0].dimensions[0] < 0.05 or \
00182                obj.object.primitives[0].dimensions[0] > 0.07 or \
00183                obj.object.primitives[0].dimensions[0] < 0.05 or \
00184                obj.object.primitives[0].dimensions[0] > 0.07 or \
00185                obj.object.primitives[0].dimensions[0] < 0.05 or \
00186                obj.object.primitives[0].dimensions[0] > 0.07:
00187                 continue
00188             # has to be on table
00189             if obj.object.primitive_poses[0].position.z < 0.5:
00190                 continue
00191             return obj.object, obj.grasps
00192         # nothing detected
00193         return None, None
00194 
00195     def getSupportSurface(self, name):
00196         for surface in self.support_surfaces:
00197             if surface.name == name:
00198                 return surface
00199         return None
00200 
00201     def getPlaceLocation(self):
00202         pass
00203 
00204     def pick(self, block, grasps):
00205         success, pick_result = self.pickplace.pick_with_retry(block.name,
00206                                                               grasps,
00207                                                               support_name=block.support_surface,
00208                                                               scene=self.scene)
00209         self.pick_result = pick_result
00210         return success
00211 
00212     def place(self, block, pose_stamped):
00213         places = list()
00214         l = PlaceLocation()
00215         l.place_pose.pose = pose_stamped.pose
00216         l.place_pose.header.frame_id = pose_stamped.header.frame_id
00217 
00218         # copy the posture, approach and retreat from the grasp used
00219         l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
00220         l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
00221         l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
00222         places.append(copy.deepcopy(l))
00223         # create another several places, rotate each by 360/m degrees in yaw direction
00224         m = 16 # number of possible place poses
00225         pi = 3.141592653589
00226         for i in range(0, m-1):
00227             l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
00228             places.append(copy.deepcopy(l))
00229 
00230         success, place_result = self.pickplace.place_with_retry(block.name,
00231                                                                 places,
00232                                                                 scene=self.scene)
00233         return success
00234 
00235     def tuck(self):
00236         joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
00237                   "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
00238         pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
00239         while not rospy.is_shutdown():
00240             result = self.move_group.moveToJointPosition(joints, pose, 0.02)
00241             if result.error_code.val == MoveItErrorCodes.SUCCESS:
00242                 return
00243 
00244 if __name__ == "__main__":
00245     # Create a node
00246     rospy.init_node("demo")
00247 
00248     # Make sure sim time is working
00249     while not rospy.Time.now():
00250         pass
00251 
00252     # Setup clients
00253     move_base = MoveBaseClient()
00254     torso_action = FollowTrajectoryClient("torso_controller", ["torso_lift_joint"])
00255     head_action = PointHeadClient()
00256     grasping_client = GraspingClient()
00257 
00258     # Move the base to be in front of the table
00259     # Demonstrates the use of the navigation stack
00260     rospy.loginfo("Moving to table...")
00261     move_base.goto(2.250, 3.118, 0.0)
00262     move_base.goto(2.750, 3.118, 0.0)
00263 
00264     # Raise the torso using just a controller
00265     rospy.loginfo("Raising torso...")
00266     torso_action.move_to([0.4, ])
00267 
00268     # Point the head at the cube we want to pick
00269     head_action.look_at(3.7, 3.18, 0.0, "map")
00270 
00271     # Get block to pick
00272     while not rospy.is_shutdown():
00273         rospy.loginfo("Picking object...")
00274         grasping_client.updateScene()
00275         cube, grasps = grasping_client.getGraspableCube()
00276         if cube == None:
00277             rospy.logwarn("Perception failed.")
00278             continue
00279 
00280         # Pick the block
00281         if grasping_client.pick(cube, grasps):
00282             break
00283         rospy.logwarn("Grasping failed.")
00284 
00285     # Tuck the arm
00286     grasping_client.tuck()
00287 
00288     # Lower torso
00289     rospy.loginfo("Lowering torso...")
00290     torso_action.move_to([0.0, ])
00291 
00292     # Move to second table
00293     rospy.loginfo("Moving to second table...")
00294     move_base.goto(-3.53, 3.75, 1.57)
00295     move_base.goto(-3.53, 4.15, 1.57)
00296 
00297     # Raise the torso using just a controller
00298     rospy.loginfo("Raising torso...")
00299     torso_action.move_to([0.4, ])
00300 
00301     # Place the block
00302     while not rospy.is_shutdown():
00303         rospy.loginfo("Placing object...")
00304         pose = PoseStamped()
00305         pose.pose = cube.primitive_poses[0]
00306         pose.pose.position.z += 0.05
00307         pose.header.frame_id = cube.header.frame_id
00308         if grasping_client.place(cube, pose):
00309             break
00310         rospy.logwarn("Placing failed.")
00311 
00312     # Tuck the arm, lower the torso
00313     grasping_client.tuck()
00314     torso_action.move_to([0.0, ])


fetch_gazebo_demo
Author(s): Michael Ferguson
autogenerated on Wed Apr 3 2019 02:48:52