From b0c4d122d7b6f314a9e65d6add29e59dc2314dfb Mon Sep 17 00:00:00 2001
From: Lorenzo Conti <lorenzo.conti@iit.it>
Date: Fri, 5 Jul 2024 16:08:34 +0200
Subject: [PATCH] Implemented initial reviews from
 https://github.com/ami-iit/jaxsim/pull/156

- Restructured mesh_collision creation method
- Removed unnecessary hash inheritance
---
 src/jaxsim/parsers/descriptions/collision.py | 1 -
 src/jaxsim/parsers/rod/utils.py              | 9 +++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

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