diff --git a/examples/cart_pulling/base_and_ee_path_following_from_planner.py b/examples/cart_pulling/base_and_ee_path_following_from_planner.py
index 54c7dfbc2a1427f7eb960c33437eb3584a30eb80..67da1a3069cfd4d05916edfad0b9d3c04b69da46 100644
--- a/examples/cart_pulling/base_and_ee_path_following_from_planner.py
+++ b/examples/cart_pulling/base_and_ee_path_following_from_planner.py
@@ -85,6 +85,7 @@ if __name__ == "__main__":
p_base[2] = 0.0
print(pathSE3[0].translation)
print(p_base)
+ # TODO: UNCOMMENT
EEAndBaseP2PMPC(args, robot, pathSE3[0], p_base)
print("initialized!")
BaseAndEEPathFollowingMPC(args, robot, path_planner)
diff --git a/python/smc/control/controller_templates/path_following_template.py b/python/smc/control/controller_templates/path_following_template.py
index 4dd851c1f036606f5d57215d1989132a325ca01d..e7c815ea6f817ad48c87bec343400cb2b8dcd313 100644
--- a/python/smc/control/controller_templates/path_following_template.py
+++ b/python/smc/control/controller_templates/path_following_template.py
@@ -5,6 +5,7 @@ from pinocchio import SE3, log6
from argparse import Namespace
from typing import Any, Callable
import numpy as np
+from collections import deque
global control_loop_return
control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
@@ -13,21 +14,28 @@ control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
def PathFollowingFromPlannerControlLoop(
path_planner: ProcessManager,
get_position: Callable[[AbstractRobotManager], np.ndarray],
- SOLVER: Callable,
+ SOLVER: Any,
control_loop: Callable[
- [Any, np.ndarray, Namespace, AbstractRobotManager, dict[str, np.ndarray], int],
- np.ndarray,
+ [
+ Any,
+ np.ndarray,
+ Namespace,
+ AbstractRobotManager,
+ int,
+ dict[str, deque[np.ndarray]],
+ ],
+ tuple[np.ndarray, dict[str, np.ndarray]],
],
args: Namespace,
robot: AbstractRobotManager,
- past_data: dict[str, np.ndarray],
t: int, # will be float eventually
+ past_data: dict[str, deque[np.ndarray]],
) -> control_loop_return:
"""
- Point2PointControlLoop
+ PathFollowingFromPlannerControlLoop
---------------
- generic control loop for point to point motion
- (handling error to final point etc).
+ handles getting path and with comm with the planner.
+ then you do whatever you want with the path, and execute a path following controller
"""
breakFlag = False
log_item = {}
@@ -51,12 +59,13 @@ def PathFollowingFromPlannerControlLoop(
path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
v_cmd, log_item_inner = control_loop(
- SOLVER, path2D_untimed, args, robot, past_data, t
+ SOLVER, path2D_untimed, args, robot, t, past_data
)
robot.sendVelocityCommand(v_cmd)
log_item.update(log_item_inner)
log_item["qs"] = robot.q
log_item["dqs"] = robot.v
- log_item["dqs_cmd"] = v_cmd.reshape((robot.model.nv,))
+ # NOTE: shouldn't be here
+ save_past_item["path2D_untimed"] = p
return breakFlag, save_past_item, log_item
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 e6500f3f409034b0ad17062e7dc30b2b2d1d1843..134e942ea42cec649bd7e89973035cd37b773b1a 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -11,16 +11,19 @@ from smc.path_generation.path_math.path2d_to_6d import (
path2D_to_SE3_fixed,
)
from smc.path_generation.path_math.cart_pulling_path_math import (
- time_base_path,
construct_EE_path,
)
from smc.path_generation.path_math.path_to_trajectory import path2D_timed
+from smc.control.controller_templates.path_following_template import (
+ PathFollowingFromPlannerControlLoop,
+)
import numpy as np
from functools import partial
import types
from argparse import Namespace
from pinocchio import SE3, log6
+from collections import deque
def CrocoEEPathFollowingMPCControlLoop(
@@ -47,10 +50,10 @@ def CrocoEEPathFollowingMPCControlLoop(
q = robot.q
T_w_e = robot.T_w_e
- pathSE3 = path_planner(T_w_e, t)
+ path_EE_SE3 = path_planner(T_w_e, t)
x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=pathSE3)
+ ocp.warmstartAndReSolve(x0, data=path_EE_SE3)
xs = np.array(ocp.solver.xs)
us = np.array(ocp.solver.us)
vel_cmds = xs[1, robot.model.nq :]
@@ -60,21 +63,23 @@ def CrocoEEPathFollowingMPCControlLoop(
# 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})
+ robot.visualizer_manager.sendCommand({"frame_path": path_EE_SE3})
+ err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0]))
+ log_item["err_vec_ee"] = err_vector_ee
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,
- path_planner: ProcessManager,
- t,
- _,
-):
+ path2D_untimed: np.ndarray,
+ args: Namespace,
+ robot: SingleArmInterface,
+ t: int,
+ _: dict[str, np.ndarray],
+) -> tuple[np.ndarray, dict[str, np.ndarray]]:
"""
CrocoPathFollowingMPCControlLoop
-----------------------------
@@ -84,56 +89,28 @@ def CrocoEEPathFollowingFromPlannerMPCControlLoop(
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
- p = T_w_e.translation[:2]
-
- # NOTE: it's pointless to recalculate the path every time
- # if it's the same 2D path
-
- path_planner.sendCommand(p)
- data = path_planner.getData()
-
- 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
-
- _, 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)
+ path_EE_SE3 = path2D_to_SE3_fixed(path2D, args.handlebar_height)
+ for pose in path_EE_SE3:
+ print(pose.translation)
# 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})
+ robot.visualizer_manager.sendCommand({"frame_path": path_EE_SE3})
x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=pathSE3)
+ ocp.warmstartAndReSolve(x0, data=path_EE_SE3)
xs = np.array(ocp.solver.xs)
- us = np.array(ocp.solver.us)
- vel_cmds = xs[1, robot.model.nq :]
+ v_cmd = xs[1, robot.model.nq :]
- robot.sendVelocityCommand(vel_cmds)
+ err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0]))
+ log_item = {"err_vec_ee": err_vector_ee}
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.v.reshape((robot.model.nv,))
- return breakFlag, save_past_item, log_item
+ return v_cmd, log_item
def CrocoEEPathFollowingMPC(
@@ -161,14 +138,21 @@ def CrocoEEPathFollowingMPC(
CrocoEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
)
else:
+ get_position = lambda robot: robot.T_w_e.translation[:2]
controlLoop = partial(
+ PathFollowingFromPlannerControlLoop,
+ path_planner,
+ get_position,
+ ocp,
CrocoEEPathFollowingFromPlannerMPCControlLoop,
args,
robot,
- ocp,
- path_planner,
)
- log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee": np.zeros(6),
+ }
save_past_item = {}
loop_manager = ControlLoopManager(
robot, controlLoop, args, save_past_item, log_item
@@ -213,8 +197,9 @@ def BaseAndEEPathFollowingMPCControlLoop(
robot.visualizer_manager.sendCommand({"path": path_base})
robot.visualizer_manager.sendCommand({"frame_path": path_EE})
- err_vector_ee = T_w_e.actInv(path_EE[0])
+ err_vector_ee = log6(T_w_e.actInv(path_EE[0]))
err_vector_base = np.linalg.norm(q[:2] - path_base[0][:2]) # z axis is irrelevant
+ 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,))
log_item["qs"] = q.reshape((robot.model.nq,))
@@ -223,63 +208,35 @@ def BaseAndEEPathFollowingMPCControlLoop(
def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
- args,
- robot,
ocp: CrocoOCP,
- path_planner: ProcessManager,
- t,
- past_data,
-):
- """
- 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
- p = q[:2]
- # NOTE: this is the actual position, not what the path suggested
- # whether this or path reference should be put in is debateable.
- # this feels more correct to me.
- save_past_dict["path2D_untimed"] = p
- path_planner.sendCommand(p)
+ path2D_untimed_base: np.ndarray,
+ args: Namespace,
+ robot: SingleArmInterface,
+ t: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray]]:
+ p = robot.q[:2]
- ###########################
- # get path from planner #
- ###########################
- data = path_planner.getData()
+ # NOTE: this one is obtained as the future path from path planner
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+ path_base = path2D_timed(args, path2D_untimed_base, max_base_v)
+ path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
- if data == "done":
- breakFlag = True
- if data == "done" or data is None:
- robot.sendVelocityCommand(np.zeros(robot.model.nv))
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.v.reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
+ pathSE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
- ##########################################
- # construct timed 2D path for the base #
- ##########################################
- _, path2D_untimed_base = data
- path_base = time_base_path(
- args, path2D_untimed_base, np.linalg.norm(robot._max_v[:2])
- )
+ #print("BASEcurrent_position", p)
+ #print("EEcurrent_position", robot.T_w_e.translation)
+ #print("=" * 5, "desired handlebar traj", "=" * 5)
+ #for pose in pathSE3_handlebar:
+ # print(pose.translation)
- pathSE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
+ ###########################################
# TODO: remove
# SETTING ROTATION IS BROKEN ATM
# BUT SETTING DISTANCES (TRANSLATIONS) IS TOO.
# WE DEAL WITH DISTANCES FIRST, UNTIL THAT'S DONE THIS STAYS HERE
for ppp in pathSE3_handlebar:
- ppp.rotation = T_w_e.rotation
+ ppp.rotation = robot.T_w_e.rotation
###########################################
if args.visualizer:
@@ -290,19 +247,15 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
x0 = np.concatenate([robot.q, robot.v])
ocp.warmstartAndReSolve(x0, data=(path_base, pathSE3_handlebar))
xs = np.array(ocp.solver.xs)
- us = np.array(ocp.solver.us)
- vel_cmds = xs[1, robot.model.nq :]
+ v_cmd = xs[1, robot.model.nq :]
- robot.sendVelocityCommand(vel_cmds)
-
- err_vector_ee = log6(T_w_e.actInv(pathSE3_handlebar[0]))
- err_vector_base = np.linalg.norm(q[:2] - path_base[0][:2]) # z axis is irrelevant
+ 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,))
- log_item["qs"] = q.reshape((robot.model.nq,))
- # log_item["dqs"] = robot.v.reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
+ return v_cmd, log_item
def initializePastData(
@@ -312,19 +265,8 @@ def initializePastData(
# (which didn't actually happen because this just started)
p_ee = T_w_e.translation[:2]
straight_line_path = np.linspace(p_ee, p_base, args.past_window_size)
- straight_line_path_timed = path2D_timed(args, straight_line_path, max_base_v)
- # time it the same way the base path is timed
- # time_past = np.linspace(
- # 0.0, args.past_window_size * (1 / args.ctrl_freq), args.past_window_size
- # )
- # s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.past_window_size)
- # path2D_handlebar = np.hstack(
- # (
- # np.interp(s, time_past, straight_line_path[:, 0]).reshape((-1, 1)),
- # np.interp(s, time_past, straight_line_path[:, 1]).reshape((-1, 1)),
- # )
- # )
- # return path2D_handlebar
+ # straight_line_path_timed = path2D_timed(args, straight_line_path, max_base_v)
+ # return straight_line_path_timed # this one is shortened to args.n_knots! and we want the whole buffer
return straight_line_path
@@ -356,16 +298,19 @@ def BaseAndEEPathFollowingMPC(
BaseAndEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
)
else:
+ get_position = lambda robot: robot.q[:2]
controlLoop = partial(
+ PathFollowingFromPlannerControlLoop,
+ path_planner,
+ get_position,
+ ocp,
BaseAndEEPathFollowingFromPlannerMPCControlLoop,
args,
robot,
- ocp,
- path_planner,
)
log_item = {
"qs": np.zeros(robot.model.nq),
- # "dqs": np.zeros(robot.model.nv),
+ "dqs": np.zeros(robot.model.nv),
"err_vec_ee": np.zeros((6,)),
"err_norm_ee": np.zeros((1,)),
"err_norm_base": np.zeros((1,)),
@@ -522,7 +467,7 @@ def BaseAndDualArmEEPathFollowingMPCControlLoop(
return breakFlag, save_past_dict, log_item
-def BaseAndDualARmEEPathFollowingMPC(args, robot, path_planner):
+def BaseAndDualArmEEPathFollowingMPC(args, robot, path_planner):
"""
CrocoEndEffectorPathFollowingMPC
-----
diff --git a/python/smc/path_generation/path_math/cart_pulling_path_math.py b/python/smc/path_generation/path_math/cart_pulling_path_math.py
index 499fc63baa86c3ea7ce00da1f31a6e8ac79a2a15..c3c1f12443b98898435f96608a19554c81f7a4e2 100644
--- a/python/smc/path_generation/path_math/cart_pulling_path_math.py
+++ b/python/smc/path_generation/path_math/cart_pulling_path_math.py
@@ -57,18 +57,6 @@ def getCurrentPositionHandlebarInPastBasePath(
return handlebar_path_index
-def time_base_path(
- args: Namespace, path2D_untimed_base, max_base_v: float
-) -> np.ndarray:
- # NOTE: this one is obtained as the future path from path planner
- path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1, 2))
- # ideally should be precomputed somewhere
- path_base = path2D_timed(args, path2D_untimed_base, max_base_v)
- # and it's of height 0 (i.e. the height of base's planar joint)
- path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
- return path_base
-
-
# NOTE: extremely inefficient.
# there should be no copy-pasting of the whole path at every single point in time,
# instead all the computations should be cached.
@@ -94,7 +82,7 @@ def construct_EE_path(
1) find the previous path point of arclength base_to_handlebar_preferred_distance.
first part of the path is from there to current base position,
second is just the current base's plan.
- 2) construct timing on the first part.
+ 2) construct timing on the first part. (robot.dt =/= ocp_dt)
3) (optional) join that with the already timed second part.
--> not doing this because paths are super short and this won't ever happen with the current setup
4) turn the timed 2D path into an SE3 trajectory
@@ -114,6 +102,8 @@ def construct_EE_path(
"""
# i shouldn't need to copy-paste everything but what can you do
past_path2D = np.array(past_path2D_from_window).reshape((-1, 2))
+ # print("-" * 5, "past_window", "-" * 5)
+ # print(past_path2D)
# step (1)
handlebar_path_index = getCurrentPositionHandlebarInPastBasePath(
args, p_base_current, past_path2D
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 15d9fdd5e5193d93aea0212cb6a312cb839adefc..f07958693afdf1788eb9baa7651bd4d659b38a1a 100644
--- a/python/smc/path_generation/path_math/path_to_trajectory.py
+++ b/python/smc/path_generation/path_math/path_to_trajectory.py
@@ -53,9 +53,12 @@ def path2D_timed(args, path2D_untimed, max_base_v):
y_diff = y_i_plus_1 - y_i
y_diff = y_diff.reshape((-1, 1))
path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
- print(path_length)
+ # NOTE: sometimes the path planner gives me the same god damn points
+ # and that's it. can't do much about, expect set those point then.
+ # and i need to return now so that the math doesn't break down the line
+ if path_length < 1e-3:
+ return np.ones((args.n_knots + 1, 2)) * path2D_untimed[0]
total_time = path_length / base_v
- print(total_time)
# 2) we find the correspondence between s and time
# TODO: read from where it should be, but seba checked that it's 5 for some reason
# TODO THIS IS N IN PATH PLANNING, MAKE THIS A SHARED ARGUMENT