diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py index e992cb3b079b218d5b9f3c42d5356270fccf8ad7..90cd8498bcdc42ac3ab90468d6e3de634bb8ad86 100644 --- a/python/examples/crocoddyl_mpc.py +++ b/python/examples/crocoddyl_mpc.py @@ -1,13 +1,76 @@ -""" -TODO: this should be as generic as possible -a function that continuously solves an ocp and executes -trajectory following on it in the meantime -(that's faster than mpc rate obv). - -now it can't be completely general as it's calling -on different solvers, -but this can probably be tackled with some ifs. - -although now that i think of it having (abstract) classes -makes things more readable ? -""" +import numpy as np +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, createCrocoIKOCP, solveCrocoOCP +from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC +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 +from ur_simple_control.clik.clik import getClikArgs +import pinocchio as pin +import crocoddyl + + +def get_args(): + parser = getMinimalArgParser() + parser = get_OCP_args(parser) + parser = getClikArgs(parser) # literally just for goal error + args = parser.parse_args() + return args + + +if __name__ == "__main__": + args = get_args() + robot = RobotManager(args) + # TODO: put this back for nicer demos + #Mgoal = robot.defineGoalPointCLI() + Mgoal = pin.SE3.Random() + + robot.Mgoal = Mgoal.copy() + if args.visualize_manipulator: + # TODO document this somewhere + robot.manipulator_visualizer_queue.put( + {"Mgoal" : Mgoal}) + # create and solve the optimal control problem of + # 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 + # starting state + x0 = np.concatenate([robot.getQ(), robot.getQd()]) + problem = createCrocoIKOCP(args, robot, x0, Mgoal) + + # NOTE: this might be useful if we solve with a large time horizon, + # lower frequency, and then follow the predicted trajectory with velocity p-control + # 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) + #if args.solver == "csqp": + # log = solver.getCallbacks()[1] + # mim_solvers.plotOCSolution(log.xs, log.us, figIndex=1, show=True) + + # we need a way to follow the reference trajectory, + # both because there can be disturbances, + # and because it is sampled at a much lower frequency + #followKinematicJointTrajP(args, robot, reference) + + CrocoIKMPC(args, robot, x0, Mgoal) + + print("final position:") + print(robot.getT_w_e()) + + robot.log_manager.plotAllControlLoops() + + if not args.pinocchio_only: + robot.stopRobot() + + if args.visualize_manipulator: + robot.killManipulatorVisualizer() + + if args.save_log: + robot.log_manager.saveLog() + #loop_manager.stopHandler(None, None) + diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run index 297d01f2c8a2e1b65b1584915dfdc4b840769aa3..ff992b84184343c3d9e4f6270f795b27a195a77f 100644 Binary files a/python/examples/data/latest_run and b/python/examples/data/latest_run differ diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py index d3c5d1ae9a74a1df098821fca9469e72b11175e9..833a629e83be093317c49121020419c044232609 100644 --- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py +++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py @@ -1,6 +1,7 @@ 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 crocoddyl import numpy as np from functools import partial @@ -22,25 +23,69 @@ from functools import partial # i'm pretty sure that's how croco devs & mim_people do mpc # TODO: FINISH -def CrocoIKMPCControlLoop(solver, robot : RobotManager, i, past_data): +def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data): + breakFlag = False + log_item = {} + save_past_dict = {} + + # check for goal + SEerror = robot.getT_w_e().actInv(robot.Mgoal) + err_vector = pin.log6(SEerror).vector + if np.linalg.norm(err_vector) < robot.args.goal_error: +# print("Convergence achieved, reached destionation!") + breakFlag = True + + # set initial state from sensor x0 = np.concatenate([robot.getQ(), robot.getQd()]) solver.problem.x0 = x0 + # warmstart solver with previous solution 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 + xs = np.array(solver.xs) + us = np.array(solver.us) + vel_cmds = xs[1, robot.model.nq:] + robot.sendQd(vel_cmds) -# TODO: FINISH -def IKMPC(args, robot, x0, goal): - problem = createCrocoIKOCP(args, robot, problem, x0) + log_item['qs'] = x0[:robot.model.nq] + log_item['dqs'] = x0[robot.model.nq:] + log_item['dqs_cmd'] = vel_cmds + log_item['u_tau'] = us[0] + + return breakFlag, save_past_dict, log_item + +def CrocoIKMPC(args, robot, x0, goal): + """ + IKMPC + ----- + 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) + """ + problem = createCrocoIKOCP(args, robot, x0, goal) if args.solver == "boxfddp": solver = crocoddyl.SolverBoxFDDP(problem) if args.solver == "csqp": solver = mim_solvers.SolverCSQP(problem) + # technically should be done in controlloop because now + # it's solved 2 times before the first command, + # but we don't have time for details rn x0 = np.concatenate([robot.getQ(), robot.getQd()]) - xs = [x0] * (solver.problem.T + 1) - us = solver.problem.quasiStatic([x0] * solver.problem.T) - CrocoMPCControlLoop(...) - CrocoMPCControlLoop.run() + xs_init = [x0] * (solver.problem.T + 1) + us_init = solver.problem.quasiStatic([x0] * solver.problem.T) + solver.solve(xs_init, us_init, args.max_solver_iter) + + controlLoop = partial(CrocoIKMPCControlLoop, args, robot, solver) + log_item = { + 'qs' : np.zeros(robot.model.nq), + 'dqs' : np.zeros(robot.model.nq), + 'dqs_cmd' : np.zeros(robot.model.nv), # we're assuming full actuation here + 'u_tau' : np.zeros(robot.model.nv), + } + save_past_dict = {} + loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item) + loop_manager.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 7d3b7cb9f3a598828e5a91f66e2a6263d57d6fc7..850c8452b1ec28f7bdebc604521b0c61316c1878 100644 --- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py +++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py @@ -20,6 +20,8 @@ def get_OCP_args(parser : argparse.ArgumentParser): if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).") parser.add_argument("--solver", type=str, default="boxfddp", \ help="optimal control problem solver you want", choices=["boxfddp", "csqp"]) + parser.add_argument("--max-solver-iter", type=int, default=200, \ + help="number of iterations the ocp solver takes. ~100-500 for just ocp (it can converge before ofc), ~10 for mpc") return parser def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):