diff --git a/src/jaxsim/parsers/descriptions/collision.py b/src/jaxsim/parsers/descriptions/collision.py index 5cba82d65..7815af488 100644 --- a/src/jaxsim/parsers/descriptions/collision.py +++ b/src/jaxsim/parsers/descriptions/collision.py @@ -163,7 +163,6 @@ class MeshCollision(CollisionShape): def __hash__(self) -> int: return hash( ( - hash(super()), hash(tuple(self.center.tolist())), hash(self.collidable_points), ) diff --git a/src/jaxsim/parsers/rod/utils.py b/src/jaxsim/parsers/rod/utils.py index 2b9ee9c15..15fd5ed70 100644 --- a/src/jaxsim/parsers/rod/utils.py +++ b/src/jaxsim/parsers/rod/utils.py @@ -10,8 +10,8 @@ import jaxsim.typing as jtp from jaxsim import logging -from jaxsim.math.inertia import Inertia from jaxsim.math.adjoint import Adjoint +from jaxsim.math.inertia import Inertia from jaxsim.parsers import descriptions @@ -249,17 +249,18 @@ def create_mesh_collision( points = mesh.vertices H = collision.pose.transform() if collision.pose is not None else np.eye(4) - center_of_collision_wrt_link = (H @ np.hstack([0, 0, 0, 1.0]))[0:-1] + # Extract translation from transformation matrix + center_of_collision_wrt_link = H[:3, 3] mesh_points_wrt_link = ( H @ np.hstack([points, np.vstack([1.0] * points.shape[0])]).T - )[0:3, :] + )[0:3, :].T collidable_points = [ descriptions.CollidablePoint( parent_link=link_description, position=point, enabled=True, ) - for point in mesh_points_wrt_link.T + for point in mesh_points_wrt_link ] return descriptions.MeshCollision( collidable_points=collidable_points, center=center_of_collision_wrt_link