diff --git a/docs/debugging_tips.md b/docs/debugging_tips.md
new file mode 100644
index 0000000000000000000000000000000000000000..afb7d5533c85fcfc989f69c0e7b2a51e0f9053e1
--- /dev/null
+++ b/docs/debugging_tips.md
@@ -0,0 +1,5 @@
+debuging tips
+-------------
+1. log (thereby also plotting) the interesting quantities, like error norms
+2. import IPython.embed, and just drop in embed() whereever in the code - this drops you into an ipython shell with all the variables there.
+ this is the most pain-free way to quickly look at what's going on
diff --git a/examples/cart_pulling/base_and_ee_path_following.py b/examples/cart_pulling/control_sub_problems/fixed_paths/base_and_ee_path_following.py
similarity index 100%
rename from examples/cart_pulling/base_and_ee_path_following.py
rename to examples/cart_pulling/control_sub_problems/fixed_paths/base_and_ee_path_following.py
diff --git a/examples/cart_pulling/dual_arm_path_following.py b/examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py
similarity index 100%
rename from examples/cart_pulling/dual_arm_path_following.py
rename to examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py
diff --git a/examples/cart_pulling/control_sub_problems/fixed_paths/ee_only_path_following.py b/examples/cart_pulling/control_sub_problems/fixed_paths/ee_only_path_following.py
new file mode 100644
index 0000000000000000000000000000000000000000..2e11a54104211e46764924edde04603c624414d5
--- /dev/null
+++ b/examples/cart_pulling/control_sub_problems/fixed_paths/ee_only_path_following.py
@@ -0,0 +1,53 @@
+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_path_following import (
+ CrocoEEPathFollowingMPC,
+)
+import numpy as np
+
+
+def path(T_w_e, i):
+ # 2) do T_mobile_base_ee_pos to get
+ # end-effector reference frame(s)
+
+ # generate bullshit just to see it works
+ path = []
+ t = i * robot.dt
+ for i in range(args.n_knots):
+ t += 0.01
+ new = T_w_e.copy()
+ translation = 2 * np.array([np.cos(t / 20), np.sin(t / 20), 0.3])
+ new.translation = translation
+ path.append(new)
+ return path
+
+
+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)
+ x0 = np.concatenate([robot.q, robot.v])
+ robot._step()
+
+ CrocoEEPathFollowingMPC(args, robot, x0, path)
+
+ print("final position:", robot.T_w_e)
+
+ 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/cart_pulling/base_and_ee_path_following_from_planner.py b/examples/cart_pulling/control_sub_problems/with_planner/base_and_ee_path_following_from_planner.py
similarity index 100%
rename from examples/cart_pulling/base_and_ee_path_following_from_planner.py
rename to examples/cart_pulling/control_sub_problems/with_planner/base_and_ee_path_following_from_planner.py
diff --git a/examples/cart_pulling/ee_only_path_following_from_plannner.py b/examples/cart_pulling/control_sub_problems/with_planner/ee_only_path_following_from_plannner.py
similarity index 100%
rename from examples/cart_pulling/ee_only_path_following_from_plannner.py
rename to examples/cart_pulling/control_sub_problems/with_planner/ee_only_path_following_from_plannner.py
diff --git a/examples/cart_pulling/disjoint_control/; b/examples/cart_pulling/disjoint_control/;
new file mode 100644
index 0000000000000000000000000000000000000000..b244f1397da7be08cc7d5a7ed068d1e3af2e1a95
--- /dev/null
+++ b/examples/cart_pulling/disjoint_control/;
@@ -0,0 +1,268 @@
+from smc.robots.implementations import *
+
+import numpy as np
+import pinocchio as pin
+import argparse
+
+
+def getMinimalArgParser():
+ """
+ getDefaultEssentialArgs
+ ------------------------
+ returns a parser containing:
+ - essential arguments (run in real or in sim)
+ - parameters for (compliant)moveJ
+ - parameters for (compliant)moveL
+ """
+ parser = argparse.ArgumentParser(
+ description="Run something with \
+ Simple Manipulator Control",
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter,
+ )
+ #################################################
+ # general arguments: connection, plotting etc #
+ #################################################
+ parser.add_argument(
+ "--robot",
+ type=str,
+ help="which robot you're running or simulating",
+ default="ur5e",
+ choices=["ur5e", "heron", "heronros", "gripperlessur5e", "mirros", "yumi"],
+ )
+ parser.add_argument(
+ "--real",
+ action=argparse.BooleanOptionalAction,
+ help="whether you're running on the real robot or not",
+ default=False,
+ )
+ # if this ends up working, replace --real with --mode, which can be {real, integration (simulation), debugging}
+ parser.add_argument(
+ "--robot-ip",
+ type=str,
+ help="robot's ip address (only needed if running on the real robot)",
+ default="192.168.1.102",
+ )
+ parser.add_argument(
+ "--ctrl-freq",
+ type=int,
+ help="frequency of the control loop. select -1 if you want to go as fast as possible (useful for running tests in sim)",
+ default=500,
+ )
+ parser.add_argument(
+ "--visualizer",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to visualize the manipulator and workspace with meshcat",
+ default=True,
+ )
+ parser.add_argument(
+ "--viz-update-rate",
+ type=int,
+ help="frequency of visual updates. visualizer and plotter update every viz-update-rate^th iteration of the control loop.",
+ default=20,
+ )
+ parser.add_argument(
+ "--plotter",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)",
+ default=True,
+ )
+ parser.add_argument(
+ "--gripper",
+ type=str,
+ help="gripper you're using (no gripper is the default)",
+ default="none",
+ choices=["none", "robotiq", "onrobot"],
+ )
+ # TODO: make controlloop manager run in a while True loop and remove this
+ # ==> max-iterations actually needs to be an option. sometimes you want to simulate and exit
+ # if the convergence does not happen in a reasonable amount of time,
+ # ex. goal outside of workspace has been passed or something
+ # =======> if it's set to 0 then the loops run infinitely long
+ parser.add_argument(
+ "--max-iterations",
+ type=int,
+ help="maximum allowable iteration number (it runs at 500Hz)",
+ default=100000,
+ )
+ parser.add_argument(
+ "--start-from-current-pose",
+ action=argparse.BooleanOptionalAction,
+ help="if connected to the robot, read the current pose and set it as the initial pose for the robot. \
+ very useful and convenient when running simulation before running on real",
+ default=False,
+ )
+ parser.add_argument(
+ "--acceleration",
+ type=float,
+ help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.3. \
+ BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
+ TODO: check what this means",
+ default=0.3,
+ )
+ parser.add_argument(
+ "--max-v-percentage",
+ type=float,
+ help="select the percentage of the maximum joint velocity the robot can achieve to be the control input maximum (control inputs are clipped to perc * max_v)",
+ default=0.3,
+ )
+ parser.add_argument(
+ "--debug-prints",
+ action=argparse.BooleanOptionalAction,
+ help="print some debug info",
+ default=False,
+ )
+ parser.add_argument(
+ "--save-log",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to save the log of the run. it saves \
+ what you pass to ControlLoopManager. check other parameters for saving directory and log name.",
+ default=False,
+ )
+ parser.add_argument(
+ "--save-dir",
+ type=str,
+ help="path to where you store your logs. default is ./data, but if that directory doesn't exist, then /tmp/data is created and used.",
+ default="./data",
+ )
+ parser.add_argument(
+ "--run-name",
+ type=str,
+ help="name the whole run/experiment (name of log file). note that indexing of runs is automatic and under a different argument.",
+ default="latest_run",
+ )
+ parser.add_argument(
+ "--index-runs",
+ action=argparse.BooleanOptionalAction,
+ help="if you want more runs of the same name, this option will automatically assign an index to a new run (useful for data collection).",
+ default=False,
+ )
+ parser.add_argument(
+ "--past-window-size",
+ type=int,
+ help="how many timesteps of past data you want to save",
+ default=5,
+ )
+ # maybe you want to scale the control signal (TODO: NOT HERE)
+ parser.add_argument(
+ "--controller-speed-scaling",
+ type=float,
+ default="1.0",
+ help="not actually_used atm",
+ )
+ ########################################
+ # environment interaction parameters #
+ ########################################
+ parser.add_argument(
+ "--contact-detecting-force",
+ type=float, # default=1.3, help='the force used to detect contact (collision) in the moveUntilContact function')
+ default=2.8,
+ help="the force used to detect contact (collision) in the moveUntilContact function",
+ )
+ parser.add_argument(
+ "--minimum-detectable-force-norm",
+ type=float,
+ help="we need to disregard noise to converge despite filtering. \
+ a quick fix is to zero all forces of norm below this argument threshold.",
+ default=3.0,
+ )
+ # TODO make this work without parsing (or make it possible to parse two times)
+ # if (args.gripper != "none") and args.simulation:
+ # raise NotImplementedError('Did not figure out how to put the gripper in \
+ # the simulation yet, sorry :/ . You can have only 1 these flags right now')
+ parser.add_argument(
+ "--visualize-collision-approximation",
+ action=argparse.BooleanOptionalAction,
+ help="whether you want to visualize the collision approximation used in controllers with obstacle avoidance",
+ default=False,
+ )
+ return parser
+
+
+# TODO: make robot-independent
+def defineGoalPointCLI(robot):
+ """
+ defineGoalPointCLI
+ ------------------
+ get a nice TUI-type prompt to put in a frame goal for p2p motion.
+ --> best way to handle the goal is to tell the user where the gripper is
+ in both UR tcp frame and with pinocchio and have them
+ manually input it when running.
+ this way you force the thinking before the moving,
+ but you also get to view and analyze the information first
+ TODO get the visual thing you did in ivc project with sliders also.
+ it's just text input for now because it's totally usable, just not superb.
+ but also you do want to have both options. obviously you go for the sliders
+ in the case you're visualizing, makes no sense otherwise.
+ """
+ robot._step()
+ q = robot.q
+ # define goal
+ T_w_e = robot.T_w_e
+ print("You can only specify the translation right now.")
+ if robot.args.real:
+ print(
+ "In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles"
+ )
+ print(
+ "Here's where the robot is currently. Ensure you know what the base frame is first."
+ )
+ print(
+ "base frame end-effector pose from pinocchio:\n",
+ *robot.data.oMi[6].translation.round(4),
+ *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4)
+ )
+ print("UR5e TCP:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
+ # remain with the current orientation
+ # TODO: add something, probably rpy for orientation because it's the least number
+ # of numbers you need to type in
+ Mgoal = T_w_e.copy()
+ # this is a reasonable way to do it too, maybe implement it later
+ # Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
+ # do a while loop until this is parsed correctly
+ while True:
+ goal = input(
+ "Please enter the target end-effector position in the x.x,y.y,z.z format: "
+ )
+ try:
+ e = "ok"
+ goal_list = goal.split(",")
+ for i in range(len(goal_list)):
+ goal_list[i] = float(goal_list[i])
+ except:
+ e = exc_info()
+ print("The input is not in the expected format. Try again.")
+ print(e)
+ if e == "ok":
+ Mgoal.translation = np.array(goal_list)
+ break
+ print("this is goal pose you defined:\n", Mgoal)
+
+ # NOTE i'm not deepcopying this on purpose
+ # but that might be the preferred thing, we'll see
+ robot.Mgoal = Mgoal
+ if robot.args.visualizer:
+ # TODO document this somewhere
+ robot.visualizer_manager.sendCommand({"Mgoal": Mgoal})
+ return Mgoal
+
+
+# TODO: finish
+def getRobotFromArgs(args: argparse.Namespace) -> AbstractRobotManager:
+ if args.robot == "ur5e":
+ if args.real:
+ return RealUR5eRobotManager(args)
+ else:
+ return SimulatedUR5eRobotManager(args)
+ if args.robot == "heron":
+ if args.real:
+ pass
+ # TODO: finish it
+ # return RealHeronRobotManager(args)
+ else:
+ return SimulatedHeronRobotManager(args)
+
+
+# if args.robot == "mir":
+# return RealUR5eRobotManager(args)
+# if args.robot == "yumi":
+# return RealUR5eRobotManager(args)
diff --git a/examples/cart_pulling/disjoint_control/mpc_base_clik_arm_control_loop.py b/examples/cart_pulling/disjoint_control/mpc_base_clik_arm_control_loop.py
index 7ac104519d83e02ea389ea62cde4357b2f47bbc8..86100d8a434f7e0a7161ef64b52d22249538989d 100644
--- a/examples/cart_pulling/disjoint_control/mpc_base_clik_arm_control_loop.py
+++ b/examples/cart_pulling/disjoint_control/mpc_base_clik_arm_control_loop.py
@@ -24,6 +24,7 @@ import types
from argparse import Namespace
from pinocchio import SE3, log6
from collections import deque
+from IPython import embed
def BaseMPCEECLIKPathFollowingFromPlannerMPCControlLoop(
@@ -62,10 +63,11 @@ def BaseMPCEECLIKPathFollowingFromPlannerMPCControlLoop(
v_cmd[3:] = v_arm
if args.visualizer:
- if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ if t % args.viz_update_rate == 0:
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 = {}
diff --git a/examples/cart_pulling/starify_nav2_map.py b/examples/cart_pulling/map_tests/starify_nav2_map.py
similarity index 100%
rename from examples/cart_pulling/starify_nav2_map.py
rename to examples/cart_pulling/map_tests/starify_nav2_map.py
diff --git a/examples/cart_pulling/path6d_from_path2d.py b/examples/cart_pulling/path_construction_tests/path6d_from_path2d.py
similarity index 100%
rename from examples/cart_pulling/path6d_from_path2d.py
rename to examples/cart_pulling/path_construction_tests/path6d_from_path2d.py
diff --git a/examples/cart_pulling/pose_initialization.py b/examples/cart_pulling/path_construction_tests/pose_initialization.py
similarity index 100%
rename from examples/cart_pulling/pose_initialization.py
rename to examples/cart_pulling/path_construction_tests/pose_initialization.py
diff --git a/examples/cart_pulling/technical_report/cart_pulling.aux b/examples/cart_pulling/technical_report/cart_pulling.aux
new file mode 100644
index 0000000000000000000000000000000000000000..932fa4c6bc095b427310eb7b1ab8ed3f133d8324
--- /dev/null
+++ b/examples/cart_pulling/technical_report/cart_pulling.aux
@@ -0,0 +1,22 @@
+\relax
+\@writefile{toc}{\contentsline {section}{\numberline {1}Problem description}{1}{}\protected@file@percent }
+\newlabel{sec-problem-description}{{1}{1}{}{}{}}
+\@writefile{toc}{\contentsline {section}{\numberline {2}Solution approach}{2}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsection}{\numberline {2.1}Formal task description}{2}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsection}{\numberline {2.2}System architecture}{2}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {section}{\numberline {3}Control design}{2}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsection}{\numberline {3.1}Optimal control formulation}{2}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsubsection}{\numberline {3.1.1}Turning OCPs into MPCs}{3}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsubsection}{\numberline {3.1.2}Common OCP parts}{3}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsubsection}{\numberline {3.1.3}Decision variables, dynamics contraint}{4}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsubsection}{\numberline {3.1.4}State constraints}{4}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsubsection}{\numberline {3.1.5}Regulation costs (regularization)}{4}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsection}{\numberline {3.2}Task-defining objective functions}{4}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsubsection}{\numberline {3.2.1}Point-to-point tasks}{5}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {paragraph}{End-effector pose task}{5}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {paragraph}{End-effector pose and base position}{5}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsubsection}{\numberline {3.2.2}Path-following tasks}{5}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsubsection}{\numberline {3.2.3}Solver choice}{6}{}\protected@file@percent }
+\@writefile{toc}{\contentsline {subsection}{\numberline {3.3}Reference trajectory construction}{6}{}\protected@file@percent }
+\newlabel{sub-reference-traj-construction}{{3.3}{6}{}{}{}}
+\gdef \@abspage@last{6}
diff --git a/examples/cart_pulling/technical_report/cart_pulling.log b/examples/cart_pulling/technical_report/cart_pulling.log
new file mode 100644
index 0000000000000000000000000000000000000000..c5967794efb1f4ba92483b5a6995f03abad9e627
--- /dev/null
+++ b/examples/cart_pulling/technical_report/cart_pulling.log
@@ -0,0 +1,289 @@
+This is pdfTeX, Version 3.141592653-2.6-1.40.26 (TeX Live 2024/Arch Linux) (preloaded format=pdflatex 2025.2.22) 24 FEB 2025 17:23
+entering extended mode
+ \write18 enabled.
+ %&-line parsing enabled.
+**cart_pulling.tex
+(./cart_pulling.tex
+LaTeX2e <2023-11-01> patch level 1
+L3 programming layer <2024-02-20>
+(/usr/share/texmf-dist/tex/latex/base/article.cls
+Document Class: article 2023/05/17 v1.4n Standard LaTeX document class
+(/usr/share/texmf-dist/tex/latex/base/size10.clo
+File: size10.clo 2023/05/17 v1.4n Standard LaTeX file (size option)
+)
+\c@part=\count188
+\c@section=\count189
+\c@subsection=\count190
+\c@subsubsection=\count191
+\c@paragraph=\count192
+\c@subparagraph=\count193
+\c@figure=\count194
+\c@table=\count195
+\abovecaptionskip=\skip48
+\belowcaptionskip=\skip49
+\bibindent=\dimen140
+)
+(/usr/share/texmf-dist/tex/latex/amsmath/amsmath.sty
+Package: amsmath 2023/05/13 v2.17o AMS math features
+\@mathmargin=\skip50
+
+For additional information on amsmath, use the `?' option.
+(/usr/share/texmf-dist/tex/latex/amsmath/amstext.sty
+Package: amstext 2021/08/26 v2.01 AMS text
+
+(/usr/share/texmf-dist/tex/latex/amsmath/amsgen.sty
+File: amsgen.sty 1999/11/30 v2.0 generic functions
+\@emptytoks=\toks17
+\ex@=\dimen141
+))
+(/usr/share/texmf-dist/tex/latex/amsmath/amsbsy.sty
+Package: amsbsy 1999/11/29 v1.2d Bold Symbols
+\pmbraise@=\dimen142
+)
+(/usr/share/texmf-dist/tex/latex/amsmath/amsopn.sty
+Package: amsopn 2022/04/08 v2.04 operator names
+)
+\inf@bad=\count196
+LaTeX Info: Redefining \frac on input line 234.
+\uproot@=\count197
+\leftroot@=\count198
+LaTeX Info: Redefining \overline on input line 399.
+LaTeX Info: Redefining \colon on input line 410.
+\classnum@=\count199
+\DOTSCASE@=\count266
+LaTeX Info: Redefining \ldots on input line 496.
+LaTeX Info: Redefining \dots on input line 499.
+LaTeX Info: Redefining \cdots on input line 620.
+\Mathstrutbox@=\box51
+\strutbox@=\box52
+LaTeX Info: Redefining \big on input line 722.
+LaTeX Info: Redefining \Big on input line 723.
+LaTeX Info: Redefining \bigg on input line 724.
+LaTeX Info: Redefining \Bigg on input line 725.
+\big@size=\dimen143
+LaTeX Font Info: Redeclaring font encoding OML on input line 743.
+LaTeX Font Info: Redeclaring font encoding OMS on input line 744.
+\macc@depth=\count267
+LaTeX Info: Redefining \bmod on input line 905.
+LaTeX Info: Redefining \pmod on input line 910.
+LaTeX Info: Redefining \smash on input line 940.
+LaTeX Info: Redefining \relbar on input line 970.
+LaTeX Info: Redefining \Relbar on input line 971.
+\c@MaxMatrixCols=\count268
+\dotsspace@=\muskip16
+\c@parentequation=\count269
+\dspbrk@lvl=\count270
+\tag@help=\toks18
+\row@=\count271
+\column@=\count272
+\maxfields@=\count273
+\andhelp@=\toks19
+\eqnshift@=\dimen144
+\alignsep@=\dimen145
+\tagshift@=\dimen146
+\tagwidth@=\dimen147
+\totwidth@=\dimen148
+\lineht@=\dimen149
+\@envbody=\toks20
+\multlinegap=\skip51
+\multlinetaggap=\skip52
+\mathdisplay@stack=\toks21
+LaTeX Info: Redefining \[ on input line 2953.
+LaTeX Info: Redefining \] on input line 2954.
+)
+(/usr/share/texmf-dist/tex/latex/amsfonts/amssymb.sty
+Package: amssymb 2013/01/14 v3.01 AMS font symbols
+
+(/usr/share/texmf-dist/tex/latex/amsfonts/amsfonts.sty
+Package: amsfonts 2013/01/14 v3.01 Basic AMSFonts support
+\symAMSa=\mathgroup4
+\symAMSb=\mathgroup5
+LaTeX Font Info: Redeclaring math symbol \hbar on input line 98.
+LaTeX Font Info: Overwriting math alphabet `\mathfrak' in version `bold'
+(Font) U/euf/m/n --> U/euf/b/n on input line 106.
+))
+(/usr/share/texmf-dist/tex/latex/base/fontenc.sty
+Package: fontenc 2021/04/29 v2.0v Standard LaTeX package
+)
+(/usr/share/texmf-dist/tex/latex/tools/bm.sty
+Package: bm 2023/07/08 v1.2f Bold Symbol Support (DPC/FMi)
+\symboldoperators=\mathgroup6
+\symboldletters=\mathgroup7
+\symboldsymbols=\mathgroup8
+Package bm Info: No bold for \OMX/cmex/m/n, using \pmb.
+Package bm Info: No bold for \U/msa/m/n, using \pmb.
+Package bm Info: No bold for \U/msb/m/n, using \pmb.
+LaTeX Font Info: Redeclaring math alphabet \mathbf on input line 149.
+)
+(/usr/share/texmf-dist/tex/latex/tools/array.sty
+Package: array 2023/10/16 v2.5g Tabular extension package (FMi)
+\col@sep=\dimen150
+\ar@mcellbox=\box53
+\extrarowheight=\dimen151
+\NC@list=\toks22
+\extratabsurround=\skip53
+\backup@length=\skip54
+\ar@cellbox=\box54
+)
+(/usr/share/texmf-dist/tex/latex/graphics/graphicx.sty
+Package: graphicx 2021/09/16 v1.2d Enhanced LaTeX Graphics (DPC,SPQR)
+
+(/usr/share/texmf-dist/tex/latex/graphics/keyval.sty
+Package: keyval 2022/05/29 v1.15 key=value parser (DPC)
+\KV@toks@=\toks23
+)
+(/usr/share/texmf-dist/tex/latex/graphics/graphics.sty
+Package: graphics 2022/03/10 v1.4e Standard LaTeX Graphics (DPC,SPQR)
+
+(/usr/share/texmf-dist/tex/latex/graphics/trig.sty
+Package: trig 2021/08/11 v1.11 sin cos tan (DPC)
+)
+(/usr/share/texmf-dist/tex/latex/graphics-cfg/graphics.cfg
+File: graphics.cfg 2016/06/04 v1.11 sample graphics configuration
+)
+Package graphics Info: Driver file: pdftex.def on input line 107.
+
+(/usr/share/texmf-dist/tex/latex/graphics-def/pdftex.def
+File: pdftex.def 2022/09/22 v1.2b Graphics/color driver for pdftex
+))
+\Gin@req@height=\dimen152
+\Gin@req@width=\dimen153
+)
+(/usr/share/texmf-dist/tex/latex/base/inputenc.sty
+Package: inputenc 2021/02/14 v1.3d Input encoding file
+\inpenc@prehook=\toks24
+\inpenc@posthook=\toks25
+)
+(/usr/share/texmf-dist/tex/latex/l3backend/l3backend-pdftex.def
+File: l3backend-pdftex.def 2024-02-20 L3 backend support: PDF output (pdfTeX)
+\l__color_backend_stack_int=\count274
+\l__pdf_internal_box=\box55
+)
+(./cart_pulling.aux)
+\openout1 = `cart_pulling.aux'.
+
+LaTeX Font Info: Checking defaults for OML/cmm/m/it on input line 17.
+LaTeX Font Info: ... okay on input line 17.
+LaTeX Font Info: Checking defaults for OMS/cmsy/m/n on input line 17.
+LaTeX Font Info: ... okay on input line 17.
+LaTeX Font Info: Checking defaults for OT1/cmr/m/n on input line 17.
+LaTeX Font Info: ... okay on input line 17.
+LaTeX Font Info: Checking defaults for T1/cmr/m/n on input line 17.
+LaTeX Font Info: ... okay on input line 17.
+LaTeX Font Info: Checking defaults for TS1/cmr/m/n on input line 17.
+LaTeX Font Info: ... okay on input line 17.
+LaTeX Font Info: Checking defaults for OMX/cmex/m/n on input line 17.
+LaTeX Font Info: ... okay on input line 17.
+LaTeX Font Info: Checking defaults for U/cmr/m/n on input line 17.
+LaTeX Font Info: ... okay on input line 17.
+ (/usr/share/texmf-dist/tex/context/base/mkii/supp-pdf.mkii
+[Loading MPS to PDF converter (version 2006.09.02).]
+\scratchcounter=\count275
+\scratchdimen=\dimen154
+\scratchbox=\box56
+\nofMPsegments=\count276
+\nofMParguments=\count277
+\everyMPshowfont=\toks26
+\MPscratchCnt=\count278
+\MPscratchDim=\dimen155
+\MPnumerator=\count279
+\makeMPintoPDFobject=\count280
+\everyMPtoPDFconversion=\toks27
+) (/usr/share/texmf-dist/tex/latex/epstopdf-pkg/epstopdf-base.sty
+Package: epstopdf-base 2020-01-24 v2.11 Base part for package epstopdf
+
+(/usr/share/texmf-dist/tex/generic/infwarerr/infwarerr.sty
+Package: infwarerr 2019/12/03 v1.5 Providing info/warning/error messages (HO)
+)
+(/usr/share/texmf-dist/tex/latex/grfext/grfext.sty
+Package: grfext 2019/12/03 v1.3 Manage graphics extensions (HO)
+
+(/usr/share/texmf-dist/tex/generic/kvdefinekeys/kvdefinekeys.sty
+Package: kvdefinekeys 2019-12-19 v1.6 Define keys (HO)
+))
+(/usr/share/texmf-dist/tex/latex/kvoptions/kvoptions.sty
+Package: kvoptions 2022-06-15 v3.15 Key value format for package options (HO)
+
+(/usr/share/texmf-dist/tex/generic/ltxcmds/ltxcmds.sty
+Package: ltxcmds 2023-12-04 v1.26 LaTeX kernel commands for general use (HO)
+)
+(/usr/share/texmf-dist/tex/latex/kvsetkeys/kvsetkeys.sty
+Package: kvsetkeys 2022-10-05 v1.19 Key value parser (HO)
+))
+(/usr/share/texmf-dist/tex/generic/pdftexcmds/pdftexcmds.sty
+Package: pdftexcmds 2020-06-27 v0.33 Utility functions of pdfTeX for LuaTeX (HO
+)
+
+(/usr/share/texmf-dist/tex/generic/iftex/iftex.sty
+Package: iftex 2022/02/03 v1.0f TeX engine tests
+)
+Package pdftexcmds Info: \pdf@primitive is available.
+Package pdftexcmds Info: \pdf@ifprimitive is available.
+Package pdftexcmds Info: \pdfdraftmode found.
+)
+Package epstopdf-base Info: Redefining graphics rule for `.eps' on input line 4
+85.
+Package grfext Info: Graphics extension search list:
+(grfext) [.pdf,.png,.jpg,.mps,.jpeg,.jbig2,.jb2,.PDF,.PNG,.JPG,.JPE
+G,.JBIG2,.JB2,.eps]
+(grfext) \AppendGraphicsExtensions on input line 504.
+
+(/usr/share/texmf-dist/tex/latex/latexconfig/epstopdf-sys.cfg
+File: epstopdf-sys.cfg 2010/07/13 v1.3 Configuration of (r)epstopdf for TeX Liv
+e
+))
+LaTeX Font Info: Trying to load font information for U+msa on input line 19.
+
+
+(/usr/share/texmf-dist/tex/latex/amsfonts/umsa.fd
+File: umsa.fd 2013/01/14 v3.01 AMS symbols A
+)
+LaTeX Font Info: Trying to load font information for U+msb on input line 19.
+
+
+(/usr/share/texmf-dist/tex/latex/amsfonts/umsb.fd
+File: umsb.fd 2013/01/14 v3.01 AMS symbols B
+) (./cart_pulling.toc)
+\tf@toc=\write3
+\openout3 = `cart_pulling.toc'.
+
+
+[1
+
+{/var/lib/texmf/fonts/map/pdftex/updmap/pdftex.map}{/usr/share/texmf-dist/fonts
+/enc/dvips/cm-super/cm-super-t1.enc}] [2{/usr/share/texmf-dist/fonts/enc/dvips/
+cm-super/cm-super-ts1.enc}] [3] [4] [5] [6] (./cart_pulling.aux)
+ ***********
+LaTeX2e <2023-11-01> patch level 1
+L3 programming layer <2024-02-20>
+ ***********
+ )
+Here is how much of TeX's memory you used:
+ 3584 strings out of 476076
+ 51971 string characters out of 5793775
+ 1936187 words of memory out of 5000000
+ 25617 multiletter control sequences out of 15000+600000
+ 578758 words of font info for 92 fonts, out of 8000000 for 9000
+ 14 hyphenation exceptions out of 8191
+ 61i,16n,69p,240b,221s stack positions out of 10000i,1000n,20000p,200000b,200000s
+</usr/share/texmf-dist/fonts/type1/public/amsfonts/cm/cmex10.pfb></usr/share/
+texmf-dist/fonts/type1/public/amsfonts/cm/cmmi10.pfb></usr/share/texmf-dist/fon
+ts/type1/public/amsfonts/cm/cmmi5.pfb></usr/share/texmf-dist/fonts/type1/public
+/amsfonts/cm/cmmi7.pfb></usr/share/texmf-dist/fonts/type1/public/amsfonts/cm/cm
+r10.pfb></usr/share/texmf-dist/fonts/type1/public/amsfonts/cm/cmr5.pfb></usr/sh
+are/texmf-dist/fonts/type1/public/amsfonts/cm/cmr7.pfb></usr/share/texmf-dist/f
+onts/type1/public/amsfonts/cm/cmsy10.pfb></usr/share/texmf-dist/fonts/type1/pub
+lic/amsfonts/cm/cmsy7.pfb></usr/share/texmf-dist/fonts/type1/public/cm-super/sf
+bx0700.pfb></usr/share/texmf-dist/fonts/type1/public/cm-super/sfbx1000.pfb></us
+r/share/texmf-dist/fonts/type1/public/cm-super/sfbx1200.pfb></usr/share/texmf-d
+ist/fonts/type1/public/cm-super/sfbx1440.pfb></usr/share/texmf-dist/fonts/type1
+/public/cm-super/sfrm0700.pfb></usr/share/texmf-dist/fonts/type1/public/cm-supe
+r/sfrm1000.pfb></usr/share/texmf-dist/fonts/type1/public/cm-super/sfrm1200.pfb>
+</usr/share/texmf-dist/fonts/type1/public/cm-super/sfrm1728.pfb>
+Output written on cart_pulling.pdf (6 pages, 249938 bytes).
+PDF statistics:
+ 113 PDF objects out of 1000 (max. 8388607)
+ 69 compressed objects within 1 object stream
+ 0 named destinations out of 1000 (max. 500000)
+ 1 words of extra memory for PDF output out of 10000 (max. 10000000)
+
diff --git a/examples/cart_pulling/technical_report/cart_pulling.pdf b/examples/cart_pulling/technical_report/cart_pulling.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..ef8d310f74a933b2857e3eefe9231e4b92e1be83
Binary files /dev/null and b/examples/cart_pulling/technical_report/cart_pulling.pdf differ
diff --git a/examples/cart_pulling/technical_report/cart_pulling.synctex.gz b/examples/cart_pulling/technical_report/cart_pulling.synctex.gz
new file mode 100644
index 0000000000000000000000000000000000000000..cad3b26b6193f91c6e77cc65ad7363a2175f04ba
Binary files /dev/null and b/examples/cart_pulling/technical_report/cart_pulling.synctex.gz differ
diff --git a/examples/cart_pulling/technical_report/cart_pulling.tex b/examples/cart_pulling/technical_report/cart_pulling.tex
new file mode 100644
index 0000000000000000000000000000000000000000..7658d68ff590c4884aa0d022c6561d78a90e544f
--- /dev/null
+++ b/examples/cart_pulling/technical_report/cart_pulling.tex
@@ -0,0 +1,320 @@
+\documentclass{article}
+\usepackage{amsmath}
+\newcommand{\argmin}{\arg\!\min}
+\newcommand{\argmax}{\arg\!\max}
+\usepackage{amssymb}
+\usepackage{amsfonts}
+\usepackage[T1]{fontenc}
+\usepackage{bm}
+\usepackage{array}
+\usepackage{graphicx}
+\usepackage[utf8]{inputenc}
+
+\title{Cart pulling research notes}
+\date{\today}
+\author{Marko Guberina}
+
+\begin{document}
+\pagenumbering{gobble}
+\maketitle
+\tableofcontents
+\pagenumbering{arabic}
+
+\section{Problem description}
+\label{sec-problem-description}
+We have a mobile robot with hands at our disposal.
+We want the robot to take a cart of things, and pull it from point A to B.
+This is taking place in a crowded environment, so the robot needs to be able
+to detect dynamic obstacles (people) and navigate around them.
+The robot has lidar, a camera, positional encoders on everything, an IMU
+and TODO: [other things we use].
+
+The key aspects of this problem are:
+\begin{enumerate}
+ \item mapping and localization, as for anything mobile
+ \item detecting obstacles
+ \item very quick planning - we can't know where the people will go,
+ so we need to plan quickly (can't rely on old plan which
+ assumes obstacle follows a particular trajectory)
+ \item controller capable of simultaneously following the
+ prescribed path and handling of the cart
+\end{enumerate}
+We tackle each of these problem as follows:
+\begin{enumerate}
+\item Use off-the-shelf ROS2 Nav2 stack which has an implementation
+ of SLAM. We know the environment we're in, and rely on a map made in
+ advance.
+\item Some part of the ROS2 stack on the robot TODO: [know which one]
+ takes the lidar point point cloud, filters out the unmapped
+ detected things as a separate layer on the map. Those can be assumed to be dynamic obstacles.
+\item Use Albin Dahlin's [TODO: citation] dynamic system planning in star-world.
+ Furthermore, use his code implementation. During operation,
+ the planner's own map needs to be updated, as it relies on star-shaped
+ obstacles, and not on a pixel-grid. This should use
+ as much caching as possible, but at least the static part of the map.
+\item The ideal solution seems to be creating a whole-body MPC.
+Optimal control strategies can accommodate
+multiple optimization objectives, and underactuation in the base of the robot.
+An alternative would be to disjointly control the base and the arms.
+As will be explained later, we construct an exact path for the end-effector
+to follow, so as long as it follows it, it doesn't matter whether
+it's full or disjoint body control (as long as we don't have additional criteria
+to on top of the base and the end-effector positions).
+\end{enumerate}
+
+\section{Solution approach}
+
+\subsection{Formal task description}
+\subsection{System architecture}
+TODO: finish
+
+The only communication between the planner and the controller is
+the planner sending the path.
+However, in the current implementation,
+it was easier for the controller to communicate
+the current position to the planner.
+
+\section{Control design}
+\subsection{Optimal control formulation}
+We propose a direct trajectory optimization approach for our conception
+of the cart pulling problem. Furthermore, we solve the problem
+on the dynamics level. We use the Box-FDDP solver [reference]
+from the Crocoddyl optimal control suite [reference].
+This approach has the following benefits:
+\begin{itemize}
+\item control of underactuated mobile bases like differential drive
+\item flexibility in objective function design
+makes the approach extensible, for example allowing to penalization of centrifugal
+forces when handling tall carts, or extending the approach to legged robots
+\end{itemize}
+
+A generic optimal control problem looks like
+%\begin{align*}
+% \label{eq-original-problem}
+%\min_{x (\cdot), u (\cdot), T} &=
+%\int_{0}^{T} L (x (t), u (t)) dt + E (x (T))\\
+%\text{subject to } &
+%x (0) = x_{ 0 } \\
+%& \dot{x} (t) = f (x (t), u (t)) \text{ (dynamics)}\\
+%& h (x (t), u (t)) \geq 0 \text{ (path constraints)}\\
+%& r (x (T)) =0
+%\end{align*}
+
+\begin{align}
+X^{ * }, U^{ * } &= \argmin_{X,U} \sum_{k=1}^{N-1} \int_{t_{ k }}^{t_{ k+1 }} l (x,u) dt +L (x (N)) \\
+\text{such that } & \dot{x} = f (x,u)\\
+& x \in \mathcal{X}, u \in \mathcal{U}\\
+\end{align}
+
+A common choice for $ \Delta t = t_{ k+1 } - t_{ k } $ is
+$ \Delta t = 0.01 $.
+
+
+%We solve the inverse dynamics problem to solve for accelerations
+%$ \tau = M(q)\ddot{q}+C(q,\dot{q})+G(q)$.
+%TODO: what's the reason i can you either again????
+%TODO: what's the reason i can you either again????
+%TODO: what's the reason i can you either again????
+%TODO: what's the reason i can you either again????
+%TODO: what's the reason i can you either again????
+% TODO: try both ,see if there's a difference.
+% inv dyn should be better because no M(q) inversion, but idk
+
+\subsubsection{Turning OCPs into MPCs}
+Any OCP described here is turned into an MPC by:
+\begin{itemize}
+\item \textbf{Shortening the time horizon $ N $, capping the maximum number of
+solver iterations $ n_{ \text{solver\_iter} } $,
+and reducing control frequency $ f_{ \text{ctrl} } $}
+until satisfactory performance is reached.
+From limited qualitative testing, the most common numbers
+are $ N = 30 $, $ n_{ \text{solver\_iter} } = 10 $,
+and $ f_{ \text{ctrl} } = 200 $.
+\item \textbf{Warmstarting} with the previous solution.
+ The previous solution is offset by the amount of time passed.
+ TODO: make sure it is, make sure the ctrl\_freq amount of time,
+ not $ \Delta t $ time. If the second is the case,
+ you probably want to linearly interpolate to offset correctly
+ - also make sure you're time-testing whether this is actually helpful
+ (if it doesn't shave off an iteration off of the solver,
+ it isn't)
+\end{itemize}
+
+\subsubsection{Common OCP parts}
+Depending on the robot used, and the task at hand,
+there are different OCPs being used.
+For example, to initialize the robot's position, we use an objective
+function for point-to-point motions,
+while we use a different one for path tracking.
+
+Due to the complexity of the final controller, and of the total
+control system design (including planning), during the development
+process if was necessary to build smaller controllers
+to increase understanding of the controller, informing
+and guiding the final design.
+
+It's easiest to begin by describing the common parts
+of the all OCPs.
+\subsubsection{Decision variables, dynamics contraint}
+In our case, the state is a vector of joint positions and velocities:
+\begin{equation}
+ x = \begin{bmatrix} q & v \end{bmatrix}^{ T } .
+\end{equation}
+The control inputs can be either acceleration or torques,
+depending on which we formulate either the inverse or the forward dynamics
+problem respectively, and solve for the other quantity.
+We used inverse dynamics.
+In any case, because the states are also decision variables,
+a state-input pair must follow the system dynamics into the next that.
+Thus to put in the discrete version of $ \dot{x}= f (x,u) $, we can write
+\begin{gather}
+ \text{dynamics} ( \delta x_{ k+1 }, \delta x_{ k }, \delta u_{ k } ) =0 \\
+ \text{dynamics} (\delta \dot{x}, \delta x, \delta u)=
+\delta \dot{x} - (f_{ x } \delta x + f_{ u } \delta u)
+\end{gather}
+TODO: elaborate further . Also explain to self whether efforts are torques.
+also TODO: probably a good idea to actually formulate with the forward
+dynamics version too, it's about 10 basic lines of code and an argument.
+
+\subsubsection{State constraints}
+Since we use Box-FDDP, we can only have box constraints on torque inputs,
+i.e. $ \tau_{ \min } \leq \tau \leq \tau_{ \max } $.
+In the case of an underactuated joint, we set the torque limit on it to zero.
+
+To put constraints on the state,
+namely on arms' joint limits and on all joints' velocities,
+we put them into a quadratic barrier function
+which we put into the objective function.
+In particular (TODO make sure this is correct):
+\begin{gather}
+ r_{ lb } = x - x_{ \min } \\
+ r_{ ub } = x - x_{ \max } \\
+ B(x) =
+\left\{
+\begin{array}{ll}
+ \frac{1}{2} (\left\lVert r_{ lb } \right \rVert ^{ 2 }
+ + \left\lVert r_{ ub } \right \rVert ^{ 2 }) & \text{ if }
+ lb \leq x \leq ub \\
+ \max \text{ float} & \text{ otherwise}
+\end{array}
+\right .
+\end{gather}
+
+\subsubsection{Regulation costs (regularization)}
+Finally, we add regulation costs on both the state $ c_{ 1 } x^{ T }x $,
+and control inputs $ c_{ 2 } u^{ T }u $.
+
+\subsection{Task-defining objective functions}
+Depending on the task, we create a different objective function.
+Before going into the functions themselves, the reader
+should be briefed about some specifics of Crocoddyl, the library
+used for optimal control (more detailed text can be found in the appendix
+of ``Robotics notes'').
+
+At every timestep of the OCP, also known as a knot,
+there is an ActionModel. An ActionModel store all data and functions
+about the OCP at this point. This means that an OCP can be constructed
+as a combination of different dynamics models, constraints,
+and objective functions, all defined just at one knot.
+This is used in path-following, where every knot is assigned
+a each own target (in this case a path point )to reach in the objective function.
+Another useful feature is setting different cost coefficients,
+which most useful in setting zero terminal velocities at the final knot.
+Note that the last point is not a good idea in MPC!
+
+\subsubsection{Point-to-point tasks}
+In case there is a fixed end-goal to reach, we use the se3 error
+norm as the task-specific function to put into the objective function.
+\paragraph{End-effector pose task}
+In the particular case of the goal being defined for the end-effector,
+whose pose is denoted as $ T_{ w }^{ e }$ reaching a goal
+denoted as $ T_{ w }^{ \text{goal} }$:
+\begin{equation}
+l_{ e } (x)= \left\lVert
+\log \left(
+ \left(
+T_{ w }^{ e }
+ \right)^{ -1 }
+\right) T_{ w }^{ \text{goal} }
+\right \rVert _{ 2 }
+\end{equation}
+Here we of course use only joint values $ q $, not the full state $ x $,
+because the end-effector pose is only a function of $ q $.
+
+We put this cost function at every knot.
+
+\paragraph{End-effector pose and base position}
+In case we also want to specify the position of the base,
+we additionally put in the translational cost for the base.
+If we denote the pose of the base is
+\begin{equation}
+T_{ w }^{ b } = \begin{bmatrix}
+ R_{ w }^{ b } & p_{ b } \\
+ 0 & 1
+\end{bmatrix}
+\end{equation}
+then the translational cost is given by
+\begin{equation}
+ l_{ b } (x) = \left\lVert p_{ \text{goal} } - p_{ b } \right \rVert
+\end{equation}
+Here it should be noted that the $ z $ coordinate is 0 for
+both the goal and the base pose.
+The orientation of the base could be accounted for in the same manner.
+
+To get the joined task, we simply sum the two goal costs:
+\begin{equation}
+l_{ \text{joined} } (x)= l_{e } (x) + l_{ b } (x)
+\end{equation}
+
+\subsubsection{Path-following tasks}
+As discussed in \ref{sec-problem-description}, the controller is tasked with following
+a path prescribed by a path planner.
+One way, and probably the best way,
+to do this would be to parametrize the path with a parameter $ s \in [0,1] $,
+and task the controller
+with timing it, i.e. constructing a trajectory on it. In this case the
+time-scaling factor of the path $ \lambda $ in the equation
+$ t = \lambda s $ would be a decision variable in the OCP.
+
+However, we found it easier to manually construct a trajectory, i.e. a timing law
+on the path. This is because it is very easy to construct a trajectory following OCP.
+Namely, it is done by modifying the point-to-point formulation by replacing
+$ T_{ w }^{ goal } $ with $ _{ i } T_{ w }^{ traj } $ where
+$ _{ i } T_{ w }^{ traj } $ is the point on the desired trajectory corresponding
+to the desired timing law on the path. More on path and trajectory construction can
+be found in \ref{sub-reference-traj-construction}.
+
+TODO write the function
+
+\subsubsection{Solver choice}
+Box-FDDP from Crocoddy seems to be the fastest,
+and provides control input constraints
+which is crucial - otherwise one would need to overtune control input regulation,
+leading to a steep decrease in performance.
+
+CSQP from mim-solvers was also tested, but it was too slow (NOTE: idk
+if I set it up right, the paper said it should be faster than what I got).
+The benefit it has is that it can accommodate other linear constraints
+(it's based on sequential quadratic programming (SQP), made faster
+for trajectory optimization through it's implementation).
+
+Casadi' IPOPT is a good choice for experimenting
+with different problem formulations,
+but being a generic optimization solver, and all the derivatives
+being obtained through finite differencing, it is of course too slow
+for practical implementation.
+
+
+\subsection{Reference trajectory construction}
+\label{sub-reference-traj-construction}
+The path planner produces a plan for the base of the robot.
+
+
+
+
+
+
+
+
+
+\end{document}
diff --git a/examples/cart_pulling/technical_report/cart_pulling.toc b/examples/cart_pulling/technical_report/cart_pulling.toc
new file mode 100644
index 0000000000000000000000000000000000000000..10fd76c26af60284e20ff24197e847d5eb7b1058
--- /dev/null
+++ b/examples/cart_pulling/technical_report/cart_pulling.toc
@@ -0,0 +1,18 @@
+\contentsline {section}{\numberline {1}Problem description}{1}{}%
+\contentsline {section}{\numberline {2}Solution approach}{2}{}%
+\contentsline {subsection}{\numberline {2.1}Formal task description}{2}{}%
+\contentsline {subsection}{\numberline {2.2}System architecture}{2}{}%
+\contentsline {section}{\numberline {3}Control design}{2}{}%
+\contentsline {subsection}{\numberline {3.1}Optimal control formulation}{2}{}%
+\contentsline {subsubsection}{\numberline {3.1.1}Turning OCPs into MPCs}{3}{}%
+\contentsline {subsubsection}{\numberline {3.1.2}Common OCP parts}{3}{}%
+\contentsline {subsubsection}{\numberline {3.1.3}Decision variables, dynamics contraint}{4}{}%
+\contentsline {subsubsection}{\numberline {3.1.4}State constraints}{4}{}%
+\contentsline {subsubsection}{\numberline {3.1.5}Regulation costs (regularization)}{4}{}%
+\contentsline {subsection}{\numberline {3.2}Task-defining objective functions}{4}{}%
+\contentsline {subsubsection}{\numberline {3.2.1}Point-to-point tasks}{5}{}%
+\contentsline {paragraph}{End-effector pose task}{5}{}%
+\contentsline {paragraph}{End-effector pose and base position}{5}{}%
+\contentsline {subsubsection}{\numberline {3.2.2}Path-following tasks}{5}{}%
+\contentsline {subsubsection}{\numberline {3.2.3}Solver choice}{6}{}%
+\contentsline {subsection}{\numberline {3.3}Reference trajectory construction}{6}{}%
diff --git a/examples/log_analysis/comparing_logs_example.py b/examples/log_analysis/comparing_logs_example.py
index f646df854589f3b3b5bdfb6d3964c840563d28e8..06b15a8317a9f7a3701fc32ac6f938227b3e463f 100644
--- a/examples/log_analysis/comparing_logs_example.py
+++ b/examples/log_analysis/comparing_logs_example.py
@@ -1,4 +1,4 @@
-from smc.visualize.manipulator_comparison_visualizer import (
+from smc.visualization.manipulator_comparison_visualizer import (
getLogComparisonArgs,
ManipulatorComparisonManager,
)
diff --git a/python/smc/control/control_loop_manager.py b/python/smc/control/control_loop_manager.py
index 9ad9ef4557e9442ab7d1d9fcd9009b4124a0dc31..6b92aabf90dfc76c978df72a0f3a1646af65216b 100644
--- a/python/smc/control/control_loop_manager.py
+++ b/python/smc/control/control_loop_manager.py
@@ -2,8 +2,8 @@ from argparse import Namespace
from smc.robots.robotmanager_abstract import AbstractRobotManager
from smc.multiprocessing.process_manager import ProcessManager
from smc.visualization.plotters import realTimePlotter
-from functools import partial
+from functools import partial
import signal
import time
import numpy as np
@@ -74,7 +74,10 @@ class ControlLoopManager:
self.past_data: dict[str, deque[np.ndarray]] = {}
# NOTE: viz update rate is a magic number that seems to work fine and i don't have
# any plans to make it smarter
- self.viz_update_rate: int = int(np.ceil(self.args.ctrl_freq / 25))
+ if args.viz_update_rate < 0:
+ self.viz_update_rate: int = int(np.ceil(self.args.ctrl_freq / 25))
+ else:
+ self.viz_update_rate: int = args.viz_update_rate
# save_past_dict has to have the key and 1 example of what you're saving
# so that it's type can be inferred (but we're in python so types don't really work).
# the good thing is this way you also immediatelly put in the initial values
diff --git a/python/smc/logging/logger.py b/python/smc/logging/logger.py
index 39b875764c38063dde88df1bb584d9e72b9e9344..a8546a96d94486e49c08bb6b53bb3182c1e9c7f3 100644
--- a/python/smc/logging/logger.py
+++ b/python/smc/logging/logger.py
@@ -94,6 +94,11 @@ class Logger:
(including the data and the args).
Uses default (un)pickling.
"""
+ if save_file_path is None:
+ print(
+ "you need to provide a path to the log file you want to analyze with --log-file1 [path]. exiting"
+ )
+ exit()
if os.path.exists(save_file_path):
log_file = open(save_file_path, "rb")
tmp_dict = pickle.load(log_file)
diff --git a/python/smc/multiprocessing/process_manager.py b/python/smc/multiprocessing/process_manager.py
index 315587b082242461a417c6722b4c2571aed17309..4615bbe2febd8afbf14baa27b2bfed1e7b05e4c2 100644
--- a/python/smc/multiprocessing/process_manager.py
+++ b/python/smc/multiprocessing/process_manager.py
@@ -86,9 +86,17 @@ class ProcessManager:
# "sending" the command via shared memory
# TODO: the name should be random and send over as function argument
shm_name = "command"
- self.shm_cmd = shared_memory.SharedMemory(
- shm_name, create=True, size=init_command.nbytes
- )
+ # NOTE: if we didn't close properly it will just linger on.
+ # since there is no exist_ok argument for SharedMemory, we catch the error on the fly here
+ try:
+ self.shm_cmd = shared_memory.SharedMemory(
+ shm_name, create=True, size=init_command.nbytes
+ )
+ except FileExistsError:
+ self.shm_cmd = shared_memory.SharedMemory(
+ shm_name, create=False, size=init_command.nbytes
+ )
+
self.shared_command = np.ndarray(
init_command.shape, dtype=init_command.dtype, buffer=self.shm_cmd.buf
)
@@ -98,9 +106,14 @@ class ProcessManager:
# getting data via different shared memory
shm_data_name = "data"
# size is chosen arbitrarily but 10k should be more than enough for anything really
- self.shm_data = shared_memory.SharedMemory(
- shm_data_name, create=True, size=10000
- )
+ try:
+ self.shm_data = shared_memory.SharedMemory(
+ shm_data_name, create=True, size=10000
+ )
+ except FileExistsError:
+ self.shm_data = shared_memory.SharedMemory(
+ shm_data_name, create=False, size=10000
+ )
# initialize empty
p = pickle.dumps(None)
self.shm_data.buf[: len(p)] = p
diff --git a/python/smc/path_generation/path_math/path_to_trajectory.py b/python/smc/path_generation/path_math/path_to_trajectory.py
index f07958693afdf1788eb9baa7651bd4d659b38a1a..7c0686e8e3a73a617da03685a79c75eab66d8547 100644
--- a/python/smc/path_generation/path_math/path_to_trajectory.py
+++ b/python/smc/path_generation/path_math/path_to_trajectory.py
@@ -64,6 +64,8 @@ def path2D_timed(args, path2D_untimed, max_base_v):
# TODO THIS IS N IN PATH PLANNING, MAKE THIS A SHARED ARGUMENT
max_s = 5
t_to_s = max_s / total_time
+ TODO TODO trajectory for base and the end-effector are timed differently!!! this kill the MPC, and whatever else !!!
+ NOTE that the either this or the ee timing is set up wrong, but in any event they need to be the same
# 3) construct the ocp-timed 2D path
# TODO MAKE REFERENCE_STEP_SIZE A SHARED ARGUMENT
# TODO: we should know max s a priori
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index c3720b626724c3e9d8fe9ff84506284db6195809..6f83f346604f5bd9b32d13f17863195d2e758544 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -35,6 +35,7 @@ def getMinimalArgParser():
help="whether you're running on the real robot or not",
default=False,
)
+ # if this ends up working, replace --real with --mode, which can be {real, integration (simulation), debugging}
parser.add_argument(
"--robot-ip",
type=str,
@@ -53,6 +54,12 @@ def getMinimalArgParser():
help="whether you want to visualize the manipulator and workspace with meshcat",
default=True,
)
+ parser.add_argument(
+ "--viz-update-rate",
+ type=int,
+ help="frequency of visual updates. visualizer and plotter update every viz-update-rate^th iteration of the control loop. put to -1 to get a reasonable heuristic",
+ default=-1,
+ )
parser.add_argument(
"--plotter",
action=argparse.BooleanOptionalAction,
diff --git a/python/smc/util/debug_entries.py b/python/smc/util/debug_entries.py
new file mode 100644
index 0000000000000000000000000000000000000000..55610b1bcfa3d166d8bab407dd8f2a10774a1f94
--- /dev/null
+++ b/python/smc/util/debug_entries.py
@@ -0,0 +1,48 @@
+import code
+import inspect
+
+
+def get_debug_answer():
+ while True:
+ answer = input(
+ """
+ Do you want to:
+ (1) run an iteration of the control loop?
+ (2) run this control_loop until completion?
+ (3) get dropped in an interactive shell?
+ Answer with [1/2/3]
+ """
+ )
+ try:
+ answer = int(answer)
+ except ValueError:
+ pass
+ if answer in [1, 2, 3]:
+ return answer
+ else:
+ print(
+ "Whatever you typed in is neither 1, 2 or 3. Give it to me straight cheif!"
+ )
+ continue
+
+
+# NOTE: this one works, and you can update
+# internal varibles, but it's a shitty baren python shell.
+# so not the solution i want
+# def start_interactive_shell():
+# # Get stack frame for caller
+# stack = inspect.stack()[1]
+# frame = stack.frame
+#
+# # Copy locals and globals of caller's stack frame
+# locals_copy = dict(frame.f_locals)
+# globals_copy = dict(frame.f_globals)
+# shell_locals = dict()
+# shell_locals.update(globals_copy)
+# shell_locals.update(locals_copy)
+#
+# # Start interactive shell
+# code.interact(local=shell_locals)
+#
+# # Delete frame to avoid cyclic references
+# del stack
diff --git a/python/smc/visualization/path_visualizer.py b/python/smc/visualization/2Dpath_visualizer.py
similarity index 100%
rename from python/smc/visualization/path_visualizer.py
rename to python/smc/visualization/2Dpath_visualizer.py
diff --git a/python/smc/visualization/manipulator_comparison_visualizer.py b/python/smc/visualization/manipulator_comparison_visualizer.py
index 5c07d23e6d6b17897583c031948ad2b8a8ceb8e9..c982bb1308fc67fb39373aed13849b8303e7ca8c 100644
--- a/python/smc/visualization/manipulator_comparison_visualizer.py
+++ b/python/smc/visualization/manipulator_comparison_visualizer.py
@@ -21,18 +21,28 @@ def getLogComparisonArgs():
"--log-file1", type=str, help="first log file to load for comparison"
)
parser.add_argument(
- "--log-file2", type=str, help="first log file to load for comparison"
+ "--log-file2", type=str, help="second log file to load for comparison"
)
args = parser.parse_args()
# these are obligatory
- args.visualize_manipulator = False
- args.pinocchio_only = True
+ args.visualizer = False
+ args.real = False
args.simulation = False
return args
# TODO: use ProcessManager for this
class ManipulatorComparisonManager:
+ """
+ visualizes 2 control loops simulateneously + arbitrary plots.
+ requires that the robots are the same.
+ makes most sense if you are solving the same task with a different controller,
+ or with different parameters on the same controller.
+ the key thing this class brings to the table is the micro-management of what to show
+ when due to the fact that you might have different controlloops in one example
+ and them having different total times.
+ """
+
def __init__(self, args):
self.args = args
# these are obligatory
@@ -40,14 +50,6 @@ class ManipulatorComparisonManager:
args.pinocchio_only = True
args.simulation = False
- self.robot1 = getRobotFromArgs(args)
- self.robot2 = getRobotFromArgs(args)
-
- # no two loops will have the same amount of timesteps.
- # we need to store the last available step for both robots.
- self.lastq1 = np.zeros(self.robot1.model.nq)
- self.lastq2 = np.zeros(self.robot2.model.nq)
-
if os.path.exists(args.log_file1):
self.logm1 = Logger(None)
self.logm1.loadLog(args.log_file1)
@@ -61,13 +63,22 @@ class ManipulatorComparisonManager:
print("you did not give me a valid path for log2, exiting")
exit()
+ args.robot = self.logm1.args.robot
+ self.robot1 = getRobotFromArgs(args)
+ self.robot2 = getRobotFromArgs(args)
+
+ # no two loops will have the same amount of timesteps.
+ # we need to store the last available step for both robots.
+ self.lastq1 = np.zeros(self.robot1.model.nq)
+ self.lastq2 = np.zeros(self.robot2.model.nq)
+
side_function = partial(
manipulatorComparisonVisualizer,
self.robot1.model,
self.robot1.visual_model,
self.robot1.collision_model,
)
- cmd = (np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq))
+ cmd = (np.zeros(self.robot1.nq), np.ones(self.robot2.nq))
self.visualizer_manager = ProcessManager(args, side_function, cmd, 2)
# wait until it's ready (otherwise you miss half the sim potentially)
# 5 seconds should be more than enough,
diff --git a/python/smc/visualization/arms/another_debug_dh b/python/smc/visualization/obsolete_to_be_removed/arms/another_debug_dh
similarity index 100%
rename from python/smc/visualization/arms/another_debug_dh
rename to python/smc/visualization/obsolete_to_be_removed/arms/another_debug_dh
diff --git a/python/smc/visualization/arms/j2n6s300_dh_params b/python/smc/visualization/obsolete_to_be_removed/arms/j2n6s300_dh_params
similarity index 100%
rename from python/smc/visualization/arms/j2n6s300_dh_params
rename to python/smc/visualization/obsolete_to_be_removed/arms/j2n6s300_dh_params
diff --git a/python/smc/visualization/arms/j2s7s300_dh_params b/python/smc/visualization/obsolete_to_be_removed/arms/j2s7s300_dh_params
similarity index 100%
rename from python/smc/visualization/arms/j2s7s300_dh_params
rename to python/smc/visualization/obsolete_to_be_removed/arms/j2s7s300_dh_params
diff --git a/python/smc/visualization/arms/kinova_jaco_params b/python/smc/visualization/obsolete_to_be_removed/arms/kinova_jaco_params
similarity index 100%
rename from python/smc/visualization/arms/kinova_jaco_params
rename to python/smc/visualization/obsolete_to_be_removed/arms/kinova_jaco_params
diff --git a/python/smc/visualization/arms/kuka_lbw_iiwa_dh_params b/python/smc/visualization/obsolete_to_be_removed/arms/kuka_lbw_iiwa_dh_params
similarity index 100%
rename from python/smc/visualization/arms/kuka_lbw_iiwa_dh_params
rename to python/smc/visualization/obsolete_to_be_removed/arms/kuka_lbw_iiwa_dh_params
diff --git a/python/smc/visualization/arms/lbr_iiwa_jos_jedan_dh b/python/smc/visualization/obsolete_to_be_removed/arms/lbr_iiwa_jos_jedan_dh
similarity index 100%
rename from python/smc/visualization/arms/lbr_iiwa_jos_jedan_dh
rename to python/smc/visualization/obsolete_to_be_removed/arms/lbr_iiwa_jos_jedan_dh
diff --git a/python/smc/visualization/arms/robot_parameters2 b/python/smc/visualization/obsolete_to_be_removed/arms/robot_parameters2
similarity index 100%
rename from python/smc/visualization/arms/robot_parameters2
rename to python/smc/visualization/obsolete_to_be_removed/arms/robot_parameters2
diff --git a/python/smc/visualization/arms/testing_dh_parameters b/python/smc/visualization/obsolete_to_be_removed/arms/testing_dh_parameters
similarity index 100%
rename from python/smc/visualization/arms/testing_dh_parameters
rename to python/smc/visualization/obsolete_to_be_removed/arms/testing_dh_parameters
diff --git a/python/smc/visualization/arms/ur10e_dh_parameters_from_the_ur_site b/python/smc/visualization/obsolete_to_be_removed/arms/ur10e_dh_parameters_from_the_ur_site
similarity index 100%
rename from python/smc/visualization/arms/ur10e_dh_parameters_from_the_ur_site
rename to python/smc/visualization/obsolete_to_be_removed/arms/ur10e_dh_parameters_from_the_ur_site
diff --git a/python/smc/visualization/arms/ur5e_dh b/python/smc/visualization/obsolete_to_be_removed/arms/ur5e_dh
similarity index 100%
rename from python/smc/visualization/arms/ur5e_dh
rename to python/smc/visualization/obsolete_to_be_removed/arms/ur5e_dh
diff --git a/python/smc/visualization/manipulator_visual_motion_analyzer.py b/python/smc/visualization/obsolete_to_be_removed/manipulator_visual_motion_analyzer.py
similarity index 99%
rename from python/smc/visualization/manipulator_visual_motion_analyzer.py
rename to python/smc/visualization/obsolete_to_be_removed/manipulator_visual_motion_analyzer.py
index 3b9a4254c4fbc6a2514384abd03fd665749c9496..e94a260160205ecb529822e415bce0597933ec09 100644
--- a/python/smc/visualization/manipulator_visual_motion_analyzer.py
+++ b/python/smc/visualization/obsolete_to_be_removed/manipulator_visual_motion_analyzer.py
@@ -17,8 +17,8 @@ from matplotlib.gridspec import GridSpec
from robot_stuff.InverseKinematics import InverseKinematicsEnv
from robot_stuff.drawing import *
from robot_stuff.inv_kinm import *
-from smc.visualize.make_run import makeRun, loadRun
-from smc.managers import RobotManager
+from smc.visualization.obsolete_to_be_removed.make_run import makeRun, loadRun
+from smc import getRobotFromArgs
from smc.logging.logger import loadRunForAnalysis, cleanUpRun
import numpy as np
@@ -29,7 +29,7 @@ from multiprocessing import Queue
# for local thread which manages local queue (can be made better,
# but again, who cares, all of this is ugly as hell anyway ('cos it's front-end))
import threading
-import time
+import time
import pickle
diff --git a/python/smc/visualization/robot_stuff/InverseKinematics.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/InverseKinematics.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/InverseKinematics.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/InverseKinematics.py
diff --git a/python/smc/visualization/robot_stuff/drawing.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/drawing.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/drawing.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/drawing.py
diff --git a/python/smc/visualization/robot_stuff/drawing_for_anim.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/drawing_for_anim.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/drawing_for_anim.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/drawing_for_anim.py
diff --git a/python/smc/visualization/robot_stuff/follow_curve.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/follow_curve.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/follow_curve.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/follow_curve.py
diff --git a/python/smc/visualization/robot_stuff/forw_kinm.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/forw_kinm.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/forw_kinm.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/forw_kinm.py
diff --git a/python/smc/visualization/robot_stuff/inv_kinm.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/inv_kinm.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/inv_kinm.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/inv_kinm.py
diff --git a/python/smc/visualization/robot_stuff/joint_as_hom_mat.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/joint_as_hom_mat.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/joint_as_hom_mat.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/joint_as_hom_mat.py
diff --git a/python/smc/visualization/robot_stuff/utils.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/utils.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/utils.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/utils.py
diff --git a/python/smc/visualization/robot_stuff/webots_api_helper_funs.py b/python/smc/visualization/obsolete_to_be_removed/robot_stuff/webots_api_helper_funs.py
similarity index 100%
rename from python/smc/visualization/robot_stuff/webots_api_helper_funs.py
rename to python/smc/visualization/obsolete_to_be_removed/robot_stuff/webots_api_helper_funs.py
diff --git a/python/smc/visualization/visualizer.py b/python/smc/visualization/visualizer.py
index 98c07a28d1aa31e26ba92af222c7fae7073ce972..2851764d1b7b6fd55bfa0fb0e5150afae7caf8db 100644
--- a/python/smc/visualization/visualizer.py
+++ b/python/smc/visualization/visualizer.py
@@ -144,6 +144,7 @@ def manipulatorComparisonVisualizer(
viz.viewer.window.server_proc.wait()
break
q1, q2 = q
+ print(q)
viz.display(q1)
viz2.display(q2)
# this doesn't really work because meshcat is it's own server