Python ReferenceFrame.dcm方法代码示例

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import dcm [as 别名]
def test_dcm():
    q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
    N = ReferenceFrame('N')
    A = N.orientnew('A', 'Axis', [q1, N.z])
    B = A.orientnew('B', 'Axis', [q2, A.x])
    C = B.orientnew('C', 'Axis', [q3, B.y])
    D = N.orientnew('D', 'Axis', [q4, N.y])
    E = N.orientnew('E', 'Space', [q1, q2, q3], '123')
    assert N.dcm(C) == Matrix([
        [- sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), - sin(q1) *
        cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1)], [sin(q1) *
        cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) *
            sin(q3) - sin(q2) * cos(q1) * cos(q3)], [- sin(q3) * cos(q2), sin(q2),
        cos(q2) * cos(q3)]])
    # This is a little touchy.  Is it ok to use simplify in assert?
    assert D.dcm(C) == Matrix(
        [[cos(q1) * cos(q3) * cos(q4) - sin(q3) * (- sin(q4) * cos(q2) +
        sin(q1) * sin(q2) * cos(q4)), - sin(q2) * sin(q4) - sin(q1) *
            cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * (- sin(q4) *
        cos(q2) + sin(q1) * sin(q2) * cos(q4))], [sin(q1) * cos(q3) +
        sin(q2) * sin(q3) * cos(q1), cos(q1) * cos(q2), sin(q1) * sin(q3) -
            sin(q2) * cos(q1) * cos(q3)], [sin(q4) * cos(q1) * cos(q3) -
        sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)), sin(q2) *
                cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * sin(q4) * cos(q1) +
                cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4))]])
    assert E.dcm(N) == Matrix(
        [[cos(q2)*cos(q3), sin(q3)*cos(q2), -sin(q2)],
        [sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), sin(q1)*sin(q2)*sin(q3) +
        cos(q1)*cos(q3), sin(q1)*cos(q2)], [sin(q1)*sin(q3) +
        sin(q2)*cos(q1)*cos(q3), - sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1),
         cos(q1)*cos(q2)]])

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import dcm [as 别名]
def test_dcm():
    q1, q2, q3, q4 = dynamicsymbols("q1 q2 q3 q4")
    N = ReferenceFrame("N")
    A = N.orientnew("A", "Axis", [q1, N.z])
    B = A.orientnew("B", "Axis", [q2, A.x])
    C = B.orientnew("C", "Axis", [q3, B.y])
    D = N.orientnew("D", "Axis", [q4, N.y])
    E = N.orientnew("E", "Space", [q1, q2, q3], "123")
    assert N.dcm(C) == Matrix(
        [
            [
                -sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
                -sin(q1) * cos(q2),
                sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1),
            ],
            [
                sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
                cos(q1) * cos(q2),
                sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3),
            ],
            [-sin(q3) * cos(q2), sin(q2), cos(q2) * cos(q3)],
        ]
    )
    # This is a little touchy.  Is it ok to use simplify in assert?
    test_mat = D.dcm(C) - Matrix(
        [
            [
                cos(q1) * cos(q3) * cos(q4) - sin(q3) * (-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)),
                -sin(q2) * sin(q4) - sin(q1) * cos(q2) * cos(q4),
                sin(q3) * cos(q1) * cos(q4) + cos(q3) * (-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)),
            ],
            [
                sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
                cos(q1) * cos(q2),
                sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3),
            ],
            [
                sin(q4) * cos(q1) * cos(q3) - sin(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)),
                sin(q2) * cos(q4) - sin(q1) * sin(q4) * cos(q2),
                sin(q3) * sin(q4) * cos(q1) + cos(q3) * (cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)),
            ],
        ]
    )
    assert test_mat.expand() == zeros(3, 3)
    assert E.dcm(N) == Matrix(
        [
            [cos(q2) * cos(q3), sin(q3) * cos(q2), -sin(q2)],
            [
                sin(q1) * sin(q2) * cos(q3) - sin(q3) * cos(q1),
                sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
                sin(q1) * cos(q2),
            ],
            [
                sin(q1) * sin(q3) + sin(q2) * cos(q1) * cos(q3),
                -sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
                cos(q1) * cos(q2),
            ],
        ]
    )

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import dcm [as 别名]
I_1s_O = inertia_of_point_mass(m, pP1s.pos_from(pO), N)
I_2s_O = inertia_of_point_mass(m, pP2s.pos_from(pO), N)
I_3s_O = inertia_of_point_mass(m, pP3s.pos_from(pO), N)
print("\nI_1s_rel_O = {0}".format(I_1s_O))
print("\nI_2s_rel_O = {0}".format(I_2s_O))
print("\nI_3s_rel_O = {0}".format(I_3s_O))


I_1_1s = inertia(N, m*b**2/12, m*b**2/12, 2*m*b**2/12)
I_2_2s = inertia(N, 2*m*b**2/12, m*b**2/12, m*b**2/12)

I_3_3s_N2 = Matrix([[2, 0, 0],
                    [0, 1, 0],
                    [0, 0, 1]])
I_3_3s_N = m*b**2/12 * simplify(N.dcm(N2) * I_3_3s_N2 * N2.dcm(N))
I_3_3s = inertia(N, I_3_3s_N[0, 0], I_3_3s_N[1, 1], I_3_3s_N[2, 2],
                    I_3_3s_N[0, 1], I_3_3s_N[1, 2], I_3_3s_N[2, 0])

I_1_O = I_1_1s + I_1s_O
I_2_O = I_2_2s + I_2s_O
I_3_O = I_3_3s + I_3s_O
print("\nI_1_rel_O = {0}".format(I_1_O))
print("\nI_2_rel_O = {0}".format(I_2_O))
print("\nI_3_rel_O = {0}".format(I_3_O))

# assembly inertia tensor is the sum of the inertia tensor of each component
I_B_O = I_1_O + I_2_O + I_3_O
print("\nI_B_rel_O = {0}".format(I_B_O))

# n_a is parallel to line L

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

ang_vel = Matrix(speeds)

coriolis = simplify(forcing - g_terms - torques)

r_ai = a_frame.dcm(inertial_frame).as_mutable()
t_ai = simplify(B.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))

r_bi = simplify(b_frame.dcm(inertial_frame)).as_mutable()
t_bi = simplify(C.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))
t_ba = simplify(C.pos_from(B).express(inertial_frame).to_matrix(inertial_frame))
t_ba_in_a = simplify(C.pos_from(B).express(a_frame).to_matrix(a_frame))
tf_bi = r_bi.row_join(t_bi)

r_ci = simplify(c_frame.dcm(inertial_frame)).as_mutable()
t_ci = simplify(D.pos_from(A).express(inertial_frame).to_matrix(inertial_frame))
t_ca = simplify(D.pos_from(B).express(inertial_frame).to_matrix(inertial_frame))
t_ca_in_a = simplify(D.pos_from(B).express(a_frame).to_matrix(a_frame))

thetadot_omega_dict = dict(zip([theta_a.diff(time), theta_b.diff(time), theta_c.diff(time)], speeds))

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import dcm [as 别名]
kane = KanesMethod(inertial_frame, coordinates, speeds, kinematic_differential_equations)

loads = [t_grav_force,
         s_link_torque]
bodies = [tP]

fr, frstar = kane.kanes_equations(loads, bodies)

mass_matrix = kane.mass_matrix
forcing = kane.forcing[0]

# Constants
# ---------

constants = [s_length,
             s_mass,
             g]
time = symbols('t')
omega_s = dynamicsymbols('omega_s')
thetadot_omega_dict = dict(zip([theta_s.diff(time)], [omega_s]))
alpha_s = dynamicsymbols('alpha_s')
omegadot_alpha_dict = dict(zip([omega_s.diff(time)], [alpha_s]))

forces = forcing - s_torque

ri = s_frame.dcm(inertial_frame)

com = T.pos_from(S).express(inertial_frame).to_matrix(inertial_frame)
com_dot = com.diff(time).subs(thetadot_omega_dict)
com_ddot = com_dot.diff(time).subs(thetadot_omega_dict).subs(omegadot_alpha_dict)

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import dcm [as 别名]
mass_matrix = simplify(kane.mass_matrix)
mass_matrix_full = kane.mass_matrix_full

torques = Matrix(specified)

zero_omega = dict(zip(speeds, zeros(2)))

g_terms = simplify(forcing.subs(zero_omega) - torques)

g_terms_1 = g_terms[0]
g_terms_2 = g_terms[1]

coriolis = simplify(forcing - g_terms - torques)

r_1i = simplify(one_frame.dcm(inertial_frame))
t_1i = simplify(two.pos_from(one).express(inertial_frame).to_matrix(inertial_frame))

t_2i = simplify(three.pos_from(one).express(inertial_frame).to_matrix(inertial_frame))
time = symbols('t')

alpha1, alpha2 = dynamicsymbols('a_1, a_2')

accelerations = [alpha1, alpha2]

thetadot_omega_dict = dict(zip([theta1.diff(time), theta2.diff(time)], [omega1, omega2]))
omegadot_alpha_dict = dict(zip([omega1.diff(time), omega2.diff(time)], [alpha1, alpha2]))

com = (t_1i*one_mass + t_2i*two_mass)/(one_mass+two_mass)

com_dot = com.diff(time).subs(thetadot_omega_dict)

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

# 需要导入模块: from sympy.physics.mechanics import ReferenceFrame [as 别名]
# 或者: from sympy.physics.mechanics.ReferenceFrame import dcm [as 别名]
# In[26]:




# In[70]:

from sympy.physics.mechanics import ReferenceFrame, Vector
q0, q1, q2, q3, q4 = symbols('q0 q1 q2 q3 q4')
N = ReferenceFrame('N')


# In[75]:

N.dcm(N)


# In[26]:




# In[103]:

from sympy.physics.mechanics import ReferenceFrame, Vector
N = ReferenceFrame('N')
x, y , q1 = symbols('x, y, q1')
B = ReferenceFrame('B')
B.orient(N, 'Axis', [q1, N.x])
B.dcm(N)