diff --git a/examples/cart_pulling/base_and_ee_path_following.py b/examples/cart_pulling/base_and_ee_path_following.py
new file mode 100644
index 0000000000000000000000000000000000000000..f7c1fd338071b2cb6a7e836b4fcc7576cca945f0
--- /dev/null
+++ b/examples/cart_pulling/base_and_ee_path_following.py
@@ -0,0 +1,56 @@
+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 (
+ BaseAndEEPathFollowingMPC,
+)
+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_ee = []
+ path_base = []
+ 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_ee.append(new)
+ translation[2] = 0.0
+ path_base.append(translation)
+ return path_base, path_ee
+
+
+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()
+
+ BaseAndEEPathFollowingMPC(args, robot, 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/cart_pulling_notes.md b/examples/cart_pulling/cart_pulling_notes.md
index 95fe861a8f701e20a69c7a2dbcf069332f625155..5c6eed59576c0a5a930e2aa71b36452bd8b09864 100644
--- a/examples/cart_pulling/cart_pulling_notes.md
+++ b/examples/cart_pulling/cart_pulling_notes.md
@@ -20,6 +20,11 @@ we go with option 2) because option 2) is false! it does not account for orienta
you could also try "freezing" the first point tbh
+# should the reference for the base be just a translation or a full pose?
+-------------------------------------------------------------------------------
+on one level we don't care, but then again do know what it should be. so idk
+- ideally just test this shit (annoying as fuck to implement but what can you do)
+
# the desided joint space position
diff --git a/examples/cart_pulling/ee_only_path_following_from_plannner.py b/examples/cart_pulling/ee_only_path_following_from_plannner.py
index e3b7f77b06fd755b7563fa5ad6c583bf6ecda658..d7aea034e81b18a48e7681c8eee405bfe64156aa 100644
--- a/examples/cart_pulling/ee_only_path_following_from_plannner.py
+++ b/examples/cart_pulling/ee_only_path_following_from_plannner.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 CrocoIKMPC
from smc.control.optimal_control.croco_mpc_path_following import (
- CrocoEndEffectorPathFollowingMPC,
+ CrocoEEPathFollowingMPC,
)
from smc.multiprocessing import ProcessManager
@@ -73,7 +73,7 @@ if __name__ == "__main__":
print("going to initial path position")
CrocoIKMPC(args, robot, pathSE3[0])
print("initialized!")
- CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner)
+ CrocoEEPathFollowingMPC(args, robot, x0, path_planner)
print("final position:", robot.T_w_e)
diff --git a/examples/cartesian_space/clik.py b/examples/cartesian_space/clik_point_to_point.py
similarity index 100%
rename from examples/cartesian_space/clik.py
rename to examples/cartesian_space/clik_point_to_point.py
diff --git a/examples/optimal_control/path_following_mpc.py b/examples/optimal_control/path_following_mpc.py
index 439352518beb563d076069f21bb3597cdd607b68..2e11a54104211e46764924edde04603c624414d5 100644
--- a/examples/optimal_control/path_following_mpc.py
+++ b/examples/optimal_control/path_following_mpc.py
@@ -3,7 +3,7 @@ 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 (
- CrocoEndEffectorPathFollowingMPC,
+ CrocoEEPathFollowingMPC,
)
import numpy as np
@@ -38,7 +38,7 @@ if __name__ == "__main__":
x0 = np.concatenate([robot.q, robot.v])
robot._step()
- CrocoEndEffectorPathFollowingMPC(args, robot, x0, path)
+ CrocoEEPathFollowingMPC(args, robot, x0, path)
print("final position:", robot.T_w_e)
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 1f7fdb83313f44233a08043e21312b154c997f89..eac9b86877b47847190d09ed190b7eef73586745 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -18,7 +18,51 @@ import types
from argparse import Namespace
-def CrocoEndEffectorPathFollowingMPCControlLoop(
+def CrocoEEPathFollowingMPCControlLoop(
+ args,
+ robot: SingleArmInterface,
+ ocp: CrocoOCP,
+ path_planner: types.FunctionType,
+ t,
+ _,
+):
+ """
+ CrocoPathFollowingMPCControlLoop
+ -----------------------------
+ end-effector(s) follow their path(s).
+
+ path planner can either be a function which spits out a list of path points
+ or an instance of ProcessManager which spits out path points
+ by calling ProcessManager.getData()
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_item = {}
+
+ q = robot.q
+ T_w_e = robot.T_w_e
+
+ pathSE3 = path_planner(T_w_e, t)
+
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0, data=pathSE3)
+ xs = np.array(ocp.solver.xs)
+ us = np.array(ocp.solver.us)
+ vel_cmds = xs[1, robot.model.nq :]
+
+ robot.sendVelocityCommand(vel_cmds)
+
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ if args.visualizer:
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ robot.visualizer_manager.sendCommand({"frame_path": pathSE3})
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.v.reshape((robot.model.nv,))
+ return breakFlag, save_past_item, log_item
+
+
+def CrocoEEPathFollowingFromPlannerMPCControlLoop(
args,
robot: SingleArmInterface,
ocp: CrocoOCP,
@@ -46,31 +90,28 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
# NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
- if type(path_planner) == types.FunctionType:
- pathSE3 = path_planner(T_w_e, t)
- else:
- path_planner.sendCommand(p)
- data = path_planner.getData()
+ path_planner.sendCommand(p)
+ data = path_planner.getData()
- if data == "done":
- breakFlag = True
+ if data == "done":
+ breakFlag = True
- if data == "done" or data is None:
- robot.sendVelocityCommand(np.zeros(robot.model.nv))
- log_item["qs"] = q
- log_item["dqs"] = robot.v
- return breakFlag, save_past_item, log_item
+ if data == "done" or data is None:
+ robot.sendVelocityCommand(np.zeros(robot.model.nv))
+ log_item["qs"] = q
+ log_item["dqs"] = robot.v
+ return breakFlag, save_past_item, log_item
- _, path2D_untimed = data
- path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
- # who cares if the velocity is right,
- # it should be kinda right so that we need less ocp iterations
- # and that's it
- max_base_v = np.linalg.norm(robot._max_v[:2])
- path2D = path2D_timed(args, path2D_untimed, max_base_v)
+ _, path2D_untimed = data
+ path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
+ # who cares if the velocity is right,
+ # it should be kinda right so that we need less ocp iterations
+ # and that's it
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+ path2D = path2D_timed(args, path2D_untimed, max_base_v)
- # create a 3D reference out of the path
- pathSE3 = path2D_to_SE3_fixed(path2D, args.handlebar_height)
+ # create a 3D reference out of the path
+ pathSE3 = path2D_to_SE3_fixed(path2D, args.handlebar_height)
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualizer:
@@ -90,11 +131,11 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
return breakFlag, save_past_item, log_item
-def CrocoEndEffectorPathFollowingMPC(
+def CrocoEEPathFollowingMPC(
args: Namespace,
robot: SingleArmInterface,
x0: np.ndarray,
- path_planner: ProcessManager,
+ path_planner: ProcessManager | types.FunctionType,
):
"""
CrocoEndEffectorPathFollowingMPC
@@ -111,9 +152,18 @@ def CrocoEndEffectorPathFollowingMPC(
x0 = np.concatenate([robot.q, robot.v])
ocp.solveInitialOCP(x0)
- controlLoop = partial(
- CrocoEndEffectorPathFollowingMPCControlLoop, args, robot, ocp, path_planner
- )
+ if type(path_planner) == types.FunctionType:
+ controlLoop = partial(
+ CrocoEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
+ )
+ else:
+ controlLoop = partial(
+ CrocoEEPathFollowingFromPlannerMPCControlLoop,
+ args,
+ robot,
+ ocp,
+ path_planner,
+ )
log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
save_past_item = {}
loop_manager = ControlLoopManager(
@@ -122,14 +172,54 @@ def CrocoEndEffectorPathFollowingMPC(
loop_manager.run()
-# TODO: the robot put in is a whole body robot,
-# which you need to implement
def BaseAndEEPathFollowingMPCControlLoop(
+ args,
+ robot,
+ ocp: CrocoOCP,
+ path_planner: types.FunctionType,
+ t,
+ _,
+):
+ """
+ CrocoPathFollowingMPCControlLoop
+ -----------------------------
+ end-effector(s) follow their path(s).
+
+ path planner can either be a function which spits out a list of path points
+ or an instance of ProcessManager which spits out path points
+ by calling ProcessManager.getData()
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.q
+ T_w_e = robot.T_w_e
+ path_base, path_EE = path_planner(T_w_e, t)
+
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0, data=(path_base, path_EE))
+ xs = np.array(ocp.solver.xs)
+ us = np.array(ocp.solver.us)
+ vel_cmds = xs[1, robot.model.nq :]
+
+ robot.sendVelocityCommand(vel_cmds)
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ path_base = np.array(path_base)
+ robot.visualizer_manager.sendCommand({"path": path_base})
+ robot.visualizer_manager.sendCommand({"frame_path": path_EE})
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.v.reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+
+def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
args,
robot,
ocp: CrocoOCP,
path_planner: ProcessManager,
- iter_n,
+ t,
past_data,
):
"""
@@ -227,7 +317,7 @@ def BaseAndEEPathFollowingMPCControlLoop(
pathSE3_handlebar = path2D_to_SE3_fixed(path2D_handlebar_1, args.handlebar_height)
if args.visualizer:
- if iter_n % 20 == 0:
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
robot.visualizer_manager.sendCommand({"path": path_base})
robot.visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
@@ -244,7 +334,11 @@ def BaseAndEEPathFollowingMPCControlLoop(
return breakFlag, save_past_dict, log_item
-def BaseAndEEPathFollowingMPC(args, robot, path_planner):
+def BaseAndEEPathFollowingMPC(
+ args: Namespace,
+ robot: SingleArmInterface,
+ path_planner: ProcessManager | types.FunctionType,
+):
"""
CrocoEndEffectorPathFollowingMPC
-----
@@ -258,7 +352,7 @@ def BaseAndEEPathFollowingMPC(args, robot, path_planner):
ocp = BaseAndEEPathFollowingOCP(args, robot, x0)
ocp.solveInitialOCP(x0)
- # prepopulate past data to make base and cart be on the same path in the past
+ # prepopulate past data to make base and cart be on the same path in the past
# (which didn't actually happen because this just started)
T_w_e = robot.T_w_e
p_cart = robot.q[:2]
@@ -276,16 +370,24 @@ def BaseAndEEPathFollowingMPC(args, robot, path_planner):
)
)
- controlLoop = partial(
- BaseAndEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
- )
+ if type(path_planner) == types.FunctionType:
+ controlLoop = partial(
+ BaseAndEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
+ )
+ else:
+ controlLoop = partial(
+ BaseAndEEPathFollowingFromPlannerMPCControlLoop,
+ args,
+ robot,
+ ocp,
+ path_planner,
+ )
log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
save_past_dict = {"path2D_untimed": T_w_e.translation[:2]}
loop_manager = ControlLoopManager(
robot, controlLoop, args, save_past_dict, log_item
)
-
loop_manager.past_data["path2D_untimed"].clear()
loop_manager.past_data["path2D_untimed"].extend(
path2D_handlebar[i] for i in range(args.past_window_size)