Skip to content

Commit

Permalink
Add bias forces check in Coriolis matrix test
Browse files Browse the repository at this point in the history
  • Loading branch information
flferretti committed Feb 2, 2024
1 parent 8fa37a5 commit 591a60a
Showing 1 changed file with 7 additions and 1 deletion.
8 changes: 7 additions & 1 deletion tests/test_coriolis.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,13 @@ def test_coriolis(robot: utils_models.Robot, vel_repr: VelRepr) -> None:
physics_model=model.physics_model
)

# Compute model acceleration with ABA
# Compute output of the coriolis matrix algorithm
M, M_dot, C = model.coriolis_matrix()

# Compute the bias forces and the model acceleration
h = model.free_floating_bias_forces()
nu = np.array(model.forward_dynamics_aba())

assert np.allclose(h - (C @ nu))

assert np.allclose(M_dot - (C @ C.T), np.zeros_like(M_dot), atol=1e-5)

0 comments on commit 591a60a

Please sign in to comment.