Skip to content

Commit

Permalink
Update FD test
Browse files Browse the repository at this point in the history
  • Loading branch information
flferretti committed Mar 6, 2024
1 parent 8b0020a commit a5b777a
Showing 1 changed file with 21 additions and 17 deletions.
38 changes: 21 additions & 17 deletions tests/test_forward_dynamics.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@
import pytest
from pytest import param as p

from jaxsim.high_level.common import VelRepr
from jaxsim.high_level.model import Model
import jaxsim.api as js
from jaxsim import VelRepr

from . import utils_models, utils_rng
from .utils_models import Robot
Expand Down Expand Up @@ -37,35 +37,39 @@ def test_aba(robot: utils_models.Robot, vel_repr: VelRepr) -> None:
# Get the URDF of the robot
urdf_file_path = utils_models.ModelFactory.get_model_description(robot=robot)

# Build the high-level model
model = Model.build_from_model_description(
# Build the model
model = js.model.JaxSimModel.build_from_model_description(
model_description=urdf_file_path,
vel_repr=vel_repr,
gravity=gravity,
is_urdf=True,
).mutable(mutable=True, validate=True)
gravity=gravity,
)

random_state = utils_rng.random_model_state(model=model)

# Initialize the model with a random state
model.data.model_state = utils_rng.random_physics_model_state(
physics_model=model.physics_model
data = js.data.JaxSimModelData.build(
model=model, velocity_representation=vel_repr, **random_state
)

# Initialize the model with a random input
model.data.model_input = utils_rng.random_physics_model_input(
physics_model=model.physics_model
)

# Get the joint torques
tau = model.joint_generalized_forces_targets()
tau, _ = utils_rng.random_model_input(model=model)

# Compute model acceleration with ABA
v̇_WB_aba, s̈_aba = model.forward_dynamics_aba(tau=tau)
v̇_WB_aba, s̈_aba = js.model.forward_dynamics_aba(
model=model,
data=data,
joint_forces=tau,
)

# ==============================================
# Compute forward dynamics with dedicated method
# ==============================================

v̇_WB, = model.forward_dynamics_crb(tau=tau)
v̇_WB, = js.model.forward_dynamics_crb(
model=model,
data=data,
joint_forces=tau,
)

assert .squeeze() == pytest.approx(s̈_aba.squeeze(), abs=0.5)
assert v̇_WB.squeeze() == pytest.approx(v̇_WB_aba.squeeze(), abs=0.2)

0 comments on commit a5b777a

Please sign in to comment.