Python Executor.gripper_close方法代码示例

# 需要导入模块: from executor import Executor [as 别名]
# 或者: from executor.Executor import gripper_close [as 别名]

#.........这里部分代码省略.........
        self._executor.move_to_pose(approach)

    def _retract(self):
        # retrieve current pose from endpoint
        retract = copy.deepcopy(self._executor.get_current_pose())
        retract.position.z = retract.position.z + self._hover_distance
        self._executor.move_to_pose(retract)

    def transfer(self, start_pose, end_pose, obstacles):

        print("Picking up the object")
        self.pick(start_pose)

        joint_names = self._executor.joint_names()
        joints = self._executor.ik_request(start_pose)
        start_joints = [joints[i] for i in joint_names]
        joints = self._executor.ik_request(end_pose)
        end_joints = [joints[i] for i in joint_names]

        print start_joints
        print end_joints

        print "Adapting Trajectory Learned from Demonstrations"
        resp = self._trajsvc(start_joints, end_joints, obstacles, True)
#         resp = AdaptationResponse()
#         resp.filename = '/home/aecins/ros_ws/src/baxter_adapt/MPC/generated/JointsTrajectory'
#         resp.response = True
        print resp

        if resp.response is True:
            while os.path.isfile(resp.filename) is False:
                print "Wait for trajectory generation"
                rospy.sleep(2.0)
            print("Perform trajectory")
            traj = open(resp.filename, 'r').readlines()
            self._executor.move_as_trajectory(traj)

    def transfer_imitation(self, start_pose, end_pose):
        print("Picking up the object")
        self.pick(start_pose)

        print("Computing imitation trajectory")
        joint_names = self._executor.joint_names()
        joints = self._executor.ik_request(start_pose)
        start_joints = [joints[i] for i in joint_names]
        joints = self._executor.ik_request(end_pose)
        end_joints = [joints[i] for i in joint_names]

        print start_joints
        print end_joints

        obstacles = []
        resp = self._trajsvc(start_joints, end_joints, obstacles, False)
        print resp

        if resp.response is True:
            print("Perform trajectory")
            traj = open(resp.filename, 'r').readlines()
            self._executor.move_as_trajectory(traj)

    def get_current_joints(self):
        return self._executor.get_current_joints()

    def get_current_pose(self):
        return self._executor.get_current_pose()

    def pick(self, pose):
        # open the gripper
        self._executor.gripper_open()
        # servo above pose
        self._approach(pose)
        # servo to pose
        self._executor.move_to_pose(pose)
        # close gripper
        self._executor.gripper_close()
        # retract to clear object
        self._retract()

    def place(self, pose):
        # servo above pose
        #self._approach(pose)
        # servo to pose
        self._executor.move_to_pose(pose)
        # open the gripper
        self._executor.gripper_open()
        # retract to clear object
        self._retract()

    def learning(self):
        print('Learning weights from feedback')
        feedback = self.get_feedback()
        resp = self._learningsvc(feedback)
        print('Updated weights for movement adaptation')

    def get_feedback(self):
        filename = os.getcwd() + '/trajectory_improved'
        print('Please help me improve the task.')
        self._executor.record(filename)
        print('Done')
        return filename

# 需要导入模块: from executor import Executor [as 别名]
# 或者: from executor.Executor import gripper_close [as 别名]
class Task(object):
    def __init__(self, limb, hover_distance = 0.15, verbose=True):
        self._limb_name = limb # string
        self._hover_distance = hover_distance # in meters
        self._verbose = verbose # bool
        self._executor = Executor(limb, verbose)
        trajsvc = "baxter_adapt/imitation_server"
        rospy.wait_for_service(trajsvc, 5.0)
        self._trajsvc = rospy.ServiceProxy(trajsvc, Imitation)

    def move_to_start(self, start_angles=None):
        print("Moving the {0} arm to start pose...".format(self._limb_name))
        if not start_angles:
            start_angles = dict(zip(self._joint_names, [0]*7))
        self._executor.move_to_joint(start_angles)
        self._executor.gripper_open()
        rospy.sleep(1.0)
        print("Running. Ctrl-c to quit")

    def _approach(self, pose):
        approach = copy.deepcopy(pose)
        # approach with a pose the hover-distance above the requested pose
        approach.position.z = approach.position.z + self._hover_distance
        self._executor.move_to_pose(approach)

    def _retract(self):
        # retrieve current pose from endpoint
        retract = copy.deepcopy(self._executor.get_current_pose())
        retract.position.z = retract.position.z + self._hover_distance
        self._executor.move_to_pose(retract)

    def transfer(self, pose):
        # request Matlab to compute trajectory
        pose_start = self._executor.get_current_pose()
        y_start = pose_start.position
        y_end = pose.position
        resp = self._trajsvc(y_start, y_end)
        print resp

        if resp.response is True:
            # perform the trajectory via Inverse Kinematics
            self._executor.move_as_trajectory(resp.filename)

    def pick(self, pose):
        # open the gripper
        self._executor.gripper_open()
        # servo above pose
        self._approach(pose)
        # servo to pose
        self._executor.move_to_pose(pose)
        # close gripper
        self._executor.gripper_close()
        # retract to clear object
        #self._retract()

    def place(self, pose):
        # servo above pose
        self._approach(pose)
        # servo to pose
        self._executor.move_to_pose(pose)
        # open the gripper
        self._executor.gripper_open()
        # retract to clear object
        self._retract()