def test_kinetic_energy():
m, M, l1 = symbols('m M l1')
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))
raises(TypeError, lambda: kinetic_energy(Pa, Pa, A))
raises(TypeError, lambda: kinetic_energy(N, N, A))
assert 0 == (kinetic_energy(N, Pa, A) - (M*l1**2*omega**2/2
+ 2*l1**2*m*omega**2 + omega**2/2)).expand()
def test_kinetic_energy():
m, M, l1 = symbols("m M l1")
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))
raises(TypeError, lambda: kinetic_energy(Pa, Pa, A))
raises(TypeError, lambda: kinetic_energy(N, N, A))
assert (0 == (kinetic_energy(N, Pa, A) -
(M * l1**2 * omega**2 / 2 + 2 * l1**2 * m * omega**2 +
omega**2 / 2)).expand())
def test_kinetic_energy():
m, M, l1 = symbols('m M l1')
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))
assert 0 == kinetic_energy(N, Pa, A) - (M*l1**2*omega**2/2
+ 2*l1**2*m*omega**2 + omega**2/2)
def test_kinetic_energy():
m, M, l1 = symbols("m M l1")
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))
assert 0 == kinetic_energy(N, Pa, A) - (
M * l1 ** 2 * omega ** 2 / 2 + 2 * l1 ** 2 * m * omega ** 2 + omega ** 2 / 2
)
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))
# Create frame for rod BC
rodBCFrame = mech.ReferenceFrame('rodBCFrame')
rodBCFrame.set_ang_vel(N, -phiDot * N.z)
# Create point for the center of mass of BC
comBC_x = (L / 2) * sympy.cos(phi)
comBC_y = (L / 2) * sympy.sin(phi)
comBC_vx = phiDot * comBC_x
comBC_vy = phiDot * comBC_y
comBC = mech.Point('comBC')
comBC.set_vel(N, comBC_vx * N.x + comBC_vy * N.y)
comBC.set_pos(B, comBC_x * N.x + comBC_y * N.y)
# Create rigid body for blade BC
I_BC = mech.inertia(N, 0, 0, Izz)
rodBC = mech.RigidBody('rodBC', comBC, rodBCFrame, m, (I_BC, comBC))
# Get the kinetic energy of each component to check results
mech.kinetic_energy(N, rodBC)
# Set potential energies
rodBC.potential_energy = (1 / 2) * kT * phi**2
# Get (unforced) Lagrangian of the system
L = mech.Lagrangian(N, rodBC)
# Get equation of motion
LM = mech.LagrangesMethod(L, [rodBC], frame=N)
LM.form_lagranges_equations()
r_leg_grav_force,
l_leg_torque,
body_torque,
r_leg_torque]
bodies = [l_leg, body, r_leg]
fr, frstar = kane.kanes_equations(loads, bodies)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
# Set up kinetic and potential energies
# =====================================
ke_lleg = kinetic_energy(inertial_frame, l_leg)
l_leg.set_potential_energy(l_leg_mass*g*l_leg_com_length*cos(theta1))
ke_body = kinetic_energy(inertial_frame, body) - ke_lleg
body.set_potential_energy(body_mass*g*(l_leg_length*cos(theta1) + body_com_length*sin(theta1+theta2)+body_com_height*sin(theta1+theta2+1.57)))
ke_rleg = kinetic_energy(inertial_frame, r_leg) - ke_body - ke_lleg
r_leg.set_potential_energy(r_leg_mass*g*(l_leg_length*cos(theta1) + hip_width*sin(theta1+theta2)+-1*r_leg_com_length*cos(theta1+theta2+theta3)))
# List the symbolic arguments
# ===========================
# Constants
# ---------
# 9.81] # acceleration due to gravity [m/s^2]
# )
numerical_constants = array([1.0,
1.0,
1.0,
1.0,
1.0,
1.0,
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)))
# 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, 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)))
numerical_constants = array([1.0,
1.0,
1.0,
1.0,
1.0,
1.0,
1.0,
1.0,
9.81])
#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)
# Create reference frame for analysis
N = mech.ReferenceFrame( 'N' );
# Set cart position, mass, and velocity
cPoint = mech.Point( 'cPoint' );
cPoint.set_vel(N, xcDot*N.x);
C = mech.Particle('C', cPoint, 4*m );
# Set box position, mass, and velocity
# Create rotated frame
boxFrame = mech.ReferenceFrame('boxFrame');
boxFrame.orient(N, 'Body', [0, 0, -theta], 'XYZ');
bPoint = mech.Point('bPoint');
bPoint.set_vel(N, xbDot*boxFrame.x);
B = mech.Particle('B', bPoint, m );
# Get the kinetic energy of each component to check results
mech.kinetic_energy(N, C)
mech.kinetic_energy(N, B)
# Set potential energies
C.potential_energy = (1/2)*(2*k)*xc**2;
B.potential_energy = (1/2)*k*xb**2 - m*g*xb*sympy.cos(theta);
# Get (unforced) Lagrangian of the system
L = mech.Lagrangian( N, C, B );
# Get equation of motion
LM = mech.LagrangesMethod(L, [xb, xc], frame=N);
LM.form_lagranges_equations()
x2 = x1 + -1*numerical_constants[4]*sin(y[:,0] + y[:,1])
y2 = y1 + numerical_constants[4]*cos(y[:,0] + y[:,1])
#------------------------------------------------------------
# set up figure and animation
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
xlim=(-2, 2), ylim=(-2, 2))
ax.grid()
line, = ax.plot([], [], 'o-', lw=2)
time_text = ax.text(0.02, 0.95, '', transform=ax.transAxes)
energy_text = ax.text(0.02, 0.90, '', transform = ax.transAxes)
ke = kinetic_energy(inertial_frame, lower_leg, upper_leg)
lam_ke = lambdify([theta1, theta2, omega1, omega2], ke.subs(parameter_dict))
def energy(i):
global y, x1, y1, x2, y2
m1 = numerical_constants[2]
m2 = numerical_constants[6]
l1 = numerical_constants[0]
l2 = numerical_constants[4]
x0_local = y[i][0]
x1_local = y[i][2]
x2_local = y[i][1]
x3_local = y[i][3]
xe = [x1[i], x2[i]]
ye = [y1[i], y2[i]]
vx = np.cumsum([l1 * x1_local*cos(x0_local), l2*x3_local*cos(x2_local)])
l_leg_torque,
crotch_torque,
body_torque,
r_leg_torque,
]
bodies = [l_leg, crotch, body, r_leg]
fr, frstar = kane.kanes_equations(loads, bodies)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
# Energies
# ========
ke_lleg = kinetic_energy(inertial_frame, l_leg)
l_leg.set_potential_energy(l_leg_mass * g * l_leg_com_length * cos(theta1))
ke_crotch = kinetic_energy(inertial_frame, crotch) - ke_lleg
crotch.set_potential_energy(
crotch_mass
* g
* (
l_leg_length * cos(theta1)
+ (hip_width / 2) * sin(theta1 + theta2)
+ crotch_com_height * sin(theta1 + theta2 + 1.57)
)
)
ke_body = kinetic_energy(inertial_frame, body) - ke_crotch - ke_lleg
body.set_potential_energy(
B = mech.Point('B')
B.set_vel(N, vB * N.y)
# Create vertical rod (this one isn't moving)
I1 = mech.inertia(N, 0, 0, 0)
rod1 = mech.RigidBody('rod1', B, N, m, (I1, B))
# Create rod AB
Izz = m * L**2 / 12
I2 = mech.inertia(N, 0, 0, Izz)
rod2Frame = mech.ReferenceFrame('rod2Frame')
rod2Frame.set_ang_vel(N, -thetaDot * N.z)
rod2 = mech.RigidBody('rod2', B, rod2Frame, m, (I2, B))
# Get the kinetic energy of each component tp check results
mech.kinetic_energy(N, rod1)
mech.kinetic_energy(N, rod2)
# Set potential energies
deltaz = (L / 2) * (sympy.sin(theta) - 1 / 2)
V1 = m * g * deltaz
# Rod 1
V2 = m * g * deltaz
# Rod 2
Vs = (1 / 2) * k * deltaz**2
# Spring
rod1.potential_energy = V1 + Vs
rod2.potential_energy = V2
# Get (unforced) Lagrangian of the system
L = mech.Lagrangian(N, rod1, rod2)
