-
-
Save Hasenpfote/034cc499827da562fed3c8f676b868f0 to your computer and use it in GitHub Desktop.
Rigid-body dynamics
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
{ | |
"cells": [ | |
{ | |
"cell_type": "code", | |
"execution_count": 1, | |
"id": "c4acd667-3a03-42dc-b98d-2ec5b1d85046", | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"import numpy as np\n", | |
"# https://pypi.org/project/numpy-quaternion/\n", | |
"import quaternion" | |
] | |
}, | |
{ | |
"cell_type": "markdown", | |
"id": "d66d69fa-cbc0-4d8d-adc7-96aa25bbb23a", | |
"metadata": {}, | |
"source": [ | |
"## 慣性テンソル" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 2, | |
"id": "c670e926-b8c0-4050-a91f-28d5172493aa", | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"# 直方体の質量と慣性テンソルを計算する\n", | |
"def calc_cuboid_mass_and_inertia(density, half_extent):\n", | |
" mass = 8.0 * np.prod(half_extent) * density\n", | |
" sqr = half_extent * half_extent\n", | |
" inertia = mass * np.array([sqr[1] + sqr[2], sqr[0] + sqr[2], sqr[0] + sqr[1]]) / 3.0\n", | |
" return mass, np.diag(inertia)\n", | |
"\n", | |
"# 球の質量と慣性テンソルを計算する\n", | |
"def calc_sphere_mass_and_inertia(density, radius):\n", | |
" mass = (4.0 / 3.0) * np.pi * (radius ** 3) * density\n", | |
" value = 0.4 * mass * (radius ** 2)\n", | |
" return mass, np.diag([value, value, value])\n" | |
] | |
}, | |
{ | |
"cell_type": "markdown", | |
"id": "0e24b58c-a00c-4251-847d-3e7014b38aed", | |
"metadata": {}, | |
"source": [ | |
"## 剛体のパラメータ" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 3, | |
"id": "9b57dcfe-6fa4-4fa1-989c-1cb1652687f7", | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"# 剛体の状態\n", | |
"class State:\n", | |
" def __init__(self):\n", | |
" # 位置\n", | |
" self.position = np.array([0.0, 0.0, 0.0])\n", | |
" # 姿勢\n", | |
" self.orientation = np.quaternion(1.0, 0.0, 0.0, 0.0)\n", | |
" # 並進速度\n", | |
" self.linear_velocity = np.array([0.0, 0.0, 0.0])\n", | |
" # 回転速度\n", | |
" self.angular_velocity = np.array([0.0, 0.0, 0.0])\n", | |
"\n", | |
" def __str__(self):\n", | |
" axis = quaternion.as_rotation_vector(self.orientation)\n", | |
" angle = np.linalg.norm(axis)\n", | |
" axis = axis / angle if angle > 0.0 else np.array([0.0, 0.0, 0.0])\n", | |
"\n", | |
" return 'pos: [{:.5f},{:.5f},{:.5f}] axis: [{:.5f},{:.5f},{:.5f}] angle: {:.5f} ({:.5f})'.format(\n", | |
" self.position[0], self.position[1], self.position[2],\n", | |
" axis[0], axis[1], axis[2], angle, np.rad2deg(angle))\n", | |
" \n", | |
"# 剛体の属性\n", | |
"class RigidBody:\n", | |
" def __init__(self):\n", | |
" # 慣性テンソル\n", | |
" self.inertia = np.identity(3)\n", | |
" # 質量\n", | |
" self.mass = 1.0\n", | |
" # 反発係数\n", | |
" self.restitution = 0.2\n", | |
" # 摩擦係数\n", | |
" self.friction = 0.6\n", | |
"\n", | |
" def __str__(self):\n", | |
" return 'I: {} m: {:.5f} e: {:.5f} f: {:.5f}'.format(\n", | |
" self.inertia, self.mass, self.restitution, self.friction)\n" | |
] | |
}, | |
{ | |
"cell_type": "markdown", | |
"id": "2de7db4b-ecc4-49f6-badb-73e1f7e870f3", | |
"metadata": {}, | |
"source": [ | |
"## 力の適用" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 4, | |
"id": "b21e6186-5127-405a-8b46-054a4a750a8f", | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"# 力を適用する\n", | |
"def apply_force(state, body, force, time_step):\n", | |
" state.linear_velocity += (force / body.mass) * time_step\n", | |
"\n", | |
"# トルクを適用する\n", | |
"def apply_torque(state, body, torque, time_step):\n", | |
" orientation = quaternion.as_rotation_matrix(state.orientation)\n", | |
" world_inertia = orientation @ body.inertia @ orientation.T\n", | |
" world_inertia_inv = np.linalg.inv(world_inertia)\n", | |
"\n", | |
" angular_momentum = world_inertia @ state.angular_velocity\n", | |
" angular_momentum += torque * time_step\n", | |
"\n", | |
" state.angular_velocity = world_inertia_inv @ angular_momentum\n", | |
"\n", | |
"# (剛体上の)位置に力を適用する\n", | |
"def apply_force_at_position(state, body, force, position, time_step):\n", | |
" apply_force(state, body, force, time_step)\n", | |
" torque = np.cross(position - state.position, force)\n", | |
" apply_torque(state, body, torque, time_step)\n" | |
] | |
}, | |
{ | |
"cell_type": "markdown", | |
"id": "d8d22013-090c-488c-a02d-0f4dc26dfe5d", | |
"metadata": {}, | |
"source": [ | |
"## 力積の適用" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 5, | |
"id": "6b5fdddb-86e5-4825-a7f0-5677061e4fc6", | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"# 力積を適用する\n", | |
"def apply_impulse(state, body, impulse):\n", | |
" state.linear_velocity += impulse / body.mass\n", | |
"\n", | |
"# 角力積を適用する\n", | |
"def apply_angular_impulse(state, body, impulse):\n", | |
" orientation = quaternion.as_rotation_matrix(state.orientation)\n", | |
" world_inertia = orientation @ body.inertia @ orientation.T\n", | |
" world_inertia_inv = np.linalg.inv(world_inertia)\n", | |
"\n", | |
" angular_momentum = world_inertia @ state.angular_velocity\n", | |
" angular_momentum += impulse\n", | |
"\n", | |
" state.angular_velocity = world_inertia_inv @ angular_momentum\n", | |
"\n", | |
"# (剛体上の)位置に力積を適用する\n", | |
"def apply_impulse_at_position(state, body, impulse, position):\n", | |
" apply_impulse(state, body, impulse)\n", | |
" angular_impulse = np.cross(position - state.position, impulse)\n", | |
" apply_angular_impulse(state, body, angular_impulse)\n" | |
] | |
}, | |
{ | |
"cell_type": "markdown", | |
"id": "eff1c91f-0443-4768-b469-d476d6133d9c", | |
"metadata": {}, | |
"source": [ | |
"## 更新" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 6, | |
"id": "b57843e0-7f9e-4e08-a675-a5ac4588246d", | |
"metadata": {}, | |
"outputs": [], | |
"source": [ | |
"# 時間積分をする\n", | |
"def integrate(state, time_step):\n", | |
" # 位置の更新\n", | |
" state.position += state.linear_velocity * time_step\n", | |
" # 姿勢の更新\n", | |
" x, y, z = state.angular_velocity\n", | |
" dang = 0.5 * np.quaternion(x, y, z) * state.orientation\n", | |
" state.orientation = (state.orientation + dang * time_step).normalized()\n" | |
] | |
}, | |
{ | |
"cell_type": "markdown", | |
"id": "41990227-b93a-4c79-a948-4ddc4c1d8d85", | |
"metadata": {}, | |
"source": [ | |
"## シミュレーション" | |
] | |
}, | |
{ | |
"cell_type": "code", | |
"execution_count": 7, | |
"id": "8c8a634f-bd90-4ef0-bb53-eba7ec9da7b5", | |
"metadata": {}, | |
"outputs": [ | |
{ | |
"name": "stdout", | |
"output_type": "stream", | |
"text": [ | |
"inertia:\n", | |
"[[0.16666667 0. 0. ]\n", | |
" [0. 0.16666667 0. ]\n", | |
" [0. 0. 0.16666667]]\n", | |
"000: pos: [0.00000,-0.00272,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"001: pos: [0.00000,-0.00817,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"002: pos: [0.00000,-0.01633,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"003: pos: [0.00000,-0.02722,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"004: pos: [0.00000,-0.04083,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"005: pos: [0.00000,-0.05717,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"006: pos: [0.00000,-0.07622,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"007: pos: [0.00000,-0.09800,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"008: pos: [0.00000,-0.12250,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"009: pos: [0.00000,-0.14972,0.00000] axis: [0.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n", | |
"010: pos: [0.00000,-0.17967,0.00000] axis: [0.00000,0.00000,1.00000] angle: 0.16628 (9.52728)\n", | |
"011: pos: [0.00000,-0.21233,0.00000] axis: [0.00000,0.00000,1.00000] angle: 0.33256 (19.05457)\n", | |
"012: pos: [0.00000,-0.24772,0.00000] axis: [0.00000,0.00000,1.00000] angle: 0.49885 (28.58185)\n", | |
"013: pos: [0.00000,-0.28583,0.00000] axis: [0.00000,0.00000,1.00000] angle: 0.66513 (38.10913)\n", | |
"014: pos: [0.00000,-0.32667,0.00000] axis: [0.00000,0.00000,1.00000] angle: 0.83141 (47.63642)\n", | |
"015: pos: [0.00000,-0.37022,0.00000] axis: [0.00000,0.00000,1.00000] angle: 0.99769 (57.16370)\n", | |
"016: pos: [0.00000,-0.41650,0.00000] axis: [0.00000,0.00000,1.00000] angle: 1.16398 (66.69098)\n", | |
"017: pos: [0.00000,-0.46550,0.00000] axis: [0.00000,0.00000,1.00000] angle: 1.33026 (76.21827)\n", | |
"018: pos: [0.00000,-0.51722,0.00000] axis: [0.00000,0.00000,1.00000] angle: 1.49654 (85.74555)\n", | |
"019: pos: [0.00000,-0.57167,0.00000] axis: [0.00000,0.00000,1.00000] angle: 1.66282 (95.27283)\n", | |
"020: pos: [0.00000,-0.62883,0.00000] axis: [0.00000,0.00000,1.00000] angle: 1.82911 (104.80012)\n", | |
"021: pos: [0.00000,-0.68872,0.00000] axis: [0.00000,0.00000,1.00000] angle: 1.99539 (114.32740)\n", | |
"022: pos: [0.00000,-0.75133,0.00000] axis: [0.00000,0.00000,1.00000] angle: 2.16167 (123.85468)\n", | |
"023: pos: [0.00000,-0.81667,0.00000] axis: [0.00000,0.00000,1.00000] angle: 2.32795 (133.38197)\n", | |
"024: pos: [0.00000,-0.88472,0.00000] axis: [0.00000,0.00000,1.00000] angle: 2.49424 (142.90925)\n", | |
"025: pos: [0.00000,-0.95550,0.00000] axis: [0.00000,0.00000,1.00000] angle: 2.66052 (152.43653)\n", | |
"026: pos: [0.00000,-1.02900,0.00000] axis: [0.00000,0.00000,1.00000] angle: 2.82680 (161.96382)\n", | |
"027: pos: [0.00000,-1.10522,0.00000] axis: [0.00000,0.00000,1.00000] angle: 2.99308 (171.49110)\n", | |
"028: pos: [0.00000,-1.18417,0.00000] axis: [0.00000,0.00000,1.00000] angle: 3.15937 (181.01838)\n", | |
"029: pos: [0.00000,-1.26583,0.00000] axis: [0.00000,0.00000,1.00000] angle: 3.32565 (190.54567)\n", | |
"030: pos: [0.00000,-1.35022,0.00000] axis: [0.00000,0.00000,1.00000] angle: 3.49193 (200.07295)\n", | |
"031: pos: [0.00000,-1.43733,0.00000] axis: [0.00000,0.00000,1.00000] angle: 3.65821 (209.60023)\n", | |
"032: pos: [0.00000,-1.52717,0.00000] axis: [0.00000,0.00000,1.00000] angle: 3.82450 (219.12752)\n", | |
"033: pos: [0.00000,-1.61972,0.00000] axis: [0.00000,0.00000,1.00000] angle: 3.99078 (228.65480)\n", | |
"034: pos: [0.00000,-1.71500,0.00000] axis: [0.00000,0.00000,1.00000] angle: 4.15706 (238.18208)\n", | |
"035: pos: [0.00000,-1.81300,0.00000] axis: [0.00000,0.00000,1.00000] angle: 4.32334 (247.70937)\n", | |
"036: pos: [0.00000,-1.91372,0.00000] axis: [0.00000,0.00000,1.00000] angle: 4.48963 (257.23665)\n", | |
"037: pos: [0.00000,-2.01717,0.00000] axis: [0.00000,0.00000,1.00000] angle: 4.65591 (266.76393)\n", | |
"038: pos: [0.00000,-2.12333,0.00000] axis: [0.00000,0.00000,1.00000] angle: 4.82219 (276.29122)\n", | |
"039: pos: [0.00000,-2.23222,0.00000] axis: [0.00000,0.00000,1.00000] angle: 4.98847 (285.81850)\n", | |
"040: pos: [0.00000,-2.34383,0.00000] axis: [0.00000,0.00000,1.00000] angle: 5.15476 (295.34578)\n", | |
"041: pos: [0.00000,-2.45817,0.00000] axis: [0.00000,0.00000,1.00000] angle: 5.32104 (304.87307)\n", | |
"042: pos: [0.00000,-2.57522,0.00000] axis: [0.00000,0.00000,1.00000] angle: 5.48732 (314.40035)\n", | |
"043: pos: [0.00000,-2.69500,0.00000] axis: [0.00000,0.00000,1.00000] angle: 5.65360 (323.92763)\n", | |
"044: pos: [0.00000,-2.81750,0.00000] axis: [0.00000,0.00000,1.00000] angle: 5.81989 (333.45492)\n", | |
"045: pos: [0.00000,-2.94272,0.00000] axis: [0.00000,0.00000,1.00000] angle: 5.98617 (342.98220)\n", | |
"046: pos: [0.00000,-3.07067,0.00000] axis: [0.00000,0.00000,1.00000] angle: 6.15245 (352.50949)\n", | |
"047: pos: [0.00000,-3.20133,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 6.24764 (357.96323)\n", | |
"048: pos: [0.00000,-3.33472,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 6.08135 (348.43595)\n", | |
"049: pos: [0.00000,-3.47083,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 5.91507 (338.90866)\n", | |
"050: pos: [0.00000,-3.60967,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 5.74879 (329.38138)\n", | |
"051: pos: [0.00000,-3.75122,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 5.58251 (319.85410)\n", | |
"052: pos: [0.00000,-3.89550,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 5.41622 (310.32681)\n", | |
"053: pos: [0.00000,-4.04250,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 5.24994 (300.79953)\n", | |
"054: pos: [0.00000,-4.19222,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 5.08366 (291.27225)\n", | |
"055: pos: [0.00000,-4.34467,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 4.91738 (281.74496)\n", | |
"056: pos: [0.00000,-4.49983,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 4.75109 (272.21768)\n", | |
"057: pos: [0.00000,-4.65772,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 4.58481 (262.69040)\n", | |
"058: pos: [0.00000,-4.81833,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 4.41853 (253.16311)\n", | |
"059: pos: [0.00000,-4.98167,0.00000] axis: [0.00000,0.00000,-1.00000] angle: 4.25225 (243.63583)\n" | |
] | |
} | |
], | |
"source": [ | |
"state = State()\n", | |
"body = RigidBody()\n", | |
"\n", | |
"body.mass, body.inertia = calc_cuboid_mass_and_inertia(1.0, np.array([0.5, 0.5, 0.5]))\n", | |
"#body.mass, body.inertia = calc_sphere_mass_and_inertia(1.0, 1.0)\n", | |
"\n", | |
"print('inertia:')\n", | |
"print(body.inertia)\n", | |
"\n", | |
"# メインループ\n", | |
"time_step = 1.0 / 60.0\n", | |
"for frame in range(60):\n", | |
" # Apply gravity\n", | |
" force = np.array([0.0, -9.8, 0.0]) * body.mass\n", | |
" apply_force(state, body, force, time_step)\n", | |
"\n", | |
" # Apply impulse\n", | |
" if frame == 10:\n", | |
" alpha = np.array([0.0, 0.0, 10.0])\n", | |
" orientation = quaternion.as_rotation_matrix(state.orientation)\n", | |
" world_inertia = orientation @ body.inertia @ orientation.T\n", | |
" torque = world_inertia @ alpha\n", | |
" apply_angular_impulse(state, body, torque)\n", | |
"\n", | |
" # Integrate\n", | |
" integrate(state, time_step)\n", | |
"\n", | |
" print('{:03}: '.format(frame), state)\n" | |
] | |
} | |
], | |
"metadata": { | |
"kernelspec": { | |
"display_name": "Python 3", | |
"language": "python", | |
"name": "python3" | |
}, | |
"language_info": { | |
"codemirror_mode": { | |
"name": "ipython", | |
"version": 3 | |
}, | |
"file_extension": ".py", | |
"mimetype": "text/x-python", | |
"name": "python", | |
"nbconvert_exporter": "python", | |
"pygments_lexer": "ipython3", | |
"version": "3.8.3" | |
} | |
}, | |
"nbformat": 4, | |
"nbformat_minor": 5 | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment