Skip to content

Commit

Permalink
Map Coriolis matrix over velocity representation
Browse files Browse the repository at this point in the history
  • Loading branch information
flferretti committed Feb 2, 2024
1 parent b38ff1d commit d1f81b9
Showing 1 changed file with 14 additions and 9 deletions.
23 changes: 14 additions & 9 deletions src/jaxsim/high_level/model.py
Original file line number Diff line number Diff line change
Expand Up @@ -960,22 +960,27 @@ def coriolis_matrix(self) -> jtp.Matrix:
xfb=self.data.model_state.xfb(),
)

if self.velocity_representation is VelRepr.Inertial:
return C

elif self.velocity_representation is VelRepr.Body:
def body(C):
W_H_B = self.base_transform()
B_X_W = sixd.se3.SE3.from_matrix(W_H_B).inverse().adjoint()
return B_X_W.T @ C @ B_X_W
C = B_X_W.T @ C @ B_X_W

elif self.velocity_representation is VelRepr.Mixed:
def mixed(C):
W_H_B = self.base_transform()
W_H_BW = W_H_B.at[0:3, 0:3].set(jnp.eye(3))
BW_X_W = sixd.se3.SE3.from_matrix(W_H_BW).inverse().adjoint()
return BW_X_W.T @ C @ BW_X_W
C = BW_X_W.T @ C @ BW_X_W

else:
raise ValueError(self.velocity_representation)
to_active = {
VelRepr.Body: body,
VelRepr.Mixed: mixed,
VelRepr.Inertial: lambda x: x,
}

try:
C = to_active[self.velocity_representation](C)
except ValueError as e:
raise e

return M, M_dot, C

Expand Down

0 comments on commit d1f81b9

Please sign in to comment.