move_group.py 文件源码

def set_pose_target(self, pose, end_effector_link = ""):
        """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
        """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
        if len(end_effector_link) > 0 or self.has_end_effector_link():
            ok = False
            if type(pose) is PoseStamped:
                old = self.get_pose_reference_frame()
                self.set_pose_reference_frame(pose.header.frame_id)
                ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link)
                self.set_pose_reference_frame(old)
            elif type(pose) is Pose:
                ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link)
            else:
                ok = self._g.set_pose_target(pose, end_effector_link)
            if not ok:
                raise MoveItCommanderException("Unable to set target pose")
        else:
            raise MoveItCommanderException("There is no end effector to set the pose for")