fixed heron on python3.13

python/smc/robots/implementations/heron.py

@@ -283,7 +283,9 @@ class RealHeronRobotManager(AbstractHeronRobotManager, AbstractRealRobotManager)
 
 def heron_approximation():
     # arm + gripper
-    model_arm, collision_model_arm, visual_model_arm, _ = get_model()
+    model_arm, collision_model_arm, visual_model_arm, _ = get_model(
+        with_gripper_joints=True
+    )
 
     # mobile base as planar joint (there's probably a better
     # option but whatever right now)
@@ -297,6 +299,7 @@ def heron_approximation():
         1,
         pin.SE3.Identity(),
     )
+    model = pin.buildReducedModel(model, [8, 9], np.zeros(model.nq))
     data = model.createData()
 
     # fix gripper

python/smc/robots/implementations/ur5e.py

@@ -178,7 +178,9 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
         self._rtde_control.endFreedriveMode()
 
 
-def get_model() -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]:
+def get_model(
+    with_gripper_joints=False,
+) -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]:
 
     urdf_path_relative = files("smc.robots.robot_descriptions.urdf").joinpath(
         "ur5e_with_robotiq_hande_FIXED_PATHS.urdf"
@@ -259,6 +261,7 @@ def get_model() -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Da
     model.jointPlacements[5] = wrist_2_se3
     model.jointPlacements[6] = wrist_3_se3
     # TODO: fix where the fingers end up by setting a better position here (or maybe not here idk)
+    if not with_gripper_joints:
         model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
     data = pin.Data(model)