Skip to content

Commit 14087b3

Browse files
lvjonoklvjonok
authored
Feature/match dynamics (#6)
* feat: add tests for body_regressor and joint_body_regressor * dump * dump * dump * feat: add test with separate mujoco and pin * feat: matched joint body regressor * fix: temporary comment * fix: update ci * fix: update ci * fix: update pyproject toml * feat: add joint torque regressor * fix: prettify jacobian use * dump * feat: rename and started rne post constraint * feat: rename for consistency
1 parent 430b83d commit 14087b3

File tree

16 files changed

+2305
-93
lines changed

16 files changed

+2305
-93
lines changed

‎.github/workflows/build.yaml‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,8 @@ jobs:
2121
pip install ruff==0.1.11
2222
# Update output format to enable automatic inline annotations.
2323
- name: Run Ruff
24-
run: ruff check --output-format=github .
24+
# run: ruff check --output-format=github .
25+
run: ruff format --diff .
2526

2627
tests:
2728
runs-on: ubuntu-latest

‎data/ltv_lqr_traj.json‎

Lines changed: 1 addition & 0 deletions
Large diffs are not rendered by default.

‎examples/dynamics_a1.ipynb‎

Lines changed: 212 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,212 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "code",
5+
"execution_count": 1,
6+
"metadata": {},
7+
"outputs": [],
8+
"source": [
9+
"import mujoco\n",
10+
"import numpy as np\n",
11+
"import pinocchio as pin\n",
12+
"from robot_descriptions.h1_description import URDF_PATH\n",
13+
"from robot_descriptions.h1_mj_description import MJCF_PATH\n",
14+
"\n",
15+
"from mujoco_sysid import parameters\n",
16+
"from mujoco_sysid.utils import muj2pin\n",
17+
"\n",
18+
"mjmodel = mujoco.MjModel.from_xml_path(MJCF_PATH)\n",
19+
"mjdata = mujoco.MjData(mjmodel)\n",
20+
"\n",
21+
"pinmodel = pin.buildModelFromUrdf(URDF_PATH, pin.JointModelFreeFlyer())"
22+
]
23+
},
24+
{
25+
"cell_type": "markdown",
26+
"metadata": {},
27+
"source": [
28+
"## Setup the model to have the same dynamic parameters"
29+
]
30+
},
31+
{
32+
"cell_type": "code",
33+
"execution_count": 2,
34+
"metadata": {},
35+
"outputs": [
36+
{
37+
"name": "stdout",
38+
"output_type": "stream",
39+
"text": [
40+
"[ 5.39 -0.00108 0.00022 -0.24374 0.0556 0.00009 0.01927 -0.00007 0.00001 0.04902]\n",
41+
"[ 5.39 -0.00108 0.00022 -0.24374 0.0556 0.00009 0.01927 -0.00007 0.00001 0.04902]\n",
42+
"[ 2.244 -0.11047 0.00022 0.01616 0.00269 0.00002 0.0086 0.00028 0. 0.00773]\n",
43+
"[ 2.244 -0.11047 0.00022 0.01616 0.00269 0.00002 0.0086 0.00028 0. 0.00773]\n",
44+
"[ 2.232 -0.01295 -0.00712 -0.0002 0.00208 -0.00001 0.00232 0. -0.00001 0.00253]\n",
45+
"[ 2.232 -0.01295 -0.00712 -0.0002 0.00208 -0.00001 0.00232 0. -0.00001 0.00253]\n",
46+
"[ 4.152 0.03097 -0.09741 -0.34017 0.11277 0.00006 0.10968 0.00661 -0.00078 0.00852]\n",
47+
"[ 4.152 0.03097 -0.09741 -0.34017 0.11277 0.00006 0.10968 0.00661 -0.00078 0.00852]\n",
48+
"[ 1.721 -0.00234 -0.00881 -0.23819 0.04522 -0.00008 0.04548 0.00076 -0.00099 0.00211]\n",
49+
"[ 1.721 -0.00234 -0.00881 -0.23819 0.04522 -0.00008 0.04548 0.00076 -0.00099 0.00211]\n",
50+
"[ 0.446 0.02998 0.00007 -0.02006 0.00116 -0. 0.00513 0.00163 0. 0.00416]\n",
51+
"[ 0.474 0.02018 -0. -0.02117 0.00111 0. 0.00471 0.00104 -0. 0.00366]\n",
52+
"changed to [ 0.474 0.02018 -0. -0.02117 0.00111 0. 0.00471 0.00104 -0. 0.00366]\n",
53+
"[ 2.244 -0.11047 -0.00022 0.01616 0.00269 -0.00002 0.0086 0.00028 -0. 0.00773]\n",
54+
"[ 2.244 -0.11047 -0.00022 0.01616 0.00269 -0.00002 0.0086 0.00028 -0. 0.00773]\n",
55+
"[ 2.232 -0.01295 0.00712 -0.0002 0.00208 0.00001 0.00232 0. 0.00001 0.00253]\n",
56+
"[ 2.232 -0.01295 0.00712 -0.0002 0.00208 0.00001 0.00232 0. 0.00001 0.00253]\n",
57+
"[ 4.152 0.03097 0.09741 -0.34017 0.11277 -0.00006 0.10968 0.00661 0.00078 0.00852]\n",
58+
"[ 4.152 0.03097 0.09741 -0.34017 0.11277 -0.00006 0.10968 0.00661 0.00078 0.00852]\n",
59+
"[ 1.721 -0.00234 0.00881 -0.23819 0.04522 0.00008 0.04548 0.00076 0.00099 0.00211]\n",
60+
"[ 1.721 -0.00234 0.00881 -0.23819 0.04522 0.00008 0.04548 0.00076 0.00099 0.00211]\n",
61+
"[ 0.446 0.02998 -0.00007 -0.02006 0.00116 0. 0.00513 0.00163 -0. 0.00416]\n",
62+
"[ 0.474 0.02018 0. -0.02117 0.00111 -0. 0.00471 0.00104 0. 0.00366]\n",
63+
"changed to [ 0.474 0.02018 0. -0.02117 0.00111 -0. 0.00471 0.00104 0. 0.00366]\n",
64+
"[17.789 0.0087 0.04976 3.6439 1.23386 -0.00056 1.15605 0.00025 -0.01094 0.12799]\n",
65+
"[17.789 0.0087 0.04976 3.6439 1.23386 -0.00056 1.15605 0.00025 -0.01094 0.12799]\n",
66+
"[ 1.033 0.00521 0.05543 -0.01623 0.00453 -0.0003 0.00115 0.00009 0.00091 0.00397]\n",
67+
"[ 1.033 0.00521 0.05543 -0.01623 0.00453 -0.0003 0.00115 0.00009 0.00091 0.00397]\n",
68+
"[ 0.793 0.00054 0.00091 -0.0746 0.00859 0. 0.00872 -0.00002 0.00002 0.00102]\n",
69+
"[ 0.793 0.00054 0.00091 -0.0746 0.00859 0. 0.00872 -0.00002 0.00002 0.00102]\n",
70+
"[ 0.839 0.01145 0.00232 -0.13647 0.02587 -0.00004 0.02643 0.00221 0.00045 0.00083]\n",
71+
"[ 0.839 0.01145 0.00232 -0.13647 0.02587 -0.00004 0.02643 0.00221 0.00045 0.00083]\n",
72+
"[ 0.669 0.10642 -0.0001 -0.01055 0.00059 -0.00002 0.0231 0.00197 0. 0.02293]\n",
73+
"[ 0.723 0.1192 0.00009 -0.01138 0.0006 -0.00005 0.02584 0.00217 0.00001 0.02565]\n",
74+
"changed to [ 0.723 0.1192 0.00009 -0.01138 0.0006 -0.00005 0.02584 0.00217 0.00001 0.02565]\n",
75+
"[ 1.033 0.00521 -0.05543 -0.01623 0.00453 0.0003 0.00115 0.00009 -0.00091 0.00397]\n",
76+
"[ 1.033 0.00521 -0.05543 -0.01623 0.00453 0.0003 0.00115 0.00009 -0.00091 0.00397]\n",
77+
"[ 0.793 0.00054 -0.00091 -0.0746 0.00859 -0. 0.00872 -0.00002 -0.00002 0.00102]\n",
78+
"[ 0.793 0.00054 -0.00091 -0.0746 0.00859 -0. 0.00872 -0.00002 -0.00002 0.00102]\n",
79+
"[ 0.839 0.01145 -0.00232 -0.13647 0.02587 0.00004 0.02643 0.00221 -0.00045 0.00083]\n",
80+
"[ 0.839 0.01145 -0.00232 -0.13647 0.02587 0.00004 0.02643 0.00221 -0.00045 0.00083]\n",
81+
"[ 0.669 0.10642 0.0001 -0.01055 0.00059 0.00002 0.0231 0.00197 -0. 0.02293]\n",
82+
"[ 0.723 0.1192 -0.00009 -0.01138 0.0006 0.00005 0.02584 0.00217 -0.00001 0.02565]\n",
83+
"changed to [ 0.723 0.1192 -0.00009 -0.01138 0.0006 0.00005 0.02584 0.00217 -0.00001 0.02565]\n"
84+
]
85+
}
86+
],
87+
"source": [
88+
"with np.printoptions(precision=5, suppress=True, linewidth=400):\n",
89+
" for body_id in mjmodel.jnt_bodyid:\n",
90+
" mjparams = parameters.get_dynamic_parameters(mjmodel, body_id)\n",
91+
" pinparams = pinmodel.inertias[int(body_id)].toDynamicParameters()\n",
92+
"\n",
93+
" print(mjparams)\n",
94+
" print(pinparams)\n",
95+
" if not np.allclose(mjparams, pinparams, atol=1e-6):\n",
96+
" parameters.set_dynamic_parameters(mjmodel, body_id, pinparams)\n",
97+
"\n",
98+
" mjparams = parameters.get_dynamic_parameters(mjmodel, body_id)\n",
99+
" print(\"changed to\", mjparams)"
100+
]
101+
},
102+
{
103+
"cell_type": "code",
104+
"execution_count": 3,
105+
"metadata": {},
106+
"outputs": [],
107+
"source": [
108+
"q = np.random.randn(mjmodel.nq)\n",
109+
"v = np.random.randn(mjmodel.nv)\n",
110+
"dv = np.random.randn(mjmodel.nv)\n",
111+
"tau = np.random.randn(mjmodel.nu)\n",
112+
"\n",
113+
"mjdata.qpos[:] = q\n",
114+
"mjdata.qvel[:] = v\n",
115+
"mjdata.qacc[:] = dv\n",
116+
"mjdata.ctrl[:] = tau\n",
117+
"\n",
118+
"mujoco.mj_step(mjmodel, mjdata)\n",
119+
"mujoco.mj_inverse(mjmodel, mjdata)"
120+
]
121+
},
122+
{
123+
"cell_type": "code",
124+
"execution_count": 4,
125+
"metadata": {},
126+
"outputs": [],
127+
"source": [
128+
"# setup same data in pinocchio\n",
129+
"pinq, pinv = muj2pin(mjdata.qpos, mjdata.qvel)"
130+
]
131+
},
132+
{
133+
"cell_type": "code",
134+
"execution_count": 5,
135+
"metadata": {},
136+
"outputs": [
137+
{
138+
"name": "stdout",
139+
"output_type": "stream",
140+
"text": [
141+
"[[ -26.04972 -2.26343 951.46826 -99.99949 0. 0. 0. 0. 0. 0. ]\n",
142+
" [ 5.94784 -950.59184 -3.00354 68.78174 0. 0. 0. 0. 0. 0. ]\n",
143+
" [ 28.77278 102.78788 -67.48682 -1.14711 0. 0. 0. 0. 0. 0. ]\n",
144+
" [ 0. 0. 28.77278 -5.94784 -68.13428 -102.78788 -0.64746 -950.59184 -1.85643 0.64746]\n",
145+
" [ 0. -28.77278 0. -26.04972 1.39419 -67.48682 -101.39368 1.11632 -951.46826 -1.39419]\n",
146+
" [ 0. 5.94784 26.04972 0. -0.43821 0.74011 0.43821 -68.78174 -99.99949 -951.03005]]\n"
147+
]
148+
}
149+
],
150+
"source": [
151+
"from mujoco_sysid.regressors import joint_body_regressor\n",
152+
"\n",
153+
"with np.printoptions(precision=5, suppress=True, linewidth=400):\n",
154+
" print(joint_body_regressor(mjmodel, mjdata, 2))"
155+
]
156+
},
157+
{
158+
"cell_type": "code",
159+
"execution_count": 6,
160+
"metadata": {},
161+
"outputs": [
162+
{
163+
"name": "stdout",
164+
"output_type": "stream",
165+
"text": [
166+
"[[ -27.22494 -2.26343 951.46826 -99.99949 0. 0. 0. 0. 0. 0. ]\n",
167+
" [ 19.5201 -950.59184 -3.00354 68.78174 0. 0. 0. 0. 0. 0. ]\n",
168+
" [ 28.80256 102.78788 -67.48682 -1.14711 0. 0. 0. 0. 0. 0. ]\n",
169+
" [ 0. 0. 28.80256 -19.5201 -68.13428 -102.78788 -0.64746 -950.59184 -1.85643 0.64746]\n",
170+
" [ 0. -28.80256 0. -27.22494 1.39419 -67.48682 -101.39368 1.11632 -951.46826 -1.39419]\n",
171+
" [ 0. 19.5201 27.22494 0. -0.43821 0.74011 0.43821 -68.78174 -99.99949 -951.03005]]\n"
172+
]
173+
}
174+
],
175+
"source": [
176+
"pindata = pin.Data(pinmodel)\n",
177+
"tau = pin.rnea(pinmodel, pindata, pinq, pinv, mjdata.qacc.copy())\n",
178+
"\n",
179+
"with np.printoptions(precision=5, suppress=True, linewidth=400):\n",
180+
" print(pin.jointBodyRegressor(pinmodel, pindata, 2))"
181+
]
182+
},
183+
{
184+
"cell_type": "code",
185+
"execution_count": null,
186+
"metadata": {},
187+
"outputs": [],
188+
"source": []
189+
}
190+
],
191+
"metadata": {
192+
"kernelspec": {
193+
"display_name": "venv",
194+
"language": "python",
195+
"name": "python3"
196+
},
197+
"language_info": {
198+
"codemirror_mode": {
199+
"name": "ipython",
200+
"version": 3
201+
},
202+
"file_extension": ".py",
203+
"mimetype": "text/x-python",
204+
"name": "python",
205+
"nbconvert_exporter": "python",
206+
"pygments_lexer": "ipython3",
207+
"version": "3.10.12"
208+
}
209+
},
210+
"nbformat": 4,
211+
"nbformat_minor": 2
212+
}

0 commit comments

Comments
 (0)