class PickPlaceDemo(): def __init__(self): self._scene = PlanningSceneInterface('base_link') self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True) self._move_group = MoveGroupInterface('xarm5', 'base_link') def setup_scene(self): # remove previous objects for name in self._scene.getKnownCollisionObjects(): self._scene.removeCollisionObject(name, False) for name in self._scene.getKnownAttachedObjects(): self._scene.removeAttachedObject(name, False) self._scene.waitForSync() self.__addBoxWithOrientation('box', 0.25, 0.25, 0.25, -0.45, 0.1, 0.125, 0, 0, np.radians(45), wait=False) self._scene.waitForSync() def __addBoxWithOrientation(self, name, size_x, size_y, size_z, x, y, z, roll, pitch, yaw, wait=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX ps = PoseStamped() ps.header.frame_id = self._scene._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) ps.pose.orientation.x = q[0] ps.pose.orientation.y = q[1] ps.pose.orientation.z = q[2] ps.pose.orientation.w = q[3] self._scene.addSolidPrimitive(name, s, ps.pose, wait) def pickup(self): g = Grasp() self._pickplace.pickup("box", [g], support_name="box")
def main(): rospy.init_node("arm_obstacle_demo") wait_for_time() argv = rospy.myargv() planning_scene = PlanningSceneInterface("base_link") # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.2 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 gripper = robot_api.Gripper() arm = robot_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } planning_scene.removeAttachedObject('tray') gripper.open() error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') rospy.sleep(2) frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() gripper.close() rospy.sleep(2) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') gripper.open() planning_scene.removeAttachedObject('tray') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') return
class GripperInterfaceClient(): def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print '==========', goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0]+goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Try mesh representation #idx = -1 #for obj in find_result.objects: # idx += 1 # obj.object.name = "object%d"%idx # self.scene.addMesh(obj.object.name, # obj.object.mesh_poses[0], # obj.object.meshes[0], # wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1, 2.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size # if obj.object.primitives[0].dimensions[0] < 0.03 or \ # obj.object.primitives[0].dimensions[0] > 0.04 or \ # obj.object.primitives[0].dimensions[0] < 0.07 or \ # obj.object.primitives[0].dimensions[0] > 0.09 or \ # obj.object.primitives[2].dimensions[2] < 0.19 or \ # obj.object.primitives[2].dimensions[2] > 0.20: # continue # has to be on table #if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def plan_pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, allow_gripper_support_collision=False, planner_id = 'PRMkConfigDefault', retries = 2, scene=self.scene) #self.pick_result = pick_result if success: return pick_result.trajectory_stages else: rospy.loginfo("Planning Failed.") return None
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() planning_scene = PlanningSceneInterface('base_link') # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.4 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) #planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) # and add an orientation constraint to pose 2: oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 kwargs = { 'allowed_planning_time': 100, 'execution_timeout': 100, 'num_planning_attempts': 15, 'replan': True, 'replan_attempts': 10 } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) kwargs['orientation'] = oc error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray')
description="Remove objects from the MoveIt planning scene.") parser.add_argument("name", nargs="?", help="Name of object to remove") parser.add_argument("--all", help="Remove all objects.", action="store_true") parser.add_argument("--attached", help="Remove an attached object.", action="store_true") args = parser.parse_args() if args.all: rospy.init_node("remove_objects") scene = PlanningSceneInterface("base_link") for name in scene.getKnownCollisionObjects(): print("Removing %s" % name) scene.removeCollisionObject(name, use_service=False) scene.waitForSync() elif args.name: rospy.init_node("remove_objects") scene = PlanningSceneInterface("base_link") print("Removing %s" % args.name) if args.attached: scene.removeAttachedObject(args.name) else: scene.removeCollisionObject(args.name) else: parser.print_help()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") # 初始化需要使用move group控制的机械臂中的arm group self.arm = MoveGroupCommander('arm') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 self.arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' self.arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s self.arm.set_planning_time(5) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene objects = list() idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) if obj.object.primitive_poses[0].position.x < 0.85: objects.append([obj, obj.object.primitive_poses[0].position.z]) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping #self.objects = find_result.objects self.surfaces = find_result.support_surfaces # store graspable objects by Z objects.sort(key=lambda object: object[1]) objects.reverse() self.objects = [object[0] for object in objects] #for object in objects: # print(object[0].object.name, object[1]) #exit(-1) def getGraspableObject(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue print obj.object.primitive_poses[0], obj.object.primitives[0] return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m-1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success def check_collision(self, pose): # [0, 0, 0, 0, 0, 0, 0] self.arm.set_joint_value_target(pose) # 控制机械臂完成运动 #print self.arm.go() #self.arm.set_joint_value_target([1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]) return self.arm.go() def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def stow(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def intermediate_stow(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class MoveItDemo: def __init__(self): rospy.init_node('moveit_demo') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() self.server = InteractiveMarkerServer("obj_find") # Initialize the move group for the right arm #######arm = MoveGroupCommander(GROUP_NAME_ARM) self.arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper #######gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link #######end_effector_link = arm.get_end_effector_link() self.end_effector_link = self.arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) #######arm.set_goal_position_tolerance(0.05) #######arm.set_goal_orientation_tolerance(0.1) self.arm.set_goal_position_tolerance(0.05) self.arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution #######arm.allow_replanning(True) self.arm.allow_replanning(True) # Set the right arm reference frame #######arm.set_pose_reference_frame(REFERENCE_FRAME) self.arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt #######arm.set_planning_time(15) self.arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing #######max_pick_attempts = 1 self.max_pick_attempts = 1 # Set a limit on the number of place attempts ########max_place_attempts = 3 self.max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) ''' rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") rospy.sleep(1) ''' self.arm.set_named_target('right_up') if self.arm.go() != True: rospy.logwarn(" Go failed") self.gripper.set_joint_value_target(self.gripper_opened) if self.gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(1) # Initialize the grasping goal #goal = FindGraspableObjectsGoal() #goal.plan_grasps = False ''' find_objects.send_goal(goal) find_objects.wait_for_result(rospy.Duration(5.0)) find_result = find_objects.get_result() rospy.loginfo("Found %d objects" %len(find_result.objects)) ''' for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() self.scene._colors = dict() # Use the nearest object on the table as the target self.target_pose = PoseStamped() self.target_pose.header.frame_id = REFERENCE_FRAME self.target_id = 'target' self.target_size = None self.the_object = None self.the_object_dist = 0.30 self.the_object_dist_min = 0.10 self.count = -1 #for obj in find_result.objects: # count +=1 #dx = obj.object.primitive_poses[0].position.x #dy = obj.object.primitive_poses[0].position.y dx = 0.25 dy = 0.0 d = math.sqrt((dx * dx) + (dy * dy)) m = Marker() m.type = Marker.CUBE m.scale.x = 0.02 m.scale.y = 0.02 m.scale.z = 0.05 m.color.r = 0.0 m.color.g = 0.5 m.color.b = 0.5 m.color.a = 1.0 int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.name = "block_1" int_marker.pose.position.x = dx int_marker.pose.position.y = dy int_marker.pose.position.z = 0.04 int_marker.pose.orientation.x = 0.0 int_marker.pose.orientation.y = 0.0 int_marker.pose.orientation.z = 0.0 int_marker.pose.orientation.w = 1.0 control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE int_marker.controls.append(control) control.markers.append(m) control.always_visible = True int_marker.controls.append(control) self.server.insert(int_marker,self.processFeedback) self.server.applyChanges() def processFeedback(self,feedback): #p.pose.position = feedback.pose.position #self.dist_xy.pose.position = feedback.pose.position old_pose = Pose() new_pose = Pose() result = None if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_DOWN: old_pose.position.x = feedback.pose.position.x old_pose.position.y = feedback.pose.position.y result = self.pick_pose(old_pose) print "old pose is " + str(old_pose.position.x) + " , " + str(old_pose.position.y) if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_UP: new_pose.position.x = feedback.pose.position.x new_pose.position.y = feedback.pose.position.y self.pick_and_place_pose(old_pose, new_pose) print "new pose is " + str(new_pose.position.x) + " , " + str(new_pose.position.y) def pick_and_place_pose(self,pick_pose,place_pose): pass def pick_pose(self, pick_pose): moveit_commander.roscpp_initialize(sys.argv) dx = pick_pose.position.x dy = pick_pose.position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < self.the_object_dist and d > self.the_object_dist_min: #if dx > the_object_dist_xmin and dx < the_object_dist_xmax: # if dy > the_object_dist_ymin and dy < the_object_dist_ymax: rospy.loginfo("object is in the working zone") self.the_object_dist = d self.the_object = self.count self.target_size = [0.02, 0.02, 0.05] #target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0 #target_pose.pose.position.y = obj.object.primitive_poses[0].position.y self.target_pose.pose.position.x = dx + self.target_size[0] / 2.0 self.target_pose.pose.position.y = dy self.target_pose.pose.position.z = 0.04 self.target_pose.pose.orientation.x = 0.0 self.target_pose.pose.orientation.y = 0.0 self.target_pose.pose.orientation.z = 0.0 self.target_pose.pose.orientation.w = 1.0 # wait for the scene to sync self.scene.waitForSync() self.scene.setColor(self.target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) self.scene.sendColors() grasp_pose = self.target_pose grasp_pose.pose.position.y -= self.target_size[1] / 2.0 grasp_pose.pose.position.x += self.target_size[0] grasps = self.make_grasps(grasp_pose, [self.target_id], [self.target_size[1] - self.gripper_tighten]) # Track success/failure and number of attempts for pick operation ''' for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) ''' # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state self.arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < self.max_pick_attempts: result = self.arm.pick(self.target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) result = self.arm.pick(self.target_id, grasps) else: rospy.loginfo("object is not in the working zone") rospy.sleep(1) self.arm.set_named_target('right_up') self.arm.go() # Open the gripper to the neutral position self.gripper.set_joint_value_target(self.gripper_neutral) self.gripper.go() rospy.sleep(1) return result # Get the gripper posture as a JointTrajectory def make_gripper_posture(self, joint_positions): # Initialize the joint trajectory for the gripper joints t = JointTrajectory() # Set the joint names to the gripper joint names t.header.stamp = rospy.get_rostime() t.joint_names = GRIPPER_JOINT_NAMES # Initialize a joint trajectory point to represent the goal tp = JointTrajectoryPoint() # Assign the trajectory joint positions to the input positions tp.positions = joint_positions # Set the gripper effort tp.effort = GRIPPER_EFFORT tp.time_from_start = rospy.Duration(0.0) # Append the goal point to the trajectory points t.points.append(tp) # Return the joint trajectory return t # Generate a gripper translation in the direction given by vector def make_gripper_translation(self, min_dist, desired, vector): # Initialize the gripper translation object g = GripperTranslation() # Set the direction vector components to the input g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] # The vector is relative to the gripper frame g.direction.header.frame_id = GRIPPER_FRAME # Assign the min and desired distances from the input g.min_distance = min_dist g.desired_distance = desired return g # Generate a list of possible grasps def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately; # grasp_opening should be a bit smaller than target width g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened) g.grasp_posture = self.make_gripper_posture(grasp_opening) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5]) g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading # from arm base to the object to pick (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle for yaw in yaw_vals: for pitch in pitch_vals: # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(pitch) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps # Generate a list of possible place poses def make_places(self, target_id, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015] # A list of pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] # A list to hold the places places = [] # Generate a place pose for each angle and translation for pitch in pitch_vals: for dy in y_vals: for dx in x_vals: place.pose.position.x = init_pose.pose.position.x + dx place.pose.position.y = init_pose.pose.position.y + dy # Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from # arm base to the place location (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw = atan2(y, x) # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains # the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the # objects orientation!), so we cancel this transformation. It is applied here: # https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64 # More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577 acobjs = self.scene.get_attached_objects([target_id]) aco_pose = self.get_pose(acobjs[target_id]) if aco_pose is None: rospy.logerr("Attached collision object '%s' not found" % target_id) return None aco_tf = self.pose_to_mat(aco_pose) place_tf = self.pose_to_mat(place.pose) place.pose = self.mat_to_pose(place_tf, aco_tf) rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \ % (aco_pose, place.pose)) # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places # Set the color of an object def setColor(self, name, r, g, b, a=0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p) def get_pose(self, co): # We get object's pose from the mesh/primitive poses; try first with the meshes if isinstance(co, CollisionObject): if co.mesh_poses: return co.mesh_poses[0] elif co.primitive_poses: return co.primitive_poses[0] else: rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id) return None elif isinstance(co, AttachedCollisionObject): if co.object.mesh_poses: return co.object.mesh_poses[0] elif co.object.primitive_poses: return co.object.primitive_poses[0] else: rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id) return None else: rospy.logerr("Input parameter is not a collision object") return None def pose_to_mat(self, pose): '''Convert a pose message to a 4x4 numpy matrix. Args: pose (geometry_msgs.msg.Pose): Pose rospy message class. Returns: mat (numpy.matrix): 4x4 numpy matrix ''' quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T mat = np.matrix(quaternion_matrix(quat)) mat[0:3, 3] = pos return mat def mat_to_pose(self, mat, transform=None): '''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform. Args: mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform. transform (numpy.ndarray): Optional 4x4 array representing additional transform Returns: pose (geometry_msgs.msg.Pose): Pose message representing transform. ''' if transform != None: mat = np.dot(mat, transform) pose = Pose() pose.position.x = mat[0,3] pose.position.y = mat[1,3] pose.position.z = mat[2,3] quat = quaternion_from_matrix(mat) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose
class World: def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) """Calls a method to display the collision objects. """ def create_planning_scene(self): print "[INFO] Create_planning_scene" self.display_towers_and_disks() """This method collects all needed information and publish them to other methods. """ def display_towers_and_disks(self): print "[INFO] Display towers and disks" for tower in self.tower_array: # call method to publish new tower self.publish_scene(tower.get_position(), tower) # set color by name self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0) # publish color self.planning_scene_interface.sendColors() for disk in self.disk_array: self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors() # wait for sync after publishing collision objects self.planning_scene_interface.waitForSync(5.0) rospy.sleep(5) """This method creates a box or a cylinder with methods of the planning scene interface. :param position: the position of the new collision object :param tower: the tower object :param disk: the disk object """ def publish_scene(self, position, tower=None, disk=None): if tower is not None: self.planning_scene_interface.addBox(tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0, (self.tower_positions[tower.get_id() - 1][2] * 2), position[0], position[1], position[2]) else: self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2, position[0], position[1], position[2]) """This method cleans the planning scene. :param close: with this flag a new planning scene objects will be removed in sync otherwise the object will be deleted without syncing the scene """ def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass """This method corrects the position of the moved disk. :param tower: parent tower of the disk """ def refresh_disk_pose(self, tower): print "[INFO] Refresh disk pose" disk = tower.get_last() if self.DEBUG is 1: print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\ "tower pose", tower.get_position() # remove the disk from planning scene self.planning_scene_interface.removeCollisionObject(disk.get_id(), False) # publish collision object and set old color self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") self.objectname_sub = rospy.Subscriber("/objects", Float32MultiArray, self.callback) self.interestedObjectName = "" #self.objects = [] find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def callback(self, msg): if len(msg.data) != 0: self.interestedObjectName = "/object_" + (str(int(msg.data[0]))) #print(self.interestedObjectName) def getInterestedObject(self, objects): listener = tf.TransformListener() while not rospy.is_shutdown(): try: if len(self.interestedObjectName) == 0: continue (trans, rot) = listener.lookupTransform("/base_link", self.interestedObjectName, rospy.Time(0)) if len(trans) == 3: break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue final_objects = [] for obj in objects: print(obj[0].object.name) print("obj_x: " + str(trans[0]) + "----" + str(obj[0].object.primitive_poses[0].position.x)) print("obj_x: " + str(trans[1]) + "----" + str(obj[0].object.primitive_poses[0].position.y)) print("obj_x: " + str(trans[2]) + "----" + str(obj[0].object.primitive_poses[0].position.z)) if (abs(obj[0].object.primitive_poses[0].position.x - trans[0]) <= 0.1 and abs(obj[0].object.primitive_poses[0].position.y - trans[1]) <= 0.1 and abs(obj[0].object.primitive_poses[0].position.z - trans[2]) <= 0.1): return [obj[0]] break return [] rospy.loginfo("No Interested Objects found") def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene objects = list() idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx temp_height = obj.object.primitive_poses[0].position.z obj.object.primitive_poses[0].position.z -= 0.02 * temp_height self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False) if obj.object.primitive_poses[0].position.x < 0.85: objects.append([obj, obj.object.primitive_poses[0].position.z]) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping #self.objects = find_result.objects self.surfaces = find_result.support_surfaces # store graspable objects by Z #objects.sort(key=lambda object: object[1]) #objects.reverse() #self.objects = [object[0] for object in objects] self.objects = self.getInterestedObject(objects) #for object in objects: # print(object[0].object.name, object[1]) #exit(-1) def getGraspableObject(self): graspable = None if len(self.objects) == 0: return None, None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] < 0.03: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue print obj.object.primitive_poses[0], obj.object.primitives[0] return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, support_name=block.support_surface, scene=self.scene) return success def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def stow(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def intermediate_stow(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects # here i am declaring goal as an object. https://github.com/mikeferguson/grasping_msgs/blob/master/action/FindGraspableObjects.action goal = FindGraspableObjectsGoal() goal.plan_grasps = True # passing the object to find_client # now find_client is wer magic happens # on demo.launch file i am runnning basic_grasping_perception. (below <!-- Start Perception -->) # this keeps running on background and i use actionlib (initalize on init) to get a hook to it. # find_client is connected to basic_grasping_perception self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) # here we get all the objects find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() rospy.loginfo("updating scene") idx = 0 # insert objects to the planning scene #TODO so these two for loops yo can hardcode the values. try printing all the params and u will understand for obj in find_result.objects: rospy.loginfo("object number -> %d" %idx) obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) idx += 1 # for grp in obj.grasps: # grp.grasp_pose.pose.position.z = 0.37 # this is just minor adjustments i did mess up with this code. just follwed simple gasp thingy for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z rospy.loginfo("height before => %f" % height) obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 rospy.loginfo("height after => %f" % obj.primitive_poses[0].position.z) # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps rospy.loginfo("no. of grasps detected %d dim => %f" % (len(obj.grasps), obj.object.primitives[0].dimensions[0])) if len(obj.grasps) < 1: continue # check size our object is a 0.05^3 cube if obj.object.primitives[0].dimensions[0] < 0.04 or \ obj.object.primitives[0].dimensions[0] > 0.06 or \ obj.object.primitives[0].dimensions[1] < 0.04 or \ obj.object.primitives[0].dimensions[1] > 0.06 or \ obj.object.primitives[0].dimensions[2] < 0.04 or \ obj.object.primitives[0].dimensions[2] > 0.06: continue rospy.loginfo("###### size: x => %f, y => %f, z => %f" % (obj.object.primitive_poses[0].position.x,obj.object.primitive_poses[0].position.y,obj.object.primitive_poses[0].position.z)) # has to be on table #if obj.object.primitive_poses[0].position.z < 0.5:grasping_client # continue return obj.object, obj.grasps # nothing detected rospy.loginfo("nothing detected") return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m-1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success def tuck(self): # so for each joint i will pass a specific value. first arms are move to pose1 then pose2 then pose3 sequentially joints = ["shoulder_roll_joint", "shoulder_pitch_joint", "shoulder_yaw_joint", "elbow_pitch_joint", "elbow_yaw_joint", "wrist_pitch_joint", "wrist_roll_joint"] pose1 = [1.57079633, 0, 0, 0, 0, 0, 0.0] pose2 = [1.57079633, 0, -1.57079633, 0, -1.57079633, 0, 0.0] pose3 = [1.57079633, 1.57079633, -1.57079633, 0, -1.57079633, 1.57079633, 0.0] while not rospy.is_shutdown(): self.move_group.moveToJointPosition(joints, pose1, 0.02) self.move_group.moveToJointPosition(joints, pose2, 0.02) result = self.move_group.moveToJointPosition(joints, pose3, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 1 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the simple_grasping find_objects action server rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #right_arm.set_named_target('resting') #right_arm.go() # Open the gripper to the neutral position arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(1) while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the simple_grasping grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # Clear the virtual object colors self.scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_id = 'target' target_size = None the_object = None the_object_dist = 0.30 the_object_dist_min = 0.10 count = -1 for obj in find_result.objects: count += 1 self.scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist and d > the_object_dist_min: rospy.loginfo("object is in the working zone") the_object_dist = d the_object = count target_size = [0.02, 0.02, 0.05] target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0 target_pose.pose.position.y = obj.object.primitive_poses[0].position.y target_pose.pose.position.z = 0.04 target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 else: rospy.loginfo("object is not in the working zone") rospy.sleep(1) # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] - height] obj.primitive_poses[0].position.z += -height/2.0 # Add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync self.scene.waitForSync() # Set colors of the table and the object we are grabbing self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange self.scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey self.scene.sendColors() if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object arm.set_support_surface_name(support_surface) # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) rospy.sleep(2) # Open the gripper to the neutral position gripper.set_joint_value_target(self.gripper_neutral) gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('right_up') arm.go() rospy.sleep(2) if args.once: moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0) # Get the gripper posture as a JointTrajectory def make_gripper_posture(self, joint_positions): # Initialize the joint trajectory for the gripper joints t = JointTrajectory() # Set the joint names to the gripper joint names t.header.stamp = rospy.get_rostime() t.joint_names = GRIPPER_JOINT_NAMES # Initialize a joint trajectory point to represent the goal tp = JointTrajectoryPoint() # Assign the trajectory joint positions to the input positions tp.positions = joint_positions # Set the gripper effort tp.effort = GRIPPER_EFFORT tp.time_from_start = rospy.Duration(1.0) # Append the goal point to the trajectory points t.points.append(tp) # Return the joint trajectory return t # Generate a gripper translation in the direction given by vector def make_gripper_translation(self, min_dist, desired, vector): # Initialize the gripper translation object g = GripperTranslation() # Set the direction vector components to the input g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] # The vector is relative to the gripper frame g.direction.header.frame_id = GRIPPER_FRAME # Assign the min and desired distances from the input g.min_distance = min_dist g.desired_distance = desired return g # Generate a list of possible grasps def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately; # grasp_opening should be a bit smaller than target width g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened) g.grasp_posture = self.make_gripper_posture(grasp_opening) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5]) g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading # from arm base to the object to pick (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle for yaw in yaw_vals: for pitch in pitch_vals: # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(pitch) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps # Generate a list of possible place poses def make_places(self, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015] # A list of pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] # A list to hold the places places = [] # Generate a place pose for each angle and translation for pitch in pitch_vals: for dy in y_vals: for dx in x_vals: place.pose.position.x = init_pose.pose.position.x + dx place.pose.position.y = init_pose.pose.position.y + dy # Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from # arm base to the place location (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw = atan2(y, x) # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains # the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the # objects orientation!), so we cancel this transformation. It is applied here: # https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64 # More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577 acobjs = self.scene.get_attached_objects([target_id]) aco_pose = self.get_pose(acobjs[target_id]) if aco_pose is None: rospy.logerr("Attached collision object '%s' not found" % target_id) return None aco_tf = self.pose_to_mat(aco_pose) place_tf = self.pose_to_mat(place.pose) place.pose = self.mat_to_pose(place_tf, aco_tf) rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \ % (aco_pose, place.pose)) # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p) def get_pose(self, co): # We get object's pose from the mesh/primitive poses; try first with the meshes if isinstance(co, CollisionObject): if co.mesh_poses: return co.mesh_poses[0] elif co.primitive_poses: return co.primitive_poses[0] else: rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id) return None elif isinstance(co, AttachedCollisionObject): if co.object.mesh_poses: return co.object.mesh_poses[0] elif co.object.primitive_poses: return co.object.primitive_poses[0] else: rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id) return None else: rospy.logerr("Input parameter is not a collision object") return None def pose_to_mat(self, pose): '''Convert a pose message to a 4x4 numpy matrix. Args: pose (geometry_msgs.msg.Pose): Pose rospy message class. Returns: mat (numpy.matrix): 4x4 numpy matrix ''' quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T mat = np.matrix(quaternion_matrix(quat)) mat[0:3, 3] = pos return mat def mat_to_pose(self, mat, transform=None): '''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform. Args: mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform. transform (numpy.ndarray): Optional 4x4 array representing additional transform Returns: pose (geometry_msgs.msg.Pose): Pose message representing transform. ''' if transform != None: mat = np.dot(mat, transform) pose = Pose() pose.position.x = mat[0,3] pose.position.y = mat[1,3] pose.position.z = mat[2,3] quat = quaternion_from_matrix(mat) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose
def main(): print('We are running the main of arm_obstable_demo') rospy.init_node('arm_obstacle_demo') wait_for_time() print( "If planning scene fails to init, make sure you have launched move_group.launch" ) planning_scene = PlanningSceneInterface("base_link") # Create table object planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider object planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.2 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) # Before moving to the first pose planning_scene.removeAttachedObject('tray') pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': False } error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray')
def main(): # Create table obstacle rospy.init_node('arm_obstacle_demo') planning_scene = PlanningSceneInterface('base_link') planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # planning_scene.removeCollisionObject('divider') # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) #Orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 # move to pose args kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray') # Was working without this but now its needed planning_scene.clear()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects rospy.loginfo("get new grasps") goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.removeCollisionObject("my_front_ground") self.scene.removeCollisionObject("my_back_ground") self.scene.removeCollisionObject("my_right_ground") self.scene.removeCollisionObject("my_left_ground") self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TODO: ADD BOX self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z # added + .1 obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def zero(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() obj_cts = 0 def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True print " find goal" self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) print " find client" find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): print "the object %s should be removed" %(name) self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): print "the object %s should be removed" %(name) self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene objects = list() idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx print "-----------------------" print obj.object.primitive_poses[0] print " x: %d" %(obj.object.primitive_poses[0].position.x) # if obj.object.primitive_poses[0].position.y > 0.0 self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service = False) # def addSolidPrimitive (self, name, solid, pose, use_service=True) print "1, %d" %(idx) if obj.object.primitive_poses[0].position.x < 1.25: objects.append([obj, obj.object.primitive_poses[0].position.z]) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view print "2," height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], use_service = True ) self.scene.waitForSync() # store for grasping #self.objects = find_result.objects self.surfaces = find_result.support_surfaces # store graspable objects by Z objects.sort(key=lambda object: object[1]) objects.reverse() self.objects = [object[0] for object in objects] rospy.loginfo("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") rospy.loginfo("number of objects...:::::::" + str(len(objects))) #for object in objects: # print(object[0].object.name, object[1]) #exit(-1) def getGraspableObject(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: rospy.loginfo("must more than one object") continue # check size if obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25: continue # has to located in +y if obj.object.primitive_poses[0].position.y < 0.0: print "has to located in +y" continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: print "z has to larger than 0.5 " continue rospy.loginfo("object pose:") print obj.object.primitive_poses[0], obj.object.primitives[0] return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m-1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def stow(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def intermediate_stow(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class GripperInterfaceClient: def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print "==========", goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0] + goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive( obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False ) # Try mesh representation # idx = -1 # for obj in find_result.objects: # idx += 1 # obj.object.name = "object%d"%idx # self.scene.addMesh(obj.object.name, # obj.object.mesh_poses[0], # obj.object.meshes[0], # wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0] + 0.1, 2.5, # wider obj.primitives[0].dimensions[2] + height, ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size # if obj.object.primitives[0].dimensions[0] < 0.03 or \ # obj.object.primitives[0].dimensions[0] > 0.04 or \ # obj.object.primitives[0].dimensions[0] < 0.07 or \ # obj.object.primitives[0].dimensions[0] > 0.09 or \ # obj.object.primitives[2].dimensions[2] < 0.19 or \ # obj.object.primitives[2].dimensions[2] > 0.20: # continue # has to be on table # if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def plan_pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", retries=2, scene=self.scene, ) # self.pick_result = pick_result if success: return pick_result.trajectory_stages else: rospy.loginfo("Planning Failed.") return None
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") grasp_topic = "fetch_grasp_planner_node/plan" rospy.loginfo("Waiting for %s..." % grasp_topic) self.grasp_planner_client = actionlib.SimpleActionClient( grasp_topic, GraspPlanningAction) wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5)) if (wait): print("successfully connected to grasp server") else: print("failed to connect to grasp server") rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose, self.apple_pose_callback) self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg", String, queue_size=10) self.object = Object() self.grasps = Grasp() self.ready_for_grasp = False self.pick_place_finished = False self.first_time_grasp = True self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0) self.tuck() def apple_pose_callback(self, message): if self.pick_place_finished or self.first_time_grasp: self.first_time_grasp = False self.pick_place_finished = False print("apple_pose_callback") apple = SolidPrimitive() apple.type = SolidPrimitive.SPHERE apple.dimensions = [0.03, 0.03, 0.03] self.object.name = "apple" self.object.primitives = [apple] self.object.primitive_poses = [message] # add stamp and frame self.object.header.stamp = rospy.Time.now() self.object.header.frame_id = "base_link" goal = GraspPlanningGoal() goal.object = self.object self.grasp_planner_client.send_goal(goal) self.grasp_planner_client.wait_for_result(rospy.Duration(5.0)) grasp_planner_result = self.grasp_planner_client.get_result() self.grasps = grasp_planner_result.grasps self.ready_for_grasp = True def updateScene(self): # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0) self.scene.addSolidPrimitive(self.object.name, self.object.primitives[0], self.object.primitive_poses[0]) self.scene.waitForSync() def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def stow(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def intermediate_stow(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
gripper.wait_for_result(rospy.Duration(1.0)) print "item: " + str(item_translation) scene.addCube("item", 0.05, item_translation[0], item_translation[1], item_translation[2]) p.position.x = item_translation[0] - 0.01 - 0.06 p.position.y = item_translation[1] p.position.z = item_translation[2] + 0.04 + 0.14 p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0)) arm.set_pose_target(p) arm.go(True) #os.system("rosservice call clear_octomap") gripper_goal.command.position = 0 gripper.send_goal(gripper_goal) gripper.wait_for_result(rospy.Duration(2.0)) scene.removeAttachedObject("item") clear_octomap() p.position.x = 0.00 p.position.y = -0.25 p.position.z = 0.75 - .1 p.orientation = Quaternion(*quaternion_from_euler(0, -1.5, -1.5)) arm.set_pose_target(p) arm.go(True) break
class GripperInterfaceClient: def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() rospy.loginfo("...connected.") self.scene = PlanningSceneInterface("base_link") self.attached_object_publisher = rospy.Publisher( "attached_collision_object", AttachedCollisionObject, queue_size=10 ) self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() self.debug_index = -1 self.graspables = [] def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print "==========", goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0] + goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, wait=False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, wait=False) rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition. self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive( obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False ) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0] + 0.1, 1.5, # wider obj.primitives[0].dimensions[2] + height, ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size # if obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07: # continue # has to be on table # if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def set_target_object(self, primitive_type, target_pose, dimensions): primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4} self.sought_object = Object() primitive = SolidPrimitive() primitive.type = primitive_types[primitive_type.upper()] primitive.dimensions = dimensions self.sought_object.primitives.append(primitive) p = Pose() p.position.x = target_pose[0] p.position.y = target_pose[1] p.position.z = target_pose[2] quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5]) p.orientation.x = quaternion[0] p.orientation.y = quaternion[1] p.orientation.z = quaternion[2] p.orientation.w = quaternion[3] self.sought_object.primitive_poses.append(p) def find_graspable_object(self, tolerance=0.01): self.updateScene() self.current_grasping_target = None for obj in self.objects: # Must have grasps if len(obj.grasps) < 1: continue # Compare to sought object detected_object_dimensions = np.array(obj.object.primitives[0].dimensions) sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions) try: difference = detected_object_dimensions - sought_object_dimensions print "===DEBUG: Difference: ", difference mag_diff = np.linalg.norm(difference) print "===DEBUG: mag_diff: ", mag_diff if mag_diff <= tolerance: self.current_grasping_target = obj tolerance = mag_diff print "===DEBUG: Tolerance: ", tolerance else: rospy.loginfo("Object dimensions do not match. Trying another object...") except: rospy.loginfo("Object geometries do not match. Trying another object...") if self.current_grasping_target is None: tolerance += 0.01 self.find_graspable_object(tolerance=tolerance) # Nothing detected def computeGraspToPickMatrix(self): pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion) translation = [pick_translation_distance, 0, 0] rotation_matrix[[0, 1, 2], 3] = translation pick_to_grasp_matrix = np.mat(rotation_matrix) grasp_to_pick_matrix = pick_to_grasp_matrix.getI() return grasp_to_pick_matrix def computePlaceToBaseMatrix(self, place): place_quaternion = place[3:] rotation_matrix = quaternion_matrix(place_quaternion) translation = place[0:3] rotation_matrix[[0, 1, 2], 3] = translation place_to_base_matrix = rotation_matrix return place_to_base_matrix def broadcastCurrentGraspingTargetTransform(self): pose = self.current_grasping_target.object.primitive_poses[0] br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "base_link" t.child_frame_id = "current_grasping_target" t.transform.translation.x = pose.position.x t.transform.translation.y = pose.position.y t.transform.translation.z = pose.position.z t.transform.rotation.x = pose.orientation.x t.transform.rotation.y = pose.orientation.y t.transform.rotation.z = pose.orientation.z t.transform.rotation.w = pose.orientation.w br.sendTransform(t) def plan_pick(self): success, pick_result = self.pickplace.pick_with_retry( self.current_grasping_target.object.name, self.current_grasping_target.grasps, support_name=self.current_grasping_target.object.support_surface, scene=self.scene, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", ) self.pick_result = pick_result print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects() if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return pick_result.trajectory_stages else: rospy.loginfo("Plan rejected. Getting new grasps for replan...") else: rospy.logwarn("Planning failed. Getting new grasps for replan...") self.find_graspable_object() return self.plan_pick() def plan_place(self, desired_pose): places = list() ps = PoseStamped() # ps.pose = self.current_grasping_target.object.primitive_poses[0] ps.header.frame_id = self.current_grasping_target.object.header.frame_id grasp_to_pick_matrix = self.computeGraspToPickMatrix() place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose) grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix position_vector = grasp2_to_place_matrix[0:-1, 3] quaternion = quaternion_from_matrix(grasp2_to_place_matrix) position_array = position_vector.getA1() l = PlaceLocation() direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector l.place_pose.header.frame_id = ps.header.frame_id l.place_pose.pose.position.x = position_array[0] l.place_pose.pose.position.y = position_array[1] l.place_pose.pose.position.z = position_array[2] l.place_pose.pose.orientation.x = quaternion[0] l.place_pose.pose.orientation.y = quaternion[1] l.place_pose.pose.orientation.z = quaternion[2] l.place_pose.pose.orientation.w = quaternion[3] # copy the posture, approach and retreat from the grasp used approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach) approach.desired_distance /= 2.0 l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in roll direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) print "DEBUG: Places: ", places[0] success, place_result = self.pickplace.place_with_retry( self.current_grasping_target.object.name, places, scene=self.scene, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", ) if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return place_result.trajectory_stages else: rospy.loginfo("Plan rejected. Replanning...") else: rospy.logwarn("Planning failed. Replanning...") desired_pose[2] += 0.05 return self.plan_place(desired_pose) def recognizeAttachedObject(self): print "========= Recognizing attached object" name = self.current_grasping_target.object.name size_x = self.current_grasping_target.object.primitives[0].dimensions[0] size_y = self.current_grasping_target.object.primitives[0].dimensions[1] size_z = self.current_grasping_target.object.primitives[0].dimensions[2] (x, y, z) = (0.03, 0.0, 0.0) link_name = "gripper_link" touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"] detach_posture = None weight = 0.0 wait = True """ object_x = self.current_grasping_target.object.primitive_poses[0].position.x object_y = self.current_grasping_target.object.primitive_poses[0].position.y object_z = self.current_grasping_target.object.primitive_poses[0].position.z pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation grasp_position = self.pick_result.grasp.grasp_pose.pose.position pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion) grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array) displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]])) print displacement attached_location = list() attached_location.append(object_x - displacement[0,0]) attached_location.append(object_y - displacement[1,0]) attached_location.append(object_z - displacement[2,0]) print attached_location quaternion = Quaternion() quaternion.x = pick_to_grasp_quaternion[0] quaternion.y = pick_to_grasp_quaternion[1] quaternion.z = pick_to_grasp_quaternion[2] quaternion.w = pick_to_grasp_quaternion[3] pose = Pose() pose.position.x = attached_location[0] pose.position.y = attached_location[1] pose.position.z = attached_location[2] pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name, self.current_grasping_target.object.primitives[0], pose) attached_collision_object = self.scene.makeAttached("gripper_link", collision_object, touch_links, detach_posture, 0.0) self.attached_object_publisher.publish(attached_collision_object) """ self.scene.attachBox( name, size_x, size_y, size_z, x, y, z, link_name, touch_links=touch_links, detach_posture=detach_posture, weight=weight, wait=wait, ) def removeCollisionObjects(self): collision_object_names = self.scene.getKnownCollisionObjects() print self.current_grasping_target.object.name print collision_object_names raw_input("press ENTER") x = self.scene.getKnownAttachedObjects() print x raw_input("press Enter") for name in collision_object_names: if name != self.current_grasping_target.object.name: self.scene.removeCollisionObject(name, wait=False)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface("base_link") # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the UBR-1 find_objects action server rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Begin the main perception and pick-and-place loop while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the UBR-1 grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in scene.getKnownCollisionObjects(): scene.removeCollisionObject(name, False) for name in scene.getKnownAttachedObjects(): scene.removeAttachedObject(name, False) scene.waitForSync() # Clear the virtual object colors scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_size = None the_object = None the_object_dist = 1.0 count = -1 # Cycle through all detected objects and keep the nearest one for obj in find_result.objects: count += 1 scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x - args.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist: the_object_dist = d the_object = count # Get the size of the target target_size = obj.object.primitives[0].dimensions # Set the target pose target_pose.pose = obj.object.primitive_poses[0] # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # Add to scene scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync scene.waitForSync() # Set colors of the table and the object we are grabbing scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey scene.sendColors() # Skip pick-and-place if we are just detecting objects if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object right_arm.set_support_surface_name(support_surface) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = target_pose.pose.position.x place_pose.pose.position.y = 0.03 place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") continue # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Set the start state to the current state #right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(2) # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) # Give the servos a rest arbotix_relax_all_servos() rospy.sleep(2) if args.once: # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() # ####################################################################### # # test fetch_grasp_planner_node/plan with pose subcriber # # use fetch_grasp_planner_node/plan with fetch_grasp_planner_node/plan # grasp_topic = "fetch_grasp_planner_node/plan" # rospy.loginfo("Waiting for %s..." % grasp_topic) # self.grasp_planner_client = actionlib.SimpleActionClient(grasp_topic, GraspPlanningAction) # self.grasp_planner_client.wait_for_server() # ####################################################################### # ##################################################################### # # test fetch_grasp_planner_node/plan with pose subcriber # rospy.Subscriber("perception/apple_pose", Pose, self.apple_pose_callback) # self.object = Object() # self.grasps = Grasp() # def apple_pose_callback(self, message): # apple = SolidPrimitive() # apple.type = SolidPrimitive.SPHERE # apple.dimensions = 0.04 # self.object.primitives = apple # self.object.primitive_poses = message # # add stamp and frame # self.object.header.stamp = rospy.Time.now() # self.object.header.frame_id = message.frame_id # goal = GraspPlanningGoal() # goal.object = self.object # self.grasp_planner_client.send_goal(goal) # self.grasp_planner_client.wait_for_result(rospy.Duration(5.0)) # grasp_planner_result = self.grasp_planner_client.get_result() # self.grasps = grasp_planner_result.grasps # #################################################################### def updateScene(self): # #################################################################### # # use fetch_grasp_planner_node/plan with fetch_grasp_planner_node/plan # # find objects # goal = FindGraspableObjectsGoal() # goal.plan_grasps = False # self.find_client.send_goal(goal) # self.find_client.wait_for_result(rospy.Duration(5.0)) # find_result = self.find_client.get_result() # for obj in find_result.objects: # goal = GraspPlanningGoal() # goal.object = obj.object # self.grasp_planner_client.send_goal(goal) # self.grasp_planner_client.wait_for_result(rospy.Duration(5.0)) # grasp_planner_result = self.grasp_planner_client.get_result() # obj.grasps = grasp_planner_result.grasps # #################################################################### # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene objects = list() idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service=False) if obj.object.primitive_poses[0].position.x < 0.85: objects.append([obj, obj.object.primitive_poses[0].position.z]) print("object ", idx, " frame_id = ", obj.object.header.frame_id) print("object ", idx, " pose = ", obj.object.primitive_poses[0]) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], use_service=False) self.scene.waitForSync() # store for grasping #self.objects = find_result.objects self.surfaces = find_result.support_surfaces # store graspable objects by Z objects.sort(key=lambda object: object[1]) objects.reverse() self.objects = [object[0] for object in objects] #for object in objects: # print(object[0].object.name, object[1]) #exit(-1) def getGraspableObject(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue print obj.object.primitive_poses[0], obj.object.primitives[0] return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def stow(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def intermediate_stow(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects rospy.loginfo("get new grasps") goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.removeCollisionObject("my_front_ground") self.scene.removeCollisionObject("my_back_ground") self.scene.removeCollisionObject("my_right_ground") self.scene.removeCollisionObject("my_left_ground") self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TODO: ADD BOX self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z # added + .1 obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def zero(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() # argv = rospy.myargv() planning_scene = PlanningSceneInterface(frame='base_link') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray') # Create table obstacle table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) # add orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 # # set torso to max height torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) arm = robot_api.Arm() # register shutdown method def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 20, 'num_planning_attempts': 10, 'replan': True } error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') # attach an object if pose1 is successfully reached frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() # close the gripper # gripper = robot_api.Gripper() # gripper.close() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') # At the end of the program planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray')
gripper_goal.command.position = 0.15 gripper.send_goal(gripper_goal) gripper.wait_for_result(rospy.Duration(1.0)) print('Item: ' + str(item_translation)) scene.addCube('item', 0.05, item_translation[0], item_translation[1], item_translation[2]) p.position.x = item_translation[0] - 0.01 - 0.06 p.position.y = item_translation[1] p.position.z = item_translation[2] + 0.04 + 0.14 p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0)) arm.set_pose_target(p) arm.go(True) gripper_goal.command.position = 0 gripper.send_goal(gripper_goal) gripper.wait_for_result(rospy.Duration(2.0)) scene.removeAttachedObject('item') clear_octomap() p.position.x = 0.00 p.position.y = -0.25 p.position.z = 0.75 - 0.1 p.orientation = Quaternion(*quaternion_from_euler(0, -1.5, -1.5)) arm.set_pose_target(p) arm.go(True) break
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the postrection m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class GraspingClient(GripperCommandClient): def __init__(self): # initialize GripperCommandClient GripperCommandClient.__init__(self) # initialize Moveit Scene self.scene = PlanningSceneInterface("base_link") self.scene2 = moveit_commander.PlanningSceneInterface() find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() # creat target pose publisher self.marker_pub = rospy.Publisher("/TargetMarker", MarkerArray, queue_size=1) # creat basket pos publisher self.marker_pub2 = rospy.Publisher("/basket", MarkerArray, queue_size=1) # creat publisher for requesting grasp pose self.request_cloud_client = rospy.Publisher("/request_cloud", Int32, queue_size=1) # instantiate a RobotCommander self.robot = moveit_commander.RobotCommander() # instantiate two MoveGroupCommander self.group = moveit_commander.MoveGroupCommander("arm") # basket's parameters self.basket_pos = 'left' self.basket_found = False self.basket_pos_x = 0 self.basket_pos_y = 0 self.basket_search_t = 0 def updateScene(self): # detect objects goal = FindGraspableObjectsGoal() goal.plan_grasps = False self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() #remove original objects self.clear_scene() # all the objects in scene self.objects = list() # height is used for grasping selection self.height = -1 # object number object_num = -1 # add support surface to scene for obj in find_result.support_surfaces: # extend surface to floor h = obj.primitive_poses[0].position.z if (h + obj.primitives[0].dimensions[2] / 2.0) > self.height: self.height = h + obj.primitives[0].dimensions[2] / 2.0 self.table = obj obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0] + 0.02, obj.primitives[0].dimensions[1] + 0.02, obj.primitives[0].dimensions[2] + h - 0.02 ] obj.primitive_poses[0].position.z += -h / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) # add objects Solid to scene for obj in find_result.objects: object_num += 1 obj.object.name = "object%d" % object_num self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False) if obj.object.primitive_poses[0].position.z > self.height: self.objects.append([ obj, numpy.array([ obj.object.primitive_poses[0].position.x, obj.object.primitive_poses[0].position.y, obj.object.primitive_poses[0].position.z ]), obj.object.primitive_poses[0].position.x ]) # localize basket if obj.object.primitive_poses[0].position.y > self.table.primitive_poses[0].position.y + self.table.primitives[0].dimensions[1] / 2.0 + 0.1 or \ obj.object.primitive_poses[0].position.y < self.table.primitive_poses[0].position.y - self.table.primitives[0].dimensions[1] / 2.0 - 0.1: if obj.object.primitives[0].dimensions[ 1] > 0.20 and obj.object.primitive_poses[ 0].position.z > 0.3 and not self.basket_found: if self.basket_search_t == 1: self.basket_pos_x = obj.object.primitive_poses[ 0].position.x self.basket_pos_y = obj.object.primitive_poses[ 0].position.y self.basket_search_t += 1 elif self.basket_search_t == 2: if self.basket_pos_x - obj.object.primitive_poses[0].position.x < 0.04 and \ self.basket_pos_y - obj.object.primitive_poses[0].position.y < 0.04: self.basket_found = True if obj.object.primitive_poses[0].position.y < 0: self.basket_pos = 'right' self.basket = obj self.marker_pub2.publish(self.basket_marker()) rospy.loginfo("Basket is found at %s..." % self.basket_pos) else: self.basket_search_t -= 1 if self.basket_search_t == 0: self.basket_search_t += 1 self.scene.waitForSync() def clear_scene(self): for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() def clear_target_object(self, pose): if len(self.objects) == 1: self.scene.removeCollisionObject(self.objects[0][0].object.name) self.tname = self.objects[0][0].object.name else: dist = float('inf') vec = numpy.array([ pose[0].center.x + 0.33 * pose[0].approach.x, pose[0].center.y + 0.33 * pose[0].approach.y, pose[0].center.z + 0.33 * pose[0].approach.z ]) for obj in self.objects: temp = numpy.linalg.norm(obj[1] - vec) if temp < dist: dist = temp self.tname = obj[0].object.name self.scene.removeCollisionObject(self.tname, False) def readd_target_object(self): for obj in self.objects: if obj[0].object.name == self.tname: self.scene.addSolidPrimitive(obj[0].object.name, obj[0].object.primitives[0], obj[0].object.primitive_poses[0], wait=False) def request_grasp_poses(self, mode=None): # possible grasp poses self.grasp_pose = list() rospy.wait_for_service('grasp_pose') res = rospy.ServiceProxy('grasp_pose', grasp_pose) grasps_obtained = False while not grasps_obtained: try: self.grasp_pose[:] = [] rospy.loginfo("Try to find feasible grasp poses ...") # request PointCloud data for agile grasp package self.request_cloud_client.publish(1) grasps = res() if mode == 'cluster': self.pose_selection_cluster(grasps.grasps) else: self.pose_selection_general(grasps.grasps) if len(self.grasp_pose) > 0: self.TargetGraspPoses, self.Rots = self.creat_target_grasp_pose( self.grasp_pose) if self.TargetGraspPoses != None: grasps_obtained = True except rospy.ServiceException, e: rospy.loginfo("Service call failed: %s" % e) continue
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True) self.move_group = MoveGroupInterface("left_arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server(rospy.Duration(15.0)) self.cubes = [] self.tf_listener = TransformListener() # self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction) # self.gripper_client.wait_for_server() # rospy.loginfo("...connected.") rospy.sleep(2.0) def updateScene(self, remove_collision = False): if remove_collision: for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() return # find objects self.cubes = [] goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(15.0)) find_result = self.find_client.get_result() # rospy.loginfo(find_result) # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 print(find_result.objects) # print(find_result.support_surfaces) # for obj in find_result.objects: # idx += 1 # print(idx) # obj.object.name = "object%d" % idx # self.scene.addSolidPrimitive(obj.object.name, # obj.object.primitives[0], # obj.object.primitive_poses[0], # wait = False) # self.cubes.append(obj.object.primitive_poses[0]) # # for obj in find_result.support_surfaces: # # extend surface to floor, and make wider since we have narrow field of view # height = obj.primitive_poses[0].position.z # obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], # 1.5, # wider # obj.primitives[0].dimensions[2] + height] # obj.primitive_poses[0].position.z += -height / 2.0 # # # add to scene # self.scene.addSolidPrimitive(obj.name, # obj.primitives[0], # obj.primitive_poses[0], # wait = False) # # self.scene.waitForSync() # # # store for grasping # self.objects = find_result.objects # self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue return obj.object, obj.grasps # nothing detected return None, None def get_sequence(self, seq_list): start = [self.objects[seq_list[0]].object, self.objects[seq_list[0]].grasps] or [None, None] target = [self.objects[seq_list[1]].object, self.objects[seq_list[1]].grasps] or [None, None] res = {'start': start, 'target': target} return res def get_transform_pose(self, pose): point = PoseStamped() point.header.frame_id = 'base_link' point.header.stamp = rospy.Time() point.pose = pose return self.tf_listener.transformPose('/map', point).pose.position def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, retries = 5, support_name = block.support_surface, scene = self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene = self.scene, retries = 5) # print(success) return success def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] # pose =[ 1.58, -0.056, -0.01, -1.31, -0.178, -0.13, 0.16] gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 0.0 gripper_goal.command.position = 0.09 self.gripper_client.send_goal(gripper_goal) self.gripper_client.wait_for_result(rospy.Duration(5)) start = rospy.Time.now() while rospy.Time.now() - start <= rospy.Duration(10.0): # rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return return def gripper_opening(self, opening = 0.09): gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 0.0 gripper_goal.command.position = opening self.gripper_client.send_goal(gripper_goal) self.gripper_client.wait_for_result(rospy.Duration(5))
if __name__ == "__main__": parser = argparse.ArgumentParser( description="Remove objects from the MoveIt planning scene.") parser.add_argument("name", nargs="?", help="Name of object to remove") parser.add_argument("--all", help="Remove all objects.", action="store_true") parser.add_argument("--attached", help="Remove an attached object.", action="store_true") args = parser.parse_args() if args.all: rospy.init_node("remove_objects") scene = PlanningSceneInterface("base_link") for name in scene.getKnownCollisionObjects(): print("Removing %s" % name) scene.removeCollisionObject(name, use_service=False) scene.waitForSync() elif args.name: rospy.init_node("remove_objects") scene = PlanningSceneInterface("base_link") print("Removing %s" % args.name) if args.attached: scene.removeAttachedObject(args.name) else: scene.removeCollisionObject(args.name) else: parser.print_help()
class Grasping(object): def __init__(self): self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.scene = PlanningSceneInterface("base_link") self.move_group = MoveGroupInterface("arm", "base_link") find_objects = "basic_grasping_perception/find_objects" self.find_client = actionlib.SimpleActionClient( find_objects, FindGraspableObjectsAction) self.find_client.wait_for_server() def pickup(self, block, grasps): rospy.sleep(1) success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success #swaps two blocks positions. def swapBlockPos(self, block1Pos, block2Pos): #intermediate point for movement self.armIntermediatePose() #intermediate positon on the table, used for short term storage of the first block manipulated in the sorting. posIntermediate = np.array([0.725, 0]) self.armIntermediatePose() # Get block to pick while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(block1Pos) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") #self.tuck() #Place the block while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, posIntermediate): break rospy.logwarn("Placing failed.") #place block 2 in block 1's self.armIntermediatePose() # Get block to pick while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(block2Pos) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, block1Pos): break rospy.logwarn("Placing failed.") #place block1 in block 2's spot self.armIntermediatePose() while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(posIntermediate) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") #self.tuck() self.armIntermediatePose() while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, block2Pos): break rospy.logwarn("Placing failed.") return def place(self, block, pose_stamped, placePos): rospy.sleep(1) #creates a list of place positons for the block, with a specified x, y, and z. places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = "base_link" l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat #this x and y value are input as placePos through the function call. l.place_pose.pose.position.x = placePos[0] l.place_pose.pose.position.y = placePos[1] places.append(copy.deepcopy(l)) #success = self.pickplace.place_with_retry(block.name, places, scene = self.scene) #return success ## create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success def updateScene(self): rospy.sleep(1) # find objectsw goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service=False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], use_service=True) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def getGraspableObject(self, pos): graspable = None for obj in self.objects: if len(obj.grasps) < 1: continue if (obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[1] < 0.05 or \ obj.object.primitives[0].dimensions[1] > 0.07 or \ obj.object.primitives[0].dimensions[2] < 0.05 or \ obj.object.primitives[0].dimensions[2] > 0.07): continue if (obj.object.primitive_poses[0].position.z < 0.3) or \ (obj.object.primitive_poses[0].position.y > pos[1]+0.05) or \ (obj.object.primitive_poses[0].position.y < pos[1]-0.05) or \ (obj.object.primitive_poses[0].position.x > pos[0]+0.05) or \ (obj.object.primitive_poses[0].position.x < pos[0]-0.05): continue return obj.object, obj.grasps return None, None def armToXYZ(self, x, y, z): rospy.sleep(1) #new pose_stamped of the end effector that moves the arm out of the way of the vision for planning. intermediatePose = PoseStamped() intermediatePose.header.frame_id = 'base_link' #position intermediatePose.pose.position.x = x intermediatePose.pose.position.y = y intermediatePose.pose.position.z = z #quaternion for the end position intermediatePose.pose.orientation.w = 1 while not rospy.is_shutdown(): result = self.move_group.moveToPose(intermediatePose, "wrist_roll_link") if result.error_code.val == MoveItErrorCodes.SUCCESS: return def armIntermediatePose(self): self.armToXYZ(0.1, -0.7, 0.9) def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class GripperInterfaceClient(): def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() rospy.loginfo("...connected.") self.scene = PlanningSceneInterface("base_link") self.attached_object_publisher = rospy.Publisher('attached_collision_object', AttachedCollisionObject, queue_size = 10) self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() self.debug_index = -1 self.graspables = [] def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print '==========', goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0]+goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, wait=False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, wait=False) rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition. self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1, 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size #if obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07: # continue # has to be on table #if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def set_target_object(self, primitive_type, target_pose, dimensions): primitive_types = {"BOX":1, "SPHERE":2, "CYLINDER":3, "CONE":4} self.sought_object = Object() primitive = SolidPrimitive() primitive.type = primitive_types[primitive_type.upper()] primitive.dimensions = dimensions self.sought_object.primitives.append(primitive) p = Pose() p.position.x = target_pose[0] p.position.y = target_pose[1] p.position.z = target_pose[2] quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5]) p.orientation.x = quaternion[0] p.orientation.y = quaternion[1] p.orientation.z = quaternion[2] p.orientation.w = quaternion[3] self.sought_object.primitive_poses.append(p) def find_graspable_object(self, tolerance=0.01): self.updateScene() self.current_grasping_target = None for obj in self.objects: # Must have grasps if len(obj.grasps) < 1: continue # Compare to sought object detected_object_dimensions = np.array(obj.object.primitives[0].dimensions) sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions) try: difference = detected_object_dimensions - sought_object_dimensions print "===DEBUG: Difference: " , difference mag_diff = np.linalg.norm(difference) print "===DEBUG: mag_diff: ", mag_diff if mag_diff <= tolerance: self.current_grasping_target = obj tolerance = mag_diff print "===DEBUG: Tolerance: ", tolerance else: rospy.loginfo("Object dimensions do not match. Trying another object...") except: rospy.loginfo("Object geometries do not match. Trying another object...") if self.current_grasping_target is None: tolerance += .01 self.find_graspable_object(tolerance = tolerance) # Nothing detected def computeGraspToPickMatrix(self): pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion) translation = [pick_translation_distance, 0, 0] rotation_matrix[[0,1,2],3] = translation pick_to_grasp_matrix = np.mat(rotation_matrix) grasp_to_pick_matrix = pick_to_grasp_matrix.getI() return grasp_to_pick_matrix def computePlaceToBaseMatrix(self, place): place_quaternion = place[3:] rotation_matrix = quaternion_matrix(place_quaternion) translation = place[0:3] rotation_matrix[[0,1,2], 3] = translation place_to_base_matrix = rotation_matrix return place_to_base_matrix def broadcastCurrentGraspingTargetTransform(self): pose = self.current_grasping_target.object.primitive_poses[0] br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "base_link" t.child_frame_id = "current_grasping_target" t.transform.translation.x = pose.position.x t.transform.translation.y = pose.position.y t.transform.translation.z = pose.position.z t.transform.rotation.x = pose.orientation.x t.transform.rotation.y = pose.orientation.y t.transform.rotation.z = pose.orientation.z t.transform.rotation.w = pose.orientation.w br.sendTransform(t) def plan_pick(self): success, pick_result = self.pickplace.pick_with_retry(self.current_grasping_target.object.name, self.current_grasping_target.grasps, support_name = self.current_grasping_target.object.support_surface, scene=self.scene, allow_gripper_support_collision=False, planner_id='PRMkConfigDefault') self.pick_result = pick_result print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects() if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return pick_result.trajectory_stages else: rospy.loginfo("Plan rejected. Getting new grasps for replan...") else: rospy.logwarn("Planning failed. Getting new grasps for replan...") self.find_graspable_object() return self.plan_pick() def plan_place(self, desired_pose): places = list() ps = PoseStamped() #ps.pose = self.current_grasping_target.object.primitive_poses[0] ps.header.frame_id = self.current_grasping_target.object.header.frame_id grasp_to_pick_matrix = self.computeGraspToPickMatrix() place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose) grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix position_vector = grasp2_to_place_matrix[0:-1,3] quaternion = quaternion_from_matrix(grasp2_to_place_matrix) position_array = position_vector.getA1() l = PlaceLocation() direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector l.place_pose.header.frame_id = ps.header.frame_id l.place_pose.pose.position.x = position_array[0] l.place_pose.pose.position.y = position_array[1] l.place_pose.pose.position.z = position_array[2] l.place_pose.pose.orientation.x = quaternion[0] l.place_pose.pose.orientation.y = quaternion[1] l.place_pose.pose.orientation.z = quaternion[2] l.place_pose.pose.orientation.w = quaternion[3] # copy the posture, approach and retreat from the grasp used approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach) approach.desired_distance /= 2.0 l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in roll direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) print "DEBUG: Places: ", places[0] success, place_result = self.pickplace.place_with_retry(self.current_grasping_target.object.name, places, scene=self.scene, allow_gripper_support_collision=False, planner_id='PRMkConfigDefault') if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return place_result.trajectory_stages else: rospy.loginfo("Plan rejected. Replanning...") else: rospy.logwarn("Planning failed. Replanning...") desired_pose[2] += 0.05 return self.plan_place(desired_pose) def recognizeAttachedObject(self): print "========= Recognizing attached object" name = self.current_grasping_target.object.name size_x = self.current_grasping_target.object.primitives[0].dimensions[0] size_y = self.current_grasping_target.object.primitives[0].dimensions[1] size_z = self.current_grasping_target.object.primitives[0].dimensions[2] (x, y, z) = (0.03, 0.0, 0.0) link_name = "gripper_link" touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"] detach_posture = None weight = 0.0 wait = True """ object_x = self.current_grasping_target.object.primitive_poses[0].position.x object_y = self.current_grasping_target.object.primitive_poses[0].position.y object_z = self.current_grasping_target.object.primitive_poses[0].position.z pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation grasp_position = self.pick_result.grasp.grasp_pose.pose.position pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion) grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array) displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]])) print displacement attached_location = list() attached_location.append(object_x - displacement[0,0]) attached_location.append(object_y - displacement[1,0]) attached_location.append(object_z - displacement[2,0]) print attached_location quaternion = Quaternion() quaternion.x = pick_to_grasp_quaternion[0] quaternion.y = pick_to_grasp_quaternion[1] quaternion.z = pick_to_grasp_quaternion[2] quaternion.w = pick_to_grasp_quaternion[3] pose = Pose() pose.position.x = attached_location[0] pose.position.y = attached_location[1] pose.position.z = attached_location[2] pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name, self.current_grasping_target.object.primitives[0], pose) attached_collision_object = self.scene.makeAttached("gripper_link", collision_object, touch_links, detach_posture, 0.0) self.attached_object_publisher.publish(attached_collision_object) """ self.scene.attachBox(name, size_x, size_y, size_z, x, y, z, link_name, touch_links=touch_links, detach_posture=detach_posture, weight=weight, wait=wait) def removeCollisionObjects(self): collision_object_names = self.scene.getKnownCollisionObjects() print self.current_grasping_target.object.name print collision_object_names raw_input("press ENTER") x = self.scene.getKnownAttachedObjects() print x raw_input("press Enter") for name in collision_object_names: if name != self.current_grasping_target.object.name: self.scene.removeCollisionObject(name, wait=False)
class World: def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) """Calls a method to display the collision objects. """ def create_planning_scene(self): print "[INFO] Create_planning_scene" self.display_towers_and_disks() """This method collects all needed information and publish them to other methods. """ def display_towers_and_disks(self): print "[INFO] Display towers and disks" for tower in self.tower_array: # call method to publish new tower self.publish_scene(tower.get_position(), tower) # set color by name self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0) # publish color self.planning_scene_interface.sendColors() for disk in self.disk_array: self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors() # wait for sync after publishing collision objects self.planning_scene_interface.waitForSync(5.0) rospy.sleep(5) """This method creates a box or a cylinder with methods of the planning scene interface. :param position: the position of the new collision object :param tower: the tower object :param disk: the disk object """ def publish_scene(self, position, tower=None, disk=None): if tower is not None: self.planning_scene_interface.addBox( tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0, (self.tower_positions[tower.get_id() - 1][2] * 2), position[0], position[1], position[2]) else: self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2, position[0], position[1], position[2]) """This method cleans the planning scene. :param close: with this flag a new planning scene objects will be removed in sync otherwise the object will be deleted without syncing the scene """ def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface( REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass """This method corrects the position of the moved disk. :param tower: parent tower of the disk """ def refresh_disk_pose(self, tower): print "[INFO] Refresh disk pose" disk = tower.get_last() if self.DEBUG is 1: print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\ "tower pose", tower.get_position() # remove the disk from planning scene self.planning_scene_interface.removeCollisionObject( disk.get_id(), False) # publish collision object and set old color self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() """ might be useful if we want to pick and place multiple objects """ # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id """ can change the place location based on the pick location. Let the pick location be the origin. """ # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m-1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class RoboEatsServer(object): DEFAULT_TORSO_HEIGHT = 0.4 MICROWAVE_LOCATION_NAME = "microwave_location" DROPOFF_LOCATION_NAME = "dropoff_location" FOOD_ALIAS = "food_id" def __init__(self, save_file_path="food_items.pkl", nav_file_path="annotator_positions.pkl"): self._food_items_pub = rospy.Publisher(FOOD_ITEMS_TOPIC, FoodItems, queue_size=10, latch=True) rospy.loginfo("Given save file path: " + save_file_path) if os.path.isfile(save_file_path): rospy.loginfo("File already exists, loading saved positions.") with open(save_file_path, "rb") as save_file: try: self._food_items = pickle.load(save_file) except EOFError: # this can be caused if the file is empty. self._food_items = {} rospy.loginfo("File loaded...") else: rospy.loginfo("File doesn't exist.") self._food_items = {} self.__print_food_items__() self._save_file_path = save_file_path self.__pub_food_items__() rospy.loginfo("initializing arm...") self.arm = robot_api.Arm() rospy.loginfo("initializing gripper...") self.gripper = robot_api.Gripper() rospy.loginfo("initializing head...") self.head = robot_api.Head() rospy.loginfo("initializing torso...") self.torso = robot_api.Torso() rospy.loginfo("initializing planning scene...") self.planning_scene = PlanningSceneInterface('base_link') self.curr_obstacle = None rospy.loginfo("Starting map annotator...") # We should expect the nav file given to contain the annotated positions: # MICROWAVE_LOCATION_NAME - starting location in front of the microwave. # DROPOFF_LOCATION_NAME - ending dropoff location. # TODO: Remember to uncomment this section when we get the map working. # self._map_annotator = Annotator(save_file_path=nav_file_path) # if not self._map_annotator.exists(self.MICROWAVE_LOCATION_NAME): # rospy.logwarn("Annotator is missing location '%s'" % # (self.MICROWAVE_LOCATION_NAME)) # if not self._map_annotator.exists(self.DROPOFF_LOCATION_NAME): # rospy.logwarn("Annotator is missing location '%s'" % # (self.DROPOFF_LOCATION_NAME)) rospy.loginfo("Initialization finished...") def __print_food_items__(self): rospy.loginfo("Current food items:") for f in self._food_items.values(): rospy.loginfo("\t" + str(f)) def __save_file__(self): """ Saves the pickle file containing food item information. """ with open(self._save_file_path, "wb") as save_file: pickle.dump(self._food_items, save_file, protocol=pickle.HIGHEST_PROTOCOL) # flush() saves file immediately instead of buffering changes. save_file.flush() def __pub_food_items__(self): """ Publishes the current list of food items to the FOOD_ITEMS_TOPIC topic. """ food_items = FoodItems() if len(self._food_items) > 0: food_items.names, food_items.descriptions, food_items.ids = zip(*[(f.name, f.description, f.id) for f in self._food_items.values()]) self._food_items_pub.publish(food_items) def __food_list_modified__(self): """ Helper method which does any actions that're needed after the food item dict has been modified. """ self.__save_file__() self.__pub_food_items__() def __remove_food_item__(self, id): if id in self._food_items: self._food_items.pop(id) self.__food_list_modified__() def handle_create_food_item(self, request): """ input: request(name, description, id) Creates and adds the given food item. If a food item with the same ID already exists, then it will be overridden by this food item. """ rospy.loginfo(request) self._food_items[request.id] = FoodItem(request.name, request.description, request.id) self.__food_list_modified__() return CreateFoodItemResponse() def handle_remove_food_item(self, request): """ input: request(id) Removes the food item with the given id iff it exists. """ self.__remove_food_item__(request.id) return RemoveFoodItemResponse() def __food_id_to_ar_frame__(self, id): return "ar_marker_" + str(id) def __load_program_and_run__(self, program_fp, id): """Load a pickle file from the given file path and run it with respect to the given food id. Arguments: program_fp {str} -- program file path id {int} -- food id """ if os.path.isfile(program_fp): rospy.loginfo("File " + program_fp + " exists. Loading...") with open(program_fp, "rb") as load_file: program = Program(self.arm, self.gripper, self.head, self.torso) program.commands = pickle.load(load_file) rospy.loginfo("Program loaded...") ar_marker_frame = self.__food_id_to_ar_frame__(id) rospy.loginfo("Program before changes:") program.print_program() # Make the program relative to this food item rospy.loginfo("Program after changes:") program.replace_frame(self.FOOD_ALIAS, ar_marker_frame) program.run() else: rospy.logerr("Program from given file path does not exist: " + program_fp) raise Exception def init_robot(self): """ 0a. Move torso to default position 0b. reset head 0c. open gripper 0d. move arm to starting pos (start_pos.pkl) """ rospy.loginfo("STARTING SEGMENT 1") rospy.loginfo("0a. Move torso to default position") torso = robot_api.Torso() torso.set_height(0.4) rospy.sleep(2) rospy.loginfo("0b. reset head") head = robot_api.Head() head.pan_tilt(-0.1, 0.57) rospy.sleep(2) rospy.loginfo("0c. open gripper") self.gripper.open() rospy.loginfo("0d. Move arm to starting pos") self.__load_program_and_run__("start_pos.pkl", id) rospy.sleep(1.5) def clear_obstacles(self): self.planning_scene.clear() self.planning_scene.removeCollisionObject('table') self.planning_scene.removeCollisionObject('floor') self.planning_scene.removeCollisionObject('lunchbox') self.planning_scene.removeAttachedObject('lunchbox') def start_obstacles_1(self): """ This scene has the microwave in a closed position. """ if self.curr_obstacle == 1: # don't change it if it's already set to this obstacle return self.planning_scene.clear() self.planning_scene.removeCollisionObject('table') self.planning_scene.removeCollisionObject('floor') self.planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2) table_height = 0.767 table_width = 0.7 table_x = 0.95 self.planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2) self.planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2) microwave_height = 0.28 microwave_width = 0.48 # microwave_depth = 0.33 microwave_depth = 0.27 microwave_x = 0.97 microwave_z = 0.06 microwave_y = 0.18 self.planning_scene.addBox('microwave', microwave_depth, microwave_width, microwave_height, microwave_x, microwave_y, table_height + microwave_z + microwave_height/2) self.curr_obstacle = 1 rospy.sleep(3.5) def start_obstacles_2(self): """ this scene has the microwave in an open position with the lid open. """ if self.curr_obstacle == 2: # don't change it if it's already set to this obstacle return self.planning_scene.clear() self.planning_scene.removeCollisionObject('table') self.planning_scene.removeCollisionObject('floor') self.planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2) table_height = 0.767 table_width = 0.7 table_x = 0.95 self.planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2) self.planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2) microwave_height = 0.28 microwave_width = 0.48 microwave_depth = 0.27 microwave_x = 0.97 microwave_y = 0.18 microwave_z = 0.06 microwave_side_height = 0.2 microwave_r_width = 0.04 + 0.05 microwave_r_y = microwave_y - 0.19 microwave_l_width = 0.035 microwave_l_y = microwave_y + 0.2 microwave_bottom_height = 0.05 microwave_top_height = 0.04 microwave_back_depth = 0.03 microwave_back_x = table_x + (microwave_depth / 2) + (microwave_back_depth/2) microwave_door_width = 0.045 microwave_door_x = microwave_x - 0.33 microwave_door_y = microwave_l_y + 0.05 self.planning_scene.addBox('microwave_top', microwave_depth, microwave_width, microwave_top_height, microwave_x, microwave_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height + (microwave_top_height/2)) self.planning_scene.addBox('microwave_bottom', microwave_depth, microwave_width, microwave_bottom_height, microwave_x, microwave_y, table_height + microwave_z + (microwave_bottom_height/2) - 0.015) self.planning_scene.addBox('microwave_side_r', microwave_depth, microwave_r_width, microwave_side_height, microwave_x, microwave_r_y, table_height + microwave_z + microwave_bottom_height + (microwave_side_height/2)) self.planning_scene.addBox('microwave_side_l', microwave_depth, microwave_l_width, microwave_side_height, microwave_x, microwave_l_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height/2) self.planning_scene.addBox('microwave_back', microwave_back_depth, microwave_width, microwave_height, microwave_back_x, microwave_y, table_height + microwave_z + microwave_height/2) self.planning_scene.addBox('microwave_door', 0.42, microwave_door_width, microwave_height + 0.01, microwave_door_x, microwave_door_y, table_height + microwave_z + microwave_height/2 + 0.005) self.curr_obstacle = 2 rospy.sleep(3.5) def attach_lunchbox(self): self.remove_lunchbox() frame_attached_to = 'gripper_link' frames_okay_collide_with = ['gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link', 'wrist_roll_link'] lunchbox_x_offset = 0.1 self.planning_scene.attachBox("lunchbox", 0.10, 0.10, 0.02, lunchbox_x_offset, 0, 0, frame_attached_to, frames_okay_collide_with) def remove_lunchbox(self): self.planning_scene.removeAttachedObject('lunchbox') self.planning_scene.removeCollisionObject('lunchbox') def start_segment1a(self, id): """ (Segment 1a) 0. Initialize robot 1. (OMITTED) Move to start pose 2. Open microwave (p2.pkl) (done) 2b. Move microwave lid (p2b.pkl) (done) """ # if id in self._food_items: rospy.loginfo("0. Initialize robot") self.init_robot() self.start_obstacles_2() # rospy.loginfo("1. Move to start pose") # self._map_annotator.goto_position(self.MICROWAVE_LOCATION_NAME) # rospy.sleep(2) rospy.loginfo("2. Open microwave") self.__load_program_and_run__("p2.pkl", id) rospy.sleep(1.5) rospy.loginfo("2b. Move microwave lid") self.__load_program_and_run__("p2b.pkl", id) rospy.sleep(1.5) rospy.loginfo("FINISHED SEGMENT 1a") # else: # print("Food item " + str(id) + " does not exist.") def start_segment1b(self, id): """ (Segment 1b) 3. Grab lunchbox (p1.pkl) (done - but redo if have enough time) 4. Put it into microwave (p3.pkl) (done-iffy) """ self.start_obstacles_2() rospy.loginfo("STARTING SEGMENT 1b") rospy.loginfo("3. Grab lunchbox") self.__load_program_and_run__("p1.pkl", id) rospy.sleep(1.5) self.attach_lunchbox() rospy.sleep(2) rospy.loginfo("4. Put it into microwave") self.__load_program_and_run__("p3.pkl", id) rospy.sleep(1.5) self.remove_lunchbox() # else: # print("Food item " + str(id) + " does not exist.") def start_segment1c(self, id): self.start_obstacles_2() rospy.loginfo("5a. Close microwave pt. 1") self.__load_program_and_run__("p4a.pkl", id) rospy.sleep(1.5) rospy.loginfo("5b. Changing obstacles...") self.start_obstacles_1() rospy.loginfo("5b. Close microwave pt. 2") self.__load_program_and_run__("p4b.pkl", id) rospy.sleep(1.5) rospy.loginfo("FINISHED SEGMENT 1c") def start_segment2(self, id): """ (Segment 2) 6. Enter time (1 min) & 7. Start microwave (p5.pkl) 8. Wait for food to finish microwaving 9. Wait for cooldown """ # if id in self._food_items: rospy.loginfo("STARTING SEGMENT 2") rospy.loginfo("6. Enter time(1 min)") self.__load_program_and_run__("p5.pkl", id) rospy.loginfo("8. Wait for food to finish microwaving (in seconds)") rospy.sleep(5) rospy.loginfo("9. Wait for cooldown (in seconds)") # rospy.sleep(40) rospy.loginfo("FINISHED SEGMENT 2") # else: # print("Food item " + str(id) + " does not exist.") def start_segment3a(self, id): """ (Segment 3) 10a. Open microwave (p2.pkl) (done) 10b. Move microwave lid (p2b.pkl) (done) """ # if id in self._food_items: self.start_obstacles_2() rospy.loginfo("PRE INITIALIZING FOR SEGMENT 3") self.__load_program_and_run__("segment3a-pre.pkl", id) self.__load_program_and_run__("start_pos.pkl", id) rospy.loginfo("STARTING SEGMENT 3") rospy.loginfo("10a. Open microwave") self.__load_program_and_run__("p2.pkl", id) rospy.loginfo("10b. Move microwave lid ") self.__load_program_and_run__("p2b.pkl", id) # else: # print("Food item " + str(id) + " does not exist.") def start_segment3b(self, id): """ (Segment 3) 11. Grab lunchbox (p6a.pkl, p6b.pkl) """ self.start_obstacles_2() rospy.loginfo("11. Grab lunchbox") # print('lowering torso for better view') # self.torso.set_height(0.35) self.__load_program_and_run__("p6a.pkl", id) self.attach_lunchbox() rospy.sleep(2) self.__load_program_and_run__("p6b.pkl", id) def start_segment3c(self, id): """ (Segment 3) 12. (OMITTED) Move to dropoff pose 13. Put down lunchbox (p7.pkl) """ self.start_obstacles_2() # rospy.loginfo("12. Move to dropoff pose") # self._map_annotator.goto_position(self.DROPOFF_LOCATION_NAME) # rospy.sleep(2) rospy.loginfo("13. Put down lunchbox") self.__load_program_and_run__("p7.pkl", id) self.remove_lunchbox() rospy.loginfo("FINISHED SEGMENT 3") def start_segment4(self, id): """ (Segment 4) 14. (OMITTED) Move to start pose 15. Close microwave (p4a.pkl, p4b.pkl) """ # if id in self._food_items: rospy.loginfo("STARTING SEGMENT 4") # rospy.loginfo("14. Move to start pose") # self._map_annotator.goto_position(self.MICROWAVE_LOCATION_NAME) # rospy.sleep(2) self.start_obstacles_2() rospy.loginfo("15a. Close microwave pt. 1") self.__load_program_and_run__("p4a.pkl", id) rospy.sleep(1.5) rospy.loginfo("15b. Changing obstacles...") self.start_obstacles_1() rospy.loginfo("15b. Close microwave pt. 2") self.__load_program_and_run__("p4b.pkl", id) rospy.sleep(1.5) rospy.loginfo("FINISHED SEGMENT 4") # else: # print("Food item " + str(id) + " does not exist.") def handle_start_sequence(self, request): """ input: request(id) Starts the entire food sequence and removes the food item from the dictionary after it has been finished. """ id = request.id # if id in self._food_items: rospy.loginfo("Starting sequence for food item: " + str(self._food_items[id])) self.start_segment1a(id) self.start_segment1b(id) self.start_segment1c(id) self.start_segment2(id) self.start_segment3a(id) self.start_segment3b(id) self.start_segment3c(id) # we decioded that we don't need to close the door :) self.start_segment4(id) rospy.loginfo("Remove food item from the list.") # self.__remove_food_item__(id) rospy.loginfo("Finished sequence.") # else: # print("Food item " + str(id) + " does not exist.") return StartSequenceResponse()