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")
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("Add collision objects") 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.removeCollisionObject("box_1") 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) self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34) self.scene.waitForSync() 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 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") 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(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() rospy.loginfo("Found %d objects" % len(find_result.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 objects = list() idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "obj%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 < 1.3: #< 0.85: objects.append([obj, obj.object.primitive_poses[0].position.y]) #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]) self.scene.waitForSync() # store for grasping self.surfaces = find_result.support_surfaces # store graspable objects by y objects.sort(key=lambda object: object[1]) #objects.reverse() self.objects = [object[0] for object in objects] def getGraspableObject(self, goal_obj_x, goal_obj_y): 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.4: continue if obj.object.primitive_poses[0].position.x < 0.3: continue # get goal object if ( abs(obj.object.primitive_poses[0].position.x - goal_obj_x) > 0.03): continue if ( abs(obj.object.primitive_poses[0].position.y - goal_obj_y) > 0.03): continue print("**************************************Object Name: ", obj.object.name) print("Object Pose: ", 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 #hold the object #touch_links = ['l_gripper_finger_link', 'r_gripper_finger_link'] #self.hand_group.attach_object('apple', 'base_link', touch_links=touch_links) 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] #pose = [-1.60, -1.10, -1.20, -1.50, 0.0, -1.51, 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
def picknplace(): # Define initial parameters p = PlanningSceneInterface("base") g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] # Clear planning scene p.clear() # Add table as attached object p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) # Move both arms to start state g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Loop to continue pick and place until all objects are cleared from table j=0 k=0 while locs_x: p.clear() # CLear planning scene of all collision objects # Attach table as attached collision object to base of baxter p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) xn = locs_x[0] yn = locs_y[0] zn = -0.06 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add collision objects into planning scene objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] for i in range(1,len(locs_x)): p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05) p.waitForSync() # Move both arms to pos2 (Right arm away and left arm on table) g.moveToJointPosition(jts_both, pos2, plan_only=False) # Move left arm to pick object and pick object pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn+0.1 pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", plan_only=False) pickgoal.pose.position.z = zn gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False) leftgripper.close() pickgoal.pose.position.z = zn+0.1 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Move both arms to pos1 g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Place object stleft = PoseStamped() stleft.header.frame_id = "base" stleft.header.stamp = rospy.Time.now() stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.55 stleft.pose.position.z = 0.1 stleft.pose.orientation.x = 1.0 stleft.pose.orientation.y = 0.0 stleft.pose.orientation.z = 0.0 stleft.pose.orientation.w = 0.0 # Stack objects (large cubes) if size if greater than some threshold if sz > 16.: stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.7 stleft.pose.position.z = -0.04+(k*0.05) k += 1 gl.moveToPose(stleft, "left_gripper", plan_only=False) leftgripper.open() stleft.pose.position.z = 0.3 gl.moveToPose(stleft, "left_gripper", plan_only=False)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') ptu_action = FollowTrajectoryClient( "ptu_controller", ["ptu_pan_joint", "ptu_tilt_joint"]) ptu_action.move_to([ 0.0, -0.75, ]) # Use the planning scene object to add or remove objects scene = PlanningSceneInterface(REFERENCE_FRAME) # 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() print("End Effector Link", end_effector_link) # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.05) # 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 15 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # 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 find_topic = "basic_grasping_perception/find_objects" sub_topic = "handles_position" rospy.Subscriber(sub_topic, target_pose, getHandlesCb) rospy.loginfo( "Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient( find_topic, 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('right_down') # 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(10.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)) rospy.loginfo("Found %d Support Surfaces" % len(find_result.support_surfaces)) # print("perception_results_primitives",find_result.objects.primitives[0]) # print("perception_results_primitives_poses",find_result.objects.primitives_poses) # 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 # target_position = None the_object = None the_object_dist = 1.0 count = -1 # target_size # 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 print("target_size %d ", count, target_size) # target_position = obj.object.primitive_poses[0].position # print("target_size %d ", %count, target_size ) # Set the target pose target_pose.pose = obj.object.primitive_poses[0] print("target_pose_before ", target_pose) # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.519617092464 target_pose.pose.orientation.y = -0.510439243162 target_pose.pose.orientation.z = 0.479912175114 target_pose.pose.orientation.w = -0.489013456294 print("target_pose_after ", target_pose) # 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] ] # obj.primitive_poses[0].position.z += -height/1.5 # 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 print("table_size", table_size) # 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] 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") 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
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 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") 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 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 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
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 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 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() 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 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 picknplace(): # Initialize the planning scene interface. p = PlanningSceneInterface("base") # Connect the arms to the move group. g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") # Create baxter_interface limb instance. leftarm = baxter_interface.limb.Limb('left') # Create baxter_interface gripper instance. leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() # Define the joints for the positions. jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] placegoal = PoseStamped() placegoal.header.frame_id = "base" placegoal.header.stamp = rospy.Time.now() placegoal.pose.position.x = 0.55 placegoal.pose.position.y = 0.28 placegoal.pose.position.z = 0 placegoal.pose.orientation.x = 1.0 placegoal.pose.orientation.y = 0.0 placegoal.pose.orientation.z = 0.0 placegoal.pose.orientation.w = 0.0 # Define variables. offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 # The distance from the zero point in Moveit to the ground is 0.903 m. # The value is not allways the same. (look in Readme) center_z_cube= -offset_zero_point+table_size_z+0.0275/2 pressure_ok=0 j=0 k=0 start=1 locs_x = [] # Initialize a list for the objects and the stacked cubes. objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] boxlist= ['box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11'] # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Move both arms to start state where the right camera looks for objects. g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False) time.sleep(0.5) # cProfile to measure the performance (time) of the task. pr = cProfile.Profile() pr.enable() # Loop to continue pick and place until all objects are cleared from table. while locs_x or start: # Only for the start. if start: start = 0 # Receive the data from all objects from the topic "detected_objects". temp = rospy.wait_for_message("detected_objects", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] # Adds the data from the objects. for i in range(len(locs)): locs_x.append(locs[i].position.x) locs_y.append(locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected locations for same objects. ind_rmv = [] for i in range(0,len(locs) ): if (locs_y[i] > 0.24 or locs_x[i] > 0.75): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Do the task only if there are still objects on the table. if locs_x: # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes. ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Initialize the data of the biggest object on the table. xn = locs_x[0] yn = locs_y[0] zn = -0.16 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add the detected objects into the planning scene. #for i in range(1,len(locs_x)): #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube) # Add the stacked objects as collision objects into the planning scene to avoid moving against them. #for e in range(0, k): #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes']) if k>0: p.attachBox(boxlist[0], 0.07, 0.07, 0.0275*k, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube, 'base', touch_links=['cubes']) p.waitForSync() # Move both arms to the second position. g.moveToJointPosition(jts_both, pos2, max_velocity_scaling_factor = 1, plan_only=False) # Initialize the pickgoal. pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 # Move to the approach pickgoal. (5 cm to pickgoal) pickgoal.pose.position.z = zn+0.05 # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) pickgoal.pose.position.z = zn # Move to the pickgoal. gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.3, plan_only=False) # Read the force in z direction. b=leftarm.endpoint_effort() z_= b['force'] z_force= z_.z # Search again for objects, if the gripper isn't at the right position and presses on an object. #print("----->force in z direction:", z_force) if z_force>-4: leftgripper.close() attempts=0 pressure_ok=1 # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects. while(leftgripper.force()<25 and pressure_ok==1): time.sleep(0.04) attempts+=1 if(attempts>50): leftgripper.open() pressure_ok=0 print("----->pressure is to low<-----") else: print("----->gripper presses on an object<-----") # Move back to the approach pickgoal. pickgoal.pose.position.z = zn+0.05 gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Move both arms to position 1. g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False) if pressure_ok and z_force>-4: # Define the approach placegoal. # Increase the height of the tower every time by 2.75 cm. placegoal.pose.position.z =-0.145+(k*0.0275)+0.08 # Move to the approach pickgoal. gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Define the placegoal. placegoal.pose.position.z =-0.145+(k*0.0275) gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) leftgripper.open() k += 1 # Move to the approach placegoal. placegoal.pose.position.z = -0.145+(k*0.0275)+0.08 gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Move left arm to start position pos1. gl.moveToJointPosition(jts_left, lpos1, max_velocity_scaling_factor = 1, plan_only=False) pr.disable() sortby = 'cumulative' ps=pstats.Stats(pr).sort_stats(sortby).print_stats(0.0) p.clear()
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
# box_pose = geometry_msgs.msg.PoseStamped() # box_pose.header.frame_id = robot.get_planning_frame() # box_pose.pose.position.x = 1. # box_pose.pose.position.y = 0.5 # box_pose.pose.position.z = 0.5 # scene.add_box("table", box_pose, (0.5, 1.5, 0.6)) # name size and position p.addBox("first part", 0.8, 0.43, 0.04, 1.23, 0, 0.36) p.addBox("under part", 0.8, 0.43, 0.04, 1.23, 0, 0.0) p.addBox("second part", 0.8, 0.01, 1.6, 1.23, -0.22, 0) p.addBox("third part", 0.8, 0.01, 1.6, 1.23, 0.22, 0) p.waitForSync() def load_gazebo_models(table_pose=Pose(position=Point(x=0.7, y=0.0, z=0.0)), table_reference_frame="world", block_pose1=Pose(position=Point(x=0.6725, y=0.0765, z=-0.135)), block_pose2=Pose(position=Point(x=0.55, y=-0.2, z=-0.135)), block_pose3=Pose(position=Point(x=0.7, y=-0.1, z=-0.135)), block_pose4=Pose(position=Point(x=0.58, y=-0.03, z=-0.135)), block_reference_frame="base"): pass # Get Models' Path. model_path = rospkg.RosPack().get_path('baxter_pnp_one_arm_cartesian_sim')+"/models/" # Load Table SDF. table_xml = '' with open (model_path + "cafe_table/model.sdf", "r") as table_file: table_xml=table_file.read().replace('\n', '')
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()
def clasificar(): adaptative = True pr_b = False precision = 0.7 # Can free memory? pub = rospy.Publisher("finished", String, queue_size=10) # Initialize MoveIt! scene p = PlanningSceneInterface("base") arms_group = MoveGroupInterface("both_arms", "base") rightarm_group = MoveGroupInterface("right_arm", "base") leftarm_group = MoveGroupInterface("left_arm", "base") # Create right arm instance right_arm = baxter_interface.limb.Limb('right') # Create right gripper instance right_gripper = baxter_interface.Gripper('right') right_gripper.calibrate() right_gripper.open() right_gripper.close() offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 table_distance_from_gripper = -offset_zero_point + table_size_z + 0.0275 / 2 j = 0 k = 0 # Initialize object list objlist = [ 'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11' ] p.clear() p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move both arms to start state. # Initial pos rpos = PoseStamped() rpos.header.frame_id = "base" rpos.header.stamp = rospy.Time.now() rpos.pose.position.x = 0.555 rpos.pose.position.y = 0.0 rpos.pose.position.z = 0.206 rpos.pose.orientation.x = 1.0 rpos.pose.orientation.y = 0.0 rpos.pose.orientation.z = 0.0 rpos.pose.orientation.w = 0.0 lpos = PoseStamped() lpos.header.frame_id = "base" lpos.header.stamp = rospy.Time.now() lpos.pose.position.x = 0.65 lpos.pose.position.y = 0.6 lpos.pose.position.z = 0.206 lpos.pose.orientation.x = 1.0 lpos.pose.orientation.y = 0.0 lpos.pose.orientation.z = 0.0 lpos.pose.orientation.w = 0.0 while True: # Move to initial position rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False) # Get the middle point locs = PoseArray() locs = rospy.wait_for_message("clasificacion", PoseArray) if (len(locs.poses) != 0): punto_medio = PoseStamped() punto_medio.header.frame_id = "base" punto_medio.header.stamp = rospy.Time.now() punto_medio.pose.position.x = locs.poses[0].position.x punto_medio.pose.position.y = locs.poses[0].position.y punto_medio.pose.position.z = locs.poses[0].position.z punto_medio.pose.orientation.x = locs.poses[0].orientation.x punto_medio.pose.orientation.y = locs.poses[0].orientation.y punto_medio.pose.orientation.z = locs.poses[0].orientation.z punto_medio.pose.orientation.w = locs.poses[0].orientation.w alfa = 0.1 # Two parameters: # When pr_b == 1, the program will try to adapt the trajectory for getting the precision established in the parameter of the same name # When adaptative == 1, the program will try to get the best precision based on the precision of the last execution. if (pr_b and not adaptative): if precision >= 1: punto_medio.pose.position.x += 0.01 else: punto_medio.pose.position.x -= alfa * (1 - precision) else: print("If it is the first time executing, write -1") precision_value = input() if precision_value != -1.0: punto_medio.pose.position.x += alfa * (1 - precision_value) elif precision_value == 1.0: adaptative = False # Get the normal orientation for separating the objects orient = (-1) * (1 / punto_medio.pose.orientation.x) punto_medio.pose = rotate_pose_msg_by_euler_angles( punto_medio.pose, 0.0, 0.0, orient) rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) orient = 1.57 - orient punto_medio.pose = rotate_pose_msg_by_euler_angles( punto_medio.pose, 0.0, 0.0, orient) rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) punto_medio.pose.position.x += 0.15 rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) time.sleep(1) punto_medio.pose.position.x -= 0.3 rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) time.sleep(1) rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) # Free the memory when finished freemem = "free" if not adaptative: pub.publish(freemem) else: sys.exit("Cannot identify any object")
class GraspingClient(object): def __init__(self, sim=False): self.scene = PlanningSceneInterface("base_link") self.dof = rospy.get_param('~jaco_dof') self.move_group = MoveGroupInterface("upper_body", "base_link") self.lmove_group = MoveGroupInterface("left_arm", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") self.move_group.setPlannerId("RRTConnectkConfigDefault") self.lmove_group.setPlannerId("RRTConnectkConfigDefault") self.rmove_group.setPlannerId("RRTConnectkConfigDefault") if "6dof" == self.dof: self._upper_body_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint" ] self.tucked = [ -2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571, 0.0371, 0.0, 0.0 ] self.constrained_stow = [ 2.28, 2.17, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56, 0.09, -0.15, 2.06, 0.42, 0.0, 0.0 ] self.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08] self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06] self.tableDist = 0.7 elif "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] self.constrained_stow = [ -2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0 ] self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0] self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0] self.tableDist = 0.8 else: rospy.logerr("DoF needs to be set 6 or 7, aborting demo") return self.pickplace = [None] * 2 self.pickplace[0] = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace[0].planner_id = "RRTConnectkConfigDefault" self.pickplace[1] = PickPlaceInterface("right_side", "right_gripper", verbose=True) self.pickplace[1].planner_id = "RRTConnectkConfigDefault" self.pick_result = [None] * 2 self._last_gripper_picked = 0 self.place_result = [None] * 2 self._last_gripper_placed = 0 self._objs_to_keep = [] self._listener = tf.TransformListener() self._lgripper = GripperActionClient('left') self._rgripper = GripperActionClient('right') 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.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.00 else: self._gripper_closed = 0.01 self._gripper_open = 0.165 def add_objects_to_keep(self, obj): self._objs_to_keep.append(obj) def clearScene(self): self.scene.clear() def getPickCoordinates(self): self.updateScene(0, False) beer, grasps = self.getGraspableBeer(False) pringles, grasps = self.getGraspablePringles(False) if (None == beer) or (None == pringles): return None center_objects = (beer.primitive_poses[0].position.y + pringles.primitive_poses[0].position.y) / 2 surface = self.getSupportSurface(beer.support_surface) tmp1 = surface.primitive_poses[ 0].position.x - surface.primitives[0].dimensions[0] / 2 surface = self.getSupportSurface(pringles.support_surface) tmp2 = surface.primitive_poses[ 0].position.x - surface.primitives[0].dimensions[0] / 2 front_edge = (tmp1 + tmp2) / 2 coords = Pose2D(x=(front_edge - self.tableDist), y=center_objects, theta=0.0) return coords def updateScene(self, gripper=0, plan=True): # find objects rospy.loginfo("Updating scene...") goal = FindGraspableObjectsGoal() goal.plan_grasps = plan goal.gripper = gripper 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, True) # insert objects to scene idx = -1 for obj in find_result.objects: if obj.object.primitive_poses[ 0].position.z < 0.5 or obj.object.primitive_poses[ 0].position.x > 2.0 or obj.object.primitive_poses[ 0].position.y > 0.5: continue idx += 1 obj.object.name = "object%d_%d" % (idx, gripper) self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=True) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view if obj.primitive_poses[0].position.z < 0.5: continue height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], obj.primitives[0].dimensions[1], # 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=True) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableBeer(self, planned=True): for obj in self.objects: # need grasps if len(obj.grasps) < 1 and planned: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5 or \ obj.object.primitive_poses[0].position.z > 1.0 or \ obj.object.primitive_poses[0].position.x > 2.0: continue elif (obj.object.primitives[0].type == sp.CYLINDER): if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.102 or \ obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.142: continue elif (obj.object.primitives[0].type == sp.BOX): if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.102 or \ obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.142: continue else: continue print "beer: ", obj.object.primitive_poses[0] return obj.object, obj.grasps # nothing detected return None, None def getGraspablePringles(self, planned=True): for obj in self.objects: # need grasps if len(obj.grasps) < 1 and planned: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5 or \ obj.object.primitive_poses[0].position.z > 1.0 or \ obj.object.primitive_poses[0].position.x > 2.0: continue elif (obj.object.primitives[0].type == sp.CYLINDER): if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.21 or \ obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.28: continue elif (obj.object.primitives[0].type == sp.BOX): if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.21 or \ obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.28: continue else: continue return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps, gripper=0): success, pick_result = self.pickplace[gripper].pick_with_retry( block.name, grasps, retries=1, support_name=block.support_surface, scene=self.scene) self.pick_result[gripper] = pick_result self._last_gripper_picked = gripper return success def place(self, block, pose_stamped, gripper=0): 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[ gripper].grasp.pre_grasp_posture l.pre_place_approach = self.pick_result[ gripper].grasp.pre_grasp_approach l.post_place_retreat = self.pick_result[ gripper].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[gripper].place_with_retry( block.name, places, retries=1, scene=self.scene) self.place_result[gripper] = place_result self._last_gripper_placed = gripper return success def goto_tuck(self): # remove previous objects while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.tucked, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def goto_plan_grasp(self): while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.constrained_stow, 0.05) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def left_arm_constrained_stow(self): c1 = Constraints() c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.stamp = rospy.get_rostime() c1.orientation_constraints[0].header.frame_id = "base_link" c1.orientation_constraints[0].link_name = "left_ee_link" c1.orientation_constraints[0].orientation.w = 1.0 c1.orientation_constraints[ 0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.lmove_group.moveToJointPosition( self._left_arm_joints, self.larm_const_stow, 0.05, path_constraints=c1, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def right_arm_constrained_stow(self): c1 = Constraints() c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.stamp = rospy.get_rostime() c1.orientation_constraints[0].header.frame_id = "base_link" c1.orientation_constraints[0].link_name = "right_ee_link" c1.orientation_constraints[0].orientation.w = 1.0 c1.orientation_constraints[ 0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.rmove_group.moveToJointPosition( self._right_arm_joints, self.rarm_const_stow, 0.05, path_constraints=c1, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def open_gripper(self): self._lgripper.command(self._gripper_open, block=True) self._rgripper.command(self._gripper_open, block=True) def close_gripper(self): self._lgripper.command(self._gripper_closed, block=True) self._rgripper.command(self._gripper_closed, block=True)
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 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 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))
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 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 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
def dyn_wedge(): pub = rospy.Publisher("finished", String, queue_size=10) # Initialize MoveIt scene p = PlanningSceneInterface("base") arms_group = MoveGroupInterface("both_arms", "base") rightarm_group = MoveGroupInterface("right_arm", "base") leftarm_group = MoveGroupInterface("left_arm", "base") # Create right arm instance right_arm = baxter_interface.limb.Limb('right') # Create right gripper instance right_gripper = baxter_interface.Gripper('right') right_gripper.calibrate() right_gripper.open() right_gripper.close() offset_zero_point=0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 table_distance_from_gripper = -offset_zero_point+table_size_z+0.0275/2 j=0 k=0 # Initialize object list objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] p.clear() p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move both arms to start state. # Initial pose rpos = PoseStamped() rpos.header.frame_id = "base" rpos.header.stamp = rospy.Time.now() rpos.pose.position.x = 0.555 rpos.pose.position.y = 0.0 rpos.pose.position.z = 0.206 #0.18 rpos.pose.orientation.x = 1.0 rpos.pose.orientation.y = 0.0 rpos.pose.orientation.z = 0.0 rpos.pose.orientation.w = 0.0 lpos = PoseStamped() lpos.header.frame_id = "base" lpos.header.stamp = rospy.Time.now() lpos.pose.position.x = 0.555 lpos.pose.position.y = 0.65 lpos.pose.position.z = 0.206#0.35 lpos.pose.orientation.x = 1.0 lpos.pose.orientation.y = 0.0 lpos.pose.orientation.z = 0.0 lpos.pose.orientation.w = 0.0 rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False) # Get the middle point between the two centroids locs = rospy.wait_for_message("clasificacion", PoseArray) if(len(locs.poses)!=0): punto_medio1 = PoseStamped() punto_medio1.header.frame_id = "base" punto_medio1.header.stamp = rospy.Time.now() punto_medio1.pose.position.x = locs.poses[0].position.x punto_medio1.pose.position.y = locs.poses[0].position.y punto_medio1.pose.position.z = -0.08 punto_medio1.pose.orientation.x = locs.poses[0].orientation.x punto_medio1.pose.orientation.y = locs.poses[0].orientation.y punto_medio1.pose.orientation.z = locs.poses[0].orientation.z punto_medio1.pose.orientation.w = locs.poses[0].orientation.w else: sys.exit("No se encuentran los puntos") # Get the middle point again after a few seconds tiempo = 3 time.sleep(tiempo) locs = rospy.wait_for_message("clasificacion", PoseArray) if(len(locs.poses)!=0): punto_medio2 = PoseStamped() punto_medio2.header.frame_id = "base" punto_medio2.header.stamp = rospy.Time.now() punto_medio2.pose.position.x = locs.poses[0].position.x punto_medio2.pose.position.y = locs.poses[0].position.y punto_medio2.pose.position.z = -0.08 punto_medio2.pose.orientation.x = locs.poses[0].orientation.x punto_medio2.pose.orientation.y = locs.poses[0].orientation.y punto_medio2.pose.orientation.z = locs.poses[0].orientation.z punto_medio2.pose.orientation.w = locs.poses[0].orientation.w else: sys.exit("No se encuentran los puntos") # Calculate speed of objects vel = (punto_medio2.pose.position.y - punto_medio1.pose.position.y) / tiempo # Predict position within a certain time nuevo_tiempo = 2.1 posicion = nuevo_tiempo * vel * 2 if(posicion>=0): nueva_y = punto_medio2.pose.position.y - posicion else: nueva_y = punto_medio2.pose.position.y + posicion orientacion = (-1) * 1/punto_medio2.pose.orientation.x new_or = 0.26 if orientacion > 0: new_or = -0.26 # If there is time enough to sweep the objects if vel > -0.08: start = time.time() punto_medio2.pose.position.y = nueva_y - nueva_y/2 punto_medio2.pose.position.x = punto_medio1.pose.position.x - 0.03 punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion - new_or) rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) end = time.time() tiempo = end - start while(nuevo_tiempo - tiempo >= 1): end = time.time() nuevo_tiempo = end - start else: punto_medio2.pose.position.y = nueva_y print(punto_medio2.pose.position.y) punto_medio2.pose.position.x = punto_medio1.pose.position.x punto_medio2.pose.position.y += 0.05 punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion) rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) obj = punto_medio2.pose.position.y + 0.16 neg = 1 while punto_medio2.pose.position.y < obj: punto_medio2.pose.position.y += 0.03 # Shake arm punto_medio2.pose.position.x += neg * 0.09 rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) neg = (-1)*neg time.sleep(2) rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) freemem = "free" pub.publish(freemem)
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)
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 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 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