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)