def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in Kane. The inertia of the pendulum is
# defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols('g')
k, ls = symbols('k ls')
a, mA, mC = symbols('a mA mC')
F = dynamicsymbols('F')
Ix, Iy, Iz = symbols('Ix Iy Iz')
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
# Creating reference frames
N = ReferenceFrame('N')
A = ReferenceFrame('A')
A.orient(N, 'Axis', [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point('O')
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew('C', q1 * N.x)
Ao = C.locatenew('Ao', a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle('Cart', C, mC)
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F)]
km=Kane(N)
km.coords([q1, q2])
km.speeds([u1, u2])
km.kindiffeq(kindiffs)
(fr,frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == -Iz
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols("g")
k, ls = symbols("k ls")
a, mA, mC = symbols("a mA mC")
F = dynamicsymbols("F")
Ix, Iy, Iz = symbols("Ix Iy Iz")
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
# Creating reference frames
N = ReferenceFrame("N")
A = ReferenceFrame("A")
A.orient(N, "Axis", [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point("O")
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew("C", q1 * N.x)
Ao = C.locatenew("Ao", a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle("Cart", C, mC)
Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [
(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F),
]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warns_deprecated_sympy():
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols('g')
k, ls = symbols('k ls')
a, mA, mC = symbols('a mA mC')
F = dynamicsymbols('F')
Ix, Iy, Iz = symbols('Ix Iy Iz')
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols('q1 q2')
q1d, q2d = dynamicsymbols('q1 q2', 1)
u1, u2 = dynamicsymbols('u1 u2')
u1d, u2d = dynamicsymbols('u1 u2', 1)
# Creating reference frames
N = ReferenceFrame('N')
A = ReferenceFrame('A')
A.orient(N, 'Axis', [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point('O')
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew('C', q1 * N.x)
Ao = C.locatenew('Ao', a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle('Cart', C, mC)
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA),
(C, -N.y * gravity * mC),
(C, -N.x * k * (q1 - ls)),
(C, N.x * F)]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def test_parallel_axis():
# This is for a 2 dof inverted pendulum on a cart.
# This tests the parallel axis code in KanesMethod. The inertia of the
# pendulum is defined about the hinge, not about the center of mass.
# Defining the constants and knowns of the system
gravity = symbols("g")
k, ls = symbols("k ls")
a, mA, mC = symbols("a mA mC")
F = dynamicsymbols("F")
Ix, Iy, Iz = symbols("Ix Iy Iz")
# Declaring the Generalized coordinates and speeds
q1, q2 = dynamicsymbols("q1 q2")
q1d, q2d = dynamicsymbols("q1 q2", 1)
u1, u2 = dynamicsymbols("u1 u2")
u1d, u2d = dynamicsymbols("u1 u2", 1)
# Creating reference frames
N = ReferenceFrame("N")
A = ReferenceFrame("A")
A.orient(N, "Axis", [-q2, N.z])
A.set_ang_vel(N, -u2 * N.z)
# Origin of Newtonian reference frame
O = Point("O")
# Creating and Locating the positions of the cart, C, and the
# center of mass of the pendulum, A
C = O.locatenew("C", q1 * N.x)
Ao = C.locatenew("Ao", a * A.y)
# Defining velocities of the points
O.set_vel(N, 0)
C.set_vel(N, u1 * N.x)
Ao.v2pt_theory(C, N, A)
Cart = Particle("Cart", C, mC)
Pendulum = RigidBody("Pendulum", Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
# kinematical differential equations
kindiffs = [q1d - u1, q2d - u2]
bodyList = [Cart, Pendulum]
forceList = [(Ao, -N.y * gravity * mA), (C, -N.y * gravity * mC), (C, -N.x * k * (q1 - ls)), (C, N.x * F)]
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=SymPyDeprecationWarning)
(fr, frstar) = km.kanes_equations(forceList, bodyList)
mm = km.mass_matrix_full
assert mm[3, 3] == Iz
def main():
print = lambda x: sympy.pprint(x, use_unicode=False, wrap_line=False)
# init_session()
# init_printing()
theta, phi, psi = dynamicsymbols('theta, phi, psi')
inertial_frame = ReferenceFrame('O')
gyro_frame = ReferenceFrame('e')
gyro_frame.orient(inertial_frame, 'Body', (phi, theta, psi), 'ZXZ')
print('>>>>')
print('>>>>')
omega = gyro_frame.ang_vel_in(
inertial_frame) # Angular velocity of a frame in another
print(omega.to_matrix(gyro_frame))
# ellipse as function of current state
init_vprinting(use_latex='mathjax', pretty_print=True)
#Kinematics ------------------------------
#init reference frames, assume base attached to floor
inertial_frame = ReferenceFrame('I')
j0_frame = ReferenceFrame('J0')
j1_frame = ReferenceFrame('J1')
j2_frame = ReferenceFrame('J2')
#declare dynamic symbols for the three joints
theta0, theta1, theta2 = dynamicsymbols('theta0, theta1, theta2')
#orient frames
j0_frame.orient(inertial_frame, 'Axis', (theta0, inertial_frame.y))
j1_frame.orient(j0_frame, 'Axis', (theta1, j0_frame.z))
j2_frame.orient(j1_frame, 'Axis', (theta2, j1_frame.x))
#TODO figure out better name for joint points
#init joints
joint0 = Point('j0')
j0_length = symbols('l_j0')
joint1 = Point('j1')
joint1.set_pos(joint0, j0_length * j0_frame.y)
j1_length = symbols('l_j1')
joint2 = Point('j2')
joint2.set_pos(joint1, j1_length * j1_frame.y)
#create End Effector
EE = Point('E')
init_vprinting(use_latex='mathjax', pretty_print=False)
#%%
# define all reference frames of all individually moving links of the robot
print("Defining reference frames")
inertial_frame = ReferenceFrame('I')
lower_leg_left_frame = ReferenceFrame('R_1')
upper_leg_left_frame = ReferenceFrame('R_2')
hip_frame = ReferenceFrame('R_3')
upper_leg_right_frame = ReferenceFrame('R_4')
lower_leg_right_frame = ReferenceFrame('R_5')
#%% Angles
# everything is symbolic, so create all angles of your robot
# NOTE: the angle phi is the angle between your robot and the inertial frame
theta0, theta1, theta2, theta3, phi = dynamicsymbols('theta0, theta1, theta2, theta3, phi')
lower_leg_left_frame.orient(inertial_frame, 'Axis', (phi, inertial_frame.z))
simplify(lower_leg_left_frame.dcm(inertial_frame))
upper_leg_left_frame.orient(lower_leg_left_frame, 'Axis', (theta0, -lower_leg_left_frame.z))
simplify(upper_leg_left_frame.dcm(inertial_frame))
hip_frame.orient(upper_leg_left_frame, 'Axis', (theta1, -upper_leg_left_frame.z))
hip_frame.dcm(inertial_frame)
upper_leg_right_frame.orient(hip_frame, 'Axis', (theta2, -hip_frame.z))
simplify(upper_leg_right_frame.dcm(inertial_frame))
lower_leg_right_frame.orient(upper_leg_right_frame, 'Axis', (theta3, -upper_leg_right_frame.z))
simplify(lower_leg_right_frame.dcm(inertial_frame))
#%% Points and Locations
# define the kinematical chain of your robot
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, inertia, Particle, KanesMethod, kinetic_energy, potential_energy
from pydy.codegen.code import generate_ode_function
from sympy.physics.vector import init_vprinting, vlatex
init_vprinting()
#Sets up inertial frame as well as frames for each linkage
inertial_frame = ReferenceFrame('I')
s_frame = ReferenceFrame('S')
#Sets up symbols for joint angles
theta_s = dynamicsymbols('theta_s')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
s_frame.orient(inertial_frame, 'Axis', (theta_s, inertial_frame.z))
#Sets up points for the joints and places them relative to each other
S = Point('S')
s_length = symbols('l_s')
T = Point('T')
T.set_pos(S, s_length*s_frame.y)
#Sets up the angular velocities
omega_s = dynamicsymbols('omega_s')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_s - theta_s.diff()]
#Sets up the rotational axes of the angular velocities
s_frame.set_ang_vel(inertial_frame, omega_s*inertial_frame.z)
from sympy.physics.vector import init_vprinting, vlatex
init_vprinting()
time = symbols('t')
#Sets up inertial frame as well as frames for each linkage
inertial_frame = ReferenceFrame('I')
a_frame = ReferenceFrame('A')
b_frame = ReferenceFrame('B')
c_frame = ReferenceFrame('C')
#Sets up symbols for joint angles
theta_a, theta_b, theta_c = dynamicsymbols('theta_a, theta_b, theta_c')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z))
b_frame.orient(a_frame, 'Axis', (theta_b, a_frame.z))
c_frame.orient(b_frame, 'Axis', (theta_c, b_frame.z))
#Sets up points for the joints and places them relative to each other
A = Point('A')
a_length = symbols('l_a')
B = Point('B')
B.set_pos(A, a_length*a_frame.y)
C = Point('C')
b_length = symbols('l_b')
C.set_pos(B, b_length*b_frame.y)
D = Point('D')
c_length = symbols('l_c')
D.set_pos(C, c_length*c_frame.y)
# In[47]:
# In[26]:
from sympy.physics.mechanics import ReferenceFrame, Vector
from sympy import symbols
N = ReferenceFrame('N')
N1 = ReferenceFrame('N1')
N2 = ReferenceFrame('N2')
N3 = ReferenceFrame('N3')
q1,q2,q3 = symbols('q1 q2 q3')
N1.orient(N,'Axis', [q1, N.x])
N2.orient(N1,'Axis', [q2, N1.x])
N3.orient(N2,'Axis', [q3, N2.z])
dot(N3.x, N2.x)
# In[37]:
from numpy import arange
from sympy import Matrix, eye
m = eye(3)
a1, a2, a3, b1, b2, b3 = symbols('a1, a2, a3, b1, b2, b3')
A = m.subs(1, a1)
B = m.subs(1, b1)
for i in arange(3):
for j in arange(3):
from sympy import symbols, simplify, trigsimp, cos, sin, Matrix
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, inertia, Particle, KanesMethod, kinetic_energy, potential_energy
from sympy.physics.vector import init_vprinting, vlatex
init_vprinting()
#Sets up inertial frame as well as frames for each linkage
inertial_frame = ReferenceFrame('I')
one_frame = ReferenceFrame('1')
two_frame = ReferenceFrame('2')
#Sets up symbols for joint angles
theta1, theta2 = dynamicsymbols('theta1, theta2')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
one_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z))
#Sets up points for the joints and places them relative to each other
one = Point('1')
one_length = symbols('l_1')
two = Point('2')
two.set_pos(one, one_length*one_frame.y)
three = Point('3')
two_length = symbols('l_2')
three.set_pos(two, two_length*two_frame.y)
#Sets up the angular velocities
omega1, omega2 = dynamicsymbols('omega1, omega2')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega1 - theta1.diff(),
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, inertia, RigidBody, KanesMethod
from sympy import symbols
from numpy import array, zeros
import numpy as np
# Orientations
# ============
theta1, theta2, theta3 = dynamicsymbols('theta1, theta2, theta3')
inertial_frame = ReferenceFrame('I')
l_leg_frame = ReferenceFrame('L')
l_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
body_frame = ReferenceFrame('B')
body_frame.orient(l_leg_frame, 'Axis', (theta2, l_leg_frame.z))
r_leg_frame = ReferenceFrame('R')
r_leg_frame.orient(body_frame, 'Axis', (theta3, body_frame.z))
# Point Locations
# ===============
# Joints
# ------
l_leg_length, hip_width = symbols('l_L, h_W')
#!/usr/bin/env python
from sympy import symbols
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point
# Orientations
# ============
theta1, theta2, theta3 = dynamicsymbols('theta1, theta2, theta3')
inertial_frame = ReferenceFrame('I')
lower_leg_frame = ReferenceFrame('L')
lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
upper_leg_frame = ReferenceFrame('U')
upper_leg_frame.orient(lower_leg_frame, 'Axis', (theta2, lower_leg_frame.z))
torso_frame = ReferenceFrame('T')
torso_frame.orient(upper_leg_frame, 'Axis', (theta3, upper_leg_frame.z))
# Point Locations
# ===============
# Joints
# ------
lower_leg_length, upper_leg_length = symbols('l_L, l_U')
from numpy import array, zeros
from sympy import symbols, simplify, trigsimp, cos, sin
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, inertia, Particle, KanesMethod, kinetic_energy, potential_energy
#Sets up inertial frame as well as frames for each linkage
inertial_frame = ReferenceFrame('I')
leg_frame = ReferenceFrame('L')
body_frame = ReferenceFrame('B')
#Sets up symbols for joint angles
theta1, theta2 = dynamicsymbols('theta1, theta2')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
body_frame.orient(leg_frame, 'Axis', (theta2, leg_frame.z))
#Sets up points for the joints and places them relative to each other
ankle = Point('A')
leg_length = symbols('l_L')
waist = Point('W')
waist.set_pos(ankle, leg_length*leg_frame.y)
body = Point('B')
body_length = symbols('l_B')
body.set_pos(waist, body_length*body_frame.y)
#Sets up the angular velocities
omega1, omega2 = dynamicsymbols('omega1, omega2')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega1 - theta1.diff(),
omega2 - theta2.diff()]
from __future__ import print_function, division
from sympy import symbols, simplify
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)
inertial_frame = ReferenceFrame('I')
lower_leg_frame = ReferenceFrame('L')
theta1, theta2, theta3 = dynamicsymbols('theta1, theta2, theta3')
lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
lower_leg_frame.dcm(inertial_frame)
omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3')
lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z)
print(lower_leg_frame.ang_vel_in(inertial_frame))
init_vprinting()
# Orientations
# ============
theta1_dp1, theta2_dp1, theta1_dp2, theta2_dp2 = dynamicsymbols("theta_1dp1, theta2_dp1, theta_1dp2, theta2_dp2")
inertial_frame_dp1 = ReferenceFrame("I_dp1")
inertial_frame_dp2 = ReferenceFrame("I_dp2")
one_frame_dp1 = ReferenceFrame("1_dp1")
one_frame_dp2 = ReferenceFrame("2_dp2")
one_frame_dp1.orient(inertial_frame_dp1, "Axis", (theta1_dp1, inertial_frame_dp1.z))
one_frame_dp2.orient(inertial_frame_dp2, "Axis", (theta1_dp2, inertial_frame_dp2.z))
two_frame_dp1 = ReferenceFrame("2_dp1")
two_frame_dp2 = ReferenceFrame("2_dp2")
two_frame_dp1.orient(one_frame_dp1, "Axis", (theta2_dp1, one_frame_dp1.z))
two_frame_dp2.orient(one_frame_dp2, "Axis", (theta2_dp2, one_frame_dp2.z))
# Point Locations
# ===============
# Joints
# ------
one_length_dp1, one_length_dp2, two_length_dp2 = symbols("l_1dp1, l_1dp2, l_2dp2")
# Variáveis Simbólicas
THETA_1, THETA_2 = dynamicsymbols('theta_1 theta_2')
DTHETA_1, DTHETA_2 = dynamicsymbols('theta_1 theta_2', 1)
TAU_1, TAU_2 = symbols('tau_1 tau_2')
L_1, L_2 = symbols('l_1 l_2', positive=True)
R_1, R_2 = symbols('r_1 r_2', positive=True)
M_1, M_2, G = symbols('m_1 m_2 g')
I_1_ZZ, I_2_ZZ = symbols('I_{1zz}, I_{2zz}')
# Referenciais
# Referencial Inercial
B0 = ReferenceFrame('B0')
# Referencial móvel: theta_1 em relação a B0.z
B1 = ReferenceFrame('B1')
B1.orient(B0, 'Axis', [THETA_1, B0.z])
# Referencial móvel: theta_2 em relação a B1.z
B2 = ReferenceFrame('B2')
B2.orient(B1, 'Axis', [THETA_2, B1.z])
# Pontos e Centros de Massa
O = Point('O')
O.set_vel(B0, 0)
A = Point('A')
A.set_pos(O, L_1 * B1.x)
A.v2pt_theory(O, B0, B1)
CM_1 = Point('CM_1')
CM_1.set_pos(O, R_1 * B1.x)
CM_1.v2pt_theory(O, B0, B1)
CM_2 = Point('CM_2')
CM_2.set_pos(A, R_2 * B2.x)
kinetic_energy,
)
from sympy import symbols, cos, sin
from numpy import array, zeros
import numpy as np
# Orientations
# ============
theta1, theta2, theta3, theta4 = dynamicsymbols("theta1, theta2, theta3, theta4")
inertial_frame = ReferenceFrame("I")
l_leg_frame = ReferenceFrame("L")
l_leg_frame.orient(inertial_frame, "Axis", (theta1, inertial_frame.z))
crotch_frame = ReferenceFrame("C")
crotch_frame.orient(l_leg_frame, "Axis", (theta2, l_leg_frame.z))
r_leg_frame = ReferenceFrame("R")
r_leg_frame.orient(crotch_frame, "Axis", (theta3, crotch_frame.z))
body_frame = ReferenceFrame("B")
body_frame.orient(crotch_frame, "Axis", (theta4, crotch_frame.z))
# Point Locations
# ===============
thRTRx,thRTRy,thRTRz = dynamicsymbols('thRTRx,thRTRy,thRTRz')
thRSpdlx,thRSpdly,thRSpdlz = dynamicsymbols('thRSpdlx,thRSpdly,thRSpdlz')
thHsgx, thHsgy, thHsgz = dynamicsymbols('thHsgx, thHsgy, thHsgz')
thLBCy, thRBCy = dynamicsymbols('thLBCy, thRBCy')
thLUBx, thLUBy, thLUBz = dynamicsymbols('thLUBx, thLUBy, thLUBz')
thRUBx, thRUBy, thRUBz = dynamicsymbols('thRUBx, thRUBy, thRUBz')
thLLBx, thLLBy, thLLBz = dynamicsymbols('thLLBx, thLLBy, thLLBz')
thRLBx, thRLBy, thRLBz = dynamicsymbols('thRLBx, thRLBy, thRLBz')
thJbarx, thJbary, thJbarz = dynamicsymbols('thJbarx, thJbary, thJbarz')
thLFTy, thRFTy, thLRTy, thRRTy = dynamicsymbols('thLFTy, thRFTy, thLRTy, thRRTy')
transRPy = dynamicsymbols('transRPy') # Dynamic symbol for the translation of the rack and pinion
#___________________________________________________________________________________________________________________________________________________
# Orient the reference frames
Chassis_frame.orient(inertial_frame, 'Body', [thRoll,thPitch,thYaw],'XYZ')
RP_frame.orient(Chassis_frame,'Body',[thRPx,thRPy,thRPz],'XYZ')
LUCA_frame.orient(Chassis_frame, 'Body', [thLUCAx,thLUCAy,thLUCAz],'ZYX')
LLCA_frame.orient(Chassis_frame,'Body',[thLLCAx,thLLCAy,thLLCAz],'ZYX')
LTR_frame.orient(Chassis_frame,'Body',[thLTRx,thLTRy,thLTRz],'XYZ')
LSpdl_frame.orient(Chassis_frame,'Body',[thLSpdlx,thLSpdly,thLSpdlz],'XYZ')
RUCA_frame.orient(Chassis_frame, 'Body', [thRUCAx,thRUCAy,thRUCAz],'ZYX')
RLCA_frame.orient(Chassis_frame,'Body',[thRLCAx,thRLCAy,thRLCAz],'ZYX')
RTR_frame.orient(Chassis_frame,'Body',[thRTRx,thRTRy,thRTRz],'XYZ')
RSpdl_frame.orient(Chassis_frame,'Body',[thRSpdlx,thRSpdly,thRSpdlz],'XYZ')
Hsg_frame.orient(inertial_frame,'Body',[thHsgx,thHsgy,thHsgz],'XYZ')
LBC_frame.orient(Hsg_frame,'Axis',(thLBCy,Hsg_frame.y))
LUB_frame.orient(Chassis_frame,'Body',[thLUBx,thLUBy,thLUBz],'XYZ')
LLB_frame.orient(Chassis_frame,'Body',[thLLBx,thLLBy,thLLBz],'XYZ')
RBC_frame.orient(Hsg_frame,'Axis',(thRBCy,Hsg_frame.y))
RUB_frame.orient(Chassis_frame,'Body',[thRUBx,thRUBy,thRUBz],'XYZ')
from sympy import symbols
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, inertia, RigidBody, KanesMethod
from pydy.codegen.code import generate_ode_function
from numpy import array, linspace, deg2rad, ones, concatenate
from scipy.integrate import odeint
# Orientations
# ============
theta1, theta2 = dynamicsymbols('theta1, theta2')
inertial_frame = ReferenceFrame('I')
lower_leg_frame = ReferenceFrame('L')
lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z))
upper_leg_frame = ReferenceFrame('U')
upper_leg_frame.orient(lower_leg_frame, 'Axis', (theta2, lower_leg_frame.z))
# Point Locations
# ===============
# Joints
# ------
lower_leg_length, upper_leg_length = symbols('l_L, l_U')
ankle = Point('A')
from numpy import array, linspace, deg2rad, ones, concatenate
from scipy.integrate import odeint
#from sympy import *
from sympy.physics.vector import init_vprinting, vlatex
init_vprinting()
# Orientations
# ============
theta_a, theta_b, theta_c, theta_go, theta_bco = dynamicsymbols('theta_a, theta_b, theta_c, theta_go, theta_bco')
inertial_frame = ReferenceFrame('I')
a_frame = ReferenceFrame('A')
a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z))
b_frame = ReferenceFrame('B')
b_frame.orient(a_frame, 'Axis', (theta_b, inertial_frame.z))
c_frame = ReferenceFrame('C')
c_frame.orient(b_frame, 'Axis', (theta_c, inertial_frame.z))
go_frame = ReferenceFrame('G_o')
bco_frame = ReferenceFrame('BC_o')
# Point Locations
# ===============
#%%
# define all reference frames of all individually moving links of the robot
print("Defining reference frames")
inertial_frame = ReferenceFrame('I')
lower_leg_left_frame = ReferenceFrame('R_1')
upper_leg_left_frame = ReferenceFrame('R_2')
hip_frame = ReferenceFrame('R_3')
upper_leg_right_frame = ReferenceFrame('R_4')
lower_leg_right_frame = ReferenceFrame('R_5')
#%% Angles
# everything is symbolic, so create all angles of your robot
# NOTE: the angle phi is the angle between your robot and the inertial frame
theta0, theta1, theta2, theta3, phi = dynamicsymbols(
'theta0, theta1, theta2, theta3, phi')
lower_leg_left_frame.orient(inertial_frame, 'Axis', (phi, inertial_frame.z))
simplify(lower_leg_left_frame.dcm(inertial_frame))
upper_leg_left_frame.orient(lower_leg_left_frame, 'Axis',
(theta0, -lower_leg_left_frame.z))
simplify(upper_leg_left_frame.dcm(inertial_frame))
hip_frame.orient(upper_leg_left_frame, 'Axis',
(theta1, -upper_leg_left_frame.z))
hip_frame.dcm(inertial_frame)
upper_leg_right_frame.orient(hip_frame, 'Axis', (theta2, -hip_frame.z))
simplify(upper_leg_right_frame.dcm(inertial_frame))
lower_leg_right_frame.orient(upper_leg_right_frame, 'Axis',
(theta3, -upper_leg_right_frame.z))
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, inertia, RigidBody, KanesMethod
from sympy import symbols
from sympy import pi
from numpy import array, zeros
import numpy as np
# Orientations
# ============
theta1, theta2 = dynamicsymbols("theta1, theta2")
inertial_frame = ReferenceFrame("I")
l_leg_frame = ReferenceFrame("L")
l_leg_frame.orient(inertial_frame, "Axis", (theta1, inertial_frame.z))
body_frame = ReferenceFrame("B")
body_frame.orient(l_leg_frame, "Axis", (theta2, l_leg_frame.z))
# Point Locations
# ===============
# Joints
# ------
l_leg_length, hip_width = symbols("l_L, h_W")
l_ankle = Point("LA")
from pydy.codegen.code import generate_ode_function
from sympy.physics.vector import init_vprinting, vlatex
init_vprinting()
#Sets up inertial frame as well as frames for each linkage
inertial_frame = ReferenceFrame('I')
a_frame = ReferenceFrame('A')
base_frame = ReferenceFrame('Base')
#Sets up symbols for joint angles
theta_a = dynamicsymbols('theta_a')
theta_base = dynamicsymbols('theta_base')
#Orients the leg frame to the inertial frame by angle theta1
#and the body frame to to the leg frame by angle theta2
a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z))
base_frame.orient(inertial_frame, 'Axis', (theta_base, inertial_frame.z))
#Sets up points for the joints and places them relative to each other
A = Point('A')
a_length = symbols('l_a')
B = Point('B')
B.set_pos(A, a_length*a_frame.y)
#Sets up the angular velocities
omega_base, omega_a = dynamicsymbols('omega_b, omega_a')
#Relates angular velocity values to the angular positions theta1 and theta2
kinematic_differential_equations = [omega_a - theta_a.diff()]
#Sets up the rotational axes of the angular velocities
a_frame.set_ang_vel(inertial_frame, omega_base*inertial_frame.z + omega_a*inertial_frame.z)
