コード例 #1
0
    def __init__(self, config, debug=0):
        print "[INFO] Initialise Robot"
        self.DEBUG = debug
        # configuration
        self.config = config
        # initialise move groups
        self.arm = MoveGroupCommander(ARM_GROUP)
        self.gripper = MoveGroupCommander(GRIPPER_GROUP)
        # initialise pick and place manager
        if self.DEBUG is 1:
            # verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP,
                                                     False, True)
        else:
            # non verbose mode
            self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP,
                                                     False, False)

        # tolerance: position (in m), orientation (in rad)
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.1)

        self.place_manager = None
        self.pick_manager = None

        self.initialise_move_actions()
        self.start_position()
コード例 #2
0
    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()
コード例 #3
0
    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()
コード例 #4
0
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")
コード例 #5
0
    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()
コード例 #6
0
    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()
コード例 #7
0
	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)
コード例 #8
0
    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()
コード例 #9
0
    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()
コード例 #10
0
    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 = []
コード例 #11
0
    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()
コード例 #12
0
    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 = []
コード例 #13
0
def setup_scene():
    scene = PlanningSceneInterface('base_link')
    scene.removeCollisionObject('box')
    scene.addCube('box', 0.25, -0.45, 0.1, 0.125)

    pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True)
    grasp = Grasp()

    # fill in g
    # setup object named object_name using PlanningSceneInterface
    pick_place.pickup('box', [grasp], support_name='supporting_surface')

    place_loc = PlaceLocation()
    # fill in l
    pick_place.place('box'[place_loc],
                     goal_is_eef=True,
                     support_name='supporting_surface')
コード例 #14
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")
        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
コード例 #15
0
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))
コード例 #16
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() 
        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)
コード例 #17
0
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    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
コード例 #18
0
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()
        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
コード例 #19
0
 def __init__(self):
     self._scene = PlanningSceneInterface('base_link')
     self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True)
     self._move_group = MoveGroupInterface('xarm5', 'base_link')
コード例 #20
0
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
コード例 #21
0
class GraspingClient(object):
    def __init__(self, sim=False):
        # Define planning groups
        self.scene = PlanningSceneInterface("base_link")
        self.dof = rospy.get_param('~jaco_dof')
        self.move_group = MoveGroupInterface("upper_body", "base_link")
        self.rmove_group = MoveGroupInterface("right_arm", "base_link")

        planner_id = "RRTConnectkConfigDefault"
        self.move_group.setPlannerId(planner_id)
        self.rmove_group.setPlannerId(planner_id)

        self.objects_heights = [0.122, 0.240]
        self.objects_heights_tolerances = [0.02, 0.03]

        # Define joints and positions for 6 and 7 DOF
        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.00, -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.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
            #self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]

        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.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]

        else:
            rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
            return

        # Define the MoveIt pickplace pipeline
        self.pickplace = None
        self.pickplace = PickPlaceInterface("left_side",
                                            "left_gripper",
                                            verbose=True)
        self.pickplace.planner_id = planner_id
        self.pick_result = None
        self.place_result = None

        self._listener = tf.TransformListener()

        self._lgripper = GripperActionClient('left')

        find_grasp_planning_topic = "grasp_planner/plan"
        rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
        self.find_grasp_planning_client = actionlib.SimpleActionClient(
            find_grasp_planning_topic, GraspPlanningAction)
        self.find_grasp_planning_client.wait_for_server()
        rospy.loginfo("...connected")

        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 __del__(self):
        self.scene.clear()

    def add_objects_to_keep(self, obj):
        self._objs_to_keep.append(obj)

    def clearScene(self):
        self.scene.clear()

    def calculateGraspForObject(self, object_to_grasp, gripper):
        goal = GraspPlanningGoal()
        goal.object = object_to_grasp
        goal.gripper = gripper
        self.find_grasp_planning_client.send_goal(goal)
        self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0))

        return self.find_grasp_planning_client.get_result(
        ).grasps  #moveit_msgs/Grasp[]

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name,
                                                              grasps,
                                                              retries=1,
                                                              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, retries=1, scene=self.scene)
        self.place_result = place_result
        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,
                planning_time=120.0)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def open_gripper(self):
        self._lgripper.command(self._gripper_open, block=True)

    def close_gripper(self):
        self._lgripper.command(self._gripper_closed, block=True)
コード例 #22
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()
        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)
コード例 #23
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")

        # 初始化需要使用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
コード例 #24
0
class GraspingClient(object):
    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(
            find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    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
コード例 #25
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
コード例 #26
0
ファイル: demo.py プロジェクト: hishamcg/DrdoProject
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
コード例 #27
0
    def __init__(self, sim=False):
        self.scene = PlanningSceneInterface("base_link")
        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")
        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.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.0
        else:
            self._gripper_closed = 0.0
            self._gripper_open = 0.165
コード例 #28
0
class GraspingClient(object):
    def __init__(self, scene):
        self.scene = scene
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Connecting to %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(
            find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()
        rospy.loginfo("...%s connected" % find_topic)

    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
        objects = []
        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. objects are ordered by Z
        objects.sort(key=lambda obj: obj[1])
        objects.reverse()
        self.objects = [obj[0] for obj in objects]
        self.surfaces = find_result.support_surfaces

    def getGraspableCube(self,
                         bounds=DEFAULT_CUBE_BOUNDS,
                         location=DEFAULT_CUBE_LOCATION):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                continue
            # check size and type
            if obj.object.primitives[0].type != SolidPrimitive.BOX:
                continue
            if obj.object.primitives[0].dimensions[0] < bounds["xmin"] or \
               obj.object.primitives[0].dimensions[0] > bounds["xmax"] or \
               obj.object.primitives[0].dimensions[1] < bounds["ymin"] or \
               obj.object.primitives[0].dimensions[1] > bounds["ymax"] or \
               obj.object.primitives[0].dimensions[2] < bounds["zmin"] or \
               obj.object.primitives[0].dimensions[2] > bounds["zmax"]:
                continue
            # has to be on table
            if obj.object.primitive_poses[0].position.x < location["xmin"] or \
               obj.object.primitive_poses[0].position.x > location["xmax"] or \
               obj.object.primitive_poses[0].position.y < location["ymin"] or \
               obj.object.primitive_poses[0].position.y > location["ymax"] or \
               obj.object.primitive_poses[0].position.z < location["zmin"] or \
               obj.object.primitive_poses[0].position.z > location["zmax"]:
                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)
        return success, pick_result

    def place(self, block, pose_stamped, pick_result):
        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 = pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = 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
コード例 #29
0
 def __init__(self):
     self.scene = PlanningSceneInterface("base_link")
     self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
     self.move_group = MoveGroupInterface("arm", "base_link")
コード例 #30
0
class GraspingClient(object):

    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    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
コード例 #31
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
コード例 #32
0
ファイル: lx_pick_place.py プロジェクト: ALAN-NUS/kinova_movo
    def __init__(self, sim=False):
        self.scene = PlanningSceneInterface("base_link")
        self.dof = rospy.get_param('~jaco_dof')
        self.robot = moveit_commander.RobotCommander()

        self._listener = tf.TransformListener()
        # self.frame = self._listener.lookupTransform('/left_ee_link', '/base_link', rospy.Time(0))
        # rospy.loginfo("========== Left_ee to base_link is: ")
        # rospy.loginfo(self.frame)
        # curr_pos = self._listener.lookupTransform('/base_link', '/left_ee_link', rospy.Time(0))
        # rospy.loginfo(curr_pos)

        # self.move_group = moveit_commander.MoveGroupCommander("upper_body")
        # rospy.loginfo("======upper_body_group connected =========")
        # rospy.loginfo(self.move_group.get_joints())
        # self.lmove_group = moveit_commander.MoveGroupCommander("left_arm")
        # rospy.loginfo("======left_arm_group connected =========")
        # self.rmove_group = moveit_commander.MoveGroupCommander("right_arm")
        # rospy.loginfo("======right_arm_group connected =========")
        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:
            rospy.logwarn(
                "======= Please change launch param to 7DoF =========")

        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]
            # flipping pose
            self.constrained_stow = [
                -0.23, -0.71, -1.02, -1.0, 0.9, 1.89, -2.41, 0.44, 0.71, 1.12,
                1.21, -1.02, -1.84, 2.61, 0.20, 0, 0
            ]

            # self.constrained_stow_2 = [-0.34, -0.66, -0.92, -1.18, 0.94, 1.75, -2.49, 0.54, 0.65, 1.0, 1.35, -1.07, -1.75, 2.67, 0.20, 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
        '''
        pickplace = [lg_itf, rg_itf]
        pick_result = [None, None]
        '''

        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._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()
        rospy.loginfo(
            "============= FindGraspableObjectsAction connected! ============")

        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
コード例 #33
0
class GraspingClient(object):
    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(
            find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

    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
コード例 #34
0
class GraspingClient(object):
    def __init__(self):
        self.scene = PlanningSceneInterface("base_link")
        self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
        self.move_group = MoveGroupInterface("arm", "base_link")

        find_topic = "basic_grasping_perception/find_objects"
        rospy.loginfo("Waiting for %s..." % find_topic)
        self.find_client = actionlib.SimpleActionClient(
            find_topic, FindGraspableObjectsAction)
        self.find_client.wait_for_server()

        # #######################################################################
        # # test fetch_grasp_planner_node/plan with pose subcriber
        # # use fetch_grasp_planner_node/plan with fetch_grasp_planner_node/plan
        # grasp_topic = "fetch_grasp_planner_node/plan"
        # rospy.loginfo("Waiting for %s..." % grasp_topic)
        # self.grasp_planner_client = actionlib.SimpleActionClient(grasp_topic, GraspPlanningAction)
        # self.grasp_planner_client.wait_for_server()
        # #######################################################################

    #     #####################################################################
    #     # test fetch_grasp_planner_node/plan with pose subcriber
    #     rospy.Subscriber("perception/apple_pose", Pose, self.apple_pose_callback)
    #     self.object = Object()
    #     self.grasps = Grasp()

    # def apple_pose_callback(self, message):
    #     apple = SolidPrimitive()
    #     apple.type = SolidPrimitive.SPHERE
    #     apple.dimensions = 0.04
    #     self.object.primitives = apple
    #     self.object.primitive_poses = message
    #     # add stamp and frame
    #     self.object.header.stamp = rospy.Time.now()
    #     self.object.header.frame_id = message.frame_id

    #     goal = GraspPlanningGoal()
    #     goal.object = self.object
    #     self.grasp_planner_client.send_goal(goal)
    #     self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
    #     grasp_planner_result = self.grasp_planner_client.get_result()
    #     self.grasps = grasp_planner_result.grasps
    #     ####################################################################

    def updateScene(self):
        # ####################################################################
        # # use fetch_grasp_planner_node/plan with fetch_grasp_planner_node/plan
        # # find objects
        # goal = FindGraspableObjectsGoal()
        # goal.plan_grasps = False
        # self.find_client.send_goal(goal)
        # self.find_client.wait_for_result(rospy.Duration(5.0))
        # find_result = self.find_client.get_result()

        # for obj in find_result.objects:
        #     goal = GraspPlanningGoal()
        #     goal.object = obj.object
        #     self.grasp_planner_client.send_goal(goal)
        #     self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
        #     grasp_planner_result = self.grasp_planner_client.get_result()
        #     obj.grasps = grasp_planner_result.grasps
        # ####################################################################

        # find objects
        goal = FindGraspableObjectsGoal()
        goal.plan_grasps = True
        self.find_client.send_goal(goal)
        self.find_client.wait_for_result(rospy.Duration(5.0))
        find_result = self.find_client.get_result()

        # remove previous objects
        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)
        self.scene.waitForSync()

        # insert objects to scene
        objects = list()
        idx = -1
        for obj in find_result.objects:
            idx += 1
            obj.object.name = "object%d" % idx
            self.scene.addSolidPrimitive(obj.object.name,
                                         obj.object.primitives[0],
                                         obj.object.primitive_poses[0],
                                         use_service=False)
            if obj.object.primitive_poses[0].position.x < 0.85:
                objects.append([obj, obj.object.primitive_poses[0].position.z])
            print("object ", idx, " frame_id = ", obj.object.header.frame_id)
            print("object ", idx, " pose = ", obj.object.primitive_poses[0])

        for obj in find_result.support_surfaces:
            # extend surface to floor, and make wider since we have narrow field of view
            height = obj.primitive_poses[0].position.z
            obj.primitives[0].dimensions = [
                obj.primitives[0].dimensions[0],
                1.5,  # wider
                obj.primitives[0].dimensions[2] + height
            ]
            obj.primitive_poses[0].position.z += -height / 2.0

            # add to scene
            self.scene.addSolidPrimitive(obj.name,
                                         obj.primitives[0],
                                         obj.primitive_poses[0],
                                         use_service=False)

        self.scene.waitForSync()

        # store for grasping
        #self.objects = find_result.objects
        self.surfaces = find_result.support_surfaces

        # store graspable objects by Z
        objects.sort(key=lambda object: object[1])
        objects.reverse()
        self.objects = [object[0] for object in objects]
        #for object in objects:
        #    print(object[0].object.name, object[1])
        #exit(-1)

    def getGraspableObject(self):
        graspable = None
        for obj in self.objects:
            # need grasps
            if len(obj.grasps) < 1:
                continue
            # check size
            if obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25 or \
               obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25 or \
               obj.object.primitives[0].dimensions[0] < 0.03 or \
               obj.object.primitives[0].dimensions[0] > 0.25:
                continue
            # has to be on table
            if obj.object.primitive_poses[0].position.z < 0.5:
                continue
            print obj.object.primitive_poses[0], obj.object.primitives[0]
            return obj.object, obj.grasps
        # nothing detected
        return None, None

    def getSupportSurface(self, name):
        for surface in self.support_surfaces:
            if surface.name == name:
                return surface
        return None

    def getPlaceLocation(self):
        pass

    def pick(self, block, grasps):
        success, pick_result = self.pickplace.pick_with_retry(block.name,
                                                              grasps,
                                                              scene=self.scene)
        self.pick_result = pick_result
        return success

    def place(self, block, pose_stamped):
        places = list()
        l = PlaceLocation()
        l.place_pose.pose = pose_stamped.pose
        l.place_pose.header.frame_id = pose_stamped.header.frame_id

        # copy the posture, approach and retreat from the grasp used
        l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
        l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
        l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
        places.append(copy.deepcopy(l))
        # create another several places, rotate each by 360/m degrees in yaw direction
        m = 16  # number of possible place poses
        pi = 3.141592653589
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(
                l.place_pose.pose, 0, 0, 2 * pi / m)
            places.append(copy.deepcopy(l))

        success, place_result = self.pickplace.place_with_retry(
            block.name, places, scene=self.scene)
        return success

    def tuck(self):
        joints = [
            "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
            "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
            "wrist_roll_joint"
        ]
        pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def stow(self):
        joints = [
            "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
            "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
            "wrist_roll_joint"
        ]
        pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return

    def intermediate_stow(self):
        joints = [
            "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
            "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
            "wrist_roll_joint"
        ]
        pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
        while not rospy.is_shutdown():
            result = self.move_group.moveToJointPosition(joints, pose, 0.02)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                return
コード例 #35
0
ファイル: yw_demo.py プロジェクト: ALAN-NUS/kinova_movo
    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
コード例 #36
0
    def __init__(self, sim=False):
        # Define planning groups
        self.scene = PlanningSceneInterface("base_link")
        self.dof = rospy.get_param('~jaco_dof')
        self.move_group = MoveGroupInterface("upper_body", "base_link")
        self.rmove_group = MoveGroupInterface("right_arm", "base_link")

        planner_id = "RRTConnectkConfigDefault"
        self.move_group.setPlannerId(planner_id)
        self.rmove_group.setPlannerId(planner_id)

        self.objects_heights = [0.122, 0.240]
        self.objects_heights_tolerances = [0.02, 0.03]

        # Define joints and positions for 6 and 7 DOF
        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.00, -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.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
            #self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]

        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.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]

        else:
            rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
            return

        # Define the MoveIt pickplace pipeline
        self.pickplace = None
        self.pickplace = PickPlaceInterface("left_side",
                                            "left_gripper",
                                            verbose=True)
        self.pickplace.planner_id = planner_id
        self.pick_result = None
        self.place_result = None

        self._listener = tf.TransformListener()

        self._lgripper = GripperActionClient('left')

        find_grasp_planning_topic = "grasp_planner/plan"
        rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
        self.find_grasp_planning_client = actionlib.SimpleActionClient(
            find_grasp_planning_topic, GraspPlanningAction)
        self.find_grasp_planning_client.wait_for_server()
        rospy.loginfo("...connected")

        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
コード例 #37
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")


    def getGraspableCube(self):

        # 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 90 degrees in yaw direction
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        places.append(copy.deepcopy(l))
        l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
        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