From b0672e5bcf2d50e90f0fec29e7db9c160d1a95a3 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 17 Feb 2025 12:27:56 +0100
Subject: [PATCH] fixed the velocity bounds (missed to update them in ocp).
initialization for path following does not work, am trying to fix it now.
ikocp by self runs fine, but when run inside pathfollowing to start it, it
refuses cooperation
---
.../path_following_from_planner.py | 4 +-
examples/log_analysis/log_analysis_example.py | 7 ++-
.../optimal_control/abstract_croco_ocp.py | 6 ++-
.../croco_mpc_path_following.py | 53 +++++++++++++++----
.../croco_mpc_point_to_point.py | 5 +-
python/smc/robots/implementations/ur5e.py | 1 +
.../robots/interfaces/single_arm_interface.py | 2 +-
python/smc/robots/robotmanager_abstract.py | 2 +
.../manipulator_comparison_visualizer.py | 10 ++--
9 files changed, 63 insertions(+), 27 deletions(-)
diff --git a/examples/cart_pulling/path_following_from_planner.py b/examples/cart_pulling/path_following_from_planner.py
index 54581b9..8470014 100644
--- a/examples/cart_pulling/path_following_from_planner.py
+++ b/examples/cart_pulling/path_following_from_planner.py
@@ -32,8 +32,8 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = getRobotFromArgs(args)
- robot._q[0] = 9.0
- robot._q[1] = 4.0
+ # 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])
diff --git a/examples/log_analysis/log_analysis_example.py b/examples/log_analysis/log_analysis_example.py
index 4d92c97..ea531f1 100644
--- a/examples/log_analysis/log_analysis_example.py
+++ b/examples/log_analysis/log_analysis_example.py
@@ -1,16 +1,15 @@
import pinocchio as pin
import numpy as np
import argparse
-from smc.util.logging_utils import LogManager
-from smc.visualize.manipulator_comparison_visualizer import (
+from smc.logging.logger import Logger
+from smc.visualization.manipulator_comparison_visualizer import (
getLogComparisonArgs,
- ManipulatorComparisonManager,
)
if __name__ == "__main__":
args = getLogComparisonArgs()
- log_manager = LogManager(None)
+ log_manager = Logger(None)
log_manager.loadLog(args.log_file1)
# mcm = ManipulatorComparisonManager(args)
# mcm.visualizeWholeRuns()
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index fd68018..59a267f 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -105,10 +105,10 @@ class CrocoOCP(abc.ABC):
# the first state is unlimited there idk what that means really,
# but the arm's base isn't doing a full rotation anyway, let alone 2 or more
self.xlb = np.concatenate(
- [self.robot.model.lowerPositionLimit, -1 * self.robot.model.velocityLimit]
+ [self.robot.model.lowerPositionLimit, -1 * self.robot._max_v]
)
self.xub = np.concatenate(
- [self.robot.model.upperPositionLimit, self.robot.model.velocityLimit]
+ [self.robot.model.upperPositionLimit, self.robot._max_v]
)
# NOTE: in an ideal universe this is handled elsewhere
@@ -135,6 +135,8 @@ 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 2ce182f..1f1338c 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -1,4 +1,5 @@
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.optimal_control.croco_mpc_point_to_point import CrocoIKMPC
from smc.control.control_loop_manager import ControlLoopManager
from smc.multiprocessing.process_manager import ProcessManager
from smc.control.optimal_control.path_following_croco_ocp import (
@@ -16,6 +17,9 @@ import numpy as np
from functools import partial
import types
import importlib.util
+from argparse import Namespace
+import pinocchio as pin
+import time
if importlib.util.find_spec("mim_solvers"):
try:
@@ -45,7 +49,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
"""
breakFlag = False
log_item = {}
- save_past_dict = {}
+ save_past_item = {}
q = robot.q
T_w_e = robot.T_w_e
@@ -65,7 +69,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
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
+ return breakFlag, save_past_item, log_item
if data == "done":
breakFlag = True
@@ -80,10 +84,11 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
# 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:
- # if t % 20 == 0:
if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
robot.visualizer_manager.sendCommand({"frame_path": pathSE3})
@@ -115,19 +120,45 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
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 breakFlag, save_past_item, log_item
-def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
+def CrocoEndEffectorPathFollowingMPC(
+ args: Namespace,
+ robot: SingleArmInterface,
+ x0: np.ndarray,
+ path_planner: ProcessManager,
+):
"""
CrocoEndEffectorPathFollowingMPC
-----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq).
+ follow a fixed pre-determined path, or a path received from a planner.
+ 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()
@@ -143,9 +174,9 @@ def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
CrocoEndEffectorPathFollowingMPCControlLoop, args, robot, solver, path_planner
)
log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
- save_past_dict = {}
+ save_past_item = {"T_w_e": robot.T_w_e}
loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
+ robot, controlLoop, args, save_past_item, log_item
)
loop_manager.run()
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 1f643a5..3088203 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
@@ -44,11 +44,12 @@ def CrocoIKMPCControlLoop(args, robot: SingleArmInterface, solver, T_w_goal, _,
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
us = np.array(solver.us)
+ # NOTE: for some reason the first value is always some wild bs
vel_cmds = xs[1, robot.model.nq :]
robot.sendVelocityCommand(vel_cmds)
log_item["qs"] = x0[: robot.model.nq]
- log_item["dqs"] = x0[robot.model.nv :]
+ log_item["dqs"] = x0[robot.model.nq :]
log_item["dqs_cmd"] = vel_cmds
log_item["u_tau"] = us[0]
@@ -78,7 +79,7 @@ def CrocoIKMPC(args, robot, T_w_goal, run=True):
controlLoop = partial(CrocoIKMPCControlLoop, args, robot, solver, T_w_goal)
log_item = {
"qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
"dqs_cmd": np.zeros(robot.model.nv), # we're assuming full actuation here
"u_tau": np.zeros(robot.model.nv),
}
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index ba9768a..dbbcb88 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -40,6 +40,7 @@ class RobotManagerUR5e(ForceTorqueOnSingleArmWrist):
if args.debug_prints:
print("RobotManagerUR5e init")
self._model, self._collision_model, self._visual_model, self._data = get_model()
+ self._ee_frame_id = self.model.getFrameId("tool0")
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
super().__init__(args)
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index c486f97..b6f6e46 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -7,7 +7,7 @@ class SingleArmInterface(AbstractRobotManager):
def __init__(self, args):
if args.debug_prints:
print("SingleArmInterface init")
- self._ee_frame_id: int = self.model.getFrameId("tool0")
+ self._ee_frame_id: int
self._T_w_e: pin.SE3
super().__init__(args)
diff --git a/python/smc/robots/robotmanager_abstract.py b/python/smc/robots/robotmanager_abstract.py
index 0e6cee6..2b94efd 100644
--- a/python/smc/robots/robotmanager_abstract.py
+++ b/python/smc/robots/robotmanager_abstract.py
@@ -86,6 +86,8 @@ class AbstractRobotManager(abc.ABC):
# TODO: each robot should know which grippers are available
# and set this there.
self.gripper = None
+ # initialize things that depend on q here
+ self._step()
####################################################################
# processes and utilities robotmanager owns #
diff --git a/python/smc/visualization/manipulator_comparison_visualizer.py b/python/smc/visualization/manipulator_comparison_visualizer.py
index 9c1785c..5c07d23 100644
--- a/python/smc/visualization/manipulator_comparison_visualizer.py
+++ b/python/smc/visualization/manipulator_comparison_visualizer.py
@@ -1,4 +1,4 @@
-from smc.robots.implementations.simulated_robot import SimulatedRobotManager
+from smc import getRobotFromArgs
from smc.robots.utils import getMinimalArgParser
from smc.multiprocessing.process_manager import ProcessManager
from smc.logging.logger import Logger
@@ -40,8 +40,8 @@ class ManipulatorComparisonManager:
args.pinocchio_only = True
args.simulation = False
- self.robot1 = SimulatedRobotManager(args)
- self.robot2 = SimulatedRobotManager(args)
+ self.robot1 = getRobotFromArgs(args)
+ self.robot2 = getRobotFromArgs(args)
# no two loops will have the same amount of timesteps.
# we need to store the last available step for both robots.
@@ -49,13 +49,13 @@ class ManipulatorComparisonManager:
self.lastq2 = np.zeros(self.robot2.model.nq)
if os.path.exists(args.log_file1):
- self.logm1 = LogManager(None)
+ self.logm1 = Logger(None)
self.logm1.loadLog(args.log_file1)
else:
print("you did not give me a valid path for log1, exiting")
exit()
if os.path.exists(args.log_file2):
- self.logm2 = LogManager(None)
+ self.logm2 = Logger(None)
self.logm2.loadLog(args.log_file2)
else:
print("you did not give me a valid path for log2, exiting")
--
GitLab