moving closer to mpc, set the functions, set the strategy in comment above...

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

@@ -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)

python/ur_simple_control/optimal_control/crocoddyl_mpc.py

+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()

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()