def testcase_template_C():
""" Testcase template for initial conditions and constant input """
quad = rb.rigidbody_q(pos, q, ve, omegab, mass, I)
for t in np.arange(dt_sim, T_sim + dt_sim, dt_sim):
# Run the dynamic / time forward
quad.run_quadrotor(dt_sim, Fb, taub)
# Logging frequency
if (abs(t % dt_log) < 0.00001):
quad.check()
# Simulation parameters dt_sim = 0.001 # seconds T_sim = 10 # seconds dt_log = 1 # seconds # Test zero forces and moments quad = rb.rigidbody(pos,rotmb2e,ve,omegab,mass,I) name="0000_rigidbody_zeros" Fb = np.array([0,0,0]) taub = np.array([0,0,0]) unittest_template_A() # Test zero forces and moments quad = rb.rigidbody_q(pos,q,ve,omegab,mass,I) name = "0001_rigidbody_q_zeros" Fb = np.array([0,0,0]) taub = np.array([0,0,0]) unittest_template_A() # Test Fx force quad = rb.rigidbody(pos,rotmb2e,ve,omegab,mass,I) name = "0010_rigidbody_fx" Fb = np.array([0.1,0,0]) taub = np.array([0,0,0]) unittest_template_A() # Test Fx force quad = rb.rigidbody_q(pos,q,ve,omegab,mass,I) name = "0011_rigidbody_q_fx"
ve = np.array([0,0,0]) omegab = np.array([0,0,0]) # Simulation parameters dt_sim = 0.001 # seconds T_sim = 30 # seconds dt_log = 0.1 # seconds # Test Case ####################################################################### name = "0000_hoover_plus" cmd = 37278 * np.ones([round(T_sim/dt_sim)+10, 4]) index = int((5)/dt_sim) cmd[index:,:] = 0 qftau = qftau_cf.QuadFTau_CF(0, True) qrb = rb.rigidbody_q(pos, q, ve, omegab, qftau.mass, qftau.I) testcase_template_A() # Test Case ####################################################################### name = "0001_hoover_cross" cmd = 37278 * np.ones([round(T_sim/dt_sim)+10, 4]) index = int((5)/dt_sim) cmd[index:,:] = 0 qftau = qftau_cf.QuadFTau_CF(0, False) qrb = rb.rigidbody_q(pos, q, ve, omegab, qftau.mass, qftau.I) testcase_template_A() # Test Case ####################################################################### name = "0010_roll_pos_plus"