diff --git a/development_plan.toml b/development_plan.toml
index 8c4361b4572fc1934bff8669b6098c41ed7344cb..c6cd1e0863d44bba8d34d535f2628f584e351d12 100644
--- a/development_plan.toml
+++ b/development_plan.toml
@@ -94,7 +94,7 @@ dependencies = ["create/fix type-checking workflow"]
purpose = """easier code writting and debugging, making library professional-grade"""
# -1 means it should be a sum over sub-items
hours_required = 8
-is_done = false
+is_done = true
[[project_plan.action_items.action_items]]
name = "robotmanager abstract class"
@@ -168,7 +168,19 @@ deadline = 2025-01-31
dependencies = ["robotmanager abstract class"]
purpose = """making it easy, simple, and error-abstinent to only navigate the base"""
hours_required = 4
-is_done = false
+is_done = true
+
+[[project_plan.action_items.action_items]]
+name = "whole-body feature"
+description = """
+a generic class that has at least 1 manipulator plus a mobile base. what you do here is give me access to just the arm or just the base - that's it. you can do it by overriding setters or whatever. you don't need to finish it, it just needs to do what's necessary for cart pulling
+"""
+priority = 1
+deadline = 2025-01-31
+dependencies = ["robotmanager abstract class", "mobile_base_feature", "dual_arms_feature"]
+purpose = """making it easy, simple, and error-abstinent to only navigate the base"""
+hours_required = 4
+is_done = true
[[project_plan.action_items.action_items]]
@@ -289,6 +301,18 @@ street cred"""
hours_required = 80
is_done = false
+[[project_plan.action_items.action_items]]
+name = "tidy up typing"
+description = """
+sit down and get typing coverage to 100% or whatever is the limit of reason.
+"""
+priority = 3
+deadline = 2025-09-01
+dependencies = ["create/fix type-checking workflow"]
+purpose = """easier code writting and debugging, making library professional-grade"""
+# -1 means it should be a sum over sub-items
+hours_required = 8
+is_done = true
[[project_plan.action_items]]
name = "python stdlib logging instead of printing"
diff --git a/examples/cart_pulling/path_following_from_planner.py b/examples/cart_pulling/path_following_from_planner.py
index 8470014f5bbff713a4a54552c6eed5b33ef36788..e3b7f77b06fd755b7563fa5ad6c583bf6ecda658 100644
--- a/examples/cart_pulling/path_following_from_planner.py
+++ b/examples/cart_pulling/path_following_from_planner.py
@@ -1,9 +1,11 @@
from smc import getRobotFromArgs
from smc import getMinimalArgParser
from smc.path_generation.maps.premade_maps import createSampleStaticMap
+from smc.path_generation.path_math.path2d_to_6d import path2D_to_SE3_fixed
from smc.control.optimal_control.util import get_OCP_args
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,
)
@@ -12,6 +14,7 @@ from smc.multiprocessing import ProcessManager
import time
import numpy as np
from functools import partial
+import pinocchio as pin
def get_args():
@@ -32,19 +35,44 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = getRobotFromArgs(args)
- # robot._q[0] = 9.0
- # robot._q[1] = 4.0
+ # TODO: HOW IS IT POSSIBLE THAT T_W_E IS WRONG WITHOUT STEP CALLED HERE?????????????????
+ robot._step()
+ T_w_e = robot.T_w_e
+ robot._q[0] = 9.0
+ robot._q[1] = 4.0
robot._step()
x0 = np.concatenate([robot.q, robot.v])
goal = np.array([0.5, 5.5])
planning_function = partial(starPlanner, goal)
- # TODO: ensure alignment in orientation between planner and actual robot
- path_planner = ProcessManager(args, planning_function, robot.q[:2], 3, None)
+ # here we're following T_w_e reference so that's what we send
+ path_planner = ProcessManager(
+ args, planning_function, T_w_e.translation[:2], 3, None
+ )
_, map_as_list = createSampleStaticMap()
if args.visualizer:
robot.sendRectangular2DMapToVisualizer(map_as_list)
- time.sleep(5)
+ # time.sleep(5)
+
+ T_w_e = robot.T_w_e
+ data = None
+ # get first path
+ while data is None:
+ path_planner.sendCommand(T_w_e.translation[:2])
+ data = path_planner.getData()
+ # time.sleep(1 / args.ctrl_freq)
+ time.sleep(1)
+
+ _, path2D = data
+ path2D = np.array(path2D).reshape((-1, 2))
+ pathSE3 = path2D_to_SE3_fixed(path2D, args.handlebar_height)
+ if args.visualizer:
+ # TODO: document this somewhere
+ robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})
+ if np.linalg.norm(pin.log6(T_w_e.actInv(pathSE3[0]))) > 1e-2:
+ print("going to initial path position")
+ CrocoIKMPC(args, robot, pathSE3[0])
+ print("initialized!")
CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner)
print("final position:", robot.T_w_e)
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index 59a267fdc2839ca82ea83931f747ddfb9e0db901..0d4fa0f124a0a93b65eae6d8bde07763005abab9 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -135,8 +135,6 @@ class CrocoOCP(abc.ABC):
):
self.xlb = self.xlb[1:]
self.xub = self.xub[1:]
- print(self.xlb)
- print(self.xub)
def constructConstraints(self) -> None:
if self.solver_name == "boxfddp":
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 1f1338c20ee3cfece648c6e9dcba1b91cfe8c273..222dd98db36b8bae6833bb3824a40838b9ec7d83 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -67,25 +67,27 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
if args.debug_prints:
print("CTRL: got no path so i won't move")
robot.sendVelocityCommand(np.zeros(robot.model.nv))
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.v.reshape((robot.model.nv,))
+ log_item["qs"] = q
+ log_item["dqs"] = robot.v
return breakFlag, save_past_item, log_item
if data == "done":
breakFlag = True
+ robot.sendVelocityCommand(np.zeros(robot.model.nv))
+ log_item["qs"] = q
+ log_item["dqs"] = robot.v
+ return breakFlag, save_past_item, log_item
- path_pol, path2D_untimed = data
+ _, 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.model.velocityLimit[:2])
+ 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)
- print("following from", pathSE3[0])
- exit()
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualizer:
@@ -136,29 +138,6 @@ def CrocoEndEffectorPathFollowingMPC(
the path does NOT need to start from your current pose - you need to get to it yourself.
"""
- T_w_e = robot.T_w_e
- data = None
- # get first path
- while data is None:
- path_planner.sendCommand(T_w_e.translation[:2])
- data = path_planner.getData()
- # time.sleep(1 / args.ctrl_freq)
- time.sleep(1)
-
- _, path2D = data
- print(path2D[0])
- path2D = np.array(path2D).reshape((-1, 2))
- pathSE3 = path2D_to_SE3_fixed(path2D, args.handlebar_height)
- if args.visualizer:
- # TODO: document this somewhere
- robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})
- print("i'm at", T_w_e)
- print("going to", pathSE3[0])
- if np.linalg.norm(pin.log6(T_w_e.actInv(pathSE3[0]))) > 1e-2:
- print("running")
- print("error", np.linalg.norm(pin.log6(T_w_e.actInv(pathSE3[0]))))
- CrocoIKMPC(args, robot, pathSE3[0])
- print("initialized!")
ocp = CrocoEEPathFollowingOCP(args, robot, x0)
solver = ocp.getSolver()
diff --git a/python/smc/control/optimal_control/croco_mpc_point_to_point.py b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
index 30882035a5c396066e0825852da8d26be3522354..02bc018de3db160b9d8a4cb00e635037be6eda2b 100644
--- a/python/smc/control/optimal_control/croco_mpc_point_to_point.py
+++ b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
@@ -50,8 +50,9 @@ def CrocoIKMPCControlLoop(args, robot: SingleArmInterface, solver, T_w_goal, _,
log_item["qs"] = x0[: robot.model.nq]
log_item["dqs"] = x0[robot.model.nq :]
- log_item["dqs_cmd"] = vel_cmds
+ # log_item["dqs_cmd"] = vel_cmds
log_item["u_tau"] = us[0]
+ log_item["err_norm"] = np.linalg.norm(err_vector).reshape((1,))
return breakFlag, save_past_dict, log_item
@@ -80,8 +81,9 @@ def CrocoIKMPC(args, robot, T_w_goal, run=True):
log_item = {
"qs": np.zeros(robot.model.nq),
"dqs": np.zeros(robot.model.nv),
- "dqs_cmd": np.zeros(robot.model.nv), # we're assuming full actuation here
+ # "dqs_cmd": np.zeros(robot.model.nv), # we're assuming full actuation here
"u_tau": np.zeros(robot.model.nv),
+ "err_norm": np.zeros(1),
}
save_past_dict = {}
loop_manager = ControlLoopManager(
diff --git a/python/smc/path_generation/path_math/path2d_to_6d.py b/python/smc/path_generation/path_math/path2d_to_6d.py
index a90b9c5fa66a3aff4b3c007cf13bc09722095c68..7687282479a8110a2370d3018a9804ebc24225f5 100644
--- a/python/smc/path_generation/path_math/path2d_to_6d.py
+++ b/python/smc/path_generation/path_math/path2d_to_6d.py
@@ -35,8 +35,8 @@ def path2D_to_SE3_fixed(path2D: np.ndarray, path_height: float):
# TODO: make sure this one makes sense
rotation = np.array(
[
- [-np.cos(thetas[i]), np.sin(thetas[i]), 0.0],
- [-np.sin(thetas[i]), -np.cos(thetas[i]), 0.0],
+ [np.cos(thetas[i]), np.sin(thetas[i]), 0.0],
+ [np.sin(thetas[i]), -np.cos(thetas[i]), 0.0],
[0.0, 0.0, -1.0],
]
)
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 743f4c4c1b9c5e2886a59d4feb6172224574ca31..dace0769317a4bd7b436e8702fd3529ee26559e4 100644
--- a/python/smc/path_generation/path_math/path_to_trajectory.py
+++ b/python/smc/path_generation/path_math/path_to_trajectory.py
@@ -36,7 +36,7 @@ def path2D_timed(args, path2D_untimed, max_base_v):
# the strategy is somewhat reasonable at least:
# assume we're moving at 90% max velocity in the base,
# and use that.
- perc_of_max_v = 0.9
+ perc_of_max_v = 0.5
base_v = perc_of_max_v * max_base_v
# 1) the length of the path divided by 0.9 * max_vel
@@ -69,11 +69,11 @@ def path2D_timed(args, path2D_untimed, max_base_v):
# time is i * args.ocp_dt
for i in range(args.n_knots + 1):
# what it should be but gets stuck if we're not yet on path
- # s = (i * args.ocp_dt) * t_to_s
+ s = (i * args.ocp_dt) * t_to_s
# full path
# NOTE: this should be wrong, and ocp_dt correct,
# but it works better for some reason xd
- s = i * (max_s / args.n_knots)
+ # s = i * (max_s / args.n_knots)
path2D.append(
np.array(
[