Python ReferenceFrame.orient方法代码示例

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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 sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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
# ===============

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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')

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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 sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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)

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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 ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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 sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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 ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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")

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
    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
# ===============

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
# 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.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import orient [as 别名]
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)