zps.py 文件源码

def figure_target(self, zarj, previous):
            """ Set the origin """
            self.origin = previous.target
            if abs(self.distance) > 0.001:
                pelvis = zarj.transform.lookup_transform('world', 'pelvis',
                                                         rospy.Time()).transform
                quaternion = [pelvis.rotation.w, pelvis.rotation.x,
                              pelvis.rotation.y, pelvis.rotation.z]
                matrix = quaternion_matrix(quaternion)
                point = np.matrix([0, self.distance, 0, 1], dtype='float32')
                point.resize((4, 1))
                rotated = matrix*point
                self.target = Vector3(self.origin.x - rotated.item(1),
                                      self.origin.y + rotated.item(2),
                                      self.origin.z)
            else:
                self.target = self.origin

            if self.turn is not None:
                self.target_orientation = self.turn + \
                    previous.target_orientation