def __init__(self):
super().__init__(self)
self.rigid_body = rb.RigidBody()
self.added_mass_matrix = np.zeros((6,6))
self.first_order_damping_matrix = np.zeros((6,6))
self.second_order_damping_matrix = np.zeros((6,6))
self.restoring_matrix = np.zeros((6,6))
def get_rigid_body_data(ampal):
try:
com_bb = [unit.backbone.centre_of_mass for unit in ampal]
com_aa = [unit.centre_of_mass for unit in ampal]
#####################################
ampal_rb = rigid_body.RigidBody(ampal)
com_primitives = ampal_rb.get_chains_com()
com_assembly = ampal_rb.get_assembly_com()
#####################################
euler_angles = ampal_rb.euler_angles() # Based on chain primitives
#####################################
# Convert to list format for JSON serialisation
com_bb = list(map(list, com_bb))
com_aa = list(map(list, com_aa))
com_primitives = list(map(list, com_primitives))
com_assembly = list(com_assembly)
euler_angles = list(map(list, euler_angles))
#####################################
return com_bb, com_aa, com_primitives, com_assembly, euler_angles
except:
return 'Fail'
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));
integrator = intg.RungeKutta4(dt, drone.f)
x = x0
t_history = [0]
x_history = [x]
for i in range(n):
x = integrator.step(t, drone.x, u)
def __init__(self):
self.rigid_body = rb.RigidBody()
self.added_mass = np.zeros((6, 6))
self.damping = np.zeros((6, 6))
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],\
[0.140226691736355],[0.388531285984923]]),tol))
## Unit tests for rb.RigidBody init
m = 1
J = np.eye(3)
x0 = np.array(
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
x0 = x0.reshape(x0.shape[0], 1)
drone = rb.RigidBody(m, J, x0)
checks.append(within_tol(m, drone.m, tol))
checks.append(within_tol(J, drone.J, tol))
checks.append(within_tol(x0, drone.x, tol))
m = uav.m
J = uav.J
x0 = np.array([
1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 0.0, 1 / np.sqrt(2), 1 / np.sqrt(2), 0.0,
7.0, 8.0, 9.0
])
x0 = x0.reshape(x0.shape[0], 1)
drone = rb.RigidBody(m, J, x0)
checks.append(within_tol(m, drone.m, tol))
def __init__(self):
self.rigid_body = rb.RigidBody()