def solid_cont_dyn(X, F_b, M_b, P):
Xd = np.zeros(sv_size)
p_w, v_w, q_w2b, om_b = X[sv_slice_pos], X[sv_slice_vel], X[sv_slice_quat], X[sv_slice_rvel]
# Translational kinematics
Xd[sv_slice_pos] = v_w
# Newton for forces
R_w2b = pal.rmat_of_quat(q_w2b)
Xd[sv_slice_vel] = 1./P.m*(np.dot(R_w2b.T, F_b) + [0, 0, P.m*P.g])
# Rotational kinematics
Xd[sv_slice_quat] = pal.quat_derivative(q_w2b, om_b)
# Newton for moments
Xd[sv_slice_rvel] = np.dot(P.invJ, M_b - np.cross(om_b, np.dot(P.J, om_b)))
return Xd
def cont_dyn(self, X, t, U):
Fb, Mb = U[:3], U[3:]
Xd = np.zeros(self.sv_size)
p_w, v_w, q_w2b, om_b = X[self.sv_slice_pos], X[self.sv_slice_vel], X[self.sv_slice_quat], X[self.sv_slice_rvel]
# Translational kinematics
Xd[self.sv_slice_pos] = v_w
# Newton for forces
R_w2b = pal.rmat_of_quat(q_w2b)
Xd[self.sv_slice_vel] = 1./self.P.m*(np.dot(R_w2b.T, Fb) + [0, 0, self.P.m*self.P.g])
#Xd[self.sv_slice_vel] = 1./self.P.m*(np.dot(R_w2b.T, Fb))
# Rotational kinematics
Xd[self.sv_slice_quat] = pal.quat_derivative(q_w2b, om_b)
# Newton for moments
Xd[self.sv_slice_rvel] = np.dot(self.P.invJ, Mb - np.cross(om_b, np.dot(self.P.J, om_b)))
return Xd
def cont_dyn(cls, X, t, U, P, add_weight=False):
Fb, Mb = U[:3], U[3:]
Xd = np.zeros(cls.sv_size)
p_w, ivel_b, q_w2b, rvel_b = X[cls.sv_slice_pos], X[
cls.sv_slice_vel], X[cls.sv_slice_quat], X[cls.sv_slice_rvel]
R_b2w = p3_alg.rmat_of_quat(q_w2b).T
# Translational kinematics
Xd[cls.sv_slice_pos] = vel_body_to_world_R(ivel_b, R_b2w)
# Newton for forces
Xd[cls.sv_slice_vel] = 1 / P.m * Fb - 2. * np.cross(rvel_b, ivel_b)
# do we add weight or not???
#if add_weight: Xd[cls.sv_slice_vel] += 1./P.m*(np.array([0, 0, P.m*P.g]))
# Rotational kinematics
Xd[cls.sv_slice_quat] = p3_alg.quat_derivative(q_w2b, rvel_b)
# Newton for moments
Xd[cls.sv_slice_rvel] = np.dot(
P.invI, Mb - np.cross(rvel_b, np.dot(P.I, rvel_b)))
return Xd
def cont_dyn(cls, X, t, U, P, add_weight=False):
Fb, Mb = U[:3], U[3:]
Xd = np.zeros(cls.sv_size)
p_w, v_w, q_w2b, om_b = X[cls.sv_slice_pos], X[cls.sv_slice_vel], X[
cls.sv_slice_quat], X[cls.sv_slice_rvel]
# Translational kinematics
Xd[cls.sv_slice_pos] = v_w
# Newton for forces
R_b2w = p3_alg.rmat_of_quat(q_w2b).T
Xd[cls.sv_slice_vel] = 1. / P.m * (np.dot(R_b2w, Fb))
# do we add weight or not???
if add_weight:
Xd[cls.sv_slice_vel] += 1. / P.m * (np.array([0, 0, P.m * P.g]))
# Rotational kinematics
Xd[cls.sv_slice_quat] = p3_alg.quat_derivative(q_w2b, om_b)
# Newton for moments
Xd[cls.sv_slice_rvel] = np.dot(P.invI,
Mb - np.cross(om_b, np.dot(P.I, om_b)))
return Xd
def state_and_cmd_of_flat_output(self, Y, P):
#pdb.set_trace()
wind = np.zeros(3)
cd_ov_m = P.Cd / P.m #param[prm_Cd]/param[prm_mass]
a0 = np.array([
Y[_x, 2] + cd_ov_m * (Y[_x, 1] - wind[_x]),
Y[_y, 2] + cd_ov_m * (Y[_y, 1] - wind[_y]),
Y[_z, 2] + cd_ov_m * (Y[_z, 1] - wind[_z]) - 9.81
])
a1 = np.array([
Y[_x, 3] + cd_ov_m * Y[_x, 2], Y[_y, 3] + cd_ov_m * Y[_y, 2],
Y[_z, 3] + cd_ov_m * Y[_z, 2]
])
a2 = np.array([
Y[_x, 4] + cd_ov_m * Y[_x, 3], Y[_y, 4] + cd_ov_m * Y[_y, 3],
Y[_z, 4] + cd_ov_m * Y[_z, 3]
])
psi = Y[_psi, 0]
cpsi, spsi = np.cos(psi), np.sin(psi)
psi1 = Y[_psi, 1]
psi2 = Y[_psi, 2]
b0 = np.array([
cpsi * a0[_x] + spsi * a0[_y], -spsi * a0[_x] + cpsi * a0[_y],
a0[_z]
])
b1 = np.array([
cpsi * a1[_x] + spsi * a1[_y] - psi1 *
(spsi * a0[_x] - cpsi * a0[_y]), -spsi * a1[_x] + cpsi * a1[_y] -
psi1 * (cpsi * a0[_x] + spsi * a0[_y]), a1[_z]
])
b2 = np.array([
cpsi * a2[_x] + spsi * a2[_y] - 2 * psi1 *
(spsi * a1[_x] - cpsi * a1[_y]) +
(-psi2 * spsi - psi1**2 * cpsi) * a0[_x] +
(psi2 * cpsi - psi1**2 * spsi) * a0[_y], -spsi * a2[_x] +
cpsi * a2[_y] - 2 * psi1 * (cpsi * a1[_x] + spsi * a1[_y]) +
(-psi2 * cpsi + psi1**2 * spsi) * a0[_x] +
(-psi2 * spsi - psi1**2 * cpsi) * a0[_y], a2[_z]
])
c0 = math.sqrt(b0[_x]**2 + b0[_z]**2)
c1 = (b0[_x] * b1[_x] + b0[_z] * b1[_z]) / c0
c2 = (b1[_x]**2 + b0[_x] * b2[_x] + b1[_z]**2 + b0[_z] * b2[_z] -
c1**2) / c0
n2a = a0[_x]**2 + a0[_y]**2 + a0[_z]**2
na = math.sqrt(n2a)
# euler
phi0 = -np.sign(b0[_z]) * math.atan(b0[_y] / c0)
theta0 = math.atan(b0[_x] / b0[_z])
# euler dot
phi1 = (b1[_y] * c0 - b0[_y] * c1) / n2a # checked
theta1 = (b1[_x] * b0[_z] - b0[_x] * b1[_z]) / c0**2 # checked
# rvel
cph = math.cos(phi0)
sph = math.sin(phi0)
cth = math.cos(theta0)
sth = math.sin(theta0)
p = phi1 - sth * psi1
q = cph * theta1 + sph * cth * psi1
r = -sph * theta1 + cph * cth * psi1
# euler dot dot
phi2 = (b2[_y]*c0 - b0[_y]*c2)/na**2 \
-2*(b1[_y]*c0-b0[_y]*c1)*(b0[_x]*b1[_x]+b0[_y]*b1[_y]+b0[_z]*b1[_z])/na**4 # checked
theta2 = (b2[_x]*b0[_z] - b0[_x]*b2[_z])/c0**2 \
-2*(b1[_x]*b0[_z] - b0[_x]*b1[_z])*(b0[_x]*b1[_x]+b0[_z]*b1[_z])/c0**4 # checked
# raccel
p1 = phi2 - cth * theta1 * psi1 - sth * psi2
q1 = -sph * phi1 * theta1 + cph * theta2 + cph * cth * phi1 * psi1 - sph * sth * theta1 * psi1 + sph * cth * psi2
r1 = -cph * phi1 * theta1 - sph * theta2 - sph * cth * phi1 * psi1 - cph * sth * theta1 * psi1 + cph * cth * psi2
_q = pal.quat_of_euler([phi0, theta0, psi])
X = np.array([
Y[_x, 0], Y[_y, 0], Y[_z, 0], Y[_x, 1], Y[_y, 1], Y[_z, 1], _q[0],
_q[1], _q[2], _q[3], p, q, r
])
Ut = na * P.m
Jxx, Jyy, Jzz = np.diag(P.J)
#Up = Jxx/P.l * p1 + (Jzz-Jyy)/P.l*q*r
#Uq = Jyy/P.l * q1 + (Jxx-Jzz)/P.l*p*r
#Ur = Jzz/P.k * r1 + (Jyy-Jxx)/P.k*p*q
Up = Jxx * p1 + (Jzz - Jyy) * q * r
Uq = Jyy * q1 + (Jxx - Jzz) * p * r
Ur = Jzz * r1 + (Jyy - Jxx) * p * q
U = np.array([Ut, Up, Uq, Ur])
qdot = pal.quat_derivative(_q, [p, q, r])
Xd = np.array([
Y[_x, 1], Y[_y, 1], Y[_z, 1], Y[_x, 2], Y[_y, 2], Y[_z, 2],
qdot[pal.q_i], qdot[pal.q_x], qdot[pal.q_y], qdot[pal.q_z], p1, q1,
r1
])
return X, U, Xd