SymPyのLagrangesMethodで運動方程式の導出とコードの出力

sympy_lagranges.py

#coding:utf-8

from sympy import *
from sympy.physics.mechanics import LagrangesMethod, Lagrangian
from sympy.physics.mechanics import ReferenceFrame, Particle, Point
from sympy.physics.mechanics import dynamicsymbols, kinetic_energy
from sympy.utilities.codegen import *

if __name__ == "__main__":

	# IPythonで数式をlatexっぽく出来る
	#from sympy.interactive import printing
	#printing.init_printing(use_latex=True)

	# 角度
	theta1 = dynamicsymbols("theta_1")
	theta2 = dynamicsymbols("theta_2")

	# 角度の微分
	theta1_d = dynamicsymbols("theta_1",1)
	theta2_d = dynamicsymbols("theta_2",1)

	# 重さと長さと重力加速度と時間
	m1,l1,m2,l2,g,t = symbols("m_1 l_1 m_2 l_2 g t")

	# 位置座標をtheta1,theta2で表してから……
	x1 = l1 * sin(theta1)
	y1 = l1 * cos(theta1)
	x2 = x1 + l2 * sin(theta2)
	y2 = y1 + l2 * cos(theta2)

	# 時間tで微分してやるとx,y方向の速さになる
	vx1 = diff(x1,t)
	vy1 = diff(y1,t)
	vx2 = diff(x2,t)
	vy2 = diff(y2,t)

	# 基準座標系を定義
	N = ReferenceFrame("N")

	# 点を作成
	P1 = Point("P_1")
	P2 = Point("P_2")

	# 計算させた速度を質点に指定．
	P1.set_vel(N,vx1 * N.x + vy1 * N.y)
	P2.set_vel(N,vx2 * N.x + vy2 * N.y)

	# 質点を作成してる(と思う．Particleってなんだ)
	Pa1 = Particle("Pa_1",P1,m1)
	Pa2 = Particle("Pa_2",P2,m2)

	# 位置エネルギーを与えてやる
	Pa1.potential_energy = m1 * g * y1
	Pa2.potential_energy = m2 * g * y2

	# 角質点にかかる力をリストアップ
	fl = [(P1,-m1*g*N.y),(P2,-m2*g*N.y)]

	# ラグランジアンを作成
	L = Lagrangian(N,Pa1,Pa2)

	# ラグランジアンから微分方程式を生成
	LM = LagrangesMethod(L,[theta1,theta2],forcelist = fl,frame=N)
	LM.form_lagranges_equations()
	
	# 数値計算用に連立微分方程式の形に整形する
	# ついでにsimplifyで単純な形にする
	eq1 = simplify( LM.rhs() )
	
	# コードを吐き出させる
	# ただしtheta1,2とその微分をtの関数としているためそのまま使えない
	# その辺については妥協策を探してる
	[(c_name, c_code), (h_name, c_header)] = codegen(
		# 式を指定
		name_expr=("diff",eq1),
		# 言語を指定．"C"ならC言語，"F95"ならFortran
		language="C",
		# プロジェクト名
		project="multi_pendulum",
		# ファイルへの出力の有無
		# Trueにするとタプルを返さないので代入するとエラーになる
		to_files=False
	)
	print( c_name )
	print( c_code )
	print( h_name )
	print( c_header )