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()))