Skip to content

Commit

Permalink
Add test of jacobian derivative
Browse files Browse the repository at this point in the history
  • Loading branch information
diegoferigo committed Jun 5, 2024
1 parent 80a27b1 commit f6295c9
Showing 1 changed file with 40 additions and 0 deletions.
40 changes: 40 additions & 0 deletions tests/test_api_link.py
Original file line number Diff line number Diff line change
Expand Up @@ -211,3 +211,43 @@ def test_link_bias_acceleration(
Jν_idt = kin_dyn.frame_bias_acc(frame_name=name)
Jν_js = js.link.bias_acceleration(model=model, data=data, link_index=index)
assert pytest.approx(Jν_idt) == Jν_js


def test_link_jacobian_derivative(
jaxsim_models_types: js.model.JaxSimModel,
velocity_representation: VelRepr,
prng_key: jax.Array,
):

model = jaxsim_models_types

key, subkey = jax.random.split(prng_key, num=2)
data = js.data.random_model_data(
model=model,
key=subkey,
velocity_representation=velocity_representation,
)

# =====
# Tests
# =====

# Test only the last link of the model.
link_name = model.link_names()[-1]
link_index = js.link.name_to_idx(model=model, link_name=link_name)

# Get the generalized velocity.
I_ν = data.generalized_velocity()

# Compute J̇.
O_J̇_WL_I = js.link.jacobian_derivative(
model=model, data=data, link_index=link_index
)

# Compute the product J̇ν.
O_a_bias_WL = js.link.bias_acceleration(
model=model, data=data, link_index=link_index
)

# Compare the two computations.
assert O_J̇_WL_I @ I_ν == pytest.approx(O_a_bias_WL)

0 comments on commit f6295c9

Please sign in to comment.