def cont_dyn(self, X, t, U):
#Fb = np.array([0, 0, 0])
#Mb = np.array([0, 0, 0])
Ueng, Usfc = U[:self.P.eng_nb], U[
self.P.
eng_nb:] # engines and control surfaces parts of input vector
#LOG.info(" engine {} sfc {}".format(Ueng, Usfc))
euler = pal.euler_of_quat(X[self.sv_slice_quat])
omega = X[self.sv_slice_rvel]
R_b2w = pal.rmat_of_quat(X[pat_dyn.SolidFDM.sv_slice_quat])
#pdb.set_trace()
vi_e = X[self.sv_slice_vel]
w_e = [0, 0, 0]
va, alpha, beta = pfw.get_va_alpha_beta(
vi_e, w_e, euler) # fixme: make a fonction that takes vi_b
rho = patm.get_rho(-X[self.sv_z])
Pdyn = 0.5 * rho * va**2
Fb, f_eng_body = get_forces_body(rho, va, alpha, beta, euler, omega,
Pdyn, Ueng, Usfc, self.P)
Mb = get_moments_body(alpha, beta, euler, omega, Pdyn, f_eng_body,
Usfc, self.P)
print(Usfc, Mb)
# try:
# return ps.s1_dyn(X, t, Fb, Mb, P.m, P.I, P.invI, P.inv_mamat)
# except AttributeError:
# return ps.s1_dyn(X, t, Fb, Mb, P.m, P.I, P.invI, None)
return pat_dyn.SolidFDM.cont_dyn(self, X, t, np.concatenate((Fb, Mb)))
def dyn(self, X, t, U, atm):
Ueng = U[self.P.u_slice_eng()] # engines part of input vector
Usfc = U[self.P.u_slice_sfc()] # control surfaces part of input vector
X_pos = X[self.sv_slice_pos] #
X_vel = X[self.sv_slice_vel] #
X_quat = X[self.sv_slice_quat] # quaternion
X_rvel_body = X[self.sv_slice_rvel] # body rotational velocities
earth_to_body_R = p3_alg.rmat_of_quat(X_quat)
wind_ned = atm.get_wind_ned(X_pos, t) if atm is not None else [0, 0, 0]
va, alpha, beta = p3_fr.vel_world_to_aero_quat(X_vel, X_quat, wind_ned)
h = -X[self.sv_z]
rho = p3_atm.get_rho(h)
Pdyn = 0.5 * rho * va**2
f_aero_body = p3_fw_aero.get_f_aero_body(va, alpha, beta, X_rvel_body,
Usfc, self.P, Pdyn)
f_eng_body = p3_fw_aero.get_f_eng_body(h, va, Ueng, self.P)
f_weight_body = np.dot(earth_to_body_R, [0., 0., self.P.m * self.P.g])
forces_body = f_aero_body + np.sum(f_eng_body, axis=0) + f_weight_body
m_aero_body = p3_fw_aero.get_m_aero_body(va, alpha, beta, X_rvel_body,
Usfc, self.P, Pdyn)
m_eng_body = p3_fw_aero.get_m_eng_body(f_eng_body, self.P)
moments_body = m_aero_body + m_eng_body
_U = np.concatenate((forces_body, moments_body))
return p3_fr.SixDOFEuclidianQuat.cont_dyn(X, t, _U, self.P)
def get(self, t, X, Yc):
Xref, Uref, Xrefq = self.setpoint.get(t)
self.Xref = Xref # store that here for now
pos_ref, euler_ref = Xref[r_slice_pos], Xref[r_slice_euler]
# bug...
#self.T_w2b_ref = pal.T_of_t_eu(pos_ref, euler_ref)
self.T_w2b_ref = np.eye(4)
self.T_w2b_ref[:3, :3] = pal.rmat_of_quat(
Xrefq[fdm.sv_slice_quat]).T # that is freaking weird....
self.T_w2b_ref[:3, 3] = Xrefq[fdm.sv_slice_pos]
delta_ut, delta_phi, delta_theta = self.outerloop(X, Xref, Uref)
#print(delta_ut, delta_phi, delta_theta)
delta_euler = np.array([delta_phi, delta_theta, 0])
euler = Xref[r_slice_euler] + delta_euler
qref = pal.quat_of_euler(euler)
#zc, qc = Yc[0], Yc[1:]
zc = Xref[r_z]
Uz = self.z_ctl.run(X, zc)
Upqr = self.att_ctl.run(X, qref)
U = np.dot(self.H, np.hstack((Uz, Upqr)))
#pdb.set_trace()
return U
def get(self, t, X, Xee, Yc):
Xref, Uref, Xrefq, Xrefdq = self.setpoint.get(t)
self.Xref = Xref # store that here for now
pos_ref, euler_ref = Xref[r_slice_pos], Xref[r_slice_euler]
# bug...
#self.T_w2b_ref = pal.T_of_t_eu(pos_ref, euler_ref)
self.T_w2b_ref = np.eye(4)
self.T_w2b_ref[:3, :3] = pal.rmat_of_quat(
Xrefq[fdm.sv_slice_quat]).T # that is freaking weird....
self.T_w2b_ref[:3, 3] = Xrefq[fdm.sv_slice_pos]
delta_ut, delta_phi, delta_theta = self.outerloop(X, Xref, Uref)
#print(delta_ut, delta_phi, delta_theta)
delta_euler = np.array([delta_phi, delta_theta, 0])
euler = Xref[r_slice_euler] + delta_euler
qref = pal.quat_of_euler(euler)
#qref = pal.quat_of_euler(euler_ref) # disable outer loop
rvel_ref = Xref[r_slice_rvel]
Uz = Uref[0] + delta_ut
Upqr = Uref[1:] + self.att_ctl.run(X, qref, rvel_ref)
self.Uzpqr = np.hstack((Uz, Upqr))
if self.output_type == 'multirotor':
U = self.act_alloc.get(self.Uzpqr)
elif self.output_type == 'zpqr':
U = self.Uzpqr
elif self.output_type == 'solid': # Fb, Mb
U = np.array([0, 0, -Uz, Upqr[0], Upqr[1], Upqr[2]])
return U
def cont_dyn(self, X, t, U):
Fb, Mb = [0, 0, -U[0]], U[1:] # thrust, Moments
Dw = -self.P.Cd*X[self.sv_slice_vel]
R_w2b = pal.rmat_of_quat(X[self.sv_slice_quat])
Fb += np.dot(R_w2b, Dw) # drag
Fb -= np.dot(R_w2b, [0., 0., self.P.m*self.P.g]) # lift
return SolidFDM.cont_dyn(self, X, t, np.concatenate((Fb, Mb)))
def periodic(self):
now = rospy.Time.now()
#print('periodic at {}'.format(now.to_sec()))
t_sim = self.t0 + (now.to_sec() - self.t0)*self.time_factor
Yc = self.traj.get(t_sim)
Xc, Uc = self.df.state_and_cmd_of_flat_output(Yc, self.fdm.P)
T_w2b = np.eye(4)
T_w2b[:3,:3] = pal.rmat_of_quat(Xc[fdm.sv_slice_quat]).T # that is freaking weird....
T_w2b[:3,3] = Yc[:3,0]
self.pose_pub.publish([T_w2b])
self.tf_pub.publish(now, T_w2b)
self.traj_pub.publish()
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):
# rotors Thrust in body frame
Fb = [0, 0, -np.sum(U)]
# Drag
Dw = -self.P.Cd * X[sv_slice_vel]
R_w2b = pal.rmat_of_quat(X[sv_slice_quat])
Db = np.dot(R_w2b, Dw)
# Moments of external forces
Mb = np.sum(
[np.cross(_p, [0, 0, -_f]) for _p, _f in zip(self.P.rotor_pos, U)],
axis=0)
# Torques
Mb[2] += np.sum(self.P.k * (self.P.rotor_dir * U))
return SolidFDM.cont_dyn(self, X, t, np.concatenate((Fb + Db, Mb)))
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 periodic(self):
now = rospy.Time.now()
#print('periodic at {}'.format(now.to_sec()))
t_sim = self.t0 + (now.to_sec() - self.t0)*self.time_factor
Yc = self.traj.get(t_sim)
Xc, Uc, Xdc = self.df.state_and_cmd_of_flat_output(Yc, self.fdm.P)
T_w2b = np.eye(4)
T_w2b[:3,:3] = pal.rmat_of_quat(Xc[fdm.sv_slice_quat]).T # that is freaking weird....
T_w2b[:3,3] = Yc[:3,0]
self.pose_pub.publish([T_w2b])
self.tf_pub.publish(now, T_w2b)
self.traj_pub.publish()
X1 = np.zeros(p1_fw_dyn.sv_size)
X1[p1_fw_dyn.sv_slice_pos] = Xc[fdm.sv_slice_pos]
X1[p1_fw_dyn.sv_slice_eul] = pal.euler_of_quat(Xc[fdm.sv_slice_quat])
U = self.guidance.get(t_sim, X1)
self.carrot_pub.publish(self.guidance.carrot)
#b2c_bfrd = np.dot(T_w2b[:3, :3].T, self.guidance.b2c_ned)
#print self.guidance.carrot
#b2c_bfrd = np.dot(np.linalg.inv(T_w2b), [self.guidance.carrot[0], self.guidance.carrot[1], self.guidance.carrot[2], 1])
b2c_bfrd = self.guidance.b2c_b
self.carrot_pub2.publish(b2c_bfrd)
def vel_body_to_world_quat(ivel_body, quat):
body_to_world_R = p3_alg.rmat_of_quat(quat).T
return vel_body_to_world_R(ivel_body, body_to_world_R)
def cont_dyn(self, X, t, U):
Fb, Mb = [0, 0, -U[0]], U[1:] # thrust, Moments
Dw = -self.P.Cd * X[sv_slice_vel]
Fb += np.dot(pal.rmat_of_quat(X[sv_slice_quat]), Dw) # drag
return SolidFDM.cont_dyn(self, X, t, np.concatenate((Fb, Mb)))
def update_byproducts(self):
self.T_w2b[:3, :3] = pal.rmat_of_quat(
self.X[sv_slice_quat]).T # that is freaking weird....
self.T_w2b[:3, 3] = self.X[sv_slice_pos]
def vel_world_to_aero_quat(ivel_world, quat, wind_ned):
'''
'''
world_to_body_R = p3_alg.rmat_of_quat(quat)
return vel_world_to_aero_R(ivel_world, world_to_body_R, wind_ned)
def _update_byproducts(self):
self.T_w2b[:3, :3] = p3_alg.rmat_of_quat(self.X[self.sv_slice_quat]).T
self.T_w2b[:3, 3] = self.X[self.sv_slice_pos]