Python协作运动机器人刚体力学解耦模型

🎯要点

🎯腿式或固定式机器人模型 | 🎯网格、点云和体素网格碰撞检测 | 🎯正反向运动学和动力学 | 🎯机器人刚体力学计算 | 🎯编辑参考系姿势和路径 | 🎯软件接口实体机器人模拟 | 🎯三角网格碰撞刚体模拟 | 🎯机器人比例微分积分控制、扭矩控制和速度控制模拟 | 🎯相机、激光传感器、陀螺仪、力/扭矩传感器和加速度计模拟 | 🎯集成ROS模拟 | 🎯运动规划器、约束流形上的运动规划 | 🎯快速轨迹优化 | 🎯实时运动规划 | 🎯解耦规划模型与模拟模型

📜刚体力学用例：Python自行车六自由度飞行器多连接件非线性运动方程模型

🍇Python力学动能势能

粒子 P 的线性动量定义为：
L P = m v L_P=m v LP=mv

其中 m m m是粒子 P P P的质量， v v v是粒子在惯性系中的速度。

类似地，刚体的线性动量定义为：
L B = m v ∗ L_B=m v ^* LB=mv∗

其中 m m m是刚体的质量， B B B， v ∗ v^* v∗是惯性系中 B B B质心的速度。

质点 P 绕惯性系 N 中任意点 O 的角动量定义为：
N H P / O = r × m v { }^N H ^{P / O}= r \times m v NHP/O=r×mv

其中 r r r是从点 O O O到质量 m m m的粒子的位置向量， v v v是惯性系中粒子的速度。

类似地，刚体 B 绕惯性系 N 中的点 O 的角动量定义为：
N H B / O = N H B / B ∗ + N H B ∗ / O { }^N H ^{B / O}={ }^N H ^{B / B^*}+{ }^N H ^{B^*} / O NHB/O=NHB/B∗+NHB∗/O

其中物体绕其质心的角动量为：
N H B / B ∗ = I ∗ ⋅ ω { }^N H ^{B / B^*}= I ^* \cdot \omega NHB/B∗=I∗⋅ω

质心关于 O 的角动量为：
N H B ∗ / O = r ∗ × m v ∗ { }^N H ^{B^*} / O= r ^* \times m v ^* NHB∗/O=r∗×mv∗

Python伪代码实现上述动量：

from pyx import symbols
from pyx.physics.mechanics import dynamicsymbols, ReferenceFrame
from pyx.physics.mechanics import RigidBody, Particle, Point, outer
from pyx.physics.mechanics import linear_momentum, angular_momentum
from pyx.physics.vector import init_vprinting
init_vprinting(pretty_print=False)
m, M, l1 = symbols('m M l1')
q1d = dynamicsymbols('q1d')
N = ReferenceFrame('N')
O = Point('O')
O.set_vel(N, 0 * N.x)
Ac = O.locatenew('Ac', l1 * N.x)
P = Ac.locatenew('P', l1 * N.x)
a = ReferenceFrame('a')
a.set_ang_vel(N, q1d * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)

最后，创建组成系统的主体。在这种情况下，系统由粒子 Pa 和刚体 A 组成。

Pa = Particle('Pa', P, m)
I = outer(N.z, N.z)
A = RigidBody('A', Ac, a, M, (I, Ac))

然后，人们可以选择评估系统各个组件的动量或整个系统本身的动量。

linear_momentum(N,A)
angular_momentum(O, N, Pa)
linear_momentum(N, A, Pa)
angular_momentum(O, N, A, Pa)

粒子 P 的动能定义为
T P = 1 2 m v 2 T_P=\frac{1}{2} m v ^2 TP=21mv2

其中 m m m是粒子 P P P的质量， v v v是粒子在惯性系中的速度。

类似地，刚体 B 的动能定义为
T B = T t + T τ T_B=T_t+T_\tau TB=Tt+Tτ

其中平动动能由下式给出：
T t = 1 2 m v ∗ ⋅ v ∗ T_t=\frac{1}{2} m v ^* \cdot v ^* Tt=21mv∗⋅v∗

旋转动能由下式给出：
T r = 1 2 ω ⋅ I ∗ ⋅ ω T_r=\frac{1}{2} \omega \cdot I ^* \cdot \omega Tr=21ω⋅I∗⋅ω

其中 m m m是刚体的质量， v ∗ v^* v∗是惯性系中质心的速度， ω \omega ω是刚体的惯性角速度， I ∗ I^* I∗是中心惯性二元。

势能定义为物体或系统因其位置或排列而拥有的能量。

物体或物体系统的拉格朗日定义为：
L = T − V L =T-V L=T−V

其中 T T T和 V V V分别是动能和势能。

Python伪代码实现：

from pyx import symbols
from pyx.physics.mechanics import dynamicsymbols, ReferenceFrame, outer
from pyx.physics.mechanics import RigidBody, Particle
from pyx.physics.mechanics import kinetic_energy, potential_energy, Point
from pyx.physics.vector import init_vprinting
init_vprinting(pretty_print=False)
m, M, l1, g, h, H = symbols('m M l1 g h H')
omega = dynamicsymbols('omega')
N = ReferenceFrame('N')
O = Point('O')
O.set_vel(N, 0 * N.x)
Ac = O.locatenew('Ac', l1 * N.x)
P = Ac.locatenew('P', l1 * N.x)
a = ReferenceFrame('a')
a.set_ang_vel(N, omega * N.z)
Ac.v2pt_theory(O, N, a)
P.v2pt_theory(O, N, a)
Pa = Particle('Pa', P, m)
I = outer(N.z, N.z)
A = RigidBody('A', Ac, a, M, (I, Ac))

然后，用户可以确定系统中任意数量实体的动能：

kinetic_energy(N, Pa)
kinetic_energy(N, Pa, A)

然后可以确定构成系统的任意数量的实体的势能：

Pa.potential_energy = m * g * h
A.potential_energy = M * g * H
potential_energy(A, Pa)

我们还可以确定该系统的拉格朗日量：

from pyx.physics.mechanics import Lagrangian
from pyx.physics.vector import init_vprinting
init_vprinting(pretty_print=False)
Lagrangian(N, Pa, A)

👉参阅更新：计算思维 | 亚图跨际