commit before merging onto main

.gitignore

@@ -9,3 +9,7 @@ build/
 .vscode/
 **.pickle
 **.csv
+**.toc
+**.synctex.gz
+**.log
+**.aux

examples/cart_pulling/disjoint_control/demo.py → 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

examples/cart_pulling/disjoint_control/mpc_base_clik_arm_control_loop.py → 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(

examples/drawing/test_by_running.sh

+#!/bin/bash
+# TODO: fill out different cases (w or w/out calibration, w/out picking up marker etc)

examples/test_by_running.sh

-#!/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
-#

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

python/smc/control/controller_templates/path_following_template.py

 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)

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,