def test_potential_energy(): m, M, l1, g, h, H = symbols('m M l1 g h H') omega = dynamicsymbols('omega') N = ReferenceFrame('N') O = Point('O') O.set_vel(N, 0 * N.x) Ac = O.locatenew('Ac', l1 * N.x) P = Ac.locatenew('P', l1 * N.x) a = ReferenceFrame('a') a.set_ang_vel(N, omega * N.z) Ac.v2pt_theory(O, N, a) P.v2pt_theory(O, N, a) Pa = Particle('Pa', P, m) I = outer(N.z, N.z) A = RigidBody('A', Ac, a, M, (I, Ac)) Pa.set_potential_energy(m * g * h) A.set_potential_energy(M * g * H) assert potential_energy(A, Pa) == m * g * h + M * g * H
def test_potential_energy(): m, M, l1, g, h, H = symbols("m M l1 g h H") omega = dynamicsymbols("omega") N = ReferenceFrame("N") O = Point("O") O.set_vel(N, 0 * N.x) Ac = O.locatenew("Ac", l1 * N.x) P = Ac.locatenew("P", l1 * N.x) a = ReferenceFrame("a") a.set_ang_vel(N, omega * N.z) Ac.v2pt_theory(O, N, a) P.v2pt_theory(O, N, a) Pa = Particle("Pa", P, m) I = outer(N.z, N.z) A = RigidBody("A", Ac, a, M, (I, Ac)) Pa.potential_energy = m * g * h A.potential_energy = M * g * H assert potential_energy(A, Pa) == m * g * h + M * g * H
g] #Specified contains the matrix for the input torques specified = [ankle_torque, waist_torque] #Specifies numerical constants for inertial/mass properties #Robot Params #numerical_constants = array([1.035, # leg_length[m] # 36.754, # leg_mass[kg] # 0.85, # body_length[m] # 91.61, # body_mass[kg] # 9.81] # acceleration due to gravity [m/s^2] # ) numerical_constants = array([0.75, 7.0, 0.5, 8.0, 9.81]) #Set input torques to 0 numerical_specified = zeros(2) parameter_dict = dict(zip(constants, numerical_constants)) ke_energy = simplify(kinetic_energy(inertial_frame, waistP, bodyP).subs(parameter_dict)) waistP.set_potential_energy(leg_mass*g*leg_length*cos(theta1)) bodyP.set_potential_energy(body_mass*g*(leg_length*cos(theta1)+body_length*cos(theta1+theta2))) pe_energy = simplify(potential_energy(waistP, bodyP).subs(parameter_dict))
9.81]) #Set input torques to 0 numerical_specified = zeros(3) parameter_dict = dict(zip(constants, numerical_constants)) ke_energy = simplify(kinetic_energy(inertial_frame, bP, cP, dP)) bP.set_potential_energy(a_mass*g*a_length*cos(theta_a)) cP.set_potential_energy(b_mass*g*(a_length*cos(theta_a)+b_length*cos(theta_a+theta_b))) dP.set_potential_energy(c_mass*g*(a_length*cos(theta_a)+b_length*cos(theta_a+theta_b)+c_length*cos(theta_a+theta_b+theta_c))) pe_energy = simplify(potential_energy(bP, cP, dP).subs(parameter_dict)) forcing = simplify(kane.forcing) mass_matrix = simplify(kane.mass_matrix) zero_omega = dict(zip(speeds, zeros(3))) torques = Matrix(specified) g_terms = forcing.subs(zero_omega) - torques g_terms_a = g_terms[0] g_terms_b = g_terms[1] g_terms_c = g_terms[2]
0.5, 8.0, 9.81]) #Set input torques to 0 numerical_specified = zeros(2) parameter_dict = dict(zip(constants, numerical_constants)) ke_energy = simplify(kinetic_energy(inertial_frame, twoP, threeP)) twoP.set_potential_energy(one_mass*g*one_length*cos(theta1)) threeP.set_potential_energy(two_mass*g*(one_length*cos(theta1)+two_length*cos(theta1+theta2))) pe_energy = simplify(potential_energy(twoP, threeP)) forcing = simplify(kane.forcing) mass_matrix = simplify(kane.mass_matrix) torques = Matrix(specified) zero_omega = dict(zip(speeds, zeros(2))) com = simplify(forcing.subs(zero_omega) - torques) com_1 = com[0] com_2 = com[1] coriolis = simplify(forcing - com - torques)
#Set input torques to 0 numerical_specified = zeros(4) parameter_dict = dict(zip(constants, numerical_constants)) ke_energy = kinetic_energy(inertial_frame, bP, cP, dP, eP) bP.set_potential_energy(a_mass*g*a_length*cos(theta_a)) cP.set_potential_energy(b_mass*g*(a_length*cos(theta_a)+b_length*cos(theta_a+theta_b))) dP.set_potential_energy(c_mass*g*(a_length*cos(theta_a)+b_length*cos(theta_a+theta_b)+c_length*cos(theta_a+theta_b+theta_c))) eP.set_potential_energy(d_mass*g*(a_length*cos(theta_a) + b_length*cos(theta_a+theta_b) + c_length*cos(theta_a+theta_b+theta_c) + d_length*cos(theta_a+theta_b+theta_c))) pe_energy = potential_energy(bP, cP, dP, eP).subs(parameter_dict) forcing = kane.forcing #mass_matrix = simplify(kane.mass_matrix) zero_omega = dict(zip(speeds, zeros(4))) torques = Matrix(specified) g_terms = forcing.subs(zero_omega) - torques g_terms = simplify(g_terms) g_terms_a = g_terms[0] g_terms_b = g_terms[1]
# ---------------------------------------------------------------------------- # Equations of motion # ---------------------------------------------------------------------------- if __name__ == "__main__": # Do not perform derivation when imported simplify_exps = False T = 0 for obj in (*particles, *links): T += trigsimp(obj.kinetic_energy(N)) print(f"Included {obj} in T") #sum([obj.kinetic_energy(N).simplify() for obj in (*particles, *links)]) #T = kinetic_energy(N, *particles, *links) # Kinetic energy V = potential_energy(*particles, *links) # Potential energy # Add the contribution of the spring V += simplify(0.5*k*(A.pos_from(B).magnitude() - l0)**2) L = T - V # Lagrangian # -------------------------- Friction torques ----------------------------- # N.z is used because all z-axis are parallel torques = [(pend_frame, -b_joint*dq[1]*N.z), (link1_frame, -b_joint*2*dq[2]*N.z), (link2_frame, b_joint*2*dq[2]*N.z), (link1_frame, -b_joint*(beta_dot)*N.z), (link3_frame, b_joint*(beta_dot)*N.z),