diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py index 60d054c1f4606e4ae6e119351b9b734c8e43ce92..aa3e26cfe600bc2115ee8bd9e5da2711e3d677fb 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 b9fd5dce9d5eca6b07c25d7c950039a4dd39092e..52361ea37d85f917981d9efc8176fa2d67df0fb1 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