From 7dc44706090278a8e8b6e1d22bb4d50e21564478 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sat, 1 Mar 2025 01:04:01 +0100
Subject: [PATCH] added comments on what's missing
---
.../cartesian_space_compliant_control.py | 2 +-
.../croco_mpc_path_following.py | 22 +++++--------------
.../path_following_croco_ocp.py | 9 +++++---
python/smc/robots/implementations/heron.py | 2 ++
4 files changed, 14 insertions(+), 21 deletions(-)
diff --git a/python/smc/control/cartesian_space/cartesian_space_compliant_control.py b/python/smc/control/cartesian_space/cartesian_space_compliant_control.py
index 8947a3c..039d1c0 100644
--- a/python/smc/control/cartesian_space/cartesian_space_compliant_control.py
+++ b/python/smc/control/cartesian_space/cartesian_space_compliant_control.py
@@ -61,7 +61,7 @@ def controlLoopCompliantClik(
# add a threshold for the wrench
def compliantMoveL(
T_w_goal: pin.SE3, args: Namespace, robot: ForceTorqueOnSingleArmWrist, run=True
-):
+) -> None:
"""
compliantMoveL
-----
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 c8cb925..c2aa9cf 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -37,7 +37,7 @@ def CrocoBasePathFollowingFromPlannerMPCControlLoop(
robot: MobileBaseInterface,
t: int,
_: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray]]:
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
p = robot.T_w_b.translation[:2]
max_base_v = np.linalg.norm(robot._max_v[:2])
@@ -56,7 +56,7 @@ def CrocoBasePathFollowingFromPlannerMPCControlLoop(
err_vector_base = np.linalg.norm(p - path_base[0][:2]) # z axis is irrelevant
log_item = {}
log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
- return v_cmd, log_item
+ return v_cmd, log_item, {}
def CrocoBasePathFollowingMPC(
@@ -182,7 +182,7 @@ def CrocoEEPathFollowingFromPlannerMPCControlLoop(
xs = np.array(ocp.solver.xs)
v_cmd = xs[1, robot.model.nq :]
- err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0]))
+ err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0])).vector
log_item = {"err_vec_ee": err_vector_ee}
return v_cmd, log_item, {}
@@ -299,20 +299,6 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
pathSE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
- # 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)
-
- ###########################################
- # 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 = robot.T_w_e.rotation
- ###########################################
if args.visualizer:
if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
@@ -407,6 +393,7 @@ def BaseAndEEPathFollowingMPC(
# TODO: the robot put in is a whole body robot,
# which you need to implement
+THIS IS THE STILL BROKEN ONE
def BaseAndDualArmEEPathFollowingMPCControlLoop(
args,
robot,
@@ -543,6 +530,7 @@ def BaseAndDualArmEEPathFollowingMPCControlLoop(
return breakFlag, save_past_dict, log_item
+NOT TEMPLETIZED YET
def BaseAndDualArmEEPathFollowingMPC(args, robot, path_planner):
"""
CrocoEndEffectorPathFollowingMPC
diff --git a/python/smc/control/optimal_control/path_following_croco_ocp.py b/python/smc/control/optimal_control/path_following_croco_ocp.py
index bf1d815..d91b979 100644
--- a/python/smc/control/optimal_control/path_following_croco_ocp.py
+++ b/python/smc/control/optimal_control/path_following_croco_ocp.py
@@ -1,8 +1,8 @@
-# TODO: make a bundle method which solves and immediately follows the traj.
from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface, DualArmWholeBodyInterface
import numpy as np
import crocoddyl
@@ -115,7 +115,7 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
NOTE: the path MUST be time indexed with the SAME time used between the knots
"""
- def __init__(self, args, robot: SingleArmInterface, x0):
+ def __init__(self, args, robot: SingleArmWholeBodyInterface, x0):
goal = None
super().__init__(args, robot, x0, goal)
@@ -178,8 +178,11 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
].cost.residual.reference = pathSE3[-1]
+
+JUST THE DUAL ARM IS MISSING
+
class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
- def __init__(self, args, robot: DualArmInterface, x0):
+ def __init__(self, args, robot: DualArmWholeBodyInterface, x0):
goal = None
super().__init__(args, robot, x0, goal)
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index c8fd585..570ac09 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -60,6 +60,8 @@ class RealHeronRobotManager(AbstractHeronRobotManager):
pass
+TODO: WRITE KNOW STUFF OUT, MAKE COMMENTS FOR YUYAO ON WHAT TO FILL IN
+
"""
TODO: finish
self._speed_slider = 1.0 # const
--
GitLab