Commit 58174d6e authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

Merge branch 'devel' into 'master'

Second iteration of DDP solver

Closes #34

See merge request loco-3d/crocoddyL!36
parents 172a4a2a fb9fafb5
Pipeline #3975 failed with stage
in 8 seconds
*.pyc
*.pyd
*.pyo
*~
\ No newline at end of file
*~
*.orig
\ No newline at end of file
image: gepgitlab.laas.fr:4567/loco-3d/cddp
test:
script:
- cd unittest && python all.py
[submodule "models/hyq_description"]
path = models/hyq_description
[submodule "examples/hyq_description"]
path = examples/hyq_description
url = git@github.com:robot-locomotion/hyq-description.git
[submodule "models/talos_data"]
path = models/talos_data
url = git@gepgitlab.laas.fr:pyrene-dev/talos_data.git
FROM gepgitlab.laas.fr:4567/gepetto/buildfarm/robotpkg-jrl:16.04
RUN apt-get update -qqy \
&& apt-get install -qqy \
robotpkg-py27-pinocchio \
robotpkg-talos-data \
&& rm -rf /var/lib/apt/lists/*
BSD 3-Clause License
Copyright (C) 2014-2018 Carlos Mastalli - LAAS-CNRS & Istituto Italiano di Tecnologia.
Copyright note valid unless otherwise stated in individual files.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact RObot COntrol by Differential DYnamic programming Library (crocoddyL)
===============================================
## <img align="center" height="20" src="https://i.imgur.com/vAYeCzC.png"/> Introduction
**Crocoddyl** is an optimal control library for robot control under contact sequence. Its solver is based on an efficient Differential Dynamic Programming (DDP) algorithm. **Crocoddyl** computes optimal trajectories along to optimal feedback gains. It uses **Pinocchio** for fast computation of robot dynamics and its analytical derivatives.
The source code is released under a [BSD 3-Clause license](LICENSE).
**Author: Carlos Mastalli and Rohan Budhiraja <br />
With support from the Gepetto team at LAAS - CNRS<br />**
[![License BSD-3-Clause](https://img.shields.io/badge/license-BSD--3--Clause-blue.svg?style=flat)](https://tldrlegal.com/license/bsd-3-clause-license-%28revised%29#fulltext)
[![pipeline status](https://gepgitlab.laas.fr/loco-3d/cddp/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/loco-3d/cddp/commits/master)
[![coverage report](https://gepgitlab.laas.fr/loco-3d/cddp/badges/master/coverage.svg)](https://gepgitlab.laas.fr/loco-3d/cddp/commits/master)
If you want to follow the current developments, you can directly refer to the [devel branch](https://gepgitlab.laas.fr/loco-3d/cddp/tree/devel).
## <img align="center" height="20" src="https://i.imgur.com/x1morBF.png"/> Installation
**Crocodddyl** has the following dependecies:
* boost (unit_test_framework)
* eigen3
* [pinocchio](https://github.com/stack-of-tasks/pinocchio)
To install eigen3 on Ubuntu you can use apt-get:
sudo apt-get install libeigen3-dev
To install [pinocchio](https://github.com/stack-of-tasks/pinocchio) follow the instruction on its website.
## <img align="center" height="20" src="http://www.pvhc.net/img205/oohmbjfzlxapxqbpkawx.png"/> Publications
R. Budhiraja, J. Carpentier, C. Mastalli, N. Mansard. [Differential Dynamic Programming for Multi-Phase Rigid Contact Dynamics](https://hal.archives-ouvertes.fr/hal-01851596/document), International Conference on Humanoid Robots (ICHR), 2018
from solver import *
from system import *
from cost import *
from data import *
from math import *
from utils import *
from cost import *
from soft_constraint import *
from cost_manager import *
from quadratic_cost import *
from xu_cost import *
from lie_cost import *
from com_cost import *
from joint_lim import *
\ No newline at end of file
from cddp.cost import RunningResidualQuadraticCost
import numpy as np
import pinocchio as se3
import math
class CoMRunningCost(RunningResidualQuadraticCost):
""" Running cost given a desired CoM position.
This cost function can be only defined for geometric systems. For these
systems, the state x=(q,v) contains the configuration point q\in Q and its the
tangent velocity v\in TxQ. The dimension of the configuration space Q, its
tangent space TxQ and the control (input) vector are nq, nv and m,
respectively.
"""
def __init__(self, model, com_des):
""" Construct the running cost for a desired SE3.
It requires the robot model created by Pinocchio and the desired CoM
position.
:param model: Pinocchio model
:com_des: desired CoM position
"""
self._model = model
self._data = self._model.createData()
self.com_des = com_des
RunningResidualQuadraticCost.__init__(self, 3)
def r(self, system, data, x, u):
""" Compute CoM error vector and store the result in data.
We computed the CoM position given a state x.
:param system: system
:param data: running cost data
:param x: state vector [joint configuration, joint velocity]
:param u: control vector
:returns: SE3 error
"""
# Computing the actual CoM position
q = x[:self._model.nq]
act_com = se3.centerOfMass(self._model, self._data, q)
# CoM error
np.copyto(data.r, act_com - self.com_des)
return data.r
def rx(self, system, data, x, u):
""" Compute the state Jacobian of the CoM error vector and store the result
in data.
:param system: system
:param data: running cost data
:param x: state vector [joint configuration, joint velocity]
:param u: control vector
:returns: state Jacobian of the SE3 error vector
"""
q = x[:self._model.nq]
data.rx[:,:self._model.nv] = \
se3.jacobianCenterOfMass(self._model, self._data, q)
return data.rx
def ru(self, system, data, x, u):
""" Compute the control Jacobian of the CoM error vector and store the result
in data.
:param system: system
:param data: running cost data
:param x: state vector [joint configuration, joint velocity]
:param u: control vector
:returns: control Jacobian of the SE3 error vector
"""
# This should return an null Jacobian
return data.ru
\ No newline at end of file
import abc
class XCost(object):
""" This abstract class declares virtual methods for computing the terminal
cost value and its derivatives.
The running cost depends on the state vectors, which it has n values.
"""
__metaclass__ = abc.ABCMeta
@abc.abstractmethod
def createData(self, n):
""" Creates the terminal cost data structure.
:param n: dimension of the state
"""
pass
@abc.abstractmethod
def l(self, system, data, x):
""" Evaluates the terminal cost function and stores the result in data.
:param system: system
:param data: terminal cost data
:param x: state vector
:returns: terminal cost
"""
pass
@abc.abstractmethod
def lx(self, system, data, x):
""" Evaluates the Jacobian of the terminal cost function and stores the
result in data.
:param system: system
:param data: terminal cost data
:param x: state vector
:returns: Jacobian of the terminal cost
"""
pass
@abc.abstractmethod
def lxx(self, system, data, x):
""" Evaluates the Hessian of the terminal cost function and stores the
result in data.
:param system: system
:param data: terminal cost data
:param x: state vector
:returns: Hessian of the terminal cost
"""
pass
class XUCost(object):
""" This abstract class declares virtual methods for computing the running
cost value and its derivatives.
The running cost depends on the state and control vectors, those vectors have
n and m dimensions, respectively.
"""
__metaclass__ = abc.ABCMeta
@abc.abstractmethod
def createData(self, n, m):
""" Creates the terminal cost data structure
:param n: dimension of the state
:param m: dimensions of the control
"""
pass
@abc.abstractmethod
def l(self, system, data, x, u):
""" Evaluates the running cost function and stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: running cost
"""
pass
@abc.abstractmethod
def lx(self, system, data, x, u):
""" Evaluates the Jacobian of the running cost function w.r.t. the state
and stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: Jacobian of the running cost w.r.t. the state
"""
pass
@abc.abstractmethod
def lu(self, system, data, x, u):
""" Evaluates the Jacobian of the running cost function w.r.t. the control
and stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: Jacobian of the residual vector w.r.t. the control
"""
pass
@abc.abstractmethod
def lxx(self, system, data, x, u):
""" Evaluates the Hessian of the running cost w.r.t. the state function and
stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: Hessian of the running cost w.r.t. the state
"""
pass
@abc.abstractmethod
def luu(self, system, data, x, u):
""" Evaluates the Hessian of the running cost function w.r.t. the control
and stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: Hessian of the running cost w.r.t. the control
"""
pass
@abc.abstractmethod
def lux(self, system, data, x, u):
""" Evaluates the running cost derivatives w.r.t. the control and state and
stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: derivatives of the running cost w.r.t. the state and control
"""
pass
class TerminalCost(XCost):
""" This abstract class declares virtual methods for computing the terminal
cost value and its derivatives.
An important remark here is that the terminal cost is computed from linear
residual vector.
"""
__metaclass__ = abc.ABCMeta
def createData(self, n):
""" Creates the terminal cost data structure.
:param n: dimension of the state
"""
from cddp.data import TerminalCostData
return TerminalCostData(n)
@abc.abstractmethod
def xr(self, system, data, x):
""" Evaluates the residual vector and stores the result in data.
:param system: system
:param data: terminal cost data
:param x: state vector
:returns: state-residual vector
"""
pass
class TerminalResidualCost(XCost):
""" This abstract class declares virtual methods for computing the terminal
cost value and its derivatives.
An important remark here is that the terminal cost is computed from general
residual vector r(x). Therefore, compared to the TerminalCost class, it is
needed additionally to provide information about the residual derivatives
w.r.t. the state rx.
"""
__metaclass__ = abc.ABCMeta
def __init__(self, k):
""" Constructs a terminal cost and its residual vector data.
The residual vector dimension is defined by the user and passed it in the
construction of this class. It generates later the appropriate data
structure of the user-defined terminal cost function.
:param k: residual vector dimension
"""
# Residual vector dimension
self.k = k
def createData(self, n):
""" Creates the terminal cost data structure and its residual.
:param n: dimension of the state
"""
from cddp.data import TerminalResidualCostData
return TerminalResidualCostData(n, self.k)
@abc.abstractmethod
def r(self, system, data, x):
""" Evaluates the residual vector and stores the result in data.
:param system: system
:param data: terminal cost data
:param x: state vector
:returns: residual vector
"""
pass
@abc.abstractmethod
def rx(self, system, data, x):
""" Evaluates the Jacobian of the residual vector and stores the result in
data.
:param system: system
:param data: terminal cost data
:param x: state vector
:returns: Jacobian of the residual vector
"""
pass
class RunningCost(XUCost):
""" This abstract class declares virtual methods for computing the running
cost value and its derivatives.
An important remark here is that the running cost is computed from linear
residual vector on the state xr and control ur.
"""
__metaclass__ = abc.ABCMeta
def createData(self, n, m):
"""
:param n: dimension of the state
:param m: dimensions of the control
"""
from cddp.data import RunningCostData
return RunningCostData(n, m)
@abc.abstractmethod
def xr(self, system, data, x, u):
""" Evaluates the residual state vector and stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: state-residual vector
"""
pass
@abc.abstractmethod
def ur(self, system, data, x, u):
""" Evaluates the residual control vector and stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: control-residual vector
"""
pass
class RunningResidualCost(XUCost):
""" This abstract class declares virtual methods for computing the running
cost value and its derivatives.
An important remark here is that the running cost is computed from general
residual vector on the state and control, i.e. r(x,u). Therefore, compared
to the RunningCost class, it is additionally needed to provide the information
of the residual derivatives w.r.t. the state rx and control ru. The residual
Hessians (rxx, ruu and rux) are neglected in Gauss-Newton steps.
"""
__metaclass__ = abc.ABCMeta
def __init__(self, k):
""" Constructs a terminal cost and its residual vector data.
The residual vector dimension is defined by the user and passed it in the
construction of this class. It generates later the appropriate data
structure of the user-defined terminal cost function.
:param k: residual vector dimension
"""
# Residual vector dimension
self.k = k
def createData(self, n, m):
""" Creates the data structure for the running cost and its residual.
:param n: dimension of the state
:param m: dimensions of the control
"""
from cddp.data import RunningResidualCostData
return RunningResidualCostData(n, m, self.k)
@abc.abstractmethod
def r(self, system, data, x, u):
""" Evaluates the residual vector and stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: residual vector
"""
pass
@abc.abstractmethod
def rx(self, system, data, x, u):
""" Evaluates the Jacobian of the residual vector w.r.t. the state and
stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: Jacobian of the residual vector w.r.t. the state
"""
pass
@abc.abstractmethod
def ru(self, system, data, x, u):
""" Evaluates the Jacobian of the residual vector w.r.t. the control and
stores the result in data.
:param system: system
:param data: running cost data
:param x: state vector
:param u: control vector
:returns: Jacobian of the residual vector w.r.t. the control
"""
pass
import abc
import numpy as np
from cddp.utils import assertClass
class CostManager(object):
""" It computes the total cost and its derivatives for a set of running and
terminal costs.
The cost manager stacks a set of terminal and running cost, and from them,
it computes the total cost and its derivatives. The derivatives are Jacobian
and Hessian with respect to the state and control vectors. Each cost function
and the total has its own data, which it is allocated by calling the
createData function. Note that before doing that, you have to add the
running and terminal cost functions of your problem.
"""
__metaclass__ = abc.ABCMeta
def __init__(self):
self.terminal = []
self.running = []
def addTerminal(self, cost):
""" Add a terminal cost object to the cost manager.
Before adding it, it checks if this is a terminal cost objects.
"""
assertClass(cost, 'XCost')
self.terminal.append(cost)
def addRunning(self, cost):
""" Add a running cost object to the cost manager.
Before adding it, it checks if this is a terminal cost objects.
"""
assertClass(cost, 'XUCost')
self.running.append(cost)
def createTerminalData(self, n):
""" Creates the data of the stack of terminal costs.
Before creating the data, it's needed to add all the terminal costs
of your problem.
"""
from cddp.data import XCostData, CostManagerData
data = CostManagerData()
# Creating the terminal cost data, the residual data isn't needed
data.total = XCostData(n)
# Creating the terminal stack-of-cost data
data.soc = [cost.createData(n) for cost in self.terminal]
return data
def createRunningData(self, n, m):
""" Create the data of the stack of running costs.
Before creating the data, it's needed to add all the running costs of your
problem.
"""
from cddp.data import XUCostData, CostManagerData
data = CostManagerData()
# Creating the running cost data, the residual data isn't needed
data.total = XUCostData(n, m)
# Creating the running stack-of-cost data
data.soc = [cost.createData(n, m) for cost in self.running]
return data
def computeTerminalCost(self, system, data, x):
assert self.terminal > 0, "You didn't add the terminal costs"
assertClass(data.total, 'XCostData')
l = data.total.l[0]
l.fill(0.)
for k, cost in enumerate(self.terminal):
cost_data = data.soc[k]
l += cost.l(system, cost_data, x)
return l
def computeRunningCost(self, system, data, x, u, dt):
assert self.running > 0, "You didn't add the running costs"
assertClass(data.total, 'XUCostData')
# Computing the running cost
l = data.total.l[0]
l.fill(0.)
for k, cost in enumerate(self.running):
cost_data = data.soc[k]
l += cost.l(system, cost_data, x, u)
# Numerical integration of the cost function
# TODO we need to use the quadrature class for this
l *= dt
return l