Is there a function in sympy or scipy that corresponds to massMatrixForm in matlab?

I am learning rigid body kinematics from: https://rotations.berkeley.edu/the-poisson-top-with-friction/ . The link to the matlab source code is at the bottom of the page. Derive the Poisson top’s equations of motion: http://rotations.berkeley.edu/wp-content/uploads/2017/10/derive_poisson_top_ODEs.txt Solve the Poisson top’s equations of motion: http://rotations.berkeley.edu/wp-content/uploads/2017/10/solve_poisson_top_ODEs.txt

%  Express the state equations in mass-matrix form, M(t,Y)*Y'(t) = F(t,Y):

[Msym, Fsym] = massMatrixForm(StateEqns, StateVars);

Msym = simplify(Msym)
Fsym = simplify(Fsym)

%  Convert M(t,Y) and F(t,Y) to symbolic function handles with the input 
%  parameters specified:

M = odeFunction(Msym, StateVars, m, g, lambdat, lambdaa, L3, muk);  
F = odeFunction(Fsym, StateVars, m, g, lambdat, lambdaa, L3, muk); 

%  Save M(t,Y) and F(t,Y):

save poisson_top_ODEs.mat Msym Fsym StateVars M F

I rewrote most of the code for the derivation process in python, though may not be elegant code.

from sympy import * 
from sympy.physics.mechanics import *
from sympy.physics.vector import vlatex
import os
os.system('cls')

psi, theta, phi, Dpsi, Dtheta, Dphi, D2psi, D2theta, D2phi = dynamicsymbols('psi theta phi Dpsi Dtheta Dphi D2psi D2theta D2phi') 
x1, x2, x3, Dx1, Dx2, Dx3, D2x1, D2x2, D2x3 = dynamicsymbols('x1 x2 x3 Dx1 Dx2 Dx3 D2x1 D2x2 D2x3') 
m, g, lambdat, lambdaa, L3 = symbols('m g lambdat lambdaa L3') #, real=True, positive=True) 
muk = symbols('muk', real=True) 

R1 = Matrix([[cos(psi), sin(psi), 0],        
      [-sin(psi), cos(psi), 0],
      [0, 0, 1]])  

R2 = Matrix([[1, 0, 0],
      [0, cos(theta), sin(theta)],
      [0, -sin(theta), cos(theta)]])

R3 = Matrix([[cos(phi), sin(phi), 0],
      [-sin(phi), cos(phi), 0],
      [0, 0, 1]])  

tmp1=Matrix([0, 0, diff(psi,'t')])
tmp2=Matrix([diff(theta,'t'), 0, 0])
tmp3=Matrix([0, 0, diff(phi,'t')])

omega = simplify((R3*R2*R1)*tmp1 + (R3*R2)*tmp2 + R3*tmp3)
# print(vlatex(omega))

vP = simplify(Matrix([Dx1, Dx2, Dx3]) - transpose(R3*R2*R1)*(omega.cross(Matrix([0, 0, L3]))))
# print(vlatex(vP))

tmp1 = Matrix([1, 0, 0]).T
tmp2 = Matrix([0, 1, 0]).T
tmp3 = Matrix([0, 0, 1]).T

vP1 = tmp1.dot(vP)
vP2 = tmp2.dot(vP)
# print(vP1,vP2)

vCOM = simplify(Matrix([vP1, vP2, 0]) + transpose(R3*R2*R1)*(omega.cross(Matrix([0, 0, L3]))))
# print(vlatex(vCOM))

Dx3 = tmp3.dot(vCOM)
# print(Dx3)

omega = omega.subs([(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi)])
# print_latex(omega)

vP1 = vP1.subs([(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi)])
vP2 = vP2.subs([(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi)])
Dx3 = Dx3.subs([(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi)])

vP = simplify(sqrt(vP1**2 + vP2**2))
# print(vP1,vP2,vP,Dx3)

N = m*diff(Dx3,'t') + m*g
F1 = -muk*N*vP1/vP
F2 = -muk*N*vP2/vP
# print(vlatex(F1))

eq1 = Eq(m*diff(Dx1,'t'), F1).subs([(diff(Dx1,'t'),D2x1),(diff(Dx2,'t'),D2x2)])
eq2 = Eq(m*diff(Dx2,'t'), F2).subs([(diff(Dx1,'t'),D2x1),(diff(Dx2,'t'),D2x2)])

H = diag(lambdat, lambdat, lambdaa)*omega
omegaRF = omega

DH = diff(H,'t') + omegaRF.cross(H)

sumM = Matrix([0, 0, -L3]).cross((R3*R2*R1)*Matrix([F1, F2, N]))

tmp1=Matrix([[sin(phi), -cos(phi), 0],
            [cos(phi), sin(phi), 0],
            [0, 0, 1]])

DHt = tmp1*DH
sumMt = tmp1*sumM

eq3 = Eq(DHt[0,0],sumMt[0,0])
eq4 = Eq(DHt[1,0],sumMt[1,0])
eq5 = Eq(DHt[2,0],sumMt[2,0])

eq1 = simplify(eq1.subs(
      [(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi),
      (diff(Dpsi,'t'), D2psi), (diff(Dtheta,'t'), D2theta), (diff(Dphi,'t'), D2phi)]))

eq2 = simplify(eq2.subs(
      [(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi),
      (diff(Dpsi,'t'), D2psi), (diff(Dtheta,'t'), D2theta), (diff(Dphi,'t'), D2phi)]))

eq3 = simplify(eq3.subs(
      [(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi),
      (diff(Dpsi,'t'), D2psi), (diff(Dtheta,'t'), D2theta), (diff(Dphi,'t'), D2phi)]))

eq4 = simplify(eq4.subs(
      [(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi),
      (diff(Dpsi,'t'), D2psi), (diff(Dtheta,'t'), D2theta), (diff(Dphi,'t'), D2phi)]))

eq5 = simplify(eq5.subs(
      [(diff(psi,'t'), Dpsi), (diff(theta,'t'), Dtheta), (diff(phi,'t'), Dphi),
      (diff(Dpsi,'t'), D2psi), (diff(Dtheta,'t'), D2theta), (diff(Dphi,'t'), D2phi)]))

res = solve([eq1,eq2,eq3,eq4,eq5],[D2psi, D2theta, D2phi, D2x1, D2x2])

print('--------')
print(vlatex(res))

In fact, it is impossible to solve the expressions of D2psi, D2theta, D2phi, D2x1 and D2x2, which are the first order differential form of the equations of motion, with sympy.solve.

Is there a function in sympy or scipy that corresponds to massMatrixForm in matlab? Or something like that? So I can rewrite this simulation in python by using solve_ivp.

Answer

I don’t have time to test your code right now, but I believe you are looking for the linear_eq_to_matrix function.

Replace the solve command with the following:

A, b = linear_eq_to_matrix([eq1,eq2,eq3,eq4,eq5],[D2psi, D2theta, D2phi, D2x1, D2x2])

Advertisement

Answer