Python quaternions.quat2mat方法代碼示例

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def _check_transform(self, translation: Optional[np.ndarray],
                         rotation: Optional[np.ndarray]):
        """Checks that the given translation and rotation are valid."""
        if translation is None:
            pass
        elif translation.shape != (3,):
            raise ValueError('Expected translation to have shape 3')
        else:
            translation = translation.copy()
        if rotation is None:
            pass
        elif rotation.shape == (4,):
            rotation = quat2mat(rotation)
        elif rotation.shape == (3, 3):
            rotation = rotation.copy()
        else:
            raise ValueError('Expected rotation to have shape (3, 3)')
        return translation, rotation 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def poseToMatrix(self, pose):
        """Converts a pyBullet pose to a transformation matrix"""
        translation = pose[0]
        quaternion = pose[1]
        
        # NOTE: PyBullet quaternions are x, y, z, w
        rotation = quat2mat([quaternion[3], quaternion[0],
                             quaternion[1], quaternion[2]])

        m = np.identity(4)
        m[0:3, 0:3] = rotation
        m.T[3, 0:3] = translation

        return np.matrix(m) 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def tq2mat(tq): 
    """ transform translation-quaternion (tq) to (4x4) matrix
    """
    tq = np.array(tq)
    T = np.eye(4)
    T[:3,:3] = quaternions.quat2mat(np.roll(tq[3:], 1))
    T[:3, 3] = tq[:3]
    return T 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def move_forward(self, forward=0.05):
        x, y, z, w = self.robot_body.get_orientation()
        self.move_by(quat2mat([w, x, y, z]).dot(np.array([forward, 0, 0]))) 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def move_backward(self, backward=0.05):
        x, y, z, w = self.robot_body.get_orientation()
        self.move_by(quat2mat([w, x, y, z]).dot(np.array([-backward, 0, 0]))) 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def dist_to_semantic_pos(self):
        pos = self.robot.get_position()
        x, y, z, w = self.robot.get_orientation()
        #print(self.semantic_pos)
        #print(pos, orn)

        diff_pos = self.semantic_pos - pos
        dist_to_robot = np.sqrt(np.sum(diff_pos * diff_pos, axis = 1))
        diff_unit = (diff_pos.T / dist_to_robot).T

        #TODO: (hzyjerry) orientation is still buggy
        orn_unit = quat2mat([w, x, y, z]).dot(np.array([-1, 0, 0]))
        orn_to_robot = np.arccos(diff_unit.dot(orn_unit))
        return dist_to_robot, orn_to_robot 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def _translateIntrinsic(self, pos_world_xyz, quat_world_xyzw):
        """ Add intrinsic translation to extrinsic
        Extrinsic translation = Intrinsic delta translation +
            Extrinsic translation

        Note: order doesn't matter
        """
        delta_objec_xyz = self.d_xyz
        quat_world_wxyz = PhysicsObject.quatXyzwToWxyz(quat_world_xyzw)
        delta_world_xyz = quaternions.quat2mat(quat_world_wxyz).dot(delta_objec_xyz)
        return pos_world_xyz + delta_world_xyz 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def move_forward(self, forward=0.10):
        x, y, z, w = self.robot_body.get_orientation()
        self.move_by(quat2mat([w, x, y, z]).dot(np.array([forward, 0, 0])))
        yaw = self.robot_body.bp_pose.rpy()[2]
        self.robot_body.reset_orientation(quatToXYZW(euler2quat(0, 0, yaw), 'wxyz')) 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def move_backward(self, backward=0.14):
        x, y, z, w = self.robot_body.get_orientation()
        self.move_by(quat2mat([w, x, y, z]).dot(np.array([-backward, 0, 0]))) 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def get_rot(self, sim_scene: SimScene) -> np.ndarray:
        """Returns the (3x3) rotation matrix of the element."""
        if self.qpos_indices is not None:
            qpos = sim_scene.data.qpos[self.qpos_indices[3:]]
            if self._is_euler:
                return euler2mat(*qpos, axes='rxyz')
            return quat2mat(qpos)
        return self.element_attr(sim_scene.data,
                                 'xmat')[self.element_id].reshape((3, 3)) 

# 需要導入模塊: from transforms3d import quaternions [as 別名]
# 或者: from transforms3d.quaternions import quat2mat [as 別名]
def _get_raw_tracker_state(self, device_id: DeviceId):
        """Returns the tracker state."""
        try:
            rigid_data = self._poses.get_rigid(device_id)
            state = TrackerState(
                pos=rigid_data.position * self._position_scale,
                rot=quat2mat(rigid_data.rotation),
                vel=np.zeros(3),
                angular_vel=np.zeros(3))
            self._state_cache[device_id] = state
        except:
            if device_id not in self._state_cache:
                raise
            state = self._state_cache[device_id]
        return state