jaxsim 0.4.2__tar.gz → 0.4.2.dev12__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/PKG-INFO +1 -13
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/README.md +0 -12
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/examples/PD_controller.ipynb +50 -13
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/pyproject.toml +1 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/_version.py +2 -2
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/com.py +12 -12
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/contact.py +0 -168
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/data.py +11 -2
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/frame.py +2 -2
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/link.py +7 -10
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/model.py +22 -66
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/references.py +1 -107
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/integrators/common.py +2 -4
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/adjoint.py +4 -3
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/joint_model.py +2 -2
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/quaternion.py +4 -2
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/transform.py +3 -1
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/mujoco/loaders.py +2 -12
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/mujoco/model.py +5 -5
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/aba.py +2 -2
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/collidable_points.py +2 -2
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/contacts/soft.py +1 -1
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/crba.py +12 -24
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/forward_kinematics.py +6 -10
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/jacobian.py +12 -24
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/rnea.py +2 -2
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim.egg-info/PKG-INFO +1 -13
- jaxsim-0.4.2.dev12/tests/test_api_contact.py +58 -0
- jaxsim-0.4.2/tests/test_api_contact.py +0 -147
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.devcontainer/Dockerfile +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.devcontainer/devcontainer.json +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.gitattributes +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.github/CODEOWNERS +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.github/workflows/ci_cd.yml +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.github/workflows/read_the_docs.yml +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.gitignore +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.pre-commit-config.yaml +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/.readthedocs.yaml +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/CONTRIBUTING.md +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/LICENSE +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/Makefile +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/conf.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/examples.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/guide/install.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/index.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/make.bat +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/modules/api.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/modules/integrators.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/modules/math.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/modules/mujoco.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/modules/parsers.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/modules/rbda.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/modules/typing.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/docs/modules/utils.rst +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/environment.yml +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/examples/.gitattributes +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/examples/.gitignore +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/examples/Parallel_computing.ipynb +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/examples/README.md +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/examples/assets/cartpole.urdf +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/pixi.lock +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/setup.cfg +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/setup.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/common.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/joint.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/kin_dyn_parameters.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/ode.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/api/ode_data.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/exceptions.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/integrators/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/integrators/fixed_step.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/integrators/variable_step.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/logging.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/cross.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/inertia.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/rotation.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/math/skew.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/mujoco/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/mujoco/__main__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/mujoco/visualizer.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/descriptions/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/descriptions/collision.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/descriptions/joint.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/descriptions/link.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/descriptions/model.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/kinematic_graph.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/rod/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/rod/parser.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/parsers/rod/utils.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/contacts/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/contacts/common.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/rbda/utils.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/terrain/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/terrain/terrain.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/typing.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/utils/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/utils/jaxsim_dataclass.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/utils/tracing.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim/utils/wrappers.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim.egg-info/SOURCES.txt +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim.egg-info/dependency_links.txt +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim.egg-info/requires.txt +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/src/jaxsim.egg-info/top_level.txt +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/__init__.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/conftest.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_api_com.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_api_data.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_api_frame.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_api_joint.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_api_link.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_api_model.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_automatic_differentiation.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_contact.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_exceptions.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_pytree.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/test_simulations.py +0 -0
- {jaxsim-0.4.2 → jaxsim-0.4.2.dev12}/tests/utils_idyntree.py +0 -0
@@ -1,6 +1,6 @@
|
|
1
1
|
Metadata-Version: 2.1
|
2
2
|
Name: jaxsim
|
3
|
-
Version: 0.4.2
|
3
|
+
Version: 0.4.2.dev12
|
4
4
|
Summary: A differentiable physics engine and multibody dynamics library for control and robot learning.
|
5
5
|
Author-email: Diego Ferigo <dgferigo@gmail.com>
|
6
6
|
Maintainer-email: Diego Ferigo <dgferigo@gmail.com>, Filippo Luca Ferretti <filippo.ferretti@iit.it>
|
@@ -90,18 +90,6 @@ JaxSim is a **differentiable physics engine** and **multibody dynamics library**
|
|
90
90
|
|
91
91
|
Its design facilitates research and accelerates prototyping in the intersection of robotics and artificial intelligence.
|
92
92
|
|
93
|
-
<div align="center">
|
94
|
-
<br/>
|
95
|
-
<table>
|
96
|
-
<tr>
|
97
|
-
<th><img src="https://github.com/user-attachments/assets/115b1c1c-6ae5-4c59-92e0-1be13ba954db" width="250"></th>
|
98
|
-
<th><img src="https://github.com/user-attachments/assets/f9661fae-9a85-41dd-9a58-218758ec8c9c" width="250"></th>
|
99
|
-
<th><img src="https://github.com/user-attachments/assets/ae8adadf-3bca-47b8-97ca-3a9273633d60" width="250"></th>
|
100
|
-
</tr>
|
101
|
-
</table>
|
102
|
-
<br/>
|
103
|
-
</div>
|
104
|
-
|
105
93
|
## Features
|
106
94
|
|
107
95
|
- Physics engine in reduced coordinates supporting fixed-base and floating-base robots.
|
@@ -4,18 +4,6 @@ JaxSim is a **differentiable physics engine** and **multibody dynamics library**
|
|
4
4
|
|
5
5
|
Its design facilitates research and accelerates prototyping in the intersection of robotics and artificial intelligence.
|
6
6
|
|
7
|
-
<div align="center">
|
8
|
-
<br/>
|
9
|
-
<table>
|
10
|
-
<tr>
|
11
|
-
<th><img src="https://github.com/user-attachments/assets/115b1c1c-6ae5-4c59-92e0-1be13ba954db" width="250"></th>
|
12
|
-
<th><img src="https://github.com/user-attachments/assets/f9661fae-9a85-41dd-9a58-218758ec8c9c" width="250"></th>
|
13
|
-
<th><img src="https://github.com/user-attachments/assets/ae8adadf-3bca-47b8-97ca-3a9273633d60" width="250"></th>
|
14
|
-
</tr>
|
15
|
-
</table>
|
16
|
-
<br/>
|
17
|
-
</div>
|
18
|
-
|
19
7
|
## Features
|
20
8
|
|
21
9
|
- Physics engine in reduced coordinates supporting fixed-base and floating-base robots.
|
@@ -37,9 +37,16 @@
|
|
37
37
|
"\n",
|
38
38
|
" # Install dependencies for visualization on Colab and ReadTheDocs\n",
|
39
39
|
" !sudo apt update\n",
|
40
|
-
" !apt install
|
40
|
+
" !apt install xvfb ffmpeg python-opengl\n",
|
41
|
+
" !{sys.executable} -m pip install pyvirtualdisplay\n",
|
41
42
|
" clear_output()\n",
|
42
43
|
"\n",
|
44
|
+
" # Start Xvfb and set \"DISPLAY\" environment variable\n",
|
45
|
+
" import pyvirtualdisplay\n",
|
46
|
+
"\n",
|
47
|
+
" d = pyvirtualdisplay.Display()\n",
|
48
|
+
" d.start()\n",
|
49
|
+
"\n",
|
43
50
|
"\n",
|
44
51
|
"import jax\n",
|
45
52
|
"import jax.numpy as jnp\n",
|
@@ -147,21 +154,51 @@
|
|
147
154
|
"outputs": [],
|
148
155
|
"source": [
|
149
156
|
"# @title Set up MuJoCo renderer\n",
|
157
|
+
"import subprocess\n",
|
158
|
+
"\n",
|
150
159
|
"os.environ[\"MUJOCO_GL\"] = \"osmesa\"\n",
|
151
160
|
"\n",
|
161
|
+
"if IS_COLAB:\n",
|
162
|
+
" if subprocess.run(\"nvidia-smi\").returncode:\n",
|
163
|
+
" raise RuntimeError(\n",
|
164
|
+
" \"Cannot communicate with GPU. \"\n",
|
165
|
+
" \"Make sure you are using a GPU Colab runtime. \"\n",
|
166
|
+
" \"Go to the Runtime menu and select Choose runtime type.\"\n",
|
167
|
+
" )\n",
|
168
|
+
"\n",
|
169
|
+
" # Add an ICD config so that glvnd can pick up the Nvidia EGL driver.\n",
|
170
|
+
" # This is usually installed as part of an Nvidia driver package, but the Colab\n",
|
171
|
+
" # kernel doesn't install its driver via APT, and as a result the ICD is missing.\n",
|
172
|
+
" # (https://github.com/NVIDIA/libglvnd/blob/master/src/EGL/icd_enumeration.md)\n",
|
173
|
+
" NVIDIA_ICD_CONFIG_PATH = \"/usr/share/glvnd/egl_vendor.d/10_nvidia.json\"\n",
|
174
|
+
" if not os.path.exists(NVIDIA_ICD_CONFIG_PATH):\n",
|
175
|
+
" with open(NVIDIA_ICD_CONFIG_PATH, \"w\") as f:\n",
|
176
|
+
" f.write(\n",
|
177
|
+
" \"\"\"{\n",
|
178
|
+
" \"file_format_version\" : \"1.0.0\",\n",
|
179
|
+
" \"ICD\" : {\n",
|
180
|
+
" \"library_path\" : \"libEGL_nvidia.so.0\"\n",
|
181
|
+
" }\n",
|
182
|
+
" }\n",
|
183
|
+
" \"\"\"\n",
|
184
|
+
" )\n",
|
185
|
+
"\n",
|
186
|
+
" os.environ[\"MUJOCO_GL\"] = \"egl\"\n",
|
187
|
+
"\n",
|
188
|
+
"\n",
|
152
189
|
"from jaxsim.mujoco import MujocoModelHelper, MujocoVideoRecorder\n",
|
153
|
-
"from jaxsim.mujoco.loaders import UrdfToMjcf
|
154
|
-
"\n",
|
155
|
-
"
|
156
|
-
"
|
157
|
-
"
|
158
|
-
"
|
159
|
-
"
|
160
|
-
"
|
161
|
-
"
|
162
|
-
"
|
163
|
-
"
|
164
|
-
"
|
190
|
+
"from jaxsim.mujoco.loaders import UrdfToMjcf\n",
|
191
|
+
"\n",
|
192
|
+
"camera = {\n",
|
193
|
+
" \"name\": \"cartpole_camera\",\n",
|
194
|
+
" \"mode\": \"fixed\",\n",
|
195
|
+
" \"pos\": \"3.954 3.533 2.343\",\n",
|
196
|
+
" \"xyaxes\": \"-0.594 0.804 -0.000 -0.163 -0.120 0.979\",\n",
|
197
|
+
" \"fovy\": \"60\",\n",
|
198
|
+
"}\n",
|
199
|
+
"\n",
|
200
|
+
"mjcf_string, assets = UrdfToMjcf.convert(urdf=model.built_from, cameras=camera)\n",
|
201
|
+
"\n",
|
165
202
|
"mj_model_helper = MujocoModelHelper.build_from_xml(\n",
|
166
203
|
" mjcf_description=mjcf_string, assets=assets\n",
|
167
204
|
")\n",
|
@@ -156,6 +156,7 @@ ignore = [
|
|
156
156
|
"E501", # Line too long
|
157
157
|
"E731", # Do not assign a `lambda` expression, use a `def`
|
158
158
|
"E741", # Ambiguous variable name
|
159
|
+
"F841", # Local variable is assigned to but never used
|
159
160
|
"I001", # Import block is unsorted or unformatted
|
160
161
|
"RUF003", # Ambigous unicode character in comment
|
161
162
|
]
|
@@ -12,5 +12,5 @@ __version__: str
|
|
12
12
|
__version_tuple__: VERSION_TUPLE
|
13
13
|
version_tuple: VERSION_TUPLE
|
14
14
|
|
15
|
-
__version__ = version = '0.4.2'
|
16
|
-
__version_tuple__ = version_tuple = (0, 4, 2)
|
15
|
+
__version__ = version = '0.4.2.dev12'
|
16
|
+
__version_tuple__ = version_tuple = (0, 4, 2, 'dev12')
|
@@ -137,9 +137,9 @@ def centroidal_momentum_jacobian(
|
|
137
137
|
|
138
138
|
match data.velocity_representation:
|
139
139
|
case VelRepr.Inertial | VelRepr.Mixed:
|
140
|
-
W_H_G = W_H_GW = jnp.eye(4).at[0:3, 3].set(W_p_CoM)
|
140
|
+
W_H_G = W_H_GW = jnp.eye(4).at[0:3, 3].set(W_p_CoM)
|
141
141
|
case VelRepr.Body:
|
142
|
-
W_H_G = W_H_GB = W_H_B.at[0:3, 3].set(W_p_CoM)
|
142
|
+
W_H_G = W_H_GB = W_H_B.at[0:3, 3].set(W_p_CoM)
|
143
143
|
case _:
|
144
144
|
raise ValueError(data.velocity_representation)
|
145
145
|
|
@@ -172,9 +172,9 @@ def locked_centroidal_spatial_inertia(
|
|
172
172
|
|
173
173
|
match data.velocity_representation:
|
174
174
|
case VelRepr.Inertial | VelRepr.Mixed:
|
175
|
-
W_H_G = W_H_GW = jnp.eye(4).at[0:3, 3].set(W_p_CoM)
|
175
|
+
W_H_G = W_H_GW = jnp.eye(4).at[0:3, 3].set(W_p_CoM)
|
176
176
|
case VelRepr.Body:
|
177
|
-
W_H_G = W_H_GB = W_H_B.at[0:3, 3].set(W_p_CoM)
|
177
|
+
W_H_G = W_H_GB = W_H_B.at[0:3, 3].set(W_p_CoM)
|
178
178
|
case _:
|
179
179
|
raise ValueError(data.velocity_representation)
|
180
180
|
|
@@ -290,14 +290,14 @@ def bias_acceleration(
|
|
290
290
|
|
291
291
|
case VelRepr.Inertial:
|
292
292
|
|
293
|
-
C_v̇_WL = W_v̇_bias_WL = v̇_bias_WL
|
294
|
-
C_v_WC = W_v_WW = jnp.zeros(6)
|
293
|
+
C_v̇_WL = W_v̇_bias_WL = v̇_bias_WL
|
294
|
+
C_v_WC = W_v_WW = jnp.zeros(6)
|
295
295
|
|
296
|
-
L_H_C = L_H_W = jax.vmap(
|
296
|
+
L_H_C = L_H_W = jax.vmap(
|
297
297
|
lambda W_H_L: jaxsim.math.Transform.inverse(W_H_L)
|
298
298
|
)(W_H_L)
|
299
299
|
|
300
|
-
L_v_LC = L_v_LW = jax.vmap(
|
300
|
+
L_v_LC = L_v_LW = jax.vmap(
|
301
301
|
lambda i: -js.link.velocity(
|
302
302
|
model=model, data=data, link_index=i, output_vel_repr=VelRepr.Body
|
303
303
|
)
|
@@ -314,9 +314,9 @@ def bias_acceleration(
|
|
314
314
|
|
315
315
|
case VelRepr.Mixed:
|
316
316
|
|
317
|
-
C_v̇_WL = LW_v̇_bias_WL = v̇_bias_WL
|
317
|
+
C_v̇_WL = LW_v̇_bias_WL = v̇_bias_WL
|
318
318
|
|
319
|
-
C_v_WC = LW_v_W_LW = jax.vmap(
|
319
|
+
C_v_WC = LW_v_W_LW = jax.vmap(
|
320
320
|
lambda i: js.link.velocity(
|
321
321
|
model=model, data=data, link_index=i, output_vel_repr=VelRepr.Mixed
|
322
322
|
)
|
@@ -324,13 +324,13 @@ def bias_acceleration(
|
|
324
324
|
.set(jnp.zeros(3))
|
325
325
|
)(jnp.arange(model.number_of_links()))
|
326
326
|
|
327
|
-
L_H_C = L_H_LW = jax.vmap(
|
327
|
+
L_H_C = L_H_LW = jax.vmap(
|
328
328
|
lambda W_H_L: jaxsim.math.Transform.inverse(
|
329
329
|
W_H_L.at[0:3, 3].set(jnp.zeros(3))
|
330
330
|
)
|
331
331
|
)(W_H_L)
|
332
332
|
|
333
|
-
L_v_LC = L_v_L_LW = jax.vmap(
|
333
|
+
L_v_LC = L_v_L_LW = jax.vmap(
|
334
334
|
lambda i: -js.link.velocity(
|
335
335
|
model=model, data=data, link_index=i, output_vel_repr=VelRepr.Body
|
336
336
|
)
|
@@ -8,7 +8,6 @@ import jax.numpy as jnp
|
|
8
8
|
import jaxsim.api as js
|
9
9
|
import jaxsim.terrain
|
10
10
|
import jaxsim.typing as jtp
|
11
|
-
from jaxsim.math import Adjoint, Cross, Transform
|
12
11
|
from jaxsim.rbda.contacts.soft import SoftContactsParams
|
13
12
|
|
14
13
|
from .common import VelRepr
|
@@ -412,170 +411,3 @@ def jacobian(
|
|
412
411
|
raise ValueError(output_vel_repr)
|
413
412
|
|
414
413
|
return O_J_WC
|
415
|
-
|
416
|
-
|
417
|
-
@functools.partial(jax.jit, static_argnames=["output_vel_repr"])
|
418
|
-
def jacobian_derivative(
|
419
|
-
model: js.model.JaxSimModel,
|
420
|
-
data: js.data.JaxSimModelData,
|
421
|
-
*,
|
422
|
-
output_vel_repr: VelRepr | None = None,
|
423
|
-
) -> jtp.Matrix:
|
424
|
-
r"""
|
425
|
-
Compute the derivative of the free-floating jacobian of the contact points.
|
426
|
-
|
427
|
-
Args:
|
428
|
-
model: The model to consider.
|
429
|
-
data: The data of the considered model.
|
430
|
-
output_vel_repr:
|
431
|
-
The output velocity representation of the free-floating jacobian derivative.
|
432
|
-
|
433
|
-
Returns:
|
434
|
-
The derivative of the :math:`6 \times (6+n)` free-floating jacobian of the contact points.
|
435
|
-
|
436
|
-
Note:
|
437
|
-
The input representation of the free-floating jacobian derivative is the active
|
438
|
-
velocity representation.
|
439
|
-
"""
|
440
|
-
|
441
|
-
output_vel_repr = (
|
442
|
-
output_vel_repr if output_vel_repr is not None else data.velocity_representation
|
443
|
-
)
|
444
|
-
|
445
|
-
# Get the index of the parent link and the position of the collidable point.
|
446
|
-
parent_link_idxs = jnp.array(model.kin_dyn_parameters.contact_parameters.body)
|
447
|
-
L_p_Ci = jnp.array(model.kin_dyn_parameters.contact_parameters.point)
|
448
|
-
contact_idxs = jnp.arange(L_p_Ci.shape[0])
|
449
|
-
|
450
|
-
# Get the transforms of all the parent links.
|
451
|
-
W_H_Li = js.model.forward_kinematics(model=model, data=data)
|
452
|
-
|
453
|
-
# =====================================================
|
454
|
-
# Compute quantities to adjust the input representation
|
455
|
-
# =====================================================
|
456
|
-
|
457
|
-
def compute_T(model: js.model.JaxSimModel, X: jtp.Matrix) -> jtp.Matrix:
|
458
|
-
In = jnp.eye(model.dofs())
|
459
|
-
T = jax.scipy.linalg.block_diag(X, In)
|
460
|
-
return T
|
461
|
-
|
462
|
-
def compute_Ṫ(model: js.model.JaxSimModel, Ẋ: jtp.Matrix) -> jtp.Matrix:
|
463
|
-
On = jnp.zeros(shape=(model.dofs(), model.dofs()))
|
464
|
-
Ṫ = jax.scipy.linalg.block_diag(Ẋ, On)
|
465
|
-
return Ṫ
|
466
|
-
|
467
|
-
# Compute the operator to change the representation of ν, and its
|
468
|
-
# time derivative.
|
469
|
-
match data.velocity_representation:
|
470
|
-
case VelRepr.Inertial:
|
471
|
-
W_H_W = jnp.eye(4)
|
472
|
-
W_X_W = Adjoint.from_transform(transform=W_H_W)
|
473
|
-
W_Ẋ_W = jnp.zeros((6, 6))
|
474
|
-
|
475
|
-
T = compute_T(model=model, X=W_X_W)
|
476
|
-
Ṫ = compute_Ṫ(model=model, Ẋ=W_Ẋ_W)
|
477
|
-
|
478
|
-
case VelRepr.Body:
|
479
|
-
W_H_B = data.base_transform()
|
480
|
-
W_X_B = Adjoint.from_transform(transform=W_H_B)
|
481
|
-
B_v_WB = data.base_velocity()
|
482
|
-
B_vx_WB = Cross.vx(B_v_WB)
|
483
|
-
W_Ẋ_B = W_X_B @ B_vx_WB
|
484
|
-
|
485
|
-
T = compute_T(model=model, X=W_X_B)
|
486
|
-
Ṫ = compute_Ṫ(model=model, Ẋ=W_Ẋ_B)
|
487
|
-
|
488
|
-
case VelRepr.Mixed:
|
489
|
-
W_H_B = data.base_transform()
|
490
|
-
W_H_BW = W_H_B.at[0:3, 0:3].set(jnp.eye(3))
|
491
|
-
W_X_BW = Adjoint.from_transform(transform=W_H_BW)
|
492
|
-
BW_v_WB = data.base_velocity()
|
493
|
-
BW_v_W_BW = BW_v_WB.at[3:6].set(jnp.zeros(3))
|
494
|
-
BW_vx_W_BW = Cross.vx(BW_v_W_BW)
|
495
|
-
W_Ẋ_BW = W_X_BW @ BW_vx_W_BW
|
496
|
-
|
497
|
-
T = compute_T(model=model, X=W_X_BW)
|
498
|
-
Ṫ = compute_Ṫ(model=model, Ẋ=W_Ẋ_BW)
|
499
|
-
|
500
|
-
case _:
|
501
|
-
raise ValueError(data.velocity_representation)
|
502
|
-
|
503
|
-
# =====================================================
|
504
|
-
# Compute quantities to adjust the output representation
|
505
|
-
# =====================================================
|
506
|
-
|
507
|
-
with data.switch_velocity_representation(VelRepr.Inertial):
|
508
|
-
# Compute the Jacobian of the parent link in inertial representation.
|
509
|
-
W_J_WL_W = js.model.generalized_free_floating_jacobian(
|
510
|
-
model=model,
|
511
|
-
data=data,
|
512
|
-
output_vel_repr=VelRepr.Inertial,
|
513
|
-
)
|
514
|
-
# Compute the Jacobian derivative of the parent link in inertial representation.
|
515
|
-
W_J̇_WL_W = js.model.generalized_free_floating_jacobian_derivative(
|
516
|
-
model=model,
|
517
|
-
data=data,
|
518
|
-
output_vel_repr=VelRepr.Inertial,
|
519
|
-
)
|
520
|
-
|
521
|
-
# Get the Jacobian of the collidable points in the mixed representation.
|
522
|
-
with data.switch_velocity_representation(VelRepr.Mixed):
|
523
|
-
CW_J_WC_BW = jacobian(
|
524
|
-
model=model,
|
525
|
-
data=data,
|
526
|
-
output_vel_repr=VelRepr.Mixed,
|
527
|
-
)
|
528
|
-
|
529
|
-
def compute_O_J̇_WC_I(
|
530
|
-
L_p_C: jtp.Vector,
|
531
|
-
contact_idx: jtp.Int,
|
532
|
-
CW_J_WC_BW: jtp.Matrix,
|
533
|
-
W_H_L: jtp.Matrix,
|
534
|
-
) -> jtp.Matrix:
|
535
|
-
|
536
|
-
parent_link_idx = parent_link_idxs[contact_idx]
|
537
|
-
|
538
|
-
match output_vel_repr:
|
539
|
-
case VelRepr.Inertial:
|
540
|
-
O_X_W = W_X_W = Adjoint.from_transform( # noqa: F841
|
541
|
-
transform=jnp.eye(4)
|
542
|
-
)
|
543
|
-
O_Ẋ_W = W_Ẋ_W = jnp.zeros((6, 6)) # noqa: F841
|
544
|
-
|
545
|
-
case VelRepr.Body:
|
546
|
-
L_H_C = Transform.from_rotation_and_translation(translation=L_p_C)
|
547
|
-
W_H_C = W_H_L[parent_link_idx] @ L_H_C
|
548
|
-
O_X_W = C_X_W = Adjoint.from_transform(transform=W_H_C, inverse=True)
|
549
|
-
with data.switch_velocity_representation(VelRepr.Inertial):
|
550
|
-
W_nu = data.generalized_velocity()
|
551
|
-
W_v_WC = W_J_WL_W[parent_link_idx] @ W_nu
|
552
|
-
W_vx_WC = Cross.vx(W_v_WC)
|
553
|
-
O_Ẋ_W = C_Ẋ_W = -C_X_W @ W_vx_WC # noqa: F841
|
554
|
-
|
555
|
-
case VelRepr.Mixed:
|
556
|
-
L_H_C = Transform.from_rotation_and_translation(translation=L_p_C)
|
557
|
-
W_H_C = W_H_L[parent_link_idx] @ L_H_C
|
558
|
-
W_H_CW = W_H_C.at[0:3, 0:3].set(jnp.eye(3))
|
559
|
-
CW_H_W = Transform.inverse(W_H_CW)
|
560
|
-
O_X_W = CW_X_W = Adjoint.from_transform(transform=CW_H_W)
|
561
|
-
with data.switch_velocity_representation(VelRepr.Mixed):
|
562
|
-
CW_v_WC = CW_J_WC_BW @ data.generalized_velocity()
|
563
|
-
W_v_W_CW = jnp.zeros(6).at[0:3].set(CW_v_WC[0:3])
|
564
|
-
W_vx_W_CW = Cross.vx(W_v_W_CW)
|
565
|
-
O_Ẋ_W = CW_Ẋ_W = -CW_X_W @ W_vx_W_CW # noqa: F841
|
566
|
-
|
567
|
-
case _:
|
568
|
-
raise ValueError(output_vel_repr)
|
569
|
-
|
570
|
-
O_J̇_WC_I = jnp.zeros(shape=(6, 6 + model.dofs()))
|
571
|
-
O_J̇_WC_I += O_Ẋ_W @ W_J_WL_W[parent_link_idx] @ T
|
572
|
-
O_J̇_WC_I += O_X_W @ W_J̇_WL_W[parent_link_idx] @ T
|
573
|
-
O_J̇_WC_I += O_X_W @ W_J_WL_W[parent_link_idx] @ Ṫ
|
574
|
-
|
575
|
-
return O_J̇_WC_I
|
576
|
-
|
577
|
-
O_J̇_WC = jax.vmap(compute_O_J̇_WC_I, in_axes=(0, 0, 0, None))(
|
578
|
-
L_p_Ci, contact_idxs, CW_J_WC_BW, W_H_Li
|
579
|
-
)
|
580
|
-
|
581
|
-
return O_J̇_WC
|
@@ -12,6 +12,7 @@ import jaxlie
|
|
12
12
|
import jaxsim.api as js
|
13
13
|
import jaxsim.rbda
|
14
14
|
import jaxsim.typing as jtp
|
15
|
+
from jaxsim.math import Quaternion
|
15
16
|
from jaxsim.rbda.contacts.soft import SoftContacts
|
16
17
|
from jaxsim.utils import Mutability
|
17
18
|
from jaxsim.utils.tracing import not_tracing
|
@@ -190,7 +191,9 @@ class JaxSimModelData(common.ModelDataWithVelocityRepresentation):
|
|
190
191
|
|
191
192
|
W_H_B = jaxlie.SE3.from_rotation_and_translation(
|
192
193
|
translation=base_position,
|
193
|
-
rotation=jaxlie.SO3(
|
194
|
+
rotation=jaxlie.SO3.from_quaternion_xyzw(
|
195
|
+
base_quaternion[jnp.array([1, 2, 3, 0])]
|
196
|
+
),
|
194
197
|
).as_matrix()
|
195
198
|
|
196
199
|
v_WB = JaxSimModelData.other_representation_to_inertial(
|
@@ -377,7 +380,13 @@ class JaxSimModelData(common.ModelDataWithVelocityRepresentation):
|
|
377
380
|
on_false=W_Q_B / jnp.linalg.norm(W_Q_B),
|
378
381
|
)
|
379
382
|
|
380
|
-
return (
|
383
|
+
return (
|
384
|
+
W_Q_B
|
385
|
+
if not dcm
|
386
|
+
else jaxlie.SO3.from_quaternion_xyzw(
|
387
|
+
Quaternion.to_xyzw(wxyz=W_Q_B)
|
388
|
+
).as_matrix()
|
389
|
+
).astype(float)
|
381
390
|
|
382
391
|
@jax.jit
|
383
392
|
def base_transform(self) -> jtp.Matrix:
|
@@ -384,7 +384,7 @@ def jacobian_derivative(
|
|
384
384
|
W_nu = data.generalized_velocity()
|
385
385
|
W_v_WF = W_J_WL_W @ W_nu
|
386
386
|
W_vx_WF = Cross.vx(W_v_WF)
|
387
|
-
O_Ẋ_W = F_Ẋ_W = -F_X_W @ W_vx_WF
|
387
|
+
O_Ẋ_W = F_Ẋ_W = -F_X_W @ W_vx_WF
|
388
388
|
|
389
389
|
case VelRepr.Mixed:
|
390
390
|
W_H_F = transform(model=model, data=data, frame_index=frame_index)
|
@@ -401,7 +401,7 @@ def jacobian_derivative(
|
|
401
401
|
FW_v_WF = FW_J_WF_FW @ data.generalized_velocity()
|
402
402
|
W_v_W_FW = jnp.zeros(6).at[0:3].set(FW_v_WF[0:3])
|
403
403
|
W_vx_W_FW = Cross.vx(W_v_W_FW)
|
404
|
-
O_Ẋ_W = FW_Ẋ_W = -FW_X_W @ W_vx_W_FW
|
404
|
+
O_Ẋ_W = FW_Ẋ_W = -FW_X_W @ W_vx_W_FW
|
405
405
|
|
406
406
|
case _:
|
407
407
|
raise ValueError(output_vel_repr)
|
@@ -288,7 +288,7 @@ def jacobian(
|
|
288
288
|
case VelRepr.Inertial:
|
289
289
|
W_H_B = data.base_transform()
|
290
290
|
B_X_W = Adjoint.from_transform(transform=W_H_B, inverse=True)
|
291
|
-
B_J_WL_I = B_J_WL_W = B_J_WL_B @ jax.scipy.linalg.block_diag(
|
291
|
+
B_J_WL_I = B_J_WL_W = B_J_WL_B @ jax.scipy.linalg.block_diag(
|
292
292
|
B_X_W, jnp.eye(model.dofs())
|
293
293
|
)
|
294
294
|
|
@@ -299,7 +299,7 @@ def jacobian(
|
|
299
299
|
W_R_B = data.base_orientation(dcm=True)
|
300
300
|
BW_H_B = jnp.eye(4).at[0:3, 0:3].set(W_R_B)
|
301
301
|
B_X_BW = Adjoint.from_transform(transform=BW_H_B, inverse=True)
|
302
|
-
B_J_WL_I = B_J_WL_BW = B_J_WL_B @ jax.scipy.linalg.block_diag(
|
302
|
+
B_J_WL_I = B_J_WL_BW = B_J_WL_B @ jax.scipy.linalg.block_diag(
|
303
303
|
B_X_BW, jnp.eye(model.dofs())
|
304
304
|
)
|
305
305
|
|
@@ -313,7 +313,7 @@ def jacobian(
|
|
313
313
|
case VelRepr.Inertial:
|
314
314
|
W_H_B = data.base_transform()
|
315
315
|
W_X_B = Adjoint.from_transform(transform=W_H_B)
|
316
|
-
O_J_WL_I = W_J_WL_I = W_X_B @ B_J_WL_I
|
316
|
+
O_J_WL_I = W_J_WL_I = W_X_B @ B_J_WL_I
|
317
317
|
|
318
318
|
case VelRepr.Body:
|
319
319
|
L_X_B = Adjoint.from_transform(transform=B_H_L, inverse=True)
|
@@ -505,7 +505,7 @@ def jacobian_derivative(
|
|
505
505
|
with data.switch_velocity_representation(VelRepr.Body):
|
506
506
|
B_v_WB = data.base_velocity()
|
507
507
|
|
508
|
-
O_Ẋ_B = W_Ẋ_B = W_X_B @ jaxsim.math.Cross.vx(B_v_WB)
|
508
|
+
O_Ẋ_B = W_Ẋ_B = W_X_B @ jaxsim.math.Cross.vx(B_v_WB)
|
509
509
|
|
510
510
|
case VelRepr.Body:
|
511
511
|
|
@@ -519,9 +519,7 @@ def jacobian_derivative(
|
|
519
519
|
B_v_WB = data.base_velocity()
|
520
520
|
L_v_WL = js.link.velocity(model=model, data=data, link_index=link_index)
|
521
521
|
|
522
|
-
O_Ẋ_B = L_Ẋ_B = -L_X_B @ jaxsim.math.Cross.vx(
|
523
|
-
B_X_L @ L_v_WL - B_v_WB
|
524
|
-
)
|
522
|
+
O_Ẋ_B = L_Ẋ_B = -L_X_B @ jaxsim.math.Cross.vx(B_X_L @ L_v_WL - B_v_WB)
|
525
523
|
|
526
524
|
case VelRepr.Mixed:
|
527
525
|
|
@@ -546,9 +544,8 @@ def jacobian_derivative(
|
|
546
544
|
LW_v_LW_L = LW_v_WL - LW_v_W_LW
|
547
545
|
LW_v_B_LW = LW_v_WL - LW_X_B @ B_v_WB - LW_v_LW_L
|
548
546
|
|
549
|
-
O_Ẋ_B = LW_Ẋ_B = -LW_X_B @ jaxsim.math.Cross.vx(
|
550
|
-
|
551
|
-
)
|
547
|
+
O_Ẋ_B = LW_Ẋ_B = -LW_X_B @ jaxsim.math.Cross.vx(B_X_LW @ LW_v_B_LW)
|
548
|
+
|
552
549
|
case _:
|
553
550
|
raise ValueError(output_vel_repr)
|
554
551
|
|