Python euler.euler2quat方法代码示例

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def get_quaternion_training_data_from_bvh(bvh_filename):
    data=parse_frames(bvh_filename)
    
    train_data = []
    for raw_frame in data:
        hip_pose = raw_frame[0:3].tolist()
        hip_euler =raw_frame[3:6]
        hip_quaternion=euler.euler2quat(hip_euler[2]/180.0*np.pi, hip_euler[1]/180.0*np.pi, hip_euler[0]/180.0*np.pi).tolist()
        frame_data=hip_pose+hip_quaternion
        euler_rotations = raw_frame[6:len(raw_frame)].reshape(-1,3)
        
        euler_index=0
        for joint in skeleton:
            if(("hip" not in joint) and  (len(skeleton[joint]["channels"])==3)):
                euler_rotation=euler_rotations[euler_index]
                quaternion = euler.euler2quat(euler_rotation[2]/180.0*np.pi,euler_rotation[1]/180.0*np.pi,euler_rotation[0]/180.0*np.pi, axes="syxz").tolist() #z x y
                frame_data=frame_data+quaternion
                euler_index=euler_index+1
            elif(("hip" not in joint) and  (len(skeleton[joint]["channels"])==0)):
                frame_data=frame_data+[1,0,0,0]
        train_data=train_data + [frame_data]
    train_data=np.array(train_data)
    return train_data 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def _cameraUncalibrate(self, new_quat_xyzw):
        """ Undo the effect of _cameraCalibrate
        Needed in two cases:
        (1) Updating pose in physics engine

            self._cameraUnalibrate(sim.getBasePositionAndOrientation(uid))

        This is because object orientation in physics engine has
        been calibrated. To update parameter (alpha, beta, gamma),
        need to uncalibrate first. See updatePositionOrientation()
          (2) Sending pose to viewer renderer

             send(self._cameraUnalibrate(sim.See updatePositionOrientation(uid)))

        This is because we need to send uncalibrated view pose
        """
        x_facing_wxyz = euler.euler2quat(np.pi/2, 0, -np.pi/2)
        new_quat_wxyz = PhysicsObject.quatXyzwToWxyz(new_quat_xyzw)
        org_quat_wxyz = PhysicsObject.quatWxyzToXyzw(quaternions.qmult(new_quat_wxyz, x_facing_wxyz))
        return org_quat_wxyz 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def poses_euler2quat(poses):
	# Arguments:
		# poses: 			6D pose (translation + euler) (batch_size x 6)
	# Output: 
		# new_poses: 		7D pose (translation + quaternions) (batch_size x 7)

	new_poses = []					# Store 7D poses
	for i in range(poses.shape[0]):
		temp = t3d.euler2quat(poses[i,3],poses[i,4],poses[i,5])						# Convert from euler to quaternion. (1x4)
		temp1 = [poses[i,0],poses[i,1],poses[i,2],temp[0],temp[1],temp[2],temp[3]]		# Add translation & Quaternion (1x7)
		new_poses.append(temp1)												
	return np.asarray(new_poses)									

# Geenerate random poses equal to batch_size. 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def _distance_from_fixed_angle(angle, fixed_angle):
    """Designed to be mapped, this function finds the smallest rotation between
    two rotations. It assumes a no-symmettry system.

    The algorithm involves converting angles to quarternions, then finding the
    appropriate misorientation. It is tested against the slower more complete
    version finding the joining rotation.

    Parameters
    ----------
    angle : np.array()
        Euler angles representing rotation of interest.
    fixed_angle : np.array()
        Euler angles representing fixed reference rotation.

    Returns
    -------
    theta : np.array()
        Rotation angle between angle and fixed_angle.

    """
    angle = angle[0]
    q_data = euler2quat(*np.deg2rad(angle), axes="rzxz")
    q_fixed = euler2quat(*np.deg2rad(fixed_angle), axes="rzxz")
    if np.abs(2 * (np.dot(q_data, q_fixed)) ** 2 - 1) < 1:  # arcos will work
        # https://math.stackexchange.com/questions/90081/quaternion-distance
        theta = np.arccos(2 * (np.dot(q_data, q_fixed)) ** 2 - 1)
    else:  # slower, but also good
        q_from_mode = qmult(qinverse(q_fixed), q_data)
        axis, theta = quat2axangle(q_from_mode)
        theta = np.abs(theta)

    return np.rad2deg(theta) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def get_distance_between_two_angles_longform(angle_1, angle_2):
    """
    Using the long form to find the distance between two angles in euler form
    """
    q1 = euler2quat(*np.deg2rad(angle_1), axes="rzxz")
    q2 = euler2quat(*np.deg2rad(angle_2), axes="rzxz")
    # now assume transform of the form MODAL then Something = TOTAL
    # so we want to calculate MODAL^{-1} TOTAL

    q_from_mode = qmult(qinverse(q2), q1)
    axis, angle = quat2axangle(q_from_mode)
    return np.rad2deg(angle) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def turn_left(self, delta=0.03):
        orn = self.robot_body.get_orientation()
        new_orn = qmult((euler2quat(-delta, 0, 0)), orn)
        self.robot_body.set_orientation(new_orn) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def turn_right(self, delta=0.03):
        orn = self.robot_body.get_orientation()
        new_orn = qmult((euler2quat(delta, 0, 0)), orn)
        self.robot_body.set_orientation(new_orn) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def _cameraCalibrate(self, org_quat_xyzw):
        """ Convert object's head from +x facing to -z facing
        Note that this is not needed when you're computing view_matrix,
        only use this function for adjusting object head
        To get object rotation at current object pose:
            rotation = self._cameraCalibrate(self._rotateIntrinsic(
                   self.quat_world))
        To get view rotation at current object pose:
              rotation = self._rotateIntrinsic(self.quat_world)
          """
        z_facing_wxyz = euler.euler2quat(-np.pi/2, np.pi/2, 0)
        org_quat_wxyz = PhysicsObject.quatXyzwToWxyz(org_quat_xyzw)
        new_quat_xyzw = PhysicsObject.quatWxyzToXyzw(quaternions.qmult(org_quat_wxyz, z_facing_wxyz))
        return new_quat_xyzw 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def _rotateIntrinsic(self, quat_world_xyzw):
        """ Add intrinsic rotation to extrinsic
        Extrinsic rotation = Extrinsic rotation *
            Intrinsic delta rotation

        Note: order matters. Apply intrinsic delta quat first, then
        extrinsic rotation
        """
        quat_world_wxyz = PhysicsObject.quatXyzwToWxyz(quat_world_xyzw)
        euler_world     = euler.quat2euler(quat_world_wxyz)

        quat_objec_wxyz = euler.euler2quat(self.d_alpha, self.d_beta, self.d_gamma)
        new_quat_world_wxyz  = quaternions.qmult(quat_world_wxyz, quat_objec_wxyz)
        new_quat_world_xyzw  = PhysicsObject.quatWxyzToXyzw(new_quat_world_wxyz)
        return new_quat_world_xyzw 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [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 euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def turn_left(self, delta=0.24):
        orn = self.robot_body.get_orientation()
        new_orn = qmult((euler2quat(-delta, 0, 0)), orn)
        self.robot_body.set_orientation(new_orn) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def reset(self):
        if self.robot_ids is None:
            self._load_model()
        
        self.robot_body.reset_orientation(quatToXYZW(euler2quat(*self.config["initial_orn"]), 'wxyz'))
        self.robot_body.reset_position(self.config["initial_pos"])
        self.reset_random_pos()
        self.robot_specific_reset()
        
        state = self.calc_state()
        return state 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def test_identity(self):
        """Average one quaternion should equal itself."""
        test_quat = euler2quat(np.pi / 4, np.pi / 4, np.pi / 4)
        avg_quat = average_quaternions([test_quat])
        np.testing.assert_array_almost_equal(avg_quat, test_quat) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def test_multiple_identity(self):
        """Average multiple copies of a quaternion should equal itself."""
        test_quat = euler2quat(np.pi / 4, np.pi / 4, np.pi / 4)
        avg_quat = average_quaternions([test_quat, test_quat, test_quat])
        np.testing.assert_array_almost_equal(avg_quat, test_quat) 

# 需要导入模块: from transforms3d import euler [as 别名]
# 或者: from transforms3d.euler import euler2quat [as 别名]
def test_average_two(self):
        """Averaging two different quaternions."""
        quat1 = euler2quat(np.pi / 4, 0, 0)
        quat2 = euler2quat(-np.pi / 4, 0, 0)
        avg_quat = average_quaternions([quat1, quat2])
        result = quat2euler(avg_quat)
        np.testing.assert_array_almost_equal(result, [0, 0, 0])