From c319fe1a3780c38472cae73989262e9e6abb2d48 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 7 Nov 2024 19:10:33 +0100
Subject: [PATCH] moving closer to mpc, set the functions, set the strategy in
comment above functions. look at
/minimal_examples_crocoddyl/mpc_kuka_reaching.py for code i'm doing this
based on
---
python/examples/crocoddyl_ocp_clik.py | 8 +++-
.../optimal_control/crocoddyl_mpc.py | 46 +++++++++++++++++++
.../crocoddyl_optimal_control.py | 9 ++--
3 files changed, 57 insertions(+), 6 deletions(-)
create mode 100644 python/ur_simple_control/optimal_control/crocoddyl_mpc.py
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
index 4e481fd..986ce88 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -3,7 +3,7 @@ import time
import argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, CrocoIKOCP
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, createCrocoIKOCP, solveCrocoOCP
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
@@ -28,7 +28,11 @@ if __name__ == "__main__":
# getting from current to goal end-effector position.
# reference is position and velocity reference (as a dictionary),
# while solver is a crocoddyl object containing a lot more information
- reference, solver = CrocoIKOCP(args, robot, Mgoal)
+ # starting state
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
+ problem = createCrocoIKOCP(args, robot, x0, Mgoal)
+ # this shouldn't really depend on x0 but i can't be bothered
+ reference, solver = solveCrocoOCP(args, robot, problem, x0)
if args.solver == "boxfddp":
log = solver.getCallbacks()[1]
crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
new file mode 100644
index 0000000..d3c5d1a
--- /dev/null
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -0,0 +1,46 @@
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP
+import pinocchio as pin
+import numpy as np
+from functools import partial
+
+
+
+# solve ocp in side process
+# this is that function
+# no it can't be a control loop then
+# the actual control loop is trajectory following,
+# but it continuously checks whether it can get the new path
+# the path needs to be time-index because it's solved on old data
+# (it takes time to solve the problem, but the horizon is then longer than thing).
+
+# actually, before that, try solving the mpc at 500Hz with a short horizon
+# and a small number of iterations, that might be feasible too,
+# we need to see, there's no way to know.
+# but doing it that way is certainly much easier to implement
+# and probably better.
+# i'm pretty sure that's how croco devs & mim_people do mpc
+
+# TODO: FINISH
+def CrocoIKMPCControlLoop(solver, robot : RobotManager, i, past_data):
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
+ solver.problem.x0 = x0
+ xs_init = list(solver.xs[1:]) + [solver.xs[-1]]
+ xs_init[0] = x0
+ us_init = list(solver.us[1:]) + [solver.us[-1]]
+ solver.solve(xs_init, us_init, args.max_solver_iter)
+ pass
+
+# TODO: FINISH
+def IKMPC(args, robot, x0, goal):
+ problem = createCrocoIKOCP(args, robot, problem, x0)
+ if args.solver == "boxfddp":
+ solver = crocoddyl.SolverBoxFDDP(problem)
+ if args.solver == "csqp":
+ solver = mim_solvers.SolverCSQP(problem)
+
+ x0 = np.concatenate([robot.getQ(), robot.getQd()])
+ xs = [x0] * (solver.problem.T + 1)
+ us = solver.problem.quasiStatic([x0] * solver.problem.T)
+ CrocoMPCControlLoop(...)
+ CrocoMPCControlLoop.run()
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index f6b2a43..7d3b7cb 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -22,7 +22,7 @@ def get_OCP_args(parser : argparse.ArgumentParser):
help="optimal control problem solver you want", choices=["boxfddp", "csqp"])
return parser
-def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
+def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# create torque bounds which correspond to percentage
# of maximum allowed acceleration
# NOTE: idk if this makes any sense, but let's go for it
@@ -31,8 +31,6 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
#robot.model.effortLimit = robot.model.effortLimit * (args.acceleration / robot.MAX_ACCELERATION)
#robot.model.effortLimit = robot.model.effortLimit * 0.5
#robot.data = robot.model.createData()
- # starting state
- x0 = np.concatenate([robot.getQ(), robot.getQd()])
state = crocoddyl.StateMultibody(robot.model)
# command input IS torque
# TODO: consider ActuationModelFloatingBaseTpl for heron
@@ -185,6 +183,10 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
# now we define the problem
problem = crocoddyl.ShootingProblem(x0, [runningModel] * args.n_knots, terminalModel)
+ return problem
+
+# this shouldn't really depend on x0 but i can't be bothered
+def solveCrocoOCP(args, robot, problem, x0):
# and the solver
# TODO try out the following solvers from mim_solvers:
# - csqp
@@ -208,7 +210,6 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
xs = [x0] * (solver.problem.T + 1)
us = solver.problem.quasiStatic([x0] * solver.problem.T)
- print("set goal:", goal)
start = time.time()
solver.solve(xs, us, 500, False, 1e-9)
end = time.time()
--
GitLab