safety_stop.py 文件源码

def _transform_vector(self, v, mode='speed', to='base'):
        h = Header()
        h.stamp = rospy.Time(0)
        h.frame_id = '{}_gripper'.format(self.bx.limb_name)
        v = Vector3(*v)
        v_base = self.tl.transformVector3(to,
                                          Vector3Stamped(h, v)).vector
        if mode == 'speed':
            v_cartesian = np.array([0, 0, 0, v_base.x, v_base.y, v_base.z])
        elif mode == 'direction':
            v_cartesian = np.array([v_base.x, v_base.y, v_base.z, 0, 0, 0])
        return v_cartesian