From e726b4166e53753b0fd2300d8e750bd20bcd7eb2 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 17 Feb 2025 17:24:26 +0100
Subject: [PATCH] single ee, just ee path following is ok. the relationship
between the base and the arm would have to be weighted appropriately for this
to be runnable on anything, but that's not our concern here
---
development_plan.toml | 28 +++++++++++++-
.../path_following_from_planner.py | 38 ++++++++++++++++---
.../optimal_control/abstract_croco_ocp.py | 2 -
.../croco_mpc_path_following.py | 37 ++++--------------
.../croco_mpc_point_to_point.py | 6 ++-
.../path_generation/path_math/path2d_to_6d.py | 4 +-
.../path_math/path_to_trajectory.py | 6 +--
7 files changed, 76 insertions(+), 45 deletions(-)
diff --git a/development_plan.toml b/development_plan.toml
index 8c4361b..c6cd1e0 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 8470014..e3b7f77 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 59a267f..0d4fa0f 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 1f1338c..222dd98 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 3088203..02bc018 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 a90b9c5..7687282 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 743f4c4..dace076 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(
[
--
GitLab