From 5eb7c8d56ab23275354a271f33d70b4b83e2e379 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sun, 2 Mar 2025 21:34:16 +0100
Subject: [PATCH] solved a few bugs
---
python/smc/control/cartesian_space/ik_solvers.py | 13 ++++++++++---
python/smc/robots/implementations/mobile_yumi.py | 2 +-
2 files changed, 11 insertions(+), 4 deletions(-)
diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py
index 60d054c..aa3e26c 100644
--- a/python/smc/control/cartesian_space/ik_solvers.py
+++ b/python/smc/control/cartesian_space/ik_solvers.py
@@ -44,13 +44,20 @@ def getIKSolver(
H = np.eye(robot.nv)
g = np.zeros(robot.nv)
G = np.eye(robot.nv)
- A = np.eye(6, robot.nv)
- b = np.ones(6) * 0.1
+ # TODO: you have to check if which control mode it is
+ # probably easiest if all robots have a control mode
+ if issubclass(robot.__class__, DualArmInterface):
+ A = np.eye(12, robot.nv)
+ b = np.ones(12) * 0.1
+ qp = proxqp.dense.QP(robot.model.nv, 12, robot.model.nv)
+ else:
+ A = np.eye(6, robot.nv)
+ b = np.ones(6) * 0.1
+ qp = proxqp.dense.QP(robot.model.nv, 6, robot.model.nv)
# proxqp does lb <= Cx <= ub
C = np.eye(robot.nv)
lb = -1 * robot._max_v.copy()
ub = robot._max_v.copy()
- qp = proxqp.dense.QP(robot.model.nv, 6, robot.model.nv)
qp.init(H, g, A, b, G, lb, ub)
qp.solve()
return partial(QPproxsuite, qp)
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index b9fd5dc..52361ea 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -75,7 +75,7 @@ def get_mobile_yumi_model() -> (
tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]
):
- model_arms, visual_model_arms, collision_model_arms, _ = get_yumi_model()
+ model_arms, collision_model_arms, visual_model_arms, _ = get_yumi_model()
model_mobile_base, geom_model_mobile_base = get_mobile_base_model(False)
# frame-index should be 1
--
GitLab