Python quat_from_ypr 예제들

예제 #1

파일: run_rigid_body_kinematics_Test.py 프로젝트: donnel2-cooper/drone_control

import rigid_body as rb
import numpy as np
import integrators as intg
import aerosonde_uav as uav
import matplotlib.pyplot as plt

# Initial condition of angles
yaw0 = np.deg2rad(0)
pitch0 = np.deg2rad(0)
roll0 = np.deg2rad(0)
quat0 = rb.quat_from_ypr(yaw0,pitch0,roll0)

# X0 = [x,y,z,
#       u,v,w,
#       quat1,quat2,quat3, quat4,
#       p,q,r]

x0 = np.array([0,0,0,\
               0,0,0,\
               float(quat0[0]),float(quat0[1]),float(quat0[2]),float(quat0[3]),\
               np.deg2rad(10),np.deg2rad(0),np.deg2rad(0)])        
x0 = x0.reshape(x0.shape[0],1)

drone = rb.RigidBody(uav.m,uav.J,x0)
t = 0
# u = [fx,fy,fz,Mx,My,Mz]
u = np.array([0,0,0,\
              0,0,0])
u = u.reshape(u.shape[0],1)

tf = 10; dt = 1e-2; n = int(np.floor(tf/dt));

예제 #2

파일: unit_tests.py 프로젝트: donnel2-cooper/drone_control

                                      np.array([[0],[1],[0],[0]]),tol))
checks.append(within_tol(rb.quat_prod(np.array([[0],[0],[1],[0]]),\
                                      np.array([[1],[0],[0],[0]])),\
                                      np.array([[0],[0],[1],[0]]),tol))
checks.append(within_tol(rb.quat_prod(np.array([[0],[0],[0],[1]]),\
                                      np.array([[1],[0],[0],[0]])),\
                                      np.array([[0],[0],[0],[1]]),tol))
checks.append(within_tol(rb.quat_prod(np.array([[0],[1],[0],[0]]),\
                                      np.array([[0],[0],[1],[0]])),\
                                      np.array([[0],[0],[0],[-1]]),tol))
checks.append(within_tol(rb.quat_prod(np.array([[0],[1],[0],[0]]),\
                                      np.array([[0],[0],[0],[1]])),\
                                      np.array([[0],[0],[1],[0]]),tol))

## Unit tests for rb.quat_from_ypr
checks.append(within_tol(rb.quat_from_ypr(0, 0, 0),\
                       np.array([[1],[0],[0],[0]]),tol))
checks.append(within_tol(rb.quat_from_ypr(np.pi/2, 0, 0),\
                       np.array([[0.707106781186548],[0],[0],[0.707106781186547]]),tol))
checks.append(within_tol(rb.quat_from_ypr(-np.pi/2, 0, 0),\
                       np.array([[0.707106781186548],[0],[0],[-0.707106781186547]]),tol))
checks.append(within_tol(rb.quat_from_ypr(0, np.pi/2, 0),\
                       np.array([[0.707106781186548],[0],[0.707106781186547],[0]]),tol))
checks.append(within_tol(rb.quat_from_ypr(0, -np.pi/2, 0),\
                       np.array([[0.707106781186548],[0],[-0.707106781186547],[0]]),tol))
checks.append(within_tol(rb.quat_from_ypr(0, 0, np.pi/2 ),\
                       np.array([[0.707106781186548],[0.707106781186547],[0],[0]]),tol))
checks.append(within_tol(rb.quat_from_ypr(0, 0, -np.pi/2 ),\
                       np.array([[0.707106781186548],[-0.707106781186547],[0],[0]]),tol))
checks.append(within_tol(rb.quat_from_ypr(np.pi/180*47,np.pi/180*15,np.pi/180*6),\
                       np.array([[0.910692391306739],[-0.004391258543109],\