dh9 = DHDef(9, 'R', 0.1, 0, 0, q9 - sympy.pi / 2, 'mdh', dh8, [])
dh0.set_succ([dh1])
dh1.set_succ([dh2, dh8])
dh2.set_succ([dh3])
dh3.set_succ([dh4])
dh4.set_succ([dh5])
dh5.set_succ([dh6])
dh6.set_succ([dh7])
dh8.set_succ([dh9])
start_time = time.time()
kin = Kinematics(dh0)
kin.cal_transfmats()
print(kin._coordinates)
print(kin._coordinates_t)
print(kin._d_coordinates)
print(kin._d_coordinates_t)
print(kin._dd_coordinates)
print(kin._dd_coordinates_t)
#kin.draw_frames()
dyn = Dynamics(kin)
print(dyn._ml2r(dh1._m, dh1._l))
print(dyn._Lmr2I(dh1._L_mat, dh1._m, dh1._r))
dyn.cal_dynamics()
print(dyn._tau)
print("duration: {}".format(-start_time + time.time()))
