sympy 或 scipy 中是否有对应于 matlab 中的 massMatrixForm 的函数？

Question

我正在从以下机构学习刚体运动学： https://rotations.berkeley.edu/the-poisson-top-with-friction/。 matlab源代码的link在页面底部。导出泊松顶的运动方程： http://rotations.berkeley.edu/wp-content/uploads/2017/10/derive_poisson_top_ODEs.txt 求解泊松顶的运动方程： 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

我重写了python中推导过程的大部分代码，虽然可能不是优雅的代码。

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))

实际上无法求解运动方程的一阶微分形式D2psi、D2theta、D2phi、D2x1、D2x2的表达式，sympy.solve.

sympy或scipy中有对应matlab中massMatrixForm的函数吗？或类似的东西？所以我可以使用 solve_ivp.

在 python 中重写这个模拟

Answer 1

我现在没有时间测试您的代码，但我相信您正在寻找 linear_eq_to_matrix 函数。

将 solve 命令替换为以下内容：

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

sympy 或 scipy 中是否有对应于 matlab 中的 massMatrixForm 的函数？

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

python

scipy

sympy