def test_point_funcs(): q, q2 = dynamicsymbols('q q2') qd, q2d = dynamicsymbols('q q2', 1) qdd, q2dd = dynamicsymbols('q q2', 2) N = ReferenceFrame('N') B = ReferenceFrame('B') B.set_ang_vel(N, 5 * B.y) O = Point('O') P = O.locatenew('P', q * B.x) assert P.pos_from(O) == q * B.x P.set_vel(B, qd * B.x + q2d * B.y) assert P.vel(B) == qd * B.x + q2d * B.y O.set_vel(N, 0) assert O.vel(N) == 0 assert P.a1pt_theory(O, N, B) == ((-25 * q + qdd) * B.x + (q2dd) * B.y + (-10 * qd) * B.z) B = N.orientnew('B', 'Axis', [q, N.z]) O = Point('O') P = O.locatenew('P', 10 * B.x) O.set_vel(N, 5 * N.x) assert O.vel(N) == 5 * N.x assert P.a2pt_theory(O, N, B) == (-10 * qd**2) * B.x + (10 * qdd) * B.y B.set_ang_vel(N, 5 * B.y) O = Point('O') P = O.locatenew('P', q * B.x) P.set_vel(B, qd * B.x + q2d * B.y) O.set_vel(N, 0) assert P.v1pt_theory(O, N, B) == qd * B.x + q2d * B.y - 5 * q * B.z
def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') u4, u5 = dynamicsymbols('u4, u5') r = symbols('r') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert (partial_velocity(vel_list, u_list) == [[-r * L.y, 0, L.x], [r * L.x, 0, L.y], [0, 0, L.z], [L.x, L.x, 0], [ cos(q2) * L.y - sin(q2) * L.z, cos(q2) * L.y - sin(q2) * L.z, 0 ]])
def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3') u4, u5 = dynamicsymbols('u4, u5') r = symbols('r') N = ReferenceFrame('N') Y = N.orientnew('Y', 'Axis', [q1, N.z]) L = Y.orientnew('L', 'Axis', [q2, Y.x]) R = L.orientnew('R', 'Axis', [q3, L.y]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point('C') C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew('Dmc', r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert (partial_velocity(vel_list, u_list) == [[- r*L.y, 0, L.x], [r*L.x, 0, L.y], [0, 0, L.z], [L.x, L.x, 0], [cos(q2)*L.y - sin(q2)*L.z, cos(q2)*L.y - sin(q2)*L.z, 0]])
# Rattleback ground contact point P = Point('P') P.set_vel(N, ua[0]*Y.x + ua[1]*Y.y + ua[2]*Y.z) # Rattleback paraboloid -- parameterize coordinates of contact point by the # roll and pitch angles, and the geometry # TODO: FIXME!!! # f(x,y,z) = a*x**2 + b*y**2 + z - c mu = [dot(rk, Y.z) for rk in R] rx = mu[0]/mu[2]/2/a ry = mu[1]/mu[2]/2/b rz = 1 - (mu[0]**2/a + mu[1]**2/b)*(1/2/mu[2])**2 # Locate origin of parabaloid coordinate system relative to contact point O = P.locatenew('O', -rx*R.x - ry*R.y - rz*R.z) O.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), O.pos_from(P))) # Mass center position and velocity RO = O.locatenew('RO', d*R.x + e*R.y) RO.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(P))) qd_rhs = [(u[2]*cos(q[2]) - u[0]*sin(q[2]))/cos(q[1]), u[0]*cos(q[2]) + u[2]*sin(q[2]), u[1] + tan(q[1])*(u[0]*sin(q[2]) - u[2]*cos(q[2])), dot(P.pos_from(O).diff(t, R), N.x), dot(P.pos_from(O).diff(t, R), N.y)] # Partial angular velocities and partial velocities partial_w = [R.ang_vel_in(N).diff(ui, N) for ui in u + ua] partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua] partial_v_RO = [RO.vel(N).diff(ui, N) for ui in u + ua]
def test_aux_dep(): # This test is about rolling disc dynamics, comparing the results found # with KanesMethod to those found when deriving the equations "manually" # with SymPy. # The terms Fr, Fr*, and Fr*_steady are all compared between the two # methods. Here, Fr*_steady refers to the generalized inertia forces for an # equilibrium configuration. # Note: comparing to the test of test_rolling_disc() in test_kane.py, this # test also tests auxiliary speeds and configuration and motion constraints #, seen in the generalized dependent coordinates q[3], and depend speeds # u[3], u[4] and u[5]. # First, mannual derivation of Fr, Fr_star, Fr_star_steady. # Symbols for time and constant parameters. # Symbols for contact forces: Fx, Fy, Fz. t, r, m, g, I, J = symbols('t r m g I J') Fx, Fy, Fz = symbols('Fx Fy Fz') # Configuration variables and their time derivatives: # q[0] -- yaw # q[1] -- lean # q[2] -- spin # q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in # A.z direction # Generalized speeds and their time derivatives: # u[0] -- disc angular velocity component, disc fixed x direction # u[1] -- disc angular velocity component, disc fixed y direction # u[2] -- disc angular velocity component, disc fixed z direction # u[3] -- disc velocity component, A.x direction # u[4] -- disc velocity component, A.y direction # u[5] -- disc velocity component, A.z direction # Auxiliary generalized speeds: # ua[0] -- contact point auxiliary generalized speed, A.x direction # ua[1] -- contact point auxiliary generalized speed, A.y direction # ua[2] -- contact point auxiliary generalized speed, A.z direction q = dynamicsymbols('q:4') qd = [qi.diff(t) for qi in q] u = dynamicsymbols('u:6') ud = [ui.diff(t) for ui in u] #ud_zero = {udi : 0 for udi in ud} ud_zero = dict(zip(ud, [0.]*len(ud))) ua = dynamicsymbols('ua:3') #ua_zero = {uai : 0 for uai in ua} ua_zero = dict(zip(ua, [0.]*len(ua))) # Reference frames: # Yaw intermediate frame: A. # Lean intermediate frame: B. # Disc fixed frame: C. N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q[0], N.z]) B = A.orientnew('B', 'Axis', [q[1], A.x]) C = B.orientnew('C', 'Axis', [q[2], B.y]) # Angular velocity and angular acceleration of disc fixed frame # u[0], u[1] and u[2] are generalized independent speeds. C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z) C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B) + cross(B.ang_vel_in(N), C.ang_vel_in(N))) # Velocity and acceleration of points: # Disc-ground contact point: P. # Center of disc: O, defined from point P with depend coordinate: q[3] # u[3], u[4] and u[5] are generalized dependent speeds. P = Point('P') P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z) O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y) O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z) O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N))) # Kinematic differential equations: # Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates # directions of B, for qd0, qd1 and qd2. # the other is v_o_n_qd = O.vel(N) in A.z direction for qd3. # Then, solve for dq/dt's in terms of u's: qd_kd. w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P)) kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] + [dot(v_o_n_qd - O.vel(N), A.z)]) qd_kd = solve(kindiffs, qd) # Values of generalized speeds during a steady turn for later substitution # into the Fr_star_steady. steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u) steady_conditions.update({qd[1] : 0, qd[3] : 0}) # Partial angular velocities and velocities. partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua] partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua] partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua] # Configuration constraint: f_c, the projection of radius r in A.z direction # is q[3]. # Velocity constraints: f_v, for u3, u4 and u5. # Acceleration constraints: f_a. f_c = Matrix([dot(-r*B.z, A.z) - q[3]]) f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N), O.pos_from(P))), ai).expand() for ai in A]) v_o_n = cross(C.ang_vel_in(N), O.pos_from(P)) a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n) f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A]) # Solve for constraint equations in the form of # u_dependent = A_rs * [u_i; u_aux]. # First, obtain constraint coefficient matrix: M_v * [u; ua] = 0; # Second, taking u[0], u[1], u[2] as independent, # taking u[3], u[4], u[5] as dependent, # rearranging the matrix of M_v to be A_rs for u_dependent. # Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict. M_v = zeros(3, 9) for i in range(3): for j, ui in enumerate(u + ua): M_v[i, j] = f_v[i].diff(ui) M_v_i = M_v[:, :3] M_v_d = M_v[:, 3:6] M_v_aux = M_v[:, 6:] M_v_i_aux = M_v_i.row_join(M_v_aux) A_rs = - M_v_d.inv() * M_v_i_aux u_dep = A_rs[:, :3] * Matrix(u[:3]) u_dep_dict = dict(zip(u[3:], u_dep)) #u_dep_dict = {udi : u_depi[0] for udi, u_depi in zip(u[3:], u_dep.tolist())} # Active forces: F_O acting on point O; F_P acting on point P. # Generalized active forces (unconstrained): Fr_u = F_point * pv_point. F_O = m*g*A.z F_P = Fx * A.x + Fy * A.y + Fz * A.z Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in zip(partial_v_O, partial_v_P)]) # Inertia force: R_star_O. # Inertia of disc: I_C_O, where J is a inertia component about principal axis. # Inertia torque: T_star_C. # Generalized inertia forces (unconstrained): Fr_star_u. R_star_O = -m*O.acc(N) I_C_O = inertia(B, I, J, I) T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \ + cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N)))) Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in zip(partial_v_O, partial_w_C)]) # Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c. # Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady. Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :] Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\ + A_rs.T * Fr_star_u[3:6, :] Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\ .subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand() # Second, using KaneMethod in mechanics for fr, frstar and frstar_steady. # Rigid Bodies: disc, with inertia I_C_O. iner_tuple = (I_C_O, O) disc = RigidBody('disc', O, C, m, iner_tuple) bodyList = [disc] # Generalized forces: Gravity: F_o; Auxiliary forces: F_p. F_o = (O, F_O) F_p = (P, F_P) forceList = [F_o, F_p] # KanesMethod. kane = KanesMethod( N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs, q_dependent=q[3:], configuration_constraints = f_c, u_dependent=u[3:], velocity_constraints= f_v, u_auxiliary=ua ) # fr, frstar, frstar_steady and kdd(kinematic differential equations). (fr, frstar)= kane.kanes_equations(forceList, bodyList) frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\ .subs({q[3]: -r*cos(q[1])}).expand() kdd = kane.kindiffdict() # Test # First try Fr_c == fr; # Second try Fr_star_c == frstar; # Third try Fr_star_steady == frstar_steady. # Both signs are checked in case the equations were found with an inverse # sign. assert ((Matrix(Fr_c).expand() == fr.expand()) or (Matrix(Fr_c).expand() == (-fr).expand())) assert ((Matrix(Fr_star_c).expand() == frstar.expand()) or (Matrix(Fr_star_c).expand() == (-frstar).expand())) assert ((Matrix(Fr_star_steady).expand() == frstar_steady.expand()) or (Matrix(Fr_star_steady).expand() == (-frstar_steady).expand()))
kinematic_differential_equations = [omega1 - theta1.diff(), omega2 - theta2.diff(), omega3 - theta3.diff()] #Sets up the rotational axes of the angular velocities l_leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) l_leg_frame.ang_vel_in(inertial_frame) body_frame.set_ang_vel(l_leg_frame, omega2*inertial_frame.z) body_frame.ang_vel_in(inertial_frame) r_leg_frame.set_ang_vel(body_frame, omega3*inertial_frame.z) r_leg_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages l_ankle.set_vel(inertial_frame, 0) l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame) l_leg_mass_center.vel(inertial_frame) l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame) l_hip.vel(inertial_frame) body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame) body_mass_center.vel(inertial_frame) r_hip.v2pt_theory(l_hip, inertial_frame, body_frame) r_hip.vel(inertial_frame) r_leg_mass_center.v2pt_theory(r_hip, inertial_frame, r_leg_frame) r_leg_mass_center.vel(inertial_frame) #Sets up the masses of the linkages l_leg_mass, body_mass, r_leg_mass = symbols('m_LL, m_B, m_RL') #Sets up the rotational inertia of the linkages l_leg_inertia, body_inertia, r_leg_inertia = symbols('I_LLz, I_Bz, I_RLz')
#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()] #Sets up the rotational axes of the angular velocities leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) leg_frame.ang_vel_in(inertial_frame) body_frame.set_ang_vel(leg_frame, omega2*inertial_frame.z) body_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages ankle.set_vel(inertial_frame, 0) waist.v2pt_theory(ankle, inertial_frame, leg_frame) waist.vel(inertial_frame) body.v2pt_theory(waist, inertial_frame, body_frame) body.vel(inertial_frame) #Sets up the masses of the linkages leg_mass, body_mass = symbols('m_L, m_B') #Defines the linkages as particles waistP = Particle('waistP', waist, leg_mass) bodyP = Particle('bodyP', body, body_mass) #Sets up gravity information and assigns gravity to act on mass centers g = symbols('g') leg_grav_force_vector = -1*leg_mass*g*inertial_frame.y leg_grav_force = (waist, leg_grav_force_vector) body_grav_force_vector = -1*body_mass*g*inertial_frame.y
# Angular velocity using u's as body fixed measure numbers of angular velocity R.set_ang_vel(N, u[0]*R.x + u[1]*R.y + u[2]*R.z) # Rattleback ground contact point P = Point('P') P.set_vel(N, ua[0]*Y.x + ua[1]*Y.y + ua[2]*Y.z) # Rattleback ellipsoid center location, see: # "Realistic mathematical modeling of the rattleback", Kane, Thomas R. and # David A. Levinson, 1982, International Journal of Non-Linear Mechanics mu = [dot(rk, Y.z) for rk in R] eps = sqrt((a*mu[0])**2 + (b*mu[1])**2 + (c*mu[2])**2) O = P.locatenew('O', -a*a*mu[0]/eps*R.x -b*b*mu[1]/eps*R.y -c*c*mu[2]/eps*R.z) O.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), O.pos_from(P))) # Mass center position and velocity RO = O.locatenew('RO', d*R.x + e*R.y + f*R.z) RO.set_vel(N, P.vel(N) + cross(R.ang_vel_in(N), RO.pos_from(P))) qd_rhs = [(u[2]*cos(q[2]) - u[0]*sin(q[2]))/cos(q[1]), u[0]*cos(q[2]) + u[2]*sin(q[2]), u[1] + tan(q[1])*(u[0]*sin(q[2]) - u[2]*cos(q[2])), dot(P.pos_from(O).diff(t, R), N.x), dot(P.pos_from(O).diff(t, R), N.y)] # Partial angular velocities and partial velocities partial_w = [R.ang_vel_in(N).diff(ui, N) for ui in u + ua] partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua] partial_v_RO = [RO.vel(N).diff(ui, N) for ui in u + ua]
kinematic_differential_equations = [omega_a - theta_a.diff(), omega_b - theta_b.diff(), omega_c - theta_c.diff()] #Sets up the rotational axes of the angular velocities a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z) a_frame.ang_vel_in(inertial_frame) b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z) b_frame.ang_vel_in(inertial_frame) c_frame.set_ang_vel(b_frame, omega_c*inertial_frame.z) c_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages A.set_vel(inertial_frame, 0) B.v2pt_theory(A, inertial_frame, a_frame) B.vel(inertial_frame) C.v2pt_theory(B, inertial_frame, b_frame) C.vel(inertial_frame) D.v2pt_theory(C, inertial_frame, c_frame) D.vel(inertial_frame) #Sets up the masses of the linkages a_mass, b_mass, c_mass = symbols('m_a, m_b, m_c') #Defines the linkages as particles bP = Particle('bP', B, a_mass) cP = Particle('cP', C, b_mass) dP = Particle('dP', D, c_mass) #Sets up gravity information and assigns gravity to act on mass centers g = symbols('g')
#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()] #Sets up the rotational axes of the angular velocities one_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) one_frame.ang_vel_in(inertial_frame) two_frame.set_ang_vel(one_frame, omega2*inertial_frame.z) two_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages #one.set_vel(inertial_frame, 0) two.v2pt_theory(one, inertial_frame, one_frame) two.vel(inertial_frame) three.v2pt_theory(two, inertial_frame, two_frame) three.vel(inertial_frame) #Sets up the masses of the linkages one_mass, two_mass = symbols('m_1, m_2') #Defines the linkages as particles twoP = Particle('twoP', two, one_mass) threeP = Particle('threeP', three, two_mass) #Sets up gravity information and assigns gravity to act on mass centers g = symbols('g') two_grav_force_vector = -1*one_mass*g*inertial_frame.y two_grav_force = (two, two_grav_force_vector) three_grav_force_vector = -1*two_mass*g*inertial_frame.y
one_frame_dp2.set_ang_vel(inertial_frame_dp2, omega1_dp2 * inertial_frame_dp2.z) one_frame_dp2.ang_vel_in(inertial_frame_dp2) two_frame_dp1.set_ang_vel(one_frame_dp1, omega2_dp1 * one_frame_dp1.z - omega1_dp2 * one_frame_dp1.z) two_frame_dp1.ang_vel_in(inertial_frame_dp1) two_frame_dp2.set_ang_vel(one_frame_dp2, omega2_dp2 * one_frame_dp2.z) two_frame_dp2.ang_vel_in(inertial_frame_dp2) # Linear Velocities # ================= one_dp1.set_vel(inertial_frame_dp1, 0) one_dp2.set_vel(inertial_frame_dp2, two_dp1.vel) one_mass_center_dp1.v2pt_theory(one_dp1, inertial_frame_dp1, one_frame_dp1) one_mass_center_dp1.vel(inertial_frame_dp1) one_mass_center_dp2.v2pt_theory(one_dp2, inertial_frame_dp2, one_frame_dp2) one_mass_center_dp2.vel(inertial_frame_dp2) two_dp1.v2pt_theory(one_dp1, inertial_frame_dp1, one_frame_dp1) two_dp1.vel(inertial_frame_dp1) two_dp2.v2pt_theory(one_dp2, inertial_frame_dp2, one_frame_dp2) two_dp2.vel(inertial_frame_dp2) two_mass_center_dp1.v2pt_theory(two_dp1, inertial_frame_dp1, two_frame_dp1) two_mass_center_dp2.v2pt_theory(two_dp2, inertial_frame_dp2, two_frame_dp2) # Mass # ==== a_mass, b_mass, c_mass = symbols("m_a, m_b, m_c")
# Angular Velocities # ================== one_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) one_frame.ang_vel_in(inertial_frame) two_frame.set_ang_vel(one_frame, omega2 * one_frame.z) two_frame.ang_vel_in(inertial_frame) # Linear Velocities # ================= one.set_vel(inertial_frame, 0) one_mass_center.v2pt_theory(one, inertial_frame, one_frame) one_mass_center.vel(inertial_frame) two.v2pt_theory(one, inertial_frame, one_frame) two.vel(inertial_frame) two_mass_center.v2pt_theory(two, inertial_frame, two_frame) # Mass # ==== one_mass, two_mass = symbols('m_1, m_2') # Inertia # ======= rotI = lambda I, f: Matrix([j & I & i for i in f for j in f]).reshape(3,3)
def test_partial_velocity(): q1, q2, q3, u1, u2, u3 = dynamicsymbols("q1 q2 q3 u1 u2 u3") u4, u5 = dynamicsymbols("u4, u5") r = symbols("r") N = ReferenceFrame("N") Y = N.orientnew("Y", "Axis", [q1, N.z]) L = Y.orientnew("L", "Axis", [q2, Y.x]) R = L.orientnew("R", "Axis", [q3, L.y]) R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z) C = Point("C") C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x)) Dmc = C.locatenew("Dmc", r * L.z) Dmc.v2pt_theory(C, N, R) vel_list = [Dmc.vel(N), C.vel(N), R.ang_vel_in(N)] u_list = [u1, u2, u3, u4, u5] assert partial_velocity(vel_list, u_list) == [ [-r * L.y, 0, L.x], [r * L.x, 0, L.y], [0, 0, L.z], [L.x, L.x, 0], [cos(q2) * L.y - sin(q2) * L.z, cos(q2) * L.y - sin(q2) * L.z, 0], ]