From 7579fb012b4f636b7b575b198d54b001a4b714fd Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 3 Mar 2025 22:27:01 +0100
Subject: [PATCH] fixed heron on python3.13
---
python/smc/robots/implementations/heron.py | 5 ++++-
python/smc/robots/implementations/ur5e.py | 7 +++++--
2 files changed, 9 insertions(+), 3 deletions(-)
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index f6f4da3..a8b770b 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/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
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index a7ca945..af934b8 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/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,7 +261,8 @@ 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)
- model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
+ if not with_gripper_joints:
+ model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
data = pin.Data(model)
return model, collision_model, visual_model, data
--
GitLab