|
crocoddyl
1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
|
|
|
crocoddyl
1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
|
|
Crocoddyl is an optimal control library for robot control under contact sequence. Its solvers are based on novel and efficient Differential Dynamic Programming (DDP) algorithms. Crocoddyl computes optimal trajectories along with optimal feedback gains. It uses Pinocchio for fast computation of robots dynamics and their analytical derivatives.
Crocoddyl is open-source, mostly written in C++ with Python bindings, and distributed under the BSD licence. It is one of the most efficient libraries for computing the optimal control with particular enphasis to contact dynamics.
Crocoddyl is versatible:
Crocoddyl is efficient and flexible:
 |   |
In this documentation, you will find the usual description of the library functionalities, a quick tutorial to catch over the mathematics behind the implementation, a bunch of examples about how to implement optimal control problems and a set of practical exercices for beginners.
Crocoddyl is best installed from APT packaging on Ubuntu 16.04 and 18.04, from our repository. <– On Mac OS X, we support the installation of Pinocchio through the Homebrew package manager. --> On systems for which binaries are not provided, installation from source should be straightforward. Every release is validated in the main Linux distributions and Mac OS X.
We start with a simple optimal control formulation to reach a goal position give the end-effector.
| arm_manipulation.py |
|---|
import os
import sys
import crocoddyl
import pinocchio
import numpy as np
import example_robot_data
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# In this example test, we will solve the reaching-goal task with the Talos arm.
# For that, we use the forward dynamics (with its analytical derivatives)
# developed inside crocoddyl; it describes inside DifferentialActionModelFullyActuated class.
# Finally, we use an Euler sympletic integration scheme.
# First, let's load the Pinocchio model for the Talos arm.
talos_arm = example_robot_data.load('talos_arm')
robot_model = talos_arm.model
# Create a cost model per the running and terminal action model.
state = crocoddyl.StateMultibody(robot_model)
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
# Note that we need to include a cost model (i.e. set of cost functions) in
# order to fully define the action model for our optimal control problem.
# For this particular example, we formulate three running-cost functions:
# goal-tracking cost, state and control regularization; and one terminal-cost:
# goal cost. First, let's create the common cost functions.
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(state, robot_model.getFrameId("gripper_left_joint"),
pinocchio.SE3(np.eye(3), np.array([.0, .0, .4])))
uResidual = crocoddyl.ResidualModelControl(state)
xResidual = crocoddyl.ResidualModelControl(state)
goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
xRegCost = crocoddyl.CostModelResidual(state, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
# Then let's added the running and terminal cost functions
runningCostModel.addCost("gripperPose", goalTrackingCost, 1)
runningCostModel.addCost("xReg", xRegCost, 1e-4)
runningCostModel.addCost("uReg", uRegCost, 1e-4)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1)
# Next, we need to create an action model for running and terminal knots. The
# forward dynamics (computed using ABA) are implemented
# inside DifferentialActionModelFullyActuated.
actuationModel = crocoddyl.ActuationModelFull(state)
dt = 1e-3
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, runningCostModel), dt)
runningModel.differential.armature = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.])
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.)
terminalModel.differential.armature = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.])
# For this optimal control problem, we define 250 knots (or running action
# models) plus a terminal knot
T = 250
q0 = np.array([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005])
x0 = np.concatenate([q0, pinocchio.utils.zero(robot_model.nv)])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
solver = crocoddyl.SolverDDP(problem)
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
solver.setCallbacks([
crocoddyl.CallbackLogger(),
])
else:
solver.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
solver.solve()
# Plotting the solution and the DDP convergence
if WITHPLOT:
log = solver.getCallbacks()[0]
crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
crocoddyl.plotConvergence(log.costs, log.u_regs, log.x_regs, log.grads, log.stops, log.steps, figIndex=2)
# Visualizing the solution in gepetto-viewer
if WITHDISPLAY:
display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF)
display.displayFromSolver(solver)
|
You can run this script as:
This program loads a robot model through example-robot-data, creates a set of action models per node, configures different cost functions, and solves the optimal control problem with our Differential Dynamic Programming (DDP) solver.
The first paragraph describes the optimal control problem that we want to solve "reaching-goal task with the Talos arm". We have developed custom differential action model (action model in continuous time) and its analytical derivatives for forward dynamics without contact. This differential action model is called DifferentialActionModelFreeFwdDynamics. We use an Euler sympletic integrator to convert the differential action model into an action model. Note that our solvers use action model.
To handle the differential manifold of the configuration space, we have developed a state multibody class which can be used with any robotics problem. We create two kind of action models in this example: running and terminal models. In the terminal model we include the desired goal cost function. For the running models, we also include regularization terms that are important for well posing the optimization problem.
We create a trajectory with 250 nodes using the ShootingProblem class. We decouple problem formulation and resoltion through this class. Note that in Crocoddyl we have the freedom to specialized each node with different cost functions, dynamics and constraints.
After that, we create our DDP solver and define a set of callbacks for analysis: display of iteration in Gepetto viewer, and print of iteration values. Finally, we have created custom plot functions for easily check of results.
Crocoddyl is written in C++, with a full template-based C++ API, for code generation and automatic differentiation purposes. All the functionalities are available in C++. Extension of the library should be preferably in C++.
C++ interface is efficient and easy to code. It depends on virtualization with a minimal templatization for codegen and autodiff. The Python API mirrors quite closely the C++ interface. The greatest difference is that the C++ interface is proposed using Eigen objects for matrices and vectors, that are exposed as NumPy matrices in Python.
When working with Crocoddyl, we often suggest to first prototype your ideas in Python. Models, costs and constraints can be derived in Python as well; making the whole prototyping process quite easy. Both the auto-typing and the scripting make it much faster to develop. Once you are happy with your prototype, then translate it in C++ while binding the API to have a mirror in Python that you can use to extend your idea. Currently, the codegen and autodiff are only available in C++ interface. However, it is in our plan to deploy in Python too. Your contributions are welcome!
Happy with Crocoddyl? Please cite us with the following format.
The following is the preferred way to cite Crocoddyl or the feasibility-drive DDP solver. The paper is publicly available in (ArXiv).
This documentation is mostly composed of several examples and tutorials for newcomers, along with a technical documentation and a reference guide. If you want to make sure Crocoddyl matches your needs, you may first want to check the list of features. Several examples in Python will then directly give you the keys to implement the most classical applications based on a Crocoddyl library. For nonexperts, we also provide the main mathematical fundamentals of optimal control. A long tutorial in Python contains everything you need if you are not a Python expert and want to start with Crocoddyl. This tutorial was first written as course material for the MEMMO winter school.
That's it for beginners. We then give an overview of the technical choices we made to write the library and make it efficient. A description of the benchmarks we did to test the library efficiency is also provided.
1.8.17