diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..e992cb3b079b218d5b9f3c42d5356270fccf8ad7
--- /dev/null
+++ b/python/examples/crocoddyl_mpc.py
@@ -0,0 +1,13 @@
+"""
+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 ?
+"""
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
index 4763547af329a0c642f3441909079a728d079934..4e481fd3c9c5a8c611b8761a8091b6505b56300e 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -29,8 +29,9 @@ if __name__ == "__main__":
# 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)
- log = solver.getCallbacks()[1]
- crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
+ if args.solver == "boxfddp":
+ log = solver.getCallbacks()[1]
+ crocoddyl.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
diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run
index 87c7943d861356b43e2f96d2c2bf7f90abd4efac..297d01f2c8a2e1b65b1584915dfdc4b840769aa3 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_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index e1db433a90b218ca1f63d51fa8fce3992a910c28..f6b2a430d72d1f0381d6f30409267cdad72bc3cc 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -1,6 +1,7 @@
import numpy as np
import pinocchio as pin
import crocoddyl
+import mim_solvers
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
import argparse
from ur_simple_control.basics.basics import followKinematicJointTrajP
@@ -17,9 +18,10 @@ def get_OCP_args(parser : argparse.ArgumentParser):
parser.add_argument("--stop-at-final", type=int, default=1, \
help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
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"])
return parser
-
def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
# create torque bounds which correspond to percentage
# of maximum allowed acceleration
@@ -34,6 +36,11 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
state = crocoddyl.StateMultibody(robot.model)
# command input IS torque
# TODO: consider ActuationModelFloatingBaseTpl for heron
+ # TODO: create a different actuation model (or whatever)
+ # for the mobile base - basically just remove the y movement in the base
+ # and update the corresponding derivates to 0
+ # there's python examples for this, ex. acrobot.
+ # you might want to implement the entire action model too idk what's really necessary here
actuation = crocoddyl.ActuationModelFull(state)
# we will be summing 4 different costs
@@ -73,6 +80,14 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
terminalCostModel.addCost("uReg", uRegCost, 1e3)
terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
+ ######################################################################
+ # state constraints #
+ #################################################
+ # - added to costs via barrier functions for fddp
+ # - added as actual constraints via crocoddyl.constraintmanager for csqp
+ ###########################################################################
+
+
# the 4th cost is for defining bounds via costs
# NOTE: could have gotten the same info from state.lb and state.ub.
# the first state is unlimited there idk what that means really,
@@ -96,53 +111,98 @@ def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
# point activation input and the residual need to be of the same length obviously,
# and this should be 2 * model.nv the way things are defined here.
- # TODO: try using crocoddyl.ConstraintModelResidual
- # instead to create actual box constraints (inequality constraints)
- # TODO: same goes for control input
- # NOTE: i'm not sure whether any solver uses these tho lel
if robot.robot_name == "heron":
xlb = xlb[1:]
xub = xub[1:]
- bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
- xLimitResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
- xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
- print(len(x0), len(xlb), len(xub))
- print(xLimitResidual.nr)
- print(xLimitActivation.nr)
- limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
- runningCostModel.addCost("limitCost", limitCost, 1e3)
- terminalCostModel.addCost("limitCost", limitCost, 1e3)
+ if args.solver == "boxfddp":
+ bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
+ xLimitResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
+ xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
+
+ limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual)
+ runningCostModel.addCost("limitCost", limitCost, 1e3)
+ terminalCostModel.addCost("limitCost", limitCost, 1e3)
+
+ # TODO: try using crocoddyl.ConstraintModelResidual
+ # instead to create actual box constraints (inequality constraints)
+ # TODO: same goes for control input
+ # NOTE: i'm not sure whether any solver uses these tho lel
+ # --> you only do that for mim_solvers' csqp!
+
+ if args.solver == "csqp":
+ # this just store all the constraints
+ constraints = crocoddyl.ConstraintModelManager(state, robot.model.nv)
+ u_constraint = crocoddyl.ConstraintModelResidual(
+ state,
+ uResidual,
+ -1 * robot.model.effortLimit * 0.1,
+ robot.model.effortLimit * 0.1)
+ constraints.addConstraint("u_box_constraint", u_constraint)
+ x_constraint = crocoddyl.ConstraintModelResidual(
+ state,
+ xResidual,
+ xlb,
+ xub)
+ constraints.addConstraint("x_box_constraint", x_constraint)
# Next, we need to create an action model for running and terminal knots. The
# forward dynamics (computed using ABA) are implemented
# inside DifferentialActionModelFreeFwdDynamics.
- runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, runningCostModel
- ),
- args.ocp_dt,
- )
- runningModel.u_lb = -1 * robot.model.effortLimit * 0.1
- runningModel.u_ub = robot.model.effortLimit * 0.1
- terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, terminalCostModel
- ),
- 0.0,
- )
- terminalModel.u_lb = -1 * robot.model.effortLimit * 0.1
- terminalModel.u_ub = robot.model.effortLimit * 0.1
+ if args.solver == "boxfddp":
+ runningModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ state, actuation, runningCostModel
+ ),
+ args.ocp_dt,
+ )
+ runningModel.u_lb = -1 * robot.model.effortLimit * 0.1
+ runningModel.u_ub = robot.model.effortLimit * 0.1
+ terminalModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ state, actuation, terminalCostModel
+ ),
+ 0.0,
+ )
+ terminalModel.u_lb = -1 * robot.model.effortLimit * 0.1
+ terminalModel.u_ub = robot.model.effortLimit * 0.1
+ if args.solver == "csqp":
+ runningModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ state, actuation, runningCostModel, constraints
+ ),
+ args.ocp_dt,
+ )
+ terminalModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ state, actuation, terminalCostModel, constraints
+ ),
+ 0.0,
+ )
+
# now we define the problem
problem = crocoddyl.ShootingProblem(x0, [runningModel] * args.n_knots, terminalModel)
# and the solver
- solver = crocoddyl.SolverBoxFDDP(problem)
+ # TODO try out the following solvers from mim_solvers:
+ # - csqp
+ # - stagewise qp
+ # both of these have generic inequalities you can put in.
+ # and both are basically QP versions of iLQR if i'm not wrong
+ # (i have no idea tho)
+ if args.solver == "boxfddp":
+ solver = crocoddyl.SolverBoxFDDP(problem)
+ if args.solver == "csqp":
+ solver = mim_solvers.SolverCSQP(problem)
+ #solver = mim_solvers.SolverSQP(problem)
#solver = crocoddyl.SolverIpopt(problem)
# TODO: remove / place elsewhere once it works (make it an argument)
- solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()])
+ if args.solver == "boxfddp":
+ solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()])
+ if args.solver == "csqp":
+ solver.setCallbacks([mim_solvers.CallbackVerbose(), mim_solvers.CallbackLogger()])
# run solve
# NOTE: there are some possible parameters here that I'm not using
xs = [x0] * (solver.problem.T + 1)
@@ -181,9 +241,10 @@ if __name__ == "__main__":
reference, solver = CrocoIKOCP(args, robot, goal)
# we only work within -pi - pi (or 0-2pi idk anymore)
#reference['qs'] = reference['qs'] % (2*np.pi) - np.pi
- log = solver.getCallbacks()[1]
- crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
- crocoddyl.plotConvergence(log.costs, log.pregs, log.dregs, log.grads, log.stops, log.steps, figIndex=2)
+ if args.solver == "boxfddp":
+ log = solver.getCallbacks()[1]
+ crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
+ crocoddyl.plotConvergence(log.costs, log.pregs, log.dregs, log.grads, log.stops, log.steps, figIndex=2)
followKinematicJointTrajP(args, robot, reference)
reference.pop('dt')
plotFromDict(reference, args.n_knots + 1, args)