Skip to content

Instantly share code, notes, and snippets.

@Hasenpfote

Hasenpfote/rigid-body-dynamics-2.ipynb Secret

Created June 9, 2021 10:21
Show Gist options
  • Save Hasenpfote/034cc499827da562fed3c8f676b868f0 to your computer and use it in GitHub Desktop.
Save Hasenpfote/034cc499827da562fed3c8f676b868f0 to your computer and use it in GitHub Desktop.
Download ZIP
Rigid-body dynamics
Display the source blob
Display the rendered blob
Raw
rigid-body-dynamics-2.ipynb
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"id": "69412475-9a86-4552-8811-3c0d115242bf",
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"# https://pypi.org/project/numpy-quaternion/\n",
"import quaternion"
]
},
{
"cell_type": "markdown",
"id": "9f2de810-4b68-4e1b-ad37-27aba698e05e",
"metadata": {},
"source": [
"## 慣性テンソル"
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "72c5e36a-8ba1-432a-8ae0-aaa9e68a9e38",
"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": "74241e8c-2be4-4971-836f-70426af90718",
"metadata": {},
"source": [
"## 剛体のパラメータ"
]
},
{
"cell_type": "code",
"execution_count": 3,
"id": "84e45f92-3147-4db0-961d-6eb2a4d80256",
"metadata": {},
"outputs": [],
"source": [
"# 剛体の状態\n",
"class State:\n",
" def __init__(self):\n",
" # 位置\n",
" self.position = np.zeros(3)\n",
" # 姿勢\n",
" self.orientation = np.quaternion(1.0, 0.0, 0.0, 0.0)\n",
" # 並進速度\n",
" self.linear_velocity = np.zeros(3)\n",
" # 回転速度\n",
" self.angular_velocity = np.zeros(3)\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([1.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_tensor = np.zeros(3)\n",
" # 慣性テンソルの回転\n",
" self.inertia_tensor_rotation = np.quaternion(1.0, 0.0, 0.0, 0.0)\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: {} Ir: {} m: {:.5f} e: {:.5f} f: {:.5f}'.format(\n",
" self.inertia_tensor, self.inertia_tensor_rotation, self.mass, self.restitution, self.friction)\n"
]
},
{
"cell_type": "markdown",
"id": "802b90c4-eb02-4a76-b3f8-1fafd912d566",
"metadata": {},
"source": [
"## 力の適用"
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "c8fe18c4-7789-4b30-9f07-2206a79ac148",
"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",
" q = state.orientation * body.inertia_tensor_rotation\n",
" inv_q = q.conj()\n",
"\n",
" angular_momentum = body.inertia_tensor * quaternion.rotate_vectors(inv_q, state.angular_velocity)\n",
" angular_momentum = quaternion.rotate_vectors(q, angular_momentum)\n",
" angular_momentum += torque * time_step\n",
"\n",
" angular_velocity = np.reciprocal(body.inertia_tensor) * quaternion.rotate_vectors(inv_q, angular_momentum)\n",
" state.angular_velocity = quaternion.rotate_vectors(q, angular_velocity)\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": "e05784b5-84bc-4b90-846b-2ff2e51460d4",
"metadata": {},
"source": [
"## 力積の適用"
]
},
{
"cell_type": "code",
"execution_count": 5,
"id": "883281f6-24da-46fe-8f65-f239031b3aa0",
"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",
" q = state.orientation * body.inertia_tensor_rotation\n",
" inv_q = q.conj()\n",
"\n",
" angular_momentum = body.inertia_tensor * quaternion.rotate_vectors(inv_q, state.angular_velocity)\n",
" angular_momentum = quaternion.rotate_vectors(q, angular_momentum)\n",
" angular_momentum += impulse\n",
"\n",
" angular_velocity = np.reciprocal(body.inertia_tensor) * quaternion.rotate_vectors(inv_q, angular_momentum)\n",
" state.angular_velocity = quaternion.rotate_vectors(q, angular_velocity)\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": "8988405c-746d-4685-ab47-015dbc196b05",
"metadata": {},
"source": [
"## 更新"
]
},
{
"cell_type": "code",
"execution_count": 6,
"id": "ff56beb6-7668-498b-84c9-3b792c12ba27",
"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": "e319f52f-5560-4370-9b0e-12e8c79a2374",
"metadata": {},
"source": [
"## シミュレーション"
]
},
{
"cell_type": "code",
"execution_count": 7,
"id": "5788a919-f5f5-4987-9e61-c907647cb2d4",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"inertia_tensor: [0.16666667 0.16666667 0.16666667]\n",
"inertia_tensor_rotation: quaternion(1, -0, -0, -0)\n",
"000: pos: [0.00000,-0.00272,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"001: pos: [0.00000,-0.00817,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"002: pos: [0.00000,-0.01633,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"003: pos: [0.00000,-0.02722,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"004: pos: [0.00000,-0.04083,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"005: pos: [0.00000,-0.05717,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"006: pos: [0.00000,-0.07622,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"007: pos: [0.00000,-0.09800,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"008: pos: [0.00000,-0.12250,0.00000] axis: [1.00000,0.00000,0.00000] angle: 0.00000 (0.00000)\n",
"009: pos: [0.00000,-0.14972,0.00000] axis: [1.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, inertia_tensor = calc_cuboid_mass_and_inertia(1.0, np.array([0.5, 0.5, 0.5]))\n",
"#body.mass, inertia_tensor = calc_sphere_mass_and_inertia(1.0, 1.0)\n",
"\n",
"# 主慣性モーメントと慣性主軸\n",
"values, vectors = np.linalg.eig(inertia_tensor)\n",
"body.inertia_tensor = values\n",
"body.inertia_tensor_rotation = quaternion.from_rotation_matrix(vectors)\n",
"\n",
"print('inertia_tensor: ', body.inertia_tensor)\n",
"print('inertia_tensor_rotation: ', body.inertia_tensor_rotation)\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",
" q = state.orientation * body.inertia_tensor_rotation\n",
" inv_q = q.conj()\n",
" torque = body.inertia_tensor * quaternion.rotate_vectors(inv_q, alpha)\n",
" torque = quaternion.rotate_vectors(q, torque)\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
}
Display the source blob
Display the rendered blob
Raw
rigid-body-dynamics.ipynb
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment