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]