solved a few bugs

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)
+        # 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)

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