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.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
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),
