From a8012183b264ffbc0c4f4b3eda81edd81963f596 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 3 Mar 2025 23:03:18 +0100
Subject: [PATCH] commit before merging onto main

---
 .gitignore                                    |  4 +
 .../{demo.py => mpc_base_clik_single_arm.py}  |  2 +-
 ... mpc_base_clik_single_arm_control_loop.py} |  5 +-
 examples/drawing/test_by_running.sh           |  2 +
 examples/test_by_running.sh                   | 73 -------------------
 .../generic_control_loop.py                   | 12 ++-
 .../path_following_template.py                |  3 +-
 .../croco_mpc_path_following.py               |  8 +-
 8 files changed, 23 insertions(+), 86 deletions(-)
 rename examples/cart_pulling/disjoint_control/{demo.py => mpc_base_clik_single_arm.py} (97%)
 rename examples/cart_pulling/disjoint_control/{mpc_base_clik_arm_control_loop.py => mpc_base_clik_single_arm_control_loop.py} (97%)
 create mode 100755 examples/drawing/test_by_running.sh
 delete mode 100755 examples/test_by_running.sh

diff --git a/.gitignore b/.gitignore
index a6402f4..b55b3f9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,3 +9,7 @@ build/
 .vscode/
 **.pickle
 **.csv
+**.toc
+**.synctex.gz
+**.log
+**.aux
diff --git a/examples/cart_pulling/disjoint_control/demo.py b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
similarity index 97%
rename from examples/cart_pulling/disjoint_control/demo.py
rename to examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
index ba2f6bd..4852ebc 100644
--- a/examples/cart_pulling/disjoint_control/demo.py
+++ b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
@@ -7,7 +7,7 @@ from smc.control.cartesian_space import getClikArgs
 from smc.path_generation.planner import starPlanner, getPlanningArgs
 from smc.control.optimal_control.croco_mpc_point_to_point import CrocoEEAndBaseP2PMPC
 from smc.multiprocessing import ProcessManager
-from mpc_base_clik_arm_control_loop import BaseMPCANDEECLIKCartPulling
+from mpc_base_clik_single_arm_control_loop import BaseMPCANDEECLIKCartPulling
 
 import time
 import numpy as np
diff --git a/examples/cart_pulling/disjoint_control/mpc_base_clik_arm_control_loop.py b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm_control_loop.py
similarity index 97%
rename from examples/cart_pulling/disjoint_control/mpc_base_clik_arm_control_loop.py
rename to examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm_control_loop.py
index ff8d908..6f2d7e9 100644
--- a/examples/cart_pulling/disjoint_control/mpc_base_clik_arm_control_loop.py
+++ b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm_control_loop.py
@@ -34,7 +34,7 @@ def BaseMPCEECLIKPathFollowingFromPlannerMPCControlLoop(
     robot: SingleArmWholeBodyInterface,
     t: int,
     past_data: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray]]:
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
 
     robot._mode = SingleArmWholeBodyInterface.control_mode.whole_body
     p = robot.T_w_b.translation[:2]
@@ -67,14 +67,13 @@ def BaseMPCEECLIKPathFollowingFromPlannerMPCControlLoop(
             robot.visualizer_manager.sendCommand({"path": path_base})
             robot.visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
 
-    embed()
     err_vector_ee = log6(robot.T_w_e.actInv(pathSE3_handlebar[0]))
     err_vector_base = np.linalg.norm(p - path_base[0][:2])  # z axis is irrelevant
     log_item = {}
     log_item["err_vec_ee"] = err_vector_ee
     log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
     log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
-    return v_cmd, log_item
+    return v_cmd, {"path2D_untimed": p}, log_item
 
 
 def BaseMPCANDEECLIKCartPulling(
diff --git a/examples/drawing/test_by_running.sh b/examples/drawing/test_by_running.sh
new file mode 100755
index 0000000..36f3c00
--- /dev/null
+++ b/examples/drawing/test_by_running.sh
@@ -0,0 +1,2 @@
+#!/bin/bash
+# TODO: fill out different cases (w or w/out calibration, w/out picking up marker etc)
diff --git a/examples/test_by_running.sh b/examples/test_by_running.sh
deleted file mode 100755
index dfe4fde..0000000
--- a/examples/test_by_running.sh
+++ /dev/null
@@ -1,73 +0,0 @@
-#!/bin/bash
-# the idea here is to run all the runnable things
-# and test for syntax errors 
-
-############
-#  camera  #
-############
-# the only one where you check plotter
-runnable="./vision/camera_no_lag.py --max-iterations=1500 --no-visualize-manipulator"
-echo $runnable
-python $runnable
-echo "=========================================================="
-
-###################
-#  classic cliks  #
-###################
-# the only one where you check visualizer
-# damped pseudoinverse arm
-runnable="./cartesian_space/clik_point_to_point.py --randomly-generate-goal"
-echo $runnable
-python $runnable
-
-# damped pseudoinverse whole body mobile
-runnable="./cartesian_space/clik_point_to_point.py --robot=heron --randomly-generate-goal --fast-simulation --no-visualize-manipulator --no-real-time-plotting --max-iterations=2000"
-echo $runnable
-python $runnable
-
-# QP arm
-runnable="./cartesian_space/clik_point_to_point.py --randomly-generate-goal --clik-controller=invKinmQP --fast-simulation --no-visualize-manipulator --no-real-time-plotting --max-iterations=2000"
-echo $runnable
-python $runnable
-
-# QP whole body mobile
-runnable="./cartesian_space/clik_point_to_point.py --robot=heron --randomly-generate-goal --clik-controller=invKinmQP --fast-simulation --no-visualize-manipulator --no-real-time-plotting --max-iterations=2000"
-echo $runnable
-python $runnable
-
-# cart pulling mpc
-runnable="cart_pulling.py --max-solver-iter=10 --n-knots=30 --robot=heron --past-window-size=200"
-echo $runnable
-python $runnable
-
-
-#python cart_pulling.py
-#python casadi_ocp_collision_avoidance.py
-#python challenge_main.py
-#python comparing_logs_example.py
-#python crocoddyl_mpc.py
-#python crocoddyl_ocp_./cartesian_space/clik_point_to_point.py
-#python data
-#python drawing_from_input_drawing.py
-#python force_control_test.py
-#python graz
-#python heron_pls.py
-#python joint_trajectory.csv
-#python log_analysis_example.py
-#python old_or_experimental
-#python path_following_mpc.py
-#python path_in_pixels.csv
-#python pin_contact3d.py
-#python plane_pose.pickle
-#python plane_pose.pickle_save
-#python plane_pose_sim.pickle
-#python point_force_control.py
-#python point_impedance_control.py
-#python pushing_via_friction_cones.py
-#python __pycache__
-#python ros2_./cartesian_space/clik_point_to_point.py
-#python smc_node.py
-#python test_by_running.sh
-#python test_crocoddyl_opt_ctrl.py
-#python wiping_path.csv_save
-#
diff --git a/python/smc/control/controller_templates/generic_control_loop.py b/python/smc/control/controller_templates/generic_control_loop.py
index 5422547..268a0f9 100644
--- a/python/smc/control/controller_templates/generic_control_loop.py
+++ b/python/smc/control/controller_templates/generic_control_loop.py
@@ -7,12 +7,14 @@ from collections import deque
 
 global control_loop_return
 control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
+global inner_loop_return
+inner_loop_return = tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]
 
 
 def GenericControlLoop(
     X: Any,
     control_loop: Callable[
-        [Any, Namespace, AbstractRobotManager, float, dict[str, np.ndarray]],
+        [Any, Namespace, AbstractRobotManager, int, dict[str, deque[np.ndarray]]],
         control_loop_return,
     ],
     args: Namespace,
@@ -24,6 +26,10 @@ def GenericControlLoop(
     log_item = {}
     save_past_item = {}
 
-    control_loop(X, args, robot, t, past_data)
+    v_cmd, past_item_inner, log_item_inner = control_loop(X, args, robot, t, past_data)
 
-    return breakFlag, log_item, save_past_item
+    robot.sendVelocityCommand(v_cmd)
+
+    log_item.update(log_item_inner)
+    save_past_item.update(past_item_inner)
+    return breakFlag, save_past_item, log_item
diff --git a/python/smc/control/controller_templates/path_following_template.py b/python/smc/control/controller_templates/path_following_template.py
index 4df88cd..b33d4ec 100644
--- a/python/smc/control/controller_templates/path_following_template.py
+++ b/python/smc/control/controller_templates/path_following_template.py
@@ -1,7 +1,6 @@
 from smc.robots.abstract_robotmanager import AbstractRobotManager
 from smc.multiprocessing.process_manager import ProcessManager
 
-from pinocchio import SE3, log6
 from argparse import Namespace
 from typing import Any, Callable
 import numpy as np
@@ -58,7 +57,7 @@ def PathFollowingFromPlannerControlLoop(
     _, path2D_untimed = data
     path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
 
-    v_cmd, log_item_inner, past_item_inner = control_loop(
+    v_cmd, past_item_inner, log_item_inner = control_loop(
         SOLVER, path2D_untimed, args, robot, t, past_data
     )
     robot.sendVelocityCommand(v_cmd)
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 cf890f7..7efc59a 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -56,7 +56,7 @@ def CrocoBasePathFollowingFromPlannerMPCControlLoop(
     err_vector_base = np.linalg.norm(p - path_base[0][:2])  # z axis is irrelevant
     log_item = {}
     log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
-    return v_cmd, log_item, {}
+    return v_cmd, {}, log_item
 
 
 def CrocoBasePathFollowingMPC(
@@ -185,7 +185,7 @@ def CrocoEEPathFollowingFromPlannerMPCControlLoop(
     err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0])).vector
     log_item = {"err_vec_ee": err_vector_ee}
 
-    return v_cmd, log_item, {}
+    return v_cmd, {}, log_item
 
 
 def CrocoEEPathFollowingMPC(
@@ -316,7 +316,7 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
     log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
     log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
     save_past_item = {"path2D_untimed": p}
-    return v_cmd, log_item, save_past_item
+    return v_cmd, save_past_item, log_item
 
 
 def initializePastData(
@@ -444,7 +444,7 @@ def BaseAndDualEEPathFollowingMPCControlLoop(
     log_item["err_norm_ee_r"] = err_norm_ee_r.reshape((1,))
     log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
     save_past_item = {"path2D_untimed": p}
-    return v_cmd, log_item, save_past_item
+    return v_cmd, save_past_item, log_item
 
 def BaseAndDualEEPathFollowingMPC(
     args: Namespace,
-- 
GitLab