gbionics / gbionics/jaxsim

Mismatch between `data.base_transform` and `jaxsim.api.model.forward_kinematics` for fixed-base models with non trivial base-to-world transform

Open
#337 3 comments 1 reaction 0 assignees View on GitHub
bug
Dominant language
Python
Stars
203
Forks
23
Avg merge
2m
Merged PRs (30d)
1

Description

I've encountered a strange behavior when trying to simulate a fixed base model, i.e. a model with a description in which it is present a fixed joint between a `world` link and the model `base` link.

In particular when in the model URDF I set a base pose ${} ^W H _B \neq I _4$ through the `` tag of the fixed joint, in Jaxsim I get different results by calling [`data.base_transform`](https://github.com/ami-iit/jaxsim/blob/77f04c2d908aaa2dcf55e6cbff95f621a1102494/src/jaxsim/api/data.py#L394-L411) and [`jaxsim.api.model.forward_kinematics`](https://github.com/ami-iit/jaxsim/blob/77f04c2d908aaa2dcf55e6cbff95f621a1102494/src/jaxsim/api/model.py#L539-L559) for the base.

## Repro steps

Using a modified version of [`single_pendulum model`](https://github.com/ami-iit/jaxsim/blob/77f04c2d908aaa2dcf55e6cbff95f621a1102494/tests/conftest.py#L333-L440) defined in unit tests, in which a non trivial base to world pose has been defined:

```python
import jax.numpy as jnp
import jaxsim.api as js
import numpy as np
import rod
from jaxsim import VelRepr
from jaxsim.integrators.fixed_step import ForwardEuler
from rod.urdf.exporter import UrdfExporter

def jaxsim_model_single_pendulum() -> js.model.JaxSimModel:
"""
Fixture providing the JaxSim model of a single pendulum.

Returns:
The JaxSim model of a single pendulum.
"""

import rod.builder.primitives

base_height = 2.15
upper_height = 1.0

# ===================
# Create the builders
# ===================

base_builder = rod.builder.primitives.BoxBuilder(
name="base",
mass=1.0,
x=0.15,
y=0.15,
z=base_height,
)

upper_builder = rod.builder.primitives.BoxBuilder(
name="upper",
mass=0.5,
x=0.15,
y=0.15,
z=upper_height,
)

# =================
# Create the joints
# =================

fixed = rod.Joint(
name="fixed_joint",
type="fixed",
parent="world",
child=base_builder.name,
pose=rod.Pose(pose=[0, 0, 0, 1.075, 1.57, 0]),
)

pivot = rod.Joint(
name="upper_joint",
type="continuous",
parent=base_builder.name,
child=upper_builder.name,
axis=rod.Axis(
xyz=rod.Xyz([1, 0, 0]),
limit=rod.Limit(),
),
)

# ================
# Create the links
# ================

base = (
base_builder.build_link(
name=base_builder.name,
pose=rod.builder.primitives.PrimitiveBuilder.build_pose(
pos=np.array([0, 0, base_height / 2])
),
)
.add_inertial()
.add_visual()
.add_collision()
.build()
)

upper_pose = rod.builder.primitives.PrimitiveBuilder.build_pose(
pos=np.array([0, 0, upper_height / 2])
)

upper = (
upper_builder.build_link(
name=upper_builder.name,
pose=rod.builder.primitives.PrimitiveBuilder.build_pose(
relative_to=base.name, pos=np.array([0, 0, upper_height])
),
)
.add_inertial(pose=upper_pose)
.add_visual(pose=upper_pose)
.add_collision(pose=upper_pose)
.build()
)

rod_model = rod.Sdf(
version="1.10",
model=rod.Model(
name="single_pendulum",
link=[base, upper],
joint=[fixed, pivot],
),
)

rod_model.model.resolve_frames()

urdf_string = UrdfExporter(pretty=True).to_urdf_string(sdf=rod_model.models()[0])

model = js.model.JaxSimModel.build_from_model_description(
model_description=urdf_string,
integrator=ForwardEuler,
time_step=0.001,
contact_model=js.contact.contacts.SoftContacts.build(),
)

return model

model = jaxsim_model_single_pendulum()
data: js.data.JaxSimModelData = js.data.JaxSimModelData.build(
model=model,
velocity_representation=VelRepr.Inertial,
joint_positions=jnp.array([-np.pi / 2]),
)

W_H_B_from_data = data.base_transform()
W_H_B_from_fk = js.model.forward_kinematics(model, data)

print(W_H_B_from_data)

print(W_H_B_from_fk[0])

assert jnp.allclose(W_H_B_from_data, W_H_B_from_fk[0])

```

Which outputs

```
jaxsim[41566] INFO Enabling JAX to use 64-bit precision
rod[41566] INFO Calling sdformat through '/home/acroci/repos/component_darwin/.pixi/envs/default/bin/gz sdf'
rod[41566] DEBUG Converting model 'single_pendulum' to URDF
rod[41566] DEBUG Detected 'base' as root link
rod[41566] DEBUG Building kinematic tree of model 'single_pendulum'
rod[41566] DEBUG Selecting 'base' as canonical link
rod[41566] DEBUG Edge 'fixed_joint' became a frame attached to 'base'
rod[41566] DEBUG Node 'world' became a frame attached to 'fixed_joint'
rod[41566] WARNING Ignoring non-trivial pose of link 'base'
jaxsim[41566] DEBUG Found model 'single_pendulum' in SDF resource
rod[41566] DEBUG Building kinematic tree of model 'single_pendulum'
rod[41566] DEBUG Selecting 'base' as canonical link
rod[41566] DEBUG Edge 'fixed_joint' became a frame attached to 'base'
rod[41566] DEBUG Node 'world' became a frame attached to 'fixed_joint'
jaxsim[41566] DEBUG Model 'single_pendulum' is fixed-base
jaxsim[41566] DEBUG Considering 'base' as base link
jaxsim[41566] DEBUG Found joints connecting to world: ['fixed_joint']
jaxsim[41566] INFO Combining the pose of base link 'base' with the pose of joint 'fixed_joint'
jaxsim[41566] INFO The kinematic graph doesn't need to be reduced
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]]
[[-0.47573 0.41948 -0.77312 0.51141]
[ 0.87959 0.22571 -0.41878 -0.94556]
[-0.00118 -0.87926 -0.47635 1.07541]
[ 0. 0. 0. 1. ]]
Traceback (most recent call last):
File "/home/acroci/repos/component_darwin/src/test_base_transform.py", line 137, in
assert jnp.allclose(W_H_B_from_data, W_H_B_from_fk[0])
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
AssertionError

```

Instead the two quantities match if the base link is defined as:
```python
base = (
base_builder.build_link(
name=base_builder.name,
pose=rod.builder.primitives.PrimitiveBuilder.build_pose(
# pos=np.array([0, 0, base_height / 2])
),
)
.add_inertial()
.add_visual()
.add_collision()
.build()
)
```

Output:
```
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]]
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]]
```

Contributor guide

Open the contributing guide

Assessment

This issue has not been assessed yet.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.