def unitttest_template_B():
file_b.write("\n\nUnit Test %s: \n" % u_name)
file_b.write("####################################################\n")
rotmb2e = np.identity(3)
qftau = qftau_cf.QuadFTau_CF(0)
qftau_b = qftau_cf.QuadFTau_CF_b(qftau.cT, qftau.cQ,
qftau.radius, qftau.input2omegar_coeff)
try:
file_b.write("cmd = %s\n" % np.array2string(cmd))
fb, taub = qftau_b.input2ftau(cmd)
# add gravity
fb = fb + np.dot( np.transpose(rotmb2e),
np.array([0,0,-qftau.mass*cts.g_CONST]) )
file_b.write("fb = %s\n" % np.array2string(fb))
file_b.write("taub = %s\n" % np.array2string(taub))
# remove gravity
fb = fb + np.dot(np.transpose(rotmb2e),
np.array([0,0,+qftau.mass*cts.g_CONST]))
omegar = qftau_b.ftau2omegar(fb, taub)
cmd_recovered = qftau_b.omegar2input(omegar)
file_b.write("cmd_recovered = %s\n" % np.array2string(cmd_recovered))
except Exception as e:
file_b.write("TEST FAILED: %s\n" % e)
rotmb2e = ut.quat2rotm(q) 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 #######################################################################
u_name = "0009_cmd_yawing_pos"
cmd = np.array([38000+1000,38000,38000+1000,38000])
unitttest_template_A()
unitttest_template_B()
u_name = "0010_cmd_yawing_neg"
cmd = np.array([38000,38000+1000,38000,38000+1000])
unitttest_template_A()
unitttest_template_B()
u_name = "0100_write_cT_cQ"
# f_thrust_i = cT * omegar_i^2
file.write("\n\nUnit Test %s: \n" % u_name)
file.write("####################################################\n")
qftau = qftau_cf.QuadFTau_CF(0)
file.write("cT estimate is %s with std %s. \n" % (qftau.cT, qftau.cT_std))
file.write("cQ estimate is %s \n" % qftau.cQ)
u_name = "0200_test_cTcQmodelcheck"
qftau = qftau_cf.QuadFTau_CF(0)
qftau_b = qftau_cf.QuadFTau_CF_b(qftau.cT, qftau.cQ,
qftau.radius, qftau.input2omegar_coeff)
rotmb2e = np.identity(3)
vb = np.array([0,0,0])
with open(fullname_c,"w") as file_c, open(fullname_d,"w") as file_d:
file_c.write("\n\nUnit Test %s: \n" % u_name)
file_c.write("####################################################\n")
file_d.write("\n\nUnit Test %s: \n" % u_name)
file_d.write("####################################################\n")
pos = np.array([0, 0, 3])
q = np.array([1, 0, 0, 0])
rotmb2e = ut.quat2rotm(q)
vb = np.array([0, 0, 0])
omegab = np.array([0, 0, 0])
# Simulation parameters
dt_sim = 0.001 # seconds
T_sim = 100 # seconds
dt_log = 0.001 # seconds
dt_vis = 1 / 60 # 60 frame per second
# Test Case
#######################################################################
cmd = 37278 * np.ones(4)
qftau = qftau_cf.QuadFTau_CF(0, plus)
qrb = rb.rigidbody_q(pos, q, vb, omegab, qftau.mass, qftau.I)
# Initialize the logger object
fullname = "testresults/ftau_plus_rigidbody/" + name
logger = log.Logger(fullname, name)
plotter = plot.Plotter()
# Initialize the visualization
panda3D_app = Panda3DApp()
readkeys = ReadKeys()
# The main simulation loop
for t in np.arange(dt_sim, T_sim + dt_sim, dt_sim):
# Calculate body-based forces and torques