def unittest_template_A():
""" Unit test template for initial conditions and constant input """
print("Running Test Case: %s \n" % name)
logger = log.Logger("testresults/rigidbody",name)
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 - round(t/dt_log)) < 0.000001 :
quad.check()
logger.log_rigidbody(t, quad)
logger.log2file_rigidbody()
def testcase_template_A():
# Initialize the logger object
fullname = "testresults/ftau_plus_rigidbody/" + name
logger = log.Logger(fullname, name)
plotter = plot.Plotter()
idx = 0
# The main simulation loop
for t in np.arange(dt_sim,T_sim+dt_sim,dt_sim):
# Calculate body-based forces and torques
fb, taub = qftau.input2ftau(cmd[idx,:],qrb.vb)
fe_e, taue_e = envir.applyenv2ftaue(qrb)
fb = fb + np.transpose(qrb.rotmb2e)@fe_e
taub = taub + np.transpose(qrb.rotmb2e)@taue_e
# Run the kinematic / time forward
qrb.run_quadrotor(dt_sim, fb, taub)
idx += 1
# Logging frequency
if abs(t/dt_log - round(t/dt_log)) < 0.000001 :
qrb.check()
logger.log_rigidbody(t, qrb)
logger.log_cmd(t, cmd[idx,:])
logger.log_ftau(t, np.transpose(qrb.rotmb2e)@fb, fb,
np.transpose(qrb.rotmb2e)@taub, taub)
logger.log2file_rigidbody()
logger.log2file_cmd()
logger.log2file_ftau()
plotter.plot_rigidbody(logger)
plotter.plot_cmd(logger)
# 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
fb, taub = qftau.input2ftau(cmd, qrb.vb)
fe_e, taue_e = envir.applyenv2ftaue(qrb)
fb = fb + np.transpose(qrb.rotmb2e) @ fe_e
ve = np.array([0, 0, 0])
omegab = np.array([0, 0, 0])
mass = 1
I = np.identity(3)
# Simulation parameters
dt_sim = 0.001 # seconds
T_sim = 10 # seconds
dt_log = 1 # seconds
Fb = np.array([0.01, 0.01, 0.01])
taub = np.array([0.01, 0.01, 0.01])
N = 100 # number of speed tests
name = "1000_rigidbody_speedcheck"
print("\nRunning Test Case: %s " % name)
logger = log.Logger("testresults", name)
timer_rigidbody = timeit.timeit(testcase_template_B,
number=N,
globals=globals())
print("Average time for rigidbody class %s \n" % (timer_rigidbody / N))
# Test zero forces and moments
name = "1001_rigidbody_q_speedcheck"
print("Running Test Case: %s " % name)
logger = log.Logger("testresults", name)
timer_rigidbody_q = timeit.timeit(testcase_template_C,
number=N,
globals=globals())
print("Average time for rigidbody_q class %s \n" % (timer_rigidbody_q / N))