diff --git a/.gitignore b/.gitignore
index 91ea4571fb5bdd8b6b01d4a12ca325eab44cc37f..89a563ee7af31a527387de5f0eea14316bbf31b9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,7 +4,7 @@
*~
*.orig
*.ipynb_checkpoints
-build/*
+build*/*
*.aux
*.log
*.pdf
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a8a58375d9ef7e0efda18bd9edf4fd9d727d87a6..9cc1f6d78a600e3b7581bcdc123470b45a4d16ac 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -41,6 +41,7 @@ OPTION(ENABLE_VECTORIZATION "Enable vectorization and futhers processor-related
OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
OPTION(BUILD_UNIT_TESTS "Build the unitary tests" ON)
OPTION(BUILD_BENCHMARK "Build the benchmark" ON)
+OPTION(BUILD_EXAMPLES "Build the examples" ON)
IF(ENABLE_VECTORIZATION)
@@ -53,6 +54,7 @@ SETUP_PROJECT()
ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
ADD_REQUIRED_DEPENDENCY("eigenpy")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.1.0")
+ADD_REQUIRED_DEPENDENCY("example-robot-data >= 2.1.0")
ADD_OPTIONAL_DEPENDENCY("multicontact-api >= 1.1.0")
ADD_OPTIONAL_DEPENDENCY("quadprog")
ADD_OPTIONAL_DEPENDENCY("scipy")
@@ -88,7 +90,7 @@ IF(BUILD_PYTHON_INTERFACE)
SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python)
FINDPYTHON()
FIND_NUMPY()
- INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
+ INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS} ${NUMPY_INCLUDE_DIRS})
ENDIF(BUILD_PYTHON_INTERFACE)
SET(BOOST_COMPONENTS ${BOOST_REQUIERED_COMPONENTS} ${BOOST_OPTIONAL_COMPONENTS} ${BOOST_BUILD_COMPONENTS})
@@ -119,5 +121,10 @@ IF(BUILD_BENCHMARK)
ADD_SUBDIRECTORY(benchmark)
ENDIF(BUILD_BENCHMARK)
+# Build the examples
+IF(BUILD_EXAMPLES)
+ ADD_SUBDIRECTORY(examples)
+ENDIF(BUILD_EXAMPLES)
+
SETUP_PROJECT_FINALIZE()
diff --git a/README.md b/README.md
index a7e56475d6771fc4d72fdb7388fa79c21d4ef184..63e8382665aeda69b5c8e92e0f8913e922fea269 100644
--- a/README.md
+++ b/README.md
@@ -8,85 +8,111 @@ Contact RObot COntrol by Differential DYnamic programming Library (crocoddyl)
The source code is released under the [BSD 3-Clause license](LICENSE).
**Authors:** [Carlos Mastalli](https://cmastalli.github.io/) and Rohan Budhiraja <br />
-**Instructors:** Justin Carpentier and Nicolas Mansard <br />
+**Instructors:** Nicolas Mansard <br />
**With additional support from the Gepetto team at LAAS-CNRS.**
[](https://tldrlegal.com/license/bsd-3-clause-license-%28revised%29#fulltext)
[](https://gepgitlab.laas.fr/loco-3d/crocoddyl/pipelines?ref=devel)
[](https://gepettoweb.laas.fr/doc/loco-3d/crocoddyl/devel/coverage/)
+[](https://gepgitlab.laas.fr/loco-3d/crocoddyl/-/tags)
+[](https://img.shields.io/github/repo-size/loco-3d/crocoddyl)
+[](https://gepgitlab.laas.fr/loco-3d/crocoddyl/-/graphs/master)
+
+[](https://img.shields.io/github/release-date/loco-3d/crocoddyl)
+[](https://img.shields.io/github/last-commit/loco-3d/crocoddyl)
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
-**Crocoddyl** can be easily installed on various Linux (Ubuntu, Fedora, etc.) and Unix distributions (Mac OS X, BSD, etc.). Please refer to
+**Crocoddyl** can be easily installed on various Linux (Ubuntu, Fedora, etc.) and Unix distributions (Mac OS X, BSD, etc.). Please refer to
### Installation through robotpkg
-You can install this package throught robotpkg. robotpkg is a package manager tailored for robotics softwares. It greatly simplifies the release of new versions along with the management of their dependencies. You just need to add the robotpkg apt repository to your sources.list and then use `sudo apt install robotpkg-py27-crocoddyl`:
+You can install this package through robotpkg. robotpkg is a package manager tailored for robotics softwares. It greatly simplifies the release of new versions along with the management of their dependencies. You just need to add the robotpkg apt repository to your sources.list and then use `sudo apt install robotpkg-py27-crocoddyl`:
If you have never added robotpkg as a softwares repository, please follow first the instructions from 1 to 3. Otherwise, go directly to instruction 4. Those instructions are similar to the installation procedures presented in [http://robotpkg.openrobots.org/debian.html](http://robotpkg.openrobots.org/debian.html).
1. Add robotpkg as source repository to apt:
- sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
- deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub $(lsb_release -sc) robotpkg
- deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg
- EOF
+```bash
+sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
+deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub $(lsb_release -sc) robotpkg
+deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg
+EOF
+```
2. Register the authentication certificate of robotpkg:
- curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+```bash
+curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+```
3. You need to run at least once apt update to fetch the package descriptions:
- sudo apt-get update
+```bash
+sudo apt-get update
+```
4. The installation of Crocoddyl:
- sudo apt install robotpkg-py27-crocoddyl # for Python 2
+```bash
+sudo apt install robotpkg-py27-crocoddyl # for Python 2
- sudo apt install robotpkg-py35-crocoddyl # for Python 3
+sudo apt install robotpkg-py35-crocoddyl # for Python 3
+```
Finally you will need to configure your environment variables, e.g.:
- export PATH=/opt/openrobots/bin:$PATH
- export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
- export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
- export PYTHONPATH=/opt/openrobots/lib/python2.7/site-packages:$PYTHONPATH
+```bash
+export PATH=/opt/openrobots/bin:$PATH
+export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
+export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
+export PYTHONPATH=/opt/openrobots/lib/python2.7/site-packages:$PYTHONPATH
+```
### Building from source
-**Crocoddyl** is c++ library with Python bindings for versatible and fast prototyping. It has the following dependecies:
+**Crocoddyl** is c++ library with Python bindings for versatile and fast prototyping. It has the following dependencies:
* [pinocchio](https://github.com/stack-of-tasks/pinocchio)
* [quadprog](https://pypi.org/project/quadprog/)
* [multicontact-api](https://gepgitlab.laas.fr/loco-3d/multicontact-api)
-* [example-robot-data](https://gepgitlab.laas.fr/gepetto/example-robot-data) (optional for running examples, install Python loaders)
-* [gepetto-viewer-corba](https://github.com/Gepetto/gepetto-viewer-corba) (optional for running examples and notebooks)
-* [jupyter](https://jupyter.org/) (optional for running notebooks)
-* [matplotlib](https://matplotlib.org/) (optional for running examples)
+* [example-robot-data](https://gepgitlab.laas.fr/gepetto/example-robot-data) (optional for examples, install Python loaders)
+* [gepetto-viewer-corba](https://github.com/Gepetto/gepetto-viewer-corba) (optional for display)
+* [jupyter](https://jupyter.org/) (optional for notebooks)
+* [matplotlib](https://matplotlib.org/) (optional for examples)
+
+You can run examples, unit-tests and benchmarks from your build dir:
-You can run examples and tests from the root dir of the repository:
+```bash
+cd build
+make test
+make examples-bipedal_walk
+make benchmarks-bipedal_walk
+```
- cd PATH_TO_CROCODDYL
- python examples/talos_arm.py
- python unittest/all.py
+If you want to see the 3D result and/or graphs of your run examples, you can use
+
+```bash
+export CROCODDYL_DISPLAY=1
+export CROCODDYL_PLOT=1
+```
If you want to learn about Crocoddyl, take a look at the Jupyter notebooks. Start in the following order.
- [examples/notebooks/unicycle_towards_origin.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/unicycle_towards_origin.ipynb)
- [examples/notebooks/cartpole_swing_up.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/cartpole_swing_up.py)
-- [examples/notebooks/manipulator.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/manipulator.ipynb)
-- [examples/notebooks/bipedal_walking_from_foot_traj.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/bipedal_walking_from_foot_traj.ipynb)
+- [examples/notebooks/arm_manipulation.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/arm_manipulation.ipynb)
+- [examples/notebooks/bipedal_walking.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/bipedal_walking.ipynb)
- [examples/notebooks/introduction_to_crocoddyl.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/introduction_to_crocoddyl.ipynb)
## Citing Crocoddyl
To cite **Crocoddyl** in your academic research, please use the following bibtex lines:
-```
+```tex
@misc{crocoddylweb,
author = {Carlos Mastalli, Rohan Budhiraja and Nicolas Mansard and others},
title = {Crocoddyl: a fast and flexible optimal control library for robot control under contact sequence},
@@ -95,11 +121,22 @@ To cite **Crocoddyl** in your academic research, please use the following bibtex
}
```
-and the following paper describes different algorithm implemented in **Crocoddyl**:
+and the following one for the reference to the paper introducing **Crocoddyl**:
+```tex
+@unpublished{mastalli2020crocoddyl,
+ author={Mastalli, Carlos and Budhiraja, Rohan and Merkt, Wolfgang and Saurel, Guilhem and Hammoud, Bilal
+ and Naveau, Maximilien and Carpentier, Justin and Vijayakumar, Sethu and Mansard, Nicolas},
+ title={{Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control}},
+ year={2020}
+}
+```
+
+The rest of the publications describes different component of **Crocoddyl**:
### Publications
-- R. Budhiraja, J. Carpentier, C. Mastalli and N. Mansard. [Differential Dynamic Programming for Multi-Phase Rigid Contact Dynamics](https://hal.archives-ouvertes.fr/hal-01851596/document), IEEE RAS International Conference on Humanoid Robots (ICHR), 2018
+- C. Mastalli et al. [Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control](https://cmastalli.github.io/publications/crocoddyl20unpub.html), pre-print, 2020
+- R. Budhiraja, J. Carpentier, C. Mastalli and N. Mansard. [Differential Dynamic Programming for Multi-Phase Rigid Contact Dynamics](https://cmastalli.github.io/publications/mddp18.html), IEEE RAS International Conference on Humanoid Robots (ICHR), 2018
- Y. Tassa, N. Mansard, E. Todorov. [Control-Limited Differential Dynamic Programming](https://homes.cs.washington.edu/~todorov/papers/TassaICRA14.pdf), IEEE International Conference on Automation and Robotics (ICRA), 2014
- R. Budhiraja, J. Carpentier and N. Mansard. [Dynamics Consensus between Centroidal and Whole-Body Models for Locomotion of Legged Robots](https://hal.laas.fr/hal-01875031/document), IEEE International Conference on Automation and Robotics (ICRA), 2019
@@ -108,3 +145,19 @@ and the following paper describes different algorithm implemented in **Crocoddyl
You have a question or an issue? You may either directly open a [new issue](https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues) or use the mailing list <crocoddyl@laas.fr>.
+
+## Credits
+
+The following people have been involved in the development of **Crocoddyl**:
+
+- [Carlos Mastalli](https://cmastalli.github.io/) (LAAS-CNRS): main developer and manager of the project
+- [Nicolas Mansard](http://projects.laas.fr/gepetto/index.php/Members/NicolasMansard) (LAAS-CNRS): project instructor
+- [Rohan Budhiraja](https://scholar.google.com/citations?user=NW9Io9AAAAAJ) (LAAS-CNRS): features extension
+- [Maximilien Naveau](https://scholar.google.fr/citations?user=y_-cGlUAAAAJ&hl=fr) (MPI): unit-test support
+- [Guilhem Saurel](http://projects.laas.fr/gepetto/index.php/Members/GuilhemSaurel) (LAAS-CNRS): continuous integration and deployment
+- [Bilal Hammoud](https://scholar.google.com/citations?hl=en&user=h_4NKpsAAAAJ) (MPI): features extension
+
+
+## Acknowledgments
+
+The development of **Pinocchio** is supported by the [EU MEMMO project](http://www.memmo-project.eu/), and the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr).
\ No newline at end of file
diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt
index 88be29bb393bca4bc197febc185b6605228ea9fc..f110e1f19dc937fe9edcb6f3a70f1f692062694e 100644
--- a/benchmark/CMakeLists.txt
+++ b/benchmark/CMakeLists.txt
@@ -11,16 +11,16 @@ FOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK})
ENDFOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK})
SET(${PROJECT_NAME}_BENCHMARK_PYTHON
- hyq
- lqr
- talos_arm
- talos_legs
unicycle
+ lqr
+ arm_manipulation
+ bipedal_walk
+ quadrupedal_gaits
)
FOREACH(benchmark ${${PROJECT_NAME}_BENCHMARK_PYTHON})
PYTHON_BUILD(. "${benchmark}.py")
ADD_CUSTOM_TARGET("benchmark-${benchmark}"
- ${CMAKE_COMMAND} -E env PYTHONPATH=${PROJECT_BINARY_DIR}/bindings/python
+ ${CMAKE_COMMAND} -E env PYTHONPATH=$ENV{PYTHONPATH}:${PROJECT_BINARY_DIR}/bindings/python
${PYTHON_EXECUTABLE} -c "from ${benchmark} import *")
ENDFOREACH(benchmark ${${PROJECT_NAME}_BENCHMARK_PYTHON})
diff --git a/benchmark/talos_arm.py b/benchmark/arm_manipulation.py
similarity index 100%
rename from benchmark/talos_arm.py
rename to benchmark/arm_manipulation.py
diff --git a/benchmark/talos_legs.py b/benchmark/bipedal_walk.py
similarity index 100%
rename from benchmark/talos_legs.py
rename to benchmark/bipedal_walk.py
diff --git a/benchmark/lqr.cpp b/benchmark/lqr.cpp
index 19d9f6104f733999f02872b2936b57aace639cc6..d71b9bc4c0a46c4f6d98c9a2cdf033b956e55774 100644
--- a/benchmark/lqr.cpp
+++ b/benchmark/lqr.cpp
@@ -1,3 +1,11 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
#include "crocoddyl/core/states/euclidean.hpp"
#include "crocoddyl/core/actions/lqr.hpp"
#include "crocoddyl/core/utils/callbacks.hpp"
@@ -47,8 +55,8 @@ int main() {
clock_gettime(CLOCK_MONOTONIC, &start);
ddp.solve(xs, us, MAXITER);
clock_gettime(CLOCK_MONOTONIC, &finish);
- elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
- elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+ elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+ elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
duration[i] = elapsed / 1000.;
}
@@ -62,8 +70,8 @@ int main() {
clock_gettime(CLOCK_MONOTONIC, &start);
problem.calc(xs, us);
clock_gettime(CLOCK_MONOTONIC, &finish);
- elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
- elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+ elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+ elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
duration[i] = elapsed / 1000.;
}
@@ -78,8 +86,8 @@ int main() {
clock_gettime(CLOCK_MONOTONIC, &start);
problem.calcDiff(xs, us);
clock_gettime(CLOCK_MONOTONIC, &finish);
- elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
- elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+ elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+ elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
duration[i] = elapsed / 1000.;
}
diff --git a/benchmark/hyq.py b/benchmark/quadrupedal_gaits.py
similarity index 80%
rename from benchmark/hyq.py
rename to benchmark/quadrupedal_gaits.py
index 1f6a3e1fa35b689f8e35ea95f14b06f8a6b850cf..9c934f825e108d8f9e4c89bf5b15001e398587e9 100644
--- a/benchmark/hyq.py
+++ b/benchmark/quadrupedal_gaits.py
@@ -1,3 +1,4 @@
+import sys
import time
import example_robot_data
@@ -9,11 +10,28 @@ from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem
T = int(5e3) # number of trials
MAXITER = 1
-GAIT = "walking" # 115 nodes
-# GAIT = "trotting" # 63 nodes
-# GAIT = "pacing" # 63 nodes
-# GAIT = "bounding" # 63 nodes
-# GAIT = "jumping" # 61 nodes
+WALKING = 'walk' in sys.argv
+TROTTING = 'trot' in sys.argv
+PACING = 'pace' in sys.argv
+BOUNDING = 'bound' in sys.argv
+JUMPING = 'jump' in sys.argv
+
+GAIT = "walking" # 104 nodes
+if WALKING:
+ print('running walking benchmark ...')
+ GAIT = "walking" # 104 nodes
+if TROTTING:
+ print('running trotting benchmark ...')
+ GAIT = "trotting" # 54 nodes
+if PACING:
+ print('running pacing benchmark ...')
+ GAIT = "pacing" # 54 nodes
+if BOUNDING:
+ print('running bounding benchmark ...')
+ GAIT = "bounding" # 54 nodes
+if JUMPING:
+ print('running jumping benchmark ...')
+ GAIT = "jumping" # 61 nodes
def runBenchmark(gait_phase):
@@ -69,11 +87,11 @@ def runBenchmark(gait_phase):
if GAIT == 'walking':
GAITPHASE = {
'walking': {
- 'stepLength': 0.15,
- 'stepHeight': 0.2,
+ 'stepLength': 0.25,
+ 'stepHeight': 0.25,
'timeStep': 1e-2,
'stepKnots': 25,
- 'supportKnots': 5
+ 'supportKnots': 2
}
}
elif GAIT == 'trotting':
@@ -83,7 +101,7 @@ elif GAIT == 'trotting':
'stepHeight': 0.2,
'timeStep': 1e-2,
'stepKnots': 25,
- 'supportKnots': 5
+ 'supportKnots': 2
}
}
elif GAIT == 'pacing':
@@ -93,17 +111,17 @@ elif GAIT == 'pacing':
'stepHeight': 0.2,
'timeStep': 1e-2,
'stepKnots': 25,
- 'supportKnots': 5
+ 'supportKnots': 2
}
}
elif GAIT == 'bounding':
GAITPHASE = {
'bounding': {
- 'stepLength': 0.15,
- 'stepHeight': 0.2,
+ 'stepLength': 0.007,
+ 'stepHeight': 0.05,
'timeStep': 1e-2,
'stepKnots': 25,
- 'supportKnots': 5
+ 'supportKnots': 12
}
}
elif GAIT == 'jumping':
diff --git a/benchmark/unicycle.cpp b/benchmark/unicycle.cpp
index 92a726560e29b55110a93b8d98ea513d86d88117..d1ec001ab9066e59832c3c472bd377448ba1f17c 100644
--- a/benchmark/unicycle.cpp
+++ b/benchmark/unicycle.cpp
@@ -1,3 +1,11 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
#include "crocoddyl/core/actions/unicycle.hpp"
#include "crocoddyl/core/utils/callbacks.hpp"
#include "crocoddyl/core/solvers/ddp.hpp"
@@ -48,8 +56,8 @@ int main() {
clock_gettime(CLOCK_MONOTONIC, &start);
ddp.solve(xs, us, MAXITER);
clock_gettime(CLOCK_MONOTONIC, &finish);
- elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
- elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+ elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+ elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
duration[i] = elapsed / 1000.;
}
@@ -64,8 +72,8 @@ int main() {
clock_gettime(CLOCK_MONOTONIC, &start);
problem.calc(xs, us);
clock_gettime(CLOCK_MONOTONIC, &finish);
- elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
- elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+ elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+ elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
duration[i] = elapsed / 1000.;
}
@@ -80,8 +88,8 @@ int main() {
clock_gettime(CLOCK_MONOTONIC, &start);
problem.calcDiff(xs, us);
clock_gettime(CLOCK_MONOTONIC, &finish);
- elapsed = (finish.tv_sec - start.tv_sec) * 1000000.0;
- elapsed += (finish.tv_nsec - start.tv_nsec) / 1000.0;
+ elapsed = static_cast<double>(finish.tv_sec - start.tv_sec) * 1000000;
+ elapsed += static_cast<double>(finish.tv_nsec - start.tv_nsec) / 1000;
duration[i] = elapsed / 1000.;
}
diff --git a/bindings/python/crocoddyl/__init__.py b/bindings/python/crocoddyl/__init__.py
index 908fcad3ba6a9768ee83d699714a9ac45321b345..0727003b9e848b960855fd24626a5abdcef3e243 100644
--- a/bindings/python/crocoddyl/__init__.py
+++ b/bindings/python/crocoddyl/__init__.py
@@ -38,7 +38,7 @@ def displayTrajectory(robot, xs, dt=0.1, rate=-1, cameraTF=None):
time.sleep(dt)
-class CallbackSolverDisplay(libcrocoddyl_pywrap.CallbackAbstract):
+class CallbackDisplay(libcrocoddyl_pywrap.CallbackAbstract):
def __init__(self, robotwrapper, rate=-1, freq=1, cameraTF=None):
libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
self.robotwrapper = robotwrapper
@@ -53,7 +53,7 @@ class CallbackSolverDisplay(libcrocoddyl_pywrap.CallbackAbstract):
displayTrajectory(self.robotwrapper, solver.xs, dt, self.rate, self.cameraTF)
-class CallbackSolverLogger(libcrocoddyl_pywrap.CallbackAbstract):
+class CallbackLogger(libcrocoddyl_pywrap.CallbackAbstract):
def __init__(self):
libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
self.steps = []
@@ -77,6 +77,70 @@ class CallbackSolverLogger(libcrocoddyl_pywrap.CallbackAbstract):
self.costs.append(solver.cost)
self.control_regs.append(solver.u_reg)
self.state_regs.append(solver.x_reg)
- self.th_stops.append(solver.stoppingCriteria)
+ self.th_stops.append(solver.stoppingCriteria())
self.gm_stops.append(-np.asscalar(solver.expectedImprovement()[1]))
- self.gaps.append(copy.copy(solver.gaps))
\ No newline at end of file
+ self.gaps.append(copy.copy(solver.gaps))
+
+
+def plotOCSolution(xs, us, figIndex=1, show=True):
+ import matplotlib.pyplot as plt
+ import numpy as np
+ # Getting the state and control trajectories
+ nx, nu = xs[0].shape[0], us[0].shape[0]
+ X = [0.] * nx
+ U = [0.] * nu
+ for i in range(nx):
+ X[i] = [np.asscalar(x[i]) for x in xs]
+ for i in range(nu):
+ U[i] = [np.asscalar(u[i]) for u in us]
+
+ plt.figure(figIndex)
+
+ # Plotting the state trajectories
+ plt.subplot(211)
+ [plt.plot(X[i], label='x' + str(i)) for i in range(nx)]
+ plt.legend()
+
+ # Plotting the control commands
+ plt.subplot(212)
+ [plt.plot(U[i], label='u' + str(i)) for i in range(nu)]
+ plt.legend()
+ plt.xlabel('knots')
+ if show:
+ plt.show()
+
+
+def plotConvergence(costs, muLM, muV, gamma, theta, alpha, figIndex=1, show=True, figTitle=""):
+ import matplotlib.pyplot as plt
+ import numpy as np
+
+ plt.figure(figIndex, figsize=(6.4, 8))
+ plt.suptitle(figTitle, fontsize=14)
+ # Plotting the total cost sequence
+ plt.subplot(511)
+ plt.ylabel('cost')
+ plt.plot(costs)
+
+ # Ploting mu sequences
+ plt.subplot(512)
+ plt.ylabel('mu')
+ plt.plot(muLM, label='LM')
+ plt.plot(muV, label='V')
+ plt.legend()
+
+ # Plotting the gradient sequence (gamma and theta)
+ plt.subplot(513)
+ plt.ylabel('gamma')
+ plt.plot(gamma)
+ plt.subplot(514)
+ plt.ylabel('theta')
+ plt.plot(theta)
+
+ # Plotting the alpha sequence
+ plt.subplot(515)
+ plt.ylabel('alpha')
+ ind = np.arange(len(alpha))
+ plt.bar(ind, alpha)
+ plt.xlabel('iteration')
+ if show:
+ plt.show()
diff --git a/bindings/python/crocoddyl/core/action-base.hpp b/bindings/python/crocoddyl/core/action-base.hpp
index 2784bf6dd261bf966539274121c8af51701cb275..ff08ae0a04283e8cf009b4233695fc5a8e266ba3 100644
--- a/bindings/python/crocoddyl/core/action-base.hpp
+++ b/bindings/python/crocoddyl/core/action-base.hpp
@@ -24,14 +24,14 @@ class ActionModelAbstract_wrap : public ActionModelAbstract, public bp::wrapper<
void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u, recalc);
}
};
@@ -51,6 +51,7 @@ void exposeActionAbstract() {
bp::init<StateAbstract&, int, bp::optional<int> >(
bp::args(" self", " state", " nu", " nr=1"),
"Initialize the action model.\n\n"
+ "You can also describe autonomous systems by setting nu = 0.\n"
":param state: state description,\n"
":param nu: dimension of control vector,\n"
":param nr: dimension of the cost-residual vector")[bp::with_custodian_and_ward<1, 2>()])
diff --git a/bindings/python/crocoddyl/core/actions/lqr.hpp b/bindings/python/crocoddyl/core/actions/lqr.hpp
index 54af9fbf7642e91b6abff7f1f5d2eb75de656301..1a54d63c49d6faff8d942f05ae2d32be352c4737 100644
--- a/bindings/python/crocoddyl/core/actions/lqr.hpp
+++ b/bindings/python/crocoddyl/core/actions/lqr.hpp
@@ -23,7 +23,7 @@ void exposeActionLQR() {
"A Linear-Quadratic Regulator problem has a transition model of the form\n"
"xnext(x,u) = Fx*x + Fu*u + f0. Its cost function is quadratic of the\n"
"form: 1/2 [x,u].T [Lxx Lxu; Lxu.T Luu] [x,u] + [lx,lu].T [x,u].",
- bp::init<int, int, bp::optional<bool> >(bp::args(" self", " nx", " ndu", " driftFree=True"),
+ bp::init<int, int, bp::optional<bool> >(bp::args(" self", " nx", " nu", " driftFree=True"),
"Initialize the LQR action model.\n\n"
":param nx: dimension of the state vector\n"
":param nu: dimension of the control vector\n"
diff --git a/bindings/python/crocoddyl/core/actuation-base.hpp b/bindings/python/crocoddyl/core/actuation-base.hpp
index 2d0468cc2ce567f82fc2b12e7c59337077a0dc49..51bc5077296331197c25b388990fcd5d4a2f00a3 100644
--- a/bindings/python/crocoddyl/core/actuation-base.hpp
+++ b/bindings/python/crocoddyl/core/actuation-base.hpp
@@ -90,12 +90,15 @@ void exposeActuationAbstract() {
"Create common data shared between actuation models.\n\n"
"The actuation data uses the model in order to first process it.\n"
":param model: actuation model"))
- .add_property("a", bp::make_getter(&ActuationDataAbstract::a, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&ActuationDataAbstract::a), "actuation signal")
- .add_property("Ax", bp::make_getter(&ActuationDataAbstract::Ax, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&ActuationDataAbstract::Ax), "Jacobian of the actuation model")
- .add_property("Au", bp::make_getter(&ActuationDataAbstract::Au, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&ActuationDataAbstract::Au), "Jacobian of the actuation model");
+ .add_property("tau",
+ bp::make_getter(&ActuationDataAbstract::tau, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActuationDataAbstract::tau), "actuation-force signal")
+ .add_property("dtau_dx",
+ bp::make_getter(&ActuationDataAbstract::dtau_dx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActuationDataAbstract::dtau_dx), "Jacobian of the actuation model")
+ .add_property("dtau_du",
+ bp::make_getter(&ActuationDataAbstract::dtau_du, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActuationDataAbstract::dtau_du), "Jacobian of the actuation model");
}
} // namespace python
diff --git a/bindings/python/crocoddyl/core/diff-action-base.hpp b/bindings/python/crocoddyl/core/diff-action-base.hpp
index 5007e7fba8dbb595b1b0fe6cd491cd3c933479fb..02d82c1455f59ddb3d323fbdcdac9347d436fb4f 100644
--- a/bindings/python/crocoddyl/core/diff-action-base.hpp
+++ b/bindings/python/crocoddyl/core/diff-action-base.hpp
@@ -25,7 +25,7 @@ class DifferentialActionModelAbstract_wrap : public DifferentialActionModelAbstr
void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
@@ -33,7 +33,7 @@ class DifferentialActionModelAbstract_wrap : public DifferentialActionModelAbstr
const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
const bool& recalc = true) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u, recalc);
}
};
@@ -52,6 +52,7 @@ void exposeDifferentialActionAbstract() {
bp::init<StateAbstract&, int, bp::optional<int> >(
bp::args(" self", " state", " nu", " nr=1"),
"Initialize the differential action model.\n\n"
+ "You can also describe autonomous systems by setting nu = 0.\n"
":param state: state\n"
":param nu: dimension of control vector\n"
":param nr: dimension of cost-residual vector)")[bp::with_custodian_and_ward<1, 2>()])
@@ -109,9 +110,6 @@ void exposeDifferentialActionAbstract() {
"Create common data shared between DAMs.\n\n"
"The differential action data uses the model in order to first process it.\n"
":param model: differential action model"))
- .def("shareCostMemory", &DifferentialActionDataAbstract::shareCostMemory, bp::args(" self", " cost"),
- "Share memory with a give cost\n\n"
- ":param cost: cost in which we want to share memory")
.add_property(
"cost",
bp::make_getter(&DifferentialActionDataAbstract::cost, bp::return_value_policy<bp::return_by_value>()),
@@ -121,9 +119,8 @@ void exposeDifferentialActionAbstract() {
bp::make_getter(&DifferentialActionDataAbstract::xout, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&DifferentialActionDataAbstract::xout), "evolution state")
.add_property(
- "r",
- bp::make_function(&DifferentialActionDataAbstract::get_r, bp::return_value_policy<bp::return_by_value>()),
- bp::make_function(&DifferentialActionDataAbstract::set_r), "cost residual")
+ "r", bp::make_getter(&DifferentialActionDataAbstract::r, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&DifferentialActionDataAbstract::r), "cost residual")
.add_property(
"Fx", bp::make_getter(&DifferentialActionDataAbstract::Fx, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&DifferentialActionDataAbstract::Fx), "Jacobian of the dynamics")
@@ -131,25 +128,20 @@ void exposeDifferentialActionAbstract() {
"Fu", bp::make_getter(&DifferentialActionDataAbstract::Fu, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&DifferentialActionDataAbstract::Fu), "Jacobian of the dynamics")
.add_property(
- "Lx",
- bp::make_function(&DifferentialActionDataAbstract::get_Lx, bp::return_value_policy<bp::return_by_value>()),
- bp::make_function(&DifferentialActionDataAbstract::set_Lx), "Jacobian of the cost")
+ "Lx", bp::make_getter(&DifferentialActionDataAbstract::Lx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&DifferentialActionDataAbstract::Lx), "Jacobian of the cost")
.add_property(
- "Lu",
- bp::make_function(&DifferentialActionDataAbstract::get_Lu, bp::return_value_policy<bp::return_by_value>()),
- bp::make_function(&DifferentialActionDataAbstract::set_Lu), "Jacobian of the cost")
+ "Lu", bp::make_getter(&DifferentialActionDataAbstract::Lu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&DifferentialActionDataAbstract::Lu), "Jacobian of the cost")
.add_property(
- "Lxx",
- bp::make_function(&DifferentialActionDataAbstract::get_Lxx, bp::return_value_policy<bp::return_by_value>()),
- bp::make_function(&DifferentialActionDataAbstract::set_Lxx), "Hessian of the cost")
+ "Lxx", bp::make_getter(&DifferentialActionDataAbstract::Lxx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&DifferentialActionDataAbstract::Lxx), "Hessian of the cost")
.add_property(
- "Lxu",
- bp::make_function(&DifferentialActionDataAbstract::get_Lxu, bp::return_value_policy<bp::return_by_value>()),
- bp::make_function(&DifferentialActionDataAbstract::set_Lxu), "Hessian of the cost")
+ "Lxu", bp::make_getter(&DifferentialActionDataAbstract::Lxu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&DifferentialActionDataAbstract::Lxu), "Hessian of the cost")
.add_property(
- "Luu",
- bp::make_function(&DifferentialActionDataAbstract::get_Luu, bp::return_value_policy<bp::return_by_value>()),
- bp::make_function(&DifferentialActionDataAbstract::set_Luu), "Hessian of the cost");
+ "Luu", bp::make_getter(&DifferentialActionDataAbstract::Luu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&DifferentialActionDataAbstract::Luu), "Hessian of the cost");
}
} // namespace python
diff --git a/bindings/python/crocoddyl/core/numdiff/action.hpp b/bindings/python/crocoddyl/core/numdiff/action.hpp
index d31b34050321437b54f2d8e649663329429dbd1b..9cbdc478ccc93aa48c450976ced38c26ec590f6e 100644
--- a/bindings/python/crocoddyl/core/numdiff/action.hpp
+++ b/bindings/python/crocoddyl/core/numdiff/action.hpp
@@ -19,11 +19,10 @@ namespace bp = boost::python;
void exposeActionNumDiff() {
bp::class_<ActionModelNumDiff, bp::bases<ActionModelAbstract> >(
"ActionModelNumDiff", "Abstract class for computing calcDiff by using numerical differentiation.\n\n",
- bp::init<ActionModelAbstract&, bp::optional<bool> >(
- bp::args(" self", " model", " gaussApprox=False"),
- "Initialize the action model NumDiff.\n\n"
- ":param model: action model where we compute the derivatives through NumDiff,\n"
- ":param gaussApprox: compute the Hessian using Gauss approximation")[bp::with_custodian_and_ward<1, 2>()])
+ bp::init<ActionModelAbstract&>(bp::args(" self", " model"),
+ "Initialize the action model NumDiff.\n\n"
+ ":param model: action model where we compute the derivatives through NumDiff")
+ [bp::with_custodian_and_ward<1, 2>()])
.def("calc", &ActionModelNumDiff::calc_wrap,
ActionModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
"Compute the next state and cost value.\n\n"
diff --git a/bindings/python/crocoddyl/core/solver-base.hpp b/bindings/python/crocoddyl/core/solver-base.hpp
index 0b4f3239a7df850641ea8d2b14f64f1ced9e9615..94bb47d2a059569cdd64d34d43370c09b25fb887 100644
--- a/bindings/python/crocoddyl/core/solver-base.hpp
+++ b/bindings/python/crocoddyl/core/solver-base.hpp
@@ -158,6 +158,10 @@ void exposeSolverAbstract() {
"Each iteration, the solver calls these set of functions in order to\n"
"allowed user the diagnostic of the solver's performance.\n"
":param callbacks: set of callback functions.")
+ .def("getCallbacks", &SolverAbstract_wrap::getCallbacks, bp::return_value_policy<bp::return_by_value>(),
+ bp::args(" self"),
+ "Return the list of callback functions using for diagnostic.\n\n"
+ ":return set of callback functions.")
.add_property("problem", bp::make_function(&SolverAbstract_wrap::get_problem, bp::return_internal_reference<>()),
"shooting problem")
.def("models", &SolverAbstract_wrap::get_models, bp::return_value_policy<bp::return_by_value>(), "models")
diff --git a/bindings/python/crocoddyl/core/solvers/ddp.hpp b/bindings/python/crocoddyl/core/solvers/ddp.hpp
index 70778bc59f153206b5075aeeaa6cd1c14d15aeeb..368cff8754bd9905cfb5ad419e4ed70fc8e6ad3c 100644
--- a/bindings/python/crocoddyl/core/solvers/ddp.hpp
+++ b/bindings/python/crocoddyl/core/solvers/ddp.hpp
@@ -98,7 +98,29 @@ void exposeSolverDDP() {
.add_property("K", make_function(&SolverDDP::get_K, bp::return_value_policy<bp::copy_const_reference>()), "K")
.add_property("k", make_function(&SolverDDP::get_k, bp::return_value_policy<bp::copy_const_reference>()), "k")
.add_property("gaps", make_function(&SolverDDP::get_gaps, bp::return_value_policy<bp::copy_const_reference>()),
- "gaps");
+ "gaps")
+ .add_property("regFactor",
+ bp::make_function(&SolverDDP::get_regfactor, bp::return_value_policy<bp::copy_const_reference>()),
+ bp::make_function(&SolverDDP::set_regfactor),
+ "regularization factor used for increasing or decreasing the value.")
+ .add_property("regMin",
+ bp::make_function(&SolverDDP::get_regmin, bp::return_value_policy<bp::copy_const_reference>()),
+ bp::make_function(&SolverDDP::set_regmin), "minimum regularization value.")
+ .add_property("regMax",
+ bp::make_function(&SolverDDP::get_regmax, bp::return_value_policy<bp::copy_const_reference>()),
+ bp::make_function(&SolverDDP::set_regmax), "maximum regularization value.")
+ .add_property("th_step",
+ bp::make_function(&SolverDDP::get_th_step, bp::return_value_policy<bp::copy_const_reference>()),
+ bp::make_function(&SolverDDP::set_th_step),
+ "threshold for decreasing the regularization after approving a step (higher values decreases the "
+ "regularization)")
+ .add_property("th_grad",
+ bp::make_function(&SolverDDP::get_th_grad, bp::return_value_policy<bp::copy_const_reference>()),
+ bp::make_function(&SolverDDP::set_th_grad),
+ "threshold for accepting step which gradients is lower than this value")
+ .add_property("alphas",
+ bp::make_function(&SolverDDP::get_alphas, bp::return_value_policy<bp::copy_const_reference>()),
+ bp::make_function(&SolverDDP::set_alphas), "list of step length (alpha) values");
}
} // namespace python
diff --git a/bindings/python/crocoddyl/core/solvers/fddp.hpp b/bindings/python/crocoddyl/core/solvers/fddp.hpp
index 0223d34e5833ce2a68a759543831af51b31fcd17..4fee4488529cab29b1b0f59602d49f7326f72eb6 100644
--- a/bindings/python/crocoddyl/core/solvers/fddp.hpp
+++ b/bindings/python/crocoddyl/core/solvers/fddp.hpp
@@ -17,13 +17,13 @@ namespace python {
namespace bp = boost::python;
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverFDDP_solves, SolverFDDP::solve, 0, 5)
-BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverFDDP_computeDirections, SolverFDDP::computeDirection, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverFDDP_computeDirections, SolverDDP::computeDirection, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverFDDP_trySteps, SolverFDDP::tryStep, 0, 1)
void exposeSolverFDDP() {
bp::class_<SolverFDDP, bp::bases<SolverDDP> >(
"SolverFDDP",
- "FDDP solver.\n\n"
+ "Feasibility-prone DDP (FDDP) solver.\n\n"
"The FDDP solver computes an optimal trajectory and control commands by iterates\n"
"running backward and forward passes. The backward-pass updates locally the\n"
"quadratic approximation of the problem and computes descent direction,\n"
@@ -48,30 +48,47 @@ void exposeSolverFDDP() {
":param regInit: initial guess for the regularization value. Very low values are typical\n"
" used with very good guess points (init_xs, init_us).\n"
":returns the optimal trajectory xopt, uopt and a boolean that describes if convergence was reached."))
-
+ .def("computeDirection", &SolverFDDP::computeDirection,
+ SolverFDDP_computeDirections(
+ bp::args(" self", " recalc=True"),
+ "Compute the search direction (dx, du) for the current guess (xs, us).\n\n"
+ "You must call setCandidate first in order to define the current\n"
+ "guess. A current guess defines a state and control trajectory\n"
+ "(xs, us) of T+1 and T elements, respectively.\n"
+ ":params recalc: true for recalculating the derivatives at current state and control.\n"
+ ":returns the search direction dx, du and the dual lambdas as lists of T+1, T and T+1 lengths."))
+ .def("tryStep", &SolverFDDP::tryStep,
+ SolverFDDP_trySteps(bp::args(" self", " stepLength=1"),
+ "Rollout the system with a predefined step length.\n\n"
+ ":param stepLength: step length\n"
+ ":returns the cost improvement."))
.def("expectedImprovement", &SolverFDDP::expectedImprovement,
bp::return_value_policy<bp::copy_const_reference>(), bp::args(" self"),
"Return two scalars denoting the quadratic improvement model\n\n"
"For computing the expected improvement, you need to compute first\n"
"the search direction by running computeDirection. The quadratic\n"
- "improvement model is described as dV = f_0 - f_+ = d1*a + d2*a**2/2.")
+ "improvement model is described as dV = f_0 - f_+ = d1*a + d2*a**2/2.\n"
+ "Additionally, you need to update the expected model by running\n"
+ "updateExpectedImprovement.")
+ .def("updateExpectedImprovement", &SolverFDDP::updateExpectedImprovement,
+ bp::return_value_policy<bp::copy_const_reference>(), bp::args(" self"),
+ "Update the expected improvement model\n\n")
.def("calc", &SolverFDDP::calc, bp::args(" self"),
"Update the Jacobian and Hessian of the optimal control problem\n\n"
"These derivatives are computed around the guess state and control\n"
"trajectory. These trajectory can be set by using setCandidate.\n"
":return the total cost around the guess trajectory.")
- .def("backwardPass", &SolverFDDP::backwardPass, bp::args(" self"),
- "Run the backward pass (Riccati sweep)\n\n"
- "It assumes that the Jacobian and Hessians of the optimal control problem have been\n"
- "compute. These terms are computed by running calc.")
.def("forwardPass", &SolverFDDP::forwardPass, bp::args(" self", " stepLength=1"),
"Run the forward pass or rollout\n\n"
"It rollouts the action model give the computed policy (feedfoward terns and feedback\n"
"gains) by the backwardPass. We can define different step lengths\n"
- ":param stepLength: applied step length (<= 1. and >= 0.)");
+ ":param stepLength: applied step length (<= 1. and >= 0.)")
+ .add_property("th_acceptNegStep", bp::make_function(&SolverFDDP::get_th_acceptnegstep),
+ bp::make_function(&SolverFDDP::set_th_acceptnegstep),
+ "threshold for step acceptance in ascent direction");
}
} // namespace python
} // namespace crocoddyl
-#endif // BINDINGS_PYTHON_CROCODDYL_CORE_SOLVERS_DDP_HPP_
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_SOLVERS_FDDP_HPP_
diff --git a/bindings/python/crocoddyl/crocoddyl.cpp b/bindings/python/crocoddyl/crocoddyl.cpp
index 2deaf62ff45b7d4c08a68657f4b4a015a8abbb2d..f7d0a54746129c2c3c817a21cc2aae85180ae95c 100644
--- a/bindings/python/crocoddyl/crocoddyl.cpp
+++ b/bindings/python/crocoddyl/crocoddyl.cpp
@@ -37,10 +37,13 @@ BOOST_PYTHON_MODULE(libcrocoddyl_pywrap) {
eigenpy::enableEigenPySpecific<VectorX>();
eigenpy::enableEigenPySpecific<MatrixX>();
- // Register Eigen converters between std::vector and Python list
+ // Register converters between std::vector and Python list
+ // TODO(cmastalli): figure out how to convert std::vector<double> to Python list
+ // bp::to_python_converter<std::vector<double, std::allocator<double> >, vector_to_list<double, false>, true>();
bp::to_python_converter<std::vector<VectorX, std::allocator<VectorX> >, vector_to_list<VectorX, false>, true>();
bp::to_python_converter<std::vector<MatrixX, std::allocator<MatrixX> >, vector_to_list<MatrixX, false>, true>();
list_to_vector()
+ .from_python<std::vector<double, std::allocator<double> > >()
.from_python<std::vector<VectorX, std::allocator<VectorX> > >()
.from_python<std::vector<MatrixX, std::allocator<MatrixX> > >();
diff --git a/bindings/python/crocoddyl/multibody.hpp b/bindings/python/crocoddyl/multibody.hpp
index 8f925d04dc0c4585dcfb40f3e6a40fde3e3728eb..e92a97cd5290d2ea418ebcced7b3b99bc4aca83f 100644
--- a/bindings/python/crocoddyl/multibody.hpp
+++ b/bindings/python/crocoddyl/multibody.hpp
@@ -31,6 +31,7 @@
#include "python/crocoddyl/multibody/impulses/impulse-6d.hpp"
#include "python/crocoddyl/multibody/actions/free-fwddyn.hpp"
#include "python/crocoddyl/multibody/actions/contact-fwddyn.hpp"
+#include "python/crocoddyl/multibody/actions/impulse-fwddyn.hpp"
namespace crocoddyl {
namespace python {
@@ -58,6 +59,7 @@ void exposeMultibody() {
exposeImpulse6D();
exposeDifferentialActionFreeFwdDynamics();
exposeDifferentialActionContactFwdDynamics();
+ exposeActionImpulseFwdDynamics();
}
} // namespace python
diff --git a/bindings/python/crocoddyl/multibody/actions/free-fwddyn.hpp b/bindings/python/crocoddyl/multibody/actions/free-fwddyn.hpp
index e8f59cea519617b5ca6c6923fb335c69f16d0633..d567f5faa709d53929a2e77a4ee848d09aa73eaf 100644
--- a/bindings/python/crocoddyl/multibody/actions/free-fwddyn.hpp
+++ b/bindings/python/crocoddyl/multibody/actions/free-fwddyn.hpp
@@ -81,7 +81,7 @@ void exposeDifferentialActionFreeFwdDynamics() {
bp::register_ptr_to_python<boost::shared_ptr<DifferentialActionDataFreeFwdDynamics> >();
bp::class_<DifferentialActionDataFreeFwdDynamics, bp::bases<DifferentialActionDataAbstract> >(
- "DifferentialActionDataFreeFwdDynamics", "Action data for the differential LQR system.",
+ "DifferentialActionDataFreeFwdDynamics", "Action data for the free forward dynamics system.",
bp::init<DifferentialActionModelFreeFwdDynamics*>(bp::args(" self", " model"),
"Create free forward-dynamics action data.\n\n"
":param model: free forward-dynamics action model"))
diff --git a/bindings/python/crocoddyl/multibody/actions/impulse-fwddyn.hpp b/bindings/python/crocoddyl/multibody/actions/impulse-fwddyn.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1ba85d9985f3b3a0eaa4901e0740718eb7524229
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/actions/impulse-fwddyn.hpp
@@ -0,0 +1,105 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
+
+#include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeActionImpulseFwdDynamics() {
+ bp::class_<ActionModelImpulseFwdDynamics, bp::bases<ActionModelAbstract> >(
+ "ActionModelImpulseFwdDynamics",
+ "Action model for impulse forward dynamics in multibody systems.\n\n"
+ "The impulse is modelled as holonomic constraits in the contact frame. There\n"
+ "is also a custom implementation in case of system with armatures. If you want to\n"
+ "include the armature, you need to use setArmature(). On the other hand, the\n"
+ "stack of cost functions are implemented in CostModelSum().",
+ bp::init<StateMultibody&, ImpulseModelMultiple&, CostModelSum&, bp::optional<double, double, bool> >(
+ bp::args(" self", " state", " impulses", " costs", " r_coeff=0.", " inv_damping=0.", "enable_force=False"),
+ "Initialize the impulse forward-dynamics action model.\n\n"
+ "The damping factor is needed when the contact Jacobian is not full-rank. Otherwise,\n"
+ "a good damping factor could be 1e-12. In addition, if you have cost based on forces,\n"
+ "you need to enable the computation of the force Jacobians (i.e. enable_force=True)."
+ ":param state: multibody state\n"
+ ":param impulses: multiple impulse model\n"
+ ":param costs: stack of cost functions\n"
+ ":param r_coeff: restitution coefficient\n"
+ ":param inv_damping: Damping factor for cholesky decomposition of JMinvJt\n"
+ ":param enable_force: Enable the computation of force Jacobians")[bp::with_custodian_and_ward<
+ 1, 2, bp::with_custodian_and_ward<1, 3, bp::with_custodian_and_ward<1, 4> > >()])
+ .def("calc", &ActionModelImpulseFwdDynamics::calc_wrap,
+ ActionModel_calc_wraps(
+ bp::args(" self", " data", " x", " u=None"),
+ "Compute the next state and cost value.\n\n"
+ "It describes the time-continuous evolution of the multibody system with impulse. The\n"
+ "impulses are modelled as holonomic constraints.\n"
+ "Additionally it computes the cost value associated to this state and control pair.\n"
+ ":param data: impulse forward-dynamics action data\n"
+ ":param x: time-continuous state vector\n"
+ ":param u: time-continuous control input"))
+ .def<void (ActionModelImpulseFwdDynamics::*)(const boost::shared_ptr<ActionDataAbstract>&,
+ const Eigen::VectorXd&, const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &ActionModelImpulseFwdDynamics::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the differential multibody system and its cost\n"
+ "functions.\n\n"
+ "It computes the partial derivatives of the differential multibody system and the\n"
+ "cost function. If recalc == True, it first updates the state evolution\n"
+ "and cost value. This function builds a quadratic approximation of the\n"
+ "action model (i.e. dynamical system and cost function).\n"
+ ":param data: impulse forward-dynamics action data\n"
+ ":param x: time-continuous state vector\n"
+ ":param u: time-continuous control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (ActionModelImpulseFwdDynamics::*)(const boost::shared_ptr<ActionDataAbstract>&,
+ const Eigen::VectorXd&, const Eigen::VectorXd&)>(
+ "calcDiff", &ActionModelImpulseFwdDynamics::calcDiff_wrap, bp::args(" self", " data", " x", " u"))
+ .def<void (ActionModelImpulseFwdDynamics::*)(const boost::shared_ptr<ActionDataAbstract>&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &ActionModelImpulseFwdDynamics::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (ActionModelImpulseFwdDynamics::*)(const boost::shared_ptr<ActionDataAbstract>&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &ActionModelImpulseFwdDynamics::calcDiff_wrap, bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &ActionModelImpulseFwdDynamics::createData, bp::args(" self"),
+ "Create the impulse forward dynamics differential action data.")
+ .add_property(
+ "pinocchio",
+ bp::make_function(&ActionModelImpulseFwdDynamics::get_pinocchio, bp::return_internal_reference<>()),
+ "multibody model (i.e. pinocchio model)")
+ .add_property("impulses",
+ bp::make_function(&ActionModelImpulseFwdDynamics::get_impulses, bp::return_internal_reference<>()),
+ "multiple contact model")
+ .add_property("costs",
+ bp::make_function(&ActionModelImpulseFwdDynamics::get_costs, bp::return_internal_reference<>()),
+ "total cost model")
+ .add_property("armature",
+ bp::make_function(&ActionModelImpulseFwdDynamics::get_armature,
+ bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&ActionModelImpulseFwdDynamics::set_armature),
+ "set an armature mechanism in the joints")
+ .add_property("r_coeff",
+ bp::make_function(&ActionModelImpulseFwdDynamics::get_restitution_coefficient,
+ bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&ActionModelImpulseFwdDynamics::set_restitution_coefficient),
+ "Restitution coefficient that describes elastic impacts")
+ .add_property("JMinvJt_damping",
+ bp::make_function(&ActionModelImpulseFwdDynamics::get_damping_factor,
+ bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&ActionModelImpulseFwdDynamics::set_damping_factor),
+ "Damping factor for cholesky decomposition of JMinvJt");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
diff --git a/bindings/python/crocoddyl/multibody/contact-base.hpp b/bindings/python/crocoddyl/multibody/contact-base.hpp
index d7a2bb831743af06d19a349629ef9d1d12c31b60..82c4d572a484a0927ce31a751ae2807e7d472197 100644
--- a/bindings/python/crocoddyl/multibody/contact-base.hpp
+++ b/bindings/python/crocoddyl/multibody/contact-base.hpp
@@ -32,9 +32,9 @@ class ContactModelAbstract_wrap : public ContactModelAbstract, public bp::wrappe
return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, recalc);
}
- void updateLagrangian(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& lambda) {
- assert(lambda.size() == nc_ && "lambda has wrong dimension");
- return bp::call<void>(this->get_override("updateLagrangian").ptr(), data, lambda);
+ void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& force) {
+ assert(force.size() == nc_ && "force has wrong dimension");
+ return bp::call<void>(this->get_override("updateForce").ptr(), data, force);
}
};
@@ -64,25 +64,24 @@ void exposeContactAbstract() {
"Compute the derivatives of contact holonomic constraint.\n\n"
"The rigid contact model throught acceleration-base holonomic constraint\n"
"of the contact frame placement.\n"
- ":param data: cost data\n"
+ ":param data: contact data\n"
":param x: state vector\n"
":param recalc: If true, it updates the contact Jacobian and drift.")
- .def("updateLagrangian", pure_virtual(&ContactModelAbstract_wrap::updateLagrangian),
- bp::args(" self", " data", " lambda"),
- "Convert the Lagrangian into a stack of spatial forces.\n\n"
- ":param data: cost data\n"
- ":param lambda: Lagrangian vector")
- .def("updateLagrangianDiff", &ContactModelAbstract_wrap::updateLagrangianDiff,
- bp::args(" self", " data", " Gx", " Gu"),
- "Update the Jacobian of the Lagrangian.\n\n"
- ":param data: cost data\n"
- ":param Gx: Jacobian of Lagrangian w.r.t. the state\n"
- ":param Gu: Jacobian of the Lagrangian w.r.t. the control")
+ .def("updateForce", pure_virtual(&ContactModelAbstract_wrap::updateForce), bp::args(" self", " data", " force"),
+ "Convert the force into a stack of spatial forces.\n\n"
+ ":param data: contact data\n"
+ ":param force: force vector (dimension nc)")
+ .def("updateForceDiff", &ContactModelAbstract_wrap::updateForceDiff,
+ bp::args(" self", " data", " df_dx", " df_du"),
+ "Update the Jacobians of the force.\n\n"
+ ":param data: contact data\n"
+ ":param df_dx: Jacobian of the force with respect to the state\n"
+ ":param df_du: Jacobian of the force with respect to the control")
.def("createData", &ContactModelAbstract_wrap::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args(" self", " data"),
"Create the contact data.\n\n"
"Each contact model has its own data that needs to be allocated. This function\n"
- "returns the allocated data for a predefined cost.\n"
+ "returns the allocated data for a predefined contact.\n"
":param data: Pinocchio data\n"
":return contact data.")
.add_property("state",
@@ -100,20 +99,23 @@ void exposeContactAbstract() {
bp::init<ContactModelAbstract*, pinocchio::Data*>(
bp::args(" self", " model", " data"),
"Create common data shared between contact models.\n\n"
- ":param model: cost model\n"
+ ":param model: contact model\n"
":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 3>()])
.add_property("pinocchio", bp::make_getter(&ContactDataAbstract::pinocchio, bp::return_internal_reference<>()),
"pinocchio data")
.add_property("Jc", bp::make_getter(&ContactDataAbstract::Jc, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&ContactDataAbstract::Jc), "contact Jacobian")
.add_property("a0", bp::make_getter(&ContactDataAbstract::a0, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&ContactDataAbstract::a0), "contact drift")
- .add_property("Ax", bp::make_getter(&ContactDataAbstract::Ax, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&ContactDataAbstract::Ax), "Jacobian of the contact constraint")
- .add_property("Gx", bp::make_getter(&ContactDataAbstract::Ax, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&ContactDataAbstract::Ax), "Jacobian of the contact forces")
- .add_property("Gu", bp::make_getter(&ContactDataAbstract::Ax, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&ContactDataAbstract::Ax), "Jacobian of the contact forces")
+ bp::make_setter(&ContactDataAbstract::a0), "desired contact acceleration")
+ .add_property("da0_dx",
+ bp::make_getter(&ContactDataAbstract::da0_dx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ContactDataAbstract::da0_dx), "Jacobian of the desired contact acceleration")
+ .add_property("df_dx",
+ bp::make_getter(&ContactDataAbstract::df_dx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ContactDataAbstract::df_dx), "Jacobian of the contact forces")
+ .add_property("df_du",
+ bp::make_getter(&ContactDataAbstract::df_du, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ContactDataAbstract::df_du), "Jacobian of the contact forces")
.def_readwrite("joint", &ContactDataAbstract::joint, "joint index of the contact frame")
.def_readwrite("f", &ContactDataAbstract::f, "external spatial forces");
}
diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-3d.hpp b/bindings/python/crocoddyl/multibody/contacts/contact-3d.hpp
index 18d8188122e6d745bd1731c61402e38f6a2d0491..c55991ac5d9124b842ebb6724e6500c5455a8210 100644
--- a/bindings/python/crocoddyl/multibody/contacts/contact-3d.hpp
+++ b/bindings/python/crocoddyl/multibody/contacts/contact-3d.hpp
@@ -50,10 +50,10 @@ void exposeContact3D() {
":param data: cost data\n"
":param x: state vector\n"
":param recalc: If true, it updates the contact Jacobian and drift."))
- .def("updateLagrangian", &ContactModel3D::updateLagrangian, bp::args(" self", " data", " lambda"),
- "Convert the Lagrangian into a stack of spatial forces.\n\n"
+ .def("updateForce", &ContactModel3D::updateForce, bp::args(" self", " data", " force"),
+ "Convert the force into a stack of spatial forces.\n\n"
":param data: cost data\n"
- ":param lambda: Lagrangian vector")
+ ":param force: force vector (dimension 3)")
.def("createData", &ContactModel3D::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args(" self", " data"),
"Create the 3D contact data.\n\n"
diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-6d.hpp b/bindings/python/crocoddyl/multibody/contacts/contact-6d.hpp
index ca184785dbe95de1b971b5bb378cffeedf287194..cc320b3315ac02d396db12973e44270f9f0a1f76 100644
--- a/bindings/python/crocoddyl/multibody/contacts/contact-6d.hpp
+++ b/bindings/python/crocoddyl/multibody/contacts/contact-6d.hpp
@@ -50,10 +50,10 @@ void exposeContact6D() {
":param data: cost data\n"
":param x: state vector\n"
":param recalc: If true, it updates the contact Jacobian and drift."))
- .def("updateLagrangian", &ContactModel6D::updateLagrangian, bp::args(" self", " data", " lambda"),
+ .def("updateForce", &ContactModel6D::updateForce, bp::args(" self", " data", " force"),
"Convert the Lagrangian into a stack of spatial forces.\n\n"
":param data: cost data\n"
- ":param lambda: Lagrangian vector")
+ ":param force: force vector (dimension 6)")
.def("createData", &ContactModel6D::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args(" self", " data"),
"Create the 6D contact data.\n\n"
diff --git a/bindings/python/crocoddyl/multibody/contacts/multiple-contacts.hpp b/bindings/python/crocoddyl/multibody/contacts/multiple-contacts.hpp
index 5a84631652ed7a65884cf4a8cf3c24ce647c3073..21a588e92190f8c1c920a1f9fe13c6777c25601a 100644
--- a/bindings/python/crocoddyl/multibody/contacts/multiple-contacts.hpp
+++ b/bindings/python/crocoddyl/multibody/contacts/multiple-contacts.hpp
@@ -72,19 +72,22 @@ void exposeContactMultiple() {
"Compute the derivatives of the total contact holonomic constraint.\n\n"
"The rigid contact model throught acceleration-base holonomic constraint\n"
"of the contact frame placement.\n"
- ":param data: cost data\n"
+ ":param data: contact data\n"
":param x: state vector\n"
":param recalc: If true, it updates the contact Jacobian and drift."))
- .def("updateLagrangian", &ContactModelMultiple::updateLagrangian, bp::args(" self", " data", " lambda"),
- "Convert the Lagrangian into a stack of spatial forces.\n\n"
- ":param data: cost data\n"
- ":param lambda: Lagrangian vector")
- .def("updateLagrangianDiff", &ContactModelMultiple::updateLagrangianDiff,
- bp::args(" self", " data", " Gx", " Gu"),
- "Update the Jacobian of the Lagrangian.\n\n"
- ":param data: cost data\n"
- ":param Gx: Jacobian of Lagrangian w.r.t. the state\n"
- ":param Gu: Jacobian of the Lagrangian w.r.t. the control")
+ .def("updateAcceleration", &ContactModelMultiple::updateAcceleration, bp::args(" self", " data", " dv"),
+ "Update the constrained acceleration.\n\n"
+ ":param data: contact data\n"
+ ":param dv: constrained acceleration (dimension nv)")
+ .def("updateForce", &ContactModelMultiple::updateForce, bp::args(" self", " data", " force"),
+ "Convert the force into a stack of spatial forces.\n\n"
+ ":param data: contact data\n"
+ ":param force: force vector (dimension nc)")
+ .def("updateForceDiff", &ContactModelMultiple::updateForceDiff, bp::args(" self", " data", " df_dx", " df_du"),
+ "Update the Jacobians of the force.\n\n"
+ ":param data: contact data\n"
+ ":param df_dx: Jacobian of the force with respect to the state (dimension nc*ndx)\n"
+ ":param df_du: Jacobian of the force with respect to the control (dimension nc*nu)")
.def("createData", &ContactModelMultiple::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args(" self", " data"),
"Create the total contact data.\n\n"
@@ -110,6 +113,12 @@ void exposeContactMultiple() {
"Create multicontact data.\n\n"
":param model: multicontact model\n"
":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
+ .add_property("dv", bp::make_getter(&ContactDataMultiple::dv, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ContactDataMultiple::dv), "constrained acceleration in generalized coordinates")
+ .add_property("ddv_dx",
+ bp::make_getter(&ContactDataMultiple::ddv_dx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ContactDataMultiple::ddv_dx),
+ "Jacobian of the constrained acceleration in generalized coordinates")
.add_property("contacts",
bp::make_getter(&ContactDataMultiple::contacts, bp::return_value_policy<bp::return_by_value>()),
"stack of contacts data")
diff --git a/bindings/python/crocoddyl/multibody/cost-base.hpp b/bindings/python/crocoddyl/multibody/cost-base.hpp
index 4aa449ac34cda2b574057577de95e57dd62d23d6..73d5299e899b303f35e7b541ddd657889b900a60 100644
--- a/bindings/python/crocoddyl/multibody/cost-base.hpp
+++ b/bindings/python/crocoddyl/multibody/cost-base.hpp
@@ -34,14 +34,14 @@ class CostModelAbstract_wrap : public CostModelAbstract, public bp::wrapper<Cost
void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u, recalc);
}
};
diff --git a/bindings/python/crocoddyl/multibody/costs/cost-sum.hpp b/bindings/python/crocoddyl/multibody/costs/cost-sum.hpp
index 06ff9368fa197824d716d2088c7694d1bdc58a39..e2dba8abc3e7d0944fb65fa107c49bb58191c1fa 100644
--- a/bindings/python/crocoddyl/multibody/costs/cost-sum.hpp
+++ b/bindings/python/crocoddyl/multibody/costs/cost-sum.hpp
@@ -117,24 +117,30 @@ void exposeCostSum() {
"Create total cost data.\n\n"
":param model: total cost model\n"
":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 3>()])
+ .def("shareMemory", &CostDataSum::shareMemory<DifferentialActionDataAbstract>, bp::args(" self", " model"),
+ "Share memory with a given differential action data\n\n"
+ ":param model: differential action data that we want to share memory")
+ .def("shareMemory", &CostDataSum::shareMemory<ActionDataAbstract>, bp::args(" self", " model"),
+ "Share memory with a given action data\n\n"
+ ":param model: action data that we want to share memory")
.add_property("costs", bp::make_getter(&CostDataSum::costs, bp::return_value_policy<bp::return_by_value>()),
"stack of costs data")
.add_property("pinocchio", bp::make_getter(&CostDataSum::pinocchio, bp::return_internal_reference<>()),
"pinocchio data")
.add_property("cost", bp::make_getter(&CostDataSum::cost, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&CostDataSum::cost), "cost value")
- .add_property("Lx", bp::make_getter(&CostDataSum::Lx, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&CostDataSum::Lx), "Jacobian of the cost")
- .add_property("Lu", bp::make_getter(&CostDataSum::Lu, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&CostDataSum::Lu), "Jacobian of the cost")
- .add_property("Lxx", bp::make_getter(&CostDataSum::Lxx, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&CostDataSum::Lxx), "Hessian of the cost")
- .add_property("Lxu", bp::make_getter(&CostDataSum::Lxu, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&CostDataSum::Lxu), "Hessian of the cost")
- .add_property("Luu", bp::make_getter(&CostDataSum::Luu, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&CostDataSum::Luu), "Hessian of the cost")
- .add_property("r", bp::make_getter(&CostDataSum::r, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&CostDataSum::r), "cost residual")
+ .add_property("r", bp::make_function(&CostDataSum::get_r, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&CostDataSum::set_r), "cost residual")
+ .add_property("Lx", bp::make_function(&CostDataSum::get_Lx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&CostDataSum::set_Lx), "Jacobian of the cost")
+ .add_property("Lu", bp::make_function(&CostDataSum::get_Lu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&CostDataSum::set_Lu), "Jacobian of the cost")
+ .add_property("Lxx", bp::make_function(&CostDataSum::get_Lxx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&CostDataSum::set_Lxx), "Hessian of the cost")
+ .add_property("Lxu", bp::make_function(&CostDataSum::get_Lxu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&CostDataSum::set_Lxu), "Hessian of the cost")
+ .add_property("Luu", bp::make_function(&CostDataSum::get_Luu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&CostDataSum::set_Luu), "Hessian of the cost")
.add_property("Rx", bp::make_getter(&CostDataSum::Rx, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&CostDataSum::Rx), "Jacobian of the cost residual")
.add_property("Ru", bp::make_getter(&CostDataSum::Ru, bp::return_value_policy<bp::return_by_value>()),
diff --git a/bindings/python/crocoddyl/multibody/impulse-base.hpp b/bindings/python/crocoddyl/multibody/impulse-base.hpp
index be0becea38c53840d9ff54b06e0de9eee23a66ef..2b316843974045ed9e7a32c97d8603930368ef43 100644
--- a/bindings/python/crocoddyl/multibody/impulse-base.hpp
+++ b/bindings/python/crocoddyl/multibody/impulse-base.hpp
@@ -31,9 +31,9 @@ class ImpulseModelAbstract_wrap : public ImpulseModelAbstract, public bp::wrappe
return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, recalc);
}
- void updateLagrangian(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& lambda) {
- assert(lambda.size() == ni_ && "lambda has wrong dimension");
- return bp::call<void>(this->get_override("updateLagrangian").ptr(), data, lambda);
+ void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& force) {
+ assert(force.size() == ni_ && "force has wrong dimension");
+ return bp::call<void>(this->get_override("updateForce").ptr(), data, force);
}
};
@@ -57,19 +57,23 @@ void exposeImpulseAbstract() {
.def("calcDiff", pure_virtual(&ImpulseModelAbstract_wrap::calcDiff),
bp::args(" self", " data", " x", " recalc=True"),
"Compute the derivatives of impulse Jacobian\n"
- ":param data: cost data\n"
+ ":param data: impulse data\n"
":param x: state vector\n"
":param recalc: If true, it updates the impulse Jacobian")
- .def("updateLagrangian", pure_virtual(&ImpulseModelAbstract_wrap::updateLagrangian),
- bp::args(" self", " data", " lambda"),
- "Convert the Lagrangian into a stack of spatial forces.\n\n"
- ":param data: cost data\n"
- ":param lambda: Lagrangian vector")
+ .def("updateForce", pure_virtual(&ImpulseModelAbstract_wrap::updateForce), bp::args(" self", " data", " force"),
+ "Convert the force into a stack of spatial forces.\n\n"
+ ":param data: impulse data\n"
+ ":param force: force vector (dimension ni)")
+ .def("updateForceDiff", &ImpulseModelAbstract_wrap::updateForceDiff, bp::args(" self", " data", " df_dq"),
+ "Update the Jacobian of the impulse force.\n\n"
+ "The Jacobian df_dv is zero, then we ignore it\n"
+ ":param data: impulse data\n"
+ ":param df_dq: Jacobian of the impulse force (dimension ni*nv)")
.def("createData", &ImpulseModelAbstract_wrap::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args(" self", " data"),
"Create the impulse data.\n\n"
"Each impulse model has its own data that needs to be allocated. This function\n"
- "returns the allocated data for a predefined cost.\n"
+ "returns the allocated data for a predefined impulse.\n"
":param data: Pinocchio data\n"
":return impulse data.")
.add_property("state",
@@ -84,14 +88,15 @@ void exposeImpulseAbstract() {
bp::init<ImpulseModelAbstract*, pinocchio::Data*>(
bp::args(" self", " model", " data"),
"Create common data shared between impulse models.\n\n"
- ":param model: cost model\n"
+ ":param model: impulse model\n"
":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 3>()])
.add_property("pinocchio", bp::make_getter(&ImpulseDataAbstract::pinocchio, bp::return_internal_reference<>()),
"pinocchio data")
.add_property("Jc", bp::make_getter(&ImpulseDataAbstract::Jc, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&ImpulseDataAbstract::Jc), "impulse Jacobian")
- .add_property("Vq", bp::make_getter(&ImpulseDataAbstract::Vq, bp::return_value_policy<bp::return_by_value>()),
- bp::make_setter(&ImpulseDataAbstract::Vq), "Jacobian of the impulse constraint")
+ .add_property("dv0_dq",
+ bp::make_getter(&ImpulseDataAbstract::dv0_dq, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ImpulseDataAbstract::dv0_dq), "Jacobian of the previous impulse velocity")
.def_readwrite("joint", &ImpulseDataAbstract::joint, "joint index of the impulse frame")
.def_readwrite("f", &ImpulseDataAbstract::f, "external spatial forces");
}
diff --git a/bindings/python/crocoddyl/multibody/impulses/impulse-3d.hpp b/bindings/python/crocoddyl/multibody/impulses/impulse-3d.hpp
index c7edc032b72ace7797169b3cb78249d07aeb8ef4..c3176a1a9b9d64d68508dfaf437e90ea06f789a1 100644
--- a/bindings/python/crocoddyl/multibody/impulses/impulse-3d.hpp
+++ b/bindings/python/crocoddyl/multibody/impulses/impulse-3d.hpp
@@ -41,10 +41,10 @@ void exposeImpulse3D() {
":param data: cost data\n"
":param x: state vector\n"
":param recalc: If true, it updates the impulse Jacobian and drift."))
- .def("updateLagrangian", &ImpulseModel3D::updateLagrangian, bp::args(" self", " data", " lambda"),
- "Convert the Lagrangian into a stack of spatial forces.\n\n"
+ .def("updateForce", &ImpulseModel3D::updateForce, bp::args(" self", " data", " force"),
+ "Convert the force into a stack of spatial forces.\n\n"
":param data: cost data\n"
- ":param lambda: Lagrangian vector")
+ ":param force: force vector (dimension 3)")
.def("createData", &ImpulseModel3D::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args(" self", " data"),
"Create the 3D impulse data.\n\n"
diff --git a/bindings/python/crocoddyl/multibody/impulses/impulse-6d.hpp b/bindings/python/crocoddyl/multibody/impulses/impulse-6d.hpp
index 4b428ce9b30dd4163bacf1a491746ebbe7943a69..5a54b12811814c01565f40b373566479a3a8363e 100644
--- a/bindings/python/crocoddyl/multibody/impulses/impulse-6d.hpp
+++ b/bindings/python/crocoddyl/multibody/impulses/impulse-6d.hpp
@@ -41,10 +41,10 @@ void exposeImpulse6D() {
":param data: cost data\n"
":param x: state vector\n"
":param recalc: If true, it updates the impulse Jacobian and drift."))
- .def("updateLagrangian", &ImpulseModel6D::updateLagrangian, bp::args(" self", " data", " lambda"),
- "Convert the Lagrangian into a stack of spatial forces.\n\n"
+ .def("updateForce", &ImpulseModel6D::updateForce, bp::args(" self", " data", " force"),
+ "Convert the force into a stack of spatial forces.\n\n"
":param data: cost data\n"
- ":param lambda: Lagrangian vector")
+ ":param lambda: force vector (dimension 6)")
.def("createData", &ImpulseModel6D::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args(" self", " data"),
"Create the 6D impulse data.\n\n"
diff --git a/bindings/python/crocoddyl/multibody/impulses/multiple-impulses.hpp b/bindings/python/crocoddyl/multibody/impulses/multiple-impulses.hpp
index 26c43b07e07f8d0b1f9e2437f979da6fbe3ac803..56a5e1c1374144476c415eefef5cd7f2e96c01b7 100644
--- a/bindings/python/crocoddyl/multibody/impulses/multiple-impulses.hpp
+++ b/bindings/python/crocoddyl/multibody/impulses/multiple-impulses.hpp
@@ -71,13 +71,26 @@ void exposeImpulseMultiple() {
"Compute the derivatives of the total impulse holonomic constraint.\n\n"
"The rigid impulse model throught acceleration-base holonomic constraint\n"
"of the impulse frame placement.\n"
- ":param data: cost data\n"
+ ":param data: impulse data\n"
":param x: state vector\n"
":param recalc: If true, it updates the impulse Jacobian and drift."))
- .def("updateLagrangian", &ImpulseModelMultiple::updateLagrangian, bp::args(" self", " data", " lambda"),
- "Convert the Lagrangian into a stack of spatial forces.\n\n"
- ":param data: cost data\n"
- ":param lambda: Lagrangian vector")
+ .def("updateVelocity", &ImpulseModelMultiple::updateVelocity, bp::args(" self", " data", " vnext"),
+ "Update the velocity after impulse.\n\n"
+ ":param data: impulse data\n"
+ ":param vnext: velocity after impulse (dimension nv)")
+ .def("updateForce", &ImpulseModelMultiple::updateForce, bp::args(" self", " data", " lambda"),
+ "Convert the force into a stack of spatial forces.\n\n"
+ ":param data: impulse data\n"
+ ":param force: force vector (dimension ni)")
+ .def("updateVelocityDiff", &ImpulseModelMultiple::updateVelocityDiff, bp::args(" self", " data", " dvnext_dx"),
+ "Update the velocity after impulse.\n\n"
+ ":param data: impulse data\n"
+ ":param dvnext_dx: Jacobian of the impulse velocity (dimension nv*ndx)")
+ .def("updateForceDiff", &ImpulseModelMultiple::updateForceDiff, bp::args(" self", " data", " df_dq"),
+ "Update the Jacobian of the impulse force.\n\n"
+ "The Jacobian df_dv is zero, then we ignore it\n"
+ ":param data: impulse data\n"
+ ":param df_dq: Jacobian of the impulse force (dimension ni*nv)")
.def("createData", &ImpulseModelMultiple::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args(" self", " data"),
"Create the total impulse data.\n\n"
@@ -100,6 +113,12 @@ void exposeImpulseMultiple() {
"Create multiimpulse data.\n\n"
":param model: multiimpulse model\n"
":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
+ .add_property("vnext",
+ bp::make_getter(&ImpulseDataMultiple::vnext, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ImpulseDataMultiple::vnext), "impulse velocity")
+ .add_property("dvnext_dx",
+ bp::make_getter(&ImpulseDataMultiple::dvnext_dx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ImpulseDataMultiple::dvnext_dx), "Jacobian of the impulse velocity")
.add_property("impulses",
bp::make_getter(&ImpulseDataMultiple::impulses, bp::return_value_policy<bp::return_by_value>()),
"stack of impulses data")
diff --git a/bindings/python/crocoddyl/utils.hpp b/bindings/python/crocoddyl/utils.hpp
index 0039a64aae093e49b700bad4a13c98a832fab0d8..96198af666086d115e96135981c46f7c70e849fc 100644
--- a/bindings/python/crocoddyl/utils.hpp
+++ b/bindings/python/crocoddyl/utils.hpp
@@ -25,12 +25,13 @@ namespace bp = boost::python;
template <class T, bool NoProxy = true>
struct vector_to_list {
static PyObject* convert(const std::vector<T>& vec) {
+ typedef typename std::vector<T>::const_iterator const_iter;
bp::list* l = new boost::python::list();
- for (size_t i = 0; i < vec.size(); i++) {
+ for (const_iter it = vec.begin(); it != vec.end(); ++it) {
if (NoProxy) {
- l->append(boost::ref(vec[i]));
+ l->append(boost::ref(*it));
} else {
- l->append(vec[i]);
+ l->append(*it);
}
}
return l->ptr();
diff --git a/bindings/python/crocoddyl/utils/__init__.py b/bindings/python/crocoddyl/utils/__init__.py
index 5e732efafb4bd4efa1f4c9606ccbb811b00a6e1a..c25c914d4a03d6abe1bd59790bb402991ee5c240 100644
--- a/bindings/python/crocoddyl/utils/__init__.py
+++ b/bindings/python/crocoddyl/utils/__init__.py
@@ -127,13 +127,13 @@ class FreeFloatingActuationDerived(crocoddyl.ActuationModelAbstract):
crocoddyl.ActuationModelAbstract.__init__(self, state, state.nv - 6)
def calc(self, data, x, u):
- data.a = np.vstack([pinocchio.utils.zero(6), u])
+ data.tau = np.vstack([pinocchio.utils.zero(6), u])
def calcDiff(self, data, x, u, recalc=True):
if recalc:
self.calc(data, x, u)
- Au = np.vstack([pinocchio.utils.zero((6, self.nu)), pinocchio.utils.eye(self.nu)])
- data.Au = Au
+ dtau_du = np.vstack([pinocchio.utils.zero((6, self.nu)), pinocchio.utils.eye(self.nu)])
+ data.dtau_du = dtau_du
class FullActuationDerived(crocoddyl.ActuationModelAbstract):
@@ -142,12 +142,12 @@ class FullActuationDerived(crocoddyl.ActuationModelAbstract):
crocoddyl.ActuationModelAbstract.__init__(self, state, state.nv)
def calc(self, data, x, u):
- data.a = u
+ data.tau = u
def calcDiff(self, data, x, u, recalc=True):
if recalc:
self.calc(data, x, u)
- data.Au = pinocchio.utils.eye(self.nu)
+ data.dtau_du = pinocchio.utils.eye(self.nu)
class UnicycleDerived(crocoddyl.ActionModelAbstract):
@@ -261,7 +261,7 @@ class DifferentialFreeFwdDynamicsDerived(crocoddyl.DifferentialActionModelAbstra
def __init__(self, state, costModel):
crocoddyl.DifferentialActionModelAbstract.__init__(self, state, state.nv, costModel.nr)
self.costs = costModel
- self.forceAba = True
+ self.enable_force = True
self.armature = np.matrix(np.zeros(0))
def calc(self, data, x, u=None):
@@ -269,7 +269,7 @@ class DifferentialFreeFwdDynamicsDerived(crocoddyl.DifferentialActionModelAbstra
u = self.unone
q, v = x[:self.state.nq], x[-self.state.nv:]
# Computing the dynamics using ABA or manually for armature case
- if self.forceAba:
+ if self.enable_force:
data.xout = pinocchio.aba(self.state.pinocchio, data.pinocchio, q, v, u)
else:
pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)
@@ -292,7 +292,7 @@ class DifferentialFreeFwdDynamicsDerived(crocoddyl.DifferentialActionModelAbstra
self.calc(data, x, u)
pinocchio.computeJointJacobians(self.state.pinocchio, data.pinocchio, q)
# Computing the dynamics derivatives
- if self.forceAba:
+ if self.enable_force:
pinocchio.computeABADerivatives(self.state.pinocchio, data.pinocchio, q, v, u)
data.Fx = np.hstack([data.pinocchio.ddq_dq, data.pinocchio.ddq_dv])
data.Fu = data.pinocchio.Minv
@@ -307,14 +307,14 @@ class DifferentialFreeFwdDynamicsDerived(crocoddyl.DifferentialActionModelAbstra
if armature.size is not self.state.nv:
print('The armature dimension is wrong, we cannot set it.')
else:
- self.forceAba = False
+ self.enable_force = False
self.armature = armature.T
def createData(self):
data = crocoddyl.DifferentialActionModelAbstract.createData(self)
data.pinocchio = pinocchio.Data(self.state.pinocchio)
data.costs = self.costs.createData(data.pinocchio)
- data.shareCostMemory(data.costs)
+ data.costs.shareMemory(data)
return data
@@ -547,17 +547,17 @@ class Contact3DDerived(crocoddyl.ContactModelAbstract):
vv_skew = pinocchio.utils.skew(self.vv)
vw_skew = pinocchio.utils.skew(self.vw)
fXjdv_dq = self.fXj * v_partial_dq
- Aq = (self.fXj * a_partial_dq)[:3, :] + vw_skew * fXjdv_dq[:3, :] - vv_skew * fXjdv_dq[3:, :]
- Av = (self.fXj * a_partial_dv)[:3, :] + vw_skew * data.Jc - vv_skew * self.Jw
+ da0_dq = (self.fXj * a_partial_dq)[:3, :] + vw_skew * fXjdv_dq[:3, :] - vv_skew * fXjdv_dq[3:, :]
+ da0_dv = (self.fXj * a_partial_dv)[:3, :] + vw_skew * data.Jc - vv_skew * self.Jw
if np.asscalar(self.gains[0]) != 0.:
R = data.pinocchio.oMf[self.xref.frame].rotation
- Aq += np.asscalar(self.gains[0]) * R * pinocchio.getFrameJacobian(
+ da0_dq += np.asscalar(self.gains[0]) * R * pinocchio.getFrameJacobian(
self.state.pinocchio, data.pinocchio, self.xref.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :]
if np.asscalar(self.gains[1]) != 0.:
- Aq += np.asscalar(self.gains[1]) * (self.fXj[:3, :] * v_partial_dq)
- Av += np.asscalar(self.gains[1]) * (self.fXj[:3, :] * a_partial_da)
- data.Ax = np.hstack([Aq, Av])
+ da0_dq += np.asscalar(self.gains[1]) * (self.fXj[:3, :] * v_partial_dq)
+ da0_dv += np.asscalar(self.gains[1]) * (self.fXj[:3, :] * a_partial_da)
+ data.da0_dx = np.hstack([da0_dq, da0_dv])
class Contact6DDerived(crocoddyl.ContactModelAbstract):
@@ -586,15 +586,15 @@ class Contact6DDerived(crocoddyl.ContactModelAbstract):
v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da = pinocchio.getJointAccelerationDerivatives(
self.state.pinocchio, data.pinocchio, self.joint, pinocchio.ReferenceFrame.LOCAL)
- Aq = (self.fXj * a_partial_dq)
- Av = (self.fXj * a_partial_dv)
+ da0_dq = (self.fXj * a_partial_dq)
+ da0_dv = (self.fXj * a_partial_dv)
if np.asscalar(self.gains[0]) != 0.:
- Aq += np.asscalar(self.gains[0]) * pinocchio.Jlog6(self.rMf) * data.Jc
+ da0_dq += np.asscalar(self.gains[0]) * pinocchio.Jlog6(self.rMf) * data.Jc
if np.asscalar(self.gains[1]) != 0.:
- Aq += np.asscalar(self.gains[1]) * (self.fXj * v_partial_dq)
- Av += np.asscalar(self.gains[1]) * (self.fXj * a_partial_da)
- data.Ax = np.hstack([Aq, Av])
+ da0_dq += np.asscalar(self.gains[1]) * (self.fXj * v_partial_dq)
+ da0_dv += np.asscalar(self.gains[1]) * (self.fXj * a_partial_da)
+ data.da0_dx = np.hstack([da0_dq, da0_dv])
class Impulse3DDerived(crocoddyl.ImpulseModelAbstract):
@@ -613,7 +613,7 @@ class Impulse3DDerived(crocoddyl.ImpulseModelAbstract):
self.calc(data, x)
v_partial_dq, v_partial_dv = pinocchio.getJointVelocityDerivatives(self.state.pinocchio, data.pinocchio,
self.joint, pinocchio.ReferenceFrame.LOCAL)
- data.Vq = self.fXj[:3, :] * v_partial_dq
+ data.dv0_dq = self.fXj[:3, :] * v_partial_dq
class Impulse6DDerived(crocoddyl.ImpulseModelAbstract):
@@ -632,7 +632,7 @@ class Impulse6DDerived(crocoddyl.ImpulseModelAbstract):
self.calc(data, x)
v_partial_dq, v_partial_dv = pinocchio.getJointVelocityDerivatives(self.state.pinocchio, data.pinocchio,
self.joint, pinocchio.ReferenceFrame.LOCAL)
- data.Vq = self.fXj * v_partial_dq
+ data.dv0_dq = self.fXj * v_partial_dq
class DDPDerived(crocoddyl.SolverAbstract):
@@ -640,7 +640,7 @@ class DDPDerived(crocoddyl.SolverAbstract):
crocoddyl.SolverAbstract.__init__(self, shootingProblem)
self.allocateData() # TODO remove it?
- self.isFeasible = False # Change it to true if you know that datas[t].xnext = xs[t+1]
+ self.isFeasible = False
self.alphas = [2**(-n) for n in range(10)]
self.th_grad = 1e-12
@@ -654,8 +654,6 @@ class DDPDerived(crocoddyl.SolverAbstract):
def calc(self):
self.cost = self.problem.calcDiff(self.xs, self.us)
if not self.isFeasible:
- # Gap store the state defect from the guess to feasible (rollout) trajectory, i.e.
- # gap = x_rollout [-] x_guess = DIFF(x_guess, x_rollout)
self.gaps[0] = self.problem.runningModels[0].state.diff(self.xs[0], self.problem.x0)
for i, (m, d, x) in enumerate(zip(self.problem.runningModels, self.problem.runningDatas, self.xs[1:])):
self.gaps[i + 1] = m.state.diff(x, d.xnext)
@@ -728,8 +726,6 @@ class DDPDerived(crocoddyl.SolverAbstract):
if self.wasFeasible and self.stop < self.th_stop:
return self.xs, self.us, True
-
- # Warning: no convergence in max iterations
return self.xs, self.us, False
def increaseRegularization(self):
@@ -744,7 +740,6 @@ class DDPDerived(crocoddyl.SolverAbstract):
self.x_reg = self.regMin
self.u_reg = self.x_reg
- # DDP Specific
def allocateData(self):
self.Vxx = [a2m(np.zeros([m.state.ndx, m.state.ndx])) for m in self.models()]
self.Vx = [a2m(np.zeros([m.state.ndx])) for m in self.models()]
@@ -774,17 +769,16 @@ class DDPDerived(crocoddyl.SolverAbstract):
ndx = self.problem.terminalModel.state.ndx
self.Vxx[-1][range(ndx), range(ndx)] += self.x_reg
+ # Compute and store the Vx gradient at end of the interval (rollout state)
+ if not self.isFeasible:
+ self.Vx[-1] += np.dot(self.Vxx[-1], self.gaps[-1])
+
for t, (model, data) in rev_enumerate(zip(self.problem.runningModels, self.problem.runningDatas)):
self.Qxx[t][:, :] = data.Lxx + data.Fx.T * self.Vxx[t + 1] * data.Fx
self.Qxu[t][:, :] = data.Lxu + data.Fx.T * self.Vxx[t + 1] * data.Fu
self.Quu[t][:, :] = data.Luu + data.Fu.T * self.Vxx[t + 1] * data.Fu
self.Qx[t][:] = data.Lx + data.Fx.T * self.Vx[t + 1]
self.Qu[t][:] = data.Lu + data.Fu.T * self.Vx[t + 1]
- if not self.isFeasible:
- # In case the xt+1 are not f(xt,ut) i.e warm start not obtained from roll-out.
- relinearization = self.Vxx[t + 1] * self.gaps[t + 1]
- self.Qx[t][:] += data.Fx.T * relinearization
- self.Qu[t][:] += data.Fu.T * relinearization
if self.u_reg != 0:
self.Quu[t][range(model.nu), range(model.nu)] += self.u_reg
@@ -800,6 +794,11 @@ class DDPDerived(crocoddyl.SolverAbstract):
if self.x_reg != 0:
self.Vxx[t][range(model.state.ndx), range(model.state.ndx)] += self.x_reg
+
+ # Compute and store the Vx gradient at end of the interval (rollout state)
+ if not self.isFeasible:
+ self.Vx[t] += np.dot(self.Vxx[t], self.gaps[t])
+
raiseIfNan(self.Vxx[t], ArithmeticError('backward error'))
raiseIfNan(self.Vx[t], ArithmeticError('backward error'))
@@ -815,8 +814,6 @@ class DDPDerived(crocoddyl.SolverAbstract):
raise ArithmeticError('backward error')
def forwardPass(self, stepLength, warning='ignore'):
- # Argument warning is also introduce for debug: by default, it masks the numpy warnings
- # that can be reactivated during debug.
xs, us = self.xs, self.us
xtry, utry = self.xs_try, self.us_try
ctry = 0
@@ -837,3 +834,145 @@ class DDPDerived(crocoddyl.SolverAbstract):
raiseIfNan(ctry, ArithmeticError('forward error'))
self.cost_try = ctry
return xtry, utry, ctry
+
+
+class FDDPDerived(DDPDerived):
+ def __init__(self, shootingProblem):
+ DDPDerived.__init__(self, shootingProblem)
+
+ self.th_acceptNegStep = 2.
+ self.dg = 0.
+ self.dq = 0.
+ self.dv = 0.
+
+ def solve(self, init_xs=[], init_us=[], maxiter=100, isFeasible=False, regInit=None):
+ self.setCandidate(init_xs, init_us, isFeasible)
+ self.x_reg = regInit if regInit is not None else self.regMin
+ self.u_reg = regInit if regInit is not None else self.regMin
+ self.wasFeasible = False
+ for i in range(maxiter):
+ recalc = True
+ while True:
+ try:
+ self.computeDirection(recalc=recalc)
+ except ArithmeticError:
+ recalc = False
+ self.increaseRegularization()
+ if self.x_reg == self.regMax:
+ return self.xs, self.us, False
+ else:
+ continue
+ break
+ self.updateExpectedImprovement()
+
+ for a in self.alphas:
+ try:
+ self.dV = self.tryStep(a)
+ except ArithmeticError:
+ continue
+ d1, d2 = self.expectedImprovement()
+
+ self.dV_exp = a * (d1 + .5 * d2 * a)
+ if self.dV_exp > 0.: # descend direction
+ if d1 < self.th_grad or self.dV > self.th_acceptStep * self.dV_exp:
+ self.wasFeasible = self.isFeasible
+ self.setCandidate(self.xs_try, self.us_try, (self.wasFeasible or a == 1))
+ self.cost = self.cost_try
+ break
+ else: # reducing the gaps by allowing a small increment in the cost value
+ if d1 < self.th_grad or self.dV < self.th_acceptNegStep * self.dV_exp:
+ self.wasFeasible = self.isFeasible
+ self.setCandidate(self.xs_try, self.us_try, (self.wasFeasible or a == 1))
+ self.cost = self.cost_try
+ break
+ if a > self.th_step:
+ self.decreaseRegularization()
+ if a == self.alphas[-1]:
+ self.increaseRegularization()
+ if self.x_reg == self.regMax:
+ return self.xs, self.us, False
+ self.stepLength = a
+ self.iter = i
+ self.stop = self.stoppingCriteria()
+ # TODO @Carlos bind the callbacks
+ # if self.callback is not None:
+ # [c(self) for c in self.callback]
+
+ if self.wasFeasible and self.stop < self.th_stop:
+ return self.xs, self.us, True
+ return self.xs, self.us, False
+
+ def computeDirection(self, recalc=True):
+ if recalc:
+ self.calc()
+ self.backwardPass()
+ return [np.nan] * (self.problem.T + 1), self.k, self.Vx
+
+ def tryStep(self, stepLength=1):
+ self.forwardPass(stepLength)
+ return self.cost - self.cost_try
+
+ def updateExpectedImprovement(self):
+ self.dg = 0.
+ self.dq = 0.
+ if not self.isFeasible:
+ self.dg -= np.asscalar(self.Vx[-1].T * self.gaps[-1])
+ self.dq += np.asscalar(self.gaps[-1].T * self.Vxx[-1] * self.gaps[-1])
+ for t in range(self.problem.T):
+ self.dg += np.asscalar(self.Qu[t].T * self.k[t])
+ self.dq -= np.asscalar(self.k[t].T * self.Quu[t] * self.k[t])
+ if not self.isFeasible:
+ self.dg -= np.asscalar(self.Vx[t].T * self.gaps[t])
+ self.dq += np.asscalar(self.gaps[t].T * self.Vxx[t] * self.gaps[t])
+
+ def expectedImprovement(self):
+ self.dv = 0.
+ if not self.isFeasible:
+ dx = self.problem.runningModels[-1].state.diff(self.xs_try[-1], self.xs[-1])
+ self.dv -= np.asscalar(self.gaps[-1].T * self.Vxx[-1] * dx)
+ for t in range(self.problem.T):
+ dx = self.problem.runningModels[t].state.diff(self.xs_try[t], self.xs[t])
+ self.dv -= np.asscalar(self.gaps[t].T * self.Vxx[t] * dx)
+ d1 = self.dg + self.dv
+ d2 = self.dq - 2 * self.dv
+ return np.matrix([d1, d2]).T
+
+ def calc(self):
+ self.cost = self.problem.calcDiff(self.xs, self.us)
+ if not self.isFeasible:
+ self.gaps[0] = self.problem.runningModels[0].state.diff(self.xs[0], self.problem.x0)
+ for i, (m, d, x) in enumerate(zip(self.problem.runningModels, self.problem.runningDatas, self.xs[1:])):
+ self.gaps[i + 1] = m.state.diff(x, d.xnext)
+ elif not self.wasFeasible:
+ self.gaps[:] = [np.zeros_like(f) for f in self.gaps]
+ return self.cost
+
+ def forwardPass(self, stepLength, warning='ignore'):
+ xs, us = self.xs, self.us
+ xtry, utry = self.xs_try, self.us_try
+ ctry = 0
+ xnext = self.problem.x0
+ for t, (m, d) in enumerate(zip(self.problem.runningModels, self.problem.runningDatas)):
+ if self.isFeasible or stepLength == 1:
+ xtry[t] = xnext.copy()
+ else:
+ xtry[t] = m.state.integrate(xnext, self.gaps[t] * (stepLength - 1))
+ utry[t] = us[t] - self.k[t] * stepLength - self.K[t] * m.state.diff(xs[t], xtry[t])
+ with np.warnings.catch_warnings():
+ np.warnings.simplefilter(warning)
+ m.calc(d, xtry[t], utry[t])
+ xnext, cost = d.xnext, d.cost
+ ctry += cost
+ raiseIfNan([ctry, cost], ArithmeticError('forward error'))
+ raiseIfNan(xnext, ArithmeticError('forward error'))
+ if self.isFeasible or stepLength == 1:
+ xtry[-1] = xnext.copy()
+ else:
+ xtry[-1] = self.problem.terminalModel.state.integrate(xnext, self.gaps[-1] * (stepLength - 1))
+ with np.warnings.catch_warnings():
+ np.warnings.simplefilter(warning)
+ self.problem.terminalModel.calc(self.problem.terminalData, xtry[-1])
+ ctry += self.problem.terminalData.cost
+ raiseIfNan(ctry, ArithmeticError('forward error'))
+ self.cost_try = ctry
+ return xtry, utry, ctry
diff --git a/bindings/python/crocoddyl/utils/biped.py b/bindings/python/crocoddyl/utils/biped.py
index ffb92cfb87cebffa9d056ed6e827456609e73781..6274c10d95960cad49ba8371b4c50e2a2993e03e 100644
--- a/bindings/python/crocoddyl/utils/biped.py
+++ b/bindings/python/crocoddyl/utils/biped.py
@@ -39,7 +39,7 @@ class SimpleBipedGaitProblem:
rfPos0 = self.rdata.oMf[self.rfId].translation
lfPos0 = self.rdata.oMf[self.lfId].translation
comRef = (rfPos0 + lfPos0) / 2
- comRef[2] = 0.6185
+ comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
# Defining the action models along the time instances
loco3dModel = []
@@ -63,6 +63,56 @@ class SimpleBipedGaitProblem:
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
+ def createJumpingProblem(self, x0, jumpHeight, jumpLength, timeStep, groundKnots, flyingKnots, final=False):
+ q0 = x0[:self.rmodel.nq]
+ pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
+ pinocchio.updateFramePlacements(self.rmodel, self.rdata)
+ rfFootPos0 = self.rdata.oMf[self.rfId].translation
+ lfFootPos0 = self.rdata.oMf[self.lfId].translation
+ df = jumpLength[2] - rfFootPos0[2]
+ rfFootPos0[2] = 0.
+ lfFootPos0[2] = 0.
+ comRef = (rfFootPos0 + lfFootPos0) / 2
+ comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
+
+ self.rWeight = 1e1
+ loco3dModel = []
+ takeOff = [self.createSwingFootModel(
+ timeStep,
+ [self.lfId, self.rfId],
+ ) for k in range(groundKnots)]
+ flyingUpPhase = [
+ self.createSwingFootModel(
+ timeStep, [],
+ np.matrix([jumpLength[0], jumpLength[1], jumpLength[2] + jumpHeight]).T * (k + 1) / flyingKnots +
+ comRef) for k in range(flyingKnots)
+ ]
+ flyingDownPhase = []
+ for k in range(flyingKnots):
+ flyingDownPhase += [self.createSwingFootModel(timeStep, [])]
+
+ f0 = np.matrix(jumpLength).T
+ footTask = [
+ crocoddyl.FramePlacement(self.lfId, pinocchio.SE3(np.eye(3), self.lfId + f0)),
+ crocoddyl.FramePlacement(self.rfId, pinocchio.SE3(np.eye(3), self.rfId + f0))
+ ]
+ landingPhase = [self.createFootSwitchModel([self.lfId, self.rfId], footTask, False)]
+ f0[2] = df
+ if final is True:
+ self.rWeight = 1e4
+ landed = [
+ self.createSwingFootModel(timeStep, [self.lfId, self.rfId], comTask=comRef + f0)
+ for k in range(groundKnots)
+ ]
+ loco3dModel += takeOff
+ loco3dModel += flyingUpPhase
+ loco3dModel += flyingDownPhase
+ loco3dModel += landingPhase
+ loco3dModel += landed
+
+ problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
+ return problem
+
def createFootstepModels(self, comPos0, feetPos0, stepLength, stepHeight, timeStep, numKnots, supportFootIds,
swingFootIds):
""" Action models for a footstep phase.
@@ -108,8 +158,7 @@ class SimpleBipedGaitProblem:
]
# Action model for the foot switch
- footSwitchModel = \
- self.createFootSwitchModel(supportFootIds, swingFootTask)
+ footSwitchModel = self.createFootSwitchModel(supportFootIds, swingFootTask)
# Updating the current foot position for next step
comPos0 += np.matrix([stepLength * comPercentage, 0., 0.]).T
@@ -160,13 +209,28 @@ class SimpleBipedGaitProblem:
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
return model
- def createFootSwitchModel(self, supportFootIds, swingFootTask):
+ def createFootSwitchModel(self, supportFootIds, swingFootTask, pseudoImpulse=True):
""" Action model for a foot switch phase.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
+ :param pseudoImpulse: true for pseudo impulse models, otherwise it uses the impulse model
:return action model for a foot switch phase
"""
+ if pseudoImpulse:
+ return self.createPseudoImpulseModel(supportFootIds, swingFootTask)
+ else:
+ return self.createImpulseModel(supportFootIds, swingFootTask)
+
+ def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
+ """ Action model for pseudo-impulse models.
+
+ A pseudo-impulse model consists of adding high-penalty cost for the contact velocities.
+ :param supportFootIds: Ids of the constrained feet
+ :param swingFootTask: swinging foot task
+ :return pseudo-impulse differential action model
+ """
+
# Creating a 6D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
@@ -182,8 +246,8 @@ class SimpleBipedGaitProblem:
footTrack = crocoddyl.CostModelFramePlacement(self.state, i, self.actuation.nu)
costModel.addCost("footTrack_" + str(i), footTrack, 1e8)
footVel = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
- impactFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, footVel, self.actuation.nu)
- costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6)
+ impulseFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, footVel, self.actuation.nu)
+ costModel.addCost('impulseVel_' + str(i.frame), impulseFootVelCost, 1e6)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv)
stateReg = crocoddyl.CostModelState(self.state,
@@ -200,6 +264,38 @@ class SimpleBipedGaitProblem:
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
+ def createImpulseModel(self, supportFootIds, swingFootTask):
+ """ Action model for impulse models.
+
+ An impulse model consists of describing the impulse dynamics against a set of contacts.
+ :param supportFootIds: Ids of the constrained feet
+ :param swingFootTask: swinging foot task
+ :return impulse action model
+ """
+ # Creating a 6D multi-contact model, and then including the supporting foot
+ impulseModel = crocoddyl.ImpulseModelMultiple(self.state)
+ for i in supportFootIds:
+ supportContactModel = crocoddyl.ImpulseModel6D(self.state, i)
+ impulseModel.addImpulse("impulse_" + str(i), supportContactModel)
+
+ # Creating the cost model for a contact phase
+ costModel = crocoddyl.CostModelSum(self.state, 0)
+ if swingFootTask is not None:
+ for i in swingFootTask:
+ xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
+ footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0)
+ costModel.addCost("footTrack_" + str(i), footTrack, 1e8)
+ stateWeights = np.array([1.] * 6 + [0.1] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv)
+ stateReg = crocoddyl.CostModelState(self.state,
+ crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
+ self.rmodel.defaultState, 0)
+ costModel.addCost("stateReg", stateReg, 1e1)
+
+ # Creating the action model for the KKT dynamics with simpletic Euler
+ # integration scheme
+ model = crocoddyl.ActionModelImpulseFwdDynamics(self.state, impulseModel, costModel)
+ return model
+
def plotSolution(rmodel, xs, us, figIndex=1, show=True):
import matplotlib.pyplot as plt
@@ -210,7 +306,7 @@ def plotSolution(rmodel, xs, us, figIndex=1, show=True):
for i in range(nx):
X[i] = [np.asscalar(x[i]) for x in xs]
for i in range(nu):
- U[i] = [np.asscalar(u[i]) for u in us]
+ U[i] = [np.asscalar(u[i]) if u.shape[0] != 0 else 0 for u in us]
# Plotting the joint positions, velocities and torques
plt.figure(figIndex)
diff --git a/bindings/python/crocoddyl/utils/quadruped.py b/bindings/python/crocoddyl/utils/quadruped.py
index eb9dd4841d015baeaf9e648285f27c8e634f82e4..f2a64ae9c25d5b616957718a5cb4295ffce3eabb 100644
--- a/bindings/python/crocoddyl/utils/quadruped.py
+++ b/bindings/python/crocoddyl/utils/quadruped.py
@@ -33,10 +33,6 @@ class SimpleQuadrupedalGaitProblem:
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
com0 = pinocchio.centerOfMass(self.rmodel, self.rdata, q0)
- # lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
- # rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
- # lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
- # rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
# Defining the action models along the time instances
comModels = []
@@ -92,7 +88,7 @@ class SimpleQuadrupedalGaitProblem:
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
+ comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
# Defining the action models along the time instances
loco3dModel = []
@@ -143,7 +139,7 @@ class SimpleQuadrupedalGaitProblem:
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
+ comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
# Defining the action models along the time instances
loco3dModel = []
@@ -191,7 +187,7 @@ class SimpleQuadrupedalGaitProblem:
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
+ comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
# Defining the action models along the time instances
loco3dModel = []
@@ -240,7 +236,7 @@ class SimpleQuadrupedalGaitProblem:
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
+ comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
# Defining the action models along the time instances
loco3dModel = []
@@ -261,32 +257,59 @@ class SimpleQuadrupedalGaitProblem:
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
- def createJumpingProblem(self, x0, jumpHeight, timeStep):
+ def createJumpingProblem(self, x0, jumpHeight, jumpLength, timeStep, groundKnots, flyingKnots):
+ q0 = x0[:self.rmodel.nq]
+ pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
+ pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
+ df = jumpLength[2] - rfFootPos0[2]
+ rfFootPos0[2] = 0.
+ rhFootPos0[2] = 0.
+ lfFootPos0[2] = 0.
+ lhFootPos0[2] = 0.
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
-
- takeOffKnots = 30
- flyingKnots = 30
+ comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
loco3dModel = []
takeOff = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- ) for k in range(takeOffKnots)
+ ) for k in range(groundKnots)
]
- flyingPhase = [
- self.createSwingFootModel(timeStep, [],
- np.matrix([0., 0., jumpHeight * (k + 1) / flyingKnots]).T + comRef)
- for k in range(flyingKnots)
+ flyingUpPhase = [
+ self.createSwingFootModel(
+ timeStep, [],
+ np.matrix([jumpLength[0], jumpLength[1], jumpLength[2] + jumpHeight]).T * (k + 1) / flyingKnots +
+ comRef) for k in range(flyingKnots)
+ ]
+ flyingDownPhase = []
+ for k in range(flyingKnots):
+ flyingDownPhase += [self.createSwingFootModel(timeStep, [])]
+
+ f0 = np.matrix(jumpLength).T
+ footTask = [
+ crocoddyl.FramePlacement(self.lfFootId, pinocchio.SE3(np.eye(3), lfFootPos0 + f0)),
+ crocoddyl.FramePlacement(self.rfFootId, pinocchio.SE3(np.eye(3), rfFootPos0 + f0)),
+ crocoddyl.FramePlacement(self.lhFootId, pinocchio.SE3(np.eye(3), lhFootPos0 + f0)),
+ crocoddyl.FramePlacement(self.rhFootId, pinocchio.SE3(np.eye(3), rhFootPos0 + f0))
+ ]
+ landingPhase = [
+ self.createFootSwitchModel([self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], footTask, False)
+ ]
+ f0[2] = df
+ landed = [
+ self.createSwingFootModel(timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ comTask=comRef + f0) for k in range(groundKnots)
]
-
loco3dModel += takeOff
- loco3dModel += flyingPhase
+ loco3dModel += flyingUpPhase
+ loco3dModel += flyingDownPhase
+ loco3dModel += landingPhase
+ loco3dModel += landed
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
@@ -327,11 +350,11 @@ class SimpleQuadrupedalGaitProblem:
swingFootTask += [crocoddyl.FramePlacement(i, pinocchio.SE3(np.eye(3), tref))]
- # Adding an action model for this knot
comTask = np.matrix([stepLength * (k + 1) / numKnots, 0., 0.]).T * comPercentage + comPos0
footSwingModel += [
self.createSwingFootModel(timeStep, supportFootIds, comTask=comTask, swingFootTask=swingFootTask)
]
+
# Action model for the foot switch
footSwitchModel = self.createFootSwitchModel(supportFootIds, swingFootTask)
@@ -355,7 +378,7 @@ class SimpleQuadrupedalGaitProblem:
contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
- supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 0.]).T)
+ supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).T)
contactModel.addContact('contact_' + str(i), supportContactModel)
# Creating the cost model for a contact phase
@@ -369,7 +392,7 @@ class SimpleQuadrupedalGaitProblem:
footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu)
costModel.addCost("footTrack_" + str(i), footTrack, 1e4)
- stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv)
+ stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
stateReg = crocoddyl.CostModelState(self.state,
crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
self.rmodel.defaultState, self.actuation.nu)
@@ -384,19 +407,33 @@ class SimpleQuadrupedalGaitProblem:
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
return model
- def createFootSwitchModel(self, supportFootIds, swingFootTask):
+ def createFootSwitchModel(self, supportFootIds, swingFootTask, pseudoImpulse=True):
""" Action model for a foot switch phase.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
+ :param pseudoImpulse: true for pseudo-impulse models, otherwise it uses the impulse model
:return action model for a foot switch phase
"""
+ if pseudoImpulse:
+ return self.createPseudoImpulseModel(supportFootIds, swingFootTask)
+ else:
+ return self.createImpulseModel(supportFootIds, swingFootTask)
+
+ def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
+ """ Action model for pseudo-impulse models.
+
+ A pseudo-impulse model consists of adding high-penalty cost for the contact velocities.
+ :param supportFootIds: Ids of the constrained feet
+ :param swingFootTask: swinging foot task
+ :return pseudo-impulse differential action model
+ """
# Creating a 3D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
- supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 0.]).T)
+ supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).T)
contactModel.addContact('contact_' + str(i), supportContactModel)
# Creating the cost model for a contact phase
@@ -404,9 +441,12 @@ class SimpleQuadrupedalGaitProblem:
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
+ vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu)
+ impulseFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, vref, self.actuation.nu)
costModel.addCost("footTrack_" + str(i), footTrack, 1e7)
- stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv)
+ costModel.addCost('impulseVel_' + str(i.frame), impulseFootVelCost, 1e6)
+ stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
stateReg = crocoddyl.CostModelState(self.state,
crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
self.rmodel.defaultState, self.actuation.nu)
@@ -414,11 +454,6 @@ class SimpleQuadrupedalGaitProblem:
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-3)
- for i in swingFootTask:
- vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
- impactFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, vref, self.actuation.nu)
- costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6)
-
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel,
@@ -426,8 +461,40 @@ class SimpleQuadrupedalGaitProblem:
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
+ def createImpulseModel(self, supportFootIds, swingFootTask):
+ """ Action model for impulse models.
+
+ An impulse model consists of describing the impulse dynamics against a set of contacts.
+ :param supportFootIds: Ids of the constrained feet
+ :param swingFootTask: swinging foot task
+ :return impulse action model
+ """
+ # Creating a 3D multi-contact model, and then including the supporting foot
+ impulseModel = crocoddyl.ImpulseModelMultiple(self.state)
+ for i in supportFootIds:
+ supportContactModel = crocoddyl.ImpulseModel3D(self.state, i)
+ impulseModel.addImpulse("impulse_" + str(i), supportContactModel)
+
+ # Creating the cost model for a contact phase
+ costModel = crocoddyl.CostModelSum(self.state, 0)
+ if swingFootTask is not None:
+ for i in swingFootTask:
+ xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
+ footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0)
+ costModel.addCost("footTrack_" + str(i), footTrack, 1e7)
+ stateWeights = np.array([1.] * 6 + [10.] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
+ stateReg = crocoddyl.CostModelState(self.state,
+ crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
+ self.rmodel.defaultState, 0)
+ costModel.addCost("stateReg", stateReg, 1e1)
+
+ # Creating the action model for the KKT dynamics with simpletic Euler
+ # integration scheme
+ model = crocoddyl.ActionModelImpulseFwdDynamics(self.state, impulseModel, costModel)
+ return model
+
-def plotSolution(rmodel, xs, us):
+def plotSolution(rmodel, xs, us, figIndex=1, show=True):
import matplotlib.pyplot as plt
# Getting the state and control trajectories
nx, nq, nu = xs[0].shape[0], rmodel.nq, us[0].shape[0]
@@ -436,10 +503,10 @@ def plotSolution(rmodel, xs, us):
for i in range(nx):
X[i] = [np.asscalar(x[i]) for x in xs]
for i in range(nu):
- U[i] = [np.asscalar(u[i]) for u in us]
+ U[i] = [np.asscalar(u[i]) if u.shape[0] != 0 else 0 for u in us]
# Plotting the joint positions, velocities and torques
- plt.figure(1)
+ plt.figure(figIndex)
legJointNames = ['HAA', 'HFE', 'KFE']
# LF foot
plt.subplot(4, 3, 1)
@@ -502,9 +569,8 @@ def plotSolution(rmodel, xs, us):
plt.ylabel('RH')
plt.legend()
plt.xlabel('knots')
- plt.show()
- plt.figure(2)
+ plt.figure(figIndex + 1)
rdata = rmodel.createData()
Cx = []
Cy = []
@@ -518,4 +584,5 @@ def plotSolution(rmodel, xs, us):
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.grid(True)
- plt.show()
+ if show:
+ plt.show()
diff --git a/cmake b/cmake
index 0fb087eb8e78db616329de0e4ab55abbe015bbe8..bbdade93ad2a4b7818cb5d5b9f0bf131627e0b71 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 0fb087eb8e78db616329de0e4ab55abbe015bbe8
+Subproject commit bbdade93ad2a4b7818cb5d5b9f0bf131627e0b71
diff --git a/crocoddyl/contact.py b/crocoddyl/contact.py
index 7a66f812d0c7a11aa03b7a36e15870a479a48a05..9ef3813c76242fdb7071b8ae5e124d413798e80f 100644
--- a/crocoddyl/contact.py
+++ b/crocoddyl/contact.py
@@ -57,7 +57,7 @@ class ContactDataPinocchio:
self.Aq = self.Ax[:, :nv]
self.Av = self.Ax[:, nv:]
self.f = np.nan # not set at construction type
- self.forces = pinocchio.StdVect_Force()
+ self.forces = pinocchio.StdVec_Force()
for i in range(model.pinocchio.njoints):
self.forces.append(pinocchio.Force.Zero())
diff --git a/crocoddyl/impact.py b/crocoddyl/impact.py
index c20a085bb5e86ba07c66b55d921a49f981c8b1ca..50950c32e80e56efdd5ab12c56934021ab9426fe 100644
--- a/crocoddyl/impact.py
+++ b/crocoddyl/impact.py
@@ -49,7 +49,7 @@ class ImpulseDataPinocchio:
self.J = np.zeros([nc, nv])
self.Jq = np.zeros([nc, nv])
self.f = np.nan # not set at construction type
- self.forces = pinocchio.StdVect_Force()
+ self.forces = pinocchio.StdVec_Force()
for i in range(model.pinocchio.njoints):
self.forces.append(pinocchio.Force.Zero())
self.Vq = np.zeros([nc, nv])
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..88fa4e3e071a1d9cb846bca09d342eacee763de3
--- /dev/null
+++ b/examples/CMakeLists.txt
@@ -0,0 +1,12 @@
+SET(${PROJECT_NAME}_EXAMPLES_PYTHON
+ arm_manipulation
+ bipedal_walk
+ quadrupedal_gaits
+ )
+
+FOREACH(examples ${${PROJECT_NAME}_EXAMPLES_PYTHON})
+ PYTHON_BUILD(. "${examples}.py")
+ ADD_CUSTOM_TARGET("examples-${examples}"
+ ${CMAKE_COMMAND} -E env PYTHONPATH=$ENV{PYTHONPATH}:${PROJECT_BINARY_DIR}/bindings/python
+ ${PYTHON_EXECUTABLE} -c "import ${examples}")
+ENDFOREACH(examples ${${PROJECT_NAME}_EXAMPLES_PYTHON})
diff --git a/examples/talos_arm.py b/examples/arm_manipulation.py
similarity index 77%
rename from examples/talos_arm.py
rename to examples/arm_manipulation.py
index fd23d5f91b0f890ed7677d1a60179ee31b0b2943..c804e41cd4efed2b02956e3755f0c9cc9782b3f1 100644
--- a/examples/talos_arm.py
+++ b/examples/arm_manipulation.py
@@ -1,3 +1,4 @@
+import os
import sys
import crocoddyl
@@ -5,8 +6,8 @@ import pinocchio
import numpy as np
import example_robot_data
-WITHDISPLAY = 'disp' in sys.argv
-WITHPLOT = 'plot' in sys.argv
+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)
@@ -60,8 +61,19 @@ problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
ddp = crocoddyl.SolverDDP(problem)
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
-if WITHDISPLAY:
- ddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(talos_arm, 4, 4, cameraTF)])
+if WITHDISPLAY and WITHPLOT:
+ ddp.setCallbacks([
+ crocoddyl.CallbackLogger(),
+ crocoddyl.CallbackVerbose(),
+ crocoddyl.CallbackDisplay(talos_arm, 4, 4, cameraTF)
+ ])
+elif WITHDISPLAY:
+ ddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackVerbose()])
+elif WITHPLOT:
+ ddp.setCallbacks([
+ crocoddyl.CallbackLogger(),
+ crocoddyl.CallbackVerbose(),
+ ])
else:
ddp.setCallbacks([crocoddyl.CallbackVerbose()])
@@ -69,11 +81,16 @@ else:
ddp.solve()
# Plotting the solution and the DDP convergence
-# if WITHPLOT:
-# log = ddp.callback[1]
-# plotOCSolution(log.xs, log.us, figIndex=1, show=False)
-# plotDDPConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps,
-# figIndex=2)
+if WITHPLOT:
+ log = ddp.getCallbacks()[0]
+ crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
+ crocoddyl.plotConvergence(log.costs,
+ log.control_regs,
+ log.state_regs,
+ log.gm_stops,
+ log.th_stops,
+ log.steps,
+ figIndex=2)
# Visualizing the solution in gepetto-viewer
if WITHDISPLAY:
diff --git a/examples/bipedal_walking_from_foot_traj.py b/examples/bipedal_walk.py
similarity index 66%
rename from examples/bipedal_walking_from_foot_traj.py
rename to examples/bipedal_walk.py
index 5dc018defba3ed3e7293b8caa172752d39921c54..70f40a7fa694886f0d4a5c3bcef26ee4101d0075 100644
--- a/examples/bipedal_walking_from_foot_traj.py
+++ b/examples/bipedal_walk.py
@@ -1,3 +1,4 @@
+import os
import sys
import numpy as np
@@ -7,8 +8,8 @@ import example_robot_data
import pinocchio
from crocoddyl.utils.biped import SimpleBipedGaitProblem, plotSolution
-WITHDISPLAY = 'display' in sys.argv
-WITHPLOT = 'plot' in sys.argv
+WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
+WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# Creating the lower-body part of Talos
talos_legs = example_robot_data.loadTalosLegs()
@@ -46,8 +47,19 @@ for i, phase in enumerate(GAITPHASES):
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
- if WITHDISPLAY:
- ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(talos_legs, 4, 4, cameraTF)])
+ if WITHDISPLAY and WITHPLOT:
+ ddp[i].setCallbacks([
+ crocoddyl.CallbackLogger(),
+ crocoddyl.CallbackVerbose(),
+ crocoddyl.CallbackDisplay(talos_legs, 4, 4, cameraTF)
+ ])
+ elif WITHDISPLAY:
+ ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackVerbose()])
+ elif WITHPLOT:
+ ddp[i].setCallbacks([
+ crocoddyl.CallbackLogger(),
+ crocoddyl.CallbackVerbose(),
+ ])
else:
ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
@@ -70,6 +82,20 @@ if WITHPLOT:
xs = []
us = []
for i, phase in enumerate(GAITPHASES):
- xs.extend(ddp[i].xs)
+ xs.extend(ddp[i].xs[:-1])
us.extend(ddp[i].us)
- plotSolution(talos_legs.model, xs, us)
+ log = ddp[0].getCallbacks()[0]
+ plotSolution(talos_legs.model, xs, us, figIndex=1, show=False)
+
+ for i, phase in enumerate(GAITPHASES):
+ title = phase.keys()[0] + " (phase " + str(i) + ")"
+ log = ddp[i].getCallbacks()[0]
+ crocoddyl.plotConvergence(log.costs,
+ log.control_regs,
+ log.state_regs,
+ log.gm_stops,
+ log.th_stops,
+ log.steps,
+ figTitle=title,
+ figIndex=i + 3,
+ show=True if i == len(GAITPHASES) - 1 else False)
diff --git a/examples/log/talos_arm.log b/examples/log/arm_manipulation.log
similarity index 78%
rename from examples/log/talos_arm.log
rename to examples/log/arm_manipulation.log
index ae7d84894dc23827b688b8edecf4f931394a5146..6bec912e443bb0741984f8e6e275edc82b8e3395 100644
--- a/examples/log/talos_arm.log
+++ b/examples/log/arm_manipulation.log
@@ -6,10 +6,3 @@ iter cost stop grad xreg ureg step feas
4 1.97477e-01 1.20824e-08 1.03534e-01 1.00000e-09 1.00000e-09 1.0000 1
5 1.92250e-01 1.00371e-09 9.15511e-03 1.00000e-09 1.00000e-09 1.0000 1
6 1.89677e-01 2.26074e-10 2.14801e-03 1.00000e-09 1.00000e-09 1.0000 1
-
-The reached pose by the wrist is
- R =
- 0.999966 0.005158 0.00649231
--0.00513193 0.999979 -0.00402692
--0.00651294 0.00399347 0.999971
- p = 0.0102162 0.00402174 0.304815
diff --git a/examples/log/bipedal_walk.log b/examples/log/bipedal_walk.log
new file mode 100644
index 0000000000000000000000000000000000000000..5641d0a611387c3ada881ad2f8a1409be5695a08
--- /dev/null
+++ b/examples/log/bipedal_walk.log
@@ -0,0 +1,56 @@
+*** SOLVE walking ***
+iter cost stop grad xreg ureg step feas
+ 0 3.79882e+06 5.92143e+11 8.10615e+07 1.00000e-02 1.00000e-02 1.0000 1
+ 1 3.32210e+05 2.59218e+14 7.59517e+06 1.00000e-03 1.00000e-03 1.0000 1
+ 2 3.52779e+04 4.05441e+12 6.62876e+05 1.00000e-04 1.00000e-04 1.0000 1
+ 3 1.10639e+03 3.51410e+12 6.90638e+04 1.00000e-05 1.00000e-05 1.0000 1
+ 4 7.45973e+02 3.22925e+10 7.20883e+02 1.00000e-06 1.00000e-06 1.0000 1
+ 5 7.45953e+02 1.98486e+06 4.06963e-02 1.00000e-07 1.00000e-07 1.0000 1
+ 6 7.45953e+02 8.50158e-01 6.98867e-06 1.00000e-08 1.00000e-08 1.0000 1
+ 7 7.45953e+02 7.41961e-05 1.04647e-07 1.00000e-09 1.00000e-09 1.0000 1
+ 8 7.45953e+02 2.13529e-07 1.64307e-09 1.00000e-09 1.00000e-09 1.0000 1
+ 9 7.45953e+02 2.13138e-09 3.44634e-11 1.00000e-09 1.00000e-09 1.0000 1
+iter cost stop grad xreg ureg step feas
+ 10 7.45953e+02 6.81549e-11 8.17711e-12 1.00000e-09 1.00000e-09 1.0000 1
+*** SOLVE walking ***
+iter cost stop grad xreg ureg step feas
+ 0 8.72974e+06 2.47561e+12 3.69452e+08 1.00000e-02 1.00000e-02 1.0000 1
+ 1 4.51365e+05 9.34606e+14 1.74565e+07 1.00000e-03 1.00000e-03 1.0000 1
+ 2 4.70803e+04 3.50339e+12 9.00802e+05 1.00000e-04 1.00000e-04 1.0000 1
+ 3 4.66196e+03 4.96423e+12 9.23052e+04 1.00000e-05 1.00000e-05 1.0000 1
+ 4 9.27458e+02 3.46862e+11 7.46924e+03 1.00000e-06 1.00000e-06 1.0000 1
+ 5 9.27341e+02 1.26462e+07 2.34127e-01 1.00000e-07 1.00000e-07 1.0000 1
+ 6 9.27341e+02 4.71575e+00 1.29549e-05 1.00000e-08 1.00000e-08 1.0000 1
+ 7 9.27341e+02 1.92036e-04 1.93143e-07 1.00000e-09 1.00000e-09 1.0000 1
+ 8 9.27341e+02 5.65419e-07 3.15528e-09 1.00000e-09 1.00000e-09 1.0000 1
+ 9 9.27341e+02 4.11015e-09 9.22155e-11 1.00000e-09 1.00000e-09 1.0000 1
+iter cost stop grad xreg ureg step feas
+ 10 9.27341e+02 1.04428e-10 1.37345e-11 1.00000e-09 1.00000e-09 0.5000 1
+*** SOLVE walking ***
+iter cost stop grad xreg ureg step feas
+ 0 1.23185e+07 5.69122e+12 8.74288e+08 1.00000e-02 1.00000e-02 1.0000 1
+ 1 6.32257e+05 1.39715e+15 2.46338e+07 1.00000e-03 1.00000e-03 1.0000 1
+ 2 3.23009e+04 1.58785e+13 1.26257e+06 1.00000e-04 1.00000e-04 1.0000 1
+ 3 1.16817e+04 3.04744e+12 6.27451e+04 1.00000e-05 1.00000e-05 1.0000 1
+ 4 9.28160e+02 1.01141e+12 2.15075e+04 1.00000e-06 1.00000e-06 1.0000 1
+ 5 9.27954e+02 1.53852e+07 4.12158e-01 1.00000e-07 1.00000e-07 1.0000 1
+ 6 9.27954e+02 1.49702e+00 1.12149e-05 1.00000e-08 1.00000e-08 1.0000 1
+ 7 9.27954e+02 1.35106e-04 1.65220e-07 1.00000e-09 1.00000e-09 1.0000 1
+ 8 9.27954e+02 4.98415e-07 2.79919e-09 1.00000e-09 1.00000e-09 1.0000 1
+ 9 9.27954e+02 3.66849e-09 1.66261e-10 1.00000e-09 1.00000e-09 1.0000 1
+iter cost stop grad xreg ureg step feas
+ 10 9.27954e+02 2.51946e-10 5.23493e-11 1.00000e-09 1.00000e-09 0.0625 1
+*** SOLVE walking ***
+iter cost stop grad xreg ureg step feas
+ 0 1.34228e+07 1.02317e+13 1.59557e+09 1.00000e-02 1.00000e-02 1.0000 1
+ 1 4.68564e+05 1.48838e+15 2.68423e+07 1.00000e-03 1.00000e-03 1.0000 1
+ 2 3.56579e+04 1.48909e+13 9.35190e+05 1.00000e-04 1.00000e-04 1.0000 1
+ 3 9.72632e+03 3.29361e+12 6.94578e+04 1.00000e-05 1.00000e-05 1.0000 1
+ 4 9.28986e+02 7.97376e+11 1.75953e+04 1.00000e-06 1.00000e-06 1.0000 1
+ 5 9.28678e+02 2.96602e+07 6.15953e-01 1.00000e-07 1.00000e-07 1.0000 1
+ 6 9.28678e+02 1.72808e+01 1.55403e-05 1.00000e-08 1.00000e-08 1.0000 1
+ 7 9.28678e+02 3.98616e-04 2.25356e-07 1.00000e-09 1.00000e-09 1.0000 1
+ 8 9.28678e+02 7.57571e-07 3.56856e-09 1.00000e-09 1.00000e-09 1.0000 1
+ 9 9.28678e+02 4.64317e-09 6.66206e-11 1.00000e-09 1.00000e-09 0.5000 1
+iter cost stop grad xreg ureg step feas
+ 10 9.28678e+02 9.39527e-10 2.23473e-11 1.00000e-09 1.00000e-09 1.0000 1
\ No newline at end of file
diff --git a/examples/log/bipedal_walking_from_foot_traj.log b/examples/log/bipedal_walking_from_foot_traj.log
deleted file mode 100644
index 8252ec5e3642427132735b5a8f5b6808472d5792..0000000000000000000000000000000000000000
--- a/examples/log/bipedal_walking_from_foot_traj.log
+++ /dev/null
@@ -1,43 +0,0 @@
-*** SOLVE walking ***
-iter cost stop grad xreg ureg step feas
- 0 1.15825e+06 3.08454e+09 8.10612e+07 1.00000e-02 1.00000e-02 1.0000 1
- 1 1.20312e+05 1.18690e+08 2.31390e+06 1.00000e-03 1.00000e-03 1.0000 1
- 2 1.73531e+03 3.46775e+07 2.39055e+05 1.00000e-04 1.00000e-04 1.0000 1
- 3 7.65753e+02 3.94554e+06 1.94737e+03 1.00000e-05 1.00000e-05 1.0000 1
- 4 7.61391e+02 1.31205e+04 8.72483e+00 1.00000e-06 1.00000e-06 1.0000 1
- 5 7.61391e+02 2.51348e-02 6.88732e-05 1.00000e-07 1.00000e-07 1.0000 1
- 6 7.61391e+02 3.59102e-07 5.86261e-07 1.00000e-08 1.00000e-08 1.0000 1
- 7 7.61391e+02 8.32838e-10 8.94723e-09 1.00000e-09 1.00000e-09 1.0000 1
-*** SOLVE walking ***
-iter cost stop grad xreg ureg step feas
- 0 1.23256e+06 1.32614e+10 3.69455e+08 1.00000e-02 1.00000e-02 1.0000 1
- 1 1.25284e+05 1.25947e+08 2.46199e+06 1.00000e-03 1.00000e-03 1.0000 1
- 2 1.96372e+03 3.80122e+07 2.48604e+05 1.00000e-04 1.00000e-04 1.0000 1
- 3 9.57185e+02 3.93920e+06 2.02273e+03 1.00000e-05 1.00000e-05 1.0000 1
- 4 9.51989e+02 1.42955e+04 1.03902e+01 1.00000e-06 1.00000e-06 1.0000 1
- 5 9.51989e+02 1.31179e-01 1.73358e-04 1.00000e-07 1.00000e-07 1.0000 1
- 6 9.51989e+02 1.04272e-06 1.49411e-06 1.00000e-08 1.00000e-08 1.0000 1
- 7 9.51989e+02 3.09588e-09 2.47886e-08 1.00000e-09 1.00000e-09 1.0000 1
- 8 9.51989e+02 8.67607e-11 4.54661e-10 1.00000e-09 1.00000e-09 1.0000 1
-*** SOLVE walking ***
-iter cost stop grad xreg ureg step feas
- 0 1.10575e+06 3.07089e+10 8.74294e+08 1.00000e-02 1.00000e-02 1.0000 1
- 1 9.82289e+04 1.04452e+08 2.20834e+06 1.00000e-03 1.00000e-03 1.0000 1
- 2 2.29493e+03 3.32464e+07 1.94494e+05 1.00000e-04 1.00000e-04 1.0000 1
- 3 9.59187e+02 4.83906e+06 2.68405e+03 1.00000e-05 1.00000e-05 1.0000 1
- 4 9.52600e+02 1.63914e+04 1.31751e+01 1.00000e-06 1.00000e-06 1.0000 1
- 5 9.52600e+02 3.84877e-01 4.84418e-04 1.00000e-07 1.00000e-07 1.0000 1
- 6 9.52600e+02 3.93861e-06 3.80077e-06 1.00000e-08 1.00000e-08 1.0000 1
- 7 9.52600e+02 1.09978e-08 6.38147e-08 1.00000e-09 1.00000e-09 1.0000 1
- 8 9.52600e+02 2.13246e-10 1.09478e-09 1.00000e-09 1.00000e-09 1.0000 1
-*** SOLVE walking ***
-iter cost stop grad xreg ureg step feas
- 0 9.97256e+05 5.54162e+10 1.59558e+09 1.00000e-02 1.00000e-02 1.0000 1
- 1 7.26949e+04 8.87034e+07 1.99131e+06 1.00000e-03 1.00000e-03 1.0000 1
- 2 2.91463e+03 2.87425e+07 1.43426e+05 1.00000e-04 1.00000e-04 1.0000 1
- 3 9.63182e+02 6.07916e+06 3.92224e+03 1.00000e-05 1.00000e-05 1.0000 1
- 4 9.53306e+02 2.02322e+04 1.97555e+01 1.00000e-06 1.00000e-06 1.0000 1
- 5 9.53305e+02 1.11507e+00 1.41303e-03 1.00000e-07 1.00000e-07 1.0000 1
- 6 9.53305e+02 1.68794e-05 8.18140e-06 1.00000e-08 1.00000e-08 1.0000 1
- 7 9.53305e+02 2.79748e-08 1.36146e-07 1.00000e-09 1.00000e-09 1.0000 1
- 8 9.53305e+02 4.50847e-10 2.30846e-09 1.00000e-09 1.00000e-09 1.0000 1
diff --git a/examples/log/quadrupedal_gaits.log b/examples/log/quadrupedal_gaits.log
new file mode 100644
index 0000000000000000000000000000000000000000..9cf31cd48419bdcef6992e3d9f8f645b23457cca
--- /dev/null
+++ b/examples/log/quadrupedal_gaits.log
@@ -0,0 +1,80 @@
+*** SOLVE walking ***
+iter cost stop grad xreg ureg step feas
+ 0 4.71060e+04 5.00529e+07 2.19805e+06 1.00000e-02 1.00000e-02 1.0000 1
+ 1 2.71985e+04 1.23011e+10 7.49126e+04 1.00000e-03 1.00000e-03 1.0000 1
+ 2 8.52084e+03 9.98222e+09 3.76239e+04 1.00000e-04 1.00000e-04 1.0000 1
+ 3 8.29487e+03 3.15308e+07 8.46511e+02 1.00000e-05 1.00000e-05 1.0000 1
+ 4 8.06767e+03 1.21786e+08 4.55753e+02 1.00000e-06 1.00000e-06 1.0000 1
+ 5 8.06650e+03 6.70379e+05 2.31832e+00 1.00000e-07 1.00000e-07 1.0000 1
+ 6 8.06650e+03 5.30707e+00 3.96525e-03 1.00000e-08 1.00000e-08 1.0000 1
+ 7 8.06650e+03 3.35865e-04 2.12454e-04 1.00000e-09 1.00000e-09 1.0000 1
+ 8 8.06650e+03 4.67649e-06 1.19350e-05 1.00000e-09 1.00000e-09 1.0000 1
+ 9 8.06650e+03 2.55334e-07 6.85312e-07 1.00000e-09 1.00000e-09 1.0000 1
+iter cost stop grad xreg ureg step feas
+ 10 8.06650e+03 1.53850e-08 3.98917e-08 1.00000e-09 1.00000e-09 1.0000 1
+ 11 8.06650e+03 9.23475e-10 2.34280e-09 1.00000e-09 1.00000e-09 1.0000 1
+*** SOLVE trotting ***
+iter cost stop grad xreg ureg step feas
+ 0 5.06631e+04 3.31465e+08 7.11857e+06 1.00000e-02 1.00000e-02 1.0000 1
+ 1 2.11975e+04 8.28258e+09 8.89572e+04 1.00000e-03 1.00000e-03 1.0000 1
+ 2 5.47426e+03 5.44889e+09 3.21343e+04 1.00000e-04 1.00000e-04 1.0000 1
+ 3 4.93778e+03 1.93635e+08 1.09837e+03 1.00000e-05 1.00000e-05 1.0000 1
+ 4 4.91204e+03 1.26305e+06 5.11245e+01 1.00000e-06 1.00000e-06 1.0000 1
+ 5 4.91198e+03 9.12812e+03 1.17757e-01 1.00000e-07 1.00000e-07 1.0000 1
+ 6 4.91198e+03 9.36311e-01 1.65643e-03 1.00000e-08 1.00000e-08 1.0000 1
+ 7 4.91198e+03 3.73906e-04 6.12648e-05 1.00000e-09 1.00000e-09 1.0000 1
+ 8 4.91198e+03 1.40822e-06 2.31119e-06 1.00000e-09 1.00000e-09 1.0000 1
+ 9 4.91198e+03 5.96580e-08 8.83018e-08 1.00000e-09 1.00000e-09 1.0000 1
+iter cost stop grad xreg ureg step feas
+ 10 4.91198e+03 2.43644e-09 3.42869e-09 1.00000e-09 1.00000e-09 1.0000 1
+ 11 4.91198e+03 9.87581e-11 1.41995e-10 1.00000e-09 1.00000e-09 1.0000 1
+*** SOLVE pacing ***
+iter cost stop grad xreg ureg step feas
+ 0 5.00587e+06 7.29811e+08 1.62092e+07 1.00000e-02 1.00000e-02 1.0000 1
+ 1 5.45480e+05 5.44671e+12 9.97788e+06 1.00000e-03 1.00000e-03 1.0000 1
+ 2 2.39173e+05 4.21088e+11 1.07453e+06 1.00000e-04 1.00000e-04 1.0000 1
+ 3 9.17900e+03 1.52753e+11 4.63455e+05 1.00000e-05 1.00000e-05 1.0000 1
+ 4 7.35430e+03 9.75817e+08 3.65511e+03 1.00000e-06 1.00000e-06 1.0000 1
+ 5 7.35003e+03 1.99372e+06 8.55166e+00 1.00000e-07 1.00000e-07 1.0000 1
+ 6 7.35002e+03 1.97563e+02 8.29399e-03 1.00000e-08 1.00000e-08 1.0000 1
+ 7 7.35002e+03 2.42706e-01 2.12813e-04 1.00000e-09 1.00000e-09 1.0000 1
+ 8 7.35002e+03 3.39558e-04 6.86314e-06 1.00000e-09 1.00000e-09 1.0000 1
+ 9 7.35002e+03 1.02090e-06 2.34919e-07 1.00000e-09 1.00000e-09 1.0000 1
+iter cost stop grad xreg ureg step feas
+ 10 7.35002e+03 4.68927e-08 8.29480e-09 1.00000e-09 1.00000e-09 1.0000 1
+ 11 7.35002e+03 1.53040e-09 2.99125e-10 1.00000e-09 1.00000e-09 1.0000 1
+ 12 7.35002e+03 5.39239e-11 1.09810e-11 1.00000e-09 1.00000e-09 1.0000 1
+*** SOLVE bounding ***
+iter cost stop grad xreg ureg step feas
+ 0 6.30222e+05 5.11011e+09 2.46895e+07 1.00000e-02 1.00000e-02 1.0000 1
+ 1 3.56791e+04 3.10021e+11 1.22924e+06 1.00000e-03 1.00000e-03 1.0000 1
+ 2 1.53406e+04 8.43694e+09 5.32585e+04 1.00000e-04 1.00000e-04 1.0000 1
+ 3 8.18536e+03 4.19892e+09 1.43797e+04 1.00000e-05 1.00000e-05 1.0000 1
+ 4 8.01180e+03 6.67017e+07 3.41062e+02 1.00000e-06 1.00000e-06 1.0000 1
+ 5 8.01115e+03 1.11178e+05 1.27823e+00 1.00000e-07 1.00000e-07 1.0000 1
+ 6 8.01114e+03 2.69712e+02 1.00216e-02 1.00000e-08 1.00000e-08 1.0000 1
+ 7 8.01114e+03 9.99891e-02 8.11450e-04 1.00000e-09 1.00000e-09 1.0000 1
+ 8 8.01114e+03 2.12973e-03 1.04521e-04 1.00000e-09 1.00000e-09 1.0000 1
+ 9 8.01114e+03 2.23460e-04 1.37404e-05 1.00000e-09 1.00000e-09 1.0000 1
+iter cost stop grad xreg ureg step feas
+ 10 8.01114e+03 2.86775e-05 1.80826e-06 1.00000e-09 1.00000e-09 1.0000 1
+ 11 8.01114e+03 3.76104e-06 2.37988e-07 1.00000e-09 1.00000e-09 1.0000 1
+ 12 8.01114e+03 4.95058e-07 3.13220e-08 1.00000e-09 1.00000e-09 1.0000 1
+ 13 8.01114e+03 6.51341e-08 4.12247e-09 1.00000e-09 1.00000e-09 1.0000 1
+ 14 8.01114e+03 8.57345e-09 5.42632e-10 1.00000e-09 1.00000e-09 1.0000 1
+ 15 8.01114e+03 1.12839e-09 7.14990e-11 1.00000e-09 1.00000e-09 1.0000 1
+ 16 8.01114e+03 1.48545e-10 9.56772e-12 1.00000e-09 1.00000e-09 1.0000 1
+*** SOLVE jumping ***
+iter cost stop grad xreg ureg step feas
+ 0 4.84990e+05 1.96114e+08 2.00583e+07 1.00000e-02 1.00000e-02 1.0000 1
+ 1 2.55518e+05 5.45932e+06 9.44614e+05 1.00000e-02 1.00000e-02 0.5000 1
+ 2 3.21623e+04 6.08478e+06 4.86889e+05 1.00000e-03 1.00000e-03 1.0000 1
+ 3 8.84222e+03 8.03098e+05 4.63662e+04 1.00000e-04 1.00000e-04 1.0000 1
+ 4 8.21026e+03 2.37356e+04 1.22753e+03 1.00000e-05 1.00000e-05 1.0000 1
+ 5 8.18969e+03 5.29795e+02 4.04915e+01 1.00000e-06 1.00000e-06 1.0000 1
+ 6 8.18963e+03 1.75809e-01 9.89714e-02 1.00000e-07 1.00000e-07 1.0000 1
+ 7 8.18963e+03 1.79513e-04 1.19506e-03 1.00000e-08 1.00000e-08 1.0000 1
+ 8 8.18963e+03 1.44032e-06 2.62011e-05 1.00000e-09 1.00000e-09 1.0000 1
+ 9 8.18963e+03 3.98758e-08 6.24980e-07 1.00000e-09 1.00000e-09 1.0000 1
+iter cost stop grad xreg ureg step feas
+ 10 8.18963e+03 9.15627e-10 1.56982e-08 1.00000e-09 1.00000e-09 1.0000 1
\ No newline at end of file
diff --git a/examples/log/quadrupedal_walking_balancing.log b/examples/log/quadrupedal_walking_balancing.log
deleted file mode 100644
index fdb151b755219b2d84c803e0d1f161366e407e8d..0000000000000000000000000000000000000000
--- a/examples/log/quadrupedal_walking_balancing.log
+++ /dev/null
@@ -1,84 +0,0 @@
-*** SOLVE walking ***
-iter cost stop grad xreg ureg step feas
- 0 1.37485e+04 1.85164e+07 7.91406e+05 1.00000e-02 1.00000e-02 1.0000 1
- 1 6.98715e+03 2.91164e+09 1.61165e+04 1.00000e-03 1.00000e-03 1.0000 1
- 2 5.00494e+03 9.61535e+08 3.87659e+03 1.00000e-04 1.00000e-04 1.0000 1
- 3 4.87427e+03 1.62571e+06 2.83792e+02 1.00000e-05 1.00000e-05 1.0000 1
- 4 4.83018e+03 1.66146e+07 8.69937e+01 1.00000e-06 1.00000e-06 1.0000 1
- 5 4.83003e+03 4.89373e+04 3.02862e-01 1.00000e-07 1.00000e-07 1.0000 1
- 6 4.83003e+03 3.25805e-01 5.72241e-04 1.00000e-08 1.00000e-08 1.0000 1
- 7 4.83003e+03 6.23636e-05 2.00015e-05 1.00000e-09 1.00000e-09 1.0000 1
- 8 4.83003e+03 9.24953e-07 7.58347e-07 1.00000e-09 1.00000e-09 1.0000 1
- 9 4.83003e+03 3.04042e-08 2.94413e-08 1.00000e-09 1.00000e-09 1.0000 1
-iter cost stop grad xreg ureg step feas
- 10 4.83003e+03 1.10821e-09 1.16218e-09 1.00000e-09 1.00000e-09 1.0000 1
- 11 4.83003e+03 4.21539e-11 4.65054e-11 1.00000e-09 1.00000e-09 1.0000 1
-*** SOLVE trotting ***
-iter cost stop grad xreg ureg step feas
- 0 2.68468e+04 4.69817e+09 4.25814e+06 1.00000e-02 1.00000e-02 1.0000 1
- 1 1.14296e+04 1.52301e+09 4.16464e+04 1.00000e-03 1.00000e-03 1.0000 1
- 2 5.59099e+03 2.13557e+09 1.26334e+04 1.00000e-04 1.00000e-04 1.0000 1
- 3 4.93701e+03 2.51384e+08 1.32067e+03 1.00000e-05 1.00000e-05 1.0000 1
- 4 4.91198e+03 1.73224e+06 4.93214e+01 1.00000e-06 1.00000e-06 1.0000 1
- 5 4.91188e+03 1.22601e+04 1.90307e-01 1.00000e-07 1.00000e-07 1.0000 1
- 6 4.91188e+03 8.56271e-01 1.51564e-03 1.00000e-08 1.00000e-08 1.0000 1
- 7 4.91188e+03 4.61460e-04 6.35166e-05 1.00000e-09 1.00000e-09 1.0000 1
- 8 4.91188e+03 4.32643e-06 2.75983e-06 1.00000e-09 1.00000e-09 1.0000 1
- 9 4.91188e+03 1.81072e-07 1.22024e-07 1.00000e-09 1.00000e-09 1.0000 1
-iter cost stop grad xreg ureg step feas
- 10 4.91188e+03 7.98564e-09 5.47036e-09 1.00000e-09 1.00000e-09 1.0000 1
- 11 4.91188e+03 3.45652e-10 2.66409e-10 1.00000e-09 1.00000e-09 1.0000 1
-*** SOLVE pacing ***
-iter cost stop grad xreg ureg step feas
- 0 7.15118e+05 4.40102e+11 1.92790e+07 1.00000e-02 1.00000e-02 1.0000 1
- 1 4.31583e+05 4.96153e+11 1.40778e+06 1.00000e-03 1.00000e-03 1.0000 1
- 2 8.39776e+03 3.16015e+11 8.47409e+05 1.00000e-04 1.00000e-04 1.0000 1
- 3 7.29461e+03 3.68041e+08 2.49227e+03 1.00000e-05 1.00000e-05 1.0000 1
- 4 7.05636e+03 1.07475e+08 4.75721e+02 1.00000e-06 1.00000e-06 1.0000 1
- 5 7.05482e+03 6.63227e+05 3.05895e+00 1.00000e-07 1.00000e-07 1.0000 1
- 6 7.05482e+03 8.99408e+00 3.95375e-03 1.00000e-08 1.00000e-08 1.0000 1
- 7 7.05482e+03 1.05298e-03 8.52418e-05 1.00000e-09 1.00000e-09 1.0000 1
- 8 7.05482e+03 6.22669e-06 2.21452e-06 1.00000e-09 1.00000e-09 1.0000 1
- 9 7.05482e+03 3.78027e-07 6.75324e-08 1.00000e-09 1.00000e-09 1.0000 1
-iter cost stop grad xreg ureg step feas
- 10 7.05482e+03 2.17786e-09 2.35971e-09 1.00000e-09 1.00000e-09 1.0000 1
- 11 7.05482e+03 4.50072e-10 9.12655e-11 1.00000e-09 1.00000e-09 1.0000 1
-*** SOLVE bounding ***
-iter cost stop grad xreg ureg step feas
- 0 8.18722e+05 1.08687e+11 2.00020e+07 1.00000e-02 1.00000e-02 1.0000 1
- 1 7.79387e+04 6.04524e+11 1.60133e+06 1.00000e-03 1.00000e-03 1.0000 1
- 2 1.84906e+04 2.28056e+10 1.32379e+05 1.00000e-04 1.00000e-04 1.0000 1
- 3 1.08699e+04 2.65616e+09 1.52839e+04 1.00000e-05 1.00000e-05 1.0000 1
- 4 1.07031e+04 6.33626e+07 3.27233e+02 1.00000e-06 1.00000e-06 1.0000 1
- 5 1.07025e+04 8.21707e+04 1.43045e+00 1.00000e-07 1.00000e-07 1.0000 1
- 6 1.07025e+04 2.70740e+03 8.84330e-02 1.00000e-08 1.00000e-08 1.0000 1
- 7 1.07025e+04 2.55970e+00 1.16696e-02 1.00000e-09 1.00000e-09 1.0000 1
- 8 1.07025e+04 1.15859e-01 1.93454e-03 1.00000e-09 1.00000e-09 1.0000 1
- 9 1.07025e+04 6.44470e-03 3.24309e-04 1.00000e-09 1.00000e-09 1.0000 1
-iter cost stop grad xreg ureg step feas
- 10 1.07025e+04 7.54970e-04 5.44404e-05 1.00000e-09 1.00000e-09 1.0000 1
- 11 1.07025e+04 1.30345e-04 9.14194e-06 1.00000e-09 1.00000e-09 1.0000 1
- 12 1.07025e+04 2.10326e-05 1.53515e-06 1.00000e-09 1.00000e-09 1.0000 1
- 13 1.07025e+04 3.57494e-06 2.57797e-07 1.00000e-09 1.00000e-09 1.0000 1
- 14 1.07025e+04 5.96885e-07 4.32913e-08 1.00000e-09 1.00000e-09 1.0000 1
- 15 1.07025e+04 1.00464e-07 7.26986e-09 1.00000e-09 1.00000e-09 1.0000 1
- 16 1.07025e+04 1.68545e-08 1.22086e-09 1.00000e-09 1.00000e-09 1.0000 1
- 17 1.07025e+04 2.83150e-09 2.05014e-10 1.00000e-09 1.00000e-09 1.0000 1
- 18 1.07025e+04 4.75414e-10 3.44277e-11 1.00000e-09 1.00000e-09 0.5000 1
-*** SOLVE jumping ***
-iter cost stop grad xreg ureg step feas
- 0 1.18138e+05 1.07909e+04 8.62193e+04 1.00000e-02 1.00000e-02 1.0000 1
- 1 2.18963e+04 8.42151e+05 2.15921e+05 1.00000e-03 1.00000e-03 1.0000 1
- 2 1.65014e+04 1.09570e+04 3.18265e+04 1.00000e-03 1.00000e-03 0.5000 1
- 3 6.98607e+03 4.92609e+03 2.01180e+04 1.00000e-04 1.00000e-04 1.0000 1
- 4 5.22205e+03 8.08190e+02 3.43545e+03 1.00000e-05 1.00000e-05 1.0000 1
- 5 5.19468e+03 6.44125e+00 4.90813e+01 1.00000e-06 1.00000e-06 1.0000 1
- 6 5.19331e+03 1.22269e-02 2.09810e+00 1.00000e-07 1.00000e-07 1.0000 1
- 7 5.19316e+03 2.12864e-04 2.26843e-01 1.00000e-08 1.00000e-08 1.0000 1
- 8 5.19314e+03 2.02209e-05 2.62528e-02 1.00000e-09 1.00000e-09 1.0000 1
- 9 5.19314e+03 3.25291e-06 3.05574e-03 1.00000e-09 1.00000e-09 1.0000 1
-iter cost stop grad xreg ureg step feas
- 10 5.19314e+03 3.51994e-07 3.56664e-04 1.00000e-09 1.00000e-09 1.0000 1
- 11 5.19314e+03 4.48866e-08 4.16925e-05 1.00000e-09 1.00000e-09 1.0000 1
- 12 5.19314e+03 5.00807e-09 4.87737e-06 1.00000e-09 1.00000e-09 1.0000 1
- 13 5.19314e+03 6.08521e-10 5.70796e-07 1.00000e-09 1.00000e-09 1.0000 1
diff --git a/examples/notebooks/manipulator.ipynb b/examples/notebooks/arm_manipulation.ipynb
similarity index 98%
rename from examples/notebooks/manipulator.ipynb
rename to examples/notebooks/arm_manipulation.ipynb
index 7c9d4d82aaac8fd544df1143501a02ded21edfab..cb03cb8a0869e7a95ca33453d7d320906d82ee05 100644
--- a/examples/notebooks/manipulator.ipynb
+++ b/examples/notebooks/arm_manipulation.ipynb
@@ -180,7 +180,7 @@
"## IV. Callbacks\n",
"\n",
"Callback functions are needed for analysing and debugging the performance of the solver for your specific problem.\n",
- "For problems defined with Pinocchio, you can display the robot trajectory per each iterate by including CallbackSolverDisplay. With this callback, you can display robot motions with different rates. Additionally, CallbackVerbose prints a message that allows us to understand the behaviour of the solver.\n",
+ "For problems defined with Pinocchio, you can display the robot trajectory per each iterate by including CallbackDisplay. With this callback, you can display robot motions with different rates. Additionally, CallbackVerbose prints a message that allows us to understand the behaviour of the solver.\n",
"\n",
"Generally speaking, an user is able to describe any callback function. This function will be run once per iterate and it has access to all data."
]
diff --git a/examples/notebooks/bipedal_walking.ipynb b/examples/notebooks/bipedal_walking.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..8c15bf9f9915f5f95efd3ca78c65162885d2fab1
--- /dev/null
+++ b/examples/notebooks/bipedal_walking.ipynb
@@ -0,0 +1,186 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Multi-Contact dynamics for biped walking (Talos legs)\n",
+ "In this example, we describe the multi-contact dynamics through holonomic constraints for the support legs. From the Gauss principle, we have derived the model as:\n",
+ "$$\n",
+ "\\left[\\begin{matrix}\n",
+ " \\mathbf{M} & \\mathbf{J}^{\\top}_c \\\\\n",
+ " {\\mathbf{J}_{c}} & \\mathbf{0} \\\\\n",
+ "\\end{matrix}\\right]\n",
+ "\\left[\\begin{matrix}\n",
+ " \\dot{\\mathbf{v}} \\\\ -\\boldsymbol{\\lambda}\n",
+ "\\end{matrix}\\right]\n",
+ " = \n",
+ "\\left[\\begin{matrix}\n",
+ " \\boldsymbol{\\tau} - \\mathbf{h} \\\\\n",
+ " -\\dot{\\mathbf{J}}_c \\mathbf{v} \\\\\n",
+ "\\end{matrix}\\right]$$.\n",
+ "\n",
+ "\n",
+ "Base on a predefined walking gait, we build per each phase a specific multi-contact dynamics. Indeed we need to describe multi-phase optimal control problem. One can formulate the multi-contact optimal control problem (MCOP) as follows:\n",
+ "\n",
+ "\n",
+ "$$\\mathbf{X}^*,\\mathbf{U}^*=\n",
+ "\\begin{Bmatrix} \\mathbf{x}^*_0,\\cdots,\\mathbf{x}^*_N \\\\\n",
+ "\t\t\t\t \\mathbf{u}^*_0,\\cdots,\\mathbf{u}^*_N\n",
+ "\\end{Bmatrix} =\n",
+ "\\arg\\min_{\\mathbf{X},\\mathbf{U}} \\sum_{p=0}^P \\sum_{k=1}^{N(p)} \\int_{t_k}^{t_k+\\Delta t} l_p(\\mathbf{x},\\mathbf{u})dt$$\n",
+ "subject to\n",
+ "$$ \\mathbf{\\dot{x}} = \\mathbf{f}_p(\\mathbf{x},\\mathbf{u}), \\text{for } t \\in [\\tau_p,\\tau_{p+1}]$$\n",
+ "\n",
+ "$$ \\mathbf{g}(\\mathbf{v}^{p+1},\\mathbf{v}^p) = \\mathbf{0}$$\n",
+ "\n",
+ "$$ \\mathbf{x}\\in\\mathcal{X}_p, \\mathbf{u}\\in\\mathcal{U}_p, \\boldsymbol{\\lambda}\\in\\mathcal{K}_p.$$\n",
+ "\n",
+ "where $\\mathbf{g}(\\cdot,\\cdot,\\cdot)$ describes the contact dynamics, and they represents terminal constraints in each walking phase. In this example we use the following impact model:\n",
+ "\n",
+ "$$\\mathbf{M}(\\mathbf{v}_{next}-\\mathbf{v}) = \\mathbf{J}_{impulse}^T$$\n",
+ "\n",
+ "$$\\mathbf{J}_{impulse} \\mathbf{v}_{next} = \\mathbf{0}$$\n",
+ "\n",
+ "$$\\mathbf{J}_{c} \\mathbf{v}_{next} = \\mathbf{J}_{c} \\mathbf{v}$$"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "SimpleBipedGaitProblem class builds action models for each locomotion phase:\n",
+ " - createSwingFootModel: defines an action model for the swing phase\n",
+ " - createFootSwitchModel: defines an action model for switch knots between phases\n",
+ " \n",
+ "Then we build a walking by combining a set of contact phases and their contact switches. This is defined by createFootstepModel\n",
+ "\n",
+ "Now let's create a walking OC problem for the Talos legs."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from crocoddyl.utils.biped import SimpleBipedGaitProblem\n",
+ "\n",
+ "# Creating the lower-body part of Talos\n",
+ "import example_robot_data\n",
+ "talos_legs = example_robot_data.loadTalosLegs()\n",
+ "\n",
+ "# Setting up the 3d walking problem\n",
+ "rightFoot = 'right_sole_link'\n",
+ "leftFoot = 'left_sole_link'\n",
+ "gait = SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot)\n",
+ "\n",
+ "\n",
+ "# Create the initial state\n",
+ "q0 = talos_legs.q0.copy()\n",
+ "v0 = zero(talos_legs.model.nv)\n",
+ "x0 = np.concatenate([q0,v0])\n",
+ "\n",
+ "\n",
+ "# Creating the walking problem\n",
+ "stepLength = 0.6 # meters\n",
+ "stepHeight = 0.1 # meters\n",
+ "timeStep = 0.0375 # seconds\n",
+ "stepKnots = 20\n",
+ "supportKnots = 10\n",
+ "problem = gait.createWalkingProblem(x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots)\n",
+ "\n",
+ "\n",
+ "# Solving the 3d walking problem using Feasibility-prone DDP\n",
+ "ddp = crocoddyl.SolverFDDP(problem)\n",
+ "cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]\n",
+ "ddp.setCallbacks([crocoddyl.CallbackLogger(),\n",
+ " crocoddyl.CallbackVerbose(),\n",
+ " crocoddyl.CallbackDisplay(talos_legs, 4, 4,cameraTF)])\n",
+ "ddp.th_stop = 1e-9\n",
+ "init_xs = [talos_legs.model.defaultState]*problem.T\n",
+ "init_us = []\n",
+ "maxiter = 1000\n",
+ "regInit = 0.1\n",
+ "ddp.solve(init_xs, init_us, maxiter, False, regInit)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "With the following commands we can plot \n",
+ " - the state and control trajectories, and\n",
+ " - the DDP performance"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "%matplotlib inline\n",
+ "\n",
+ "# Plotting the solution and the DDP convergence\n",
+ "log = ddp.getCallbacks()[0]\n",
+ "crocoddyl.plotOCSolution(log.xs, log.us)\n",
+ "crocoddyl.plotConvergence(log.costs,log.control_regs,\n",
+ " log.state_regs,log.gm_stops,\n",
+ " log.th_stops,log.steps)\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "Finally we can visualize the solution using gepetto-viewer."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "# Visualization of the DDP solution in gepetto-viewer\n",
+ "ddp.getCallbacks()[2](ddp)\n",
+ "CallbackDisplay(talos_legs)(ddp)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## VI. Understanding the walking problem\n",
+ "\n",
+ "In this problem we pre-defined a 20 and 10 knots for the step and double support phases with $dt=$3.75e-2, repectively. \n",
+ "\n",
+ " 1. Could you tell us how much is the foot step and double support duration?\n",
+ " 2. What happens when do we change $dt$ (e.g. 2e-2 secs)?\n",
+ " 3. What happens when do we change the number of step knots (e.g. 10)?\n"
+ ]
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "Python 2",
+ "language": "python",
+ "name": "python2"
+ },
+ "language_info": {
+ "codemirror_mode": {
+ "name": "ipython",
+ "version": 2
+ },
+ "file_extension": ".py",
+ "mimetype": "text/x-python",
+ "name": "python",
+ "nbconvert_exporter": "python",
+ "pygments_lexer": "ipython2",
+ "version": "2.7.12"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}
diff --git a/examples/notebooks/bipedal_walking_from_foot_traj.ipynb b/examples/notebooks/bipedal_walking_from_foot_traj.ipynb
deleted file mode 100644
index e1b64484f15dd704d2148f8b4f9746b6505678a1..0000000000000000000000000000000000000000
--- a/examples/notebooks/bipedal_walking_from_foot_traj.ipynb
+++ /dev/null
@@ -1,371 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Multi-Contact dynamics for biped walking (Talos legs)\n",
- "In this example, we describe the multi-contact dynamics through holonomic constraints for the support legs. From the Gauss principle, we have derived the model as:\n",
- "$$\n",
- "\\left[\\begin{matrix}\n",
- " \\mathbf{M} & \\mathbf{J}^{\\top}_c \\\\\n",
- " {\\mathbf{J}_{c}} & \\mathbf{0} \\\\\n",
- "\\end{matrix}\\right]\n",
- "\\left[\\begin{matrix}\n",
- " \\dot{\\mathbf{v}} \\\\ -\\boldsymbol{\\lambda}\n",
- "\\end{matrix}\\right]\n",
- " = \n",
- "\\left[\\begin{matrix}\n",
- " \\boldsymbol{\\tau} - \\mathbf{h} \\\\\n",
- " -\\dot{\\mathbf{J}}_c \\mathbf{v} \\\\\n",
- "\\end{matrix}\\right]$$.\n",
- "\n",
- "\n",
- "Base on a predefined walking gait, we build per each phase a specific multi-contact dynamics. Indeed we need to describe multi-phase optimal control problem. One can formulate the multi-contact optimal control problem (MCOP) as follows:\n",
- "\n",
- "\n",
- "$$\\mathbf{X}^*,\\mathbf{U}^*=\n",
- "\\begin{Bmatrix} \\mathbf{x}^*_0,\\cdots,\\mathbf{x}^*_N \\\\\n",
- "\t\t\t\t \\mathbf{u}^*_0,\\cdots,\\mathbf{u}^*_N\n",
- "\\end{Bmatrix} =\n",
- "\\arg\\min_{\\mathbf{X},\\mathbf{U}} \\sum_{p=0}^P \\sum_{k=1}^{N(p)} \\int_{t_k}^{t_k+\\Delta t} l_p(\\mathbf{x},\\mathbf{u})dt$$\n",
- "subject to\n",
- "$$ \\mathbf{\\dot{x}} = \\mathbf{f}_p(\\mathbf{x},\\mathbf{u}), \\text{for } t \\in [\\tau_p,\\tau_{p+1}]$$\n",
- "\n",
- "$$ \\mathbf{g}(\\mathbf{v}^{p+1},\\mathbf{v}^p) = \\mathbf{0}$$\n",
- "\n",
- "$$ \\mathbf{x}\\in\\mathcal{X}_p, \\mathbf{u}\\in\\mathcal{U}_p, \\boldsymbol{\\lambda}\\in\\mathcal{K}_p.$$\n",
- "\n",
- "where $\\mathbf{g}(\\cdot,\\cdot,\\cdot)$ describes the contact dynamics, and they represents terminal constraints in each walking phase. In this example we use the following impact model:\n",
- "\n",
- "$$\\mathbf{M}(\\mathbf{v}_{next}-\\mathbf{v}) = \\mathbf{J}_{impulse}^T$$\n",
- "\n",
- "$$\\mathbf{J}_{impulse} \\mathbf{v}_{next} = \\mathbf{0}$$\n",
- "\n",
- "$$\\mathbf{J}_{c} \\mathbf{v}_{next} = \\mathbf{J}_{c} \\mathbf{v}$$\n",
- "\n",
- "\n",
- "\n",
- "First, let's define walking shooting problem:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "from crocoddyl import *\n",
- "import pinocchio\n",
- "from pinocchio.utils import *\n",
- "\n",
- "\n",
- "class TaskSE3:\n",
- " def __init__(self, oXf, frameId):\n",
- " self.oXf = oXf\n",
- " self.frameId = frameId\n",
- "\n",
- "class SimpleBipedWalkingProblem:\n",
- " \"\"\" Defines a simple 3d locomotion problem\n",
- " \"\"\"\n",
- " def __init__(self, rmodel, rightFoot, leftFoot):\n",
- " self.rmodel = rmodel\n",
- " self.rdata = rmodel.createData()\n",
- " self.state = StatePinocchio(self.rmodel)\n",
- " self.rightFoot = rightFoot\n",
- " self.leftFoot = leftFoot\n",
- " # Defining default state\n",
- " self.rmodel.defaultState = \\\n",
- " np.concatenate([m2a(self.rmodel.neutralConfiguration),\n",
- " np.zeros(self.rmodel.nv)])\n",
- " # Remove the armature\n",
- " self.rmodel.armature[6:] = 1.\n",
- " \n",
- " def createProblem(self, x0, stepLength, timeStep, stepKnots, supportKnots):\n",
- " \"\"\" Create a shooting problem for a simple walking.\n",
- "\n",
- " :param x0: initial state\n",
- " :param stepLength: step lenght\n",
- " :param timeStep: step time for each knot\n",
- " :param stepKnots: number of knots for step phases\n",
- " :param supportKnots: number of knots for double support phases\n",
- " :return shooting problem\n",
- " \"\"\"\n",
- " # Getting the frame id for the right and left foot\n",
- " rightFootId = self.rmodel.getFrameId(self.rightFoot)\n",
- " leftFootId = self.rmodel.getFrameId(self.leftFoot)\n",
- "\n",
- " # Compute the current foot positions\n",
- " q0 = a2m(x0[:self.rmodel.nq])\n",
- " pinocchio.forwardKinematics(self.rmodel,self.rdata,q0)\n",
- " pinocchio.updateFramePlacements(self.rmodel,self.rdata)\n",
- " rightFootPos0 = self.rdata.oMf[rightFootId].translation\n",
- " leftFootPos0 = self.rdata.oMf[leftFootId].translation\n",
- "\n",
- " # Defining the action models along the time instances\n",
- " loco3dModel = []\n",
- " # Creating the action models for three steps\n",
- " firstStep = self.createFootstepModels(rightFootId, leftFootId,\n",
- " 0.5*stepLength, leftFootPos0,\n",
- " stepKnots)\n",
- " secondStep = self.createFootstepModels(leftFootId, rightFootId,\n",
- " stepLength, rightFootPos0,\n",
- " stepKnots)\n",
- " thirdStep = self.createFootstepModels(rightFootId, leftFootId,\n",
- " stepLength, leftFootPos0,\n",
- " stepKnots)\n",
- "\n",
- " # Creating the action model for the double support phase\n",
- " doubleSupport = \\\n",
- " [ self.createSwingFootModel(\n",
- " timeStep,\n",
- " [ rightFootId, leftFootId ]\n",
- " ) for k in range(supportKnots) ]\n",
- "\n",
- " # We defined the problem as:\n",
- " # STEP 1 - DS - STEP 2 - DS - STEP 3 - DS\n",
- " loco3dModel += firstStep + doubleSupport\n",
- " loco3dModel += secondStep + doubleSupport\n",
- " loco3dModel += thirdStep + doubleSupport\n",
- "\n",
- " problem = ShootingProblem(x0, loco3dModel, loco3dModel[-1])\n",
- " return problem\n",
- "\n",
- " def createFootstepModels(self, supportFootId, swingFootId, stepLength, footPos0, numKnots):\n",
- " \"\"\" Action models for a footstep phase.\n",
- "\n",
- " :param supportFootId: Id of the supporting foot\n",
- " :param swingFootId: Id of the swinging foot\n",
- " :param stepLength: step length\n",
- " :param footPos0: initial position of the swinging foot\n",
- " :param numKnots: number of knots for the footstep phase\n",
- " :return footstep action models\n",
- " \"\"\"\n",
- " # Action models for the foot swing\n",
- " footSwingModel = \\\n",
- " [ self.createSwingFootModel(\n",
- " timeStep,\n",
- " [ supportFootId ],\n",
- " TaskSE3(\n",
- " pinocchio.SE3(np.eye(3),\n",
- " np.asmatrix(a2m([ [(stepLength*k)/numKnots, 0., 0.] ]) +\n",
- " footPos0)),\n",
- " swingFootId)\n",
- " ) for k in range(numKnots) ]\n",
- " # Action model for the foot switch\n",
- " footSwitchModel = \\\n",
- " self.createFootSwitchModel(\n",
- " [ supportFootId ],\n",
- " TaskSE3(\n",
- " pinocchio.SE3(np.eye(3),\n",
- " np.asmatrix(a2m([ stepLength, 0., 0. ]) +\n",
- " footPos0)),\n",
- " swingFootId)\n",
- " )\n",
- " # Updating the current foot position for next step\n",
- " footPos0 += np.asmatrix(a2m([ stepLength, 0., 0. ]))\n",
- " return footSwingModel + [ footSwitchModel ]\n",
- "\n",
- " def createSwingFootModel(self, timeStep, supportFootIds, swingFootTask = None):\n",
- " \"\"\" Action model for a swing foot phase.\n",
- "\n",
- " :param timeStep: step duration of the action model\n",
- " :param supportFootIds: Ids of the constrained feet\n",
- " :param swingFootTask: swinging foot task\n",
- " :return action model for a swing foot phase\n",
- " \"\"\"\n",
- " # Creating the action model for floating-base systems. A walker system \n",
- " # is by default a floating-base system\n",
- " actModel = ActuationModelFreeFloating(self.rmodel)\n",
- "\n",
- " # Creating a 6D multi-contact model, and then including the supporting\n",
- " # foot\n",
- " contactModel = ContactModelMultiple(self.rmodel)\n",
- " for i in supportFootIds:\n",
- " supportContactModel = \\\n",
- " ContactModel6D(self.rmodel, i, ref=pinocchio.SE3.Identity(), gains=[0.,0.])\n",
- " contactModel.addContact('contact_'+str(i), supportContactModel)\n",
- "\n",
- " # Creating the cost model for a contact phase\n",
- " costModel = CostModelSum(self.rmodel, actModel.nu)\n",
- " if swingFootTask != None:\n",
- " footTrack = CostModelFramePlacement(self.rmodel,\n",
- " swingFootTask.frameId,\n",
- " swingFootTask.oXf,\n",
- " actModel.nu)\n",
- " costModel.addCost(\"footTrack\", footTrack, 100.)\n",
- "\n",
- " stateWeights = \\\n",
- " np.array([0]*6 + [0.01]*(self.rmodel.nv-6) + [10]*self.rmodel.nv)\n",
- " stateReg = CostModelState(self.rmodel,\n",
- " self.state,\n",
- " self.rmodel.defaultState,\n",
- " actModel.nu,\n",
- " activation=ActivationModelWeightedQuad(stateWeights**2))\n",
- " ctrlReg = CostModelControl(self.rmodel, actModel.nu)\n",
- " costModel.addCost(\"stateReg\", stateReg, 0.1)\n",
- " costModel.addCost(\"ctrlReg\", ctrlReg, 0.001)\n",
- "\n",
- " # Creating the action model for the KKT dynamics with simpletic Euler\n",
- " # integration scheme\n",
- " dmodel = \\\n",
- " DifferentialActionModelFloatingInContact(self.rmodel,\n",
- " actModel,\n",
- " contactModel,\n",
- " costModel)\n",
- " model = IntegratedActionModelEuler(dmodel)\n",
- " model.timeStep = timeStep\n",
- " return model\n",
- "\n",
- " def createFootSwitchModel(self, supportFootId, swingFootTask):\n",
- " \"\"\" Action model for a foot switch phase.\n",
- "\n",
- " :param timeStep: step duration of the action model\n",
- " :param supportFootIds: Ids of the constrained feet\n",
- " :param swingFootTask: swinging foot task\n",
- " :return action model for a foot switch phase\n",
- " \"\"\"\n",
- " model = self.createSwingFootModel(0., supportFootId, swingFootTask)\n",
- "\n",
- " impactFootVelCost = \\\n",
- " CostModelFrameVelocity(self.rmodel, swingFootTask.frameId)\n",
- " model.differential.costs.addCost('impactVel', impactFootVelCost, 10000.)\n",
- " model.differential.costs['impactVel' ].weight = 100000\n",
- " model.differential.costs['footTrack' ].weight = 100000\n",
- " model.differential.costs['stateReg'].weight = 10\n",
- " model.differential.costs['ctrlReg'].weight = 0.001\n",
- " return model"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "This class builds action models for each locomotion phase:\n",
- " - createSwingFootModel: defines an action model for the swing phase\n",
- " - createFootSwitchModel: defines an action model for switch knots between phases\n",
- " \n",
- "Then we build a walking by combining a set of contact phases and their contact switches. This is defined by createFootstepModel\n",
- "\n",
- "Now let's create a walking OC problem for the Talos legs."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# Creating the lower-body part of Talos\n",
- "talos_legs = loadTalosLegs()\n",
- "\n",
- "# Setting up the 3d walking problem\n",
- "rightFoot = 'right_sole_link'\n",
- "leftFoot = 'left_sole_link'\n",
- "walk = SimpleBipedWalkingProblem(talos_legs.model, rightFoot, leftFoot)\n",
- "\n",
- "\n",
- "# Create the initial state\n",
- "q0 = talos_legs.q0.copy()\n",
- "v0 = zero(talos_legs.model.nv)\n",
- "x0 = m2a(np.concatenate([q0,v0]))\n",
- "\n",
- "\n",
- "# Creating the walking problem\n",
- "stepLength = 0.6 # meters\n",
- "timeStep = 0.0375 # seconds\n",
- "stepKnots = 20\n",
- "supportKnots = 10\n",
- "walkProblem = walk.createProblem(x0, stepLength, timeStep, stepKnots, supportKnots)\n",
- "\n",
- "\n",
- "\n",
- "# Solving the 3d walking problem using DDP\n",
- "ddp = SolverDDP(walkProblem)\n",
- "cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]\n",
- "ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose(),\n",
- " CallbackSolverDisplay(talos_legs,4,1,cameraTF)]\n",
- "ddp.th_stop = 1e-9\n",
- "ddp.solve(maxiter=1000,regInit=.1,init_xs=[talos_legs.model.defaultState]*len(ddp.models()))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "With the following commands we can plot \n",
- " - the state and control trajectories, and\n",
- " - the DDP performance"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "%matplotlib inline\n",
- "from crocoddyl import plotOCSolution, plotDDPConvergence\n",
- "\n",
- "# Plotting the solution and the DDP convergence\n",
- "log = ddp.callback[0]\n",
- "plotOCSolution(log.xs, log.us)\n",
- "plotDDPConvergence(log.costs,log.control_regs,\n",
- " log.state_regs,log.gm_stops,\n",
- " log.th_stops,log.steps)\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "Finally we can visualize the solution using gepetto-viewer."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# Visualization of the DDP solution in gepetto-viewer\n",
- "ddp.callback[2](ddp)\n",
- "CallbackSolverDisplay(talos_legs)(ddp)"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## VI. Understanding the walking problem\n",
- "\n",
- "In this problem we pre-defined a 20 and 10 knots for the step and double support phases with $dt=$3.75e-2, repectively. \n",
- "\n",
- " 1. Could you tell us how much is the foot step and double support duration?\n",
- " 2. What happens when do we change $dt$ (e.g. 2e-2 secs)?\n",
- " 3. What happens when do we change the number of step knots (e.g. 10)?\n"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 2",
- "language": "python",
- "name": "python2"
- },
- "language_info": {
- "codemirror_mode": {
- "name": "ipython",
- "version": 2
- },
- "file_extension": ".py",
- "mimetype": "text/x-python",
- "name": "python",
- "nbconvert_exporter": "python",
- "pygments_lexer": "ipython2",
- "version": "2.7.12"
- }
- },
- "nbformat": 4,
- "nbformat_minor": 2
-}
diff --git a/examples/notebooks/cartpole_swing_up.ipynb b/examples/notebooks/cartpole_swing_up.ipynb
index a18845ffa8fc4f03faf8b1a37500f96111991354..832be40234092de4fb58933b51d225f21aa6c524 100644
--- a/examples/notebooks/cartpole_swing_up.ipynb
+++ b/examples/notebooks/cartpole_swing_up.ipynb
@@ -66,7 +66,7 @@
" def calc(self, data, x, u=None):\n",
" if u is None: u=model.unone\n",
" # Getting the state and control variables\n",
- " x, th, xdot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
+ " y, th, ydot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
" f = np.asscalar(u[0])\n",
"\n",
" # Shortname for system parameters\n",
@@ -85,7 +85,7 @@
" data.xout = np.matrix([ xddot, thddot ]).T\n",
" \n",
" # Computing the cost residual and value\n",
- " data.r = np.matrix(self.costWeights * np.array([ s, 1-c, x, xdot, thdot, f ])).T\n",
+ " data.r = np.matrix(self.costWeights * np.array([ s, 1-c, y, ydot, thdot, f ])).T\n",
" data.cost = .5* np.asscalar(sum(np.asarray(data.r)**2))\n",
"\n",
" def calcDiff(model,data,x,u=None,recalc=True):\n",
@@ -126,7 +126,7 @@
"cartpoleData = cartpoleDAM.createData()\n",
"x = cartpoleDAM.State.rand()\n",
"u = np.zeros(1)\n",
- "cartpoleDAM.calc(cartpoleData,x,u)"
+ "cartpoleDAM.calc(cartpoleData, x, u)"
]
},
{
diff --git a/examples/notebooks/cartpole_swing_up_sol.ipynb b/examples/notebooks/cartpole_swing_up_sol.ipynb
index 37a8c7c84cb03e0d0f2a2485e965dac8803be3da..aff3ed461015fc379e84754b387930fc58c8b690 100644
--- a/examples/notebooks/cartpole_swing_up_sol.ipynb
+++ b/examples/notebooks/cartpole_swing_up_sol.ipynb
@@ -78,7 +78,7 @@
" def calc(self, data, x, u=None):\n",
" if u is None: u=model.unone\n",
" # Getting the state and control variables\n",
- " x, th, xdot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
+ " y, th, ydot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
" f = np.asscalar(u[0])\n",
"\n",
" # Shortname for system parameters\n",
@@ -93,7 +93,7 @@
" data.xout = np.matrix([ xddot,thddot ]).T\n",
"\n",
" # Computing the cost residual and value\n",
- " data.r = np.matrix(self.costWeights * np.array([ s, 1-c, x, xdot, thdot, f ])).T\n",
+ " data.r = np.matrix(self.costWeights * np.array([ s, 1-c, y, ydot, thdot, f ])).T\n",
" data.cost = .5* np.asscalar(sum(np.asarray(data.r)**2))\n",
"\n",
" def calcDiff(self,data,x,u=None,recalc=True):\n",
@@ -118,7 +118,7 @@
"cartpoleData = cartpoleDAM.createData()\n",
"x = cartpoleDAM.state.rand()\n",
"u = np.zeros(1)\n",
- "cartpoleDAM.calc(cartpoleData,x,u)"
+ "cartpoleDAM.calc(cartpoleData, x, u)"
]
},
{
diff --git a/examples/notebooks/introduction_to_crocoddyl.ipynb b/examples/notebooks/introduction_to_crocoddyl.ipynb
index deea9e4b56035b1642be7f87a7be089cef7369e5..1f8069bef6ee645c891cbf9b95ca4f1db9c4a171 100644
--- a/examples/notebooks/introduction_to_crocoddyl.ipynb
+++ b/examples/notebooks/introduction_to_crocoddyl.ipynb
@@ -149,18 +149,20 @@
},
{
"cell_type": "code",
- "execution_count": 1,
+ "execution_count": null,
"metadata": {},
"outputs": [],
"source": [
- "from crocoddyl import *\n",
+ "import crocoddyl\n",
"import numpy as np\n",
+ "import example_robot_data\n",
"\n",
- "talos_arm = loadTalosArm()\n",
+ "talos_arm = example_robot_data.loadTalosArm()\n",
+ "robot_model = talos_arm.model # getting the Pinocchio model\n",
"\n",
"# Defining a initial state\n",
- "q0 = [0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]\n",
- "x0 = np.hstack([q0, np.zeros(talos_arm.model.nv)])"
+ "q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T\n",
+ "x0 = np.vstack([q0, np.matrix(np.zeros(talos_arm.model.nv)).T])"
]
},
{
@@ -186,93 +188,68 @@
"metadata": {},
"outputs": [],
"source": [
- "import numpy as np\n",
"import pinocchio\n",
"\n",
"\n",
- "class DifferentialActionModelABA:\n",
- " def __init__(self,pinocchioModel):\n",
- " # The forward dynamics and its derivatives are computed with Pinocchio\n",
- " self.pinocchio = pinocchioModel\n",
- " self.nq,self.nv = self.pinocchio.nq, self.pinocchio.nv\n",
- " \n",
- " # Describes integrate, difference, Jacobian integrate and Jacobian difference\n",
- " # for any Pinocchio model\n",
- " self.State = StatePinocchio(self.pinocchio)\n",
- " \n",
- " # Keeps a stack of cost functions\n",
- " self.costs = CostModelSum(self.pinocchio)\n",
- " \n",
- " # Dimension of the state, and its tangent space, and control\n",
- " self.nx = self.State.nx\n",
- " self.ndx = self.State.ndx\n",
- " self.nout = self.nv\n",
- " self.nu = self.nv\n",
- " self.unone = np.zeros(self.nu)\n",
- " @property\n",
- " def ncost(self): return self.costs.ncost\n",
- " def createData(self): return DifferentialActionDataABA(self) # create the data needed for this model\n",
- " def calc(model,data,x,u=None):\n",
- " if u is None: u=model.unone\n",
- " nx,nu,nq,nv,nout = model.nx,model.nu,model.nq,model.nv,model.nout\n",
- " q = a2m(x[:nq]) # from np array to matrix\n",
- " v = a2m(x[-nv:]) # from np array to matrix\n",
- " tauq = a2m(u) # from np array to matrix\n",
- " \n",
- " # Computes the next state through ABA\n",
- " data.xout[:] = pinocchio.aba(model.pinocchio,data.pinocchio,q,v,tauq).flat\n",
- " \n",
- " # Updates the kinematics needed for cost computation\n",
- " pinocchio.forwardKinematics(model.pinocchio,data.pinocchio,q,v)\n",
- " pinocchio.updateFramePlacements(model.pinocchio,data.pinocchio)\n",
- " \n",
- " # Computes the cost from a set of single cost functions\n",
- " data.cost = model.costs.calc(data.costs,x,u)\n",
- " return data.xout,data.cost\n",
- "\n",
- " def calcDiff(model,data,x,u=None,recalc=True):\n",
- " if u is None: u=model.unone\n",
- " if recalc: xout,cost = model.calc(data,x,u)\n",
- " nx,ndx,nu,nq,nv,nout = model.nx,model.State.ndx,model.nu,model.nq,model.nv,model.nout\n",
- " q = a2m(x[:nq]) # from np array to matrix\n",
- " v = a2m(x[-nv:]) # from np array to matrix\n",
- " tauq = a2m(u) # from np array to matrix\n",
- " \n",
- " # Computes the ABA derivatives (dynamics), and keeps them inside data\n",
- " pinocchio.computeABADerivatives(model.pinocchio,data.pinocchio,q,v,tauq)\n",
- " data.Fx[:,:nv] = data.pinocchio.ddq_dq\n",
- " data.Fx[:,nv:] = data.pinocchio.ddq_dv\n",
- " data.Fu[:,:] = data.Minv\n",
- "\n",
- " # Updates the kinematics Jacobians needed for getting the derivatives of the cost function\n",
- " pinocchio.computeJointJacobians(model.pinocchio,data.pinocchio,q)\n",
- " pinocchio.updateFramePlacements(model.pinocchio,data.pinocchio)\n",
- " \n",
- " # Computes all derivatives of cost function\n",
- " model.costs.calcDiff(data.costs,x,u,recalc=False)\n",
- " return data.xout,data.cost\n",
- "\n",
- "\n",
- "class DifferentialActionDataABA:\n",
- " def __init__(self,model):\n",
- " self.pinocchio = model.pinocchio.createData()\n",
- " self.costs = model.costs.createData(self.pinocchio)\n",
- " self.cost = np.nan\n",
- " self.xout = np.zeros(model.nout)\n",
- " nx,nu,ndx,nq,nv,nout = model.nx,model.nu,model.State.ndx,model.nq,model.nv,model.nout\n",
- " self.F = np.zeros([ nout,ndx+nu ])\n",
- " self.costResiduals = self.costs.residuals\n",
- " self.Fx = self.F[:,:ndx]\n",
- " self.Fu = self.F[:,-nu:]\n",
- " self.g = self.costs.g\n",
- " self.L = self.costs.L\n",
- " self.Lx = self.costs.Lx\n",
- " self.Lu = self.costs.Lu\n",
- " self.Lxx = self.costs.Lxx\n",
- " self.Lxu = self.costs.Lxu\n",
- " self.Luu = self.costs.Luu\n",
- " self.Rx = self.costs.Rx\n",
- " self.Ru = self.costs.Ru"
+ "class DifferentialFwdDynamics(crocoddyl.DifferentialActionModelAbstract):\n",
+ " def __init__(self, state, costModel):\n",
+ " crocoddyl.DifferentialActionModelAbstract.__init__(self, state, state.nv, costModel.nr)\n",
+ " self.costs = costModel\n",
+ " self.enable_force = True\n",
+ " self.armature = np.matrix(np.zeros(0))\n",
+ "\n",
+ " def calc(self, data, x, u=None):\n",
+ " if u is None:\n",
+ " u = self.unone\n",
+ " q, v = x[:self.state.nq], x[-self.state.nv:]\n",
+ " # Computing the dynamics using ABA or manually for armature case\n",
+ " if self.enable_force:\n",
+ " data.xout = pinocchio.aba(self.state.pinocchio, data.pinocchio, q, v, u)\n",
+ " else:\n",
+ " pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)\n",
+ " data.M = data.pinocchio.M\n",
+ " if self.armature.size == self.state.nv:\n",
+ " data.M[range(self.state.nv), range(self.state.nv)] += self.armature\n",
+ " data.Minv = np.linalg.inv(data.M)\n",
+ " data.xout = data.Minv * (u - data.pinocchio.nle)\n",
+ " # Computing the cost value and residuals\n",
+ " pinocchio.forwardKinematics(self.state.pinocchio, data.pinocchio, q, v)\n",
+ " pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)\n",
+ " self.costs.calc(data.costs, x, u)\n",
+ " data.cost = data.costs.cost\n",
+ "\n",
+ " def calcDiff(self, data, x, u=None, recalc=True):\n",
+ " q, v = x[:self.state.nq], x[-self.state.nv:]\n",
+ " if u is None:\n",
+ " u = self.unone\n",
+ " if recalc:\n",
+ " self.calc(data, x, u)\n",
+ " pinocchio.computeJointJacobians(self.state.pinocchio, data.pinocchio, q)\n",
+ " # Computing the dynamics derivatives\n",
+ " if self.enable_force:\n",
+ " pinocchio.computeABADerivatives(self.state.pinocchio, data.pinocchio, q, v, u)\n",
+ " data.Fx = np.hstack([data.pinocchio.ddq_dq, data.pinocchio.ddq_dv])\n",
+ " data.Fu = data.pinocchio.Minv\n",
+ " else:\n",
+ " pinocchio.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout)\n",
+ " data.Fx = -np.hstack([data.Minv * data.pinocchio.dtau_dq, data.Minv * data.pinocchio.dtau_dv])\n",
+ " data.Fu = data.Minv\n",
+ " # Computing the cost derivatives\n",
+ " self.costs.calcDiff(data.costs, x, u, False)\n",
+ "\n",
+ " def set_armature(self, armature):\n",
+ " if armature.size is not self.state.nv:\n",
+ " print('The armature dimension is wrong, we cannot set it.')\n",
+ " else:\n",
+ " self.enable_force = False\n",
+ " self.armature = armature.T\n",
+ "\n",
+ " def createData(self):\n",
+ " data = crocoddyl.DifferentialActionModelAbstract.createData(self)\n",
+ " data.pinocchio = pinocchio.Data(self.state.pinocchio)\n",
+ " data.costs = self.costs.createData(data.pinocchio)\n",
+ " data.costs.shareMemory(data) # this allows us to share the memory of cost-terms of action model\n",
+ " return data"
]
},
{
@@ -308,39 +285,21 @@
"\n",
"## III.a ABA dynamics for reaching a goal with Talos arm\n",
"\n",
- "Our optimal control solver interacts with a defined ShootingProblem. A shooting problem represents a stack of action models in which an action model defins a specific knot along the OC problem.\n",
+ "Our optimal control solver interacts with a defined ShootingProblem. A shooting problem represents a stack of action models in which an action model defines a specific node along the OC problem.\n",
"\n",
- "First we need to create an action model from DifferentialActionModelABA. We use it for building terminal and running action models. In this example, we employ an simpletic Euler integration rule as follows:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# Running and terminal action models\n",
- "runningModel = IntegratedActionModelEuler(DifferentialActionModelABA(talos_arm.model))\n",
- "terminalModel = IntegratedActionModelEuler(DifferentialActionModelABA(talos_arm.model))\n",
+ "First we need to create an action model from DifferentialFwdDynamics. We use it for building terminal and running action models. In this example, we employ an simpletic Euler integration rule.\n",
"\n",
- "# Defining the time duration for running action models and the terminal one\n",
- "dt = 1e-3\n",
- "runningModel.timeStep = dt"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "Next we define the set of cost functions for this problem. For this particular example, we formulate three running-cost functions: \n",
- " - goal-tracking cost, $log(^fXd_o \\,^oX_f)$\n",
- " \n",
- " - state and control regularization; and $\\|\\mathbf{x}-\\mathbf{x}_{ref}\\|, \\|\\mathbf{u}\\|$\n",
+ "Next we define the set of cost functions for this problem. For this particular example, we formulate three running-cost functions:\n",
+ "\n",
+ " goal-tracking cost, ð‘™ð‘œð‘”(ð‘“ð‘‹ð‘‘ð‘œð‘œð‘‹ð‘“)\n",
+ "\n",
+ "state and control regularization; and ‖ð±âˆ’ð±ð‘Ÿð‘’ð‘“‖,‖ð®â€–\n",
"\n",
"one terminal-cost:\n",
- " - goal cost. $\\|\\mathbf{u}_T\\|$\n",
- " \n",
- " First, let's create the common cost functions."
+ "\n",
+ " goal cost. ‖ð®ð‘‡â€–\n",
+ "\n",
+ "First, let's create the common cost functions."
]
},
{
@@ -349,54 +308,32 @@
"metadata": {},
"outputs": [],
"source": [
- "from pinocchio.utils import *\n",
- "\n",
- "# Goal tracking cost\n",
- "frameName = 'gripper_left_joint' #gripper_left_fingertip_2_link'\n",
- "state = StatePinocchio(talos_arm.model)\n",
- "SE3ref = pinocchio.SE3(np.eye(3), np.array([ [.0],[.0],[.4] ]))\n",
- "goalTrackingCost = CostModelFramePlacement(talos_arm.model,\n",
- " nu=talos_arm.model.nv,\n",
- " frame=talos_arm.model.getFrameId(frameName),\n",
- " ref=SE3ref)\n",
- "\n",
- "# State and control regularization\n",
- "xRegCost = CostModelState(talos_arm.model,\n",
- " state,\n",
- " ref=state.zero(),\n",
- " nu=talos_arm.model.nv)\n",
- "uRegCost = CostModelControl(talos_arm.model,nu=talos_arm.model.nv)\n",
- "\n",
- "# Adds the running and terminal cost functions\n",
- "runningCostModel = runningModel.differential.costs\n",
- "runningCostModel.addCost( name=\"pos\", weight = 1e-3, cost = goalTrackingCost)\n",
- "runningCostModel.addCost( name=\"regx\", weight = 1e-7, cost = xRegCost) \n",
- "runningCostModel.addCost( name=\"regu\", weight = 1e-7, cost = uRegCost)\n",
- "terminalCostModel = terminalModel.differential.costs\n",
- "terminalCostModel.addCost( name=\"pos\", weight = 1, cost = goalTrackingCost)\n",
- "\n",
- "\n",
- "# Let's compute the cost and its derivatives\n",
- "robot_data = talos_arm.model.createData() # Pinocchio data\n",
- "data = goalTrackingCost.createData(robot_data)\n",
- "\n",
- "# Update kinematics\n",
- "q = pinocchio.randomConfiguration(talos_arm.model)\n",
- "v = rand(talos_arm.model.nv)\n",
- "x = m2a(np.concatenate([q,v]))\n",
- "u = m2a(rand(talos_arm.model.nv))\n",
- "pinocchio.forwardKinematics(talos_arm.model,robot_data,q,v)\n",
- "pinocchio.computeJointJacobians(talos_arm.model,robot_data,q)\n",
- "pinocchio.updateFramePlacements(talos_arm.model,robot_data)\n",
- "\n",
- "print 'cost =', goalTrackingCost.calc(data, x, u)\n",
- "print 'cost =', goalTrackingCost.calcDiff(data, x, u)\n",
- "print\n",
- "print 'lx =', data.Lx\n",
- "print 'lu =', data.Lu\n",
- "print\n",
- "print 'lxx =', data.Lxx\n",
- "print 'luu =', data.Luu"
+ "# Create the cost functions\n",
+ "target = np.array([0.4, 0., .4])\n",
+ "Mref = crocoddyl.FrameTranslation(robot_model.getFrameId(\"gripper_left_joint\"), np.matrix(target).T)\n",
+ "state = crocoddyl.StateMultibody(robot_model)\n",
+ "goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref)\n",
+ "xRegCost = crocoddyl.CostModelState(state)\n",
+ "uRegCost = crocoddyl.CostModelControl(state)\n",
+ "\n",
+ "# Create cost model per each action model\n",
+ "runningCostModel = crocoddyl.CostModelSum(state)\n",
+ "terminalCostModel = crocoddyl.CostModelSum(state)\n",
+ "\n",
+ "# Then let's added the running and terminal cost functions\n",
+ "runningCostModel.addCost(\"gripperPose\", goalTrackingCost, 1.)\n",
+ "runningCostModel.addCost(\"stateReg\", xRegCost, 1e-4)\n",
+ "runningCostModel.addCost(\"ctrlReg\", uRegCost, 1e-7)\n",
+ "terminalCostModel.addCost(\"gripperPose\", goalTrackingCost, 1000.)\n",
+ "terminalCostModel.addCost(\"stateReg\", xRegCost, 1e-4)\n",
+ "terminalCostModel.addCost(\"ctrlReg\", uRegCost, 1e-7)\n",
+ "\n",
+ "# Running and terminal action models\n",
+ "DT = 1e-3\n",
+ "runningModel = crocoddyl.IntegratedActionModelEuler(\n",
+ " crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel), DT)\n",
+ "terminalModel = crocoddyl.IntegratedActionModelEuler(\n",
+ " crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel))"
]
},
{
@@ -415,7 +352,7 @@
"# For this optimal control problem, we define 250 knots (or running action\n",
"# models) plus a terminal knot\n",
"T = 250\n",
- "problem = ShootingProblem(x0, [ runningModel ]*T, terminalModel)"
+ "problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)"
]
},
{
@@ -427,7 +364,7 @@
"Please note that:\n",
"- CallbackDDPLogger: store the solution information.\n",
"- CallbackDDPVerbose(level): printing message during the iterates.\n",
- "- CallbackSolverDisplay(robot,rate): display the state trajectory using Gepetto viewer."
+ "- CallbackDisplay(robot,rate): display the state trajectory using Gepetto viewer."
]
},
{
@@ -437,18 +374,20 @@
"outputs": [],
"source": [
"# Creating the DDP solver for this OC problem, defining a logger\n",
- "ddp = SolverDDP(problem)\n",
+ "ddp = crocoddyl.SolverDDP(problem)\n",
"cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]\n",
- "ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose(1), CallbackSolverDisplay(talos_arm,4,1,cameraTF)]\n",
+ "log = crocoddyl.CallbackLogger()\n",
+ "ddp.setCallbacks([log,\n",
+ " crocoddyl.CallbackVerbose(),\n",
+ " crocoddyl.CallbackDisplay(talos_arm,4,4,cameraTF)])\n",
"\n",
"# Solving it with the DDP algorithm\n",
"ddp.solve()\n",
"\n",
"# Printing the reached position\n",
- "log = ddp.callback[0]\n",
- "frame_idx = talos_arm.model.getFrameId(frameName)\n",
- "xT = log.xs[-1]\n",
- "qT = np.asmatrix(xT[:talos_arm.model.nq]).T\n",
+ "frame_idx = talos_arm.model.getFrameId(\"gripper_left_joint\")\n",
+ "xT = ddp.xs[-1]\n",
+ "qT = xT[:talos_arm.model.nq]\n",
"print\n",
"print \"The reached pose by the wrist is\"\n",
"print talos_arm.framePlacement(qT, frame_idx)"
@@ -468,15 +407,12 @@
"outputs": [],
"source": [
"%matplotlib inline\n",
- "# Plotting the solution and the DDP convergence\n",
- "log = ddp.callback[0]\n",
- "plotOCSolution(log.xs, log.us)\n",
- "plotDDPConvergence(log.costs,log.control_regs,\n",
- " log.state_regs,log.gm_stops,\n",
- " log.th_stops,log.steps)\n",
+ "# # Plotting the solution and the DDP convergence\n",
+ "crocoddyl.plotOCSolution(log.xs, log.us)\n",
+ "crocoddyl.plotConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps)\n",
"\n",
"# Visualizing the solution in gepetto-viewer\n",
- "CallbackSolverDisplay(talos_arm)(ddp)"
+ "crocoddyl.CallbackDisplay(talos_arm)(ddp)"
]
},
{
diff --git a/examples/notebooks/solutions/cartpole_dyn.py b/examples/notebooks/solutions/cartpole_dyn.py
index 390a3ef87bdcda0a9580ee979294d795a002d969..863bd3dc234415b56b432193ed15a260841e5ea1 100644
--- a/examples/notebooks/solutions/cartpole_dyn.py
+++ b/examples/notebooks/solutions/cartpole_dyn.py
@@ -2,7 +2,7 @@
# xddot, thddot = cartpole_dynamics(self, data, x, u)
def cartpole_dynamics(model, data, x, u):
# Getting the state and control variables
- x, th, xdot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])
+ th, thdot = np.asscalar(x[1]), np.asscalar(x[3])
f = np.asscalar(u[0])
# Shortname for system parameters
diff --git a/examples/notebooks/unicycle_towards_origin.py b/examples/notebooks/unicycle_towards_origin.py
index 8c8006386fb24183d572dc95b52eada10e17d6cd..2a41fb6ff222bcd08e13fa5dd8547fdb983c3598 100644
--- a/examples/notebooks/unicycle_towards_origin.py
+++ b/examples/notebooks/unicycle_towards_origin.py
@@ -1,31 +1,31 @@
import numpy as np
-from crocoddyl import *
+import crocoddyl
from unicycle_utils import plotUnicycleSolution
# Creating an action model for the unicycle system
-model = ActionModelUnicycle()
+model = crocoddyl.ActionModelUnicycle()
# Setting up the cost weights
-model.costWeights = [
+model.r = [
10., # state weight
1. # control weight
]
# Formulating the optimal control problem
T = 20 # number of knots
-x0 = np.array([-1., -1., 1.]) #x,y,theta
-problem = ShootingProblem(x0, [model] * T, model)
+x0 = np.matrix([-1., -1., 1.]).T #x,y,theta
+problem = crocoddyl.ShootingProblem(x0, [model] * T, model)
# Creating the DDP solver for this OC problem, defining a logger
-ddp = SolverDDP(problem)
-ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()]
+ddp = crocoddyl.SolverDDP(problem)
+ddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
ddp.solve()
-# Plotting the solution, solver convergence and unycicle motion
-log = ddp.callback[0]
-plotOCSolution(log.xs, log.us)
-plotDDPConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps)
+# Plotting the solution, solver convergence and unicycle motion
+log = ddp.getCallbacks()[0]
+crocoddyl.plotOCSolution(log.xs, log.us)
+crocoddyl.plotConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps)
plotUnicycleSolution(log.xs)
diff --git a/examples/quadrupedal_walking_balancing.py b/examples/quadrupedal_gaits.py
similarity index 63%
rename from examples/quadrupedal_walking_balancing.py
rename to examples/quadrupedal_gaits.py
index 0274bd02634b02003f27849212a839fa52b02acf..9fe1d10054cccec11e16f6e534a25490b35a35de 100644
--- a/examples/quadrupedal_walking_balancing.py
+++ b/examples/quadrupedal_gaits.py
@@ -1,3 +1,4 @@
+import os
import sys
import numpy as np
@@ -7,8 +8,8 @@ import example_robot_data
import pinocchio
from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
-WITHDISPLAY = 'display' in sys.argv
-WITHPLOT = 'plot' in sys.argv
+WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
+WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# Loading the HyQ model
hyq = example_robot_data.loadHyQ()
@@ -28,11 +29,11 @@ gait = SimpleQuadrupedalGaitProblem(hyq.model, lfFoot, rfFoot, lhFoot, rhFoot)
# Setting up all tasks
GAITPHASES = [{
'walking': {
- 'stepLength': 0.15,
- 'stepHeight': 0.2,
+ 'stepLength': 0.25,
+ 'stepHeight': 0.25,
'timeStep': 1e-2,
'stepKnots': 25,
- 'supportKnots': 5
+ 'supportKnots': 2
}
}, {
'trotting': {
@@ -40,7 +41,7 @@ GAITPHASES = [{
'stepHeight': 0.2,
'timeStep': 1e-2,
'stepKnots': 25,
- 'supportKnots': 5
+ 'supportKnots': 2
}
}, {
'pacing': {
@@ -53,15 +54,18 @@ GAITPHASES = [{
}, {
'bounding': {
'stepLength': 0.15,
- 'stepHeight': 0.2,
+ 'stepHeight': 0.1,
'timeStep': 1e-2,
'stepKnots': 25,
'supportKnots': 5
}
}, {
'jumping': {
- 'jumpHeight': 0.5,
- 'timeStep': 1e-2
+ 'jumpHeight': 0.15,
+ 'jumpLength': [0.0, 0.3, 0.],
+ 'timeStep': 1e-2,
+ 'groundKnots': 10,
+ 'flyingKnots': 20
}
}]
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
@@ -71,37 +75,48 @@ for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
- ddp[i] = crocoddyl.SolverDDP(
+ ddp[i] = crocoddyl.SolverFDDP(
gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
elif key == 'trotting':
# Creating a trotting problem
- ddp[i] = crocoddyl.SolverDDP(
+ ddp[i] = crocoddyl.SolverFDDP(
gait.createTrottingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
elif key == 'pacing':
# Creating a pacing problem
- ddp[i] = crocoddyl.SolverDDP(
+ ddp[i] = crocoddyl.SolverFDDP(
gait.createPacingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
elif key == 'bounding':
# Creating a bounding problem
- ddp[i] = crocoddyl.SolverDDP(
+ ddp[i] = crocoddyl.SolverFDDP(
gait.createBoundingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
elif key == 'jumping':
# Creating a jumping problem
- ddp[i] = crocoddyl.SolverDDP(gait.createJumpingProblem(x0, value['jumpHeight'], value['timeStep']))
+ ddp[i] = crocoddyl.SolverFDDP(
+ gait.createJumpingProblem(x0, value['jumpHeight'], value['jumpLength'], value['timeStep'],
+ value['groundKnots'], value['flyingKnots']))
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
- if WITHDISPLAY:
- ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(hyq, 4, 4, cameraTF)])
+ if WITHDISPLAY and WITHPLOT:
+ ddp[i].setCallbacks(
+ [crocoddyl.CallbackLogger(),
+ crocoddyl.CallbackVerbose(),
+ crocoddyl.CallbackDisplay(hyq, 4, 4, cameraTF)])
+ elif WITHDISPLAY:
+ ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackVerbose()])
+ elif WITHPLOT:
+ ddp[i].setCallbacks([
+ crocoddyl.CallbackLogger(),
+ crocoddyl.CallbackVerbose(),
+ ])
else:
ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the DDP solver
- ddp[i].th_stop = 1e-9
xs = [hyq.model.defaultState] * len(ddp[i].models())
us = [m.quasicStatic(d, hyq.model.defaultState) for m, d in list(zip(ddp[i].models(), ddp[i].datas()))[:-1]]
ddp[i].solve(xs, us, 100, False, 0.1)
@@ -119,6 +134,20 @@ if WITHPLOT:
xs = []
us = []
for i, phase in enumerate(GAITPHASES):
- xs.extend(ddp[i].xs)
+ xs.extend(ddp[i].xs[:-1])
us.extend(ddp[i].us)
- plotSolution(hyq.model, xs, us)
+ log = ddp[0].getCallbacks()[0]
+ plotSolution(hyq.model, xs, us, figIndex=1, show=False)
+
+ for i, phase in enumerate(GAITPHASES):
+ title = phase.keys()[0] + " (phase " + str(i) + ")"
+ log = ddp[i].getCallbacks()[0]
+ crocoddyl.plotConvergence(log.costs,
+ log.control_regs,
+ log.state_regs,
+ log.gm_stops,
+ log.th_stops,
+ log.steps,
+ figTitle=title,
+ figIndex=i + 3,
+ show=True if i == len(GAITPHASES) - 1 else False)
diff --git a/include/crocoddyl/core/action-base.hpp b/include/crocoddyl/core/action-base.hpp
index 0180e8866d1b7204f521672a1afa31d77cbb052a..df24d2599227e1f3558a8ff2ec2516d40fc9ef41 100644
--- a/include/crocoddyl/core/action-base.hpp
+++ b/include/crocoddyl/core/action-base.hpp
@@ -20,7 +20,7 @@ struct ActionDataAbstract; // forward declaration
class ActionModelAbstract {
public:
- ActionModelAbstract(StateAbstract& state, unsigned int const& nu, unsigned int const& nr = 1);
+ ActionModelAbstract(StateAbstract& state, unsigned int const& nu, unsigned int const& nr = 0);
virtual ~ActionModelAbstract();
virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
@@ -41,10 +41,10 @@ class ActionModelAbstract {
StateAbstract& get_state() const;
protected:
- unsigned int nu_;
- unsigned int nr_;
- StateAbstract& state_;
- Eigen::VectorXd unone_;
+ unsigned int nu_; //!< Control dimension
+ unsigned int nr_; //!< Dimension of the cost residual
+ StateAbstract& state_; //!< Model of the state
+ Eigen::VectorXd unone_; //!< Neutral state
#ifdef PYTHON_BINDINGS
diff --git a/include/crocoddyl/core/actions/diff-lqr.hpp b/include/crocoddyl/core/actions/diff-lqr.hpp
index 1de5e0394a288c2fcf343f5e8879b901b8a8c94c..504d547fef7669a59c7819773be2425d7a765076 100644
--- a/include/crocoddyl/core/actions/diff-lqr.hpp
+++ b/include/crocoddyl/core/actions/diff-lqr.hpp
@@ -46,16 +46,13 @@ struct DifferentialActionDataLQR : public DifferentialActionDataAbstract {
template <typename Model>
explicit DifferentialActionDataLQR(Model* const model) : DifferentialActionDataAbstract(model) {
// Setting the linear model and quadratic cost here because they are constant
- const unsigned int& nq = model->get_state().get_nq();
- const unsigned int& nv = model->get_state().get_nv();
- Fx.leftCols(nq) = model->Fq_;
- Fx.rightCols(nv) = model->Fv_;
+ Fx.leftCols(model->get_state().get_nq()) = model->Fq_;
+ Fx.rightCols(model->get_state().get_nv()) = model->Fv_;
Fu = model->Fu_;
Lxx = model->Lxx_;
Luu = model->Luu_;
Lxu = model->Lxu_;
}
- ~DifferentialActionDataLQR() {}
};
} // namespace crocoddyl
diff --git a/include/crocoddyl/core/actions/lqr.hpp b/include/crocoddyl/core/actions/lqr.hpp
index d2908b0e43b24c6c42b79c2918c209f3c1654899..bc795ba6ea28771fc76fc495451600e99658587e 100644
--- a/include/crocoddyl/core/actions/lqr.hpp
+++ b/include/crocoddyl/core/actions/lqr.hpp
@@ -48,7 +48,6 @@ struct ActionDataLQR : public ActionDataAbstract {
Luu = model->Luu_;
Lxu = model->Lxu_;
}
- ~ActionDataLQR() {}
};
} // namespace crocoddyl
diff --git a/include/crocoddyl/core/actuation-base.hpp b/include/crocoddyl/core/actuation-base.hpp
index 3e79221b511e02f67dffade8f2c8382390e797e2..dad37f43b0c74c31b8324be15c37d3c7e91ba7ec 100644
--- a/include/crocoddyl/core/actuation-base.hpp
+++ b/include/crocoddyl/core/actuation-base.hpp
@@ -58,17 +58,17 @@ struct ActuationDataAbstract {
template <typename Model>
explicit ActuationDataAbstract(Model* const model)
- : a(model->get_state().get_nv()),
- Ax(model->get_state().get_nv(), model->get_state().get_ndx()),
- Au(model->get_state().get_nv(), model->get_nu()) {
- a.fill(0);
- Ax.fill(0);
- Au.fill(0);
+ : tau(model->get_state().get_nv()),
+ dtau_dx(model->get_state().get_nv(), model->get_state().get_ndx()),
+ dtau_du(model->get_state().get_nv(), model->get_nu()) {
+ tau.fill(0);
+ dtau_dx.fill(0);
+ dtau_du.fill(0);
}
- Eigen::VectorXd a;
- Eigen::MatrixXd Ax;
- Eigen::MatrixXd Au;
+ Eigen::VectorXd tau;
+ Eigen::MatrixXd dtau_dx;
+ Eigen::MatrixXd dtau_du;
};
} // namespace crocoddyl
diff --git a/include/crocoddyl/core/diff-action-base.hpp b/include/crocoddyl/core/diff-action-base.hpp
index 7894260e47776e6fa269357c476cd96a723a30bd..c581b877d612dd4351db0338882f6527d7db77d0 100644
--- a/include/crocoddyl/core/diff-action-base.hpp
+++ b/include/crocoddyl/core/diff-action-base.hpp
@@ -10,7 +10,6 @@
#define CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
#include "crocoddyl/core/state-base.hpp"
-#include "crocoddyl/multibody/costs/cost-sum.hpp"
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
@@ -20,7 +19,7 @@ struct DifferentialActionDataAbstract; // forward declaration
class DifferentialActionModelAbstract {
public:
- DifferentialActionModelAbstract(StateAbstract& state, unsigned int const& nu, unsigned int const& nr = 1);
+ DifferentialActionModelAbstract(StateAbstract& state, unsigned int const& nu, unsigned int const& nr = 0);
virtual ~DifferentialActionModelAbstract();
virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
@@ -82,26 +81,16 @@ struct DifferentialActionDataAbstract {
explicit DifferentialActionDataAbstract(Model* const model)
: cost(0.),
xout(model->get_state().get_nv()),
- r(model->get_nr()),
- qcur(model->get_state().get_nq()),
- vcur(model->get_state().get_nv()),
Fx(model->get_state().get_nv(), model->get_state().get_ndx()),
Fu(model->get_state().get_nv(), model->get_nu()),
+ r(model->get_nr()),
Lx(model->get_state().get_ndx()),
Lu(model->get_nu()),
Lxx(model->get_state().get_ndx(), model->get_state().get_ndx()),
Lxu(model->get_state().get_ndx(), model->get_nu()),
- Luu(model->get_nu(), model->get_nu()),
- r_ref(&r(0), model->get_nr()),
- Lx_ref(&Lx(0), model->get_state().get_ndx()),
- Lu_ref(&Lu(0), model->get_nu()),
- Lxx_ref(&Lxx(0), model->get_state().get_ndx(), model->get_state().get_ndx()),
- Lxu_ref(&Lxu(0), model->get_state().get_ndx(), model->get_nu()),
- Luu_ref(&Luu(0), model->get_nu(), model->get_nu()) {
+ Luu(model->get_nu(), model->get_nu()) {
xout.setZero();
r.setZero();
- qcur.setZero();
- vcur.setZero();
Fx.setZero();
Fu.setZero();
Lx.setZero();
@@ -111,55 +100,16 @@ struct DifferentialActionDataAbstract {
Luu.setZero();
}
- void shareCostMemory(const boost::shared_ptr<CostDataSum>& costs) {
- // Share memory with the cost data
- new (&r_ref) Eigen::Map<Eigen::VectorXd>(&costs->r(0), costs->r.size());
- new (&Lx_ref) Eigen::Map<Eigen::VectorXd>(&costs->Lx(0), costs->Lx.size());
- new (&Lu_ref) Eigen::Map<Eigen::VectorXd>(&costs->Lu(0), costs->Lu.size());
- new (&Lxx_ref) Eigen::Map<Eigen::MatrixXd>(&costs->Lxx(0), costs->Lxx.rows(), costs->Lxx.cols());
- new (&Lxu_ref) Eigen::Map<Eigen::MatrixXd>(&costs->Lxu(0), costs->Lxu.rows(), costs->Lxu.cols());
- new (&Luu_ref) Eigen::Map<Eigen::MatrixXd>(&costs->Luu(0), costs->Luu.rows(), costs->Luu.cols());
- }
-
- const double& get_cost() const { return cost; }
- const Eigen::VectorXd& get_xout() const { return xout; }
-
- Eigen::VectorXd get_r() const { return r_ref; }
- Eigen::VectorXd get_Lx() const { return Lx_ref; }
- Eigen::VectorXd get_Lu() const { return Lu_ref; }
- Eigen::MatrixXd get_Lxx() const { return Lxx_ref; }
- Eigen::MatrixXd get_Lxu() const { return Lxu_ref; }
- Eigen::MatrixXd get_Luu() const { return Luu_ref; }
- const Eigen::MatrixXd& get_Fx() const { return Fx; }
- const Eigen::MatrixXd& get_Fu() const { return Fu; }
-
- void set_r(Eigen::VectorXd _r) { r_ref = _r; }
- void set_Lx(Eigen::VectorXd _Lx) { Lx_ref = _Lx; }
- void set_Lu(Eigen::VectorXd _Lu) { Lu_ref = _Lu; }
- void set_Lxx(Eigen::MatrixXd _Lxx) { Lxx_ref = _Lxx; }
- void set_Lxu(Eigen::MatrixXd _Lxu) { Lxu_ref = _Lxu; }
- void set_Luu(Eigen::MatrixXd _Luu) { Luu_ref = _Luu; }
-
double cost;
Eigen::VectorXd xout;
- Eigen::VectorXd r;
- Eigen::VectorXd qcur;
- Eigen::VectorXd vcur;
Eigen::MatrixXd Fx;
Eigen::MatrixXd Fu;
+ Eigen::VectorXd r;
Eigen::VectorXd Lx;
Eigen::VectorXd Lu;
Eigen::MatrixXd Lxx;
Eigen::MatrixXd Lxu;
Eigen::MatrixXd Luu;
-
- // Using Map for sharing memory with cost-sum data
- Eigen::Map<Eigen::VectorXd> r_ref;
- Eigen::Map<Eigen::VectorXd> Lx_ref;
- Eigen::Map<Eigen::VectorXd> Lu_ref;
- Eigen::Map<Eigen::MatrixXd> Lxx_ref;
- Eigen::Map<Eigen::MatrixXd> Lxu_ref;
- Eigen::Map<Eigen::MatrixXd> Luu_ref;
};
} // namespace crocoddyl
diff --git a/include/crocoddyl/core/numdiff/action.hpp b/include/crocoddyl/core/numdiff/action.hpp
index 5f71cd007d91c6be943bd019f76130805bb86479..0d3060976e245aac3171c2dd92a07a88ac5ed7f5 100644
--- a/include/crocoddyl/core/numdiff/action.hpp
+++ b/include/crocoddyl/core/numdiff/action.hpp
@@ -15,19 +15,89 @@
namespace crocoddyl {
+/**
+ * @brief This class computes the numerical differentiation of an ActionModel.
+ *
+ * It computes the same quantity as a normal model would do but using numerical
+ * differentiation.
+ * The subtility is in the computation of the Hessian of the cost. Let us
+ * consider that the ActionModel owns a cost residual. This means that the cost
+ * is the square of a residual \f$ l(x,u) = .5 r(x,u)**2 \f$, with
+ * \f$ r(x,u) \f$ being the residual vector. Therefore the derivatives of the
+ * cost \f$ l \f$ can be expressed in function of the derivatives of the
+ * residuals (Jacobians), denoted by \f$ R_x \f$ and \f$ R_u \f$. Which would be:
+ * \f{eqnarray*}{
+ * L_x &=& R_x^T r \\
+ * L_u &=& R_u^T r \\
+ * L_{xx} &=& R_x^T R_x + R_{xx} r
+ * \f}
+ * with \f$ R_{xx} \f$ the derivatives of the Jacobian (i.e. not a matrix, but a
+ * dim-3 tensor). The Gauss approximation consists in neglecting these.
+ * So \f$ L_{xx} \sim R_x^T R_x \f$. Similarly for \f$ L_{xu} \sim R_x^T R_u \f$
+ * and \f$ L_{uu} \sim R_u^T R_u \f$. The above set of equations becomes:
+ * \f{eqnarray*}{
+ * L_x &=& R_x^T r \\
+ * L_u &=& R_u^T r \\
+ * L_{xx} &\sim& R_x^T R_x \\
+ * L_{xu} &\sim& R_x^T R_u \\
+ * L_{uu} &\sim& R_u^T R_u
+ * \f}
+ * In the case that the cost does not have a residual we set the Hessian to
+ * \f$ 0 \f$, i.e. \f$ L_{xx} = L_{xu} = L_{uu} = 0 \f$.
+ */
class ActionModelNumDiff : public ActionModelAbstract {
public:
- explicit ActionModelNumDiff(ActionModelAbstract& model, bool with_gauss_approx = false);
+ /**
+ * @brief Construct a new ActionModelNumDiff object
+ *
+ * @param model
+ */
+ explicit ActionModelNumDiff(ActionModelAbstract& model);
+
+ /**
+ * @brief Destroy the ActionModelNumDiff object
+ */
~ActionModelNumDiff();
+ /**
+ * @brief @copydoc ActionDataAbstract::calc()
+ */
void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u);
+
+ /**
+ * @brief @copydoc ActionDataAbstract::calcDiff()
+ */
void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true);
+
+ /**
+ * @brief Create a Data object from the given model.
+ *
+ * @return boost::shared_ptr<ActionDataAbstract>
+ */
boost::shared_ptr<ActionDataAbstract> createData();
+ /**
+ * @brief Get the model_ object
+ *
+ * @return ActionModelAbstract&
+ */
ActionModelAbstract& get_model() const;
+
+ /**
+ * @brief Get the disturbance_ object
+ *
+ * @return const double&
+ */
const double& get_disturbance() const;
+
+ /**
+ * @brief Identify if the Gauss approximation is going to be used or not.
+ *
+ * @return true
+ * @return false
+ */
bool get_with_gauss_approx();
private:
@@ -40,13 +110,19 @@ class ActionModelNumDiff : public ActionModelAbstract {
* For full discussions see issue
* https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
*
- * @param model object to be checked.
* @param x is the state at which the check is performed.
*/
void assertStableStateFD(const Eigen::Ref<const Eigen::VectorXd>& x);
+ /**
+ * @brief This is the model to compute the finite differenciation from
+ */
ActionModelAbstract& model_;
- bool with_gauss_approx_;
+
+ /**
+ * @brief This is the numerical disturbance value used during the numerical
+ * differenciations
+ */
double disturbance_;
};
@@ -83,14 +159,16 @@ struct ActionDataNumDiff : public ActionDataAbstract {
}
}
- Eigen::MatrixXd Rx;
- Eigen::MatrixXd Ru;
- Eigen::VectorXd dx;
- Eigen::VectorXd du;
- Eigen::VectorXd xp;
- boost::shared_ptr<ActionDataAbstract> data_0;
- std::vector<boost::shared_ptr<ActionDataAbstract> > data_x;
- std::vector<boost::shared_ptr<ActionDataAbstract> > data_u;
+ Eigen::MatrixXd Rx; //!< Cost residual jacobian: \f$ \frac{d r(x,u)}{dx} \f$
+ Eigen::MatrixXd Ru; //!< Cost residual jacobian: \f$ \frac{d r(x,u)}{du} \f$
+ Eigen::VectorXd dx; //!< State disturbance
+ Eigen::VectorXd du; //!< Control disturbance
+ Eigen::VectorXd xp; //!< The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$"
+ boost::shared_ptr<ActionDataAbstract> data_0; //!< The data that contains the final results
+ std::vector<boost::shared_ptr<ActionDataAbstract> >
+ data_x; //!< The temporary data associated with the state variation
+ std::vector<boost::shared_ptr<ActionDataAbstract> >
+ data_u; //!< The temporary data associated with the control variation
};
} // namespace crocoddyl
diff --git a/include/crocoddyl/core/solver-base.hpp b/include/crocoddyl/core/solver-base.hpp
index 85e17ce48e278cc8b5815c08a840b8babe3f0d53..06f7817fdfecbc6e0b98986223c46f8eaf2480a4 100644
--- a/include/crocoddyl/core/solver-base.hpp
+++ b/include/crocoddyl/core/solver-base.hpp
@@ -36,6 +36,7 @@ class SolverAbstract {
const std::vector<Eigen::VectorXd>& us_warm = DEFAULT_VECTOR, const bool& is_feasible = false);
void setCallbacks(const std::vector<CallbackAbstract*>& callbacks);
+ const std::vector<CallbackAbstract*>& getCallbacks() const;
const ShootingProblem& get_problem() const;
const std::vector<ActionModelAbstract*>& get_models() const;
diff --git a/include/crocoddyl/core/solvers/ddp.hpp b/include/crocoddyl/core/solvers/ddp.hpp
index 1cfa952fe08ff03f393541f61a9b78dd6bae278b..688794b042e9b89b071a2154c8cfab06002f75ec 100644
--- a/include/crocoddyl/core/solvers/ddp.hpp
+++ b/include/crocoddyl/core/solvers/ddp.hpp
@@ -33,6 +33,17 @@ class SolverDDP : public SolverAbstract {
void backwardPass();
void forwardPass(const double& stepLength);
+ void computeGains(unsigned int const& t);
+ void increaseRegularization();
+ void decreaseRegularization();
+ void allocateData();
+
+ const double& get_regfactor() const;
+ const double& get_regmin() const;
+ const double& get_regmax() const;
+ const std::vector<double>& get_alphas() const;
+ const double& get_th_step() const;
+ const double& get_th_grad() const;
const std::vector<Eigen::MatrixXd>& get_Vxx() const;
const std::vector<Eigen::VectorXd>& get_Vx() const;
const std::vector<Eigen::MatrixXd>& get_Qxx() const;
@@ -44,10 +55,12 @@ class SolverDDP : public SolverAbstract {
const std::vector<Eigen::VectorXd>& get_k() const;
const std::vector<Eigen::VectorXd>& get_gaps() const;
- void computeGains(unsigned int const& t);
- void increaseRegularization();
- void decreaseRegularization();
- void allocateData();
+ void set_regfactor(double reg_factor);
+ void set_regmin(double regmin);
+ void set_regmax(double regmax);
+ void set_alphas(const std::vector<double>& alphas);
+ void set_th_step(double th_step);
+ void set_th_grad(double th_grad);
protected:
double regfactor_;
diff --git a/include/crocoddyl/core/solvers/fddp.hpp b/include/crocoddyl/core/solvers/fddp.hpp
index 372d75240619581cb6ea8ddd12678b505c810a02..0e83cbf7c2ee2445580b43d56f21c4138b81cafe 100644
--- a/include/crocoddyl/core/solvers/fddp.hpp
+++ b/include/crocoddyl/core/solvers/fddp.hpp
@@ -21,14 +21,19 @@ class SolverFDDP : public SolverDDP {
explicit SolverFDDP(ShootingProblem& problem);
~SolverFDDP();
- double calc();
- void backwardPass();
- void forwardPass(const double& stepLength);
- const Eigen::Vector2d& expectedImprovement();
- void updateExpectedImprovement();
+
bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, unsigned int const& maxiter = 100,
const bool& is_feasible = false, const double& regInit = 1e-9);
+ void computeDirection(const bool& recalc = true);
+ double tryStep(const double& steplength = 1);
+ const Eigen::Vector2d& expectedImprovement();
+ void updateExpectedImprovement();
+ double calc();
+ void forwardPass(const double& stepLength);
+
+ double get_th_acceptnegstep() const;
+ void set_th_acceptnegstep(double th_acceptnegstep);
protected:
double dg_;
@@ -36,7 +41,7 @@ class SolverFDDP : public SolverDDP {
double dv_;
private:
- double th_acceptNegStep_;
+ double th_acceptnegstep_;
};
} // namespace crocoddyl
diff --git a/include/crocoddyl/multibody/actions/contact-fwddyn.hpp b/include/crocoddyl/multibody/actions/contact-fwddyn.hpp
index 296839dba6a548089840de87ebf9bee292682923..df1a2d0981310671b838563733f5d3426c3bf136 100644
--- a/include/crocoddyl/multibody/actions/contact-fwddyn.hpp
+++ b/include/crocoddyl/multibody/actions/contact-fwddyn.hpp
@@ -62,15 +62,15 @@ struct DifferentialActionDataContactFwdDynamics : public DifferentialActionDataA
pinocchio(pinocchio::Data(model->get_pinocchio())),
Kinv(model->get_state().get_nv() + model->get_contacts().get_nc(),
model->get_state().get_nv() + model->get_contacts().get_nc()),
- Gx(model->get_contacts().get_nc(), model->get_state().get_ndx()),
- Gu(model->get_contacts().get_nc(), model->get_nu()) {
+ df_dx(model->get_contacts().get_nc(), model->get_state().get_ndx()),
+ df_du(model->get_contacts().get_nc(), model->get_nu()) {
actuation = model->get_actuation().createData();
contacts = model->get_contacts().createData(&pinocchio);
costs = model->get_costs().createData(&pinocchio);
- shareCostMemory(costs);
+ costs->shareMemory(this);
Kinv.fill(0);
- Gx.fill(0);
- Gu.fill(0);
+ df_dx.fill(0);
+ df_du.fill(0);
}
pinocchio::Data pinocchio;
@@ -78,8 +78,8 @@ struct DifferentialActionDataContactFwdDynamics : public DifferentialActionDataA
boost::shared_ptr<ContactDataMultiple> contacts;
boost::shared_ptr<CostDataSum> costs;
Eigen::MatrixXd Kinv;
- Eigen::MatrixXd Gx;
- Eigen::MatrixXd Gu;
+ Eigen::MatrixXd df_dx;
+ Eigen::MatrixXd df_du;
};
} // namespace crocoddyl
diff --git a/include/crocoddyl/multibody/actions/free-fwddyn.hpp b/include/crocoddyl/multibody/actions/free-fwddyn.hpp
index ddb6b17e291f1bb3de780f64f0d4f31fa287a5cd..7db3e53d42ba0f0a8124a5efccb4e9a5bd240ee2 100644
--- a/include/crocoddyl/multibody/actions/free-fwddyn.hpp
+++ b/include/crocoddyl/multibody/actions/free-fwddyn.hpp
@@ -50,7 +50,7 @@ struct DifferentialActionDataFreeFwdDynamics : public DifferentialActionDataAbst
Minv(model->get_state().get_nv(), model->get_state().get_nv()),
u_drift(model->get_nu()) {
costs = model->get_costs().createData(&pinocchio);
- shareCostMemory(costs);
+ costs->shareMemory(this);
Minv.fill(0);
u_drift.fill(0);
}
diff --git a/include/crocoddyl/multibody/actions/impulse-fwddyn.hpp b/include/crocoddyl/multibody/actions/impulse-fwddyn.hpp
index 6e51a4d1afe8a0efccc7029d186a56e73542ebf4..7e5e2a53bcd7604d9687b208729823456b56c6d4 100644
--- a/include/crocoddyl/multibody/actions/impulse-fwddyn.hpp
+++ b/include/crocoddyl/multibody/actions/impulse-fwddyn.hpp
@@ -6,9 +6,81 @@
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
-#ifndef CROCODDYL_MULTIBODY_IMPULSE_FWDDYN_HPP_
-#define CROCODDYL_MULTIBODY_IMPULSE_FWDDYN_HPP_
+#ifndef CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
+#define CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
-// TODO: ActionModelImpact ActionDataImpact
+#include "crocoddyl/core/action-base.hpp"
+#include "crocoddyl/multibody/states/multibody.hpp"
+#include "crocoddyl/multibody/actuations/floating-base.hpp"
+#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
+#include "crocoddyl/multibody/costs/cost-sum.hpp"
+#include <pinocchio/multibody/data.hpp>
-#endif // CROCODDYL_MULTIBODY_IMPULSE_FWDDYN_HPP_
+namespace crocoddyl {
+
+class ActionModelImpulseFwdDynamics : public ActionModelAbstract {
+ public:
+ ActionModelImpulseFwdDynamics(StateMultibody& state, ImpulseModelMultiple& impulses, CostModelSum& costs,
+ const double& r_coeff = 0., const double& JMinvJt_damping = 0.,
+ const bool& enable_force = false);
+ ~ActionModelImpulseFwdDynamics();
+
+ void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u);
+ void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true);
+ boost::shared_ptr<ActionDataAbstract> createData();
+
+ ImpulseModelMultiple& get_impulses() const;
+ CostModelSum& get_costs() const;
+ pinocchio::Model& get_pinocchio() const;
+ const Eigen::VectorXd& get_armature() const;
+ const double& get_restitution_coefficient() const;
+ const double& get_damping_factor() const;
+
+ void set_armature(const Eigen::VectorXd& armature);
+ void set_restitution_coefficient(const double& r_coeff);
+ void set_damping_factor(const double& damping);
+
+ private:
+ ImpulseModelMultiple& impulses_;
+ CostModelSum& costs_;
+ pinocchio::Model& pinocchio_;
+ bool with_armature_;
+ Eigen::VectorXd armature_;
+ double r_coeff_;
+ double JMinvJt_damping_;
+ bool enable_force_;
+ pinocchio::Motion gravity_;
+};
+
+struct ActionDataImpulseFwdDynamics : public ActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ explicit ActionDataImpulseFwdDynamics(Model* const model)
+ : ActionDataAbstract(model),
+ pinocchio(pinocchio::Data(model->get_pinocchio())),
+ vnone(model->get_state().get_nv()),
+ Kinv(model->get_state().get_nv() + model->get_impulses().get_ni(),
+ model->get_state().get_nv() + model->get_impulses().get_ni()),
+ df_dq(model->get_impulses().get_ni(), model->get_state().get_nv()) {
+ impulses = model->get_impulses().createData(&pinocchio);
+ costs = model->get_costs().createData(&pinocchio);
+ costs->shareMemory(this);
+ vnone.fill(0);
+ Kinv.fill(0);
+ df_dq.fill(0);
+ }
+
+ pinocchio::Data pinocchio;
+ boost::shared_ptr<ImpulseDataMultiple> impulses;
+ boost::shared_ptr<CostDataSum> costs;
+ Eigen::VectorXd vnone;
+ Eigen::MatrixXd Kinv;
+ Eigen::MatrixXd df_dq;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
diff --git a/include/crocoddyl/multibody/actuations/floating-base.hpp b/include/crocoddyl/multibody/actuations/floating-base.hpp
index cc2e3f7f6472935509ce09b5b618e0decf859e85..fc14f8837970a1df5a131affcfa83f9b43264c99 100644
--- a/include/crocoddyl/multibody/actuations/floating-base.hpp
+++ b/include/crocoddyl/multibody/actuations/floating-base.hpp
@@ -27,7 +27,7 @@ class ActuationModelFloatingBase : public ActuationModelAbstract {
#ifndef NDEBUG
private:
- Eigen::MatrixXd Au_;
+ Eigen::MatrixXd dtau_du_;
#endif
};
diff --git a/include/crocoddyl/multibody/contact-base.hpp b/include/crocoddyl/multibody/contact-base.hpp
index 55b916fa751a3307509aa02645825aba1575f73e..7f4d9b81c358e62e60ab1bc53b3571ef38fe2703 100644
--- a/include/crocoddyl/multibody/contact-base.hpp
+++ b/include/crocoddyl/multibody/contact-base.hpp
@@ -27,9 +27,10 @@ class ContactModelAbstract {
const Eigen::Ref<const Eigen::VectorXd>& x) = 0;
virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const bool& recalc = true) = 0;
- virtual void updateLagrangian(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& lambda) = 0;
- void updateLagrangianDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::MatrixXd& Gx,
- const Eigen::MatrixXd& Gu);
+
+ virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& force) = 0;
+ void updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::MatrixXd& df_dx,
+ const Eigen::MatrixXd& df_du) const;
virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::Data* const data);
StateMultibody& get_state() const;
@@ -63,24 +64,24 @@ struct ContactDataAbstract {
joint(0),
Jc(model->get_nc(), model->get_state().get_nv()),
a0(model->get_nc()),
- Ax(model->get_nc(), model->get_state().get_ndx()),
- Gx(model->get_nc(), model->get_state().get_ndx()),
- Gu(model->get_nc(), model->get_nu()),
+ da0_dx(model->get_nc(), model->get_state().get_ndx()),
+ df_dx(model->get_nc(), model->get_state().get_ndx()),
+ df_du(model->get_nc(), model->get_nu()),
f(pinocchio::Force::Zero()) {
Jc.fill(0);
a0.fill(0);
- Ax.fill(0);
- Gx.fill(0);
- Gu.fill(0);
+ da0_dx.fill(0);
+ df_dx.fill(0);
+ df_du.fill(0);
}
pinocchio::Data* pinocchio;
pinocchio::JointIndex joint;
Eigen::MatrixXd Jc;
Eigen::VectorXd a0;
- Eigen::MatrixXd Ax;
- Eigen::MatrixXd Gx;
- Eigen::MatrixXd Gu;
+ Eigen::MatrixXd da0_dx;
+ Eigen::MatrixXd df_dx;
+ Eigen::MatrixXd df_du;
pinocchio::Force f;
};
diff --git a/include/crocoddyl/multibody/contacts/contact-3d.hpp b/include/crocoddyl/multibody/contacts/contact-3d.hpp
index 9f7780ba3d0521f8dad3d1cedd85d3d871d50711..db1fbc7e9c91a99da9a7f6fc0639760ef5334fa7 100644
--- a/include/crocoddyl/multibody/contacts/contact-3d.hpp
+++ b/include/crocoddyl/multibody/contacts/contact-3d.hpp
@@ -27,7 +27,7 @@ class ContactModel3D : public ContactModelAbstract {
void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const bool& recalc = true);
- void updateLagrangian(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& lambda);
+ void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& force);
boost::shared_ptr<ContactDataAbstract> createData(pinocchio::Data* const data);
const FrameTranslation& get_xref() const;
diff --git a/include/crocoddyl/multibody/contacts/contact-6d.hpp b/include/crocoddyl/multibody/contacts/contact-6d.hpp
index b60421b227c0309556815accf4ff9f751e789651..5a621408278ffb07601891ba16d0844d30825d78 100644
--- a/include/crocoddyl/multibody/contacts/contact-6d.hpp
+++ b/include/crocoddyl/multibody/contacts/contact-6d.hpp
@@ -27,7 +27,7 @@ class ContactModel6D : public ContactModelAbstract {
void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const bool& recalc = true);
- void updateLagrangian(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& lambda);
+ void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& force);
boost::shared_ptr<ContactDataAbstract> createData(pinocchio::Data* const data);
const FramePlacement& get_Mref() const;
diff --git a/include/crocoddyl/multibody/contacts/multiple-contacts.hpp b/include/crocoddyl/multibody/contacts/multiple-contacts.hpp
index 0462533bf7bf6c6be202ea58d2e4a25943f7d262..98027be66acbcd666ef65cfb29ecdf2b95b9160a 100644
--- a/include/crocoddyl/multibody/contacts/multiple-contacts.hpp
+++ b/include/crocoddyl/multibody/contacts/multiple-contacts.hpp
@@ -42,9 +42,12 @@ class ContactModelMultiple {
void calc(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
void calcDiff(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const bool& recalc = true);
- void updateLagrangian(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::VectorXd& lambda);
- void updateLagrangianDiff(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::MatrixXd& Gx,
- const Eigen::MatrixXd& Gu);
+
+ void updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::VectorXd& dv) const;
+ void updateForce(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::VectorXd& force);
+ void updateAccelerationDiff(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::MatrixXd& ddv_dx) const;
+ void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::MatrixXd& df_dx,
+ const Eigen::MatrixXd& df_du) const;
boost::shared_ptr<ContactDataMultiple> createData(pinocchio::Data* const data);
StateMultibody& get_state() const;
@@ -76,7 +79,12 @@ struct ContactDataMultiple : ContactDataAbstract {
template <typename Model>
ContactDataMultiple(Model* const model, pinocchio::Data* const data)
- : ContactDataAbstract(model, data), fext(model->get_state().get_pinocchio().njoints, pinocchio::Force::Zero()) {
+ : ContactDataAbstract(model, data),
+ dv(model->get_state().get_nv()),
+ ddv_dx(model->get_state().get_nv(), model->get_state().get_ndx()),
+ fext(model->get_state().get_pinocchio().njoints, pinocchio::Force::Zero()) {
+ dv.fill(0);
+ ddv_dx.fill(0);
for (ContactModelMultiple::ContactModelContainer::const_iterator it = model->get_contacts().begin();
it != model->get_contacts().end(); ++it) {
const ContactItem& item = it->second;
@@ -84,6 +92,8 @@ struct ContactDataMultiple : ContactDataAbstract {
}
}
+ Eigen::VectorXd dv;
+ Eigen::MatrixXd ddv_dx;
ContactModelMultiple::ContactDataContainer contacts;
pinocchio::container::aligned_vector<pinocchio::Force> fext;
};
diff --git a/include/crocoddyl/multibody/cost-base.hpp b/include/crocoddyl/multibody/cost-base.hpp
index c7371175c98a2e6c363ccf691584283c193c4f9b..1e807aa07a6e7df0c30e6f032309d45d2a9d8919 100644
--- a/include/crocoddyl/multibody/cost-base.hpp
+++ b/include/crocoddyl/multibody/cost-base.hpp
@@ -84,18 +84,25 @@ struct CostDataAbstract {
template <typename Model>
CostDataAbstract(Model* const model, pinocchio::Data* const data)
- : pinocchio(data), activation(model->get_activation().createData()), cost(0.) {
- const int& ndx = model->get_state().get_ndx();
- const int& nu = model->get_nu();
- const int& nr = model->get_activation().get_nr();
- Lx = Eigen::VectorXd::Zero(ndx);
- Lu = Eigen::VectorXd::Zero(nu);
- Lxx = Eigen::MatrixXd::Zero(ndx, ndx);
- Lxu = Eigen::MatrixXd::Zero(ndx, nu);
- Luu = Eigen::MatrixXd::Zero(nu, nu);
- r = Eigen::VectorXd::Zero(nr);
- Rx = Eigen::MatrixXd::Zero(nr, ndx);
- Ru = Eigen::MatrixXd::Zero(nr, nu);
+ : pinocchio(data),
+ activation(model->get_activation().createData()),
+ cost(0.),
+ Lx(model->get_state().get_ndx()),
+ Lu(model->get_nu()),
+ Lxx(model->get_state().get_ndx(), model->get_state().get_ndx()),
+ Lxu(model->get_state().get_ndx(), model->get_nu()),
+ Luu(model->get_nu(), model->get_nu()),
+ r(model->get_activation().get_nr()),
+ Rx(model->get_activation().get_nr(), model->get_state().get_ndx()),
+ Ru(model->get_activation().get_nr(), model->get_nu()) {
+ Lx.fill(0);
+ Lu.fill(0);
+ Lxx.fill(0);
+ Lxu.fill(0);
+ Luu.fill(0);
+ r.fill(0);
+ Rx.fill(0);
+ Ru.fill(0);
}
pinocchio::Data* pinocchio;
diff --git a/include/crocoddyl/multibody/costs/cost-sum.hpp b/include/crocoddyl/multibody/costs/cost-sum.hpp
index c4a6ec8661cbf842e1e128b4cf10991884fe1a7a..804ca362fbbd099af3dc99562e454cef6c3d902d 100644
--- a/include/crocoddyl/multibody/costs/cost-sum.hpp
+++ b/include/crocoddyl/multibody/costs/cost-sum.hpp
@@ -95,34 +95,78 @@ struct CostDataSum {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
template <typename Model>
- CostDataSum(Model* const model, pinocchio::Data* const data) : pinocchio(data), cost(0.) {
+ CostDataSum(Model* const model, pinocchio::Data* const data)
+ : r_internal(model->get_nr()),
+ Lx_internal(model->get_state().get_ndx()),
+ Lu_internal(model->get_nu()),
+ Lxx_internal(model->get_state().get_ndx(), model->get_state().get_ndx()),
+ Lxu_internal(model->get_state().get_ndx(), model->get_nu()),
+ Luu_internal(model->get_nu(), model->get_nu()),
+ pinocchio(data),
+ cost(0.),
+ r(r_internal.data(), model->get_nr()),
+ Lx(Lx_internal.data(), model->get_state().get_ndx()),
+ Lu(Lu_internal.data(), model->get_nu()),
+ Lxx(Lxx_internal.data(), model->get_state().get_ndx(), model->get_state().get_ndx()),
+ Lxu(Lxu_internal.data(), model->get_state().get_ndx(), model->get_nu()),
+ Luu(Luu_internal.data(), model->get_nu(), model->get_nu()),
+ Rx(model->get_nr(), model->get_state().get_ndx()),
+ Ru(model->get_nr(), model->get_nu()) {
+ r.setZero();
+ Lx.setZero();
+ Lu.setZero();
+ Lxx.setZero();
+ Lxu.setZero();
+ Luu.setZero();
for (CostModelSum::CostModelContainer::const_iterator it = model->get_costs().begin();
it != model->get_costs().end(); ++it) {
const CostItem& item = it->second;
costs.insert(std::make_pair(item.name, item.cost->createData(data)));
}
- const int& ndx = model->get_state().get_ndx();
- const int& nu = model->get_nu();
- const int& nr = model->get_nr();
- Lx = Eigen::VectorXd::Zero(ndx);
- Lu = Eigen::VectorXd::Zero(nu);
- Lxx = Eigen::MatrixXd::Zero(ndx, ndx);
- Lxu = Eigen::MatrixXd::Zero(ndx, nu);
- Luu = Eigen::MatrixXd::Zero(nu, nu);
- r = Eigen::VectorXd::Zero(nr);
- Rx = Eigen::MatrixXd::Zero(nr, ndx);
- Ru = Eigen::MatrixXd::Zero(nr, nu);
}
+ template <typename ActionData>
+ void shareMemory(ActionData* const data) {
+ // Share memory with the differential action data
+ new (&r) Eigen::Map<Eigen::VectorXd>(data->r.data(), data->r.size());
+ new (&Lx) Eigen::Map<Eigen::VectorXd>(data->Lx.data(), data->Lx.size());
+ new (&Lu) Eigen::Map<Eigen::VectorXd>(data->Lu.data(), data->Lu.size());
+ new (&Lxx) Eigen::Map<Eigen::MatrixXd>(data->Lxx.data(), data->Lxx.rows(), data->Lxx.cols());
+ new (&Lxu) Eigen::Map<Eigen::MatrixXd>(data->Lxu.data(), data->Lxu.rows(), data->Lxu.cols());
+ new (&Luu) Eigen::Map<Eigen::MatrixXd>(data->Luu.data(), data->Luu.rows(), data->Luu.cols());
+ }
+
+ Eigen::VectorXd get_r() const { return r; }
+ Eigen::VectorXd get_Lx() const { return Lx; }
+ Eigen::VectorXd get_Lu() const { return Lu; }
+ Eigen::MatrixXd get_Lxx() const { return Lxx; }
+ Eigen::MatrixXd get_Lxu() const { return Lxu; }
+ Eigen::MatrixXd get_Luu() const { return Luu; }
+
+ void set_r(Eigen::VectorXd _r) { r = _r; }
+ void set_Lx(Eigen::VectorXd _Lx) { Lx = _Lx; }
+ void set_Lu(Eigen::VectorXd _Lu) { Lu = _Lu; }
+ void set_Lxx(Eigen::MatrixXd _Lxx) { Lxx = _Lxx; }
+ void set_Lxu(Eigen::MatrixXd _Lxu) { Lxu = _Lxu; }
+ void set_Luu(Eigen::MatrixXd _Luu) { Luu = _Luu; }
+
+ // Creates internal data in case we don't share it externally
+ Eigen::VectorXd r_internal;
+ Eigen::VectorXd Lx_internal;
+ Eigen::VectorXd Lu_internal;
+ Eigen::MatrixXd Lxx_internal;
+ Eigen::MatrixXd Lxu_internal;
+ Eigen::MatrixXd Luu_internal;
+
CostModelSum::CostDataContainer costs;
pinocchio::Data* pinocchio;
double cost;
- Eigen::VectorXd Lx;
- Eigen::VectorXd Lu;
- Eigen::MatrixXd Lxx;
- Eigen::MatrixXd Lxu;
- Eigen::MatrixXd Luu;
- Eigen::VectorXd r;
+ Eigen::Map<Eigen::VectorXd> r;
+ Eigen::Map<Eigen::VectorXd> Lx;
+ Eigen::Map<Eigen::VectorXd> Lu;
+ Eigen::Map<Eigen::MatrixXd> Lxx;
+ Eigen::Map<Eigen::MatrixXd> Lxu;
+ Eigen::Map<Eigen::MatrixXd> Luu;
Eigen::MatrixXd Rx;
Eigen::MatrixXd Ru;
};
diff --git a/include/crocoddyl/multibody/impulse-base.hpp b/include/crocoddyl/multibody/impulse-base.hpp
index afab37a3d3cf7e905e8507c8f52579c924f81fad..530ba0afea6cb56a6e01761f0f457bde7470e954 100644
--- a/include/crocoddyl/multibody/impulse-base.hpp
+++ b/include/crocoddyl/multibody/impulse-base.hpp
@@ -26,7 +26,9 @@ class ImpulseModelAbstract {
const Eigen::Ref<const Eigen::VectorXd>& x) = 0;
virtual void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const bool& recalc = true) = 0;
- virtual void updateLagrangian(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& lambda) = 0;
+
+ virtual void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& force) = 0;
+ void updateForceDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::MatrixXd& df_dq) const;
virtual boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::Data* const data);
@@ -58,16 +60,19 @@ struct ImpulseDataAbstract {
: pinocchio(data),
joint(0),
Jc(model->get_ni(), model->get_state().get_nv()),
- Vq(model->get_ni(), model->get_state().get_nv()),
+ dv0_dq(model->get_ni(), model->get_state().get_nv()),
+ df_dq(model->get_ni(), model->get_state().get_nv()),
f(pinocchio::Force::Zero()) {
Jc.fill(0);
- Vq.fill(0);
+ dv0_dq.fill(0);
+ df_dq.fill(0);
}
pinocchio::Data* pinocchio;
pinocchio::JointIndex joint;
Eigen::MatrixXd Jc;
- Eigen::MatrixXd Vq;
+ Eigen::MatrixXd dv0_dq;
+ Eigen::MatrixXd df_dq;
pinocchio::Force f;
};
diff --git a/include/crocoddyl/multibody/impulses/impulse-3d.hpp b/include/crocoddyl/multibody/impulses/impulse-3d.hpp
index 7dd5a16679f067166569cf8b123482aa840abb76..35b26c073b29a1046df8c0cb68f76843367dd1c8 100644
--- a/include/crocoddyl/multibody/impulses/impulse-3d.hpp
+++ b/include/crocoddyl/multibody/impulses/impulse-3d.hpp
@@ -23,7 +23,7 @@ class ImpulseModel3D : public ImpulseModelAbstract {
void calc(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const bool& recalc = true);
- void updateLagrangian(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& lambda);
+ void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& force);
boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::Data* const data);
unsigned int const& get_frame() const;
diff --git a/include/crocoddyl/multibody/impulses/impulse-6d.hpp b/include/crocoddyl/multibody/impulses/impulse-6d.hpp
index 6ef3ee71bdb173e6db934d48c5647bcf00e1363e..ce800a8bf092fcb53fa0bd010e02bbc4fc2fb80b 100644
--- a/include/crocoddyl/multibody/impulses/impulse-6d.hpp
+++ b/include/crocoddyl/multibody/impulses/impulse-6d.hpp
@@ -23,7 +23,7 @@ class ImpulseModel6D : public ImpulseModelAbstract {
void calc(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const bool& recalc = true);
- void updateLagrangian(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& lambda);
+ void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& force);
boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::Data* const data);
unsigned int const& get_frame() const;
diff --git a/include/crocoddyl/multibody/impulses/multiple-impulses.hpp b/include/crocoddyl/multibody/impulses/multiple-impulses.hpp
index 6f903b777b0484d56d1572cb74a5e689789c356f..2d4d453954cca0285fa3944fa37da37f76887792 100644
--- a/include/crocoddyl/multibody/impulses/multiple-impulses.hpp
+++ b/include/crocoddyl/multibody/impulses/multiple-impulses.hpp
@@ -41,8 +41,12 @@ class ImpulseModelMultiple {
void calc(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
void calcDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const bool& recalc = true);
- void updateLagrangian(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::VectorXd& lambda);
+
+ void updateVelocity(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::VectorXd& vnext) const;
+ void updateForce(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::VectorXd& force);
boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::Data* const data);
+ void updateVelocityDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::MatrixXd& dvnext_dx) const;
+ void updateForceDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::MatrixXd& df_dq) const;
StateMultibody& get_state() const;
const ImpulseModelContainer& get_impulses() const;
@@ -71,7 +75,12 @@ struct ImpulseDataMultiple : ImpulseDataAbstract {
template <typename Model>
ImpulseDataMultiple(Model* const model, pinocchio::Data* const data)
- : ImpulseDataAbstract(model, data), fext(model->get_state().get_pinocchio().njoints, pinocchio::Force::Zero()) {
+ : ImpulseDataAbstract(model, data),
+ vnext(model->get_state().get_nv()),
+ dvnext_dx(model->get_state().get_nv(), model->get_state().get_ndx()),
+ fext(model->get_state().get_pinocchio().njoints, pinocchio::Force::Zero()) {
+ vnext.fill(0);
+ dvnext_dx.fill(0);
for (ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
it != model->get_impulses().end(); ++it) {
const ImpulseItem& item = it->second;
@@ -79,6 +88,8 @@ struct ImpulseDataMultiple : ImpulseDataAbstract {
}
}
+ Eigen::VectorXd vnext;
+ Eigen::MatrixXd dvnext_dx;
ImpulseModelMultiple::ImpulseDataContainer impulses;
pinocchio::container::aligned_vector<pinocchio::Force> fext;
};
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4f49c86a7a6212967c26dad1bc72b6fa7731030d..d12385bb97f860514c44434dcf0aa42d662ba080 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -40,6 +40,7 @@ SET(${PROJECT_NAME}_SOURCES
multibody/contacts/contact-6d.cpp
multibody/actions/free-fwddyn.cpp
multibody/actions/contact-fwddyn.cpp
+ multibody/actions/impulse-fwddyn.cpp
)
IF(UNIX)
diff --git a/src/core/action-base.cpp b/src/core/action-base.cpp
index a661bee4d312da394470dc9dcbc42c2ae7ecb45d..6147c07eb5b55daa86ad13935284af8674370f62 100644
--- a/src/core/action-base.cpp
+++ b/src/core/action-base.cpp
@@ -7,13 +7,11 @@
///////////////////////////////////////////////////////////////////////////////
#include "crocoddyl/core/action-base.hpp"
-#include <iostream>
+
namespace crocoddyl {
ActionModelAbstract::ActionModelAbstract(StateAbstract& state, unsigned int const& nu, unsigned int const& nr)
- : nu_(nu), nr_(nr), state_(state), unone_(Eigen::VectorXd::Zero(nu)) {
- assert(nu_ != 0 && "nu cannot be zero");
-}
+ : nu_(nu), nr_(nr), state_(state), unone_(Eigen::VectorXd::Zero(nu)) {}
ActionModelAbstract::~ActionModelAbstract() {}
@@ -30,19 +28,23 @@ void ActionModelAbstract::calcDiff(const boost::shared_ptr<ActionDataAbstract>&
void ActionModelAbstract::quasicStatic(const boost::shared_ptr<ActionDataAbstract>& data,
Eigen::Ref<Eigen::VectorXd> u, const Eigen::Ref<const Eigen::VectorXd>& x,
unsigned int const& maxiter, const double& tol) {
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
assert(x.size() == state_.get_nx() && "x has wrong dimension");
unsigned int const& ndx = state_.get_ndx();
Eigen::VectorXd dx = Eigen::VectorXd::Zero(ndx);
- Eigen::VectorXd du = Eigen::VectorXd::Zero(nu_);
- for (unsigned int i = 0; i < maxiter; ++i) {
- calcDiff(data, x, u);
- state_.diff(x, data->xnext, dx);
- du = -pseudoInverse(data->Fu) * data->Fx * dx;
- u += du;
- if (du.norm() <= tol) {
- break;
+ if (nu_ == 0) {
+ // TODO(cmastalli): create a method for autonomous systems
+ } else {
+ Eigen::VectorXd du = Eigen::VectorXd::Zero(nu_);
+ for (unsigned int i = 0; i < maxiter; ++i) {
+ calcDiff(data, x, u);
+ state_.diff(x, data->xnext, dx);
+ du = -pseudoInverse(data->Fu) * data->Fx * dx;
+ u += du;
+ if (du.norm() <= tol) {
+ break;
+ }
}
}
}
diff --git a/src/core/actions/diff-lqr.cpp b/src/core/actions/diff-lqr.cpp
index 11bc8a5ec033729c5567ac000ee7a782689be185..3f76233517f6a76d82226844cd5f14ecf6240044 100644
--- a/src/core/actions/diff-lqr.cpp
+++ b/src/core/actions/diff-lqr.cpp
@@ -32,8 +32,9 @@ void DifferentialActionModelLQR::calc(const boost::shared_ptr<DifferentialAction
assert(x.size() == state_.get_nx() && "x has wrong dimension");
assert(u.size() == nu_ && "u has wrong dimension");
- const Eigen::VectorXd& q = x.head(state_.get_nq());
- const Eigen::VectorXd& v = x.tail(state_.get_nv());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> q = x.head(state_.get_nq());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> v = x.tail(state_.get_nv());
+
if (drift_free_) {
data->xout = Fq_ * q + Fv_ * v + Fu_ * u;
} else {
diff --git a/src/core/actions/lqr.cpp b/src/core/actions/lqr.cpp
index 142504504ee74d34d455e421e9cb372cd0060a26..a1838b226072e6475c278a7a289a40e0d3fdd939 100644
--- a/src/core/actions/lqr.cpp
+++ b/src/core/actions/lqr.cpp
@@ -30,7 +30,7 @@ ActionModelLQR::~ActionModelLQR() {
void ActionModelLQR::calc(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
if (drift_free_) {
data->xnext = Fx_ * x + Fu_ * u;
@@ -44,7 +44,7 @@ void ActionModelLQR::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
const bool& recalc) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
if (recalc) {
calc(data, x, u);
diff --git a/src/core/diff-action-base.cpp b/src/core/diff-action-base.cpp
index 619d1f0f3e2891e977dee80d948352c9da867173..6d27152c43591e95b6193e4dcd083c8bd81ad857 100644
--- a/src/core/diff-action-base.cpp
+++ b/src/core/diff-action-base.cpp
@@ -12,10 +12,7 @@ namespace crocoddyl {
DifferentialActionModelAbstract::DifferentialActionModelAbstract(StateAbstract& state, unsigned int const& nu,
unsigned int const& nr)
- : nu_(nu), nr_(nr), state_(state), unone_(Eigen::VectorXd::Zero(nu)) {
- assert(nu_ != 0 && "nu cannot be zero");
- assert(nr_ != 0 && "nr cannot be zero");
-}
+ : nu_(nu), nr_(nr), state_(state), unone_(Eigen::VectorXd::Zero(nu)) {}
DifferentialActionModelAbstract::~DifferentialActionModelAbstract() {}
diff --git a/src/core/integrator/euler.cpp b/src/core/integrator/euler.cpp
index e0112407eaf94e47bf36fe032f75cfbe3d8f9b2f..ef0b06567dfac79b9b2e96d8b31e2047341f3ba1 100644
--- a/src/core/integrator/euler.cpp
+++ b/src/core/integrator/euler.cpp
@@ -24,7 +24,7 @@ void IntegratedActionModelEuler::calc(const boost::shared_ptr<ActionDataAbstract
const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
// Static casting the data
boost::shared_ptr<IntegratedActionDataEuler> d = boost::static_pointer_cast<IntegratedActionDataEuler>(data);
@@ -49,7 +49,7 @@ void IntegratedActionModelEuler::calcDiff(const boost::shared_ptr<ActionDataAbst
const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
const unsigned int& nv = differential_->get_state().get_nv();
if (recalc) {
@@ -72,11 +72,11 @@ void IntegratedActionModelEuler::calcDiff(const boost::shared_ptr<ActionDataAbst
}
d->Fx = d->dxnext_dx + time_step_ * d->dxnext_ddx * d->ddx_dx;
d->Fu = time_step_ * d->dxnext_ddx * d->ddx_du;
- d->Lx = d->differential->get_Lx();
- d->Lu = d->differential->get_Lu();
- d->Lxx = d->differential->get_Lxx();
- d->Lxu = d->differential->get_Lxu();
- d->Luu = d->differential->get_Luu();
+ d->Lx = d->differential->Lx;
+ d->Lu = d->differential->Lu;
+ d->Lxx = d->differential->Lxx;
+ d->Lxu = d->differential->Lxu;
+ d->Luu = d->differential->Luu;
}
boost::shared_ptr<ActionDataAbstract> IntegratedActionModelEuler::createData() {
diff --git a/src/core/numdiff/action.cpp b/src/core/numdiff/action.cpp
index e78443674607ae7406d0461f3b210016681e1954..b53aad49017ff274b8e0d035b6d23ce19b907258 100644
--- a/src/core/numdiff/action.cpp
+++ b/src/core/numdiff/action.cpp
@@ -10,11 +10,9 @@
namespace crocoddyl {
-ActionModelNumDiff::ActionModelNumDiff(ActionModelAbstract& model, bool with_gauss_approx)
+ActionModelNumDiff::ActionModelNumDiff(ActionModelAbstract& model)
: ActionModelAbstract(model.get_state(), model.get_nu(), model.get_nr()), model_(model) {
- with_gauss_approx_ = with_gauss_approx;
disturbance_ = std::sqrt(2.0 * std::numeric_limits<double>::epsilon());
- assert((!with_gauss_approx_ || nr_ > 1) && "No Gauss approximation possible with nr = 1");
}
ActionModelNumDiff::~ActionModelNumDiff() {}
@@ -22,7 +20,7 @@ ActionModelNumDiff::~ActionModelNumDiff() {}
void ActionModelNumDiff::calc(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
boost::shared_ptr<ActionDataNumDiff> data_nd = boost::static_pointer_cast<ActionDataNumDiff>(data);
model_.calc(data_nd->data_0, x, u);
data->cost = data_nd->data_0->cost;
@@ -33,7 +31,7 @@ void ActionModelNumDiff::calcDiff(const boost::shared_ptr<ActionDataAbstract>& d
const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
boost::shared_ptr<ActionDataNumDiff> data_nd = boost::static_pointer_cast<ActionDataNumDiff>(data);
if (recalc) {
@@ -58,7 +56,9 @@ void ActionModelNumDiff::calcDiff(const boost::shared_ptr<ActionDataAbstract>& d
model_.get_state().diff(xn0, xn, data_nd->Fx.col(ix));
data->Lx(ix) = (c - c0) / disturbance_;
- data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
+ if (model_.get_nr() > 0) {
+ data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
+ }
data_nd->dx(ix) = 0.0;
}
data->Fx /= disturbance_;
@@ -74,15 +74,21 @@ void ActionModelNumDiff::calcDiff(const boost::shared_ptr<ActionDataAbstract>& d
model_.get_state().diff(xn0, xn, data_nd->Fu.col(iu));
data->Lu(iu) = (c - c0) / disturbance_;
- data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
+ if (model_.get_nr() > 0) {
+ data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
+ }
data_nd->du(iu) = 0.0;
}
data->Fu /= disturbance_;
- if (with_gauss_approx_) {
+ if (model_.get_nr() > 0) {
data->Lxx = data_nd->Rx.transpose() * data_nd->Rx;
data->Lxu = data_nd->Rx.transpose() * data_nd->Ru;
data->Luu = data_nd->Ru.transpose() * data_nd->Ru;
+ } else {
+ data->Lxx.fill(0.0);
+ data->Lxu.fill(0.0);
+ data->Luu.fill(0.0);
}
}
@@ -90,36 +96,10 @@ ActionModelAbstract& ActionModelNumDiff::get_model() const { return model_; }
const double& ActionModelNumDiff::get_disturbance() const { return disturbance_; }
-bool ActionModelNumDiff::get_with_gauss_approx() { return with_gauss_approx_; }
+bool ActionModelNumDiff::get_with_gauss_approx() { return model_.get_nr() > 0; }
void ActionModelNumDiff::assertStableStateFD(const Eigen::Ref<const Eigen::VectorXd>& /** x */) {
- // TODO(mnaveau): make this method virtual and this one should do nothing, update the documentation.
- // md = model_.differential_;
- // if isinstance(md, DifferentialActionModelFloatingInContact)
- // {
- // if hasattr(md, "costs")
- // {
- // mc = md.costs;
- // if isinstance(mc, CostModelState)
- // {
- // assert (~np.isclose(model.State.diff(mc.ref, x)[3:6], np.ones(3) * np.pi, atol=1e-6).any())
- // assert (~np.isclose(model.State.diff(mc.ref, x)[3:6], -np.ones(3) * np.pi, atol=1e-6).any())
- // }
- // else if isinstance(mc, CostModelSum)
- // {
- // for (key, cost) in mc.costs.items()
- // {
- // if isinstance(cost.cost, CostModelState)
- // {
- // assert (~np.isclose(
- // model.State.diff(cost.cost.ref, x)[3:6], np.ones(3) * np.pi, atol=1e-6).any())
- // assert (~np.isclose(
- // model.State.diff(cost.cost.ref, x)[3:6], -np.ones(3) * np.pi, atol=1e-6).any())
- // }
- // }
- // }
- // }
- // }
+ // do nothing in the general case
}
boost::shared_ptr<ActionDataAbstract> ActionModelNumDiff::createData() {
diff --git a/src/core/solver-base.cpp b/src/core/solver-base.cpp
index f39d309f6a23da4ca02f86342b5797cb1f82c38d..aeaac18b76aaaacd16d8576986c78a160706c9fc 100644
--- a/src/core/solver-base.cpp
+++ b/src/core/solver-base.cpp
@@ -74,6 +74,8 @@ void SolverAbstract::setCandidate(const std::vector<Eigen::VectorXd>& xs_warm,
void SolverAbstract::setCallbacks(const std::vector<CallbackAbstract*>& callbacks) { callbacks_ = callbacks; }
+const std::vector<CallbackAbstract*>& SolverAbstract::getCallbacks() const { return callbacks_; }
+
const ShootingProblem& SolverAbstract::get_problem() const { return problem_; }
const std::vector<ActionModelAbstract*>& SolverAbstract::get_models() const { return models_; }
diff --git a/src/core/solvers/ddp.cpp b/src/core/solvers/ddp.cpp
index 0fd09beefcb92b395dfbc973f91bb6da3a60a65d..ee02437d307ed5b26234402d05167f9bf43a7abb 100644
--- a/src/core/solvers/ddp.cpp
+++ b/src/core/solvers/ddp.cpp
@@ -163,27 +163,23 @@ void SolverDDP::backwardPass() {
Vxx_.back().diagonal() += x_reg_;
}
+ if (!is_feasible_) {
+ Vx_.back().noalias() += Vxx_.back() * gaps_.back();
+ }
+
for (int t = static_cast<int>(problem_.get_T()) - 1; t >= 0; --t) {
ActionModelAbstract* m = problem_.running_models_[t];
boost::shared_ptr<ActionDataAbstract>& d = problem_.running_datas_[t];
const Eigen::MatrixXd& Vxx_p = Vxx_[t + 1];
const Eigen::VectorXd& Vx_p = Vx_[t + 1];
- const Eigen::VectorXd& gap_p = gaps_[t + 1];
FxTVxx_p_.noalias() = d->get_Fx().transpose() * Vxx_p;
FuTVxx_p_[t].noalias() = d->get_Fu().transpose() * Vxx_p;
Qxx_[t].noalias() = d->get_Lxx() + FxTVxx_p_ * d->get_Fx();
Qxu_[t].noalias() = d->get_Lxu() + FxTVxx_p_ * d->get_Fu();
Quu_[t].noalias() = d->get_Luu() + FuTVxx_p_[t] * d->get_Fu();
- if (!is_feasible_) {
- // In case the xt+1 are not f(xt,ut) i.e warm start not obtained from roll-out.
- fTVxx_p_.noalias() = Vxx_p * gap_p;
- Qx_[t].noalias() = d->get_Lx() + d->get_Fx().transpose() * Vx_p + d->get_Fx().transpose() * fTVxx_p_;
- Qu_[t].noalias() = d->get_Lu() + d->get_Fu().transpose() * Vx_p + d->get_Fu().transpose() * fTVxx_p_;
- } else {
- Qx_[t].noalias() = d->get_Lx() + d->get_Fx().transpose() * Vx_p;
- Qu_[t].noalias() = d->get_Lu() + d->get_Fu().transpose() * Vx_p;
- }
+ Qx_[t].noalias() = d->get_Lx() + d->get_Fx().transpose() * Vx_p;
+ Qu_[t].noalias() = d->get_Lu() + d->get_Fu().transpose() * Vx_p;
if (!std::isnan(ureg_)) {
unsigned int const& nu = m->get_nu();
@@ -205,6 +201,11 @@ void SolverDDP::backwardPass() {
Vxx_[t].diagonal() += x_reg_;
}
+ // Compute and store the Vx gradient at end of the interval (rollout state)
+ if (!is_feasible_) {
+ Vx_[t].noalias() += Vxx_[t] * gaps_[t];
+ }
+
if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
throw "backward_error";
}
@@ -248,11 +249,13 @@ void SolverDDP::forwardPass(const double& steplength) {
}
void SolverDDP::computeGains(const unsigned int& t) {
- Quu_llt_[t].compute(Quu_[t]);
- K_[t] = Qxu_[t].transpose();
- Quu_llt_[t].solveInPlace(K_[t]);
- k_[t] = Qu_[t];
- Quu_llt_[t].solveInPlace(k_[t]);
+ if (problem_.running_models_[t]->get_nu() > 0) {
+ Quu_llt_[t].compute(Quu_[t]);
+ K_[t] = Qxu_[t].transpose();
+ Quu_llt_[t].solveInPlace(K_[t]);
+ k_[t] = Qu_[t];
+ Quu_llt_[t].solveInPlace(k_[t]);
+ }
}
void SolverDDP::increaseRegularization() {
@@ -332,6 +335,18 @@ void SolverDDP::allocateData() {
fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
}
+const double& SolverDDP::get_regfactor() const { return regfactor_; }
+
+const double& SolverDDP::get_regmin() const { return regmin_; }
+
+const double& SolverDDP::get_regmax() const { return regmax_; }
+
+const std::vector<double>& SolverDDP::get_alphas() const { return alphas_; }
+
+const double& SolverDDP::get_th_step() const { return th_step_; }
+
+const double& SolverDDP::get_th_grad() const { return th_grad_; }
+
const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx() const { return Vxx_; }
const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx() const { return Vx_; }
@@ -352,4 +367,16 @@ const std::vector<Eigen::VectorXd>& SolverDDP::get_k() const { return k_; }
const std::vector<Eigen::VectorXd>& SolverDDP::get_gaps() const { return gaps_; }
+void SolverDDP::set_regfactor(double regfactor) { regfactor_ = regfactor; }
+
+void SolverDDP::set_regmin(double regmin) { regmin_ = regmin; }
+
+void SolverDDP::set_regmax(double regmax) { regmax_ = regmax; }
+
+void SolverDDP::set_alphas(const std::vector<double>& alphas) { alphas_ = alphas; }
+
+void SolverDDP::set_th_step(double th_step) { th_step_ = th_step; }
+
+void SolverDDP::set_th_grad(double th_grad) { th_grad_ = th_grad; }
+
} // namespace crocoddyl
diff --git a/src/core/solvers/fddp.cpp b/src/core/solvers/fddp.cpp
index 55e65b2ef28f4df4d8f115bdadc328ff75320aa6..525d59dc00eba09fada90c17626eab0c990a4cfc 100644
--- a/src/core/solvers/fddp.cpp
+++ b/src/core/solvers/fddp.cpp
@@ -10,7 +10,7 @@
namespace crocoddyl {
-SolverFDDP::SolverFDDP(ShootingProblem& problem) : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptNegStep_(2) {}
+SolverFDDP::SolverFDDP(ShootingProblem& problem) : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
SolverFDDP::~SolverFDDP() {}
@@ -60,17 +60,14 @@ bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::v
if (dVexp_ > 0) { // descend direction
if (d_[0] < th_grad_ || dV_ > th_acceptstep_ * dVexp_) {
- // Accept step
-
was_feasible_ = is_feasible_;
setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
cost_ = cost_try_;
recalc = true;
break;
}
- } else {
- if (d_[0] < th_grad_ || dV_ < th_acceptNegStep_ * dVexp_) {
- // accept step
+ } else { // reducing the gaps by allowing a small increment in the cost value
+ if (d_[0] < th_grad_ || dV_ < th_acceptnegstep_ * dVexp_) {
was_feasible_ = is_feasible_;
setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
cost_ = cost_try_;
@@ -79,6 +76,7 @@ bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::v
}
}
}
+
if (steplength_ > th_step_) {
decreaseRegularization();
}
@@ -103,37 +101,29 @@ bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::v
return false;
}
-void SolverFDDP::updateExpectedImprovement() {
- dg_ = 0;
- dq_ = 0;
- unsigned int const& T = this->problem_.get_T();
- if (!is_feasible_) {
- dg_ -= Vx_.back().transpose() * gaps_.back();
- dq_ += gaps_.back().transpose() * Vxx_.back() * gaps_.back();
- }
- for (unsigned int t = 0; t < T; ++t) {
- dg_ += Qu_[t].transpose() * k_[t];
- dq_ -= k_[t].transpose() * Quuk_[t];
- if (!is_feasible_) {
- dg_ -= Vx_[t].transpose() * gaps_[t];
- fTVxx_p_.noalias() = Vxx_[t] * gaps_[t];
- dq_ += gaps_[t].transpose() * fTVxx_p_;
- }
+void SolverFDDP::computeDirection(const bool& recalc) {
+ if (recalc) {
+ calc();
}
+ backwardPass();
+}
+
+double SolverFDDP::tryStep(const double& steplength) {
+ forwardPass(steplength);
+ return cost_ - cost_try_;
}
const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
dv_ = 0;
- d_.fill(0);
unsigned int const& T = this->problem_.get_T();
if (!is_feasible_) {
problem_.running_models_.back()->get_state().diff(xs_try_.back(), xs_.back(), dx_.back());
fTVxx_p_.noalias() = Vxx_.back() * dx_.back();
- dv_ -= gaps_.back().transpose() * fTVxx_p_;
+ dv_ -= gaps_.back().dot(fTVxx_p_);
for (unsigned int t = 0; t < T; ++t) {
problem_.running_models_[t]->get_state().diff(xs_try_[t], xs_[t], dx_[t]);
fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
- dv_ -= gaps_[t].transpose() * fTVxx_p_;
+ dv_ -= gaps_[t].dot(fTVxx_p_);
}
}
d_[0] = dg_ + dv_;
@@ -141,6 +131,26 @@ const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
return d_;
}
+void SolverFDDP::updateExpectedImprovement() {
+ dg_ = 0;
+ dq_ = 0;
+ unsigned int const& T = this->problem_.get_T();
+ if (!is_feasible_) {
+ dg_ -= Vx_.back().dot(gaps_.back());
+ fTVxx_p_.noalias() = Vxx_.back() * gaps_.back();
+ dq_ += gaps_.back().dot(fTVxx_p_);
+ }
+ for (unsigned int t = 0; t < T; ++t) {
+ dg_ += Qu_[t].dot(k_[t]);
+ dq_ -= k_[t].dot(Quuk_[t]);
+ if (!is_feasible_) {
+ dg_ -= Vx_[t].dot(gaps_[t]);
+ fTVxx_p_.noalias() = Vxx_[t] * gaps_[t];
+ dq_ += gaps_[t].dot(fTVxx_p_);
+ }
+ }
+}
+
double SolverFDDP::calc() {
cost_ = problem_.calcDiff(xs_, us_);
if (!is_feasible_) {
@@ -161,69 +171,6 @@ double SolverFDDP::calc() {
return cost_;
}
-void SolverFDDP::backwardPass() {
- boost::shared_ptr<ActionDataAbstract>& d_T = problem_.terminal_data_;
- Vxx_.back() = d_T->get_Lxx();
- Vx_.back() = d_T->get_Lx();
-
- x_reg_.fill(xreg_);
- if (!std::isnan(xreg_)) {
- Vxx_.back().diagonal() += x_reg_;
- }
-
- if (!is_feasible_) {
- Vx_.back() += Vxx_.back() * gaps_.back();
- }
-
- for (int t = static_cast<int>(problem_.get_T()) - 1; t >= 0; --t) {
- ActionModelAbstract* m = problem_.running_models_[t];
- boost::shared_ptr<ActionDataAbstract>& d = problem_.running_datas_[t];
- const Eigen::MatrixXd& Vxx_p = Vxx_[t + 1];
- const Eigen::VectorXd& Vx_p = Vx_[t + 1];
-
- FxTVxx_p_.noalias() = d->get_Fx().transpose() * Vxx_p;
- FuTVxx_p_[t].noalias() = d->get_Fu().transpose() * Vxx_p;
- Qxx_[t].noalias() = d->get_Lxx() + FxTVxx_p_ * d->get_Fx();
- Qxu_[t].noalias() = d->get_Lxu() + FxTVxx_p_ * d->get_Fu();
- Quu_[t].noalias() = d->get_Luu() + FuTVxx_p_[t] * d->get_Fu();
- Qx_[t].noalias() = d->get_Lx() + d->get_Fx().transpose() * Vx_p;
- Qu_[t].noalias() = d->get_Lu() + d->get_Fu().transpose() * Vx_p;
-
- if (!std::isnan(ureg_)) {
- unsigned int const& nu = m->get_nu();
- Quu_[t].diagonal() += Eigen::VectorXd::Constant(nu, ureg_);
- }
-
- computeGains(t);
-
- if (std::isnan(ureg_)) {
- Vx_[t].noalias() = Qx_[t] - K_[t].transpose() * Qu_[t];
- } else {
- Quuk_[t].noalias() = Quu_[t] * k_[t];
- Vx_[t].noalias() = Qx_[t] + K_[t].transpose() * Quuk_[t] - 2 * K_[t].transpose() * Qu_[t];
- }
- Vxx_[t].noalias() = Qxx_[t] - Qxu_[t] * K_[t];
- Vxx_[t] = 0.5 * (Vxx_[t] + Vxx_[t].transpose()).eval();
- // TODO(cmastalli): as suggested by Nicolas
-
- if (!std::isnan(xreg_)) {
- Vxx_[t].diagonal() += x_reg_;
- }
-
- // Compute and store the Vx gradient at end of the interval (rollout state)
- if (!is_feasible_) {
- Vx_[t].noalias() += Vxx_[t] * gaps_[t];
- }
-
- if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
- throw "backward_error";
- }
- if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
- throw "backward_error";
- }
- }
-}
-
void SolverFDDP::forwardPass(const double& steplength) {
assert(steplength <= 1. && "Step length has to be <= 1.");
assert(steplength >= 0. && "Step length has to be >= 0.");
@@ -241,12 +188,13 @@ void SolverFDDP::forwardPass(const double& steplength) {
m->get_state().diff(xs_[t], xs_try_[t], dx_[t]);
us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
m->calc(d, xs_try_[t], us_try_[t]);
+ xnext_ = d->get_xnext();
cost_try_ += d->cost;
if (raiseIfNaN(cost_try_)) {
throw "forward_error";
}
- if (raiseIfNaN(d->get_xnext().lpNorm<Eigen::Infinity>())) {
+ if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
throw "forward_error";
}
}
@@ -255,10 +203,9 @@ void SolverFDDP::forwardPass(const double& steplength) {
boost::shared_ptr<ActionDataAbstract>& d = problem_.terminal_data_;
if ((is_feasible_) || (steplength == 1)) {
- xs_try_.back() = problem_.running_datas_.back()->get_xnext();
+ xs_try_.back() = xnext_;
} else {
- m->get_state().integrate(problem_.running_datas_.back()->get_xnext(), gaps_.back() * (steplength - 1),
- xs_try_.back());
+ m->get_state().integrate(xnext_, gaps_.back() * (steplength - 1), xs_try_.back());
}
m->calc(d, xs_try_.back());
cost_try_ += d->cost;
@@ -268,4 +215,8 @@ void SolverFDDP::forwardPass(const double& steplength) {
}
}
+double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
+
+void SolverFDDP::set_th_acceptnegstep(double th_acceptnegstep) { th_acceptnegstep_ = th_acceptnegstep; }
+
} // namespace crocoddyl
diff --git a/src/multibody/actions/contact-fwddyn.cpp b/src/multibody/actions/contact-fwddyn.cpp
index e75681d31ea118e6e6a87471fbd86ed60c2e4403..0218ffcd5e50b285b035efc7ac58baded30225c7 100644
--- a/src/multibody/actions/contact-fwddyn.cpp
+++ b/src/multibody/actions/contact-fwddyn.cpp
@@ -40,11 +40,11 @@ void DifferentialActionModelContactFwdDynamics::calc(const boost::shared_ptr<Dif
assert(u.size() == nu_ && "u has wrong dimension");
DifferentialActionDataContactFwdDynamics* d = static_cast<DifferentialActionDataContactFwdDynamics*>(data.get());
- d->qcur = x.head(state_.get_nq());
- d->vcur = x.tail(state_.get_nv());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> q = x.head(state_.get_nq());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> v = x.tail(state_.get_nv());
// Computing the forward dynamics with the holonomic constraints defined by the contact model
- pinocchio::computeAllTerms(pinocchio_, d->pinocchio, d->qcur, d->vcur);
+ pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
pinocchio::updateFramePlacements(pinocchio_, d->pinocchio);
if (!with_armature_) {
@@ -61,10 +61,11 @@ void DifferentialActionModelContactFwdDynamics::calc(const boost::shared_ptr<Dif
}
#endif
- pinocchio::forwardDynamics(pinocchio_, d->pinocchio, d->qcur, d->vcur, d->actuation->a, d->contacts->Jc,
- d->contacts->a0, JMinvJt_damping_, false);
+ pinocchio::forwardDynamics(pinocchio_, d->pinocchio, q, v, d->actuation->tau, d->contacts->Jc, d->contacts->a0,
+ JMinvJt_damping_, false);
d->xout = d->pinocchio.ddq;
- contacts_.updateLagrangian(d->contacts, d->pinocchio.lambda_c);
+ contacts_.updateAcceleration(d->contacts, d->pinocchio.ddq);
+ contacts_.updateForce(d->contacts, d->pinocchio.lambda_c);
// Computing the cost value and residuals
costs_.calc(d->costs, x, u);
@@ -78,18 +79,18 @@ void DifferentialActionModelContactFwdDynamics::calcDiff(const boost::shared_ptr
assert(x.size() == state_.get_nx() && "x has wrong dimension");
assert(u.size() == nu_ && "u has wrong dimension");
- DifferentialActionDataContactFwdDynamics* d = static_cast<DifferentialActionDataContactFwdDynamics*>(data.get());
unsigned int const& nv = state_.get_nv();
unsigned int const& nc = contacts_.get_nc();
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> q = x.head(state_.get_nq());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> v = x.tail(nv);
+
+ DifferentialActionDataContactFwdDynamics* d = static_cast<DifferentialActionDataContactFwdDynamics*>(data.get());
if (recalc) {
calc(data, x, u);
- } else {
- d->qcur = x.head(state_.get_nq());
- d->vcur = x.tail(nv);
}
// Computing the dynamics derivatives
- pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, d->qcur, d->vcur, d->xout, d->contacts->fext);
+ pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout, d->contacts->fext);
pinocchio::getKKTContactDynamicMatrixInverse(pinocchio_, d->pinocchio, d->contacts->Jc, d->Kinv);
actuation_.calcDiff(d->actuation, x, u, false);
@@ -102,18 +103,19 @@ void DifferentialActionModelContactFwdDynamics::calcDiff(const boost::shared_ptr
d->Fx.leftCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
d->Fx.rightCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dv;
- d->Fx.noalias() -= a_partial_da * d->contacts->Ax;
- d->Fx.noalias() += a_partial_dtau * d->actuation->Ax;
- d->Fu.noalias() = a_partial_dtau * d->actuation->Au;
+ d->Fx.noalias() -= a_partial_da * d->contacts->da0_dx;
+ d->Fx.noalias() += a_partial_dtau * d->actuation->dtau_dx;
+ d->Fu.noalias() = a_partial_dtau * d->actuation->dtau_du;
// Computing the cost derivatives
if (enable_force_) {
- d->Gx.leftCols(nv).noalias() = f_partial_dtau * d->pinocchio.dtau_dq;
- d->Gx.rightCols(nv).noalias() = f_partial_dtau * d->pinocchio.dtau_dv;
- d->Gx.noalias() += f_partial_da * d->contacts->Ax;
- d->Gx.noalias() -= f_partial_dtau * d->actuation->Ax;
- d->Gu.noalias() = -f_partial_dtau * d->actuation->Au;
- contacts_.updateLagrangianDiff(d->contacts, d->Gx, d->Gu);
+ d->df_dx.leftCols(nv).noalias() = f_partial_dtau * d->pinocchio.dtau_dq;
+ d->df_dx.rightCols(nv).noalias() = f_partial_dtau * d->pinocchio.dtau_dv;
+ d->df_dx.noalias() += f_partial_da * d->contacts->da0_dx;
+ d->df_dx.noalias() -= f_partial_dtau * d->actuation->dtau_dx;
+ d->df_du.noalias() = -f_partial_dtau * d->actuation->dtau_du;
+ contacts_.updateAccelerationDiff(d->contacts, d->Fx.bottomRows(nv));
+ contacts_.updateForceDiff(d->contacts, d->df_dx, d->df_du);
}
costs_.calcDiff(d->costs, x, u, false);
}
diff --git a/src/multibody/actions/free-fwddyn.cpp b/src/multibody/actions/free-fwddyn.cpp
index ac3d7b9ca3b1e3072485a395a8ec298c2098bf11..ce20707e34251de74b35319e8b4e69f2f6c01ff3 100644
--- a/src/multibody/actions/free-fwddyn.cpp
+++ b/src/multibody/actions/free-fwddyn.cpp
@@ -35,14 +35,14 @@ void DifferentialActionModelFreeFwdDynamics::calc(const boost::shared_ptr<Differ
assert(u.size() == nu_ && "u has wrong dimension");
DifferentialActionDataFreeFwdDynamics* d = static_cast<DifferentialActionDataFreeFwdDynamics*>(data.get());
- d->qcur = x.head(state_.get_nq());
- d->vcur = x.tail(state_.get_nv());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> q = x.head(state_.get_nq());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> v = x.tail(state_.get_nv());
// Computing the dynamics using ABA or manually for armature case
if (with_armature_) {
- d->xout = pinocchio::aba(pinocchio_, d->pinocchio, d->qcur, d->vcur, u);
+ d->xout = pinocchio::aba(pinocchio_, d->pinocchio, q, v, u);
} else {
- pinocchio::computeAllTerms(pinocchio_, d->pinocchio, d->qcur, d->vcur);
+ pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
d->pinocchio.M.diagonal() += armature_;
pinocchio::cholesky::decompose(pinocchio_, d->pinocchio);
d->Minv.setZero();
@@ -52,7 +52,7 @@ void DifferentialActionModelFreeFwdDynamics::calc(const boost::shared_ptr<Differ
}
// Computing the cost value and residuals
- pinocchio::forwardKinematics(pinocchio_, d->pinocchio, d->qcur, d->vcur);
+ pinocchio::forwardKinematics(pinocchio_, d->pinocchio, q, v);
pinocchio::updateFramePlacements(pinocchio_, d->pinocchio);
costs_.calc(d->costs, x, u);
d->cost = d->costs->cost;
@@ -64,24 +64,24 @@ void DifferentialActionModelFreeFwdDynamics::calcDiff(const boost::shared_ptr<Di
assert(x.size() == state_.get_nx() && "x has wrong dimension");
assert(u.size() == nu_ && "u has wrong dimension");
- DifferentialActionDataFreeFwdDynamics* d = static_cast<DifferentialActionDataFreeFwdDynamics*>(data.get());
const unsigned int& nv = state_.get_nv();
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> q = x.head(state_.get_nq());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> v = x.tail(nv);
+
+ DifferentialActionDataFreeFwdDynamics* d = static_cast<DifferentialActionDataFreeFwdDynamics*>(data.get());
if (recalc) {
calc(data, x, u);
- pinocchio::computeJointJacobians(pinocchio_, d->pinocchio, d->qcur);
- } else {
- d->qcur = x.head(state_.get_nq());
- d->vcur = x.tail(nv);
+ pinocchio::computeJointJacobians(pinocchio_, d->pinocchio, q);
}
// Computing the dynamics derivatives
if (with_armature_) {
- pinocchio::computeABADerivatives(pinocchio_, d->pinocchio, d->qcur, d->vcur, u);
+ pinocchio::computeABADerivatives(pinocchio_, d->pinocchio, q, v, u);
d->Fx.leftCols(nv) = d->pinocchio.ddq_dq;
d->Fx.rightCols(nv) = d->pinocchio.ddq_dv;
d->Fu = d->pinocchio.Minv;
} else {
- pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, d->qcur, d->vcur, d->xout);
+ pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout);
d->Fx.leftCols(nv).noalias() = d->Minv * d->pinocchio.dtau_dq;
d->Fx.leftCols(nv) *= -1.;
d->Fx.rightCols(nv).noalias() = d->Minv * d->pinocchio.dtau_dv;
diff --git a/src/multibody/actions/impulse-fwddyn.cpp b/src/multibody/actions/impulse-fwddyn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f43c91f23452377a09535d2d35716aac65502884
--- /dev/null
+++ b/src/multibody/actions/impulse-fwddyn.cpp
@@ -0,0 +1,149 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
+#include <pinocchio/algorithm/compute-all-terms.hpp>
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/contact-dynamics.hpp>
+#include <pinocchio/algorithm/rnea-derivatives.hpp>
+#include <pinocchio/algorithm/kinematics-derivatives.hpp>
+
+namespace crocoddyl {
+
+ActionModelImpulseFwdDynamics::ActionModelImpulseFwdDynamics(StateMultibody& state, ImpulseModelMultiple& impulses,
+ CostModelSum& costs, const double& r_coeff,
+ const double& JMinvJt_damping, const bool& enable_force)
+ : ActionModelAbstract(state, 0, costs.get_nr()),
+ impulses_(impulses),
+ costs_(costs),
+ pinocchio_(state.get_pinocchio()),
+ with_armature_(true),
+ armature_(Eigen::VectorXd::Zero(state.get_nv())),
+ r_coeff_(r_coeff),
+ JMinvJt_damping_(JMinvJt_damping),
+ enable_force_(enable_force),
+ gravity_(state.get_pinocchio().gravity) {}
+
+ActionModelImpulseFwdDynamics::~ActionModelImpulseFwdDynamics() {}
+
+void ActionModelImpulseFwdDynamics::calc(const boost::shared_ptr<ActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u) {
+ assert(x.size() == state_.get_nx() && "x has wrong dimension");
+
+ unsigned int const& nq = state_.get_nq();
+ unsigned int const& nv = state_.get_nv();
+ ActionDataImpulseFwdDynamics* d = static_cast<ActionDataImpulseFwdDynamics*>(data.get());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> q = x.head(nq);
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> v = x.tail(nv);
+
+ // Computing the forward dynamics with the holonomic constraints defined by the contact model
+ pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
+ pinocchio::updateFramePlacements(pinocchio_, d->pinocchio);
+
+ if (!with_armature_) {
+ d->pinocchio.M.diagonal() += armature_;
+ }
+ impulses_.calc(d->impulses, x);
+
+#ifndef NDEBUG
+ Eigen::FullPivLU<Eigen::MatrixXd> Jc_lu(d->impulses->Jc);
+
+ if (Jc_lu.rank() < d->impulses->Jc.rows()) {
+ assert(JMinvJt_damping_ > 0. && "It is needed a damping factor since the contact Jacobian is not full-rank");
+ }
+#endif
+
+ pinocchio::impulseDynamics(pinocchio_, d->pinocchio, q, v, d->impulses->Jc, r_coeff_, false);
+ d->xnext.head(nq) = q;
+ d->xnext.tail(nv) = d->pinocchio.dq_after;
+ impulses_.updateVelocity(d->impulses, d->pinocchio.dq_after);
+ impulses_.updateForce(d->impulses, d->pinocchio.impulse_c);
+
+ // Computing the cost value and residuals
+ costs_.calc(d->costs, x, u);
+ d->cost = d->costs->cost;
+}
+
+void ActionModelImpulseFwdDynamics::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
+ assert(x.size() == state_.get_nx() && "x has wrong dimension");
+
+ unsigned int const& nv = state_.get_nv();
+ unsigned int const& ni = impulses_.get_ni();
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> q = x.head(state_.get_nq());
+ const Eigen::VectorBlock<const Eigen::Ref<const Eigen::VectorXd>, Eigen::Dynamic> v = x.tail(nv);
+
+ ActionDataImpulseFwdDynamics* d = static_cast<ActionDataImpulseFwdDynamics*>(data.get());
+ if (recalc) {
+ calc(data, x, u);
+ }
+
+ // Computing the dynamics derivatives
+ pinocchio_.gravity.setZero();
+ pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, d->vnone, d->pinocchio.dq_after - v,
+ d->impulses->fext);
+ pinocchio_.gravity = gravity_;
+ pinocchio::getKKTContactDynamicMatrixInverse(pinocchio_, d->pinocchio, d->impulses->Jc, d->Kinv);
+
+ pinocchio::computeForwardKinematicsDerivatives(pinocchio_, d->pinocchio, q, d->pinocchio.dq_after, d->vnone);
+ impulses_.calcDiff(d->impulses, x, false);
+
+ Eigen::Block<Eigen::MatrixXd> a_partial_dtau = d->Kinv.topLeftCorner(nv, nv);
+ Eigen::Block<Eigen::MatrixXd> a_partial_da = d->Kinv.topRightCorner(nv, ni);
+ Eigen::Block<Eigen::MatrixXd> f_partial_dtau = d->Kinv.bottomLeftCorner(ni, nv);
+ Eigen::Block<Eigen::MatrixXd> f_partial_da = d->Kinv.bottomRightCorner(ni, ni);
+
+ d->Fx.topLeftCorner(nv, nv).setIdentity();
+ d->Fx.topRightCorner(nv, nv).setZero();
+ d->Fx.bottomLeftCorner(nv, nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
+ d->Fx.bottomLeftCorner(nv, nv).noalias() -= a_partial_da * d->impulses->dv0_dq;
+ d->Fx.bottomRightCorner(nv, nv).noalias() = a_partial_dtau * d->pinocchio.M.selfadjointView<Eigen::Upper>();
+
+ // Computing the cost derivatives
+ if (enable_force_) {
+ d->df_dq.noalias() = f_partial_dtau * d->pinocchio.dtau_dq;
+ d->df_dq.noalias() += f_partial_da * d->impulses->dv0_dq;
+ impulses_.updateVelocityDiff(d->impulses, d->Fx.bottomRows(nv));
+ impulses_.updateForceDiff(d->impulses, d->df_dq);
+ }
+ costs_.calcDiff(d->costs, x, u, false);
+}
+
+boost::shared_ptr<ActionDataAbstract> ActionModelImpulseFwdDynamics::createData() {
+ return boost::make_shared<ActionDataImpulseFwdDynamics>(this);
+}
+
+pinocchio::Model& ActionModelImpulseFwdDynamics::get_pinocchio() const { return pinocchio_; }
+
+ImpulseModelMultiple& ActionModelImpulseFwdDynamics::get_impulses() const { return impulses_; }
+
+CostModelSum& ActionModelImpulseFwdDynamics::get_costs() const { return costs_; }
+
+const Eigen::VectorXd& ActionModelImpulseFwdDynamics::get_armature() const { return armature_; }
+
+const double& ActionModelImpulseFwdDynamics::get_restitution_coefficient() const { return r_coeff_; }
+
+const double& ActionModelImpulseFwdDynamics::get_damping_factor() const { return JMinvJt_damping_; }
+
+void ActionModelImpulseFwdDynamics::set_armature(const Eigen::VectorXd& armature) {
+ assert(armature.size() == state_.get_nv() && "The armature dimension is wrong, we cannot set it.");
+ if (armature.size() != state_.get_nv()) {
+ std::cout << "The armature dimension is wrong, we cannot set it." << std::endl;
+ } else {
+ armature_ = armature;
+ with_armature_ = false;
+ }
+}
+
+void ActionModelImpulseFwdDynamics::set_restitution_coefficient(const double& r_coeff) { r_coeff_ = r_coeff; }
+
+void ActionModelImpulseFwdDynamics::set_damping_factor(const double& damping) { JMinvJt_damping_ = damping; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/actuations/floating-base.cpp b/src/multibody/actuations/floating-base.cpp
index 9389f107c1d831f3bcd31fb3c5b4db2b36b90cb5..c30168857946338f11b3a657bfba00828e99986d 100644
--- a/src/multibody/actuations/floating-base.cpp
+++ b/src/multibody/actuations/floating-base.cpp
@@ -26,7 +26,7 @@ void ActuationModelFloatingBase::calc(const boost::shared_ptr<ActuationDataAbstr
const Eigen::Ref<const Eigen::VectorXd>&,
const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(u.size() == nu_ && "u has wrong dimension");
- data->a.tail(nu_) = u;
+ data->tau.tail(nu_) = u;
}
void ActuationModelFloatingBase::calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data,
@@ -36,16 +36,16 @@ void ActuationModelFloatingBase::calcDiff(const boost::shared_ptr<ActuationDataA
calc(data, x, u);
}
// The derivatives has constant values which were set in createData.
- assert(data->Ax == Eigen::MatrixXd::Zero(state_.get_nv(), state_.get_ndx()) && "Ax has wrong value");
- assert(data->Au == Au_ && "Au has wrong value");
+ assert(data->dtau_dx == Eigen::MatrixXd::Zero(state_.get_nv(), state_.get_ndx()) && "dtau_dx has wrong value");
+ assert(data->dtau_du == dtau_du_ && "dtau_du has wrong value");
}
boost::shared_ptr<ActuationDataAbstract> ActuationModelFloatingBase::createData() {
boost::shared_ptr<ActuationDataAbstract> data = boost::make_shared<ActuationDataAbstract>(this);
- data->Au.diagonal(-6).fill(1.);
+ data->dtau_du.diagonal(-6).fill(1.);
#ifndef NDEBUG
- Au_ = data->Au;
+ dtau_du_ = data->dtau_du;
#endif
return data;
diff --git a/src/multibody/actuations/full.cpp b/src/multibody/actuations/full.cpp
index d7e02c5e13ef31b26322052eb432246010f431a9..adf65fbf0ac2946e9d8d84e78790485fe6a94b02 100644
--- a/src/multibody/actuations/full.cpp
+++ b/src/multibody/actuations/full.cpp
@@ -24,7 +24,7 @@ ActuationModelFull::~ActuationModelFull() {}
void ActuationModelFull::calc(const boost::shared_ptr<ActuationDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(u.size() == nu_ && "u has wrong dimension");
- data->a = u;
+ data->tau = u;
}
void ActuationModelFull::calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data,
@@ -34,13 +34,13 @@ void ActuationModelFull::calcDiff(const boost::shared_ptr<ActuationDataAbstract>
calc(data, x, u);
}
// The derivatives has constant values which were set in createData.
- assert(data->Ax == Eigen::MatrixXd::Zero(state_.get_nv(), state_.get_ndx()) && "Ax has wrong value");
- assert(data->Au == Eigen::MatrixXd::Identity(state_.get_nv(), nu_) && "Au has wrong value");
+ assert(data->dtau_dx == Eigen::MatrixXd::Zero(state_.get_nv(), state_.get_ndx()) && "dtau_dx has wrong value");
+ assert(data->dtau_du == Eigen::MatrixXd::Identity(state_.get_nv(), nu_) && "dtau_du has wrong value");
}
boost::shared_ptr<ActuationDataAbstract> ActuationModelFull::createData() {
boost::shared_ptr<ActuationDataAbstract> data = boost::make_shared<ActuationDataAbstract>(this);
- data->Au.diagonal().fill(1);
+ data->dtau_du.diagonal().fill(1);
return data;
}
diff --git a/src/multibody/contact-base.cpp b/src/multibody/contact-base.cpp
index d8494bd51ecc6778ebb9199e137826d90053048c..b4ea2b008110f3e2ecb8620d0b952c56134f00ff 100644
--- a/src/multibody/contact-base.cpp
+++ b/src/multibody/contact-base.cpp
@@ -18,12 +18,12 @@ ContactModelAbstract::ContactModelAbstract(StateMultibody& state, unsigned int c
ContactModelAbstract::~ContactModelAbstract() {}
-void ContactModelAbstract::updateLagrangianDiff(const boost::shared_ptr<ContactDataAbstract>& data,
- const Eigen::MatrixXd& Gx, const Eigen::MatrixXd& Gu) {
- assert((Gx.rows() == nc_ || Gx.cols() == state_.get_nx()) && "Gx has wrong dimension");
- assert((Gu.rows() == nc_ || Gu.cols() == nu_) && "Gu has wrong dimension");
- data->Gx = Gx;
- data->Gu = Gu;
+void ContactModelAbstract::updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data,
+ const Eigen::MatrixXd& df_dx, const Eigen::MatrixXd& df_du) const {
+ assert((df_dx.rows() == nc_ || df_dx.cols() == state_.get_nx()) && "df_dx has wrong dimension");
+ assert((df_du.rows() == nc_ || df_du.cols() == nu_) && "df_du has wrong dimension");
+ data->df_dx = df_dx;
+ data->df_du = df_du;
}
boost::shared_ptr<ContactDataAbstract> ContactModelAbstract::createData(pinocchio::Data* const data) {
diff --git a/src/multibody/contacts/contact-3d.cpp b/src/multibody/contacts/contact-3d.cpp
index edaf95cc3d7916d7f70b3e4975ce37e76c77bf48..3254ec4bac309bcb60bef46861d21608972654fc 100644
--- a/src/multibody/contacts/contact-3d.cpp
+++ b/src/multibody/contacts/contact-3d.cpp
@@ -57,25 +57,25 @@ void ContactModel3D::calcDiff(const boost::shared_ptr<ContactDataAbstract>& data
d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
- d->Ax.leftCols(nv).noalias() =
+ d->da0_dx.leftCols(nv).noalias() =
d->fXjda_dq.topRows<3>() + d->vw_skew * d->fXjdv_dq.topRows<3>() - d->vv_skew * d->fXjdv_dq.bottomRows<3>();
- d->Ax.rightCols(nv).noalias() = d->fXjda_dv.topRows<3>() + d->vw_skew * d->Jc - d->vv_skew * d->fJf.bottomRows<3>();
+ d->da0_dx.rightCols(nv).noalias() =
+ d->fXjda_dv.topRows<3>() + d->vw_skew * d->Jc - d->vv_skew * d->fJf.bottomRows<3>();
if (gains_[0] != 0.) {
d->oRf = d->pinocchio->oMf[xref_.frame].rotation();
- d->Ax.leftCols(nv).noalias() += gains_[0] * d->oRf * d->Jc;
+ d->da0_dx.leftCols(nv).noalias() += gains_[0] * d->oRf * d->Jc;
}
if (gains_[1] != 0.) {
- d->Ax.leftCols(nv).noalias() += gains_[1] * d->fXj.topRows<3>() * d->v_partial_dq;
- d->Ax.rightCols(nv).noalias() += gains_[1] * d->fXj.topRows<3>() * d->a_partial_da;
+ d->da0_dx.leftCols(nv).noalias() += gains_[1] * d->fXj.topRows<3>() * d->v_partial_dq;
+ d->da0_dx.rightCols(nv).noalias() += gains_[1] * d->fXj.topRows<3>() * d->a_partial_da;
}
}
-void ContactModel3D::updateLagrangian(const boost::shared_ptr<ContactDataAbstract>& data,
- const Eigen::VectorXd& lambda) {
- assert(lambda.size() == 3 && "lambda has wrong dimension, it should be 3d vector");
+void ContactModel3D::updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& force) {
+ assert(force.size() == 3 && "lambda has wrong dimension, it should be 3d vector");
ContactData3D* d = static_cast<ContactData3D*>(data.get());
- data->f = d->jMf.act(pinocchio::Force(lambda, Eigen::Vector3d::Zero()));
+ data->f = d->jMf.act(pinocchio::Force(force, Eigen::Vector3d::Zero()));
}
boost::shared_ptr<ContactDataAbstract> ContactModel3D::createData(pinocchio::Data* const data) {
diff --git a/src/multibody/contacts/contact-6d.cpp b/src/multibody/contacts/contact-6d.cpp
index 65e0f63b118a00f56f90774b7b78ab0e8bee411b..36d7b00f087f258d11f06b1dde8d821d04160033 100644
--- a/src/multibody/contacts/contact-6d.cpp
+++ b/src/multibody/contacts/contact-6d.cpp
@@ -51,24 +51,23 @@ void ContactModel6D::calcDiff(const boost::shared_ptr<ContactDataAbstract>& data
pinocchio::getJointAccelerationDerivatives(state_.get_pinocchio(), *d->pinocchio, d->joint, pinocchio::LOCAL,
d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
unsigned int const& nv = state_.get_nv();
- d->Ax.leftCols(nv).noalias() = d->fXj * d->a_partial_dq;
- d->Ax.rightCols(nv).noalias() = d->fXj * d->a_partial_dv;
+ d->da0_dx.leftCols(nv).noalias() = d->fXj * d->a_partial_dq;
+ d->da0_dx.rightCols(nv).noalias() = d->fXj * d->a_partial_dv;
if (gains_[0] != 0.) {
pinocchio::Jlog6(d->rMf, d->rMf_Jlog6);
- d->Ax.leftCols(nv).noalias() += gains_[0] * d->rMf_Jlog6 * d->Jc;
+ d->da0_dx.leftCols(nv).noalias() += gains_[0] * d->rMf_Jlog6 * d->Jc;
}
if (gains_[1] != 0.) {
- d->Ax.leftCols(nv).noalias() += gains_[1] * d->fXj * d->v_partial_dq;
- d->Ax.rightCols(nv).noalias() += gains_[1] * d->fXj * d->a_partial_da;
+ d->da0_dx.leftCols(nv).noalias() += gains_[1] * d->fXj * d->v_partial_dq;
+ d->da0_dx.rightCols(nv).noalias() += gains_[1] * d->fXj * d->a_partial_da;
}
}
-void ContactModel6D::updateLagrangian(const boost::shared_ptr<ContactDataAbstract>& data,
- const Eigen::VectorXd& lambda) {
- assert(lambda.size() == 6 && "lambda has wrong dimension, it should be 6d vector");
+void ContactModel6D::updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& force) {
+ assert(force.size() == 6 && "force has wrong dimension, it should be 6d vector");
ContactData6D* d = static_cast<ContactData6D*>(data.get());
- data->f = d->jMf.act(pinocchio::Force(lambda));
+ data->f = d->jMf.act(pinocchio::Force(force));
}
boost::shared_ptr<ContactDataAbstract> ContactModel6D::createData(pinocchio::Data* const data) {
diff --git a/src/multibody/contacts/multiple-contacts.cpp b/src/multibody/contacts/multiple-contacts.cpp
index 441760c13beafc8757a85744785188e2715edb99..616edb415fe208f8481a272506b0dbfb61b64957 100644
--- a/src/multibody/contacts/multiple-contacts.cpp
+++ b/src/multibody/contacts/multiple-contacts.cpp
@@ -79,14 +79,21 @@ void ContactModelMultiple::calcDiff(const boost::shared_ptr<ContactDataMultiple>
m_i.contact->calcDiff(d_i, x, false);
unsigned int const& nc_i = m_i.contact->get_nc();
- data->Ax.block(nc, 0, nc_i, ndx) = d_i->Ax;
+ data->da0_dx.block(nc, 0, nc_i, ndx) = d_i->da0_dx;
nc += nc_i;
}
}
-void ContactModelMultiple::updateLagrangian(const boost::shared_ptr<ContactDataMultiple>& data,
- const Eigen::VectorXd& lambda) {
- assert(lambda.size() == nc_ && "lambda has wrong dimension, it should be nc vector");
+void ContactModelMultiple::updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data,
+ const Eigen::VectorXd& dv) const {
+ assert(dv.size() == state_.get_nv() && "dv has wrong dimension");
+
+ data->dv = dv;
+}
+
+void ContactModelMultiple::updateForce(const boost::shared_ptr<ContactDataMultiple>& data,
+ const Eigen::VectorXd& force) {
+ assert(force.size() == nc_ && "force has wrong dimension, it should be nc vector");
assert(data->contacts.size() == contacts_.size() && "it doesn't match the number of contact datas and models");
unsigned int nc = 0;
@@ -103,32 +110,40 @@ void ContactModelMultiple::updateLagrangian(const boost::shared_ptr<ContactDataM
assert(it_m->first == it_d->first && "it doesn't match the contact name between data and model");
unsigned int const& nc_i = m_i.contact->get_nc();
- m_i.contact->updateLagrangian(d_i, lambda.segment(nc, nc_i));
+ const Eigen::VectorBlock<const Eigen::VectorXd, Eigen::Dynamic> force_i = force.segment(nc, nc_i);
+ m_i.contact->updateForce(d_i, force_i);
data->fext[d_i->joint] = d_i->f;
nc += nc_i;
}
}
-void ContactModelMultiple::updateLagrangianDiff(const boost::shared_ptr<ContactDataMultiple>& data,
- const Eigen::MatrixXd& Gx, const Eigen::MatrixXd& Gu) {
+void ContactModelMultiple::updateAccelerationDiff(const boost::shared_ptr<ContactDataMultiple>& data,
+ const Eigen::MatrixXd& ddv_dx) const {
+ assert((ddv_dx.rows() == state_.get_nv() && ddv_dx.cols() == state_.get_ndx()) && "ddv_dx has wrong dimension");
+
+ data->ddv_dx = ddv_dx;
+}
+
+void ContactModelMultiple::updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data,
+ const Eigen::MatrixXd& df_dx, const Eigen::MatrixXd& df_du) const {
unsigned int const& ndx = state_.get_ndx();
- assert((Gx.rows() == nc_ || Gx.cols() == ndx) && "Gx has wrong dimension");
- assert((Gu.rows() == nc_ || Gu.cols() == nu_) && "Gu has wrong dimension");
+ assert((df_dx.rows() == nc_ || df_dx.cols() == ndx) && "df_dx has wrong dimension");
+ assert((df_du.rows() == nc_ || df_du.cols() == nu_) && "df_du has wrong dimension");
assert(data->contacts.size() == contacts_.size() && "it doesn't match the number of contact datas and models");
unsigned int nc = 0;
- ContactModelContainer::iterator it_m, end_m;
- ContactDataContainer::iterator it_d, end_d;
+ ContactModelContainer::const_iterator it_m, end_m;
+ ContactDataContainer::const_iterator it_d, end_d;
for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
const ContactItem& m_i = it_m->second;
- boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
+ const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
assert(it_m->first == it_d->first && "it doesn't match the contact name between data and model");
unsigned int const& nc_i = m_i.contact->get_nc();
- const Eigen::Block<const Eigen::MatrixXd> Gx_i = Gx.block(nc, 0, nc_i, ndx);
- const Eigen::Block<const Eigen::MatrixXd> Gu_i = Gu.block(nc, 0, nc_i, nu_);
- m_i.contact->updateLagrangianDiff(d_i, Gx_i, Gu_i);
+ const Eigen::Block<const Eigen::MatrixXd> df_dx_i = df_dx.block(nc, 0, nc_i, ndx);
+ const Eigen::Block<const Eigen::MatrixXd> df_du_i = df_du.block(nc, 0, nc_i, nu_);
+ m_i.contact->updateForceDiff(d_i, df_dx_i, df_du_i);
nc += nc_i;
}
}
diff --git a/src/multibody/costs/control.cpp b/src/multibody/costs/control.cpp
index 0349c86695b1c808a24905614de01f706b56245d..ca5fc1abef84cb02cc3285ff7840795c39dd326b 100644
--- a/src/multibody/costs/control.cpp
+++ b/src/multibody/costs/control.cpp
@@ -37,6 +37,7 @@ CostModelControl::~CostModelControl() {}
void CostModelControl::calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>&,
const Eigen::Ref<const Eigen::VectorXd>& u) {
+ assert(nu_ != 0 && "it seems to be an autonomous system, if so, don't add this cost function");
assert(u.size() == nu_ && "u has wrong dimension");
data->r = u - uref_;
@@ -47,6 +48,7 @@ void CostModelControl::calc(const boost::shared_ptr<CostDataAbstract>& data, con
void CostModelControl::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
const bool& recalc) {
+ assert(nu_ != 0 && "it seems to be an autonomous system, if so, don't add this cost function");
assert(u.size() == nu_ && "u has wrong dimension");
if (recalc) {
diff --git a/src/multibody/costs/cost-sum.cpp b/src/multibody/costs/cost-sum.cpp
index 4d7bcc1b73d59d82fc42cd22df0856b0f1c096a0..b2ccd7c940af30aa61b59275dc4b7d9e78dba974 100644
--- a/src/multibody/costs/cost-sum.cpp
+++ b/src/multibody/costs/cost-sum.cpp
@@ -42,7 +42,7 @@ void CostModelSum::removeCost(const std::string& name) {
void CostModelSum::calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
assert(data->costs.size() == costs_.size() && "it doesn't match the number of cost datas and models");
data->cost = 0.;
unsigned int nr = 0;
@@ -68,7 +68,7 @@ void CostModelSum::calc(const boost::shared_ptr<CostDataSum>& data, const Eigen:
void CostModelSum::calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
assert(x.size() == state_.get_nx() && "x has wrong dimension");
- assert(u.size() == nu_ && "u has wrong dimension");
+ assert((u.size() == nu_ || nu_ == 0) && "u has wrong dimension");
assert(data->costs.size() == costs_.size() && "it doesn't match the number of cost datas and models");
if (recalc) {
calc(data, x, u);
diff --git a/src/multibody/impulse-base.cpp b/src/multibody/impulse-base.cpp
index 4c7700347339b24a540b30c387b84dc781262ea6..a12e490cab0edf0de9a2fbfd535772d816596247 100644
--- a/src/multibody/impulse-base.cpp
+++ b/src/multibody/impulse-base.cpp
@@ -14,6 +14,12 @@ ImpulseModelAbstract::ImpulseModelAbstract(StateMultibody& state, unsigned int c
ImpulseModelAbstract::~ImpulseModelAbstract() {}
+void ImpulseModelAbstract::updateForceDiff(const boost::shared_ptr<ImpulseDataAbstract>& data,
+ const Eigen::MatrixXd& df_dq) const {
+ assert((df_dq.rows() == ni_ || df_dq.cols() == state_.get_nv()) && "df_dq has wrong dimension");
+ data->df_dq = df_dq;
+}
+
boost::shared_ptr<ImpulseDataAbstract> ImpulseModelAbstract::createData(pinocchio::Data* const data) {
return boost::make_shared<ImpulseDataAbstract>(this, data);
}
diff --git a/src/multibody/impulses/impulse-3d.cpp b/src/multibody/impulses/impulse-3d.cpp
index 99264d76b05f5fdeb4abab5cd53c7af4360d22fa..865812f28f064be67798e9097b07f6047713a039 100644
--- a/src/multibody/impulses/impulse-3d.cpp
+++ b/src/multibody/impulses/impulse-3d.cpp
@@ -34,14 +34,13 @@ void ImpulseModel3D::calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data
ImpulseData3D* d = static_cast<ImpulseData3D*>(data.get());
pinocchio::getJointVelocityDerivatives(state_.get_pinocchio(), *d->pinocchio, d->joint, pinocchio::LOCAL,
d->v_partial_dq, d->v_partial_dv);
- d->Vq.noalias() = d->fXj.topRows<3>() * d->v_partial_dq;
+ d->dv0_dq.noalias() = d->fXj.topRows<3>() * d->v_partial_dq;
}
-void ImpulseModel3D::updateLagrangian(const boost::shared_ptr<ImpulseDataAbstract>& data,
- const Eigen::VectorXd& lambda) {
- assert(lambda.size() == 3 && "lambda has wrong dimension, it should be 3d vector");
+void ImpulseModel3D::updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& force) {
+ assert(force.size() == 3 && "force has wrong dimension, it should be 3d vector");
ImpulseData3D* d = static_cast<ImpulseData3D*>(data.get());
- data->f = d->jMf.act(pinocchio::Force(lambda, Eigen::Vector3d::Zero()));
+ data->f = d->jMf.act(pinocchio::Force(force, Eigen::Vector3d::Zero()));
}
boost::shared_ptr<ImpulseDataAbstract> ImpulseModel3D::createData(pinocchio::Data* const data) {
diff --git a/src/multibody/impulses/impulse-6d.cpp b/src/multibody/impulses/impulse-6d.cpp
index 473f73d9037d7e7d15a5a3a7b3299dd55da3c6fb..22d8cb246ac1bd82098aaa9989598e3cf1c67400 100644
--- a/src/multibody/impulses/impulse-6d.cpp
+++ b/src/multibody/impulses/impulse-6d.cpp
@@ -33,14 +33,13 @@ void ImpulseModel6D::calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data
ImpulseData6D* d = static_cast<ImpulseData6D*>(data.get());
pinocchio::getJointVelocityDerivatives(state_.get_pinocchio(), *d->pinocchio, d->joint, pinocchio::LOCAL,
d->v_partial_dq, d->v_partial_dv);
- d->Vq.noalias() = d->fXj * d->v_partial_dq;
+ d->dv0_dq.noalias() = d->fXj * d->v_partial_dq;
}
-void ImpulseModel6D::updateLagrangian(const boost::shared_ptr<ImpulseDataAbstract>& data,
- const Eigen::VectorXd& lambda) {
- assert(lambda.size() == 3 && "lambda has wrong dimension, it should be 6d vector");
+void ImpulseModel6D::updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& force) {
+ assert(force.size() == 6 && "force has wrong dimension, it should be 6d vector");
ImpulseData6D* d = static_cast<ImpulseData6D*>(data.get());
- data->f = d->jMf.act(pinocchio::Force(lambda, Eigen::Vector3d::Zero()));
+ data->f = d->jMf.act(pinocchio::Force(force));
}
boost::shared_ptr<ImpulseDataAbstract> ImpulseModel6D::createData(pinocchio::Data* const data) {
diff --git a/src/multibody/impulses/multiple-impulses.cpp b/src/multibody/impulses/multiple-impulses.cpp
index dda0d1bb80061ecb1bb7e37c42206b78d5daa31f..5e399d5d4a3c2aed874eda6f771bf622ddb870f3 100644
--- a/src/multibody/impulses/multiple-impulses.cpp
+++ b/src/multibody/impulses/multiple-impulses.cpp
@@ -74,14 +74,21 @@ void ImpulseModelMultiple::calcDiff(const boost::shared_ptr<ImpulseDataMultiple>
m_i.impulse->calcDiff(d_i, x, false);
unsigned int const& ni_i = m_i.impulse->get_ni();
- data->Vq.block(ni, 0, ni_i, nv) = d_i->Vq;
+ data->dv0_dq.block(ni, 0, ni_i, nv) = d_i->dv0_dq;
ni += ni_i;
}
}
-void ImpulseModelMultiple::updateLagrangian(const boost::shared_ptr<ImpulseDataMultiple>& data,
- const Eigen::VectorXd& lambda) {
- assert(lambda.size() == ni_ && "lambda has wrong dimension, it should be ni vector");
+void ImpulseModelMultiple::updateVelocity(const boost::shared_ptr<ImpulseDataMultiple>& data,
+ const Eigen::VectorXd& vnext) const {
+ assert(vnext.rows() == state_.get_nv() && "vnext has wrong dimension");
+
+ data->vnext = vnext;
+}
+
+void ImpulseModelMultiple::updateForce(const boost::shared_ptr<ImpulseDataMultiple>& data,
+ const Eigen::VectorXd& force) {
+ assert(force.size() == ni_ && "force has wrong dimension, it should be ni vector");
assert(data->impulses.size() == impulses_.size() && "it doesn't match the number of impulse datas and models");
unsigned int ni = 0;
@@ -98,12 +105,43 @@ void ImpulseModelMultiple::updateLagrangian(const boost::shared_ptr<ImpulseDataM
assert(it_m->first == it_d->first && "it doesn't match the impulse name between data and model");
unsigned int const& ni_i = m_i.impulse->get_ni();
- m_i.impulse->updateLagrangian(d_i, lambda.segment(ni, ni_i));
+ const Eigen::VectorBlock<const Eigen::VectorXd, Eigen::Dynamic> force_i = force.segment(ni, ni_i);
+ m_i.impulse->updateForce(d_i, force_i);
data->fext[d_i->joint] = d_i->f;
ni += ni_i;
}
}
+void ImpulseModelMultiple::updateVelocityDiff(const boost::shared_ptr<ImpulseDataMultiple>& data,
+ const Eigen::MatrixXd& dvnext_dx) const {
+ assert((dvnext_dx.rows() == state_.get_nv() && dvnext_dx.cols() == state_.get_ndx()) &&
+ "dvnext_dx has wrong dimension");
+
+ data->dvnext_dx = dvnext_dx;
+}
+
+void ImpulseModelMultiple::updateForceDiff(const boost::shared_ptr<ImpulseDataMultiple>& data,
+ const Eigen::MatrixXd& df_dq) const {
+ unsigned int const& nv = state_.get_nv();
+ assert((df_dq.rows() == ni_ && df_dq.cols() == nv) && "df_dq has wrong dimension");
+ assert(data->impulses.size() == impulses_.size() && "it doesn't match the number of impulse datas and models");
+ unsigned int ni = 0;
+
+ ImpulseModelContainer::const_iterator it_m, end_m;
+ ImpulseDataContainer::const_iterator it_d, end_d;
+ for (it_m = impulses_.begin(), end_m = impulses_.end(), it_d = data->impulses.begin(), end_d = data->impulses.end();
+ it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
+ const ImpulseItem& m_i = it_m->second;
+ const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
+ assert(it_m->first == it_d->first && "it doesn't match the impulse name between data and model");
+
+ unsigned int const& ni_i = m_i.impulse->get_ni();
+ const Eigen::Block<const Eigen::MatrixXd> df_dq_i = df_dq.block(ni, 0, ni_i, nv);
+ m_i.impulse->updateForceDiff(d_i, df_dq_i);
+ ni += ni_i;
+ }
+}
+
boost::shared_ptr<ImpulseDataMultiple> ImpulseModelMultiple::createData(pinocchio::Data* const data) {
return boost::make_shared<ImpulseDataMultiple>(this, data);
}
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index 0b2a967a970d8c6714806fc8e35f000251573e96..61b63c1a608384a4a20e8b4f8d0cb9ccb42425eb 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -40,6 +40,7 @@ MACRO(ADD_CPP_UNIT_TEST NAME PKGS)
SET(MODULE_NAME "${NAME}Test")
STRING(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME})
ADD_TEST_CFLAGS(${unittest_name} "-DTEST_MODULE_NAME=${MODULE_NAME}")
+ ADD_TEST_CFLAGS(${unittest_name} "-DBOOST_TEST_MODULE=${MODULE_NAME}")
FOREACH(PKG ${PKGS})
PKG_CONFIG_USE_DEPENDENCY(${unittest_name} ${PKG})
@@ -54,6 +55,14 @@ ENDMACRO(ADD_CPP_UNIT_TEST NAME PKGS)
FOREACH(TEST ${${PROJECT_NAME}_CPP_TESTS})
ADD_CPP_UNIT_TEST(${TEST} "eigen3;pinocchio")
+ # first look if a robot_properties_${robot_name} package contain a config file.
+ target_compile_definitions(${TEST} PUBLIC
+ TALOS_URDF="${EXAMPLE_ROBOT_DATA_PKGDATAROOTDIR}/example-robot-data/talos_data/urdf/talos_reduced.urdf")
+ target_compile_definitions(${TEST} PUBLIC
+ HYQ_URDF="${EXAMPLE_ROBOT_DATA_PKGDATAROOTDIR}/example-robot-data/hyq_description/robots/hyq_no_sensors.urdf")
+ target_compile_definitions(${TEST} PUBLIC
+ TALOS_ARM_URDF="${EXAMPLE_ROBOT_DATA_PKGDATAROOTDIR}/example-robot-data/talos_data/robots/talos_left_arm.urdf")
+
ENDFOREACH(TEST ${${PROJECT_NAME}_CPP_TESTS})
diff --git a/unittest/bindings/test_actuations.py b/unittest/bindings/test_actuations.py
index 48eeaee211c6b9ceecf04ada8dfa9432e22c6188..c2cb160c6cdaa864f07c428b79b2518548a84791 100644
--- a/unittest/bindings/test_actuations.py
+++ b/unittest/bindings/test_actuations.py
@@ -24,15 +24,15 @@ class ActuationModelAbstractTestCase(unittest.TestCase):
self.ACTUATION.calc(self.DATA, self.x, self.u)
self.ACTUATION_DER.calc(self.DATA_DER, self.x, self.u)
# Checking the actuation signal
- self.assertTrue(np.allclose(self.DATA.a, self.DATA_DER.a, atol=1e-9), "Wrong actuation signal.")
+ self.assertTrue(np.allclose(self.DATA.tau, self.DATA_DER.tau, atol=1e-9), "Wrong actuation signal.")
def test_calcDiff(self):
# Run calcDiff for both action models
self.ACTUATION.calcDiff(self.DATA, self.x, self.u)
self.ACTUATION_DER.calcDiff(self.DATA_DER, self.x, self.u)
# Checking the Jacobians of the actuation model
- self.assertTrue(np.allclose(self.DATA.Ax, self.DATA_DER.Ax, atol=1e-9), "Wrong Ax.")
- self.assertTrue(np.allclose(self.DATA.Au, self.DATA_DER.Au, atol=1e-9), "Wrong Au.")
+ self.assertTrue(np.allclose(self.DATA.dtau_dx, self.DATA_DER.dtau_dx, atol=1e-9), "Wrong dtau_dx.")
+ self.assertTrue(np.allclose(self.DATA.dtau_du, self.DATA_DER.dtau_du, atol=1e-9), "Wrong dtau_du.")
class FloatingBaseActuationTest(ActuationModelAbstractTestCase):
diff --git a/unittest/bindings/test_contacts.py b/unittest/bindings/test_contacts.py
index b4814aba3af22a1e9c71991356e2676a11cfabab..410746b827d02337c6db5ae0c30eb3013af82933 100644
--- a/unittest/bindings/test_contacts.py
+++ b/unittest/bindings/test_contacts.py
@@ -44,8 +44,8 @@ class ContactModelAbstractTestCase(unittest.TestCase):
self.CONTACT.calcDiff(self.data, self.x, True)
self.CONTACT_DER.calcDiff(self.data_der, self.x, True)
# Checking the Jacobians of the contact constraint
- self.assertTrue(np.allclose(self.data.Ax, self.data_der.Ax, atol=1e-9),
- "Wrong derivatives of the contact constraint (Ax).")
+ self.assertTrue(np.allclose(self.data.da0_dx, self.data_der.da0_dx, atol=1e-9),
+ "Wrong derivatives of the desired contact acceleration (da0_dx).")
class ContactModelMultipleAbstractTestCase(unittest.TestCase):
@@ -87,8 +87,8 @@ class ContactModelMultipleAbstractTestCase(unittest.TestCase):
self.CONTACT.calcDiff(self.data, self.x, True)
self.contacts.calcDiff(self.data_multiple, self.x, True)
# Checking the Jacobians of the contact constraint
- self.assertTrue(np.allclose(self.data.Ax, self.data_multiple.Ax, atol=1e-9),
- "Wrong derivatives of the contact constraint (Ax).")
+ self.assertTrue(np.allclose(self.data.da0_dx, self.data_multiple.da0_dx, atol=1e-9),
+ "Wrong derivatives of the desired contact acceleration (da0_dx).")
class Contact3DTest(ContactModelAbstractTestCase):
diff --git a/unittest/bindings/test_impulses.py b/unittest/bindings/test_impulses.py
index c1c5d8e2fd34d2d337fdc4bd762df4b3e78cc9ec..dbc3c8d7456133c333a4ffec8e563b3b4d806fb1 100644
--- a/unittest/bindings/test_impulses.py
+++ b/unittest/bindings/test_impulses.py
@@ -43,7 +43,8 @@ class ImpulseModelAbstractTestCase(unittest.TestCase):
self.IMPULSE.calcDiff(self.data, self.x, True)
self.IMPULSE_DER.calcDiff(self.data_der, self.x, True)
# Checking the Jacobians of the contact constraint
- self.assertTrue(np.allclose(self.data.Vq, self.data_der.Vq, atol=1e-9), "Wrong drift acceleration (Vq).")
+ self.assertTrue(np.allclose(self.data.dv0_dq, self.data_der.dv0_dq, atol=1e-9),
+ "Wrong Jacobian of the acceleration before impulse (dv0_dq).")
class ImpulseModelMultipleAbstractTestCase(unittest.TestCase):
@@ -84,7 +85,8 @@ class ImpulseModelMultipleAbstractTestCase(unittest.TestCase):
self.IMPULSE.calcDiff(self.data, self.x, True)
self.impulses.calcDiff(self.data_multiple, self.x, True)
# Checking the Jacobians of the contact constraint
- self.assertTrue(np.allclose(self.data.Vq, self.data_multiple.Vq, atol=1e-9), "Wrong drift acceleration (Vq).")
+ self.assertTrue(np.allclose(self.data.dv0_dq, self.data_multiple.dv0_dq, atol=1e-9),
+ "Wrong Jacobian of the acceleration before impulse (dv0_dq).")
class Impulse3DTest(ImpulseModelAbstractTestCase):
diff --git a/unittest/bindings/test_solvers.py b/unittest/bindings/test_solvers.py
index 5dd124032916773054b0c44612a1f0450889d849..1e82027acb7dbb7bc627697aa2c3bf14299e5934 100644
--- a/unittest/bindings/test_solvers.py
+++ b/unittest/bindings/test_solvers.py
@@ -6,7 +6,7 @@ import numpy as np
import crocoddyl
import pinocchio
-from crocoddyl.utils import DDPDerived
+from crocoddyl.utils import DDPDerived, FDDPDerived
class SolverAbstractTestCase(unittest.TestCase):
@@ -101,6 +101,12 @@ class UnicycleDDPTest(SolverAbstractTestCase):
SOLVER_DER = DDPDerived
+class UnicycleFDDPTest(SolverAbstractTestCase):
+ MODEL = crocoddyl.ActionModelUnicycle()
+ SOLVER = crocoddyl.SolverFDDP
+ SOLVER_DER = FDDPDerived
+
+
class ManipulatorDDPTest(SolverAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
@@ -118,8 +124,25 @@ class ManipulatorDDPTest(SolverAbstractTestCase):
SOLVER_DER = DDPDerived
+class ManipulatorFDDPTest(SolverAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
+ STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+ COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv)
+ COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1e-7)
+ COST_SUM.addCost('uReg', crocoddyl.CostModelControl(STATE), 1e-7)
+ COST_SUM.addCost(
+ 'frTrack',
+ crocoddyl.CostModelFramePlacement(
+ STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.)
+ DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM)
+ MODEL = crocoddyl.IntegratedActionModelEuler(crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM),
+ 1e-3)
+ SOLVER = crocoddyl.SolverFDDP
+ SOLVER_DER = FDDPDerived
+
+
if __name__ == '__main__':
- test_classes_to_run = [UnicycleDDPTest, ManipulatorDDPTest]
+ test_classes_to_run = [UnicycleDDPTest, UnicycleFDDPTest, ManipulatorDDPTest, ManipulatorFDDPTest]
loader = unittest.TestLoader()
suites_list = []
for test_class in test_classes_to_run:
diff --git a/unittest/python/CMakeLists.txt b/unittest/python/CMakeLists.txt
index 4686560f43281dedeeca228e6b4e1814d3b625b5..6aa604c1baeb03e01093f73d492e434abf382b3b 100644
--- a/unittest/python/CMakeLists.txt
+++ b/unittest/python/CMakeLists.txt
@@ -10,13 +10,15 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
dse3
dynamic_derivatives
impact
- locomotion
quadruped
rk4
robots
solvers
state
)
+if(${MULTICONTACT_API_FOUND})
+ SET(${PROJECT_NAME}_PYTHON_TESTS ${${PROJECT_NAME}_PYTHON_TESTS} locomotion)
+endif(${MULTICONTACT_API_FOUND})
FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
ADD_PYTHON_UNIT_TEST("py-${TEST}" "unittest/python/test_${TEST}.py" ".")
diff --git a/unittest/python/test_contacts.py b/unittest/python/test_contacts.py
index 4cb7534fef147c385762bd172da638714ade6bfa..080c8acafc627af3653cc9f3d606911319ef70ab 100644
--- a/unittest/python/test_contacts.py
+++ b/unittest/python/test_contacts.py
@@ -369,7 +369,7 @@ x0 = model.State.rand()
xref = model.State.rand()
xref[:7] = x0[:7]
xref[3:7] = [0, 0, 0, 1] # TODO: remove this after adding assertion to include any case
-pinocchio.forwardKinematics(rmodel, rdata, a2m(xref))
+pinocchio.forwardKinematics(rmodel, rdata, a2m(xref[:rmodel.nq]))
pinocchio.updateFramePlacements(rmodel, rdata)
c1.ref[:] = m2a(rdata.oMf[c1.frame].translation.copy())
diff --git a/unittest/test_actions.cpp b/unittest/test_actions.cpp
index c8f9f5b80662e823b2e7788f9c7d6c5dc1a8c2e1..672db8abf269ef590d0d08888bec069d66e0a01d 100644
--- a/unittest/test_actions.cpp
+++ b/unittest/test_actions.cpp
@@ -8,13 +8,14 @@
#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API
+#include <Eigen/Dense>
+#include <pinocchio/fwd.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/bind.hpp>
#include "crocoddyl/core/action-base.hpp"
#include "crocoddyl/core/actions/lqr.hpp"
#include "crocoddyl/core/actions/unicycle.hpp"
#include "crocoddyl/core/numdiff/action.hpp"
-#include <Eigen/Dense>
using namespace boost::unit_test;
@@ -22,8 +23,6 @@ void test_construct_data(crocoddyl::ActionModelAbstract& model) {
boost::shared_ptr<crocoddyl::ActionDataAbstract> data = model.createData();
}
-//____________________________________________________________________________//
-
void test_calc_returns_state(crocoddyl::ActionModelAbstract& model) {
// create the corresponding data object
boost::shared_ptr<crocoddyl::ActionDataAbstract> data = model.createData();
@@ -38,8 +37,6 @@ void test_calc_returns_state(crocoddyl::ActionModelAbstract& model) {
BOOST_CHECK(data->get_xnext().size() == model.get_state().get_nx());
}
-//____________________________________________________________________________//
-
void test_calc_returns_a_cost(crocoddyl::ActionModelAbstract& model) {
// create the corresponding data object and set the cost to nan
boost::shared_ptr<crocoddyl::ActionDataAbstract> data = model.createData();
@@ -54,16 +51,11 @@ void test_calc_returns_a_cost(crocoddyl::ActionModelAbstract& model) {
BOOST_CHECK(!std::isnan(data->cost));
}
-//____________________________________________________________________________//
-
void test_partial_derivatives_against_numdiff(crocoddyl::ActionModelAbstract& model, double num_diff_modifier) {
// create the corresponding data object and set the cost to nan
boost::shared_ptr<crocoddyl::ActionDataAbstract> data = model.createData();
- // create the num diff model and data
- bool with_gauss_approx = model.get_nr() > 1;
-
- crocoddyl::ActionModelNumDiff model_num_diff(model, with_gauss_approx);
+ crocoddyl::ActionModelNumDiff model_num_diff(model);
boost::shared_ptr<crocoddyl::ActionDataAbstract> data_num_diff = model_num_diff.createData();
// Generating random values for the state and control
@@ -84,11 +76,13 @@ void test_partial_derivatives_against_numdiff(crocoddyl::ActionModelAbstract& mo
BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isMuchSmallerThan(1.0, tol));
BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isMuchSmallerThan(1.0, tol));
BOOST_CHECK((data->Luu - data_num_diff->Luu).isMuchSmallerThan(1.0, tol));
+ } else {
+ BOOST_CHECK((data_num_diff->Lxx).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((data_num_diff->Lxu).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((data_num_diff->Luu).isMuchSmallerThan(1.0, tol));
}
}
-//____________________________________________________________________________//
-
void register_action_model_lqr_unit_tests() {
int nx = 80;
int nu = 40;
@@ -105,14 +99,10 @@ void register_action_model_lqr_unit_tests() {
&test_partial_derivatives_against_numdiff, crocoddyl::ActionModelLQR(nx, nu, driftfree), num_diff_modifier)));
}
-//____________________________________________________________________________//
-
bool init_function() {
// Here we test the state_vector
register_action_model_lqr_unit_tests();
return true;
}
-//____________________________________________________________________________//
-
int main(int argc, char** argv) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }
diff --git a/unittest/test_state.cpp b/unittest/test_state.cpp
index a70b680b238c008b99fdc1e182ad181b9555fb31..78e7d192f856c90be553c127e16593e02fd9a350 100644
--- a/unittest/test_state.cpp
+++ b/unittest/test_state.cpp
@@ -8,176 +8,231 @@
#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API
+
+#include <Eigen/Dense>
+
+#include <pinocchio/fwd.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+#include <pinocchio/parsers/sample-models.hpp>
+
#include <boost/test/included/unit_test.hpp>
#include <boost/bind.hpp>
+
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/states/euclidean.hpp"
#include "crocoddyl/core/states/unicycle.hpp"
#include "crocoddyl/core/numdiff/state.hpp"
-#include <Eigen/Dense>
+#include "crocoddyl/multibody/states/multibody.hpp"
using namespace boost::unit_test;
-//____________________________________________________________________________//
+template <class PtrType>
+void delete_pointer(PtrType* ptr) {
+ if (ptr != NULL) {
+ delete ptr;
+ ptr = NULL;
+ }
+}
-void test_state_dimension(crocoddyl::StateAbstract& state, int nx) {
+class StateAbstractFactory {
+ public:
+ crocoddyl::StateAbstract* get_state() { return state_; }
+ crocoddyl::StateAbstract* state_;
+};
+
+class StateVectorFactory : public StateAbstractFactory {
+ public:
+ StateVectorFactory(int nx) : StateAbstractFactory() {
+ state_vector_ = new crocoddyl::StateVector(nx);
+ state_ = state_vector_;
+ }
+ ~StateVectorFactory() { delete_pointer(state_vector_); }
+ crocoddyl::StateVector* state_vector_;
+};
+
+class StateMultibodyFactory : public StateAbstractFactory {
+ public:
+ StateMultibodyFactory(const std::string& urdf_file = "", bool free_flyer = true) : StateAbstractFactory() {
+ pinocchio_model_ = new pinocchio::Model();
+ if (urdf_file.size() != 0) {
+ if (free_flyer) {
+ free_flyer_joint_ = new pinocchio::JointModelFreeFlyer();
+ pinocchio::urdf::buildModel(urdf_file, *free_flyer_joint_, *pinocchio_model_);
+ pinocchio_model_->lowerPositionLimit.head<3>().fill(-1.0);
+ pinocchio_model_->upperPositionLimit.head<3>().fill(1.0);
+ } else {
+ pinocchio::urdf::buildModel(urdf_file, *pinocchio_model_);
+ }
+ } else {
+ pinocchio::buildModels::humanoidRandom(*pinocchio_model_, free_flyer);
+ }
+ state_multibody_ = new crocoddyl::StateMultibody(*pinocchio_model_);
+ state_ = state_multibody_;
+ }
+ ~StateMultibodyFactory() {
+ delete_pointer(free_flyer_joint_);
+ delete_pointer(state_multibody_);
+ delete_pointer(pinocchio_model_);
+ }
+ pinocchio::JointModelFreeFlyer* free_flyer_joint_;
+ crocoddyl::StateMultibody* state_multibody_;
+ pinocchio::Model* pinocchio_model_;
+};
+
+//----------------------------------------------------------------------------//
+
+void test_state_dimension(StateAbstractFactory* factory, int nx) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Checking the dimension of zero and random states
- BOOST_CHECK(state.zero().size() == nx);
- BOOST_CHECK(state.rand().size() == nx);
+ BOOST_CHECK(state->zero().size() == nx);
+ BOOST_CHECK(state->rand().size() == nx);
}
-//____________________________________________________________________________//
-
-void test_integrate_against_difference(crocoddyl::StateAbstract& state) {
+void test_integrate_against_difference(StateAbstractFactory* factory) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random states
- Eigen::VectorXd x1 = state.rand();
- Eigen::VectorXd x2 = state.rand();
+ Eigen::VectorXd x1 = state->rand();
+ Eigen::VectorXd x2 = state->rand();
// Computing x2 by integrating its difference
- Eigen::VectorXd dx(state.get_ndx());
- state.diff(x1, x2, dx);
- Eigen::VectorXd x2i(state.get_nx());
- state.integrate(x1, dx, x2i);
+ Eigen::VectorXd dx(state->get_ndx());
+ state->diff(x1, x2, dx);
+ Eigen::VectorXd x2i(state->get_nx());
+ state->integrate(x1, dx, x2i);
- Eigen::VectorXd dxi(state.get_ndx());
- state.diff(x2i, x2, dxi);
+ Eigen::VectorXd dxi(state->get_ndx());
+ state->diff(x2i, x2, dxi);
// Checking that both states agree
BOOST_CHECK(dxi.isMuchSmallerThan(1.0, 1e-9));
}
-//____________________________________________________________________________//
-
-void test_difference_against_integrate(crocoddyl::StateAbstract& state) {
+void test_difference_against_integrate(StateAbstractFactory* factory) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random states
- Eigen::VectorXd x = state.rand();
- Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());
+ Eigen::VectorXd x = state->rand();
+ Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
// Computing dx by differentiation of its integrate
- Eigen::VectorXd xidx(state.get_nx());
- state.integrate(x, dx, xidx);
- Eigen::VectorXd dxd(state.get_ndx());
- state.diff(x, xidx, dxd);
+ Eigen::VectorXd xidx(state->get_nx());
+ state->integrate(x, dx, xidx);
+ Eigen::VectorXd dxd(state->get_ndx());
+ state->diff(x, xidx, dxd);
// Checking that both states agree
BOOST_CHECK((dxd - dx).isMuchSmallerThan(1.0, 1e-9));
}
-//____________________________________________________________________________//
-
-void test_Jdiff_firstsecond(crocoddyl::StateAbstract& state) {
+void test_Jdiff_firstsecond(StateAbstractFactory* factory) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random values for the initial and terminal states
- Eigen::VectorXd x1 = state.rand();
- Eigen::VectorXd x2 = state.rand();
+ Eigen::VectorXd x1 = state->rand();
+ Eigen::VectorXd x2 = state->rand();
// Computing the partial derivatives of the difference function separately
- Eigen::MatrixXd Jdiff_tmp(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdiff_first(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdiff_second(state.get_ndx(), state.get_ndx());
- state.Jdiff(x1, x2, Jdiff_first, Jdiff_tmp, crocoddyl::first);
- state.Jdiff(x1, x2, Jdiff_tmp, Jdiff_second, crocoddyl::second);
+ Eigen::MatrixXd Jdiff_tmp(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdiff_first(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdiff_second(state->get_ndx(), state->get_ndx());
+ state->Jdiff(x1, x2, Jdiff_first, Jdiff_tmp, crocoddyl::first);
+ state->Jdiff(x1, x2, Jdiff_tmp, Jdiff_second, crocoddyl::second);
// Computing the partial derivatives of the difference function separately
- Eigen::MatrixXd Jdiff_both_first(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdiff_both_second(state.get_ndx(), state.get_ndx());
- state.Jdiff(x1, x2, Jdiff_both_first, Jdiff_both_second);
+ Eigen::MatrixXd Jdiff_both_first(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdiff_both_second(state->get_ndx(), state->get_ndx());
+ state->Jdiff(x1, x2, Jdiff_both_first, Jdiff_both_second);
BOOST_CHECK((Jdiff_first - Jdiff_both_first).isMuchSmallerThan(1.0, 1e-9));
BOOST_CHECK((Jdiff_second - Jdiff_both_second).isMuchSmallerThan(1.0, 1e-9));
}
-//____________________________________________________________________________//
-
-void test_Jint_firstsecond(crocoddyl::StateAbstract& state) {
+void test_Jint_firstsecond(StateAbstractFactory* factory) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random values for the initial and terminal states
- Eigen::VectorXd x = state.rand();
- Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());
+ Eigen::VectorXd x = state->rand();
+ Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
// Computing the partial derivatives of the difference function separately
- Eigen::MatrixXd Jint_tmp(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jint_first(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jint_second(state.get_ndx(), state.get_ndx());
- state.Jintegrate(x, dx, Jint_first, Jint_tmp, crocoddyl::first);
- state.Jintegrate(x, dx, Jint_tmp, Jint_second, crocoddyl::second);
+ Eigen::MatrixXd Jint_tmp(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jint_first(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jint_second(state->get_ndx(), state->get_ndx());
+ state->Jintegrate(x, dx, Jint_first, Jint_tmp, crocoddyl::first);
+ state->Jintegrate(x, dx, Jint_tmp, Jint_second, crocoddyl::second);
// Computing the partial derivatives of the interence function separately
- Eigen::MatrixXd Jint_both_first(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jint_both_second(state.get_ndx(), state.get_ndx());
- state.Jintegrate(x, dx, Jint_both_first, Jint_both_second);
+ Eigen::MatrixXd Jint_both_first(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jint_both_second(state->get_ndx(), state->get_ndx());
+ state->Jintegrate(x, dx, Jint_both_first, Jint_both_second);
BOOST_CHECK((Jint_first - Jint_both_first).isMuchSmallerThan(1.0, 1e-9));
BOOST_CHECK((Jint_second - Jint_both_second).isMuchSmallerThan(1.0, 1e-9));
}
-//____________________________________________________________________________//
-
-void test_Jdiff_num_diff_firstsecond(crocoddyl::StateAbstract& state) {
+void test_Jdiff_num_diff_firstsecond(StateAbstractFactory* factory) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random values for the initial and terminal states
- Eigen::VectorXd x1 = state.rand();
- Eigen::VectorXd x2 = state.rand();
+ Eigen::VectorXd x1 = state->rand();
+ Eigen::VectorXd x2 = state->rand();
// Get the num diff state
- crocoddyl::StateNumDiff state_num_diff(state);
+ crocoddyl::StateNumDiff state_num_diff(*state);
// Computing the partial derivatives of the difference function separately
- Eigen::MatrixXd Jdiff_num_diff_tmp(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdiff_num_diff_first(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdiff_num_diff_second(state.get_ndx(), state.get_ndx());
+ Eigen::MatrixXd Jdiff_num_diff_tmp(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdiff_num_diff_first(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdiff_num_diff_second(state->get_ndx(), state->get_ndx());
state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_first, Jdiff_num_diff_tmp, crocoddyl::first);
state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_tmp, Jdiff_num_diff_second, crocoddyl::second);
// Computing the partial derivatives of the difference function separately
- Eigen::MatrixXd Jdiff_num_diff_both_first(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdiff_num_diff_both_second(state.get_ndx(), state.get_ndx());
+ Eigen::MatrixXd Jdiff_num_diff_both_first(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdiff_num_diff_both_second(state->get_ndx(), state->get_ndx());
state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_both_first, Jdiff_num_diff_both_second);
BOOST_CHECK((Jdiff_num_diff_first - Jdiff_num_diff_both_first).isMuchSmallerThan(1.0, 1e-9));
BOOST_CHECK((Jdiff_num_diff_second - Jdiff_num_diff_both_second).isMuchSmallerThan(1.0, 1e-9));
}
-//____________________________________________________________________________//
-
-void test_Jint_num_diff_firstsecond(crocoddyl::StateAbstract& state) {
+void test_Jint_num_diff_firstsecond(StateAbstractFactory* factory) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random values for the initial and terminal states
- Eigen::VectorXd x = state.rand();
- Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());
+ Eigen::VectorXd x = state->rand();
+ Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
// Get the num diff state
- crocoddyl::StateNumDiff state_num_diff(state);
+ crocoddyl::StateNumDiff state_num_diff(*state);
// Computing the partial derivatives of the difference function separately
- Eigen::MatrixXd Jint_num_diff_tmp(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jint_num_diff_first(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jint_num_diff_second(state.get_ndx(), state.get_ndx());
+ Eigen::MatrixXd Jint_num_diff_tmp(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jint_num_diff_first(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jint_num_diff_second(state->get_ndx(), state->get_ndx());
state_num_diff.Jintegrate(x, dx, Jint_num_diff_first, Jint_num_diff_tmp, crocoddyl::first);
state_num_diff.Jintegrate(x, dx, Jint_num_diff_tmp, Jint_num_diff_second, crocoddyl::second);
// Computing the partial derivatives of the interence function separately
- Eigen::MatrixXd Jint_num_diff_both_first(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jint_num_diff_both_second(state.get_ndx(), state.get_ndx());
+ Eigen::MatrixXd Jint_num_diff_both_first(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jint_num_diff_both_second(state->get_ndx(), state->get_ndx());
state_num_diff.Jintegrate(x, dx, Jint_num_diff_both_first, Jint_num_diff_both_second);
BOOST_CHECK((Jint_num_diff_first - Jint_num_diff_both_first).isMuchSmallerThan(1.0, 1e-9));
BOOST_CHECK((Jint_num_diff_second - Jint_num_diff_both_second).isMuchSmallerThan(1.0, 1e-9));
}
-//____________________________________________________________________________//
-
-void test_Jdiff_against_numdiff(crocoddyl::StateAbstract& state, double num_diff_modifier) {
+void test_Jdiff_against_numdiff(StateAbstractFactory* factory, double num_diff_modifier) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random values for the initial and terminal states
- Eigen::VectorXd x1 = state.rand();
- Eigen::VectorXd x2 = state.rand();
+ Eigen::VectorXd x1 = state->rand();
+ Eigen::VectorXd x2 = state->rand();
// Computing the partial derivatives of the difference function analytically
- Eigen::MatrixXd Jdiff_1(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdiff_2(state.get_ndx(), state.get_ndx());
- state.Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::first);
- state.Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::second);
+ Eigen::MatrixXd Jdiff_1(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdiff_2(state->get_ndx(), state->get_ndx());
+ state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::first);
+ state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::second);
// Computing the partial derivatives of the difference function numerically
- crocoddyl::StateNumDiff state_num_diff(state);
- Eigen::MatrixXd Jdiff_num_1(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdiff_num_2(state.get_ndx(), state.get_ndx());
+ crocoddyl::StateNumDiff state_num_diff(*state);
+ Eigen::MatrixXd Jdiff_num_1(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdiff_num_2(state->get_ndx(), state->get_ndx());
state_num_diff.Jdiff(x1, x2, Jdiff_num_1, Jdiff_num_2);
// Checking the partial derivatives against NumDiff
@@ -187,22 +242,21 @@ void test_Jdiff_against_numdiff(crocoddyl::StateAbstract& state, double num_diff
BOOST_CHECK((Jdiff_2 - Jdiff_num_2).isMuchSmallerThan(1.0, tol));
}
-//____________________________________________________________________________//
-
-void test_Jintegrate_against_numdiff(crocoddyl::StateAbstract& state, double num_diff_modifier) {
+void test_Jintegrate_against_numdiff(StateAbstractFactory* factory, double num_diff_modifier) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random values for the initial state and its rate of change
- Eigen::VectorXd x = state.rand();
- Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());
+ Eigen::VectorXd x = state->rand();
+ Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
// Computing the partial derivatives of the difference function analytically
- Eigen::MatrixXd Jint_1(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jint_2(state.get_ndx(), state.get_ndx());
- state.Jintegrate(x, dx, Jint_1, Jint_2);
+ Eigen::MatrixXd Jint_1(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jint_2(state->get_ndx(), state->get_ndx());
+ state->Jintegrate(x, dx, Jint_1, Jint_2);
// Computing the partial derivatives of the difference function numerically
- crocoddyl::StateNumDiff state_num_diff(state);
- Eigen::MatrixXd Jint_num_1(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jint_num_2(state.get_ndx(), state.get_ndx());
+ crocoddyl::StateNumDiff state_num_diff(*state);
+ Eigen::MatrixXd Jint_num_1(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jint_num_2(state->get_ndx(), state->get_ndx());
state_num_diff.Jintegrate(x, dx, Jint_num_1, Jint_num_2);
// Checking the partial derivatives against NumDiff
@@ -212,22 +266,21 @@ void test_Jintegrate_against_numdiff(crocoddyl::StateAbstract& state, double num
BOOST_CHECK((Jint_2 - Jint_num_2).isMuchSmallerThan(1.0, tol));
}
-//____________________________________________________________________________//
-
-void test_Jdiff_and_Jintegrate_are_inverses(crocoddyl::StateAbstract& state) {
+void test_Jdiff_and_Jintegrate_are_inverses(StateAbstractFactory* factory) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random states
- Eigen::VectorXd x1 = state.rand();
- Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());
- Eigen::VectorXd x2(state.get_nx());
- state.integrate(x1, dx, x2);
+ Eigen::VectorXd x1 = state->rand();
+ Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
+ Eigen::VectorXd x2(state->get_nx());
+ state->integrate(x1, dx, x2);
// Computing the partial derivatives of the integrate and difference function
- Eigen::MatrixXd Jx(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdx(state.get_ndx(), state.get_ndx());
- state.Jintegrate(x1, dx, Jx, Jdx);
- Eigen::MatrixXd J1(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd J2(state.get_ndx(), state.get_ndx());
- state.Jdiff(x1, x2, J1, J2);
+ Eigen::MatrixXd Jx(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdx(state->get_ndx(), state->get_ndx());
+ state->Jintegrate(x1, dx, Jx, Jdx);
+ Eigen::MatrixXd J1(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd J2(state->get_ndx(), state->get_ndx());
+ state->Jdiff(x1, x2, J1, J2);
// Checking that Jdiff and Jintegrate are inverses
Eigen::MatrixXd dX_dDX = Jdx;
@@ -235,84 +288,120 @@ void test_Jdiff_and_Jintegrate_are_inverses(crocoddyl::StateAbstract& state) {
BOOST_CHECK((dX_dDX - dDX_dX.inverse()).isMuchSmallerThan(1.0, 1e-9));
}
-//____________________________________________________________________________//
-
-void test_velocity_from_Jintegrate_Jdiff(crocoddyl::StateAbstract& state) {
+void test_velocity_from_Jintegrate_Jdiff(StateAbstractFactory* factory) {
+ crocoddyl::StateAbstract* state = factory->get_state();
// Generating random states
- Eigen::VectorXd x1 = state.rand();
- Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());
- Eigen::VectorXd x2(state.get_nx());
- state.integrate(x1, dx, x2);
- Eigen::VectorXd eps = Eigen::VectorXd::Random(state.get_ndx());
+ Eigen::VectorXd x1 = state->rand();
+ Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
+ Eigen::VectorXd x2(state->get_nx());
+ state->integrate(x1, dx, x2);
+ Eigen::VectorXd eps = Eigen::VectorXd::Random(state->get_ndx());
double h = 1e-8;
// Computing the partial derivatives of the integrate and difference function
- Eigen::MatrixXd Jx(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd Jdx(state.get_ndx(), state.get_ndx());
- state.Jintegrate(x1, dx, Jx, Jdx);
- Eigen::MatrixXd J1(state.get_ndx(), state.get_ndx());
- Eigen::MatrixXd J2(state.get_ndx(), state.get_ndx());
- state.Jdiff(x1, x2, J1, J2);
+ Eigen::MatrixXd Jx(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd Jdx(state->get_ndx(), state->get_ndx());
+ state->Jintegrate(x1, dx, Jx, Jdx);
+ Eigen::MatrixXd J1(state->get_ndx(), state->get_ndx());
+ Eigen::MatrixXd J2(state->get_ndx(), state->get_ndx());
+ state->Jdiff(x1, x2, J1, J2);
// Checking that computed velocity from Jintegrate
Eigen::MatrixXd dX_dDX = Jdx;
- Eigen::VectorXd x2eps(state.get_nx());
- state.integrate(x1, dx + eps * h, x2eps);
- Eigen::VectorXd x2_eps(state.get_nx());
- state.diff(x2, x2eps, x2_eps);
+ Eigen::VectorXd x2eps(state->get_nx());
+ state->integrate(x1, dx + eps * h, x2eps);
+ Eigen::VectorXd x2_eps(state->get_ndx());
+ state->diff(x2, x2eps, x2_eps);
BOOST_CHECK((dX_dDX * eps - x2_eps / h).isMuchSmallerThan(1.0, 1e-3));
// Checking the velocity computed from Jdiff
- Eigen::VectorXd x = state.rand();
+ Eigen::VectorXd x = state->rand();
dx.setZero();
- state.diff(x1, x, dx);
- Eigen::VectorXd x2i(state.get_nx());
- state.integrate(x, eps * h, x2i);
- Eigen::VectorXd dxi(state.get_ndx());
- state.diff(x1, x2i, dxi);
+ state->diff(x1, x, dx);
+ Eigen::VectorXd x2i(state->get_nx());
+ state->integrate(x, eps * h, x2i);
+ Eigen::VectorXd dxi(state->get_ndx());
+ state->diff(x1, x2i, dxi);
J1.setZero();
J2.setZero();
- state.Jdiff(x1, x, J1, J2);
+ state->Jdiff(x1, x, J1, J2);
BOOST_CHECK((J2 * eps - (-dx + dxi) / h).isMuchSmallerThan(1.0, 1e-3));
}
-//____________________________________________________________________________//
-
void register_state_vector_unit_tests() {
int nx = 10;
double num_diff_modifier = 1e4;
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_state_dimension, crocoddyl::StateVector(nx), nx)));
+ BOOST_TEST_CASE(boost::bind(&test_state_dimension, new StateVectorFactory(nx), nx)));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_integrate_against_difference, crocoddyl::StateVector(nx))));
+ BOOST_TEST_CASE(boost::bind(&test_integrate_against_difference, new StateVectorFactory(nx))));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_difference_against_integrate, crocoddyl::StateVector(nx))));
+ BOOST_TEST_CASE(boost::bind(&test_difference_against_integrate, new StateVectorFactory(nx))));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, crocoddyl::StateVector(nx))));
+ BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, new StateVectorFactory(nx))));
- framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, crocoddyl::StateVector(nx))));
+ framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, new StateVectorFactory(nx))));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_Jdiff_num_diff_firstsecond, crocoddyl::StateVector(nx))));
+ BOOST_TEST_CASE(boost::bind(&test_Jdiff_num_diff_firstsecond, new StateVectorFactory(nx))));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, crocoddyl::StateVector(nx))));
+ BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, new StateVectorFactory(nx))));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, crocoddyl::StateVector(nx), num_diff_modifier)));
+ BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, new StateVectorFactory(nx), num_diff_modifier)));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_Jintegrate_against_numdiff, crocoddyl::StateVector(nx), num_diff_modifier)));
+ BOOST_TEST_CASE(boost::bind(&test_Jintegrate_against_numdiff, new StateVectorFactory(nx), num_diff_modifier)));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, crocoddyl::StateVector(nx))));
+ BOOST_TEST_CASE(boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, new StateVectorFactory(nx))));
framework::master_test_suite().add(
- BOOST_TEST_CASE(boost::bind(&test_velocity_from_Jintegrate_Jdiff, crocoddyl::StateVector(nx))));
+ BOOST_TEST_CASE(boost::bind(&test_velocity_from_Jintegrate_Jdiff, new StateVectorFactory(nx))));
+}
+
+void register_state_multibody_unit_tests(const std::string& urdf_file = "", bool free_flyer = true) {
+ double num_diff_modifier = 1e4;
+ StateMultibodyFactory* factory = new StateMultibodyFactory(urdf_file, free_flyer);
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_state_dimension, new StateMultibodyFactory(urdf_file, free_flyer),
+ factory->pinocchio_model_->nq + factory->pinocchio_model_->nv)));
+
+ framework::master_test_suite().add(BOOST_TEST_CASE(
+ boost::bind(&test_integrate_against_difference, new StateMultibodyFactory(urdf_file, free_flyer))));
+
+ framework::master_test_suite().add(BOOST_TEST_CASE(
+ boost::bind(&test_difference_against_integrate, new StateMultibodyFactory(urdf_file, free_flyer))));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, new StateMultibodyFactory(urdf_file, free_flyer))));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, new StateMultibodyFactory(urdf_file, free_flyer))));
+
+ framework::master_test_suite().add(BOOST_TEST_CASE(
+ boost::bind(&test_Jdiff_num_diff_firstsecond, new StateMultibodyFactory(urdf_file, free_flyer))));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, new StateMultibodyFactory(urdf_file, free_flyer))));
+
+ framework::master_test_suite().add(BOOST_TEST_CASE(
+ boost::bind(&test_Jdiff_against_numdiff, new StateMultibodyFactory(urdf_file, free_flyer), num_diff_modifier)));
+
+ framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(
+ &test_Jintegrate_against_numdiff, new StateMultibodyFactory(urdf_file, free_flyer), num_diff_modifier)));
+
+ framework::master_test_suite().add(BOOST_TEST_CASE(
+ boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, new StateMultibodyFactory(urdf_file, free_flyer))));
+
+ framework::master_test_suite().add(BOOST_TEST_CASE(
+ boost::bind(&test_velocity_from_Jintegrate_Jdiff, new StateMultibodyFactory(urdf_file, free_flyer))));
}
//____________________________________________________________________________//
@@ -320,9 +409,11 @@ void register_state_vector_unit_tests() {
bool init_function() {
// Here we test the state_vector
register_state_vector_unit_tests();
+ register_state_multibody_unit_tests(TALOS_ARM_URDF, false);
+ register_state_multibody_unit_tests(HYQ_URDF, true);
+ register_state_multibody_unit_tests(TALOS_URDF);
+ register_state_multibody_unit_tests(); // random humanoid
return true;
}
-//____________________________________________________________________________//
-
int main(int argc, char** argv) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }