From 14324fab1f10ae7ec8216ef89ccee084273a57fe Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Wed, 5 Mar 2025 16:10:26 +0100
Subject: [PATCH] tried and tested now. of course half of the jacobian slicing
 was wrong lmao. not saying every controller does what it should, but at least
 the dimensions match

---
 .../cartesian_space/clik_point_to_point.py    |   1 +
 examples/cartesian_space/test_by_running.sh   | 132 ++++++++++++++++--
 .../cartesian_space_point_to_point.py         |  13 +-
 .../smc/control/cartesian_space/ik_solvers.py |  20 ++-
 .../controller_templates/point_to_point.py    |   2 +-
 python/smc/robots/abstract_robotmanager.py    |  22 +--
 python/smc/robots/implementations/yumi.py     |   3 -
 .../robots/interfaces/dual_arm_interface.py   |  54 ++++---
 .../robots/interfaces/single_arm_interface.py |   9 +-
 .../robots/interfaces/whole_body_interface.py |  79 +++++++----
 python/smc/robots/utils.py                    |   4 +-
 11 files changed, 242 insertions(+), 97 deletions(-)

diff --git a/examples/cartesian_space/clik_point_to_point.py b/examples/cartesian_space/clik_point_to_point.py
index e085952..db7ef64 100644
--- a/examples/cartesian_space/clik_point_to_point.py
+++ b/examples/cartesian_space/clik_point_to_point.py
@@ -35,6 +35,7 @@ if __name__ == "__main__":
             print("Look at the current pose, define something appropriate manually")
         T_w_goal = defineGoalPointCLI(robot)
     # compliantMoveL(args, robot, Mgoal)
+    print(robot.mode)
     moveL(args, robot, T_w_goal)
     robot.closeGripper()
     robot.openGripper()
diff --git a/examples/cartesian_space/test_by_running.sh b/examples/cartesian_space/test_by_running.sh
index 79f103c..059e2d0 100755
--- a/examples/cartesian_space/test_by_running.sh
+++ b/examples/cartesian_space/test_by_running.sh
@@ -6,17 +6,17 @@
 # single arm 
 # ###############
 # damped pseudoinverse
-runnable="clik_point_to_point.py --randomly-generate-goal --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2000"
+runnable="clik_point_to_point.py --randomly-generate-goal --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=20000"
 echo $runnable
 python $runnable
 
 # QP quadprog
-runnable="clik_point_to_point.py --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="clik_point_to_point.py --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
 # QP proxsuite
-runnable="clik_point_to_point.py --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="clik_point_to_point.py --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
@@ -24,17 +24,35 @@ python $runnable
 # whole body single arm
 ######################
 # damped pseudoinverse 
-runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
 # QP quadprog
-runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
 # QP proxsuite
-runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+############################
+# whole body single arm only
+############################
+# damped pseudoinverse 
+runnable="clik_point_to_point.py --robot=heron --mode upper_body --randomly-generate-goal --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP quadprog
+runnable="clik_point_to_point.py --robot=heron --mode upper_body --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP proxsuite
+runnable="clik_point_to_point.py --robot=heron --mode upper_body --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
@@ -42,17 +60,53 @@ python $runnable
 # dual arm
 # ####################
 # damped pseudoinverse
-runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
 # QP quadprog
-runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
 # QP proxsuite
-runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+######################
+# dual arm, left arm only
+# ####################
+# damped pseudoinverse
+runnable="clik_point_to_point.py --robot=yumi --mode=left_arm_only --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP quadprog
+runnable="clik_point_to_point.py --robot=yumi --mode=left_arm_only --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP proxsuite
+runnable="clik_point_to_point.py --robot=yumi --mode=left_arm_only --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+######################
+# dual arm, right arm only
+# ####################
+# damped pseudoinverse
+runnable="clik_point_to_point.py --robot=yumi --mode=right_arm_only --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP quadprog
+runnable="clik_point_to_point.py --robot=yumi --mode=right_arm_only --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP proxsuite
+runnable="clik_point_to_point.py --robot=yumi --mode=right_arm_only --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
@@ -60,16 +114,70 @@ python $runnable
 # whole body dual arm
 # ##########################
 # damped pseudoinverse 
-runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP quadprog
+runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP proxsuite
+runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# ##########################
+# whole body dual arm, arms only
+# ##########################
+# damped pseudoinverse 
+runnable="dual_arm_clik.py --robot=myumi --mode=upper_body --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP quadprog
+runnable="dual_arm_clik.py --robot=myumi --mode=upper_body --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP proxsuite
+runnable="dual_arm_clik.py --robot=myumi --mode=upper_body --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+######################
+# whole body dual arm, left arm only
+# ####################
+# damped pseudoinverse
+runnable="clik_point_to_point.py --robot=myumi --mode=left_arm_only --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP quadprog
+runnable="clik_point_to_point.py --robot=myumi --mode=left_arm_only --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+# QP proxsuite
+runnable="clik_point_to_point.py --robot=myumi --mode=left_arm_only --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
+echo $runnable
+python $runnable
+
+######################
+# whole body dual arm, right arm only
+# ####################
+# damped pseudoinverse
+runnable="clik_point_to_point.py --robot=myumi --mode=right_arm_only --randomly-generate-goal  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
 # QP quadprog
-runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="clik_point_to_point.py --robot=myumi --mode=right_arm_only --randomly-generate-goal --ik-solver=QPquadprog --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
 
 # QP proxsuite
-runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="clik_point_to_point.py --robot=myumi --mode=right_arm_only --randomly-generate-goal --ik-solver=QPproxsuite --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=20000"
 echo $runnable
 python $runnable
diff --git a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
index 402f6b2..7b52ccd 100644
--- a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
+++ b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
@@ -180,7 +180,7 @@ def controlLoopClikDualArm(
     T_abs_rgoal: pin.SE3,
     args: Namespace,
     robot: DualArmInterface,
-    i: int,
+    t: int,
     past_data: dict[str, deque[np.ndarray]],
 ) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
     """
@@ -203,6 +203,15 @@ def controlLoopClikDualArm(
     err_vector = np.concatenate((err_vector_left, err_vector_right))
     J = robot.getJacobian()
     v_cmd = ik_solver(J, err_vector)
+    if v_cmd is None:
+        print(
+            t,
+            "the controller you chose produced None as output, using dampedPseudoinverse instead",
+        )
+        v_cmd = dampedPseudoinverse(1e-2, J, err_vector)
+    else:
+        if args.debug_prints:
+            print(t, "ik solver success")
     return v_cmd, {}, {}
 
 
@@ -218,8 +227,6 @@ def moveLDualArm(
     moveLDualArm
     -----------
     """
-    # TODO: clean this up
-    robot._mode = DualArmInterface.control_mode.both_arms
     ik_solver = getIKSolver(args, robot)
     controlLoop = partial(
         DualEEP2PCtrlLoopTemplate,
diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py
index aa3e26c..4fa8540 100644
--- a/python/smc/control/cartesian_space/ik_solvers.py
+++ b/python/smc/control/cartesian_space/ik_solvers.py
@@ -37,8 +37,8 @@ def getIKSolver(
     # if controller_name == "invKinm_PseudoInv_half":
     #    return invKinm_PseudoInv_half
     if args.ik_solver == "QPquadprog":
-        lb = -1 * robot._max_v.copy()
-        ub = robot._max_v.copy()
+        lb = -1 * robot.max_v
+        ub = robot.max_v
         return partial(QPquadprog, lb=lb, ub=ub)
     if args.ik_solver == "QPproxsuite":
         H = np.eye(robot.nv)
@@ -46,18 +46,14 @@ def getIKSolver(
         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)
+        J = robot.getJacobian()
+        A = np.eye(J.shape[0], robot.nv)
+        b = np.ones(J.shape[0]) * 0.1
+        qp = proxqp.dense.QP(robot.nv, J.shape[0], robot.nv)
         # proxqp does lb <= Cx <= ub
         C = np.eye(robot.nv)
-        lb = -1 * robot._max_v.copy()
-        ub = robot._max_v.copy()
+        lb = -1 * robot.max_v
+        ub = robot.max_v
         qp.init(H, g, A, b, G, lb, ub)
         qp.solve()
         return partial(QPproxsuite, qp)
diff --git a/python/smc/control/controller_templates/point_to_point.py b/python/smc/control/controller_templates/point_to_point.py
index 67f64b1..113f5e0 100644
--- a/python/smc/control/controller_templates/point_to_point.py
+++ b/python/smc/control/controller_templates/point_to_point.py
@@ -116,7 +116,7 @@ def DualEEP2PCtrlLoopTemplate(
 
     log_item["qs"] = robot.q
     log_item["dqs"] = robot.v
-    log_item["dqs_cmd"] = v_cmd.reshape((robot.model.nv,))
+    log_item["dqs_cmd"] = v_cmd.reshape((robot.nv,))
     log_item["l_err_norm"] = err_vector_left_norm.reshape((1,))
     log_item["r_err_norm"] = err_vector_right_norm.reshape((1,))
     return breakFlag, save_past_item, log_item
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index a8da284..10bec21 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -29,9 +29,7 @@ class AbstractRobotManager(abc.ABC):
         [
             ("whole_body", 1),
             ("base_only", 2),
-            ("arm_only", 3),
-            ("both_arms", 3),
-            ("both_arms_only", 3),
+            ("upper_body", 3),
             ("left_arm_only", 4),
             ("right_arm_only", 5),
         ],
@@ -132,6 +130,8 @@ class AbstractRobotManager(abc.ABC):
 
     @mode.setter
     def mode(self, mode: control_mode) -> None:
+        print(mode)
+        print(self._available_modes)
         assert mode in self._available_modes
         self._mode = mode
 
@@ -151,6 +151,10 @@ class AbstractRobotManager(abc.ABC):
     def collision_model(self) -> pin.pinocchio_pywrap_default.GeometryModel:
         return self._collision_model
 
+    @property
+    def max_v(self) -> np.ndarray:
+        return self._max_v.copy()
+
     @abc.abstractmethod
     def setInitialPose(self) -> None:
         """
@@ -247,20 +251,20 @@ class AbstractRobotManager(abc.ABC):
         Look at some interfaces to see particular implementations.
         """
 
-    def sendVelocityCommand(self, v) -> None:
+    def sendVelocityCommand(self, v_cmd) -> None:
         """
         sendVelocityCommand
         -------------------
         1) saturate the command to comply with hardware limits or smaller limits you've set
         2) send it via the particular robot's particular communication interface
         """
-        assert type(v) == np.ndarray
-        assert len(v) == self.model.nv
-        v = np.clip(v, -1 * self._max_v, self._max_v)
-        self.sendVelocityCommandToReal(v)
+        assert type(v_cmd) == np.ndarray
+        assert len(v_cmd) == self.model.nv
+        v_cmd_to_real = np.clip(v_cmd, -1 * self._max_v, self._max_v)
+        self.sendVelocityCommandToReal(v_cmd_to_real)
 
     @abc.abstractmethod
-    def sendVelocityCommandToReal(self, v): ...
+    def sendVelocityCommandToReal(self, v_cmd_to_real): ...
 
     @abc.abstractmethod
     def stopRobot(self):
diff --git a/python/smc/robots/implementations/yumi.py b/python/smc/robots/implementations/yumi.py
index 2bd17e8..6f63e39 100644
--- a/python/smc/robots/implementations/yumi.py
+++ b/python/smc/robots/implementations/yumi.py
@@ -57,9 +57,6 @@ class AbstractYuMiRobotManager(DualArmInterface):
             ]
         )
 
-        self._mode: DualArmInterface.control_mode = (
-            DualArmInterface.control_mode.both_arms
-        )
         super().__init__(args)
 
 
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index b04f82a..98045ce 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -29,12 +29,15 @@ class DualArmInterface(SingleArmInterface):
         self._r_ee_frame_id: int
         if args.debug_prints:
             print("DualArmInterface init")
+        # this init might be called from wholebodyinterface in which case it has more modes,
+        # and this would override those
+        if not hasattr(self, "_available_modes"):
+            self._available_modes: list[AbstractRobotManager.control_mode] = [
+                AbstractRobotManager.control_mode.whole_body,
+                AbstractRobotManager.control_mode.left_arm_only,
+                AbstractRobotManager.control_mode.right_arm_only,
+            ]
         super().__init__(args)
-        self._available_modes: list[AbstractRobotManager.control_mode] = [
-            AbstractRobotManager.control_mode.both_arms,
-            AbstractRobotManager.control_mode.left_arm_only,
-            AbstractRobotManager.control_mode.right_arm_only,
-        ]
 
     @property
     def q(self) -> np.ndarray:
@@ -52,35 +55,42 @@ class DualArmInterface(SingleArmInterface):
     def nq(self):
         if self._mode == self.control_mode.left_arm_only:
             return self.model.nq // 2
-
         if self._mode == self.control_mode.right_arm_only:
             return self.model.nq // 2
-
         return self.model.nq
 
     @property
     def v(self) -> np.ndarray:
         if self._mode == self.control_mode.left_arm_only:
             return self._v[: self.model.nv // 2]
-
         if self._mode == self.control_mode.right_arm_only:
             return self._v[self.model.nv // 2 :]
-
         return self._v.copy()
 
     @property
     def nv(self):
         if self._mode == self.control_mode.left_arm_only:
             return self.model.nv // 2
-
         if self._mode == self.control_mode.right_arm_only:
             return self.model.nv // 2
-
         return self.model.nv
 
-    # NOTE: might be misleading and unhelpful
+    # TODO: put back in when python3.12 will be safe to use
+    #    @override
+    @property
+    def max_v(self) -> np.ndarray:
+        if self._mode == self.control_mode.left_arm_only:
+            return self._max_v[: self.model.nv // 2]
+        if self._mode == self.control_mode.right_arm_only:
+            return self._max_v[self.model.nv // 2 :]
+        return self._max_v.copy()
+
     @property
     def T_w_e(self):
+        if self.mode == self.control_mode.left_arm_only:
+            return self.T_w_l
+        if self.mode == self.control_mode.right_arm_only:
+            return self.T_w_r
         return self.T_w_abs
 
     @property
@@ -199,23 +209,25 @@ class DualArmInterface(SingleArmInterface):
 
     # TODO: put back in when python3.12 will be safe to use
     #    @override
-    def sendVelocityCommand(self, v) -> None:
+    def sendVelocityCommand(self, v_cmd) -> None:
         """
         sendVelocityCommand
         -------------------
         1) saturate the command to comply with hardware limits or smaller limits you've set
         2) send it via the particular robot's particular communication interface
         """
-        assert type(v) == np.ndarray
+        assert type(v_cmd) == np.ndarray
+
+        if self._mode == self.control_mode.whole_body:
+            v_cmd_to_real = v_cmd
+        else:
+            v_cmd_to_real = np.zeros(self.model.nv)
 
         if self._mode == self.control_mode.left_arm_only:
-            v = np.zeros(self.model.nv)
-            v[3 : self.model.nv // 2] = v
+            v_cmd_to_real[: self.model.nv // 2] = v_cmd
 
         if self._mode == self.control_mode.right_arm_only:
-            v = np.zeros(self.model.nv)
-            v[self.model.nv // 2 :] = v
+            v_cmd_to_real[self.model.nv // 2 :] = v_cmd
 
-        assert len(v) == self.model.nv
-        v = np.clip(v, -1 * self._max_v, self._max_v)
-        self.sendVelocityCommandToReal(v)
+        v_cmd_to_real = np.clip(v_cmd_to_real, -1 * self._max_v, self._max_v)
+        self.sendVelocityCommandToReal(v_cmd_to_real)
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index f82335b..667cd3d 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -9,10 +9,13 @@ class SingleArmInterface(AbstractRobotManager):
             print("SingleArmInterface init")
         self._ee_frame_id: int
         self._T_w_e: pin.SE3
+        # this init might be called from wholebodyinterface in which case it has more modes,
+        # and this would override those
+        if not hasattr(self, "_available_modes"):
+            self._available_modes: list[AbstractRobotManager.control_mode] = [
+                AbstractRobotManager.control_mode.whole_body,
+            ]
         super().__init__(args)
-        self._available_modes: list[AbstractRobotManager.control_mode] = [
-            AbstractRobotManager.control_mode.whole_body,
-        ]
 
     @property
     def ee_frame_id(self):
diff --git a/python/smc/robots/interfaces/whole_body_interface.py b/python/smc/robots/interfaces/whole_body_interface.py
index 1cc3922..417ab62 100644
--- a/python/smc/robots/interfaces/whole_body_interface.py
+++ b/python/smc/robots/interfaces/whole_body_interface.py
@@ -31,7 +31,7 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
         self._available_modes: list[AbstractRobotManager.control_mode] = [
             AbstractRobotManager.control_mode.whole_body,
             AbstractRobotManager.control_mode.base_only,
-            AbstractRobotManager.control_mode.arm_only,
+            AbstractRobotManager.control_mode.upper_body,
         ]
         super().__init__(args)
 
@@ -53,7 +53,7 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return self._q[:4]
 
-        if self._mode == self.control_mode.arm_only:
+        if self._mode == self.control_mode.upper_body:
             return self._q[4:]
 
         return self._q.copy()
@@ -63,7 +63,7 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return 4
 
-        if self._mode == self.control_mode.arm_only:
+        if self._mode == self.control_mode.upper_body:
             return self.model.nq - 4
 
         return self.model.nq
@@ -73,7 +73,7 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return self._v[:3]
 
-        if self._mode == self.control_mode.arm_only:
+        if self._mode == self.control_mode.upper_body:
             return self._v[3:]
 
         return self._v.copy()
@@ -83,11 +83,21 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return 3
 
-        if self._mode == self.control_mode.arm_only:
+        if self._mode == self.control_mode.upper_body:
             return self.model.nv - 3
 
         return self.model.nv
 
+    # TODO: put back in when python3.12 will be safe to use
+    #    @override
+    @property
+    def max_v(self) -> np.ndarray:
+        if self._mode == self.control_mode.base_only:
+            return self._max_v[:3]
+        if self._mode == self.control_mode.upper_body:
+            return self._max_v[3:]
+        return self._max_v.copy()
+
     # TODO: put back in when python3.12 will be safe to use
     #    @override
     def forwardKinematics(self) -> None:
@@ -110,7 +120,7 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return J_full[:, :3]
 
-        if self._mode == self.control_mode.arm_only:
+        if self._mode == self.control_mode.upper_body:
             return J_full[:, 3:]
 
         return J_full
@@ -129,7 +139,7 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             v = np.hstack((v, np.zeros(self.model.nv - 3)))
 
-        if self._mode == self.control_mode.arm_only:
+        if self._mode == self.control_mode.upper_body:
             v = np.hstack((np.zeros(3), v))
 
         assert len(v) == self.model.nv
@@ -141,14 +151,14 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
     def __init__(self, args: Namespace):
         if args.debug_prints:
             print("DualArmWholeBodyInterface init")
-        super().__init__(args)
         self._available_modes: list[AbstractRobotManager.control_mode] = [
             AbstractRobotManager.control_mode.whole_body,
             AbstractRobotManager.control_mode.base_only,
-            AbstractRobotManager.control_mode.both_arms_only,
+            AbstractRobotManager.control_mode.upper_body,
             AbstractRobotManager.control_mode.left_arm_only,
             AbstractRobotManager.control_mode.right_arm_only,
         ]
+        super().__init__(args)
 
     # TODO: override model property to produce the reduced version
     # depending on the control mode.
@@ -163,7 +173,7 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return self._q[:4]
 
-        if self._mode == self.control_mode.both_arms_only:
+        if self._mode == self.control_mode.upper_body:
             return self._q[4:]
 
         # NOTE: left should be on the left side of the joint values array
@@ -181,7 +191,7 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return 4
 
-        if self._mode == self.control_mode.both_arms_only:
+        if self._mode == self.control_mode.upper_body:
             return self.model.nq - 4
 
         if self._mode == self.control_mode.left_arm_only:
@@ -196,7 +206,7 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return self._v[:3]
 
-        if self._mode == self.control_mode.both_arms_only:
+        if self._mode == self.control_mode.upper_body:
             return self._v[3:]
 
         if self._mode == self.control_mode.left_arm_only:
@@ -212,7 +222,7 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.base_only:
             return 3
 
-        if self._mode == self.control_mode.both_arms_only:
+        if self._mode == self.control_mode.upper_body:
             return self.model.nv - 3
 
         if self._mode == self.control_mode.left_arm_only:
@@ -223,6 +233,20 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
 
         return self.model.nv
 
+    # TODO: put back in when python3.12 will be safe to use
+    #    @override
+    @property
+    def max_v(self) -> np.ndarray:
+        if self._mode == self.control_mode.base_only:
+            return self._max_v[:3]
+        if self._mode == self.control_mode.upper_body:
+            return self._max_v[3:]
+        if self._mode == self.control_mode.left_arm_only:
+            return self._max_v[3 : 3 + (self.model.nv - 3) // 2]
+        if self._mode == self.control_mode.right_arm_only:
+            return self._max_v[3 + (self.model.nv - 3) // 2 :]
+        return self._max_v.copy()
+
     # TODO: put back in when python3.12 will be safe to use
     #    @override
     def forwardKinematics(self) -> None:
@@ -261,43 +285,38 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
         if self._mode == self.control_mode.right_arm_only:
             return J_right
 
-        J_full = np.zeros((12, self.nv))
+        J_full = np.zeros((12, self.model.nv))
         J_full[:6, : 3 + (self.model.nv - 3) // 2] = J_left_with_base
         J_full[6:, 3 + (self.model.nv - 3) // 2 :] = J_right
         # NOTE: add base for right end-effector
         # look at note above returning base only jacobian
         J_full[6:, :3] = J_left_with_base[:, :3]
 
-        if self._mode == self.control_mode.both_arms_only:
+        if self._mode == self.control_mode.upper_body:
             return J_full[:, 3:]
 
         return J_full
 
     # TODO: put back in when python3.12 will be safe to use
     #    @override
-    def sendVelocityCommand(self, v) -> None:
+    def sendVelocityCommand(self, v_cmd) -> None:
         """
         sendVelocityCommand
         -------------------
         1) saturate the command to comply with hardware limits or smaller limits you've set
         2) send it via the particular robot's particular communication interface
         """
-        assert type(v) == np.ndarray
+        assert type(v_cmd) == np.ndarray
+        v_cmd_to_real = np.zeros(self.model.nv)
         if self._mode == self.control_mode.base_only:
-            v = np.hstack((v, np.zeros(self.model.nv - 3)))
-
-        if self._mode == self.control_mode.both_arms_only:
-            v = np.hstack((np.zeros(3), v))
-
+            v_cmd_to_real[:3] = v_cmd
+        if self._mode == self.control_mode.upper_body:
+            v_cmd_to_real[3:] = v_cmd
         if self._mode == self.control_mode.left_arm_only:
-            v = np.zeros(self.model.nv)
-            v[3 : (self.model.nv - 3) // 2] = v
-
+            v_cmd_to_real[3 : 3 + (self.model.nv - 3) // 2] = v_cmd
         if self._mode == self.control_mode.right_arm_only:
-            v = np.zeros(self.model.nv)
-            v[3 + (self.model.nv - 3) // 2 :] = v
+            v_cmd_to_real[3 + (self.model.nv - 3) // 2 :] = v_cmd
 
-        assert len(v) == self.model.nv
-        v = np.clip(v, -1 * self._max_v, self._max_v)
+        v_cmd_to_real = np.clip(v_cmd_to_real, -1 * self._max_v, self._max_v)
 
-        self.sendVelocityCommandToReal(v)
+        self.sendVelocityCommandToReal(v_cmd_to_real)
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index 95710f6..c0df4b4 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -38,9 +38,7 @@ def getMinimalArgParser():
         choices=[
             "whole_body",
             "base_only",
-            "arm_only",
-            "both_arms",
-            "both_arms_only",
+            "upper_body",
             "left_arm_only",
             "right_arm_only",
         ],
-- 
GitLab