Merge pull request #66 from ami-iit/fix-prismatic-joint

Fix homogeneous transform computation for prismatic joints
ami-iit · Feb 21, 2024 · 45f6d53 · 45f6d53
2 parents 919576d + 02a04bb
commit 45f6d53
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py
@@ -267,7 +267,7 @@ def H_prismatic_joint(
         T = self.factory.eye(4)
         R = self.R_from_RPY(rpy)
         T[:3, :3] = R
-        T[:3, 3] = xyz + q * self.factory.array(axis)
+        T[:3, 3] = self.factory.array(xyz) + q * self.factory.array(axis)
         return T
 
     def H_from_Pos_RPY(self, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLike: