From dd0350d2972248fe2da02c90318a96b7c9f442d2 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 7 Mar 2025 00:06:03 +0100
Subject: [PATCH] commiting just for the readme, will make the missing ocp
 tests next, and then refactor path planning mpcs to simultaneouly support
 fixed paths and paths from a path planner

---
 README.md                                     | 166 +++++++++++-------
 examples/navigation/mobile_base_navigation.py |   4 +-
 .../path_following_mpc.py                     |   0
 .../casadi_ocp_collision_avoidance.py         |   0
 .../croco_base_and_ee_reference_p2p.py        |  50 ++++++
 .../croco_dual_ee_reference_p2p_mpc.py}       |   0
 .../croco_ee_reference_p2p_mpc.py}            |   2 -
 .../croco_ee_reference_p2p_ocp.py}            |   0
 .../point_to_point/test_by_running.sh         |  58 ++++++
 examples/optimal_control/test_by_running.sh   |  36 ++--
 .../optimal_control/abstract_croco_ocp.py     |   2 -
 .../croco_mpc_path_following.py               |  12 ++
 .../path_following_croco_ocp.py               |  14 +-
 .../point_to_point_croco_ocp.py               |  24 +--
 python/smc/robots/abstract_robotmanager.py    |   6 +
 .../robots/abstract_simulated_robotmanager.py |   4 +-
 python/smc/robots/implementations/heron.py    |  52 +++---
 python/smc/robots/implementations/mir.py      |   4 +
 .../smc/robots/implementations/mobile_yumi.py |  21 +++
 python/smc/robots/implementations/ur5e.py     |  23 ++-
 20 files changed, 323 insertions(+), 155 deletions(-)
 rename examples/optimal_control/{ => path_following}/path_following_mpc.py (100%)
 rename examples/optimal_control/{ => point_to_point}/casadi_ocp_collision_avoidance.py (100%)
 create mode 100644 examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
 rename examples/optimal_control/{dual_arm_ik_mpc.py => point_to_point/croco_dual_ee_reference_p2p_mpc.py} (100%)
 rename examples/optimal_control/{croco_single_ee_ik_mpc.py => point_to_point/croco_ee_reference_p2p_mpc.py} (95%)
 rename examples/optimal_control/{croco_single_ee_ik_ocp.py => point_to_point/croco_ee_reference_p2p_ocp.py} (100%)
 create mode 100755 examples/optimal_control/point_to_point/test_by_running.sh

diff --git a/README.md b/README.md
index ed622b6..918c134 100644
--- a/README.md
+++ b/README.md
@@ -1,108 +1,140 @@
 # general info
--------------
-This is a software library for robot manipulator control.
+--------------
+This is a software library for robot control called 
+Simple Manipulator Control (SMC).
 Its existence serves two main goals:
-1. to speed up the development, testing and deployment 
-of new model-based control techniques
-2. to provide the introductory robot control software stack for
-engineers and engineering students
+1. to speed up the development, testing and deployment
+of model-based controllers
+2. to provide a ready-to-use collection of common control strategies
 
 These goals are achieved through the following.
-Its is designed to be as simple as possible, both in terms of logic 
+The library is designed to be as simple as possible, both in terms of logic 
 and the total amount of code. Its skeleton provides only a common
 interface to both real and simulated robots,
 and a wrapper around control loops which are to be written by the users.
-It also provides basic control examples like inverse kinematics
-point to point motion, which serve both as usage examples,
-and as basic building blocks for higher level control.
+It also provides the following common controllers: 
+- joint space control
+- inverse kinematics via:
+  - jacobian pseudoinverse
+  - various QP formulations, with multiple QP solvers
+- impedance control
+- dynamic motion primitives (DMP)
+- optimal control and MPC
 
 Note that this simplicity comes at the cost of some comfort -
-if this library was a car it would be Lada Niva,
+if this library was a car it would be a Lada Niva,
 not an S-class Mercedes. It is easy to fix or replace any part,
 and it runs though all weather conditions and deep mud.
 But there is no power steering, let alone seat heating or
-an "infotainment system". And windows are adjust manually with rotating knobs.
-My personal belief is that software should get out of the way 
+an "infotainment system". And windows are opened manually by rotating knobs.
+This is guided by a personal belief that software should get out of the way 
 of the fact that a robotic manipulator is just 6+ motorized pendulums,
 the control of which are easy provided adequate mathematical background.
- 
-Here is some basic technical information:
-- code is developed on Arch Linux, also tested on Ubuntu 22, should work on any Linux
-- code is written in Python, will be re-written in C++ with Python bindings in the future
-- all of the robotics-related libraries are written in C++ with Python bindings 
-  - ur_rtde 
-  - pinocchio 
-  - crocoddyl (will be added in the near future)
+
+## basic technical information
+- the code is written in Python
+- the code is tested on Arch Linux and Ubuntu 22 and 24, but should work on any Linux
+- all of the robotics-related dependent libraries are written in C++ with Python bindings, proving fast execution despite the Python front-end 
 - if installing on own system install the latest python dependencies and it should work
-- currently only support UR5e, porting to YuMi and Panda is ongoing work
+
+
+## currently supported robots
+  - UR robots
+  - mobile YuMi
+  - Heron (MiR platform with UR arm)
+
+### porting to new robots
 - robot models are loaded via URDF
-- porting to new robots requires providing an interface to the robot (socket with a protocol)
-  which contains functions for reading of the robot's state (joint positions, velocities, f/t sensor as vectors)
+- an interface to the robot (socket with a protocol, ros topic,...) needs to be provided.
+  The interface has to contain functions for reading of the robot's state (joint positions, velocities, f/t sensor as vectors)
   and functions to send control inputs (joint velocities)
-- extensions for torque-controlled robots are ongoing
-- RobotManager will be turned into a base class from each robot type will inherit, 
-  overriding functions as necessary to accomodate different interfaces
-  while keeping code readable
 
 
 # contents of this repository
 ---------------------------------
 ## example usage
 ----------------
-In the examples directory.
+In the examples directory. Different arguments and script description is available through python \[script.py\] --help .
+More complex examples also come with a technical report pdf which explains the math behind the code.
 
 ## the library 
 --------------------------------
-- ur_simple_control package in python/ur_simple_control, core functionality in managers.py
-- robot description files in python/ur_simple_control/robot_descriptions : 
+- smc Python package in python/smc
+- robot description files in python/smc/robots/robot_descriptions : 
     - UR5e with Robotiq Hand-e gripper: urdf containing both the arm and the gripper, and other urdf-related files
-- some basic controllers in python/ur_simple_control/basics
-- clik and extensions in python/ur_simple_control/clik
-- dmp in python/ur_simple_control/dmp
-- optimal control in python/ur_simple_control/optimal_control
-- more algorithms will be added over time
-- visualization in python/ur_simple_control/visualize
-- some basic tests will be here as well, while full projects built on this repo will be in separate repos
+- visualization in python/smc/visualization:
+  - real-time plotting in plotters.py
+  - visualizer (showing the robot and the workspace) in visualizer.py
+- control in python/smc/control:
+  - control_loop_manager - wrapper around control loops which manages visualization, plotting, logging and timing
+  - controller_templates - control loop templates for point to point tasks, path following tasks
+  - cartesian_space - control loops for inverse kinematics, impedance control
+  - freedrive - put the robot in freedrive (leadthrough) mode - move the robot around manually - can record joint angles and poses, effectively teaching by demonstration
+  - joint_space - control loops for joint space control
+  - optimal_control - OCP definitions, OCP solving + trajectory following control loops, MPC control loops
+  - dmp - dynamic motion primitives, control loop to follow a dmp (NOTE: bare bones,needs further development)
 
 ## documentation
 ------------------------
 - tutorials on linux, python and the library are in [tutorials repository](https://gitlab.control.lth.se/marko-g/LPR-tutorials)
-- basic documentation can be found in the docs directory
+- general documentation can be found in the docs directory
 - links to and basic information about the external resources are listed in the docs directory:
     - official UR documentation on the robot's capabilities and its interface
     - ur_rtde
     - pinocchio
-    - short instructions on installing Ubuntu and the project Docker image
+    - short instructions on installing Ubuntu and installing the library via Docker
 - code is well documented and short and simple enough to simply be read to be understood
-- documentation will be expanded according to demand
+- additional documentation will be expanded according to demand
+- more complex examples are documented with technical reports
 
-# libraries used and their purpose
------------------------------------
-- usual python libraries: numpy, matplotlib, tkinter (needed just for some of the visualization)
-- ur_rtde for the client-side real-time interface for Universal Robots robots (only UR5e is tested atm)
-- pinocchio for robot dynamics calculations
-- meshcat for visualization (has nice integration with pinocchio)
-- crocoddyl for optimal control
-- qpsolvers and ecos for QP formulation of IK -> will add pink because it's more convenient
-- Dockerized ur simulator as a simulator -DOES NOT WORK ATM (it's a bad simulator, but it uses the same robot-communication API 
-  as the real robot, making coding and testing easier)
-- Docker is used for shipping 
+# Installation
+---------------
+The library is in the python/smc directory. Navigate to python directory and do a local pip install with "pip install -e .".
+## Dependencies
+-------------------------
+- Linux OS - but possibly installable on other systems (depends on differences in python multiprocessing implementation and availability of pre-compiled dependencies)
+Python packages:
+- pinocchio - robot math
+- numpy - math
+- matplotlib - plotter
+- meshcat, meshcat_shapes - visualizer
+- \[optional\] UR robots interface: ur_rtde
+- \[optional\] yumi, mobile yumi, and heron interface: ros2
+- \[optional\] ik solvers: qpsolvers, quadprog, proxsuite, ecos, pin-pink (uses quadprog under the hood)
+- \[optional\] opc/mpc: crocoddyl
+- \[optional\] altertive ocp: casadi (requires pinocchio > 3.0)
+- \[optional\] path planning: albin's path planning repos: starworlds, star_navigation, tunnel_mpc (NOTE all these repos are copy-pasted to path_generation atm)
+- \[optional\] visual servoing: opencv-python
+- \[optional\] some of the visualization: tkinter
 
+## Docker
+---------
+The library is also shipped as a Docker image. Dockerfile available in docker folder. Instruction are in docs/installation.md
 
+
+# Usage on real robots
+---------------------
 ## connecting to the real robot
 -------------------------------
-- create a fixed ip connection to verify that you can connect to the robot. create a new Ethernet connection,
-  and under IPv4 setting put 192.168.1.101 as the host (your) address, 255.255.255.0 under netmask
-  and 192.168.1.1 as gateway (just in case). the robot's ip is fixed to 192.168.1.102. connect to the network 
-  once you've created it. put the robot's IP into the examples given in ur_rtde and verify the connection (select
-  an example with the robot moving and see that it moves).
-- run clik.py to verify it works
-
-# some TODOs
--------------
-- instead of using a debug argument and ifs for prints, use built-in python logging
-- implement and test other inverse kinematics algorithms
-- use pyproject.toml for python packaging. it actually brakes the current local install so it is avoided for now. 
-- write all code in c++, make python bindings with pybind. have common documentation and examples
-- do the documentation with doxygen so that you get webpages from markdown as well
-- create a DMP from multiple trajectories (actually learn a DMP from multiple examples)
+### UR robots
+The ur_rtde interface communicates with the robot over a network. It connects to the 50002 port on the robot.
+If you computer and the robot are on the same network, and you set the robot ip with the --robot-ip argument,
+everything should work. Client side's (your computer) firewalls shouldn't be a problem.
+
+The simplest way to set the network up is to:
+1. Directly connect your computer and the robot with an ethernet cable.
+2. Create a fixed ip connection. On the robot side:
+  1. On the teaching pendant go to Setting->Network
+  2. Select a manual/fixed IP connection and put 192.168.1.102. 
+On your computer:
+  1. Create a new Ethernet connection, and under IPv4 setting put 192.168.1.101 as the host (your) address, 255.255.255.0 under netmask
+  and 192.168.1.1 as gateway (just in case). 
+  2. Connect to this connection once you've created it.
+3. go to examples/cartesian_space and run "python clik.py --robot-ip 192.168.1.102" and move the robot to verify that everything works.
+
+### Mobile YuMi, Heron
+TODO: describe networking and what to start, or point to those robot's documentation
+
+# ongoing development
+---------------------
+Can be found in development_plan.toml.
diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index 8930cde..7e0f977 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -156,9 +156,9 @@ if __name__ == "__main__":
         CrocoBaseP2PMPC(args, robot, path2D[0])
         path_planner = partial(fixedPathPlanner, path2D)
 
-    # CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
+    CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
     # cartesianPathFollowingWithPlanner(args, robot, path_planner)
-    MPCWithCLIKFallback(args, robot, path_planner)
+    # MPCWithCLIKFallback(args, robot, path_planner)
 
     if args.real:
         robot.stopRobot()
diff --git a/examples/optimal_control/path_following_mpc.py b/examples/optimal_control/path_following/path_following_mpc.py
similarity index 100%
rename from examples/optimal_control/path_following_mpc.py
rename to examples/optimal_control/path_following/path_following_mpc.py
diff --git a/examples/optimal_control/casadi_ocp_collision_avoidance.py b/examples/optimal_control/point_to_point/casadi_ocp_collision_avoidance.py
similarity index 100%
rename from examples/optimal_control/casadi_ocp_collision_avoidance.py
rename to examples/optimal_control/point_to_point/casadi_ocp_collision_avoidance.py
diff --git a/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py b/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
new file mode 100644
index 0000000..e00a3cf
--- /dev/null
+++ b/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
@@ -0,0 +1,50 @@
+from smc import getRobotFromArgs
+from smc import getMinimalArgParser
+from smc.control.optimal_control.util import get_OCP_args
+from smc.control.cartesian_space import getClikArgs
+from smc.control.optimal_control.croco_mpc_point_to_point import CrocoEEAndBaseP2PMPC
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+
+from pinocchio import SE3
+import numpy as np
+
+
+def get_args():
+    parser = getMinimalArgParser()
+    parser = get_OCP_args(parser)
+    parser = getClikArgs(parser)  # literally just for goal error
+    args = parser.parse_args()
+    return args
+
+
+if __name__ == "__main__":
+    args = get_args()
+    robot = getRobotFromArgs(args)
+    assert issubclass(robot.__class__, MobileBaseInterface)
+    assert issubclass(robot.__class__, SingleArmInterface)
+    robot._step()
+    robot._step()
+    # TODO: put this back for nicer demos
+    # Mgoal = defineGoalPointCLI()
+    T_w_goal = SE3.Random()
+    p_basegoal = T_w_goal.translation.copy()
+    p_basegoal += np.random.random(3) / 4
+    p_basegoal[2] = 0.0
+
+    if args.visualizer:
+        # TODO: document defined viz messages somewhere
+        robot.updateViz({"fixed_path": p_basegoal.reshape((1, 3))})
+        robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
+
+    CrocoEEAndBaseP2PMPC(args, robot, T_w_goal, p_basegoal)
+
+    if args.real:
+        robot.stopRobot()
+
+    if args.save_log:
+        robot._log_manager.saveLog()
+        robot._log_manager.plotAllControlLoops()
+
+    if args.visualizer:
+        robot.killManipulatorVisualizer()
diff --git a/examples/optimal_control/dual_arm_ik_mpc.py b/examples/optimal_control/point_to_point/croco_dual_ee_reference_p2p_mpc.py
similarity index 100%
rename from examples/optimal_control/dual_arm_ik_mpc.py
rename to examples/optimal_control/point_to_point/croco_dual_ee_reference_p2p_mpc.py
diff --git a/examples/optimal_control/croco_single_ee_ik_mpc.py b/examples/optimal_control/point_to_point/croco_ee_reference_p2p_mpc.py
similarity index 95%
rename from examples/optimal_control/croco_single_ee_ik_mpc.py
rename to examples/optimal_control/point_to_point/croco_ee_reference_p2p_mpc.py
index bd25d55..0b21cd0 100644
--- a/examples/optimal_control/croco_single_ee_ik_mpc.py
+++ b/examples/optimal_control/point_to_point/croco_ee_reference_p2p_mpc.py
@@ -5,14 +5,12 @@ from smc.control.optimal_control.croco_mpc_point_to_point import (
 from smc.control.optimal_control.util import get_OCP_args
 from smc.control.cartesian_space import getClikArgs
 import pinocchio as pin
-import argcomplete
 
 
 def get_args():
     parser = getMinimalArgParser()
     parser = get_OCP_args(parser)
     parser = getClikArgs(parser)  # literally just for goal error
-    argcomplete.autocomplete(parser)
     args = parser.parse_args()
     return args
 
diff --git a/examples/optimal_control/croco_single_ee_ik_ocp.py b/examples/optimal_control/point_to_point/croco_ee_reference_p2p_ocp.py
similarity index 100%
rename from examples/optimal_control/croco_single_ee_ik_ocp.py
rename to examples/optimal_control/point_to_point/croco_ee_reference_p2p_ocp.py
diff --git a/examples/optimal_control/point_to_point/test_by_running.sh b/examples/optimal_control/point_to_point/test_by_running.sh
new file mode 100755
index 0000000..82ebcb7
--- /dev/null
+++ b/examples/optimal_control/point_to_point/test_by_running.sh
@@ -0,0 +1,58 @@
+#!/bin/bash
+# the idea here is to run all the runnable things
+# and test for syntax errors 
+# TODO: make these work with different modes by making robot.model a property which outputs truncated models
+
+##########################################################################################################
+#                                         point to point
+# ################################################################################################
+# single arm - ee reference
+# ###############
+# ocp
+runnable="croco_ee_reference_p2p_ocp.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2"
+echo $runnable
+python $runnable
+
+# mpc
+runnable="croco_ee_reference_p2p_mpc.py --max-solver-iter 10 --n-knots 30  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
+echo $runnable
+python $runnable
+
+# whole body single arm - ee reference
+# -------------------------------------
+# ocp
+runnable="croco_ee_reference_p2p_ocp.py --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
+echo $runnable
+python $runnable
+# mpc
+runnable="croco_ee_reference_p2p_mpc.py --max-solver-iter 10 --n-knots 30 --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
+echo $runnable
+python $runnable
+
+# dual arm - dual ee reference
+# ocp TODO: missing
+
+# mpc
+runnable="croco_dual_ee_reference_p2p_mpc.py --max-solver-iter 10 --n-knots 30 --robot=yumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
+echo $runnable
+python $runnable
+
+# whole body dual arm - dual ee reference
+# --------------------------
+# ocp TODO: missing
+#
+# mpc
+runnable="croco_dual_ee_reference_p2p_mpc.py --max-solver-iter 10 --n-knots 30 --robot=myumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
+echo $runnable
+python $runnable
+
+# whole body single arm  - base + ee reference
+# ----------------------------------
+# ocp TODO: missing
+# mpc TODO: missing
+#
+# whole body dual arm base + ee reference
+# ----------------------------------
+# ocp TODO: missing
+# mpc TODO: missing
+#
diff --git a/examples/optimal_control/test_by_running.sh b/examples/optimal_control/test_by_running.sh
index dc04fc8..57b3af2 100755
--- a/examples/optimal_control/test_by_running.sh
+++ b/examples/optimal_control/test_by_running.sh
@@ -2,8 +2,11 @@
 # the idea here is to run all the runnable things
 # and test for syntax errors 
 # TODO: make these work with different modes by making robot.model a property which outputs truncated models
-# ################
-# single arm 
+
+##########################################################################################################
+#                                         point to point
+# ################################################################################################
+# single arm - ee reference
 # ###############
 # ocp
 runnable="croco_single_ee_ik_ocp.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2"
@@ -15,9 +18,8 @@ runnable="croco_single_ee_ik_mpc.py --max-solver-iter 10 --n-knots 30  --ctrl-fr
 echo $runnable
 python $runnable
 
-######################
-# whole body single arm
-######################
+# whole body single arm - ee reference
+# -------------------------------------
 # ocp
 runnable="croco_single_ee_ik_ocp.py --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
 echo $runnable
@@ -27,9 +29,7 @@ runnable="croco_single_ee_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=he
 echo $runnable
 python $runnable
 
-######################
-# dual arm
-# ####################
+# dual arm - dual ee reference
 # ocp TODO: missing
 
 # mpc
@@ -37,9 +37,8 @@ runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=yumi --ct
 echo $runnable
 python $runnable
 
-# ##########################
-# whole body dual arm
-# ##########################
+# whole body dual arm - dual ee reference
+# --------------------------
 # ocp TODO: missing
 #
 # mpc
@@ -47,20 +46,13 @@ runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=myumi --c
 echo $runnable
 python $runnable
 
-##############################
-# base only
-# ##################################
-# ocp TODO: missing
-# mpc TODO: missing
-
-##############################
-# whole body single arm base + ee reference
-# ##################################
+# whole body single arm  - base + ee reference
+# ----------------------------------
 # ocp TODO: missing
 # mpc TODO: missing
 #
-##############################
 # whole body dual arm base + ee reference
-# ##################################
+# ----------------------------------
 # ocp TODO: missing
 # mpc TODO: missing
+#
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index 94d2ff9..f7a6a6a 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -128,8 +128,6 @@ class CrocoOCP(abc.ABC):
         # point activation input and the residual need to be of the same length obviously,
         # and this should be 2 * model.nv the way things are defined here.
 
-        # TODO: replace this with subclass or whatever call
-        # to check if the robot has mobilebase interface
         if issubclass(self.robot.__class__, MobileBaseInterface):
             self.xlb = self.xlb[1:]
             self.xub = self.xub[1:]
diff --git a/python/smc/control/optimal_control/croco_mpc_path_following.py b/python/smc/control/optimal_control/croco_mpc_path_following.py
index 73844d3..027ace1 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -51,6 +51,18 @@ def CrocoBasePathFollowingFromPlannerMPCControlLoop(
     ocp.warmstartAndReSolve(x0, data=(path_base))
     xs = np.array(ocp.solver.xs)
     v_cmd = xs[1, robot.model.nq :]
+    # NOTE: we might get stuck
+    # TODO: make it more robust, it can still get stuck with this
+    # a good idea is to do this for a some number of iterations - 
+    # you can keep this them in past data
+    if np.linalg.norm(v_cmd) < 0.05:
+        print(t, "RESOLVING FOR ONLY FINAL PATH POINT")
+        last_point_only = np.ones((len(path_base),2))
+        last_point_only =  np.hstack((last_point_only, np.zeros((len(path_base),1))))
+        last_point_only = last_point_only * path_base[-1]
+        ocp.warmstartAndReSolve(x0, data=(last_point_only))
+        xs = np.array(ocp.solver.xs)
+        v_cmd = xs[1, robot.model.nq :]
 
     err_vector_base = np.linalg.norm(p - path_base[0][:2])  # z axis is irrelevant
     log_item = {}
diff --git a/python/smc/control/optimal_control/path_following_croco_ocp.py b/python/smc/control/optimal_control/path_following_croco_ocp.py
index 64a13fd..1b9db97 100644
--- a/python/smc/control/optimal_control/path_following_croco_ocp.py
+++ b/python/smc/control/optimal_control/path_following_croco_ocp.py
@@ -62,11 +62,11 @@ class CrocoEEPathFollowingOCP(SingleArmIKOCP):
     def updateCosts(self, data):
         for i, runningModel in enumerate(self.solver.problem.runningModels):
             runningModel.differential.costs.costs[
-                "gripperPose" + str(i)
+                "ee_pose" + str(i)
             ].cost.residual.reference = data[i]
 
         self.solver.problem.terminalModel.differential.costs.costs[
-            "gripperPose" + str(self.args.n_knots)
+            "ee_pose" + str(self.args.n_knots)
         ].cost.residual.reference = data[-1]
 
 
@@ -80,7 +80,7 @@ class BaseAndEEPathFollowingOCP(BaseAndSingleArmIKOCP):
     """
 
     def __init__(self, args, robot: SingleArmWholeBodyInterface, x0):
-        goal = (robot.T_w_e, robot.T_w_b.translation.copy)
+        goal = (robot.T_w_e, robot.T_w_b.translation.copy())
         super().__init__(args, robot, x0, goal)
 
     def updateCosts(self, data):
@@ -113,17 +113,17 @@ class DualArmEEPathFollowingOCP(DualArmIKOCP):
         pathSE3_r = data[1]
         for i, runningModel in enumerate(self.solver.problem.runningModels):
             runningModel.differential.costs.costs[
-                "gripperPose_l" + str(i)
+                "l_ee_pose" + str(i)
             ].cost.residual.reference = pathSE3_l[i]
             runningModel.differential.costs.costs[
-                "gripperPose_r" + str(i)
+                "r_ee_pose" + str(i)
             ].cost.residual.reference = pathSE3_r[i]
 
         self.solver.problem.terminalModel.differential.costs.costs[
-            "gripperPose_l" + str(self.args.n_knots)
+            "l_ee_pose" + str(self.args.n_knots)
         ].cost.residual.reference = pathSE3_l[-1]
         self.solver.problem.terminalModel.differential.costs.costs[
-            "gripperPose_r" + str(self.args.n_knots)
+            "r_ee_pose" + str(self.args.n_knots)
         ].cost.residual.reference = pathSE3_r[-1]
 
 
diff --git a/python/smc/control/optimal_control/point_to_point_croco_ocp.py b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
index 616956d..1e6fada 100644
--- a/python/smc/control/optimal_control/point_to_point_croco_ocp.py
+++ b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
@@ -106,10 +106,10 @@ class SingleArmIKOCP(CrocoOCP):
         # )
         for i in range(self.args.n_knots):
             self.runningCostModels[i].addCost(
-                "gripperPose" + str(i), goalTrackingCost, self.ee_pose_cost_values[i]
+                "ee_pose" + str(i), goalTrackingCost, self.ee_pose_cost_values[i]
             )
         self.terminalCostModel.addCost(
-            "gripperPose" + str(self.args.n_knots),
+            "ee_pose" + str(self.args.n_knots),
             goalTrackingCost,
             self.ee_pose_cost_values[-1],
         )
@@ -123,10 +123,10 @@ class SingleArmIKOCP(CrocoOCP):
         T_w_goal = goal
         for i, runningModel in enumerate(self.solver.problem.runningModels):
             runningModel.differential.costs.costs[
-                "gripperPose" + str(i)
+                "ee_pose" + str(i)
             ].cost.residual.reference = T_w_goal
         self.solver.problem.terminalModel.differential.costs.costs[
-            "gripperPose" + str(self.args.n_knots)
+            "ee_pose" + str(self.args.n_knots)
         ].cost.residual.reference = T_w_goal
 
 
@@ -186,22 +186,22 @@ class DualArmIKOCP(CrocoOCP):
         # )
         for i in range(self.args.n_knots):
             self.runningCostModels[i].addCost(
-                "gripperPose_l" + str(i),
+                "l_ee_pose" + str(i),
                 goalTrackingCost_l,
                 self.ee_pose_cost_values[i],
             )
             self.runningCostModels[i].addCost(
-                "gripperPose_r" + str(i),
+                "r_ee_pose" + str(i),
                 goalTrackingCost_r,
                 self.ee_pose_cost_values[i],
             )
         self.terminalCostModel.addCost(
-            "gripperPose_l" + str(self.args.n_knots),
+            "l_ee_pose" + str(self.args.n_knots),
             goalTrackingCost_l,
             self.ee_pose_cost_values[-1],
         )
         self.terminalCostModel.addCost(
-            "gripperPose_r" + str(self.args.n_knots),
+            "r_ee_pose" + str(self.args.n_knots),
             goalTrackingCost_r,
             self.ee_pose_cost_values[-1],
         )
@@ -216,17 +216,17 @@ class DualArmIKOCP(CrocoOCP):
         T_w_lgoal, T_w_rgoal = goal
         for i, runningModel in enumerate(self.solver.problem.runningModels):
             runningModel.differential.costs.costs[
-                "gripperPose_l" + str(i)
+                "l_ee_pose" + str(i)
             ].cost.residual.reference = T_w_lgoal
             runningModel.differential.costs.costs[
-                "gripperPose_r" + str(i)
+                "r_ee_pose" + str(i)
             ].cost.residual.reference = T_w_rgoal
 
         self.solver.problem.terminalModel.differential.costs.costs[
-            "gripperPose_l" + str(self.args.n_knots)
+            "l_ee_pose" + str(self.args.n_knots)
         ].cost.residual.reference = T_w_lgoal
         self.solver.problem.terminalModel.differential.costs.costs[
-            "gripperPose_r" + str(self.args.n_knots)
+            "r_ee_pose" + str(self.args.n_knots)
         ].cost.residual.reference = T_w_rgoal
 
 
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index 5be02d2..b8fff65 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -398,6 +398,12 @@ class AbstractRealRobotManager(AbstractRobotManager, abc.ABC):
         self.connectToRobot()
         self.connectToGripper()
         self.setInitialPose()
+        if self.args.visualizer:
+            self.visualizer_manager.sendCommand(
+                {
+                    "q": self._q,
+                }
+            )
 
     @abc.abstractmethod
     def connectToRobot(self):
diff --git a/python/smc/robots/abstract_simulated_robotmanager.py b/python/smc/robots/abstract_simulated_robotmanager.py
index 42eebf1..ac79898 100644
--- a/python/smc/robots/abstract_simulated_robotmanager.py
+++ b/python/smc/robots/abstract_simulated_robotmanager.py
@@ -12,10 +12,10 @@ class AbstractSimulatedRobotManager(AbstractRealRobotManager):
             print("AbstractSimulatedRobotManager init")
         super().__init__(args)
 
-    # NOTE: can be override by any particular robot
+    # NOTE: can be overriden by any particular robot
     def setInitialPose(self):
         self._q = pin.randomConfiguration(
-            self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
+            self.model, self.model.lowerPositionLimit, self.model.upperPositionLimit
         )
 
     def sendVelocityCommandToReal(self, v):
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index 6087d3f..990ceb0 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -64,6 +64,25 @@ class SimulatedHeronRobotManager(
     def zeroFtSensor(self) -> None:
         self._wrench_bias = np.zeros(6)
 
+    def setInitialPose(self):
+        if self.args.start_from_current_pose:
+            # TODO: read position from localization topic,
+            # put that into _q
+            # HAS TO BE [x, y, cos(theta), sin(theta)] due to pinocchio's
+            # representation of planar joint state
+            self._q = np.zeros(self.nq)
+            rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
+            self._q[4:] = np.array(rtde_receive.getActualQ())
+        else:
+            self._q = pin.randomConfiguration(
+                self.model, self.model.lowerPositionLimit, self.model.upperPositionLimit
+            )
+            # pin.RandomConfiguration does not work well for planar joint,
+            # or at least i remember something along those lines being the case
+            theta = np.random.random() * 2 * np.pi - np.pi
+            self._q[2] = np.cos(theta)
+            self._q[3] = np.sin(theta)
+
 
 class RealHeronRobotManager(AbstractHeronRobotManager, AbstractRealRobotManager):
     def __init__(self, args):
@@ -98,33 +117,12 @@ class RealHeronRobotManager(AbstractHeronRobotManager, AbstractRealRobotManager)
                 exit()
 
     def setInitialPose(self):
-        if not self.args.real and self.args.start_from_current_pose:
-            # TODO: read position from localization topic,
-            # put that into _q
-            # HAS TO BE [x, y, cos(theta), sin(theta)] due to pinocchio's
-            # representation of planar joint state
-            self._q = np.zeros(self.nq)
-            self._rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
-            self._q[4:] = np.array(self._rtde_receive.getActualQ())
-            if self.args.visualize_manipulator:
-                self.visualizer_manager.sendCommand({"q": self._q})
-            raise NotImplementedError
-        if not self.args.real and not self.args.start_from_current_pose:
-            self._q = pin.randomConfiguration(
-                self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
-            )
-            # pin.RandomConfiguration does not work well for planar joint,
-            # or at least i remember something along those lines being the case
-            theta = np.random.random() * 2 * np.pi - np.pi
-            self._q[2] = np.cos(theta)
-            self._q[3] = np.sin(theta)
-        if self.args.real:
-            # TODO: read position from localization topic,
-            # put that into _q
-            # HAS TO BE [x, y, cos(theta), sin(theta)] due to pinocchio's
-            # representation of planar joint state
-            self._q = np.zeros(self.nq)
-            self._q[4:] = np.array(self._rtde_receive.getActualQ())
+        # TODO: read position from localization topic,
+        # put that into _q
+        # HAS TO BE [x, y, cos(theta), sin(theta)] due to pinocchio's
+        # representation of planar joint state
+        self._q = np.zeros(self.nq)
+        self._q[4:] = np.array(self._rtde_receive.getActualQ())
 
     def connectToRobot(self):
         # NOTE: you can't connect twice, so you can't have more than one RobotManager per robot.
diff --git a/python/smc/robots/implementations/mir.py b/python/smc/robots/implementations/mir.py
index 7bc8742..f64f9e2 100644
--- a/python/smc/robots/implementations/mir.py
+++ b/python/smc/robots/implementations/mir.py
@@ -50,6 +50,10 @@ class RealMirRobotManager(AbstractMirRobotManager):
         # self.q = pin.integrate(self.model, self.q, qd * self.dt)
         raise NotImplementedError
 
+    # TODO: implement
+    def setInitialPose(self):
+        raise NotImplementedError
+
     # TODO: implement
     def stopRobot(self):
         raise NotImplementedError
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 4aa298e..76c54c1 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -7,6 +7,7 @@ from smc.robots.implementations.yumi import get_yumi_model
 
 import pinocchio as pin
 from argparse import Namespace
+import numpy as np
 
 
 class AbstractMobileYuMiRobotManager(DualArmWholeBodyInterface):
@@ -40,6 +41,24 @@ class SimulatedMobileYuMiRobotManager(
             print("SimulatedMobileYuMiRobotManager init")
         super().__init__(args)
 
+    def setInitialPose(self):
+        if self.args.start_from_current_pose:
+            # TODO: add start from current pose for simulation, but actually read from the real robot
+            # TODO: read position from localization topic,
+            # put that into _q
+            # HAS TO BE [x, y, cos(theta), sin(theta)] due to pinocchio's
+            # representation of planar joint state
+            raise NotImplementedError
+        else:
+            self._q = pin.randomConfiguration(
+                self.model, self.model.lowerPositionLimit, self.model.upperPositionLimit
+            )
+            # pin.RandomConfiguration does not work well for planar joint,
+            # or at least i remember something along those lines being the case
+            theta = np.random.random() * 2 * np.pi - np.pi
+            self._q[2] = np.cos(theta)
+            self._q[3] = np.sin(theta)
+
 
 class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
     def __init__(self, args):
@@ -70,6 +89,8 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
 
         self.publisher_joints_cmd.publish(msg)
 
+    # TODO: define set initial pose by reading it from the real robot (well, the appropriate ros2 topic in this case)
+
 
 # TODO: define a separate mobile base for YuMi here
 # necessary because the dimensions are not the same
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index 6552d54..25d3ccf 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -56,6 +56,15 @@ class SimulatedUR5eRobotManager(
     def zeroFtSensor(self) -> None:
         self._wrench_bias = np.zeros(6)
 
+    def setInitialPose(self):
+        if self.args.start_from_current_pose:
+            rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
+            self._q = np.array(rtde_receive.getActualQ())
+        else:
+            self._q = pin.randomConfiguration(
+                self.model, self.model.lowerPositionLimit, self.model.upperPositionLimit
+            )
+
 
 class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
     def __init__(self, args: argparse.Namespace):
@@ -99,17 +108,7 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
                 exit()
 
     def setInitialPose(self):
-        if not self.args.real and self.args.start_from_current_pose:
-            self._rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
-            self._q = np.array(self._rtde_receive.getActualQ())
-            if self.args.visualize_manipulator:
-                self.visualizer_manager.sendCommand({"q": self._q})
-        if not self.args.real and not self.args.start_from_current_pose:
-            self._q = pin.randomConfiguration(
-                self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
-            )
-        if self.args.real:
-            self._q = np.array(self._rtde_receive.getActualQ())
+        self._q = np.array(self._rtde_receive.getActualQ())
 
     def connectToRobot(self):
         # NOTE: you can't connect twice, so you can't have more than one RobotManager per robot.
@@ -124,7 +123,7 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
         # NOTE: the force/torque sensor just has large offsets for no reason,
         # and you need to minus them to have usable readings.
         # we provide this with calibrateFT
-        #self.wrench_offset = self.calibrateFT(self._dt)
+        # self.wrench_offset = self.calibrateFT(self._dt)
         self.calibrateFT(self._dt)
 
     def setSpeedSlider(self, value):
-- 
GitLab