diff --git a/.gitignore b/.gitignore
index 68b18184d2f9043add44466d2a28feffb334c7a5..91ea4571fb5bdd8b6b01d4a12ca325eab44cc37f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,3 +4,8 @@
*~
*.orig
*.ipynb_checkpoints
+build/*
+*.aux
+*.log
+*.pdf
+.clang-format
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ee3dd3a6a3431ca598d522d83c98d3be4bbd8d45..a8a58375d9ef7e0efda18bd9edf4fd9d727d87a6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,25 +1,123 @@
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+# Check if the cmake (git submodule) have been initialized
+IF(NOT EXISTS "${PROJECT_SOURCE_DIR}/cmake/base.cmake")
+ MESSAGE(FATAL_ERROR "\nPlease run the following command first:\ngit submodule update --init\n")
+ENDIF()
+
+# Include important cmake modules
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/test.cmake)
+INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/python.cmake)
+# Set up project properties
SET(PROJECT_NAMESPACE loco-3d)
SET(PROJECT_NAME crocoddyl)
-SET(PROJECT_DESCRIPTION "Contact RObot COntrol by Differential DYnamic programming Library")
-
+SET(PROJECT_DESCRIPTION "Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)")
SET(PROJECT_URL https://gepgitlab.laas.fr/${PROJECT_NAMESPACE}/${PROJECT_NAME})
+
+# Print initial message
+MESSAGE("${PROJECT_DESCRIPTION}, version ${PROJECT_VERSION}")
+MESSAGE("Copyright (C) 2018-2019 CNRS-LAAS")
+MESSAGE("All rights reserved.")
+MESSAGE("Released under the BSD 3-Clause License.")
+
+
+# Set a default build type to 'Release' if none was specified
+IF(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+ MESSAGE(STATUS "Setting build type to 'Release' as none was specified.")
+ SET(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE)
+ # Set the possible values of build type for cmake-gui
+ SET_PROPERTY(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo")
+ENDIF()
+
+# Disable -Werror on Unix for now.
+SET(CXX_DISABLE_WERROR True)
+
+# Create different building options
+OPTION(ENABLE_VECTORIZATION "Enable vectorization and futhers processor-related optimizations" OFF)
+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)
+
+
+IF(ENABLE_VECTORIZATION)
+ SET(CMAKE_CXX_FLAGS "-march=native -mavx")
+ENDIF()
+
SETUP_PROJECT()
+# Add the different required and optional dependencies
+ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
+ADD_REQUIRED_DEPENDENCY("eigenpy")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.1.0")
ADD_OPTIONAL_DEPENDENCY("multicontact-api >= 1.1.0")
ADD_OPTIONAL_DEPENDENCY("quadprog")
ADD_OPTIONAL_DEPENDENCY("scipy")
-FINDPYTHON()
+OPTION(BUILD_WITH_MULTITHREADS "Build the library with the OpenMP support (required OpenMP)" OFF)
+IF(BUILD_WITH_MULTITHREADS)
+ SET(BUILD_WITH_NTHREADS "4" CACHE STRING "Number of threads")
+ string(REGEX MATCH "^[0-9]+$" BUILD_WITH_NTHREADS ${BUILD_WITH_NTHREADS})
+ IF(NOT BUILD_WITH_NTHREADS MATCHES "^[0-9]+$")
+ SET(BUILD_WITH_NTHREADS 4)
+ MESSAGE("Warning: the number of threads have to be an interger value, set to ${BUILD_WITH_NTHREADS}")
+ ENDIF()
+ENDIF()
+
+# Add OpenMP
+if(BUILD_WITH_MULTITHREADS)
+ find_package(OpenMP)
+ENDIF()
+if(OPENMP_FOUND AND BUILD_WITH_MULTITHREADS)
+ SET(CMAKE_CXX_FLAGS "-fopenmp")
+ ADD_DEFINITIONS(-DWITH_MULTITHREADING)
+ ADD_DEFINITIONS(-DWITH_NTHREADS=${BUILD_WITH_NTHREADS})
+ LIST(APPEND CFLAGS_DEPENDENCIES "-DWITH_MULTITHREADING" "-DWITH_NTHREADS")
+ENDIF()
+
+
+SET(BOOST_REQUIERED_COMPONENTS filesystem serialization system)
+SET(BOOST_BUILD_COMPONENTS unit_test_framework)
+SET(BOOST_OPTIONAL_COMPONENTS "")
+
+IF(BUILD_PYTHON_INTERFACE)
+ SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python)
+ FINDPYTHON()
+ FIND_NUMPY()
+ INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
+ENDIF(BUILD_PYTHON_INTERFACE)
+
+SET(BOOST_COMPONENTS ${BOOST_REQUIERED_COMPONENTS} ${BOOST_OPTIONAL_COMPONENTS} ${BOOST_BUILD_COMPONENTS})
+SEARCH_FOR_BOOST()
+
+# Path to boost headers
+INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIRS})
+
+
+# Include and add the main directory
ADD_SUBDIRECTORY(${PROJECT_NAME})
-ADD_SUBDIRECTORY(unittest)
+ADD_SUBDIRECTORY(src)
+
+# Build the Python interface
+IF(BUILD_PYTHON_INTERFACE)
+ # Include and add the bindings directory
+ INCLUDE_DIRECTORIES(bindings)
+ ADD_SUBDIRECTORY(bindings)
+ENDIF(BUILD_PYTHON_INTERFACE)
+
+# Build the unit tests
+IF(BUILD_UNIT_TESTS)
+ ADD_SUBDIRECTORY(unittest)
+ENDIF(BUILD_UNIT_TESTS)
+
+# Build the benchmark
+IF(BUILD_BENCHMARK)
+ ADD_SUBDIRECTORY(benchmark)
+ENDIF(BUILD_BENCHMARK)
+
SETUP_PROJECT_FINALIZE()
diff --git a/README.md b/README.md
index b68630ddd3fc605ca2528c5abc04c7c269f9dada..a7e56475d6771fc4d72fdb7388fa79c21d4ef184 100644
--- a/README.md
+++ b/README.md
@@ -7,13 +7,13 @@ Contact RObot COntrol by Differential DYnamic programming Library (crocoddyl)
The source code is released under the [BSD 3-Clause license](LICENSE).
-**Authors:** Carlos Mastalli and Rohan Budhiraja <br />
+**Authors:** [Carlos Mastalli](https://cmastalli.github.io/) and Rohan Budhiraja <br />
**Instructors:** Justin Carpentier and 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://gepgitlab.laas.fr/loco-3d/cddp/commits/devel)
+[](https://gepettoweb.laas.fr/doc/loco-3d/crocoddyl/devel/coverage/)
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).
@@ -44,7 +44,9 @@ If you have never added robotpkg as a softwares repository, please follow first
4. The installation of Crocoddyl:
- sudo apt install robotpkg-crocoddyl
+ sudo apt install robotpkg-py27-crocoddyl # for Python 2
+
+ sudo apt install robotpkg-py35-crocoddyl # for Python 3
Finally you will need to configure your environment variables, e.g.:
@@ -56,15 +58,16 @@ Finally you will need to configure your environment variables, e.g.:
### Building from source
-**Crocoddyl** is yet a pure Python library; you don't need to build it, however, you need to add it inside PYTHONPATH. It has the following dependecies:
+**Crocoddyl** is c++ library with Python bindings for versatible and fast prototyping. It has the following dependecies:
* [pinocchio](https://github.com/stack-of-tasks/pinocchio)
-* [SciPy](https://www.scipy.org/)
* [quadprog](https://pypi.org/project/quadprog/)
* [multicontact-api](https://gepgitlab.laas.fr/loco-3d/multicontact-api)
-* [matplotlib](https://matplotlib.org/) (optional)
-* [example-robot-data](https://gepgitlab.laas.fr/gepetto/example-robot-data) (optional for running examples)
+* [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)
+
You can run examples and tests from the root dir of the repository:
@@ -79,7 +82,29 @@ If you want to learn about Crocoddyl, take a look at the Jupyter notebooks. Star
- [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/introduction_to_crocoddyl.ipynb](https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/examples/notebooks/introduction_to_crocoddyl.ipynb)
-## <img align="center" height="20" src="http://www.pvhc.net/img205/oohmbjfzlxapxqbpkawx.png"/> Publications
+
+## Citing Crocoddyl
+
+To cite **Crocoddyl** in your academic research, please use the following bibtex lines:
+```
+@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},
+ howpublished = {https://gepgitlab.laas.fr/loco-3d/crocoddyl/wikis/home},
+ year = {2019}
+}
+```
+
+and the following paper describes different algorithm implemented in **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
- 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
+
+
+## Questions and Issues
+
+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>.
+
diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..88be29bb393bca4bc197febc185b6605228ea9fc
--- /dev/null
+++ b/benchmark/CMakeLists.txt
@@ -0,0 +1,26 @@
+
+SET(${PROJECT_NAME}_BENCHMARK
+ unicycle
+ lqr
+ )
+
+FOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK})
+ ADD_EXECUTABLE(${BENCHMARK_NAME} ${BENCHMARK_NAME}.cpp)
+ PKG_CONFIG_USE_DEPENDENCY(${BENCHMARK_NAME} eigen3)
+ TARGET_LINK_LIBRARIES(${BENCHMARK_NAME} ${PROJECT_NAME})
+ENDFOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK})
+
+SET(${PROJECT_NAME}_BENCHMARK_PYTHON
+ hyq
+ lqr
+ talos_arm
+ talos_legs
+ unicycle
+ )
+
+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
+ ${PYTHON_EXECUTABLE} -c "from ${benchmark} import *")
+ENDFOREACH(benchmark ${${PROJECT_NAME}_BENCHMARK_PYTHON})
diff --git a/benchmark/hyq.py b/benchmark/hyq.py
new file mode 100644
index 0000000000000000000000000000000000000000..1f6a3e1fa35b689f8e35ea95f14b06f8a6b850cf
--- /dev/null
+++ b/benchmark/hyq.py
@@ -0,0 +1,114 @@
+import time
+
+import example_robot_data
+import numpy as np
+
+import crocoddyl
+import pinocchio
+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
+
+
+def runBenchmark(gait_phase):
+ robot_model = example_robot_data.loadHyQ().model
+ lfFoot, rfFoot, lhFoot, rhFoot = 'lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'
+ gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot, rhFoot)
+ q0 = robot_model.referenceConfigurations['half_sitting'].copy()
+ v0 = pinocchio.utils.zero(robot_model.nv)
+ x0 = np.concatenate([q0, v0])
+
+ type_of_gait = list(gait_phase.keys())[0]
+ value = gait_phase[type_of_gait]
+ if type_of_gait == 'walking':
+ # Creating a walking problem
+ ddp = crocoddyl.SolverDDP(
+ gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
+ value['stepKnots'], value['supportKnots']))
+ elif type_of_gait == 'trotting':
+ # Creating a trotting problem
+ ddp = crocoddyl.SolverDDP(
+ gait.createTrottingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
+ value['stepKnots'], value['supportKnots']))
+ elif type_of_gait == 'pacing':
+ # Creating a pacing problem
+ ddp = crocoddyl.SolverDDP(
+ gait.createPacingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
+ value['stepKnots'], value['supportKnots']))
+ elif type_of_gait == 'bounding':
+ # Creating a bounding problem
+ ddp = crocoddyl.SolverDDP(
+ gait.createBoundingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
+ value['stepKnots'], value['supportKnots']))
+ elif type_of_gait == 'jumping':
+ # Creating a jumping problem
+ ddp = crocoddyl.SolverDDP(gait.createJumpingProblem(x0, value['jumpHeight'], value['timeStep']))
+
+ duration = []
+ xs = [robot_model.defaultState] * len(ddp.models())
+ us = [m.quasicStatic(d, robot_model.defaultState) for m, d in list(zip(ddp.models(), ddp.datas()))[:-1]]
+ for i in range(T):
+ c_start = time.time()
+ ddp.solve(xs, us, MAXITER, False, 0.1)
+ c_end = time.time()
+ duration.append(1e3 * (c_end - c_start))
+
+ avrg_duration = sum(duration) / len(duration)
+ min_duration = min(duration)
+ max_duration = max(duration)
+ return avrg_duration, min_duration, max_duration
+
+
+# Setting up all tasks
+if GAIT == 'walking':
+ GAITPHASE = {
+ 'walking': {
+ 'stepLength': 0.15,
+ 'stepHeight': 0.2,
+ 'timeStep': 1e-2,
+ 'stepKnots': 25,
+ 'supportKnots': 5
+ }
+ }
+elif GAIT == 'trotting':
+ GAITPHASE = {
+ 'trotting': {
+ 'stepLength': 0.15,
+ 'stepHeight': 0.2,
+ 'timeStep': 1e-2,
+ 'stepKnots': 25,
+ 'supportKnots': 5
+ }
+ }
+elif GAIT == 'pacing':
+ GAITPHASE = {
+ 'pacing': {
+ 'stepLength': 0.15,
+ 'stepHeight': 0.2,
+ 'timeStep': 1e-2,
+ 'stepKnots': 25,
+ 'supportKnots': 5
+ }
+ }
+elif GAIT == 'bounding':
+ GAITPHASE = {
+ 'bounding': {
+ 'stepLength': 0.15,
+ 'stepHeight': 0.2,
+ 'timeStep': 1e-2,
+ 'stepKnots': 25,
+ 'supportKnots': 5
+ }
+ }
+elif GAIT == 'jumping':
+ GAITPHASE = {'jumping': {'jumpHeight': 0.5, 'timeStep': 1e-2}}
+
+print('cpp-wrapped contact-forward dynamics on quadruped:')
+avrg_duration, min_duration, max_duration = runBenchmark(GAITPHASE)
+print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))
diff --git a/benchmark/lqr.cpp b/benchmark/lqr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19d9f6104f733999f02872b2936b57aace639cc6
--- /dev/null
+++ b/benchmark/lqr.cpp
@@ -0,0 +1,91 @@
+#include "crocoddyl/core/states/euclidean.hpp"
+#include "crocoddyl/core/actions/lqr.hpp"
+#include "crocoddyl/core/utils/callbacks.hpp"
+#include "crocoddyl/core/solvers/ddp.hpp"
+#include <ctime>
+
+int main() {
+ unsigned int NX = 37;
+ unsigned int NU = 12;
+ bool CALLBACKS = false;
+ unsigned int N = 100; // number of nodes
+ unsigned int T = 5e3; // number of trials
+ unsigned int MAXITER = 1;
+ using namespace crocoddyl;
+
+ Eigen::VectorXd x0;
+ std::vector<Eigen::VectorXd> xs;
+ std::vector<Eigen::VectorXd> us;
+ std::vector<ActionModelAbstract*> runningModels;
+ ActionModelAbstract* terminalModel;
+ x0 = Eigen::VectorXd::Zero(NX);
+
+ // Creating the action models and warm point for the LQR system
+ for (unsigned int i = 0; i < N; ++i) {
+ ActionModelAbstract* model_i = new ActionModelLQR(NX, NU);
+ runningModels.push_back(model_i);
+ xs.push_back(x0);
+ us.push_back(Eigen::VectorXd::Zero(NU));
+ }
+ xs.push_back(x0);
+ terminalModel = new ActionModelLQR(NX, NU);
+
+ // Formulating the optimal control problem
+ ShootingProblem problem(x0, runningModels, terminalModel);
+ SolverDDP ddp(problem);
+ if (CALLBACKS) {
+ std::vector<CallbackAbstract*> cbs;
+ cbs.push_back(new CallbackVerbose());
+ ddp.setCallbacks(cbs);
+ }
+
+ // Solving the optimal control problem
+ struct timespec start, finish;
+ double elapsed;
+ Eigen::ArrayXd duration(T);
+ for (unsigned int i = 0; i < T; ++i) {
+ 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;
+ duration[i] = elapsed / 1000.;
+ }
+
+ double avrg_duration = duration.sum() / T;
+ double min_duration = duration.minCoeff();
+ double max_duration = duration.maxCoeff();
+ std::cout << "Wall time [mu]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" << std::endl;
+
+ // Running calc
+ for (unsigned int i = 0; i < T; ++i) {
+ 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;
+ duration[i] = elapsed / 1000.;
+ }
+
+ avrg_duration = duration.sum() / T;
+ min_duration = duration.minCoeff();
+ max_duration = duration.maxCoeff();
+ std::cout << "Wall time calc [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+ << std::endl;
+
+ // Running calcDiff
+ for (unsigned int i = 0; i < T; ++i) {
+ 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;
+ duration[i] = elapsed / 1000.;
+ }
+
+ avrg_duration = duration.sum() / T;
+ min_duration = duration.minCoeff();
+ max_duration = duration.maxCoeff();
+ std::cout << "Wall time calcDiff [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+ << std::endl;
+}
\ No newline at end of file
diff --git a/benchmark/lqr.py b/benchmark/lqr.py
new file mode 100644
index 0000000000000000000000000000000000000000..224a676321626a7a1c83ae9d4f110baebcd1b94f
--- /dev/null
+++ b/benchmark/lqr.py
@@ -0,0 +1,44 @@
+import time
+
+import numpy as np
+
+import crocoddyl
+from crocoddyl.utils import LQRDerived
+
+NX = 37
+NU = 12
+N = 100 # number of nodes
+T = int(5e3) # number of trials
+MAXITER = 1
+
+
+def runBenchmark(model):
+ x0 = np.matrix(np.zeros(NX)).T
+ runningModels = [model(NX, NU)] * N
+ terminalModel = model(NX, NU)
+ xs = [x0] * (N + 1)
+ us = [np.matrix(np.zeros(NU)).T] * N
+
+ problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
+ ddp = crocoddyl.SolverDDP(problem)
+
+ duration = []
+ for i in range(T):
+ c_start = time.time()
+ ddp.solve(xs, us, MAXITER)
+ c_end = time.time()
+ duration.append(1e3 * (c_end - c_start))
+
+ avrg_duration = sum(duration) / len(duration)
+ min_duration = min(duration)
+ max_duration = max(duration)
+ return avrg_duration, min_duration, max_duration
+
+
+print('cpp-wrapped lqr:')
+avrg_duration, min_duration, max_duration = runBenchmark(crocoddyl.ActionModelLQR)
+print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))
+
+print('Python-derived lqr:')
+avrg_duration, min_duration, max_duration = runBenchmark(LQRDerived)
+print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))
diff --git a/benchmark/talos_arm.py b/benchmark/talos_arm.py
new file mode 100644
index 0000000000000000000000000000000000000000..9f844fa533dc048311e9eac256f8f13b3e79578d
--- /dev/null
+++ b/benchmark/talos_arm.py
@@ -0,0 +1,78 @@
+import crocoddyl
+import pinocchio
+# import utils
+import example_robot_data
+import numpy as np
+import time
+
+# First, let's load the Pinocchio model for the Talos arm.
+ROBOT = example_robot_data.loadTalosArm()
+N = 100 # number of nodes
+T = int(5e3) # number of trials
+MAXITER = 1
+
+
+def runBenchmark(model):
+ robot_model = ROBOT.model
+ q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T
+ x0 = np.vstack([q0, np.zeros((robot_model.nv, 1))])
+
+ # Note that we need to include a cost model (i.e. set of cost functions) in
+ # order to fully define the action model for our optimal control problem.
+ # For this particular example, we formulate three running-cost functions:
+ # goal-tracking cost, state and control regularization; and one terminal-cost:
+ # goal cost. First, let's create the common cost functions.
+ state = crocoddyl.StateMultibody(robot_model)
+ Mref = crocoddyl.FramePlacement(robot_model.getFrameId("gripper_left_joint"),
+ pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]])))
+ goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref)
+ xRegCost = crocoddyl.CostModelState(state)
+ uRegCost = crocoddyl.CostModelControl(state)
+
+ # Create a cost model per the running and terminal action model.
+ runningCostModel = crocoddyl.CostModelSum(state)
+ terminalCostModel = crocoddyl.CostModelSum(state)
+
+ # Then let's added the running and terminal cost functions
+ runningCostModel.addCost("gripperPose", goalTrackingCost, 1e-3)
+ runningCostModel.addCost("xReg", xRegCost, 1e-7)
+ runningCostModel.addCost("uReg", uRegCost, 1e-7)
+ terminalCostModel.addCost("gripperPose", goalTrackingCost, 1)
+
+ # Next, we need to create an action model for running and terminal knots. The
+ # forward dynamics (computed using ABA) are implemented
+ # inside DifferentialActionModelFullyActuated.
+ runningModel = crocoddyl.IntegratedActionModelEuler(model(state, runningCostModel), 1e-3)
+ runningModel.differential.armature = np.matrix([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T
+ terminalModel = crocoddyl.IntegratedActionModelEuler(model(state, terminalCostModel), 1e-3)
+ terminalModel.differential.armature = np.matrix([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T
+
+ # For this optimal control problem, we define 100 knots (or running action
+ # models) plus a terminal knot
+ problem = crocoddyl.ShootingProblem(x0, [runningModel] * N, terminalModel)
+
+ # Creating the DDP solver for this OC problem, defining a logger
+ ddp = crocoddyl.SolverDDP(problem)
+
+ duration = []
+ for i in range(T):
+ c_start = time.time()
+ ddp.solve([], [], MAXITER)
+ c_end = time.time()
+ duration.append(1e3 * (c_end - c_start))
+
+ avrg_duration = sum(duration) / len(duration)
+ min_duration = min(duration)
+ max_duration = max(duration)
+ return avrg_duration, min_duration, max_duration
+
+
+print('cpp-wrapped free-forward dynamics:')
+avrg_duration, min_duration, max_duration = runBenchmark(crocoddyl.DifferentialActionModelFreeFwdDynamics)
+print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))
+
+# TODO @Carlos this is not possible without making an abstract of createData.
+# At the time being, I don't have a solution
+# print('Python-derived free-forward dynamics:')
+# avrg_duration, min_duration, max_duration, tm = runBenchmark(utils.DifferentialFreeFwdDynamicsDerived)
+# print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))
diff --git a/benchmark/talos_legs.py b/benchmark/talos_legs.py
new file mode 100644
index 0000000000000000000000000000000000000000..bc67b76afe117ac1de5fffe0c5fb33d16d178de3
--- /dev/null
+++ b/benchmark/talos_legs.py
@@ -0,0 +1,60 @@
+import time
+
+import numpy as np
+
+import crocoddyl
+import example_robot_data
+import pinocchio
+from crocoddyl.utils.biped import SimpleBipedGaitProblem
+
+T = int(5e3) # number of trials
+MAXITER = 1
+GAIT = "walking" # 55 nodes
+
+
+def runBenchmark(gait_phase):
+ robot_model = example_robot_data.loadTalosLegs().model
+ rightFoot, leftFoot = 'right_sole_link', 'left_sole_link'
+ gait = SimpleBipedGaitProblem(robot_model, rightFoot, leftFoot)
+ q0 = robot_model.referenceConfigurations['half_sitting'].copy()
+ v0 = pinocchio.utils.zero(robot_model.nv)
+ x0 = np.concatenate([q0, v0])
+
+ type_of_gait = list(gait_phase.keys())[0]
+ value = gait_phase[type_of_gait]
+ if type_of_gait == 'walking':
+ # Creating a walking problem
+ ddp = crocoddyl.SolverDDP(
+ gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
+ value['stepKnots'], value['supportKnots']))
+
+ duration = []
+ xs = [robot_model.defaultState] * len(ddp.models())
+ us = [m.quasicStatic(d, robot_model.defaultState) for m, d in list(zip(ddp.models(), ddp.datas()))[:-1]]
+ for i in range(T):
+ c_start = time.time()
+ ddp.solve(xs, us, MAXITER, False, 0.1)
+ c_end = time.time()
+ duration.append(1e3 * (c_end - c_start))
+
+ avrg_duration = sum(duration) / len(duration)
+ min_duration = min(duration)
+ max_duration = max(duration)
+ return avrg_duration, min_duration, max_duration
+
+
+# Setting up all tasks
+if GAIT == 'walking':
+ GAITPHASE = {
+ 'walking': {
+ 'stepLength': 0.6,
+ 'stepHeight': 0.1,
+ 'timeStep': 0.0375,
+ 'stepKnots': 25,
+ 'supportKnots': 1
+ }
+ }
+
+print('cpp-wrapped contact-forward dynamics on quadruped:')
+avrg_duration, min_duration, max_duration = runBenchmark(GAITPHASE)
+print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))
diff --git a/benchmark/unicycle.cpp b/benchmark/unicycle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..92a726560e29b55110a93b8d98ea513d86d88117
--- /dev/null
+++ b/benchmark/unicycle.cpp
@@ -0,0 +1,93 @@
+#include "crocoddyl/core/actions/unicycle.hpp"
+#include "crocoddyl/core/utils/callbacks.hpp"
+#include "crocoddyl/core/solvers/ddp.hpp"
+#include <time.h>
+
+#ifdef WITH_MULTITHREADING
+#include <omp.h>
+#endif // WITH_MULTITHREADING
+
+int main() {
+ bool CALLBACKS = false;
+ unsigned int N = 200; // number of nodes
+ unsigned int T = 5e3; // number of trials
+ unsigned int MAXITER = 1;
+ using namespace crocoddyl;
+
+ Eigen::VectorXd x0;
+ std::vector<Eigen::VectorXd> xs;
+ std::vector<Eigen::VectorXd> us;
+ std::vector<ActionModelAbstract*> runningModels;
+ ActionModelAbstract* terminalModel;
+ x0 = Eigen::Vector3d(1., 0., 0.);
+
+ // Creating the action models and warm point for the unicycle system
+ for (unsigned int i = 0; i < N; ++i) {
+ ActionModelAbstract* model_i = new ActionModelUnicycle();
+ runningModels.push_back(model_i);
+ xs.push_back(x0);
+ us.push_back(Eigen::Vector2d::Zero());
+ }
+ xs.push_back(x0);
+ terminalModel = new ActionModelUnicycle();
+
+ // Formulating the optimal control problem
+ ShootingProblem problem(x0, runningModels, terminalModel);
+ SolverDDP ddp(problem);
+ if (CALLBACKS) {
+ std::vector<CallbackAbstract*> cbs;
+ cbs.push_back(new CallbackVerbose());
+ ddp.setCallbacks(cbs);
+ }
+
+ // Solving the optimal control problem
+ struct timespec start, finish;
+ double elapsed;
+ Eigen::ArrayXd duration(T);
+ for (unsigned int i = 0; i < T; ++i) {
+ 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;
+ duration[i] = elapsed / 1000.;
+ }
+
+ double avrg_duration = duration.sum() / T;
+ double min_duration = duration.minCoeff();
+ double max_duration = duration.maxCoeff();
+ std::cout << "Wall time solve [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+ << std::endl;
+
+ // Running calc
+ for (unsigned int i = 0; i < T; ++i) {
+ 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;
+ duration[i] = elapsed / 1000.;
+ }
+
+ avrg_duration = duration.sum() / T;
+ min_duration = duration.minCoeff();
+ max_duration = duration.maxCoeff();
+ std::cout << "Wall time calc [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+ << std::endl;
+
+ // Running calcDiff
+ for (unsigned int i = 0; i < T; ++i) {
+ 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;
+ duration[i] = elapsed / 1000.;
+ }
+
+ avrg_duration = duration.sum() / T;
+ min_duration = duration.minCoeff();
+ max_duration = duration.maxCoeff();
+ std::cout << "Wall time calcDiff [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")"
+ << std::endl;
+}
diff --git a/benchmark/unicycle.py b/benchmark/unicycle.py
new file mode 100644
index 0000000000000000000000000000000000000000..2354b3dcf512a9829c73ad8819879dfcaf02a2c1
--- /dev/null
+++ b/benchmark/unicycle.py
@@ -0,0 +1,40 @@
+import crocoddyl
+import numpy as np
+import time
+from crocoddyl.utils import UnicycleDerived
+
+N = 200 # number of nodes
+T = int(5e3) # number of trials
+MAXITER = 1
+
+
+def runBenchmark(model):
+ x0 = np.matrix([[1.], [0.], [0.]])
+ runningModels = [model()] * N
+ terminalModel = model()
+ xs = [x0] * (N + 1)
+ us = [np.matrix([[0.], [0.]])] * N
+
+ problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
+ ddp = crocoddyl.SolverDDP(problem)
+
+ duration = []
+ for i in range(T):
+ c_start = time.time()
+ ddp.solve(xs, us, MAXITER)
+ c_end = time.time()
+ duration.append(1e3 * (c_end - c_start))
+
+ avrg_duration = sum(duration) / len(duration)
+ min_duration = min(duration)
+ max_duration = max(duration)
+ return avrg_duration, min_duration, max_duration
+
+
+print('cpp-wrapped unicycle:')
+avrg_duration, min_duration, max_duration = runBenchmark(crocoddyl.ActionModelUnicycle)
+print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))
+
+print('Python-derived unicycle:')
+avrg_duration, min_duration, max_duration = runBenchmark(UnicycleDerived)
+print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration))
diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4bf1c3da0bd108d6f83c5d948cceba76aaaeafb8
--- /dev/null
+++ b/bindings/CMakeLists.txt
@@ -0,0 +1,4 @@
+IF(BUILD_PYTHON_INTERFACE)
+ ADD_SUBDIRECTORY(python/crocoddyl)
+ SET(PYWRAP ${PYWRAP} PARENT_SCOPE)
+ENDIF(BUILD_PYTHON_INTERFACE)
\ No newline at end of file
diff --git a/bindings/python/crocoddyl/CMakeLists.txt b/bindings/python/crocoddyl/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..75254719c5ca111f18238053530741b7feeb443f
--- /dev/null
+++ b/bindings/python/crocoddyl/CMakeLists.txt
@@ -0,0 +1,37 @@
+SET(${PROJECT_NAME}_PYTHON_BINDINGS_SOURCES
+ crocoddyl.cpp
+ )
+
+SET(${PROJECT_NAME}_PYTHON_BINDINGS_FILES
+ __init__.py
+ )
+
+
+ADD_LIBRARY(${PROJECT_NAME}_pywrap SHARED ${${PROJECT_NAME}_PYTHON_BINDINGS_SOURCES})
+PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME}_pywrap eigen3)
+PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME}_pywrap eigenpy)
+PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME}_pywrap pinocchio)
+
+TARGET_LINK_LIBRARIES(${PROJECT_NAME}_pywrap ${PROJECT_NAME})
+TARGET_LINK_BOOST_PYTHON(${PROJECT_NAME}_pywrap)
+
+INSTALL(TARGETS ${PROJECT_NAME}_pywrap DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME})
+
+
+FOREACH(python ${${PROJECT_NAME}_PYTHON_BINDINGS_FILES})
+ PYTHON_BUILD(. ${python})
+ INSTALL(FILES
+ "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/crocoddyl/${python}"
+ DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME})
+ENDFOREACH(python ${${PROJECT_NAME}_PYTHON_BINDINGS_FILES})
+
+
+SET(${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES
+ __init__.py
+ biped.py
+ quadruped.py
+ )
+
+FOREACH(python ${${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES})
+ PYTHON_BUILD(utils ${python})
+ENDFOREACH(python ${${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES})
diff --git a/bindings/python/crocoddyl/__init__.py b/bindings/python/crocoddyl/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..908fcad3ba6a9768ee83d699714a9ac45321b345
--- /dev/null
+++ b/bindings/python/crocoddyl/__init__.py
@@ -0,0 +1,82 @@
+from .libcrocoddyl_pywrap import *
+from .libcrocoddyl_pywrap import __version__
+
+
+def setGepettoViewerBackground(robot):
+ if not hasattr(robot, 'viewer'):
+ # Spawn robot model
+ robot.initViewer(loadModel=True)
+ # Set white background and floor
+ window_id = robot.viewer.gui.getWindowID('python-pinocchio')
+ robot.viewer.gui.setBackgroundColor1(window_id, [1., 1., 1., 1.])
+ robot.viewer.gui.setBackgroundColor2(window_id, [1., 1., 1., 1.])
+ robot.viewer.gui.addFloor('hpp-gui/floor')
+ robot.viewer.gui.setScale('hpp-gui/floor', [0.5, 0.5, 0.5])
+ robot.viewer.gui.setColor('hpp-gui/floor', [0.7, 0.7, 0.7, 1.])
+ robot.viewer.gui.setLightingMode('hpp-gui/floor', 'OFF')
+
+
+def displayTrajectory(robot, xs, dt=0.1, rate=-1, cameraTF=None):
+ """ Display a robot trajectory xs using Gepetto-viewer gui.
+
+ :param robot: Robot wrapper
+ :param xs: state trajectory
+ :param dt: step duration
+ :param rate: visualization rate
+ :param cameraTF: camera transform
+ """
+ setGepettoViewerBackground(robot)
+ if cameraTF is not None:
+ robot.viewer.gui.setCameraTransform(0, cameraTF)
+ import numpy as np
+
+ import time
+ S = 1 if rate <= 0 else max(len(xs) / rate, 1)
+ for i, x in enumerate(xs):
+ if not i % S:
+ robot.display(x[:robot.nq])
+ time.sleep(dt)
+
+
+class CallbackSolverDisplay(libcrocoddyl_pywrap.CallbackAbstract):
+ def __init__(self, robotwrapper, rate=-1, freq=1, cameraTF=None):
+ libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
+ self.robotwrapper = robotwrapper
+ self.rate = rate
+ self.cameraTF = cameraTF
+ self.freq = freq
+
+ def __call__(self, solver):
+ if (solver.iter + 1) % self.freq:
+ return
+ dt = solver.models()[0].dt
+ displayTrajectory(self.robotwrapper, solver.xs, dt, self.rate, self.cameraTF)
+
+
+class CallbackSolverLogger(libcrocoddyl_pywrap.CallbackAbstract):
+ def __init__(self):
+ libcrocoddyl_pywrap.CallbackAbstract.__init__(self)
+ self.steps = []
+ self.iters = []
+ self.costs = []
+ self.control_regs = []
+ self.state_regs = []
+ self.th_stops = []
+ self.gm_stops = []
+ self.xs = []
+ self.us = []
+ self.gaps = []
+
+ def __call__(self, solver):
+ import copy
+ import numpy as np
+ self.xs = copy.copy(solver.xs)
+ self.us = copy.copy(solver.us)
+ self.steps.append(solver.stepLength)
+ self.iters.append(solver.iter)
+ 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.gm_stops.append(-np.asscalar(solver.expectedImprovement()[1]))
+ self.gaps.append(copy.copy(solver.gaps))
\ No newline at end of file
diff --git a/bindings/python/crocoddyl/core.hpp b/bindings/python/crocoddyl/core.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1ffbc61d4b211f839b728c6c3b0a801b179f1254
--- /dev/null
+++ b/bindings/python/crocoddyl/core.hpp
@@ -0,0 +1,60 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_HPP_
+
+#include "python/crocoddyl/core/state-base.hpp"
+#include "python/crocoddyl/core/actuation-base.hpp"
+#include "python/crocoddyl/core/action-base.hpp"
+#include "python/crocoddyl/core/diff-action-base.hpp"
+#include "python/crocoddyl/core/activation-base.hpp"
+#include "python/crocoddyl/core/integrator/euler.hpp"
+#include "python/crocoddyl/core/numdiff/action.hpp"
+#include "python/crocoddyl/core/numdiff/diff-action.hpp"
+#include "python/crocoddyl/core/optctrl/shooting.hpp"
+#include "python/crocoddyl/core/solver-base.hpp"
+#include "python/crocoddyl/core/states/euclidean.hpp"
+#include "python/crocoddyl/core/actions/unicycle.hpp"
+#include "python/crocoddyl/core/actions/lqr.hpp"
+#include "python/crocoddyl/core/actions/diff-lqr.hpp"
+#include "python/crocoddyl/core/activations/quadratic.hpp"
+#include "python/crocoddyl/core/activations/weighted-quadratic.hpp"
+#include "python/crocoddyl/core/solvers/ddp.hpp"
+#include "python/crocoddyl/core/solvers/fddp.hpp"
+#include "python/crocoddyl/core/utils/callbacks.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+void exposeCore() {
+ exposeStateAbstract();
+ exposeActuationAbstract();
+ exposeActionAbstract();
+ exposeDifferentialActionAbstract();
+ exposeActivationAbstract();
+ exposeIntegratedActionEuler();
+ exposeActionNumDiff();
+ exposeDifferentialActionNumDiff();
+ exposeShootingProblem();
+ exposeSolverAbstract();
+ exposeStateEuclidean();
+ exposeActionUnicycle();
+ exposeActionLQR();
+ exposeDifferentialActionLQR();
+ exposeActivationQuad();
+ exposeActivationWeightedQuad();
+ exposeSolverDDP();
+ exposeSolverFDDP();
+ exposeCallbacks();
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_HPP_
diff --git a/bindings/python/crocoddyl/core/action-base.hpp b/bindings/python/crocoddyl/core/action-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2784bf6dd261bf966539274121c8af51701cb275
--- /dev/null
+++ b/bindings/python/crocoddyl/core/action-base.hpp
@@ -0,0 +1,141 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_ACTION_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_ACTION_BASE_HPP_
+
+#include "crocoddyl/core/action-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class ActionModelAbstract_wrap : public ActionModelAbstract, public bp::wrapper<ActionModelAbstract> {
+ public:
+ ActionModelAbstract_wrap(StateAbstract& state, unsigned int const& nu, unsigned int const& nr = 1)
+ : ActionModelAbstract(state, nu, nr), bp::wrapper<ActionModelAbstract>() {}
+
+ 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");
+ 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");
+ return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u, recalc);
+ }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ActionModel_calc_wraps, ActionModelAbstract::calc_wrap, 2, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ActionModel_quasicStatic_wraps, ActionModelAbstract::quasicStatic_wrap, 2, 4)
+
+void exposeActionAbstract() {
+ bp::class_<ActionModelAbstract_wrap, boost::noncopyable>(
+ "ActionModelAbstract",
+ "Abstract class for action models.\n\n"
+ "In crocoddyl, an action model combines dynamics and cost data. Each node, in our optimal\n"
+ "control problem, is described through an action model. Every time that we want to describe\n"
+ "a problem, we need to provide ways of computing the dynamics, cost functions and their\n"
+ "derivatives. These computations are mainly carry on inside calc() and calcDiff(),\n"
+ "respectively.",
+ bp::init<StateAbstract&, int, bp::optional<int> >(
+ bp::args(" self", " state", " nu", " nr=1"),
+ "Initialize the action model.\n\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>()])
+ .def("calc", pure_virtual(&ActionModelAbstract_wrap::calc), bp::args(" self", " data", " x", " u"),
+ "Compute the next state and cost value.\n\n"
+ "It describes the time-discrete evolution of our dynamical system\n"
+ "in which we obtain the next state. Additionally it computes the\n"
+ "cost value associated to this discrete state and control pair.\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input")
+ .def("calcDiff", pure_virtual(&ActionModelAbstract_wrap::calcDiff),
+ bp::args(" self", " data", " x", " u", " recalc=True"),
+ "Compute the derivatives of the dynamics and cost functions.\n\n"
+ "It computes the partial derivatives of the dynamical 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. linear dynamics and quadratic cost).\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def("createData", &ActionModelAbstract_wrap::createData, bp::args(" self"),
+ "Create the action data.\n\n"
+ "Each action model (AM) has its own data that needs to be allocated.\n"
+ "This function returns the allocated data for a predefined AM.\n"
+ ":return AM data.")
+ .def("quasicStatic", &ActionModelAbstract_wrap::quasicStatic_wrap,
+ ActionModel_quasicStatic_wraps(
+ bp::args(" self", " data", " x", " maxiter=100", " tol=1e-9"),
+ "Compute the quasic-static control given a state.\n\n"
+ "It runs an iterative Newton step in order to compute the quasic-static regime\n"
+ "given a state configuration.\n"
+ ":param data: action data\n"
+ ":param x: discrete-time state vector\n"
+ ":param maxiter: maximum allowed number of iterations\n"
+ ":param tol: stopping tolerance criteria\n"
+ ":return u: quasic-static control"))
+ .add_property(
+ "nu", bp::make_function(&ActionModelAbstract_wrap::get_nu, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of control vector")
+ .add_property(
+ "nr", bp::make_function(&ActionModelAbstract_wrap::get_nr, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of cost-residual vector")
+ .add_property("state",
+ bp::make_function(&ActionModelAbstract_wrap::get_state, bp::return_internal_reference<>()),
+ "state");
+
+ bp::register_ptr_to_python<boost::shared_ptr<ActionDataAbstract> >();
+
+ bp::class_<ActionDataAbstract, boost::noncopyable>(
+ "ActionDataAbstract",
+ "Abstract class for action datas.\n\n"
+ "In crocoddyl, an action data contains all the required information for processing an\n"
+ "user-defined action model. The action data typically is allocated onces by running\n"
+ "model.createData() and contains the first- and second- order derivatives of the dynamics\n"
+ "and cost function, respectively.",
+ bp::init<ActionModelAbstract*>(bp::args(" self", " model"),
+ "Create common data shared between AMs.\n\n"
+ "The action data uses the model in order to first process it.\n"
+ ":param model: action model"))
+ .add_property("cost", bp::make_getter(&ActionDataAbstract::cost, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::cost), "cost value")
+ .add_property("xnext",
+ bp::make_getter(&ActionDataAbstract::xnext, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::xnext), "next state")
+ .add_property("r", bp::make_getter(&ActionDataAbstract::r, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::r), "cost residual")
+ .add_property("Fx", bp::make_getter(&ActionDataAbstract::Fx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::Fx), "Jacobian of the dynamics")
+ .add_property("Fu", bp::make_getter(&ActionDataAbstract::Fu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::Fu), "Jacobian of the dynamics")
+ .add_property("Lx", bp::make_getter(&ActionDataAbstract::Lx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::Lx), "Jacobian of the cost")
+ .add_property("Lu", bp::make_getter(&ActionDataAbstract::Lu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::Lu), "Jacobian of the cost")
+ .add_property("Lxx", bp::make_getter(&ActionDataAbstract::Lxx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::Lxx), "Hessian of the cost")
+ .add_property("Lxu", bp::make_getter(&ActionDataAbstract::Lxu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::Lxu), "Hessian of the cost")
+ .add_property("Luu", bp::make_getter(&ActionDataAbstract::Luu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActionDataAbstract::Luu), "Hessian of the cost");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_ACTION_BASE_HPP_
diff --git a/bindings/python/crocoddyl/core/actions/diff-lqr.hpp b/bindings/python/crocoddyl/core/actions/diff-lqr.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a417201127b1f986af8c300256bc8c243b6db96c
--- /dev/null
+++ b/bindings/python/crocoddyl/core/actions/diff-lqr.hpp
@@ -0,0 +1,82 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_ACTIONS_DIFF_LQR_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
+
+#include "crocoddyl/core/actions/diff-lqr.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeDifferentialActionLQR() {
+ bp::class_<DifferentialActionModelLQR, bp::bases<DifferentialActionModelAbstract> >(
+ "DifferentialActionModelLQR",
+ "Differential action model for linear dynamics and quadratic cost.\n\n"
+ "This class implements a linear dynamics, and quadratic costs (i.e.\n"
+ "LQR action). Since the DAM is a second order system, and the integrated\n"
+ "action models are implemented as being second order integrators. This\n"
+ "class implements a second order linear system given by\n"
+ " x = [q, v]\n"
+ " dv = Fq q + Fv v + Fu u + f0\n"
+ "where Fq, Fv, Fu and f0 are randomly chosen constant terms. On the other\n"
+ "hand the cost function is given by\n"
+ " l(x,u) = 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", " nq", " nu", " driftFree=True"),
+ "Initialize the differential LQR action model.\n\n"
+ ":param nx: dimension of the state vector\n"
+ ":param nu: dimension of the control vector\n"
+ ":param driftFree: enable/disable the bias term of the linear dynamics"))
+ .def("calc", &DifferentialActionModelLQR::calc_wrap,
+ DiffActionModel_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 LQR system. Additionally it\n"
+ "computes the cost value associated to this discrete state and control pair.\n"
+ ":param data: action data\n"
+ ":param x: time-continuous state vector\n"
+ ":param u: time-continuous control input"))
+ .def<void (DifferentialActionModelLQR::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&, const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &DifferentialActionModelLQR::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the differential LQR dynamics and cost functions.\n\n"
+ "It computes the partial derivatives of the differential LQR 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: 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 (DifferentialActionModelLQR::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&, const Eigen::VectorXd&)>(
+ "calcDiff", &DifferentialActionModelLQR::calcDiff_wrap, bp::args(" self", " data", " x", " u"))
+ .def<void (DifferentialActionModelLQR::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &DifferentialActionModelLQR::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (DifferentialActionModelLQR::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &DifferentialActionModelLQR::calcDiff_wrap, bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &DifferentialActionModelLQR::createData, bp::args(" self"),
+ "Create the differential LQR action data.");
+
+ boost::python::register_ptr_to_python<boost::shared_ptr<DifferentialActionDataLQR> >();
+
+ bp::class_<DifferentialActionDataLQR, bp::bases<DifferentialActionDataAbstract> >(
+ "DifferentialActionDataLQR", "Action data for the differential LQR system.",
+ bp::init<DifferentialActionModelLQR*>(bp::args(" self", " model"),
+ "Create differential LQR data.\n\n"
+ ":param model: differential LQR action model"));
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
diff --git a/bindings/python/crocoddyl/core/actions/lqr.hpp b/bindings/python/crocoddyl/core/actions/lqr.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..54af9fbf7642e91b6abff7f1f5d2eb75de656301
--- /dev/null
+++ b/bindings/python/crocoddyl/core/actions/lqr.hpp
@@ -0,0 +1,74 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_ACTIONS_LQR_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_ACTIONS_LQR_HPP_
+
+#include "crocoddyl/core/actions/lqr.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeActionLQR() {
+ bp::class_<ActionModelLQR, bp::bases<ActionModelAbstract> >(
+ "ActionModelLQR",
+ "LQR action model.\n\n"
+ "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"),
+ "Initialize the LQR action model.\n\n"
+ ":param nx: dimension of the state vector\n"
+ ":param nu: dimension of the control vector\n"
+ ":param driftFree: enable/disable the bias term of the linear dynamics"))
+ .def("calc", &ActionModelLQR::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-discrete evolution of the LQR system. Additionally it\n"
+ "computes the cost value associated to this discrete\n"
+ "state and control pair.\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (ActionModelLQR::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &ActionModelLQR::calcDiff_wrap, bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the LQR dynamics and cost functions.\n\n"
+ "It computes the partial derivatives of the LQR 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: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (ActionModelLQR::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>("calcDiff", &ActionModelLQR::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u"))
+ .def<void (ActionModelLQR::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &ActionModelLQR::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (ActionModelLQR::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &ActionModelLQR::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &ActionModelLQR::createData, bp::args(" self"), "Create the LQR action data.");
+
+ boost::python::register_ptr_to_python<boost::shared_ptr<ActionDataLQR> >();
+
+ bp::class_<ActionDataLQR, bp::bases<ActionDataAbstract> >(
+ "ActionDataLQR", "Action data for the LQR system.",
+ bp::init<ActionModelLQR*>(bp::args(" self", " model"),
+ "Create LQR data.\n\n"
+ ":param model: LQR action model"));
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_ACTIONS_LQR_HPP_
diff --git a/bindings/python/crocoddyl/core/actions/unicycle.hpp b/bindings/python/crocoddyl/core/actions/unicycle.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c2a55ca09587e725dd78627f43d43a13c2a3b22
--- /dev/null
+++ b/bindings/python/crocoddyl/core/actions/unicycle.hpp
@@ -0,0 +1,80 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_ACTIONS_UNICYCLE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
+
+#include "crocoddyl/core/actions/unicycle.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeActionUnicycle() {
+ bp::class_<ActionModelUnicycle, bp::bases<ActionModelAbstract> >(
+ "ActionModelUnicycle",
+ "Unicycle action model.\n\n"
+ "The transition model of an unicycle system is described as\n"
+ " xnext = [v*cos(theta); v*sin(theta); w],\n"
+ "where the position is defined by (x, y, theta) and the control input\n"
+ "by (v,w). Note that the state is defined only with the position. On the\n"
+ "other hand, we define the quadratic cost functions for the state and\n"
+ "control.",
+ bp::init<>(bp::args(" self"), "Initialize the unicycle action model."))
+ .def("calc", &ActionModelUnicycle::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-discrete evolution of the unicycle system.\n"
+ "Additionally it computes the cost value associated to this discrete\n"
+ "state and control pair.\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (ActionModelUnicycle::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &ActionModelUnicycle::calcDiff_wrap, bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the unicycle dynamics and cost functions.\n\n"
+ "It computes the partial derivatives of the unicycle 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: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (ActionModelUnicycle::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>("calcDiff", &ActionModelUnicycle::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u"))
+ .def<void (ActionModelUnicycle::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &ActionModelUnicycle::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (ActionModelUnicycle::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &ActionModelUnicycle::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &ActionModelUnicycle::createData, bp::args(" self"), "Create the unicycle action data.")
+ .add_property(
+ "costWeights",
+ bp::make_function(&ActionModelUnicycle::get_cost_weights, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&ActionModelUnicycle::set_cost_weights), "cost weights");
+
+ bp::register_ptr_to_python<boost::shared_ptr<ActionDataUnicycle> >();
+
+ bp::class_<ActionDataUnicycle, bp::bases<ActionDataAbstract> >(
+ "ActionDataUnicycle",
+ "Action data for the Unicycle system.\n\n"
+ "The unicycle data, apart of common one, contains the cost residuals used\n"
+ "for the computation of calc and calcDiff.",
+ bp::init<ActionModelUnicycle*>(bp::args(" self", " model"),
+ "Create unicycle data.\n\n"
+ ":param model: unicycle action model"));
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
diff --git a/bindings/python/crocoddyl/core/activation-base.hpp b/bindings/python/crocoddyl/core/activation-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..219d5c5b67edb1c0ca9ef6aa153c7cfcf5b0f558
--- /dev/null
+++ b/bindings/python/crocoddyl/core/activation-base.hpp
@@ -0,0 +1,83 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_ACTIVATION_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_ACTIVATION_BASE_HPP_
+
+#include "crocoddyl/core/activation-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class ActivationModelAbstract_wrap : public ActivationModelAbstract, public bp::wrapper<ActivationModelAbstract> {
+ public:
+ explicit ActivationModelAbstract_wrap(unsigned int const& nr)
+ : ActivationModelAbstract(nr), bp::wrapper<ActivationModelAbstract>() {}
+
+ void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& r) {
+ assert(r.size() == nr_ && "r has wrong dimension");
+ return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)r);
+ }
+
+ void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& r,
+ const bool& recalc = true) {
+ assert(r.size() == nr_ && "r has wrong dimension");
+ return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)r, recalc);
+ }
+};
+
+void exposeActivationAbstract() {
+ bp::class_<ActivationModelAbstract_wrap, boost::noncopyable>(
+ "ActivationModelAbstract",
+ "Abstract class for activation models.\n\n"
+ "In crocoddyl, an activation model takes the residual vector and computes the activation\n"
+ "value and its derivatives from it. Activation value and its derivatives are computed by\n"
+ "calc() and calcDiff(), respectively.",
+ bp::init<int>(bp::args(" self", " nr"),
+ "Initialize the activation model.\n\n"
+ ":param nr: dimension of the cost-residual vector"))
+ .def("calc", pure_virtual(&ActivationModelAbstract_wrap::calc), bp::args(" self", " data", " r"),
+ "Compute the activation value.\n\n"
+ ":param data: activation data\n"
+ ":param r: residual vector")
+ .def("calcDiff", pure_virtual(&ActivationModelAbstract_wrap::calcDiff),
+ bp::args(" self", " data", " r", " recalc=True"),
+ "Compute the derivatives of the residual.\n\n"
+ "It computes the partial derivatives of the residual vector function\n"
+ ":param data: activation data\n"
+ ":param r: residual vector \n"
+ ":param recalc: If true, it updates the residual value.")
+ .def("createData", &ActivationModelAbstract_wrap::createData, bp::args(" self"),
+ "Create the activation data.\n\n")
+ .add_property(
+ "nr",
+ bp::make_function(&ActivationModelAbstract_wrap::get_nr, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of cost-residual vector");
+
+ bp::class_<ActivationDataAbstract, boost::shared_ptr<ActivationDataAbstract>, boost::noncopyable>(
+ "ActivationDataAbstract", "Abstract class for activation datas.\n\n",
+ bp::init<ActivationModelAbstract*>(bp::args(" self", " model"),
+ "Create common data shared between AMs.\n\n"
+ "The action data uses the model in order to first process it.\n"
+ ":param model: action model"))
+ .add_property("a",
+ bp::make_getter(&ActivationDataAbstract::a_value, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActivationDataAbstract::a_value), "cost value")
+ .add_property("Ar", bp::make_getter(&ActivationDataAbstract::Ar, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActivationDataAbstract::Ar), "Jacobian of the residual")
+ .add_property("Arr",
+ bp::make_getter(&ActivationDataAbstract::Arr, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&ActivationDataAbstract::Arr), "Hessian of the residual");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_ACTIVATION_BASE_HPP_
diff --git a/bindings/python/crocoddyl/core/activations/quadratic.hpp b/bindings/python/crocoddyl/core/activations/quadratic.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c76b351db05bb86ad3ee869c54773ec6515e4ae2
--- /dev/null
+++ b/bindings/python/crocoddyl/core/activations/quadratic.hpp
@@ -0,0 +1,49 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_ACTIVATIONS_QUADRATIC_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
+
+#include "crocoddyl/core/activations/quadratic.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeActivationQuad() {
+ bp::class_<ActivationModelQuad, bp::bases<ActivationModelAbstract> >(
+ "ActivationModelQuad",
+ "Quadratic activation model.\n\n"
+ "A quadratic action describes a quadratic function that depends on the residual, i.e.\n"
+ "0.5 *||r||^2.",
+ bp::init<int>(bp::args(" self", " nr"),
+ "Initialize the activation model.\n\n"
+ ":param nr: dimension of the cost-residual vector"))
+ .def("calc", &ActivationModelQuad::calc_wrap, bp::args(" self", " data", " r"),
+ "Compute the 0.5 * ||r||^2.\n\n"
+ ":param data: activation data\n"
+ ":param r: residual vector")
+ .def<void (ActivationModelQuad::*)(const boost::shared_ptr<ActivationDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>(
+ "calcDiff", &ActivationModelQuad::calcDiff_wrap, bp::args(" self", " data", " r", " recalc=True"),
+ "Compute the derivatives of a quadratic function.\n\n"
+ "Note that the Hessian is constant, so we don't write again this value.\n"
+ ":param data: activation data\n"
+ ":param r: residual vector \n"
+ ":param recalc: If true, it updates the residual value.")
+ .def<void (ActivationModelQuad::*)(const boost::shared_ptr<ActivationDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &ActivationModelQuad::calcDiff_wrap, bp::args(" self", " data", " r"))
+ .def("createData", &ActivationModelQuad::createData, bp::args(" self"),
+ "Create the quadratic activation data.\n\n");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
diff --git a/bindings/python/crocoddyl/core/activations/weighted-quadratic.hpp b/bindings/python/crocoddyl/core/activations/weighted-quadratic.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..84862687a5ba2815f187511003d04dd4eb448a70
--- /dev/null
+++ b/bindings/python/crocoddyl/core/activations/weighted-quadratic.hpp
@@ -0,0 +1,54 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_
+
+#include "crocoddyl/core/activations/weighted-quadratic.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeActivationWeightedQuad() {
+ bp::class_<ActivationModelWeightedQuad, bp::bases<ActivationModelAbstract> >(
+ "ActivationModelWeightedQuad",
+ "Weighted quadratic activation model.\n\n"
+ "A weighted quadratic action describes a quadratic function that depends on the residual,\n"
+ "i.e. 0.5 *||r||_w^2.",
+ bp::init<Eigen::VectorXd>(bp::args(" self", " weights"),
+ "Initialize the activation model.\n\n"
+ ":param weights: weights vector, note that nr=weights.size()"))
+ .def("calc", &ActivationModelWeightedQuad::calc_wrap, bp::args(" self", " data", " r"),
+ "Compute the 0.5 * ||r||_w^2.\n\n"
+ ":param data: activation data\n"
+ ":param r: residual vector")
+ .def<void (ActivationModelWeightedQuad::*)(const boost::shared_ptr<ActivationDataAbstract>&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &ActivationModelWeightedQuad::calcDiff_wrap, bp::args(" self", " data", " r", " recalc=True"),
+ "Compute the derivatives of a quadratic function.\n\n"
+ ":param data: activation data\n"
+ "Note that the Hessian is constant, so we don't write again this value.\n"
+ ":param r: residual vector \n"
+ ":param recalc: If true, it updates the residual value.")
+ .def<void (ActivationModelWeightedQuad::*)(const boost::shared_ptr<ActivationDataAbstract>&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &ActivationModelWeightedQuad::calcDiff_wrap, bp::args(" self", " data", " r"))
+ .def("createData", &ActivationModelWeightedQuad::createData, bp::args(" self"),
+ "Create the weighted quadratic action data.")
+ .add_property(
+ "weights",
+ bp::make_function(&ActivationModelWeightedQuad::get_weights, bp::return_value_policy<bp::return_by_value>()),
+ "weights of the quadratic term");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_
diff --git a/bindings/python/crocoddyl/core/actuation-base.hpp b/bindings/python/crocoddyl/core/actuation-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d0468cc2ce567f82fc2b12e7c59337077a0dc49
--- /dev/null
+++ b/bindings/python/crocoddyl/core/actuation-base.hpp
@@ -0,0 +1,104 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_ACTUATION_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_ACTUATION_BASE_HPP_
+
+#include "crocoddyl/core/actuation-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class ActuationModelAbstract_wrap : public ActuationModelAbstract, public bp::wrapper<ActuationModelAbstract> {
+ public:
+ ActuationModelAbstract_wrap(StateAbstract& state, unsigned int const& nu)
+ : ActuationModelAbstract(state, nu), bp::wrapper<ActuationModelAbstract>() {}
+
+ void calc(const boost::shared_ptr<ActuationDataAbstract>& 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");
+ return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u);
+ }
+
+ void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& 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");
+ return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u, recalc);
+ }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ActuationModel_calcDiff_wraps, ActuationModelAbstract::calcDiff_wrap, 3, 4)
+
+void exposeActuationAbstract() {
+ bp::class_<ActuationModelAbstract_wrap, boost::noncopyable>(
+ "ActuationModelAbstract",
+ "Abstract class for actuation models.\n\n"
+ "In crocoddyl, an actuation model is a block that takes u and outputs a (in continouos\n"
+ "time), where a is the actuation signal of our system, and it also computes the derivatives\n"
+ "of this model. These computations are mainly carry on inside calc() and calcDiff(),\n"
+ "respectively.",
+ bp::init<StateAbstract&, int>(bp::args(" self", " state", " nu"),
+ "Initialize the actuation model.\n\n"
+ ":param state: state description,\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", pure_virtual(&ActuationModelAbstract_wrap::calc), bp::args(" self", " data", " x", " u"),
+ "Compute the actuation signal from the control input u.\n\n"
+ "It describes the time-continuos evolution of the actuation model.\n"
+ ":param data: actuation data\n"
+ ":param x: state vector\n"
+ ":param u: control input")
+ .def("calcDiff", pure_virtual(&ActuationModelAbstract_wrap::calcDiff),
+ bp::args(" self", " data", " x", " u", " recalc=True"),
+ "Compute the derivatives of the actuation model.\n\n"
+ "It computes the partial derivatives of the actuation model which is\n"
+ "describes in continouos time.\n"
+ ":param data: actuation data\n"
+ ":param x: state vector\n"
+ ":param u: control input\n"
+ ":param recalc: If true, it updates the actuation signal.")
+ .def("createData", &ActuationModelAbstract_wrap::createData, bp::args(" self"),
+ "Create the actuation data.\n\n"
+ "Each actuation model (AM) has its own data that needs to be allocated.\n"
+ "This function returns the allocated data for a predefined AM.\n"
+ ":return AM data.")
+ .add_property(
+ "nu",
+ bp::make_function(&ActuationModelAbstract_wrap::get_nu, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of control vector")
+ .add_property("state",
+ bp::make_function(&ActuationModelAbstract_wrap::get_state, bp::return_internal_reference<>()),
+ "state");
+
+ bp::register_ptr_to_python<boost::shared_ptr<ActuationDataAbstract> >();
+
+ bp::class_<ActuationDataAbstract, boost::noncopyable>(
+ "ActuationDataAbstract",
+ "Abstract class for actuation datas.\n\n"
+ "In crocoddyl, an actuation data contains all the required information for processing an\n"
+ "user-defined actuation model. The actuation data typically is allocated onces by running\n"
+ "model.createData().",
+ bp::init<ActuationModelAbstract*>(bp::args(" self", " model"),
+ "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");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_ACTUATION_BASE_HPP_
diff --git a/bindings/python/crocoddyl/core/diff-action-base.hpp b/bindings/python/crocoddyl/core/diff-action-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5007e7fba8dbb595b1b0fe6cd491cd3c933479fb
--- /dev/null
+++ b/bindings/python/crocoddyl/core/diff-action-base.hpp
@@ -0,0 +1,158 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_DIFF_ACTION_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
+
+#include "crocoddyl/core/diff-action-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class DifferentialActionModelAbstract_wrap : public DifferentialActionModelAbstract,
+ public bp::wrapper<DifferentialActionModelAbstract> {
+ public:
+ DifferentialActionModelAbstract_wrap(StateAbstract& state, int nu, int nr = 1)
+ : DifferentialActionModelAbstract(state, nu, nr), bp::wrapper<DifferentialActionModelAbstract>() {}
+
+ 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");
+ return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u);
+ }
+
+ void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& 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");
+ return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u, recalc);
+ }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(DiffActionModel_calc_wraps, DifferentialActionModelAbstract::calc_wrap, 2, 3)
+
+void exposeDifferentialActionAbstract() {
+ bp::class_<DifferentialActionModelAbstract_wrap, boost::noncopyable>(
+ "DifferentialActionModelAbstract",
+ "Abstract class for the differential action model.\n\n"
+ "In crocoddyl, a differential action model combines dynamics and cost data described in\n"
+ "continuous time. Each node, in our optimal control problem, is described through an\n"
+ "action model. Every time that we want describe a problem, we need to provide ways of\n"
+ "computing the dynamics, cost functions and their derivatives. These computations are\n"
+ "mainly carry on inside calc() and calcDiff(), respectively.",
+ bp::init<StateAbstract&, int, bp::optional<int> >(
+ bp::args(" self", " state", " nu", " nr=1"),
+ "Initialize the differential action model.\n\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>()])
+ .def("calc", pure_virtual(&DifferentialActionModelAbstract_wrap::calc), bp::args(" self", " data", " x", " u"),
+ "Compute the state evolution and cost value.\n\n"
+ "First, it describes the time-continuous evolution of our dynamical system\n"
+ "in which along predefined integrated action self we might obtain the\n"
+ "next discrete state. Indeed it computes the time derivatives of the\n"
+ "state from a predefined dynamical system. Additionally it computes the\n"
+ "cost value associated to this state and control pair.\n"
+ ":param data: differential action data\n"
+ ":param x: state vector\n"
+ ":param u: control input")
+ .def("calcDiff", pure_virtual(&DifferentialActionModelAbstract_wrap::calcDiff),
+ bp::args(" self", " data", " x", " u", " recalc=True"),
+ "Compute the derivatives of the dynamics and cost functions.\n\n"
+ "It computes the partial derivatives of the dynamical system and the cost\n"
+ "function. If recalc == True, it first updates the state evolution and\n"
+ "cost value. This function builds a quadratic approximation of the\n"
+ "time-continuous action model (i.e. dynamical system and cost function).\n"
+ ":param data: differential action data\n"
+ ":param x: state vector\n"
+ ":param u: control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def("createData", &DifferentialActionModelAbstract_wrap::createData, bp::args(" self"),
+ "Create the differential action data.\n\n"
+ "Each differential action model has its own data that needs to be\n"
+ "allocated. This function returns the allocated data for a predefined\n"
+ "DAM.\n"
+ ":return DAM data.")
+ .add_property("nu",
+ bp::make_function(&DifferentialActionModelAbstract_wrap::get_nu,
+ bp::return_value_policy<bp::return_by_value>()),
+ "dimension of control vector")
+ .add_property("nr",
+ bp::make_function(&DifferentialActionModelAbstract_wrap::get_nr,
+ bp::return_value_policy<bp::return_by_value>()),
+ "dimension of cost-residual vector")
+ .add_property(
+ "state",
+ bp::make_function(&DifferentialActionModelAbstract_wrap::get_state, bp::return_internal_reference<>()),
+ "state");
+
+ bp::register_ptr_to_python<boost::shared_ptr<DifferentialActionDataAbstract> >();
+
+ bp::class_<DifferentialActionDataAbstract, boost::noncopyable>(
+ "DifferentialActionDataAbstract",
+ "Abstract class for differential action datas.\n\n"
+ "In crocoddyl, an action data contains all the required information for processing an\n"
+ "user-defined action model. The action data typically is allocated onces by running\n"
+ "model.createData() and contains the first- and second- order derivatives of the dynamics\n"
+ "and cost function, respectively.",
+ bp::init<DifferentialActionModelAbstract*>(
+ bp::args(" self", " model"),
+ "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>()),
+ bp::make_setter(&DifferentialActionDataAbstract::cost), "cost value")
+ .add_property(
+ "xout",
+ 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")
+ .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")
+ .add_property(
+ "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")
+ .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")
+ .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")
+ .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")
+ .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");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
diff --git a/bindings/python/crocoddyl/core/integrator/euler.hpp b/bindings/python/crocoddyl/core/integrator/euler.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d29f1421513b5cea2eb61babd375f2c21cf4fb9f
--- /dev/null
+++ b/bindings/python/crocoddyl/core/integrator/euler.hpp
@@ -0,0 +1,83 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_INTEGRATOR_EULER_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
+
+#include "crocoddyl/core/integrator/euler.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeIntegratedActionEuler() {
+ bp::class_<IntegratedActionModelEuler, bp::bases<ActionModelAbstract> >(
+ "IntegratedActionModelEuler",
+ "Sympletic Euler integrator for differential action models.\n\n"
+ "This class implements a sympletic Euler integrator (a.k.a semi-implicit\n"
+ "integrator) give a differential action model, i.e.:\n"
+ " [q+, v+] = State.integrate([q, v], [v + a * dt, a * dt] * dt).",
+ bp::init<DifferentialActionModelAbstract*, bp::optional<double, bool> >(
+ bp::args(" self", " diffModel", " stepTime", " withCostResidual"),
+ "Initialize the sympletic Euler integrator.\n\n"
+ ":param diffModel: differential action model\n"
+ ":param stepTime: step time\n"
+ ":param withCostResidual: includes the cost residuals and derivatives.")[bp::with_custodian_and_ward<1,
+ 2>()])
+ .def("calc", &IntegratedActionModelEuler::calc_wrap,
+ ActionModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the time-discrete evolution of a differential action model.\n\n"
+ "It describes the time-discrete evolution of action model.\n"
+ ":param data: action data\n"
+ ":param x: state vector\n"
+ ":param u: control input"))
+ .def<void (IntegratedActionModelEuler::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &IntegratedActionModelEuler::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the time-discrete derivatives of a differential action model.\n\n"
+ "It computes the time-discrete partial derivatives of a differential\n"
+ "action model. 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: action data\n"
+ ":param x: state vector\n"
+ ":param u: control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (IntegratedActionModelEuler::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &IntegratedActionModelEuler::calcDiff_wrap, bp::args(" self", " data", " x", " u"))
+ .def<void (IntegratedActionModelEuler::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &IntegratedActionModelEuler::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (IntegratedActionModelEuler::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &IntegratedActionModelEuler::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &IntegratedActionModelEuler::createData, bp::args(" self"),
+ "Create the Euler integrator data.")
+ .add_property(
+ "differential",
+ bp::make_function(&IntegratedActionModelEuler::get_differential, bp::return_internal_reference<>()),
+ "differential action model")
+ .add_property(
+ "dt", bp::make_function(&IntegratedActionModelEuler::get_dt, bp::return_value_policy<bp::return_by_value>()),
+ &IntegratedActionModelEuler::set_dt, "step time");
+
+ bp::register_ptr_to_python<boost::shared_ptr<IntegratedActionDataEuler> >();
+
+ bp::class_<IntegratedActionDataEuler, bp::bases<ActionDataAbstract> >(
+ "IntegratedActionDataEuler", "Sympletic Euler integrator data.",
+ bp::init<IntegratedActionModelEuler*>(bp::args(" self", " model"),
+ "Create sympletic Euler integrator data.\n\n"
+ ":param model: sympletic Euler integrator model"));
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
diff --git a/bindings/python/crocoddyl/core/numdiff/action.hpp b/bindings/python/crocoddyl/core/numdiff/action.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d31b34050321437b54f2d8e649663329429dbd1b
--- /dev/null
+++ b/bindings/python/crocoddyl/core/numdiff/action.hpp
@@ -0,0 +1,71 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_NUMDIFF_ACTION_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
+
+#include "crocoddyl/core/numdiff/action.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+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>()])
+ .def("calc", &ActionModelNumDiff::calc_wrap,
+ ActionModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the next state and cost value.\n\n"
+ "The system evolution is described in model.\n"
+ ":param data: NumDiff action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (ActionModelNumDiff::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &ActionModelNumDiff::calcDiff_wrap, bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the dynamics and cost functions.\n\n"
+ "It computes the Jacobian and Hessian using numerical differentiation.\n"
+ ":param data: NumDiff action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (ActionModelNumDiff::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>("calcDiff", &ActionModelNumDiff::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u"))
+ .def<void (ActionModelNumDiff::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &ActionModelNumDiff::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (ActionModelNumDiff::*)(const boost::shared_ptr<ActionDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &ActionModelNumDiff::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &ActionModelNumDiff::createData, bp::args(" self"),
+ "Create the action data.\n\n"
+ "Each action model (AM) has its own data that needs to be allocated.\n"
+ "This function returns the allocated data for a predefined AM.\n"
+ ":return AM data.")
+ .add_property("model", bp::make_function(&ActionModelNumDiff::get_model, bp::return_internal_reference<>()),
+ "action model")
+ .add_property(
+ "disturbance",
+ bp::make_function(&ActionModelNumDiff::get_disturbance, bp::return_value_policy<bp::return_by_value>()),
+ "disturbance value used in the numerical differentiation")
+ .add_property("withGaussApprox",
+ bp::make_function(&ActionModelNumDiff::get_with_gauss_approx,
+ bp::return_value_policy<bp::return_by_value>()),
+ "Gauss approximation for computing the Hessians");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
diff --git a/bindings/python/crocoddyl/core/numdiff/diff-action.hpp b/bindings/python/crocoddyl/core/numdiff/diff-action.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f29a4a9626d5fad98d6df405ae699f905c0ff050
--- /dev/null
+++ b/bindings/python/crocoddyl/core/numdiff/diff-action.hpp
@@ -0,0 +1,75 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_NUMDIFF_DIFF_ACTION_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
+
+#include "crocoddyl/core/numdiff/diff-action.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeDifferentialActionNumDiff() {
+ bp::class_<DifferentialActionModelNumDiff, bp::bases<DifferentialActionModelAbstract> >(
+ "DifferentialActionModelNumDiff",
+ "Abstract class for computing calcDiff by using numerical differentiation.\n\n",
+ bp::init<DifferentialActionModelAbstract&, 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>()])
+ .def("calc", &DifferentialActionModelNumDiff::calc_wrap,
+ DiffActionModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the next state and cost value.\n\n"
+ "The system evolution is described in model.\n"
+ ":param data: NumDiff action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (DifferentialActionModelNumDiff::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&, const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &DifferentialActionModelNumDiff::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u", " recalc=True"),
+ "Compute the derivatives of the dynamics and cost functions.\n\n"
+ "It computes the Jacobian and Hessian using numerical differentiation.\n"
+ ":param data: NumDiff action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (DifferentialActionModelNumDiff::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&, const Eigen::VectorXd&)>(
+ "calcDiff", &DifferentialActionModelNumDiff::calcDiff_wrap, bp::args(" self", " data", " x", " u"))
+ .def<void (DifferentialActionModelNumDiff::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &DifferentialActionModelNumDiff::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (DifferentialActionModelNumDiff::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &DifferentialActionModelNumDiff::calcDiff_wrap, bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &DifferentialActionModelNumDiff::createData, bp::args(" self"),
+ "Create the action data.\n\n"
+ "Each action model (AM) has its own data that needs to be allocated.\n"
+ "This function returns the allocated data for a predefined AM.\n"
+ ":return AM data.")
+ .add_property("model",
+ bp::make_function(&DifferentialActionModelNumDiff::get_model, bp::return_internal_reference<>()),
+ "action model")
+ .add_property("disturbance",
+ bp::make_function(&DifferentialActionModelNumDiff::get_disturbance,
+ bp::return_value_policy<bp::return_by_value>()),
+ "disturbance value used in the numerical differentiation")
+ .add_property("withGaussApprox",
+ bp::make_function(&DifferentialActionModelNumDiff::get_with_gauss_approx,
+ bp::return_value_policy<bp::return_by_value>()),
+ "Gauss approximation for computing the Hessians");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
diff --git a/bindings/python/crocoddyl/core/optctrl/shooting.hpp b/bindings/python/crocoddyl/core/optctrl/shooting.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..76bea13a0b095d6b08259cd6b34dab5031c2670f
--- /dev/null
+++ b/bindings/python/crocoddyl/core/optctrl/shooting.hpp
@@ -0,0 +1,87 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_OPTCTRL_SHOOTING_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
+
+#include <vector>
+#include <memory>
+#include "crocoddyl/core/optctrl/shooting.hpp"
+#include "python/crocoddyl/utils.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeShootingProblem() {
+ // Register custom converters between std::vector and Python list
+ typedef ActionModelAbstract* ActionModelPtr;
+ typedef boost::shared_ptr<ActionDataAbstract> ActionDataPtr;
+ bp::to_python_converter<std::vector<ActionModelPtr, std::allocator<ActionModelPtr> >,
+ vector_to_list<ActionModelPtr> >();
+ bp::to_python_converter<std::vector<ActionDataPtr, std::allocator<ActionDataPtr> >,
+ vector_to_list<ActionDataPtr, false> >();
+ list_to_vector().from_python<std::vector<ActionModelPtr, std::allocator<ActionModelPtr> > >();
+
+ bp::class_<ShootingProblem, boost::noncopyable>(
+ "ShootingProblem",
+ "Declare a shooting problem.\n\n"
+ "A shooting problem declares the initial state, a set of running action models and a\n"
+ "terminal action model. It has two main methods - calc, calcDiff and rollout. The\n"
+ "first computes the set of next states and cost values per each action model. calcDiff\n"
+ "updates the derivatives of all action models. The last rollouts the stacks of actions\n"
+ "models.",
+ bp::init<Eigen::VectorXd, std::vector<ActionModelAbstract*>, ActionModelAbstract*>(
+ bp::args(" self", " initialState", " runningModels", " terminalModel"),
+ "Initialize the shooting problem.\n\n"
+ ":param initialState: initial state\n"
+ ":param runningModels: running action models\n"
+ ":param terminalModel: terminal action model")
+ [bp::with_custodian_and_ward<1, 3, bp::with_custodian_and_ward<1, 4> >()])
+ .def("calc", &ShootingProblem::calc, bp::args(" self", " xs", " us"),
+ "Compute the cost and the next states.\n\n"
+ "First, it computes the next state and cost for each action model\n"
+ "along a state and control trajectory.\n"
+ ":param xs: time-discrete state trajectory\n"
+ ":param us: time-discrete control sequence\n"
+ ":returns the total cost value")
+ .def("calcDiff", &ShootingProblem::calcDiff, bp::args(" self", " xs", " us"),
+ "Compute the cost-and-dynamics derivatives.\n\n"
+ "These quantities are computed along a given pair of trajectories xs\n"
+ "(states) and us (controls).\n"
+ ":param xs: time-discrete state trajectory\n"
+ ":param us: time-discrete control sequence")
+ .def("rollout", &ShootingProblem::rollout_us, bp::args(" self", " us"),
+ "Integrate the dynamics given a control sequence.\n\n"
+ "Rollout the dynamics give a sequence of control commands\n"
+ ":param us: time-discrete control sequence")
+ .add_property("T", bp::make_function(&ShootingProblem::get_T), "number of nodes")
+ .add_property("x0", bp::make_function(&ShootingProblem::get_x0, bp::return_value_policy<bp::return_by_value>()),
+ "initial state")
+ .add_property(
+ "runningModels",
+ bp::make_function(&ShootingProblem::get_runningModels, bp::return_value_policy<bp::return_by_value>()),
+ "running models")
+ .add_property("terminalModel",
+ bp::make_function(&ShootingProblem::get_terminalModel, bp::return_internal_reference<>()),
+ "terminal model")
+ .add_property(
+ "runningDatas",
+ bp::make_function(&ShootingProblem::get_runningDatas, bp::return_value_policy<bp::return_by_value>()),
+ "running datas")
+ .add_property(
+ "terminalData",
+ bp::make_function(&ShootingProblem::get_terminalData, bp::return_value_policy<bp::return_by_value>()),
+ "terminal data");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
diff --git a/bindings/python/crocoddyl/core/solver-base.hpp b/bindings/python/crocoddyl/core/solver-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b4f3239a7df850641ea8d2b14f64f1ced9e9615
--- /dev/null
+++ b/bindings/python/crocoddyl/core/solver-base.hpp
@@ -0,0 +1,193 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_SOLVER_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_SOLVER_BASE_HPP_
+
+#include <vector>
+#include <memory>
+#include "crocoddyl/core/solver-base.hpp"
+#include "python/crocoddyl/utils.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class SolverAbstract_wrap : public SolverAbstract, public bp::wrapper<SolverAbstract> {
+ public:
+ using SolverAbstract::cost_;
+ using SolverAbstract::is_feasible_;
+ using SolverAbstract::iter_;
+ using SolverAbstract::problem_;
+ using SolverAbstract::steplength_;
+ using SolverAbstract::th_acceptstep_;
+ using SolverAbstract::th_stop_;
+ using SolverAbstract::ureg_;
+ using SolverAbstract::us_;
+ using SolverAbstract::xreg_;
+ using SolverAbstract::xs_;
+
+ explicit SolverAbstract_wrap(ShootingProblem& problem) : SolverAbstract(problem), bp::wrapper<SolverAbstract>() {}
+ ~SolverAbstract_wrap() {}
+
+ bool solve(const std::vector<Eigen::VectorXd>& init_xs, const std::vector<Eigen::VectorXd>& init_us,
+ unsigned int const& maxiter, const bool& is_feasible, const double& reg_init) {
+ return bp::call<bool>(this->get_override("solve").ptr(), init_xs, init_us, maxiter, is_feasible, reg_init);
+ }
+
+ void computeDirection(const bool& recalc = true) {
+ return bp::call<void>(this->get_override("computeDirection").ptr(), recalc);
+ }
+
+ double tryStep(const double& step_length) {
+ return bp::call<double>(this->get_override("tryStep").ptr(), step_length);
+ }
+
+ double stoppingCriteria() { return bp::call<double>(this->get_override("stoppingCriteria").ptr()); }
+
+ const Eigen::Vector2d& expectedImprovement() {
+ bp::list exp_impr = bp::call<bp::list>(this->get_override("expectedImprovement").ptr());
+ expected_improvement_ << bp::extract<double>(exp_impr[0]), bp::extract<double>(exp_impr[1]);
+ return expected_improvement_;
+ }
+
+ bp::list expectedImprovement_wrap() {
+ expectedImprovement();
+ bp::list exp_impr;
+ exp_impr.append(expected_improvement_[0]);
+ exp_impr.append(expected_improvement_[1]);
+ return exp_impr;
+ }
+
+ private:
+ Eigen::Vector2d expected_improvement_;
+};
+
+class CallbackAbstract_wrap : public CallbackAbstract, public bp::wrapper<CallbackAbstract> {
+ public:
+ CallbackAbstract_wrap() : CallbackAbstract(), bp::wrapper<CallbackAbstract>() {}
+ ~CallbackAbstract_wrap() {}
+
+ void operator()(SolverAbstract& solver) {
+ return bp::call<void>(this->get_override("__call__").ptr(), boost::ref(solver));
+ }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(setCandidate_overloads, SolverAbstract::setCandidate, 0, 3)
+
+void exposeSolverAbstract() {
+ // Register custom converters between std::vector and Python list
+ bp::to_python_converter<std::vector<CallbackAbstract*, std::allocator<CallbackAbstract*> >,
+ vector_to_list<CallbackAbstract*> >();
+ list_to_vector().from_python<std::vector<CallbackAbstract*, std::allocator<CallbackAbstract*> > >();
+
+ bp::class_<SolverAbstract_wrap, boost::noncopyable>(
+ "SolverAbstract",
+ "Abstract class for optimal control solvers.\n\n"
+ "In crocoddyl, a solver resolves an optimal control solver which is formulated in a\n"
+ "problem abstraction. The main routines are computeDirection and tryStep. The former finds\n"
+ "a search direction and typically computes the derivatives of each action model. The latter\n"
+ "rollout the dynamics and cost (i.e. the action) to try the search direction found by\n"
+ "computeDirection. Both functions used the current guess defined by setCandidate. Finally\n"
+ "solve function is used to define when the search direction and length are computed in each\n"
+ "iterate. It also describes the globalization strategy (i.e. regularization) of the\n"
+ "numerical optimization.",
+ bp::init<ShootingProblem&>(bp::args(" self", " problem"),
+ "Initialize the solver model.\n\n"
+ ":param problem: shooting problem")[bp::with_custodian_and_ward<1, 2>()])
+ .def("solve", pure_virtual(&SolverAbstract_wrap::solve),
+ bp::args(" self", " init_xs=[]", " init_us=[]", " maxiter=100", " isFeasible=False", " regInit=None"),
+ "Compute the optimal trajectory xopt,uopt as lists of T+1 and T terms.\n\n"
+ "From an initial guess init_xs,init_us (feasible or not), iterate\n"
+ "over computeDirection and tryStep until stoppingCriteria is below\n"
+ "threshold. It also describes the globalization strategy used\n"
+ "during the numerical optimization.\n"
+ ":param init_xs: initial guess for state trajectory with T+1 elements.\n"
+ ":param init_us: initial guess for control trajectory with T elements.\n"
+ ":param maxiter: maximun allowed number of iterations.\n"
+ ":param isFeasible: true if the init_xs are obtained from integrating the init_us (rollout).\n"
+ ":param regInit: initial guess for the regularization value. Very low\n"
+ " values are typical 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", pure_virtual(&SolverAbstract_wrap::computeDirection), 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", pure_virtual(&SolverAbstract_wrap::tryStep), bp::args(" self", " stepLength"),
+ "Try a predefined step length and compute its cost improvement.\n\n"
+ "It uses the search direction found by computeDirection to try a\n"
+ "determined step length; so you need to run first computeDirection.\n"
+ "Additionally it returns the cost improvement along the predefined\n"
+ "step length.\n"
+ ":param stepLength: step length\n"
+ ":returns the cost improvement.")
+ .def("stoppingCriteria", pure_virtual(&SolverAbstract_wrap::stoppingCriteria), bp::args(" self"),
+ "Return a positive value that quantifies the algorithm termination.\n\n"
+ "These values typically represents the gradient norm which tell us\n"
+ "that it's been reached the local minima. This function is used to\n"
+ "evaluate the algorithm convergence. The stopping criteria strictly\n"
+ "speaking depends on the search direction (calculated by\n"
+ "computeDirection) but it could also depend on the chosen step\n"
+ "length, tested by tryStep.")
+ .def("expectedImprovement", pure_virtual(&SolverAbstract_wrap::expectedImprovement_wrap), bp::args(" self"),
+ "Return the expected improvement from a given current search direction.\n\n"
+ "For computing the expected improvement, you need to compute first\n"
+ "the search direction by running computeDirection.")
+ .def("setCandidate", &SolverAbstract_wrap::setCandidate,
+ setCandidate_overloads(bp::args(" self", " xs=[]", " us=[]", " isFeasible=False"),
+ "Set the solver candidate warm-point values (xs, us).\n\n"
+ "The solver candidates are defined as a state and control trajectory\n"
+ "(xs, us) of T+1 and T elements, respectively. Additionally, we need\n"
+ "to define is (xs,us) pair is feasible, this means that the dynamics\n"
+ "rollout give us produces xs.\n"
+ ":param xs: state trajectory of T+1 elements.\n"
+ ":param us: control trajectory of T elements.\n"
+ ":param isFeasible: true if the xs are obtained from integrating the\n"
+ "us (rollout)."))
+ .def("setCallbacks", &SolverAbstract_wrap::setCallbacks, bp::args(" self"),
+ "Set a list of callback functions using for diagnostic.\n\n"
+ "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.")
+ .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")
+ .def("datas", &SolverAbstract_wrap::get_datas, bp::return_value_policy<bp::return_by_value>(), "datas")
+ .add_property("xs", bp::make_getter(&SolverAbstract_wrap::xs_, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&SolverAbstract_wrap::xs_, bp::return_value_policy<bp::return_by_value>()),
+ "state trajectory")
+ .add_property("us", bp::make_getter(&SolverAbstract_wrap::us_, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&SolverAbstract_wrap::us_, bp::return_value_policy<bp::return_by_value>()),
+ "control sequence")
+ .def_readwrite("isFeasible", &SolverAbstract_wrap::is_feasible_, "feasible (xs,us)")
+ .def_readwrite("cost", &SolverAbstract_wrap::cost_, "total cost")
+ .def_readwrite("x_reg", &SolverAbstract_wrap::xreg_, "state regularization")
+ .def_readwrite("u_reg", &SolverAbstract_wrap::ureg_, "control regularization")
+ .def_readwrite("stepLength", &SolverAbstract_wrap::steplength_, "applied step length")
+ .def_readwrite("th_acceptStep", &SolverAbstract_wrap::th_acceptstep_, "threshold for step acceptance")
+ .def_readwrite("th_stop", &SolverAbstract_wrap::th_stop_, "threshold for stopping criteria")
+ .def_readwrite("iter", &SolverAbstract_wrap::iter_, "number of iterations runned in solve()");
+
+ bp::class_<CallbackAbstract_wrap, boost::noncopyable>(
+ "CallbackAbstract",
+ "Abstract class for solver callbacks.\n\n"
+ "A callback is used to diagnostic the behaviour of our solver in each iteration of it.\n"
+ "For instance, it can be used to print values, record data or display motions")
+ .def("__call__", pure_virtual(&CallbackAbstract_wrap::operator()), bp::args(" self", " solver"),
+ "Run the callback function given a solver.\n\n"
+ ":param solver: solver to be diagnostic");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_SOLVER_BASE_HPP_
diff --git a/bindings/python/crocoddyl/core/solvers/ddp.hpp b/bindings/python/crocoddyl/core/solvers/ddp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..70778bc59f153206b5075aeeaa6cd1c14d15aeeb
--- /dev/null
+++ b/bindings/python/crocoddyl/core/solvers/ddp.hpp
@@ -0,0 +1,107 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_SOLVERS_DDP_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_SOLVERS_DDP_HPP_
+
+#include "crocoddyl/core/solvers/ddp.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverDDP_solves, SolverDDP::solve, 0, 5)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverDDP_computeDirections, SolverDDP::computeDirection, 0, 1)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SolverDDP_trySteps, SolverDDP::tryStep, 0, 1)
+
+void exposeSolverDDP() {
+ bp::class_<SolverDDP, bp::bases<SolverAbstract> >(
+ "SolverDDP",
+ "DDP solver.\n\n"
+ "The DDP 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"
+ "and the forward-pass rollouts this new policy by integrating the system dynamics\n"
+ "along a tuple of optimized control commands U*.\n"
+ ":param shootingProblem: shooting problem (list of action models along trajectory.)",
+ bp::init<ShootingProblem&>(bp::args(" self", " problem"),
+ "Initialize the vector dimension.\n\n"
+ ":param problem: shooting problem.")[bp::with_custodian_and_ward<1, 2>()])
+ .def("solve", &SolverDDP::solve,
+ SolverDDP_solves(
+ bp::args(" self", " init_xs=[]", " init_us=[]", " maxiter=100", " isFeasible=False", " regInit=None"),
+ "Compute the optimal trajectory xopt, uopt as lists of T+1 and T terms.\n\n"
+ "From an initial guess init_xs,init_us (feasible or not), iterate\n"
+ "over computeDirection and tryStep until stoppingCriteria is below\n"
+ "threshold. It also describes the globalization strategy used\n"
+ "during the numerical optimization.\n"
+ ":param init_xs: initial guess for state trajectory with T+1 elements.\n"
+ ":param init_us: initial guess for control trajectory with T elements.\n"
+ ":param maxiter: maximun allowed number of iterations.\n"
+ ":param isFeasible: true if the init_xs are obtained from integrating the init_us (rollout).\n"
+ ":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", &SolverDDP::computeDirection,
+ SolverDDP_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", &SolverDDP::tryStep,
+ SolverDDP_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("stoppingCriteria", &SolverDDP::stoppingCriteria, bp::args(" self"),
+ "Return a sum of positive parameters whose sum quantifies the DDP termination.")
+ .def("expectedImprovement", &SolverDDP::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.")
+ .def("calc", &SolverDDP::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", &SolverDDP::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", &SolverDDP::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.)")
+ .add_property("Vxx", make_function(&SolverDDP::get_Vxx, bp::return_value_policy<bp::copy_const_reference>()),
+ "Vxx")
+ .add_property("Vx", make_function(&SolverDDP::get_Vx, bp::return_value_policy<bp::copy_const_reference>()), "Vx")
+ .add_property("Qxx", make_function(&SolverDDP::get_Qxx, bp::return_value_policy<bp::copy_const_reference>()),
+ "Qxx")
+ .add_property("Qxu", make_function(&SolverDDP::get_Qxu, bp::return_value_policy<bp::copy_const_reference>()),
+ "Qxu")
+ .add_property("Quu", make_function(&SolverDDP::get_Quu, bp::return_value_policy<bp::copy_const_reference>()),
+ "Quu")
+ .add_property("Qx", make_function(&SolverDDP::get_Qx, bp::return_value_policy<bp::copy_const_reference>()), "Qx")
+ .add_property("Qu", make_function(&SolverDDP::get_Qu, bp::return_value_policy<bp::copy_const_reference>()), "Qu")
+ .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");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_SOLVERS_DDP_HPP_
diff --git a/bindings/python/crocoddyl/core/solvers/fddp.hpp b/bindings/python/crocoddyl/core/solvers/fddp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0223d34e5833ce2a68a759543831af51b31fcd17
--- /dev/null
+++ b/bindings/python/crocoddyl/core/solvers/fddp.hpp
@@ -0,0 +1,77 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_SOLVERS_FDDP_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_SOLVERS_FDDP_HPP_
+
+#include "crocoddyl/core/solvers/fddp.hpp"
+
+namespace crocoddyl {
+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_trySteps, SolverFDDP::tryStep, 0, 1)
+
+void exposeSolverFDDP() {
+ bp::class_<SolverFDDP, bp::bases<SolverDDP> >(
+ "SolverFDDP",
+ "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"
+ "and the forward-pass rollouts this new policy by integrating the system dynamics\n"
+ "along a tuple of optimized control commands U*.\n"
+ ":param shootingProblem: shooting problem (list of action models along trajectory.)",
+ bp::init<ShootingProblem&>(bp::args(" self", " problem"),
+ "Initialize the vector dimension.\n\n"
+ ":param problem: shooting problem.")[bp::with_custodian_and_ward<1, 2>()])
+ .def("solve", &SolverFDDP::solve,
+ SolverFDDP_solves(
+ bp::args(" self", " init_xs=[]", " init_us=[]", " maxiter=100", " isFeasible=False", " regInit=None"),
+ "Compute the optimal trajectory xopt, uopt as lists of T+1 and T terms.\n\n"
+ "From an initial guess init_xs,init_us (feasible or not), iterate\n"
+ "over computeDirection and tryStep until stoppingCriteria is below\n"
+ "threshold. It also describes the globalization strategy used\n"
+ "during the numerical optimization.\n"
+ ":param init_xs: initial guess for state trajectory with T+1 elements.\n"
+ ":param init_us: initial guess for control trajectory with T elements.\n"
+ ":param maxiter: maximun allowed number of iterations.\n"
+ ":param isFeasible: true if the init_xs are obtained from integrating the init_us (rollout).\n"
+ ":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("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.")
+ .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.)");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_SOLVERS_DDP_HPP_
diff --git a/bindings/python/crocoddyl/core/state-base.hpp b/bindings/python/crocoddyl/core/state-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..52ec3366b382b6c5138cda76d342f8b956a561bb
--- /dev/null
+++ b/bindings/python/crocoddyl/core/state-base.hpp
@@ -0,0 +1,231 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_STATE_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_STATE_BASE_HPP_
+
+#include <string>
+#include "crocoddyl/core/state-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class StateAbstract_wrap : public StateAbstract, public bp::wrapper<StateAbstract> {
+ public:
+ StateAbstract_wrap(int nx, int ndx) : StateAbstract(nx, ndx), bp::wrapper<StateAbstract>() {}
+
+ Eigen::VectorXd zero() { return bp::call<Eigen::VectorXd>(this->get_override("zero").ptr()); }
+
+ Eigen::VectorXd rand() { return bp::call<Eigen::VectorXd>(this->get_override("rand").ptr()); }
+
+ Eigen::VectorXd diff_wrap(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1) {
+ assert(x0.size() == nx_ && "x0 has wrong dimension");
+ assert(x1.size() == nx_ && "x1 has wrong dimension");
+ return bp::call<Eigen::VectorXd>(this->get_override("diff").ptr(), (Eigen::VectorXd)x0, (Eigen::VectorXd)x1);
+ }
+
+ void diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::VectorXd> dxout) {
+ dxout = diff_wrap(x0, x1);
+ }
+
+ Eigen::VectorXd integrate_wrap(const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& dx) {
+ assert(x.size() == nx_ && "x has wrong dimension");
+ assert(dx.size() == ndx_ && "dx has wrong dimension");
+ return bp::call<Eigen::VectorXd>(this->get_override("integrate").ptr(), (Eigen::VectorXd)x, (Eigen::VectorXd)dx);
+ }
+
+ void integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::VectorXd> x1out) {
+ x1out = integrate_wrap(x, dx);
+ }
+
+ void Jdiff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond, Jcomponent _firstsecond) {
+ std::string firstsecond;
+ switch (_firstsecond) {
+ case first: {
+ firstsecond = "first";
+ break;
+ }
+ case second: {
+ firstsecond = "second";
+ break;
+ }
+ case both: {
+ firstsecond = "both";
+ break;
+ }
+ default: { firstsecond = "both"; }
+ }
+
+ bp::list res = Jdiff_wrap(x0, x1, firstsecond);
+ if (firstsecond == "both") {
+ Jfirst.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
+ Jsecond.derived() = bp::extract<Eigen::MatrixXd>(res[1])();
+ } else if (firstsecond == "first") {
+ Jfirst.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
+ } else if (firstsecond == "second") {
+ Jsecond.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
+ }
+ }
+
+ bp::list Jdiff_wrap(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ std::string firstsecond) {
+ assert((firstsecond == "both" || firstsecond == "first" || firstsecond == "second") &&
+ "firstsecond must be one of the Jcomponent {both, first, second}");
+ assert(x0.size() == nx_ && "x0 has wrong dimension");
+ assert(x1.size() == nx_ && "x1 has wrong dimension");
+
+ if (firstsecond == "both") {
+ bp::list Jacs =
+ bp::call<bp::list>(this->get_override("Jdiff").ptr(), (Eigen::VectorXd)x0, (Eigen::VectorXd)x1, firstsecond);
+ return Jacs;
+ } else {
+ Eigen::MatrixXd J = bp::call<Eigen::MatrixXd>(this->get_override("Jdiff").ptr(), (Eigen::VectorXd)x0,
+ (Eigen::VectorXd)x1, firstsecond);
+ bp::list list;
+ list.append(J);
+ return list;
+ }
+ }
+
+ void Jintegrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond, Jcomponent _firstsecond) {
+ std::string firstsecond;
+ switch (_firstsecond) {
+ case first: {
+ firstsecond = "first";
+ break;
+ }
+ case second: {
+ firstsecond = "second";
+ break;
+ }
+ case both: {
+ firstsecond = "both";
+ break;
+ }
+ default: { firstsecond = "both"; }
+ }
+
+ bp::list res = Jintegrate_wrap(x, dx, firstsecond);
+ if (firstsecond == "both") {
+ Jfirst.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
+ Jsecond.derived() = bp::extract<Eigen::MatrixXd>(res[1])();
+ } else if (firstsecond == "first") {
+ Jfirst.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
+ } else if (firstsecond == "second") {
+ Jsecond.derived() = bp::extract<Eigen::MatrixXd>(res[0])();
+ }
+ }
+
+ bp::list Jintegrate_wrap(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ std::string firstsecond) {
+ assert((firstsecond == "both" || firstsecond == "first" || firstsecond == "second") &&
+ "firstsecond must be one of the Jcomponent {both, first, second}");
+ assert(x.size() == nx_ && "x has wrong dimension");
+ assert(dx.size() == ndx_ && "dx has wrong dimension");
+
+ if (firstsecond == "both") {
+ bp::list Jacs = bp::call<bp::list>(this->get_override("Jintegrate").ptr(), (Eigen::VectorXd)x,
+ (Eigen::VectorXd)dx, firstsecond);
+ return Jacs;
+ } else {
+ Eigen::MatrixXd J = bp::call<Eigen::MatrixXd>(this->get_override("Jintegrate").ptr(), (Eigen::VectorXd)x,
+ (Eigen::VectorXd)dx, firstsecond);
+ bp::list list;
+ list.append(J);
+ return list;
+ }
+ }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Jdiffs, StateAbstract::Jdiff_wrap, 2, 3)
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Jintegrates, StateAbstract::Jintegrate_wrap, 2, 3)
+
+void exposeStateAbstract() {
+ bp::class_<StateAbstract_wrap, boost::noncopyable>(
+ "StateAbstract",
+ "Abstract class for the state representation.\n\n"
+ "A state is represented by its operators: difference, integrates and their derivatives.\n"
+ "The difference operator returns the value of x1 [-] x2 operation. Instead the integrate\n"
+ "operator returns the value of x [+] dx. These operators are used to compared two points\n"
+ "on the state manifold M or to advance the state given a tangential velocity (Tx M).\n"
+ "Therefore the points x, x1 and x2 belongs to the manifold M; and dx or x1 [-] x2 lie\n"
+ "on its tangential space.",
+ bp::init<int, int>(bp::args(" self", " nx", " ndx"),
+ "Initialize the state dimensions.\n\n"
+ ":param nx: dimension of state configuration vector\n"
+ ":param ndx: dimension of state tangent vector"))
+ .def("zero", pure_virtual(&StateAbstract_wrap::zero), bp::args(" self"),
+ "Return a zero reference state.\n\n"
+ ":return zero reference state")
+ .def("rand", pure_virtual(&StateAbstract_wrap::rand), bp::args(" self"),
+ "Return a random reference state.\n\n"
+ ":return random reference state")
+ .def("diff", pure_virtual(&StateAbstract_wrap::diff_wrap), bp::args(" self", " x0", " x1"),
+ "Operator that differentiates the two state points.\n\n"
+ "It returns the value of x1 [-] x0 operation. Note tha x0 and x1 are points in the state\n"
+ "manifold (in M). Instead the operator result lies in the tangent-space of M.\n"
+ ":param x0: current state (dim state.nx).\n"
+ ":param x1: next state (dim state.nx).\n"
+ ":return x1 [-] x0 value (dim state.ndx).")
+ .def("integrate", pure_virtual(&StateAbstract_wrap::integrate), bp::args(" self", " x", " dx"),
+ "Operator that integrates the current state.\n\n"
+ "It returns the value of x [+] dx operation. x and dx are points in the state.diff(x0,x1) (in M)\n"
+ "and its tangent, respectively. Note that the operator result lies on M too.\n"
+ ":param x: current state (dim state.nx).\n"
+ ":param dx: displacement of the state (dim state.ndx).\n"
+ ":return x [+] dx value (dim state.nx).")
+ .def("Jdiff", pure_virtual(&StateAbstract_wrap::Jdiff_wrap),
+ bp::args(" self", " x0", " x1", " firstsecond = 'both'"),
+ "Compute the partial derivatives of difference operator.\n\n"
+ "The difference operator (x1 [-] x0) is defined by diff(x0, x1). Instead Jdiff\n"
+ "computes its partial derivatives, i.e. \\partial{diff(x0, x1)}{x0} and\n"
+ "\\partial{diff(x0, x1)}{x1}. By default, this function returns the derivatives of the\n"
+ "first and second argument (i.e. firstsecond='both'). However we can also specific the\n"
+ "partial derivative for the first and second variables by setting firstsecond='first'\n"
+ "or firstsecond='second', respectively.\n"
+ ":param x0: current state (dim state.nx).\n"
+ ":param x1: next state (dim state.nx).\n"
+ ":param firstsecond: desired partial derivative\n"
+ ":return the partial derivative(s) of the diff(x0, x1) function")
+ .def("Jintegrate", pure_virtual(&StateAbstract_wrap::Jintegrate_wrap),
+ bp::args(" self", " x", " dx", " firstsecond = 'both'"),
+ "Compute the partial derivatives of integrate operator.\n\n"
+ "The integrate operator (x [+] dx) is defined by integrate(x, dx). Instead Jintegrate\n"
+ "computes its partial derivatives, i.e. \\partial{integrate(x, dx)}{x} and\n"
+ "\\partial{integrate(x, dx)}{dx}. By default, this function returns the derivatives of\n"
+ "the first and second argument (i.e. firstsecond='both'). However we ask for a specific\n"
+ "partial derivative by setting firstsecond='first' or firstsecond='second'.\n"
+ ":param x: current state (dim state.nx).\n"
+ ":param dx: displacement of the state (dim state.ndx).\n"
+ ":param firstsecond: desired partial derivative\n"
+ ":return the partial derivative(s) of the integrate(x, dx) function")
+ .add_property("nx",
+ bp::make_function(&StateAbstract_wrap::get_nx, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of state configuration vector")
+ .add_property("ndx",
+ bp::make_function(&StateAbstract_wrap::get_ndx, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of state tangent vector")
+ .add_property("nq",
+ bp::make_function(&StateAbstract_wrap::get_nq, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of configuration vector")
+ .add_property("nv",
+ bp::make_function(&StateAbstract_wrap::get_nv, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of configuration tangent vector");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_STATE_BASE_HPP_
diff --git a/bindings/python/crocoddyl/core/states/euclidean.hpp b/bindings/python/crocoddyl/core/states/euclidean.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..40ce08279360894948f5ed8f692b3657bf334f94
--- /dev/null
+++ b/bindings/python/crocoddyl/core/states/euclidean.hpp
@@ -0,0 +1,80 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_STATES_EUCLIDEAN_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_STATES_EUCLIDEAN_HPP_
+
+#include "crocoddyl/core/states/euclidean.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeStateEuclidean() {
+ bp::class_<StateVector, bp::bases<StateAbstract> >(
+ "StateVector",
+ "Euclidean state vector.\n\n"
+ "For this type of states, the difference and integrate operators are described by\n"
+ "arithmetic subtraction and addition operations, respectively. Due to the Euclidean\n"
+ "point and its velocity lie in the same space, all Jacobians are described throught\n"
+ "the identity matrix.",
+ bp::init<int>(bp::args(" self", " nx"),
+ "Initialize the vector dimension.\n\n"
+ ":param nx: dimension of state"))
+ .def("zero", &StateVector::zero, bp::args(" self"),
+ "Return a zero reference state.\n\n"
+ ":return zero reference state")
+ .def("rand", &StateVector::rand, bp::args(" self"),
+ "Return a random reference state.\n\n"
+ ":return random reference state")
+ .def("diff", &StateVector::diff_wrap, bp::args(" self", " x0", " x1"),
+ "Operator that differentiates the two state points.\n\n"
+ "It returns the value of x1 [-] x0 operation. Due to a state vector lies in\n"
+ "the Euclidean space, this operator is defined with arithmetic subtraction.\n"
+ ":param x0: current state (dim state.nx()).\n"
+ ":param x1: next state (dim state.nx()).\n"
+ ":return x1 - x0 value (dim state.nx()).")
+ .def("integrate", &StateVector::integrate_wrap, bp::args(" self", " x", " dx"),
+ "Operator that integrates the current state.\n\n"
+ "It returns the value of x [+] dx operation. Due to a state vector lies in\n"
+ "the Euclidean space, this operator is defined with arithmetic addition.\n"
+ "Futhermore there is no timestep here (i.e. dx = v*dt), note this if you're\n"
+ "integrating a velocity v during an interval dt.\n"
+ ":param x: current state (dim state.nx()).\n"
+ ":param dx: displacement of the state (dim state.nx()).\n"
+ ":return x + dx value (dim state.nx()).")
+ .def("Jdiff", &StateVector::Jdiff_wrap,
+ Jdiffs(bp::args(" self", " x0", " x1", " firstsecond = 'both'"),
+ "Compute the partial derivatives of arithmetic substraction.\n\n"
+ "Both Jacobian matrices are represented throught an identity matrix, with the exception\n"
+ "that the first partial derivatives (w.r.t. x0) has negative signed. By default, this\n"
+ "function returns the derivatives of the first and second argument (i.e.\n"
+ "firstsecond='both'). However we ask for a specific partial derivative by setting\n"
+ "firstsecond='first' or firstsecond='second'.\n"
+ ":param x0: current state (dim state.nx()).\n"
+ ":param x1: next state (dim state.nx()).\n"
+ ":param firstsecond: desired partial derivative\n"
+ ":return the partial derivative(s) of the diff(x0, x1) function"))
+ .def("Jintegrate", &StateVector::Jintegrate_wrap,
+ Jintegrates(bp::args(" self", " x", " dx", " firstsecond = 'both'"),
+ "Compute the partial derivatives of arithmetic addition.\n\n"
+ "Both Jacobian matrices are represented throught an identity matrix. By default, this\n"
+ "function returns the derivatives of the first and second argument (i.e.\n"
+ "firstsecond='both'). However we ask for a specific partial derivative by setting\n"
+ "firstsecond='first' or firstsecond='second'.\n"
+ ":param x: current state (dim state.nx()).\n"
+ ":param dx: displacement of the state (dim state.nx()).\n"
+ ":param firstsecond: desired partial derivative\n"
+ ":return the partial derivative(s) of the integrate(x, dx) function"));
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_STATES_EUCLIDEAN_HPP_
diff --git a/bindings/python/crocoddyl/core/utils/callbacks.hpp b/bindings/python/crocoddyl/core/utils/callbacks.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b748aa5eb953bd15c8e448d5fedae77145ab54f
--- /dev/null
+++ b/bindings/python/crocoddyl/core/utils/callbacks.hpp
@@ -0,0 +1,35 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CORE_UTILS_CALLBACKS_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_CORE_UTILS_CALLBACKS_HPP_
+
+#include "crocoddyl/core/utils/callbacks.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeCallbacks() {
+ bp::enum_<VerboseLevel>("VerboseLevel").value("_1", _1).value("_2", _2);
+
+ bp::class_<CallbackVerbose, bp::bases<CallbackAbstract> >(
+ "CallbackVerbose", "Callback function for printing the solver values.",
+ bp::init<bp::optional<VerboseLevel> >(bp::args(" self", " level=_1"),
+ "Initialize the differential verbose callback.\n\n"
+ ":param level: verbose level"))
+ .def("__call__", &CallbackVerbose::operator(), bp::args(" self", " solver"),
+ "Run the callback function given a solver.\n\n"
+ ":param solver: solver to be diagnostic");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_CORE_UTILS_CALLBACKS_HPP_
diff --git a/bindings/python/crocoddyl/crocoddyl.cpp b/bindings/python/crocoddyl/crocoddyl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2deaf62ff45b7d4c08a68657f4b4a015a8abbb2d
--- /dev/null
+++ b/bindings/python/crocoddyl/crocoddyl.cpp
@@ -0,0 +1,52 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#define PYTHON_BINDINGS
+
+#include <pinocchio/fwd.hpp>
+#include <boost/python.hpp>
+#include <eigenpy/eigenpy.hpp>
+
+#include "python/crocoddyl/core.hpp"
+#include "python/crocoddyl/multibody.hpp"
+#include "crocoddyl/core/utils/version.hpp"
+#include "python/crocoddyl/utils.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+BOOST_PYTHON_MODULE(libcrocoddyl_pywrap) {
+ bp::scope().attr("__version__") = printVersion();
+
+ eigenpy::enableEigenPy();
+
+ typedef double Scalar;
+ typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
+ typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+ typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+ typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+ eigenpy::enableEigenPySpecific<Vector2>();
+ eigenpy::enableEigenPySpecific<Vector3>();
+ eigenpy::enableEigenPySpecific<VectorX>();
+ eigenpy::enableEigenPySpecific<MatrixX>();
+
+ // Register Eigen converters between std::vector and Python list
+ 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<VectorX, std::allocator<VectorX> > >()
+ .from_python<std::vector<MatrixX, std::allocator<MatrixX> > >();
+
+ exposeCore();
+ exposeMultibody();
+}
+
+} // namespace python
+} // namespace crocoddyl
diff --git a/bindings/python/crocoddyl/multibody.hpp b/bindings/python/crocoddyl/multibody.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..8f925d04dc0c4585dcfb40f3e6a40fde3e3728eb
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody.hpp
@@ -0,0 +1,66 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_HPP_
+
+#include "python/crocoddyl/multibody/frames.hpp"
+#include "python/crocoddyl/multibody/states/multibody.hpp"
+#include "python/crocoddyl/multibody/actuations/floating-base.hpp"
+#include "python/crocoddyl/multibody/actuations/full.hpp"
+#include "python/crocoddyl/multibody/cost-base.hpp"
+#include "python/crocoddyl/multibody/contact-base.hpp"
+#include "python/crocoddyl/multibody/impulse-base.hpp"
+#include "python/crocoddyl/multibody/costs/cost-sum.hpp"
+#include "python/crocoddyl/multibody/costs/state.hpp"
+#include "python/crocoddyl/multibody/costs/control.hpp"
+#include "python/crocoddyl/multibody/costs/com-position.hpp"
+#include "python/crocoddyl/multibody/costs/frame-placement.hpp"
+#include "python/crocoddyl/multibody/costs/frame-translation.hpp"
+#include "python/crocoddyl/multibody/costs/frame-velocity.hpp"
+#include "python/crocoddyl/multibody/contacts/multiple-contacts.hpp"
+#include "python/crocoddyl/multibody/contacts/contact-3d.hpp"
+#include "python/crocoddyl/multibody/contacts/contact-6d.hpp"
+#include "python/crocoddyl/multibody/impulses/multiple-impulses.hpp"
+#include "python/crocoddyl/multibody/impulses/impulse-3d.hpp"
+#include "python/crocoddyl/multibody/impulses/impulse-6d.hpp"
+#include "python/crocoddyl/multibody/actions/free-fwddyn.hpp"
+#include "python/crocoddyl/multibody/actions/contact-fwddyn.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+void exposeMultibody() {
+ exposeFrames();
+ exposeStateMultibody();
+ exposeActuationFloatingBase();
+ exposeActuationFull();
+ exposeCostMultibody();
+ exposeContactAbstract();
+ exposeImpulseAbstract();
+ exposeCostSum();
+ exposeCostState();
+ exposeCostControl();
+ exposeCostCoMPosition();
+ exposeCostFramePlacement();
+ exposeCostFrameTranslation();
+ exposeCostFrameVelocity();
+ exposeContactMultiple();
+ exposeContact3D();
+ exposeContact6D();
+ exposeImpulseMultiple();
+ exposeImpulse3D();
+ exposeImpulse6D();
+ exposeDifferentialActionFreeFwdDynamics();
+ exposeDifferentialActionContactFwdDynamics();
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_HPP_
diff --git a/bindings/python/crocoddyl/multibody/actions/contact-fwddyn.hpp b/bindings/python/crocoddyl/multibody/actions/contact-fwddyn.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..336ae19058fafa953ac76456fa15628978cc6928
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/actions/contact-fwddyn.hpp
@@ -0,0 +1,111 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CONTACT_FWDDYN_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
+
+#include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeDifferentialActionContactFwdDynamics() {
+ bp::class_<DifferentialActionModelContactFwdDynamics, bp::bases<DifferentialActionModelAbstract> >(
+ "DifferentialActionModelContactFwdDynamics",
+ "Differential action model for contact forward dynamics in multibody systems.\n\n"
+ "The contact 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&, ActuationModelFloatingBase&, ContactModelMultiple&, CostModelSum&,
+ bp::optional<double, bool> >(
+ bp::args(" self", " state", " actuation", " contacts", " costs", " inv_damping=0.", "enable_force=False"),
+ "Initialize the constrained 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 actuation: floating-base actuation model\n"
+ ":param contacts: multiple contact model\n"
+ ":param costs: stack of cost functions\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, bp::with_custodian_and_ward<1, 5> > > >()])
+ .def("calc", &DifferentialActionModelContactFwdDynamics::calc_wrap,
+ DiffActionModel_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 contact. The\n"
+ "contacts are modelled as holonomic constraints.\n"
+ "Additionally it computes the cost value associated to this state and control pair.\n"
+ ":param data: contact forward-dynamics action data\n"
+ ":param x: time-continuous state vector\n"
+ ":param u: time-continuous control input"))
+ .def<void (DifferentialActionModelContactFwdDynamics::*)(
+ const boost::shared_ptr<DifferentialActionDataAbstract>&, const Eigen::VectorXd&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &DifferentialActionModelContactFwdDynamics::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: contact 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 (DifferentialActionModelContactFwdDynamics::*)(
+ const boost::shared_ptr<DifferentialActionDataAbstract>&, const Eigen::VectorXd&, const Eigen::VectorXd&)>(
+ "calcDiff", &DifferentialActionModelContactFwdDynamics::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u"))
+ .def<void (DifferentialActionModelContactFwdDynamics::*)(
+ const boost::shared_ptr<DifferentialActionDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &DifferentialActionModelContactFwdDynamics::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (DifferentialActionModelContactFwdDynamics::*)(
+ const boost::shared_ptr<DifferentialActionDataAbstract>&, const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &DifferentialActionModelContactFwdDynamics::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &DifferentialActionModelContactFwdDynamics::createData, bp::args(" self"),
+ "Create the contact forward dynamics differential action data.")
+ .add_property("pinocchio",
+ bp::make_function(&DifferentialActionModelContactFwdDynamics::get_pinocchio,
+ bp::return_internal_reference<>()),
+ "multibody model (i.e. pinocchio model)")
+ .add_property("actuation",
+ bp::make_function(&DifferentialActionModelContactFwdDynamics::get_actuation,
+ bp::return_internal_reference<>()),
+ "actuation model")
+ .add_property("contacts",
+ bp::make_function(&DifferentialActionModelContactFwdDynamics::get_contacts,
+ bp::return_internal_reference<>()),
+ "multiple contact model")
+ .add_property(
+ "costs",
+ bp::make_function(&DifferentialActionModelContactFwdDynamics::get_costs, bp::return_internal_reference<>()),
+ "total cost model")
+ .add_property("armature",
+ bp::make_function(&DifferentialActionModelContactFwdDynamics::get_armature,
+ bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&DifferentialActionModelContactFwdDynamics::set_armature),
+ "set an armature mechanism in the joints")
+ .add_property("JMinvJt_damping",
+ bp::make_function(&DifferentialActionModelContactFwdDynamics::get_damping_factor,
+ bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&DifferentialActionModelContactFwdDynamics::set_damping_factor),
+ "Damping factor for cholesky decomposition of JMinvJt");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
diff --git a/bindings/python/crocoddyl/multibody/actions/free-fwddyn.hpp b/bindings/python/crocoddyl/multibody/actions/free-fwddyn.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e8f59cea519617b5ca6c6923fb335c69f16d0633
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/actions/free-fwddyn.hpp
@@ -0,0 +1,101 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_FREE_FWDDYN_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
+
+#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeDifferentialActionFreeFwdDynamics() {
+ bp::class_<DifferentialActionModelFreeFwdDynamics, bp::bases<DifferentialActionModelAbstract> >(
+ "DifferentialActionModelFreeFwdDynamics",
+ "Differential action model for free forward dynamics in multibody systems.\n\n"
+ "This class implements a the dynamics using Articulate Body Algorithm (ABA),\n"
+ "or 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&, CostModelSum&>(bp::args(" self", " state", " costs"),
+ "Initialize the free forward-dynamics action model.\n\n"
+ ":param state: multibody state\n"
+ ":param costs: stack of cost functions")
+ [bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
+ .def("calc", &DifferentialActionModelFreeFwdDynamics::calc_wrap,
+ DiffActionModel_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 without any contact.\n"
+ "Additionally it computes the cost value associated to this state and control pair.\n"
+ ":param data: free forward-dynamics action data\n"
+ ":param x: time-continuous state vector\n"
+ ":param u: time-continuous control input"))
+ .def<void (DifferentialActionModelFreeFwdDynamics::*)(
+ const boost::shared_ptr<DifferentialActionDataAbstract>&, const Eigen::VectorXd&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &DifferentialActionModelFreeFwdDynamics::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the differential multibody system (free of contact) and\n"
+ "its cost 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: free 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 (DifferentialActionModelFreeFwdDynamics::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&, const Eigen::VectorXd&)>(
+ "calcDiff", &DifferentialActionModelFreeFwdDynamics::calcDiff_wrap, bp::args(" self", " data", " x", " u"))
+ .def<void (DifferentialActionModelFreeFwdDynamics::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &DifferentialActionModelFreeFwdDynamics::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (DifferentialActionModelFreeFwdDynamics::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &DifferentialActionModelFreeFwdDynamics::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &DifferentialActionModelFreeFwdDynamics::createData, bp::args(" self"),
+ "Create the free forward dynamics differential action data.")
+ .add_property(
+ "pinocchio",
+ bp::make_function(&DifferentialActionModelFreeFwdDynamics::get_pinocchio, bp::return_internal_reference<>()),
+ "multibody model (i.e. pinocchio model)")
+ .add_property(
+ "costs",
+ bp::make_function(&DifferentialActionModelFreeFwdDynamics::get_costs, bp::return_internal_reference<>()),
+ "total cost model")
+ .add_property("armature",
+ bp::make_function(&DifferentialActionModelFreeFwdDynamics::get_armature,
+ bp::return_value_policy<bp::return_by_value>()),
+ bp::make_function(&DifferentialActionModelFreeFwdDynamics::set_armature),
+ "set an armature mechanism in the joints");
+
+ bp::register_ptr_to_python<boost::shared_ptr<DifferentialActionDataFreeFwdDynamics> >();
+
+ bp::class_<DifferentialActionDataFreeFwdDynamics, bp::bases<DifferentialActionDataAbstract> >(
+ "DifferentialActionDataFreeFwdDynamics", "Action data for the differential LQR system.",
+ bp::init<DifferentialActionModelFreeFwdDynamics*>(bp::args(" self", " model"),
+ "Create free forward-dynamics action data.\n\n"
+ ":param model: free forward-dynamics action model"))
+ .add_property(
+ "pinocchio",
+ bp::make_getter(&DifferentialActionDataFreeFwdDynamics::pinocchio, bp::return_internal_reference<>()),
+ "pinocchio data")
+ .add_property("costs",
+ bp::make_getter(&DifferentialActionDataFreeFwdDynamics::costs,
+ bp::return_value_policy<bp::return_by_value>()),
+ "total cost data");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
diff --git a/bindings/python/crocoddyl/multibody/actuations/floating-base.hpp b/bindings/python/crocoddyl/multibody/actuations/floating-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..980f56f82ec536484d5c743ff1984f467ba8d4c0
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/actuations/floating-base.hpp
@@ -0,0 +1,54 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_ACTUATIONS_FLOATING_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
+
+#include "crocoddyl/multibody/actuations/floating-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeActuationFloatingBase() {
+ bp::class_<ActuationModelFloatingBase, bp::bases<ActuationModelAbstract> >(
+ "ActuationModelFloatingBase",
+ "Floating-base actuation models.\n\n"
+ "It simplies consider a floating-base actuation model, where the first 6 elements are unactuated.",
+ bp::init<StateMultibody&>(bp::args(" self", " state"),
+ "Initialize the floating-base actuation model.\n\n"
+ ":param state: state of multibody system")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &ActuationModelFloatingBase::calc_wrap, bp::args(" self", " data", " x", " u"),
+ "Compute the actuation signal from the control input u.\n\n"
+ "It describes the time-continuos evolution of the floating-base actuation model.\n"
+ ":param data: floating-base actuation data\n"
+ ":param x: state vector\n"
+ ":param u: control input")
+ .def("calcDiff", &ActuationModelFloatingBase::calcDiff_wrap,
+ ActuationModel_calcDiff_wraps(
+ bp::args(" self", " data", " x", " u", " recalc=True"),
+ "Compute the derivatives of the actuation model.\n\n"
+ "It computes the partial derivatives of the floating-base actuation. It assumes that you\n"
+ "create the data using this class. The reason is that the derivatives are constant and\n"
+ "defined in createData. The derivatives are constant, so we don't write again these values.\n"
+ ":param data: floating-base actuation data\n"
+ ":param x: state vector\n"
+ ":param u: control input\n"
+ ":param recalc: If true, it updates the actuation signal."))
+ .def("createData", &ActuationModelFloatingBase::createData, bp::args(" self"),
+ "Create the floating-base actuation data.\n\n"
+ "Each actuation model (AM) has its own data that needs to be allocated.\n"
+ "This function returns the allocated data for a predefined AM.\n"
+ ":return AM data.");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
diff --git a/bindings/python/crocoddyl/multibody/actuations/full.hpp b/bindings/python/crocoddyl/multibody/actuations/full.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..61d80b8a96a4a4fafceb0b24f2973a355770e235
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/actuations/full.hpp
@@ -0,0 +1,51 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_ACTUATIONS_FULL_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
+
+#include "crocoddyl/multibody/actuations/full.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeActuationFull() {
+ bp::class_<ActuationModelFull, bp::bases<ActuationModelAbstract> >(
+ "ActuationModelFull", "Full actuation models.",
+ bp::init<StateMultibody&>(bp::args(" self", " state"),
+ "Initialize the full actuation model.\n\n"
+ ":param state: state of multibody system")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &ActuationModelFull::calc_wrap, bp::args(" self", " data", " x", " u"),
+ "Compute the actuation signal from the control input u.\n\n"
+ ":param data: full actuation data\n"
+ ":param x: state vector\n"
+ ":param u: control input")
+ .def("calcDiff", &ActuationModelFull::calcDiff_wrap,
+ ActuationModel_calcDiff_wraps(
+ bp::args(" self", " data", " x", " u", " recalc=True"),
+ "Compute the derivatives of the actuation model.\n\n"
+ "It computes the partial derivatives of the full actuation. It assumes that you\n"
+ "create the data using this class. The reason is that the derivatives are constant and\n"
+ "defined in createData. The Hessian is constant, so we don't write again this value.\n"
+ ":param data: full actuation data\n"
+ ":param x: state vector\n"
+ ":param u: control input\n"
+ ":param recalc: If true, it updates the actuation signal."))
+ .def("createData", &ActuationModelFull::createData, bp::args(" self"),
+ "Create the full actuation data.\n\n"
+ "Each actuation model (AM) has its own data that needs to be allocated.\n"
+ "This function returns the allocated data for a predefined AM.\n"
+ ":return AM data.");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
diff --git a/bindings/python/crocoddyl/multibody/contact-base.hpp b/bindings/python/crocoddyl/multibody/contact-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7a2bb831743af06d19a349629ef9d1d12c31b60
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/contact-base.hpp
@@ -0,0 +1,124 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CONTACT_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
+
+#include "crocoddyl/multibody/contact-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class ContactModelAbstract_wrap : public ContactModelAbstract, public bp::wrapper<ContactModelAbstract> {
+ public:
+ ContactModelAbstract_wrap(StateMultibody& state, int nc, int nu) : ContactModelAbstract(state, nc, nu) {}
+ ContactModelAbstract_wrap(StateMultibody& state, int nc) : ContactModelAbstract(state, nc) {}
+
+ void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x) {
+ assert(x.size() == state_.get_nx() && "x has wrong dimension");
+ return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)x);
+ }
+
+ void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const bool& recalc = true) {
+ assert(x.size() == state_.get_nx() && "x has wrong dimension");
+ 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);
+ }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ContactModel_calcDiff_wraps, ContactModelAbstract::calcDiff_wrap, 2, 3)
+
+void exposeContactAbstract() {
+ bp::class_<ContactModelAbstract_wrap, boost::noncopyable>(
+ "ContactModelAbstract",
+ "Abstract rigid contact model.\n\n"
+ "It defines a template for rigid contact models based on acceleration-based holonomic constraints.\n"
+ "The calc and calcDiff functions compute the contact Jacobian and drift (holonomic constraint) or\n"
+ "the derivatives of the holonomic constraint, respectively.",
+ bp::init<StateMultibody&, int, bp::optional<int> >(
+ bp::args(" self", " state", " nc", " nu=state.nv"),
+ "Initialize the contact model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param nc: dimension of contact model\n"
+ ":param nu: dimension of the control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", pure_virtual(&ContactModelAbstract_wrap::calc), bp::args(" self", " data", " x"),
+ "Compute the contact Jacobian and drift.\n\n"
+ "The rigid contact model throught acceleration-base holonomic constraint\n"
+ "of the contact frame placement.\n"
+ ":param data: contact data\n"
+ ":param x: state vector")
+ .def("calcDiff", pure_virtual(&ContactModelAbstract_wrap::calcDiff),
+ bp::args(" self", " data", " x", " recalc=True"),
+ "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 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("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"
+ ":param data: Pinocchio data\n"
+ ":return contact data.")
+ .add_property("state",
+ bp::make_function(&ContactModelAbstract_wrap::get_state, bp::return_internal_reference<>()),
+ "state of the multibody system")
+ .add_property(
+ "nc", bp::make_function(&ContactModelAbstract_wrap::get_nc, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of contact")
+ .add_property(
+ "nu", bp::make_function(&ContactModelAbstract_wrap::get_nu, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of control");
+
+ bp::class_<ContactDataAbstract, boost::shared_ptr<ContactDataAbstract>, boost::noncopyable>(
+ "ContactDataAbstract", "Abstract class for contact datas.\n\n",
+ 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 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")
+ .def_readwrite("joint", &ContactDataAbstract::joint, "joint index of the contact frame")
+ .def_readwrite("f", &ContactDataAbstract::f, "external spatial forces");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-3d.hpp b/bindings/python/crocoddyl/multibody/contacts/contact-3d.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..18d8188122e6d745bd1731c61402e38f6a2d0491
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/contacts/contact-3d.hpp
@@ -0,0 +1,74 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CONTACTS_CONTACT_3D_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
+
+#include "crocoddyl/multibody/contacts/contact-3d.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeContact3D() {
+ bp::class_<ContactModel3D, bp::bases<ContactModelAbstract> >(
+ "ContactModel3D",
+ "Rigid 3D contact model.\n\n"
+ "It defines a rigid 3D contact models (point contact) based on acceleration-based holonomic constraints.\n"
+ "The calc and calcDiff functions compute the contact Jacobian and drift (holonomic constraint) or\n"
+ "the derivatives of the holonomic constraint, respectively.",
+ bp::init<StateMultibody&, FrameTranslation, int, bp::optional<Eigen::Vector2d> >(
+ bp::args(" self", " state", " xref", " nu=state.nv", " gains=np.matrix([ [0.],[0.] ]"),
+ "Initialize the contact model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param xref: reference frame translation\n"
+ ":param nu: dimension of control vector\n"
+ ":param gains: gains of the contact model")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, FrameTranslation, bp::optional<Eigen::Vector2d> >(
+ bp::args(" self", " state", " xref"),
+ "Initialize the state cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param Mref: reference frame translation\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &ContactModel3D::calc_wrap, bp::args(" self", " data", " x"),
+ "Compute the 3D contact Jacobian and drift.\n\n"
+ "The rigid contact model throught acceleration-base holonomic constraint\n"
+ "of the contact frame placement.\n"
+ ":param data: contact data\n"
+ ":param x: state vector")
+ .def("calcDiff", &ContactModel3D::calcDiff_wrap,
+ ContactModel_calcDiff_wraps(bp::args(" self", " data", " x", " recalc=True"),
+ "Compute the derivatives of the 3D 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 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"
+ ":param data: cost data\n"
+ ":param lambda: Lagrangian vector")
+ .def("createData", &ContactModel3D::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the 3D 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"
+ ":param data: Pinocchio data\n"
+ ":return contact data.")
+ .add_property("xref", bp::make_function(&ContactModel3D::get_xref, bp::return_internal_reference<>()),
+ "reference frame translation")
+ .add_property("gains",
+ bp::make_function(&ContactModel3D::get_gains, bp::return_value_policy<bp::return_by_value>()),
+ "contact gains");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-6d.hpp b/bindings/python/crocoddyl/multibody/contacts/contact-6d.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ca184785dbe95de1b971b5bb378cffeedf287194
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/contacts/contact-6d.hpp
@@ -0,0 +1,74 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CONTACTS_CONTACT_6D_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
+
+#include "crocoddyl/multibody/contacts/contact-6d.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeContact6D() {
+ bp::class_<ContactModel6D, bp::bases<ContactModelAbstract> >(
+ "ContactModel6D",
+ "Rigid 6D contact model.\n\n"
+ "It defines a rigid 6D contact models based on acceleration-based holonomic constraints.\n"
+ "The calc and calcDiff functions compute the contact Jacobian and drift (holonomic constraint) or\n"
+ "the derivatives of the holonomic constraint, respectively.",
+ bp::init<StateMultibody&, FramePlacement, int, bp::optional<Eigen::Vector2d> >(
+ bp::args(" self", " state", " Mref", " nu=state.nv", " gains=np.matrix([ [0.],[0.] ]"),
+ "Initialize the contact model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param Mref: reference frame placement\n"
+ ":param nu: dimension of control vector\n"
+ ":param gains: gains of the contact model")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, FramePlacement, bp::optional<Eigen::Vector2d> >(
+ bp::args(" self", " state", " Mref"),
+ "Initialize the state cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param Mref: reference frame placement\n"
+ ":param nu = dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &ContactModel6D::calc_wrap, bp::args(" self", " data", " x"),
+ "Compute the 6D contact Jacobian and drift.\n\n"
+ "The rigid contact model throught acceleration-base holonomic constraint\n"
+ "of the contact frame placement.\n"
+ ":param data: contact data\n"
+ ":param x: state vector")
+ .def("calcDiff", &ContactModel6D::calcDiff_wrap,
+ ContactModel_calcDiff_wraps(bp::args(" self", " data", " x", " recalc=True"),
+ "Compute the derivatives of the 6D 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 x: state vector\n"
+ ":param recalc: If true, it updates the contact Jacobian and drift."))
+ .def("updateLagrangian", &ContactModel6D::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("createData", &ContactModel6D::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the 6D 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"
+ ":param data: Pinocchio data\n"
+ ":return contact data.")
+ .add_property("Mref", bp::make_function(&ContactModel6D::get_Mref, bp::return_internal_reference<>()),
+ "reference frame placement")
+ .add_property("gains",
+ bp::make_function(&ContactModel6D::get_gains, bp::return_value_policy<bp::return_by_value>()),
+ "contact gains");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
diff --git a/bindings/python/crocoddyl/multibody/contacts/multiple-contacts.hpp b/bindings/python/crocoddyl/multibody/contacts/multiple-contacts.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5a84631652ed7a65884cf4a8cf3c24ce647c3073
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/contacts/multiple-contacts.hpp
@@ -0,0 +1,122 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_CONTACTS_MULTIPLE_CONTACTS_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
+
+#include <functional>
+#include <map>
+#include <memory>
+#include <utility>
+#include <string>
+#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ContactModelMultiple_calcDiff_wraps, ContactModelMultiple::calcDiff_wrap, 2, 3)
+
+void exposeContactMultiple() {
+ // Register custom converters between std::map and Python dict
+ typedef boost::shared_ptr<ContactDataAbstract> ContactDataPtr;
+ bp::to_python_converter<std::map<std::string, ContactItem, std::less<std::string>,
+ std::allocator<std::pair<const std::string, ContactItem> > >,
+ map_to_dict<std::string, ContactItem> >();
+ bp::to_python_converter<std::map<std::string, ContactDataPtr, std::less<std::string>,
+ std::allocator<std::pair<const std::string, ContactDataPtr> > >,
+ map_to_dict<std::string, ContactDataPtr, false> >();
+ dict_to_map<std::string, ContactItem>().from_python();
+ dict_to_map<std::string, ContactDataPtr>().from_python();
+
+ bp::class_<ContactItem, boost::noncopyable>(
+ "ContactItem", "Describe a contact item.\n\n",
+ bp::init<std::string, ContactModelAbstract*>(
+ bp::args(" self", " name", " contact"),
+ "Initialize the contact item.\n\n"
+ ":param name: contact name\n"
+ ":param contact: contact model")[bp::with_custodian_and_ward<1, 3>()])
+ .def_readwrite("name", &ContactItem::name, "contact name")
+ .add_property("contact", bp::make_getter(&ContactItem::contact, bp::return_internal_reference<>()),
+ "contact model");
+
+ bp::class_<ContactModelMultiple, boost::noncopyable>(
+ "ContactModelMultiple", bp::init<StateMultibody&, bp::optional<int> >(
+ bp::args(" self", " state", " nu=state.nv"),
+ "Initialize the multiple contact model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def("addContact", &ContactModelMultiple::addContact, bp::with_custodian_and_ward<1, 3>(),
+ bp::args(" self", " name", " contact"),
+ "Add a contact item.\n\n"
+ ":param name: contact name\n"
+ ":param contact: contact model")
+ .def("removeContact", &ContactModelMultiple::removeContact, bp::args(" self", " name"),
+ "Remove a contact item.\n\n"
+ ":param name: contact name")
+ .def("calc", &ContactModelMultiple::calc_wrap, bp::args(" self", " data", " x"),
+ "Compute the total contact Jacobian and drift.\n\n"
+ "The rigid contact model throught acceleration-base holonomic constraint\n"
+ "of the contact frame placement.\n"
+ ":param data: contact data\n"
+ ":param x: state vector")
+ .def("calcDiff", &ContactModelMultiple::calcDiff_wrap,
+ ContactModelMultiple_calcDiff_wraps(
+ bp::args(" self", " data", " x", " recalc=True"),
+ "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 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("createData", &ContactModelMultiple::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the total contact data.\n\n"
+ ":param data: Pinocchio data\n"
+ ":return total contact data.")
+ .add_property(
+ "contacts",
+ bp::make_function(&ContactModelMultiple::get_contacts, bp::return_value_policy<bp::return_by_value>()),
+ "stack of contacts")
+ .add_property("state", bp::make_function(&ContactModelMultiple::get_state, bp::return_internal_reference<>()),
+ "state of the multibody system")
+ .add_property("nc",
+ bp::make_function(&ContactModelMultiple::get_nc, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of the total contact vector")
+ .add_property("nu",
+ bp::make_function(&ContactModelMultiple::get_nu, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of control vector");
+
+ bp::class_<ContactDataMultiple, boost::shared_ptr<ContactDataMultiple>, bp::bases<ContactDataAbstract> >(
+ "ContactDataMultiple", "Data class for multiple contacts.\n\n",
+ bp::init<ContactModelMultiple*, pinocchio::Data*>(
+ bp::args(" self", " model", " data"),
+ "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("contacts",
+ bp::make_getter(&ContactDataMultiple::contacts, bp::return_value_policy<bp::return_by_value>()),
+ "stack of contacts data")
+ .def_readwrite("fext", &ContactDataMultiple::fext, "external spatial forces");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
diff --git a/bindings/python/crocoddyl/multibody/cost-base.hpp b/bindings/python/crocoddyl/multibody/cost-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4aa449ac34cda2b574057577de95e57dd62d23d6
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/cost-base.hpp
@@ -0,0 +1,150 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_COST_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COST_BASE_HPP_
+
+#include "crocoddyl/multibody/cost-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class CostModelAbstract_wrap : public CostModelAbstract, public bp::wrapper<CostModelAbstract> {
+ public:
+ CostModelAbstract_wrap(StateMultibody& state, ActivationModelAbstract& activation, int nu,
+ bool with_residuals = true)
+ : CostModelAbstract(state, activation, nu, with_residuals) {}
+
+ CostModelAbstract_wrap(StateMultibody& state, ActivationModelAbstract& activation, bool with_residuals = true)
+ : CostModelAbstract(state, activation, with_residuals) {}
+
+ CostModelAbstract_wrap(StateMultibody& state, int nr, int nu, bool with_residuals = true)
+ : CostModelAbstract(state, nr, nu, with_residuals), bp::wrapper<CostModelAbstract>() {}
+
+ CostModelAbstract_wrap(StateMultibody& state, int nr, bool with_residuals = true)
+ : CostModelAbstract(state, nr, with_residuals), bp::wrapper<CostModelAbstract>() {}
+
+ 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");
+ 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");
+ return bp::call<void>(this->get_override("calcDiff").ptr(), data, (Eigen::VectorXd)x, (Eigen::VectorXd)u, recalc);
+ }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(CostModel_calc_wraps, CostModelAbstract::calc_wrap, 2, 3)
+
+void exposeCostMultibody() {
+ bp::class_<CostModelAbstract_wrap, boost::noncopyable>(
+ "CostModelAbstract",
+ "Abstract multibody cost model using Pinocchio.\n\n"
+ "It defines a template of cost model whose residual and derivatives can be retrieved from\n"
+ "Pinocchio data, through the calc and calcDiff functions, respectively.",
+ bp::init<StateMultibody&, ActivationModelAbstract&, int, bp::optional<bool> >(
+ bp::args(" self", " state", " activation", " nu=model.nv", " withResiduals=True"),
+ "Initialize the cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: Activation model\n"
+ ":param nu: dimension of control vector\n"
+ ":param withResiduals: true if the cost function has residuals")
+ [bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&, bp::optional<bool> >(
+ bp::args(" self", " state", " activation", " withResiduals=True"),
+ "Initialize the cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: Activation model\n"
+ ":param withResiduals: true if the cost function has residuals")
+ [bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, int, int, bp::optional<bool> >(
+ bp::args(" self", " state", " nr", " nu=model.nv", " withResiduals=True"),
+ "Initialize the cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e. crocoddyl.ActivationModelQuad(nr).\n"
+ ":param state: state of the multibody system\n"
+ ":param nr: dimension of cost vector\n"
+ ":param nu: dimension of control vector\n"
+ ":param withResiduals: true if the cost function has residuals")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, int, bp::optional<bool> >(
+ bp::args(" self", " state", " nr", " withResiduals=True"),
+ "Initialize the cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e. crocoddyl.ActivationModelQuad(nr).\n"
+ ":param state: state of the multibody system\n"
+ ":param nr: dimension of cost vector\n"
+ ":param withResiduals: true if the cost function has residuals")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", pure_virtual(&CostModelAbstract_wrap::calc), bp::args(" self", " data", " x", " u"),
+ "Compute the cost value and its residuals.\n\n"
+ ":param data: cost data\n"
+ ":param x: state vector\n"
+ ":param u: control input")
+ .def("calcDiff", pure_virtual(&CostModelAbstract_wrap::calcDiff),
+ bp::args(" self", " data", " x", " u", " recalc=True"),
+ "Compute the derivatives of the cost function and its residuals.\n\n"
+ ":param data: cost data\n"
+ ":param x: state vector\n"
+ ":param u: control input\n"
+ ":param recalc: If true, it updates the cost value.")
+ .def("createData", &CostModelAbstract_wrap::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the cost data.\n\n"
+ "Each cost model has its own data that needs to be allocated. This function\n"
+ "returns the allocated data for a predefined cost.\n"
+ ":param data: Pinocchio data\n"
+ ":return cost data.")
+ .add_property("state", bp::make_function(&CostModelAbstract_wrap::get_state, bp::return_internal_reference<>()),
+ "state of the multibody system")
+ .add_property("activation",
+ bp::make_function(&CostModelAbstract_wrap::get_activation, bp::return_internal_reference<>()),
+ "activation model")
+ .add_property("nu",
+ bp::make_function(&CostModelAbstract_wrap::get_nu, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of control vector");
+
+ bp::class_<CostDataAbstract, boost::shared_ptr<CostDataAbstract>, boost::noncopyable>(
+ "CostDataAbstract", "Abstract class for cost datas.\n\n",
+ bp::init<CostModelAbstract*, pinocchio::Data*>(
+ bp::args(" self", " model", " data"),
+ "Create common data shared between cost models.\n\n"
+ ":param model: cost model\n"
+ ":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
+ .add_property("pinocchio", bp::make_getter(&CostDataAbstract::pinocchio, bp::return_internal_reference<>()),
+ "pinocchio data")
+ .add_property("activation",
+ bp::make_getter(&CostDataAbstract::activation, bp::return_value_policy<bp::return_by_value>()),
+ "terminal data")
+ .add_property("cost", bp::make_getter(&CostDataAbstract::cost, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::cost), "cost value")
+ .add_property("Lx", bp::make_getter(&CostDataAbstract::Lx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::Lx), "Jacobian of the cost")
+ .add_property("Lu", bp::make_getter(&CostDataAbstract::Lu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::Lu), "Jacobian of the cost")
+ .add_property("Lxx", bp::make_getter(&CostDataAbstract::Lxx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::Lxx), "Hessian of the cost")
+ .add_property("Lxu", bp::make_getter(&CostDataAbstract::Lxu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::Lxu), "Hessian of the cost")
+ .add_property("Luu", bp::make_getter(&CostDataAbstract::Luu, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::Luu), "Hessian of the cost")
+ .add_property("r", bp::make_getter(&CostDataAbstract::r, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::r), "cost residual")
+ .add_property("Rx", bp::make_getter(&CostDataAbstract::Rx, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::Rx), "Jacobian of the cost residual")
+ .add_property("Ru", bp::make_getter(&CostDataAbstract::Ru, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&CostDataAbstract::Ru), "Jacobian of the cost residual");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COST_BASE_HPP_
diff --git a/bindings/python/crocoddyl/multibody/costs/com-position.hpp b/bindings/python/crocoddyl/multibody/costs/com-position.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d6e96ccdaa1c0ceb7d86e1ad239974d5d2101b46
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/costs/com-position.hpp
@@ -0,0 +1,91 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_COSTS_COM_POSITION_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_COM_POSITION_HPP_
+
+#include "crocoddyl/multibody/costs/com-position.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeCostCoMPosition() {
+ bp::class_<CostModelCoMPosition, bp::bases<CostModelAbstract> >(
+ "CostModelCoMPosition",
+ bp::init<StateMultibody&, ActivationModelAbstract&, Eigen::Vector3d, int>(
+ bp::args(" self", " state", " activation", " cref", " nu"),
+ "Initialize the CoM position cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param cref: reference CoM position\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&, Eigen::Vector3d>(
+ bp::args(" self", " state", " activation", " cref"),
+ "Initialize the CoM position cost model.\n\n"
+ "For this case the default nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param cref: reference CoM position")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, Eigen::Vector3d, int>(
+ bp::args(" self", " state", " cref", " nu"),
+ "Initialize the CoM position cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e.\n"
+ "crocoddyl.ActivationModelQuad(6).\n"
+ ":param state: state of the multibody system\n"
+ ":param cref: reference CoM position\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, Eigen::Vector3d>(
+ bp::args(" self", " state", " cref"),
+ "Initialize the CoM position cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e.\n"
+ "crocoddyl.ActivationModelQuad(3), and nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param cref: reference CoM position")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &CostModelCoMPosition::calc_wrap,
+ CostModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the CoM position cost.\n\n"
+ ":param data: cost data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (CostModelCoMPosition::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelCoMPosition::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the CoM position cost.\n\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (CostModelCoMPosition::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>("calcDiff", &CostModelCoMPosition::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u"))
+ .def<void (CostModelCoMPosition::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelCoMPosition::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (CostModelCoMPosition::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &CostModelCoMPosition::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &CostModelCoMPosition::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the CoM position cost data.\n\n"
+ "Each cost model has its own data that needs to be allocated. This function\n"
+ "returns the allocated data for a predefined cost.\n"
+ ":param data: Pinocchio data\n"
+ ":return cost data.")
+ .add_property("cref",
+ bp::make_function(&CostModelCoMPosition::get_cref, bp::return_value_policy<bp::return_by_value>()),
+ "reference CoM position");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_COM_POSITION_HPP_
diff --git a/bindings/python/crocoddyl/multibody/costs/control.hpp b/bindings/python/crocoddyl/multibody/costs/control.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e26ec65c975c247d0b086c9a5ddbb88834aaa64
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/costs/control.hpp
@@ -0,0 +1,92 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_COSTS_CONTROL_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_CONTROL_HPP_
+
+#include "crocoddyl/multibody/costs/control.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeCostControl() {
+ bp::class_<CostModelControl, bp::bases<CostModelAbstract> >(
+ "CostModelControl",
+ bp::init<StateMultibody&, ActivationModelAbstract&, Eigen::VectorXd>(
+ bp::args(" self", " state", " activation", " uref"),
+ "Initialize the control cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param uref: reference control")[bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&>(
+ bp::args(" self", " state", " activation"),
+ "Initialize the control cost model.\n\n"
+ "For this case the default uref is the zeros state, i.e. np.zero(nu), where nu is equals to activation.nr.\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&, int>(
+ bp::args(" self", " state", " activation", " nu"),
+ "Initialize the control cost model.\n\n"
+ "For this case the default uref is the zeros state, i.e. np.zero(nu).\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, Eigen::VectorXd>(
+ bp::args(" self", " state", " uref"),
+ "Initialize the control cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e. crocoddyl.ActivationModelQuad(uref.size()).\n"
+ ":param state: state of the multibody system\n"
+ ":param uref: reference control")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&>(
+ bp::args(" self", " state"),
+ "Initialize the control cost model.\n\n"
+ "For this case the default uref is the zeros vector, i.e. np.zero(model.nv), and\n"
+ "activation is quadratic, i.e. crocoddyl.ActivationModelQuad(model.nv), and nu is equals to model.nv.\n"
+ ":param state: state of the multibody system")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, int>(
+ bp::args(" self", " state", " nu"),
+ "Initialize the control cost model.\n\n"
+ "For this case the default uref is the zeros vector and the default activation\n"
+ "model is quadratic, i.e. crocoddyl.ActivationModelQuad(nu)\n"
+ ":param state: state of the multibody system\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &CostModelControl::calc_wrap,
+ CostModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the control cost.\n\n"
+ ":param data: cost data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (CostModelControl::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelControl::calcDiff_wrap, bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the control cost.\n\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (CostModelControl::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>("calcDiff", &CostModelControl::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u"))
+ .def<void (CostModelControl::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelControl::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (CostModelControl::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &CostModelControl::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .add_property("uref",
+ bp::make_function(&CostModelControl::get_uref, bp::return_value_policy<bp::return_by_value>()),
+ "reference control");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_CONTROL_HPP_
diff --git a/bindings/python/crocoddyl/multibody/costs/cost-sum.hpp b/bindings/python/crocoddyl/multibody/costs/cost-sum.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..06ff9368fa197824d716d2088c7694d1bdc58a39
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/costs/cost-sum.hpp
@@ -0,0 +1,147 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_COSTS_COST_SUM_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_
+
+#include <functional>
+#include <map>
+#include <memory>
+#include <utility>
+#include <string>
+#include "crocoddyl/multibody/costs/cost-sum.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(CostModelSum_calc_wraps, CostModelSum::calc_wrap, 2, 3)
+
+void exposeCostSum() {
+ // Register custom converters between std::map and Python dict
+ typedef boost::shared_ptr<CostDataAbstract> CostDataPtr;
+ bp::to_python_converter<std::map<std::string, CostItem, std::less<std::string>,
+ std::allocator<std::pair<const std::string, CostItem> > >,
+ map_to_dict<std::string, CostItem> >();
+ bp::to_python_converter<std::map<std::string, CostDataPtr, std::less<std::string>,
+ std::allocator<std::pair<const std::string, CostDataPtr> > >,
+ map_to_dict<std::string, CostDataPtr, false> >();
+ dict_to_map<std::string, CostItem>().from_python();
+ dict_to_map<std::string, CostDataPtr>().from_python();
+
+ bp::class_<CostItem, boost::noncopyable>("CostItem", "Describe a cost item.\n\n",
+ bp::init<std::string, CostModelAbstract*, double>(
+ bp::args(" self", " name", " cost", " weight"),
+ "Initialize the cost item.\n\n"
+ ":param name: cost name\n"
+ ":param cost: cost model\n"
+ ":param weight: cost weight")[bp::with_custodian_and_ward<1, 3>()])
+ .def_readwrite("name", &CostItem::name, "cost name")
+ .add_property("cost", bp::make_getter(&CostItem::cost, bp::return_internal_reference<>()), "cost model")
+ .def_readwrite("weight", &CostItem::weight, "cost weight");
+
+ bp::class_<CostModelSum, boost::noncopyable>(
+ "CostModelSum",
+ bp::init<StateMultibody&, unsigned int, bool>(
+ bp::args(" self", " state", " nu=state.nv", " withResiduals=True"),
+ "Initialize the total cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param nu: dimension of control vector\n"
+ ":param withResiduals: true if the cost function has residuals")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, unsigned int>(
+ bp::args(" self", " state", " nu"),
+ "Initialize the total cost model.\n\n"
+ "For this case the default nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param nu: dimension of control vector\n"
+ ":param withResiduals: true if the cost function has residuals")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&>(
+ bp::args(" self", " state"),
+ "Initialize the total cost model.\n\n"
+ "For this case the default nu is equals to model.nv.\n"
+ ":param state: state of the multibody system")[bp::with_custodian_and_ward<1, 2>()])
+ .def("addCost", &CostModelSum::addCost, bp::with_custodian_and_ward<1, 3>(),
+ bp::args(" self", " name", " cost", " weight"),
+ "Add a cost item.\n\n"
+ ":param name: cost name\n"
+ ":param cost: cost model\n"
+ ":param weight: cost weight")
+ .def("removeCost", &CostModelSum::removeCost, bp::args(" self", " name"),
+ "Remove a cost item.\n\n"
+ ":param name: cost name")
+ .def("calc", &CostModelSum::calc_wrap,
+ CostModelSum_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the total cost.\n\n"
+ ":param data: cost-sum data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (CostModelSum::*)(const boost::shared_ptr<CostDataSum>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelSum::calcDiff_wrap, bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the total cost.\n\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (CostModelSum::*)(const boost::shared_ptr<CostDataSum>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>("calcDiff", &CostModelSum::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u"))
+ .def<void (CostModelSum::*)(const boost::shared_ptr<CostDataSum>&, const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelSum::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (CostModelSum::*)(const boost::shared_ptr<CostDataSum>&, const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelSum::calcDiff_wrap, bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &CostModelSum::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the total cost data.\n\n"
+ ":param data: Pinocchio data\n"
+ ":return total cost data.")
+ .add_property("state", bp::make_function(&CostModelSum::get_state, bp::return_internal_reference<>()),
+ "state of the multibody system")
+ .add_property("costs",
+ bp::make_function(&CostModelSum::get_costs, bp::return_value_policy<bp::return_by_value>()),
+ "stack of costs")
+ .add_property("nu", bp::make_function(&CostModelSum::get_nu, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of control vector")
+ .add_property("nr", bp::make_function(&CostModelSum::get_nr, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of the total residual vector");
+
+ bp::class_<CostDataSum, boost::shared_ptr<CostDataSum>, boost::noncopyable>(
+ "CostDataSum", "Class for total cost data.\n\n",
+ bp::init<CostModelSum*, pinocchio::Data*>(bp::args(" self", " model", " data"),
+ "Create total cost data.\n\n"
+ ":param model: total cost model\n"
+ ":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 3>()])
+ .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("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>()),
+ bp::make_setter(&CostDataSum::Ru), "Jacobian of the cost residual");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_
diff --git a/bindings/python/crocoddyl/multibody/costs/frame-placement.hpp b/bindings/python/crocoddyl/multibody/costs/frame-placement.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b83da518b4f08e0605e0d7da3df61507a3693612
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/costs/frame-placement.hpp
@@ -0,0 +1,90 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_COSTS_FRAME_PLACEMENT_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_FRAME_PLACEMENT_HPP_
+
+#include "crocoddyl/multibody/costs/frame-placement.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeCostFramePlacement() {
+ bp::class_<CostModelFramePlacement, bp::bases<CostModelAbstract> >(
+ "CostModelFramePlacement",
+ bp::init<StateMultibody&, ActivationModelAbstract&, FramePlacement, int>(
+ bp::args(" self", " state", " activation", " Mref", " nu"),
+ "Initialize the frame placement cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param Mref: reference frame placement\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&, FramePlacement>(
+ bp::args(" self", " state", " activation", " Fref"),
+ "Initialize the frame placement cost model.\n\n"
+ "For this case the default nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param Mref: reference frame placement")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, FramePlacement, int>(
+ bp::args(" self", " state", " Mref", " nu"),
+ "Initialize the frame placement cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e.\n"
+ "crocoddyl.ActivationModelQuad(6).\n"
+ ":param state: state of the multibody system\n"
+ ":param Mref: reference frame placement\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, FramePlacement>(
+ bp::args(" self", " state", " Mref"),
+ "Initialize the frame placement cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e.\n"
+ "crocoddyl.ActivationModelQuad(6), and nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param Mref: reference frame placement")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &CostModelFramePlacement::calc_wrap,
+ CostModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the frame placement cost.\n\n"
+ ":param data: cost data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (CostModelFramePlacement::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelFramePlacement::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the frame placement cost.\n\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (CostModelFramePlacement::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelFramePlacement::calcDiff_wrap, bp::args(" self", " data", " x", " u"))
+ .def<void (CostModelFramePlacement::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelFramePlacement::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (CostModelFramePlacement::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &CostModelFramePlacement::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &CostModelFramePlacement::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the frame placement cost data.\n\n"
+ "Each cost model has its own data that needs to be allocated. This function\n"
+ "returns the allocated data for a predefined cost.\n"
+ ":param data: Pinocchio data\n"
+ ":return cost data.")
+ .add_property("Mref", bp::make_function(&CostModelFramePlacement::get_Mref, bp::return_internal_reference<>()),
+ "reference frame placement");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_FRAME_PLACEMENT_HPP_
diff --git a/bindings/python/crocoddyl/multibody/costs/frame-translation.hpp b/bindings/python/crocoddyl/multibody/costs/frame-translation.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1a0810724099df0037612d4d54a79c2f56264921
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/costs/frame-translation.hpp
@@ -0,0 +1,90 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_COSTS_FRAME_TRANSLATION_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_
+
+#include "crocoddyl/multibody/costs/frame-translation.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeCostFrameTranslation() {
+ bp::class_<CostModelFrameTranslation, bp::bases<CostModelAbstract> >(
+ "CostModelFrameTranslation",
+ bp::init<StateMultibody&, ActivationModelAbstract&, FrameTranslation, int>(
+ bp::args(" self", " state", " activation", " xref", " nu"),
+ "Initialize the frame translation cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param xref: reference frame translation\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&, FrameTranslation>(
+ bp::args(" self", " state", " activation", " xref"),
+ "Initialize the frame translation cost model.\n\n"
+ "For this case the default nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param xref: reference frame translation")
+ [bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, FrameTranslation, int>(
+ bp::args(" self", " state", " xref", " nu"),
+ "Initialize the frame translation cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e.\n"
+ "crocoddyl.ActivationModelQuad(3).\n"
+ ":param state: state of the multibody system\n"
+ ":param xref: reference frame translation\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, FrameTranslation>(
+ bp::args(" self", " state", " xref"),
+ "Initialize the frame translation cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e.\n"
+ "crocoddyl.ActivationModelQuad(3), and nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param xref: reference frame translation")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &CostModelFrameTranslation::calc_wrap,
+ CostModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the frame translation cost.\n\n"
+ ":param data: cost data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (CostModelFrameTranslation::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelFrameTranslation::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the frame translation cost.\n\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (CostModelFrameTranslation::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelFrameTranslation::calcDiff_wrap, bp::args(" self", " data", " x", " u"))
+ .def<void (CostModelFrameTranslation::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelFrameTranslation::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (CostModelFrameTranslation::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &CostModelFrameTranslation::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &CostModelFrameTranslation::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the frame translation cost data.\n\n"
+ "Each cost model has its own data that needs to be allocated. This function\n"
+ "returns the allocated data for a predefined cost.\n"
+ ":param data: Pinocchio data\n"
+ ":return cost data.")
+ .add_property("xref", bp::make_function(&CostModelFrameTranslation::get_xref, bp::return_internal_reference<>()),
+ "reference frame translation");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_
diff --git a/bindings/python/crocoddyl/multibody/costs/frame-velocity.hpp b/bindings/python/crocoddyl/multibody/costs/frame-velocity.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb5ff7e89e7597a2206d7df8612955b7cad5d643
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/costs/frame-velocity.hpp
@@ -0,0 +1,90 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_COSTS_FRAME_VELOCITY_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_
+
+#include "crocoddyl/multibody/costs/frame-velocity.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeCostFrameVelocity() {
+ bp::class_<CostModelFrameVelocity, bp::bases<CostModelAbstract> >(
+ "CostModelFrameVelocity",
+ bp::init<StateMultibody&, ActivationModelAbstract&, FrameMotion, int>(
+ bp::args(" self", " state", " activation", " vref", " nu"),
+ "Initialize the frame velocity cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param vref: reference frame velocity\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&, FrameMotion>(
+ bp::args(" self", " state", " activation", " vref"),
+ "Initialize the frame velocity cost model.\n\n"
+ "For this case the default nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param vref: reference frame velocity")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, FrameMotion, int>(
+ bp::args(" self", " state", " vref", " nu"),
+ "Initialize the frame velocity cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e.\n"
+ "crocoddyl.ActivationModelQuad(6).\n"
+ ":param state: state of the multibody system\n"
+ ":param vref: reference frame velocity\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, FrameMotion>(
+ bp::args(" self", " state", " vref"),
+ "Initialize the frame velocity cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e.\n"
+ "crocoddyl.ActivationModelQuad(6), and nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param vref: reference frame velocity")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &CostModelFrameVelocity::calc_wrap,
+ CostModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the frame velocity cost.\n\n"
+ ":param data: cost data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (CostModelFrameVelocity::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelFrameVelocity::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the frame velocity cost.\n\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (CostModelFrameVelocity::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelFrameVelocity::calcDiff_wrap, bp::args(" self", " data", " x", " u"))
+ .def<void (CostModelFrameVelocity::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelFrameVelocity::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (CostModelFrameVelocity::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const bool&)>("calcDiff", &CostModelFrameVelocity::calcDiff_wrap,
+ bp::args(" self", " data", " x", " recalc"))
+ .def("createData", &CostModelFrameVelocity::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the frame velocity cost data.\n\n"
+ "Each cost model has its own data that needs to be allocated. This function\n"
+ "returns the allocated data for a predefined cost.\n"
+ ":param data: Pinocchio data\n"
+ ":return cost data.")
+ .add_property("vref", bp::make_function(&CostModelFrameVelocity::get_vref, bp::return_internal_reference<>()),
+ "reference frame velocity");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_
diff --git a/bindings/python/crocoddyl/multibody/costs/state.hpp b/bindings/python/crocoddyl/multibody/costs/state.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..45b332ef9fc0bb6d20b97fe5d958a15a179ed20e
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/costs/state.hpp
@@ -0,0 +1,116 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_COSTS_STATE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_STATE_HPP_
+
+#include "crocoddyl/multibody/costs/state.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeCostState() {
+ bp::class_<CostModelState, bp::bases<CostModelAbstract> >(
+ "CostModelState",
+ bp::init<StateMultibody&, ActivationModelAbstract&, Eigen::VectorXd, int>(
+ bp::args(" self", " state", " activation=crocoddyl.ActivationModelQuad(state.ndx)", " xref=state.zero()",
+ " nu=model.nv"),
+ "Initialize the state cost model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param xref: reference state\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, Eigen::VectorXd, int>(
+ bp::args(" self", " state", " xref", " nu"),
+ "Initialize the state cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e. crocoddyl.ActivationModelQuad(state.ndx).\n"
+ ":param state: state of the multibody system\n"
+ ":param xref: reference state\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&, Eigen::VectorXd>(
+ bp::args(" self", " state", " activation", " xref"),
+ "Initialize the state cost model.\n\n"
+ "For this case the default nu values is model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param xref: reference state")[bp::with_custodian_and_ward<1, 3>()])
+ .def(bp::init<StateMultibody&, Eigen::VectorXd>(
+ bp::args(" self", " state", " xref"),
+ "Initialize the state cost model.\n\n"
+ "For this case the default activation model is quadratic, i.e. crocoddyl.ActivationModelQuad(state.ndx),\n"
+ "and nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param xref: reference state")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&, int>(
+ bp::args(" self", " state", " activation", " nu"),
+ "Initialize the state cost model.\n\n"
+ "For this case the default xref is the zeros state, i.e. state.zero().\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&, int>(
+ bp::args(" self", " state", " nu"),
+ "Initialize the state cost model.\n\n"
+ "For this case the default xref is the zeros state, i.e. state.zero(), and the default activation\n"
+ "model is quadratic, i.e. crocoddyl.ActivationModelQuad(state.ndx)\n"
+ ":param state: state of the multibody system\n"
+ ":param nu: dimension of control vector")[bp::with_custodian_and_ward<1, 2>()])
+ .def(bp::init<StateMultibody&, ActivationModelAbstract&>(
+ bp::args(" self", " state", " activation"),
+ "Initialize the state cost model.\n\n"
+ "For this case the default xref is the zeros state, i.e. state.zero(), and nu is equals to model.nv.\n"
+ ":param state: state of the multibody system\n"
+ ":param activation: activation model")[bp::with_custodian_and_ward<1, 2,
+ bp::with_custodian_and_ward<1, 3> >()])
+ .def(bp::init<StateMultibody&>(
+ bp::args(" self", " state"),
+ "Initialize the state cost model.\n\n"
+ "For this case the default xref is the zeros state, i.e. state.zero(), the default activation\n"
+ "model is quadratic, i.e. crocoddyl.ActivationModelQuad(state.ndx), and nu is equals to model.nv.\n"
+ ":param state: state of the multibody system")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &CostModelState::calc_wrap,
+ CostModel_calc_wraps(bp::args(" self", " data", " x", " u=None"),
+ "Compute the state cost.\n\n"
+ ":param data: cost data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input"))
+ .def<void (CostModelState::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelState::calcDiff_wrap, bp::args(" self", " data", " x", " u=None", " recalc=True"),
+ "Compute the derivatives of the state cost.\n\n"
+ ":param data: action data\n"
+ ":param x: time-discrete state vector\n"
+ ":param u: time-discrete control input\n"
+ ":param recalc: If true, it updates the state evolution and the cost value.")
+ .def<void (CostModelState::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&,
+ const Eigen::VectorXd&)>("calcDiff", &CostModelState::calcDiff_wrap,
+ bp::args(" self", " data", " x", " u"))
+ .def<void (CostModelState::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&)>(
+ "calcDiff", &CostModelState::calcDiff_wrap, bp::args(" self", " data", " x"))
+ .def<void (CostModelState::*)(const boost::shared_ptr<CostDataAbstract>&, const Eigen::VectorXd&, const bool&)>(
+ "calcDiff", &CostModelState::calcDiff_wrap, bp::args(" self", " data", " x", " recalc"))
+ .add_property("xref",
+ bp::make_function(&CostModelState::get_xref, bp::return_value_policy<bp::return_by_value>()),
+ "reference state")
+ .def("createData", &CostModelState::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the state cost data.\n\n"
+ "Each cost model has its own data that needs to be allocated. This function\n"
+ "returns the allocated data for a predefined cost.\n"
+ ":param data: Pinocchio data\n"
+ ":return cost data.");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COSTS_STATE_HPP_
diff --git a/bindings/python/crocoddyl/multibody/frames.hpp b/bindings/python/crocoddyl/multibody/frames.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5490ada2749fc71ab80e78852f2b4126e5e3a614
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/frames.hpp
@@ -0,0 +1,59 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_FRAMES_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_FRAMES_HPP_
+
+#include "crocoddyl/multibody/frames.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeFrames() {
+ bp::class_<FrameTranslation, boost::noncopyable>(
+ "FrameTranslation",
+ "Frame translation describe using Pinocchio.\n\n"
+ "It defines a frame translation (3D vector) for a given frame ID",
+ bp::init<int, Eigen::Vector3d>(bp::args(" self", " frame", " oxf"),
+ "Initialize the frame translation.\n\n"
+ ":param frame: frame ID\n"
+ ":param oxf: Frame translation w.r.t. the origin"))
+ .def_readwrite("frame", &FrameTranslation::frame, "frame ID")
+ .add_property("oxf", bp::make_getter(&FrameTranslation::oxf, bp::return_value_policy<bp::return_by_value>()),
+ bp::make_setter(&FrameTranslation::oxf), "frame translation");
+
+ bp::class_<FramePlacement, boost::noncopyable>(
+ "FramePlacement",
+ "Frame placement describe using Pinocchio.\n\n"
+ "It defines a frame placement (SE(3) point) for a given frame ID",
+ bp::init<int, pinocchio::SE3>(bp::args(" self", " frame", " oMf"),
+ "Initialize the frame placement.\n\n"
+ ":param frame: frame ID\n"
+ ":param oMf: Frame placement w.r.t. the origin"))
+ .def_readwrite("frame", &FramePlacement::frame, "frame ID")
+ .add_property("oMf", bp::make_getter(&FramePlacement::oMf, bp::return_internal_reference<>()),
+ "frame placement");
+
+ bp::class_<FrameMotion, boost::noncopyable>(
+ "FrameMotion",
+ "Frame motion describe using Pinocchio.\n\n"
+ "It defines a frame motion (tangent of SE(3) point) for a given frame ID",
+ bp::init<int, pinocchio::Motion>(bp::args(" self", " frame", " oMf"),
+ "Initialize the frame motion.\n\n"
+ ":param frame: frame ID\n"
+ ":param oMf: Frame motion w.r.t. the origin"))
+ .def_readwrite("frame", &FrameMotion::frame, "frame ID")
+ .add_property("oMf", bp::make_getter(&FrameMotion::oMf, bp::return_internal_reference<>()), "frame motion");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_FRAMES_HPP_
diff --git a/bindings/python/crocoddyl/multibody/impulse-base.hpp b/bindings/python/crocoddyl/multibody/impulse-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..be0becea38c53840d9ff54b06e0de9eee23a66ef
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/impulse-base.hpp
@@ -0,0 +1,102 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_IMPULSE_BASE_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
+
+#include "crocoddyl/multibody/impulse-base.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+class ImpulseModelAbstract_wrap : public ImpulseModelAbstract, public bp::wrapper<ImpulseModelAbstract> {
+ public:
+ ImpulseModelAbstract_wrap(StateMultibody& state, int ni) : ImpulseModelAbstract(state, ni) {}
+
+ void calc(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x) {
+ assert(x.size() == state_.get_nx() && "x has wrong dimension");
+ return bp::call<void>(this->get_override("calc").ptr(), data, (Eigen::VectorXd)x);
+ }
+
+ void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const bool& recalc = true) {
+ assert(x.size() == state_.get_nx() && "x has wrong dimension");
+ 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);
+ }
+};
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ImpulseModel_calcDiff_wraps, ImpulseModelAbstract::calcDiff_wrap, 2, 3)
+
+void exposeImpulseAbstract() {
+ bp::class_<ImpulseModelAbstract_wrap, boost::noncopyable>(
+ "ImpulseModelAbstract",
+ "Abstract impulse model.\n\n"
+ "It defines a template for impulse models.\n"
+ "The calc and calcDiff functions compute the impulse Jacobian\n"
+ "the derivatives respectively.",
+ bp::init<StateMultibody&, int>(bp::args(" self", " state", " ni"),
+ "Initialize the impulse model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param ni: dimension of impulse model")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", pure_virtual(&ImpulseModelAbstract_wrap::calc), bp::args(" self", " data", " x"),
+ "Compute the impulse Jacobian\n"
+ ":param data: impulse data\n"
+ ":param x: state vector")
+ .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 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("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"
+ ":param data: Pinocchio data\n"
+ ":return impulse data.")
+ .add_property("state",
+ bp::make_function(&ImpulseModelAbstract_wrap::get_state, bp::return_internal_reference<>()),
+ "state of the multibody system")
+ .add_property(
+ "ni", bp::make_function(&ImpulseModelAbstract_wrap::get_ni, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of impulse");
+
+ bp::class_<ImpulseDataAbstract, boost::shared_ptr<ImpulseDataAbstract>, boost::noncopyable>(
+ "ImpulseDataAbstract", "Abstract class for impulse datas.\n\n",
+ 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 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")
+ .def_readwrite("joint", &ImpulseDataAbstract::joint, "joint index of the impulse frame")
+ .def_readwrite("f", &ImpulseDataAbstract::f, "external spatial forces");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
diff --git a/bindings/python/crocoddyl/multibody/impulses/impulse-3d.hpp b/bindings/python/crocoddyl/multibody/impulses/impulse-3d.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7edc032b72ace7797169b3cb78249d07aeb8ef4
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/impulses/impulse-3d.hpp
@@ -0,0 +1,63 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_IMPULSES_IMPULSE_3D_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
+
+#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeImpulse3D() {
+ bp::class_<ImpulseModel3D, bp::bases<ImpulseModelAbstract> >(
+ "ImpulseModel3D",
+ "Rigid 3D impulse model.\n\n"
+ "It defines a rigid 3D impulse models (point impulse) based on acceleration-based holonomic constraints.\n"
+ "The calc and calcDiff functions compute the impulse Jacobian and drift (holonomic constraint) or\n"
+ "the derivatives of the holonomic constraint, respectively.",
+ bp::init<StateMultibody&, int>(bp::args(" self", " state", " frame"),
+ "Initialize the 3D impulse model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param frame: reference frame id")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &ImpulseModel3D::calc_wrap, bp::args(" self", " data", " x"),
+ "Compute the 3D impulse Jacobian and drift.\n\n"
+ "The rigid impulse model throught acceleration-base holonomic constraint\n"
+ "of the impulse frame placement.\n"
+ ":param data: impulse data\n"
+ ":param x: state vector")
+ .def("calcDiff", &ImpulseModel3D::calcDiff_wrap,
+ ImpulseModel_calcDiff_wraps(bp::args(" self", " data", " x", " recalc=True"),
+ "Compute the derivatives of the 3D 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 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"
+ ":param data: cost data\n"
+ ":param lambda: Lagrangian vector")
+ .def("createData", &ImpulseModel3D::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the 3D 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"
+ ":param data: Pinocchio data\n"
+ ":return impulse data.")
+ .add_property("frame",
+ bp::make_function(&ImpulseModel3D::get_frame, bp::return_value_policy<bp::return_by_value>()),
+ "reference frame id");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
diff --git a/bindings/python/crocoddyl/multibody/impulses/impulse-6d.hpp b/bindings/python/crocoddyl/multibody/impulses/impulse-6d.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4b428ce9b30dd4163bacf1a491746ebbe7943a69
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/impulses/impulse-6d.hpp
@@ -0,0 +1,63 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_IMPULSES_IMPULSE_6D_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
+
+#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeImpulse6D() {
+ bp::class_<ImpulseModel6D, bp::bases<ImpulseModelAbstract> >(
+ "ImpulseModel6D",
+ "Rigid 6D impulse model.\n\n"
+ "It defines a rigid 6D impulse models based on acceleration-based holonomic constraints.\n"
+ "The calc and calcDiff functions compute the impulse Jacobian and drift (holonomic constraint) or\n"
+ "the derivatives of the holonomic constraint, respectively.",
+ bp::init<StateMultibody&, int>(bp::args(" self", " state", " frame"),
+ "Initialize the impulse model.\n\n"
+ ":param state: state of the multibody system\n"
+ ":param frame: reference frame id")[bp::with_custodian_and_ward<1, 2>()])
+ .def("calc", &ImpulseModel6D::calc_wrap, bp::args(" self", " data", " x"),
+ "Compute the 6D impulse Jacobian and drift.\n\n"
+ "The rigid impulse model throught acceleration-base holonomic constraint\n"
+ "of the impulse frame placement.\n"
+ ":param data: impulse data\n"
+ ":param x: state vector")
+ .def("calcDiff", &ImpulseModel6D::calcDiff_wrap,
+ ImpulseModel_calcDiff_wraps(bp::args(" self", " data", " x", " recalc=True"),
+ "Compute the derivatives of the 6D 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 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"
+ ":param data: cost data\n"
+ ":param lambda: Lagrangian vector")
+ .def("createData", &ImpulseModel6D::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the 6D 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"
+ ":param data: Pinocchio data\n"
+ ":return impulse data.")
+ .add_property("frame",
+ bp::make_function(&ImpulseModel6D::get_frame, bp::return_value_policy<bp::return_by_value>()),
+ "reference frame id");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
diff --git a/bindings/python/crocoddyl/multibody/impulses/multiple-impulses.hpp b/bindings/python/crocoddyl/multibody/impulses/multiple-impulses.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..26c43b07e07f8d0b1f9e2437f979da6fbe3ac803
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/impulses/multiple-impulses.hpp
@@ -0,0 +1,112 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_IMPULSES_MULTIPLE_IMPULSES_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
+
+#include <functional>
+#include <map>
+#include <memory>
+#include <utility>
+#include <string>
+#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ImpulseModelMultiple_calcDiff_wraps, ImpulseModelMultiple::calcDiff_wrap, 2, 3)
+
+void exposeImpulseMultiple() {
+ // Register custom converters between std::map and Python dict
+ typedef boost::shared_ptr<ImpulseDataAbstract> ImpulseDataPtr;
+ bp::to_python_converter<std::map<std::string, ImpulseItem, std::less<std::string>,
+ std::allocator<std::pair<const std::string, ImpulseItem> > >,
+ map_to_dict<std::string, ImpulseItem> >();
+ bp::to_python_converter<std::map<std::string, ImpulseDataPtr, std::less<std::string>,
+ std::allocator<std::pair<const std::string, ImpulseDataPtr> > >,
+ map_to_dict<std::string, ImpulseDataPtr, false> >();
+ dict_to_map<std::string, ImpulseItem>().from_python();
+ dict_to_map<std::string, ImpulseDataPtr>().from_python();
+
+ bp::class_<ImpulseItem, boost::noncopyable>(
+ "ImpulseItem", "Describe a impulse item.\n\n",
+ bp::init<std::string, ImpulseModelAbstract*>(
+ bp::args(" self", " name", " impulse"),
+ "Initialize the impulse item.\n\n"
+ ":param name: impulse name\n"
+ ":param impulse: impulse model")[bp::with_custodian_and_ward<1, 3>()])
+ .def_readwrite("name", &ImpulseItem::name, "impulse name")
+ .add_property("impulse", bp::make_getter(&ImpulseItem::impulse, bp::return_internal_reference<>()),
+ "impulse model");
+
+ bp::class_<ImpulseModelMultiple, boost::noncopyable>(
+ "ImpulseModelMultiple",
+ bp::init<StateMultibody&>(bp::args(" self", " state"),
+ "Initialize the multiple impulse model.\n\n"
+ ":param state: state of the multibody system")[bp::with_custodian_and_ward<1, 2>()])
+ .def("addImpulse", &ImpulseModelMultiple::addImpulse, bp::with_custodian_and_ward<1, 3>(),
+ bp::args(" self", " name", " impulse"),
+ "Add a impulse item.\n\n"
+ ":param name: impulse name\n"
+ ":param impulse: impulse model")
+ .def("removeImpulse", &ImpulseModelMultiple::removeImpulse, bp::args(" self", " name"),
+ "Remove a impulse item.\n\n"
+ ":param name: impulse name")
+ .def("calc", &ImpulseModelMultiple::calc_wrap, bp::args(" self", " data", " x"),
+ "Compute the total impulse Jacobian and drift.\n\n"
+ "The rigid impulse model throught acceleration-base holonomic constraint\n"
+ "of the impulse frame placement.\n"
+ ":param data: impulse data\n"
+ ":param x: state vector")
+ .def("calcDiff", &ImpulseModelMultiple::calcDiff_wrap,
+ ImpulseModelMultiple_calcDiff_wraps(
+ bp::args(" self", " data", " x", " recalc=True"),
+ "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 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("createData", &ImpulseModelMultiple::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
+ bp::args(" self", " data"),
+ "Create the total impulse data.\n\n"
+ ":param data: Pinocchio data\n"
+ ":return total impulse data.")
+ .add_property(
+ "impulses",
+ bp::make_function(&ImpulseModelMultiple::get_impulses, bp::return_value_policy<bp::return_by_value>()),
+ "stack of impulses")
+ .add_property("state", bp::make_function(&ImpulseModelMultiple::get_state, bp::return_internal_reference<>()),
+ "state of the multibody system")
+ .add_property("ni",
+ bp::make_function(&ImpulseModelMultiple::get_ni, bp::return_value_policy<bp::return_by_value>()),
+ "dimension of the total impulse vector");
+
+ bp::class_<ImpulseDataMultiple, boost::shared_ptr<ImpulseDataMultiple>, bp::bases<ImpulseDataAbstract> >(
+ "ImpulseDataMultiple", "Data class for multiple impulses.\n\n",
+ bp::init<ImpulseModelMultiple*, pinocchio::Data*>(
+ bp::args(" self", " model", " data"),
+ "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("impulses",
+ bp::make_getter(&ImpulseDataMultiple::impulses, bp::return_value_policy<bp::return_by_value>()),
+ "stack of impulses data")
+ .def_readwrite("fext", &ImpulseDataMultiple::fext, "external spatial forces");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
diff --git a/bindings/python/crocoddyl/multibody/states/multibody.hpp b/bindings/python/crocoddyl/multibody/states/multibody.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9559927bbed783510df1195813112d1dead81280
--- /dev/null
+++ b/bindings/python/crocoddyl/multibody/states/multibody.hpp
@@ -0,0 +1,87 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_STATES_MULTIBODY_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
+
+#include "crocoddyl/multibody/states/multibody.hpp"
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+void exposeStateMultibody() {
+ bp::class_<StateMultibody, bp::bases<StateAbstract> >(
+ "StateMultibody",
+ "Multibody state defined using Pinocchio.\n\n"
+ "Pinocchio defines operators for integrating or differentiating the robot's\n"
+ "configuration space. And here we assume that the state is defined by the\n"
+ "robot's configuration and its joint velocities (x=[q,v]). Generally speaking,\n"
+ "q lies on the manifold configuration manifold (M) and v in its tangent space\n"
+ "(Tx M). Additionally the Pinocchio allows us to compute analytically the\n"
+ "Jacobians for the differentiate and integrate operators. Note that this code\n"
+ "can be reused in any robot that is described through its Pinocchio model.",
+ bp::init<pinocchio::Model&>(
+ bp::args(" self", " pinocchioModel"),
+ "Initialize the multibody state given a Pinocchio model.\n\n"
+ ":param pinocchioModel: pinocchio model (i.e. multibody model)")[bp::with_custodian_and_ward<1, 2>()])
+ .def("zero", &StateMultibody::zero, bp::args(" self"),
+ "Return the neutral robot configuration with zero velocity.\n\n"
+ ":return neutral robot configuration with zero velocity")
+ .def("rand", &StateMultibody::rand, bp::args(" self"),
+ "Return a random reference state.\n\n"
+ ":return random reference state")
+ .def("diff", &StateMultibody::diff_wrap, bp::args(" self", " x0", " x1"),
+ "Operator that differentiates the two robot states.\n\n"
+ "It returns the value of x1 [-] x0 operation. This operator uses the Lie\n"
+ "algebra since the robot's root could lie in the SE(3) manifold.\n"
+ ":param x0: current state (dim state.nx()).\n"
+ ":param x1: next state (dim state.nx()).\n"
+ ":return x1 - x0 value (dim state.nx()).")
+ .def("integrate", &StateMultibody::integrate_wrap, bp::args(" self", " x", " dx"),
+ "Operator that integrates the current robot state.\n\n"
+ "It returns the value of x [+] dx operation. This operator uses the Lie\n"
+ "algebra since the robot's root could lie in the SE(3) manifold.\n"
+ "Futhermore there is no timestep here (i.e. dx = v*dt), note this if you're\n"
+ "integrating a velocity v during an interval dt.\n"
+ ":param x: current state (dim state.nx()).\n"
+ ":param dx: displacement of the state (dim state.ndx()).\n"
+ ":return x + dx value (dim state.nx()).")
+ .def("Jdiff", &StateMultibody::Jdiff_wrap,
+ Jdiffs(bp::args(" self", " x0", " x1", " firstsecond = 'both'"),
+ "Compute the partial derivatives of the diff operator.\n\n"
+ "Both Jacobian matrices are represented throught an identity matrix, with the exception\n"
+ "that the robot's root is defined as free-flying joint (SE(3)). By default, this\n"
+ "function returns the derivatives of the first and second argument (i.e.\n"
+ "firstsecond='both'). However we ask for a specific partial derivative by setting\n"
+ "firstsecond='first' or firstsecond='second'.\n"
+ ":param x0: current state (dim state.nx()).\n"
+ ":param x1: next state (dim state.nx()).\n"
+ ":param firstsecond: desired partial derivative\n"
+ ":return the partial derivative(s) of the diff(x0, x1) function"))
+ .def("Jintegrate", &StateMultibody::Jintegrate_wrap,
+ Jintegrates(bp::args(" self", " x", " dx", " firstsecond = 'both'"),
+ "Compute the partial derivatives of arithmetic addition.\n\n"
+ "Both Jacobian matrices are represented throught an identity matrix. with the exception\n"
+ "that the robot's root is defined as free-flying joint (SE(3)). By default, this\n"
+ "function returns the derivatives of the first and second argument (i.e.\n"
+ "firstsecond='both'). However we ask for a specific partial derivative by setting\n"
+ "firstsecond='first' or firstsecond='second'.\n"
+ ":param x: current state (dim state.nx()).\n"
+ ":param dx: displacement of the state (dim state.ndx()).\n"
+ ":param firstsecond: desired partial derivative\n"
+ ":return the partial derivative(s) of the integrate(x, dx) function"))
+ .add_property("pinocchio", bp::make_function(&StateMultibody::get_pinocchio, bp::return_internal_reference<>()),
+ "pinocchio model");
+}
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
diff --git a/bindings/python/crocoddyl/utils.hpp b/bindings/python/crocoddyl/utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0039a64aae093e49b700bad4a13c98a832fab0d8
--- /dev/null
+++ b/bindings/python/crocoddyl/utils.hpp
@@ -0,0 +1,173 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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_UTILS_HPP_
+#define BINDINGS_PYTHON_CROCODDYL_UTILS_HPP_
+
+#include <Eigen/Dense>
+#include <vector>
+#include <map>
+#include <boost/python/stl_iterator.hpp>
+#include <boost/python/to_python_converter.hpp>
+
+namespace crocoddyl {
+namespace python {
+
+namespace bp = boost::python;
+
+/// @note Registers converter from a provided type to the python
+/// iterable type to the.
+template <class T, bool NoProxy = true>
+struct vector_to_list {
+ static PyObject* convert(const std::vector<T>& vec) {
+ bp::list* l = new boost::python::list();
+ for (size_t i = 0; i < vec.size(); i++) {
+ if (NoProxy) {
+ l->append(boost::ref(vec[i]));
+ } else {
+ l->append(vec[i]);
+ }
+ }
+ return l->ptr();
+ }
+ static PyTypeObject const* get_pytype() { return &PyList_Type; }
+};
+
+/// @brief Type that allows for registration of conversions from
+/// python iterable types.
+struct list_to_vector {
+ /// @note Registers converter from a python iterable type to the
+ /// provided type.
+ template <typename Container>
+ list_to_vector& from_python() {
+ boost::python::converter::registry::push_back(&list_to_vector::convertible, &list_to_vector::construct<Container>,
+ boost::python::type_id<Container>());
+
+ // Support chaining.
+ return *this;
+ }
+
+ /// @brief Check if PyObject is iterable.
+ static void* convertible(PyObject* object) { return PyObject_GetIter(object) ? object : NULL; }
+
+ /// @brief Convert iterable PyObject to C++ container type.
+ ///
+ /// Container Concept requirements:
+ ///
+ /// * Container::value_type is CopyConstructable.
+ /// * Container can be constructed and populated with two iterators.
+ /// I.e. Container(begin, end)
+ template <typename Container>
+ static void construct(PyObject* object, boost::python::converter::rvalue_from_python_stage1_data* data) {
+ namespace python = boost::python;
+ // Object is a borrowed reference, so create a handle indicting it is
+ // borrowed for proper reference counting.
+ python::handle<> handle(python::borrowed(object));
+
+ // Obtain a handle to the memory block that the converter has allocated
+ // for the C++ type.
+ typedef python::converter::rvalue_from_python_storage<Container> storage_type;
+ void* storage = reinterpret_cast<storage_type*>(data)->storage.bytes;
+
+ typedef python::stl_input_iterator<typename Container::value_type> iterator;
+
+ // Allocate the C++ type into the converter's memory block, and assign
+ // its handle to the converter's convertible variable. The C++
+ // container is populated by passing the begin and end iterators of
+ // the python object to the container's constructor.
+ new (storage) Container(iterator(python::object(handle)), // begin
+ iterator()); // end
+ data->convertible = storage;
+ }
+};
+
+/// @note Registers converter from a provided type to the python
+/// iterable type to the.
+template <class K, class V, bool NoProxy = true>
+struct map_to_dict {
+ static PyObject* convert(const std::map<K, V>& map) {
+ bp::dict* dict = new boost::python::dict();
+ typename std::map<K, V>::const_iterator it;
+ for (it = map.begin(); it != map.end(); ++it) {
+ if (NoProxy) {
+ dict->setdefault(it->first, boost::ref(it->second));
+ } else {
+ dict->setdefault(it->first, it->second);
+ }
+ }
+ return dict->ptr();
+ }
+ static PyTypeObject const* get_pytype() { return &PyDict_Type; }
+};
+
+/// Conversion from dict to map solution proposed in
+/// https://stackoverflow.com/questions/6116345/boostpython-possible-to-automatically-convert-from-dict-stdmap
+/// This template encapsulates the conversion machinery.
+template <typename key_t, typename val_t>
+struct dict_to_map {
+ /// The type of the map we convert the Python dict into
+ typedef std::map<key_t, val_t> map_t;
+
+ dict_to_map& from_python() {
+ boost::python::converter::registry::push_back(&dict_to_map::convertible, &dict_to_map::construct,
+ boost::python::type_id<map_t>());
+ // Support chaining.
+ return *this;
+ }
+
+ /// Check if conversion is possible
+ static void* convertible(PyObject* object) { return PyObject_GetIter(object) ? object : NULL; }
+
+ /// Perform the conversion
+ static void construct(PyObject* objptr, boost::python::converter::rvalue_from_python_stage1_data* data) {
+ // convert the PyObject pointed to by `objptr` to a boost::python::dict
+ boost::python::handle<> objhandle(boost::python::borrowed(objptr)); // "smart ptr"
+ boost::python::dict d(objhandle);
+
+ // get a pointer to memory into which we construct the map
+ // this is provided by the Python runtime
+ void* storage =
+ reinterpret_cast<boost::python::converter::rvalue_from_python_storage<map_t>*>(data)->storage.bytes;
+
+ // placement-new allocate the result
+ new (storage) map_t();
+
+ // iterate over the dictionary `d`, fill up the map `m`
+ map_t& m(*(static_cast<map_t*>(storage)));
+ boost::python::list keys(d.keys());
+ int keycount(static_cast<int>(boost::python::len(keys)));
+ for (int i = 0; i < keycount; ++i) {
+ // get the key
+ boost::python::object keyobj(keys[i]);
+ boost::python::extract<key_t> keyproxy(keyobj);
+ if (!keyproxy.check()) {
+ PyErr_SetString(PyExc_KeyError, "Bad key type");
+ boost::python::throw_error_already_set();
+ }
+ key_t key = keyproxy();
+
+ // get the corresponding value
+ boost::python::object valobj(d[keyobj]);
+ boost::python::extract<val_t> valproxy(valobj);
+ if (!valproxy.check()) {
+ PyErr_SetString(PyExc_ValueError, "Bad value type");
+ boost::python::throw_error_already_set();
+ }
+ val_t val = valproxy();
+ m[key] = val;
+ }
+
+ // remember the location for later
+ data->convertible = storage;
+ }
+};
+
+} // namespace python
+} // namespace crocoddyl
+
+#endif // BINDINGS_PYTHON_CROCODDYL_UTILS_HPP_
diff --git a/bindings/python/crocoddyl/utils/__init__.py b/bindings/python/crocoddyl/utils/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..5e732efafb4bd4efa1f4c9606ccbb811b00a6e1a
--- /dev/null
+++ b/bindings/python/crocoddyl/utils/__init__.py
@@ -0,0 +1,839 @@
+import crocoddyl
+import pinocchio
+import numpy as np
+import scipy.linalg as scl
+
+
+def a2m(a):
+ return np.matrix(a).T
+
+
+def m2a(m):
+ return np.array(m).squeeze()
+
+
+def rev_enumerate(l):
+ return reversed(list(enumerate(l)))
+
+
+def raiseIfNan(A, error=None):
+ if error is None:
+ error = scl.LinAlgError("NaN in array")
+ if np.any(np.isnan(A)) or np.any(np.isinf(A)) or np.any(abs(np.asarray(A)) > 1e30):
+ raise error
+
+
+class StateVectorDerived(crocoddyl.StateAbstract):
+ def __init__(self, nx):
+ crocoddyl.StateAbstract.__init__(self, nx, nx)
+
+ def zero(self):
+ return np.matrix(np.zeros(self.nx)).T
+
+ def rand(self):
+ return np.matrix(np.random.rand(self.nx)).T
+
+ def diff(self, x0, x1):
+ return x1 - x0
+
+ def integrate(self, x, dx):
+ return x + dx
+
+ def Jdiff(self, x1, x2, firstsecond='both'):
+ assert (firstsecond in ['first', 'second', 'both'])
+ if firstsecond == 'both':
+ return [self.Jdiff(x1, x2, 'first'), self.Jdiff(x1, x2, 'second')]
+
+ J = np.zeros([self.ndx, self.ndx])
+ if firstsecond == 'first':
+ J[:, :] = -np.eye(self.ndx)
+ elif firstsecond == 'second':
+ J[:, :] = np.eye(self.ndx)
+ return J
+
+ def Jintegrate(self, x, dx, firstsecond='both'):
+ assert (firstsecond in ['first', 'second', 'both'])
+ if firstsecond == 'both':
+ return [self.Jintegrate(x, dx, 'first'), self.Jintegrate(x, dx, 'second')]
+ return np.eye(self.ndx)
+
+
+class StateMultibodyDerived(crocoddyl.StateAbstract):
+ def __init__(self, pinocchioModel):
+ crocoddyl.StateAbstract.__init__(self, pinocchioModel.nq + pinocchioModel.nv, 2 * pinocchioModel.nv)
+ self.model = pinocchioModel
+
+ def zero(self):
+ q = pinocchio.neutral(self.model)
+ v = pinocchio.utils.zero(self.nv)
+ return np.concatenate([q, v])
+
+ def rand(self):
+ q = pinocchio.randomConfiguration(self.model)
+ v = pinocchio.utils.rand(self.nv)
+ return np.concatenate([q, v])
+
+ def diff(self, x0, x1):
+ q0 = x0[:self.nq]
+ q1 = x1[:self.nq]
+ v0 = x0[-self.nv:]
+ v1 = x1[-self.nv:]
+ dq = pinocchio.difference(self.model, q0, q1)
+ return np.concatenate([dq, v1 - v0])
+
+ def integrate(self, x, dx):
+ q = x[:self.nq]
+ v = x[-self.nv:]
+ dq = dx[:self.nv]
+ dv = dx[-self.nv:]
+ qn = pinocchio.integrate(self.model, q, dq)
+ return np.concatenate([qn, v + dv])
+
+ def Jdiff(self, x1, x2, firstsecond='both'):
+ assert (firstsecond in ['first', 'second', 'both'])
+ if firstsecond == 'both':
+ return [self.Jdiff(x1, x2, 'first'), self.Jdiff(x1, x2, 'second')]
+
+ if firstsecond == 'first':
+ dx = self.diff(x2, x1)
+ q = x2[:self.model.nq]
+ dq = dx[:self.model.nv]
+ Jdq = pinocchio.dIntegrate(self.model, q, dq)[1]
+ return np.matrix(-scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv)))
+ elif firstsecond == 'second':
+ dx = self.diff(x1, x2)
+ q = x1[:self.nq]
+ dq = dx[:self.nv]
+ Jdq = pinocchio.dIntegrate(self.model, q, dq)[1]
+ return np.matrix(scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv)))
+
+ def Jintegrate(self, x, dx, firstsecond='both'):
+ assert (firstsecond in ['first', 'second', 'both'])
+ if firstsecond == 'both':
+ return [self.Jintegrate(x, dx, 'first'), self.Jintegrate(x, dx, 'second')]
+
+ q = x[:self.nq]
+ dq = dx[:self.nv]
+ Jq, Jdq = pinocchio.dIntegrate(self.model, q, dq)
+ if firstsecond == 'first':
+ return np.matrix(scl.block_diag(np.linalg.inv(Jq), np.eye(self.nv)))
+ elif firstsecond == 'second':
+ return np.matrix(scl.block_diag(np.linalg.inv(Jdq), np.eye(self.nv)))
+
+
+class FreeFloatingActuationDerived(crocoddyl.ActuationModelAbstract):
+ def __init__(self, state):
+ assert (state.pinocchio.joints[1].shortname() == 'JointModelFreeFlyer')
+ crocoddyl.ActuationModelAbstract.__init__(self, state, state.nv - 6)
+
+ def calc(self, data, x, u):
+ data.a = 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
+
+
+class FullActuationDerived(crocoddyl.ActuationModelAbstract):
+ def __init__(self, state):
+ assert (state.pinocchio.joints[1].shortname() != 'JointModelFreeFlyer')
+ crocoddyl.ActuationModelAbstract.__init__(self, state, state.nv)
+
+ def calc(self, data, x, u):
+ data.a = u
+
+ def calcDiff(self, data, x, u, recalc=True):
+ if recalc:
+ self.calc(data, x, u)
+ data.Au = pinocchio.utils.eye(self.nu)
+
+
+class UnicycleDerived(crocoddyl.ActionModelAbstract):
+ def __init__(self):
+ crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(3), 2, 5)
+ self.dt = .1
+ self.costWeights = [10., 1.]
+
+ def calc(self, data, x, u=None):
+ if u is None:
+ u = self.unone
+ v, w = m2a(u)
+ px, py, theta = m2a(x)
+ c, s = np.cos(theta), np.sin(theta)
+ # Rollout the dynamics
+ data.xnext = a2m([px + c * v * self.dt, py + s * v * self.dt, theta + w * self.dt])
+ # Compute the cost value
+ data.r = np.vstack([self.costWeights[0] * x, self.costWeights[1] * u])
+ data.cost = .5 * sum(m2a(data.r)**2)
+
+ def calcDiff(self, data, x, u=None, recalc=True):
+ if u is None:
+ u = self.unone
+ if recalc:
+ self.calc(data, x, u)
+ v, w = m2a(u)
+ px, py, theta = m2a(x)
+ # Cost derivatives
+ data.Lx = a2m(m2a(x) * ([self.costWeights[0]**2] * self.state.nx))
+ data.Lu = a2m(m2a(u) * ([self.costWeights[1]**2] * self.nu))
+ data.Lxx = np.diag([self.costWeights[0]**2] * self.state.nx)
+ data.Luu = np.diag([self.costWeights[1]**2] * self.nu)
+ # Dynamic derivatives
+ c, s, dt = np.cos(theta), np.sin(theta), self.dt
+ v, w = m2a(u)
+ data.Fx = np.matrix([[1, 0, -s * v * dt], [0, 1, c * v * dt], [0, 0, 1]])
+ data.Fu = np.matrix([[c * self.dt, 0], [s * self.dt, 0], [0, self.dt]])
+
+
+class LQRDerived(crocoddyl.ActionModelAbstract):
+ def __init__(self, nx, nu, driftFree=True):
+ crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(nx), nu)
+
+ self.Fx = np.matrix(np.eye(self.state.nx))
+ self.Fu = np.matrix(np.eye(self.state.nx))[:, :self.nu]
+ self.f0 = np.matrix(np.zeros(self.state.nx)).T
+ self.Lxx = np.matrix(np.eye(self.state.nx))
+ self.Lxu = np.matrix(np.eye(self.state.nx))[:, :self.nu]
+ self.Luu = np.matrix(np.eye(self.nu))
+ self.lx = np.matrix(np.ones(self.state.nx)).T
+ self.lu = np.matrix(np.ones(self.nu)).T
+
+ def calc(self, data, x, u=None):
+ if u is None:
+ u = self.unone
+ data.xnext = self.Fx * x + self.Fu * u + self.f0
+ data.cost = 0.5 * np.asscalar(x.T * self.Lxx * x) + 0.5 * np.asscalar(u.T * self.Luu * u)
+ data.cost += np.asscalar(x.T * self.Lxu * u) + np.asscalar(self.lx.T * x) + np.asscalar(self.lu.T * u)
+
+ def calcDiff(self, data, x, u=None, recalc=True):
+ if u is None:
+ u = self.unone
+ if recalc:
+ self.calc(data, x, u)
+ data.Lx = self.lx + np.dot(self.Lxx, x) + np.dot(self.Lxu, u)
+ data.Lu = self.lu + np.dot(self.Lxu.T, x) + np.dot(self.Luu, u)
+ data.Fx = self.Fx
+ data.Fu = self.Fu
+ data.Lxx = self.Lxx
+ data.Luu = self.Luu
+ data.Lxu = self.Lxu
+
+
+class DifferentialLQRDerived(crocoddyl.DifferentialActionModelAbstract):
+ def __init__(self, nq, nu, driftFree=True):
+ crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(2 * nq), nu)
+
+ self.Fq = np.matrix(np.eye(self.state.nq))
+ self.Fv = np.matrix(np.eye(self.state.nv))
+ self.Fu = np.matrix(np.eye(self.state.nq))[:, :self.nu]
+ self.f0 = np.matrix(np.zeros(self.state.nv)).T
+ self.Lxx = np.matrix(np.eye(self.state.nx))
+ self.Lxu = np.matrix(np.eye(self.state.nx))[:, :self.nu]
+ self.Luu = np.matrix(np.eye(self.nu))
+ self.lx = np.matrix(np.ones(self.state.nx)).T
+ self.lu = np.matrix(np.ones(self.nu)).T
+
+ def calc(self, data, x, u=None):
+ if u is None:
+ u = self.unone
+ q, v = x[:self.state.nq], x[self.state.nq:]
+ data.xout = self.Fq * q + self.Fv * v + self.Fu * u + self.f0
+ data.cost = 0.5 * np.asscalar(x.T * self.Lxx * x) + 0.5 * np.asscalar(u.T * self.Luu * u)
+ data.cost += np.asscalar(x.T * self.Lxu * u) + np.asscalar(self.lx.T * x) + np.asscalar(self.lu.T * u)
+
+ def calcDiff(self, data, x, u=None, recalc=True):
+ if u is None:
+ u = self.unone
+ if recalc:
+ self.calc(data, x, u)
+ data.Lx = self.lx + np.dot(self.Lxx, x) + np.dot(self.Lxu, u)
+ data.Lu = self.lu + np.dot(self.Lxu.T, x) + np.dot(self.Luu, u)
+ data.Fx = np.hstack([self.Fq, self.Fv])
+ data.Fu = self.Fu
+ data.Lxx = self.Lxx
+ data.Luu = self.Luu
+ data.Lxu = self.Lxu
+
+
+class DifferentialFreeFwdDynamicsDerived(crocoddyl.DifferentialActionModelAbstract):
+ def __init__(self, state, costModel):
+ crocoddyl.DifferentialActionModelAbstract.__init__(self, state, state.nv, costModel.nr)
+ self.costs = costModel
+ self.forceAba = True
+ self.armature = np.matrix(np.zeros(0))
+
+ def calc(self, data, x, u=None):
+ if u is None:
+ 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:
+ data.xout = pinocchio.aba(self.state.pinocchio, data.pinocchio, q, v, u)
+ else:
+ pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)
+ data.M = data.pinocchio.M
+ if self.armature.size == self.state.nv:
+ data.M[range(self.state.nv), range(self.state.nv)] += self.armature
+ data.Minv = np.linalg.inv(data.M)
+ data.xout = data.Minv * (u - data.pinocchio.nle)
+ # Computing the cost value and residuals
+ pinocchio.forwardKinematics(self.state.pinocchio, data.pinocchio, q, v)
+ pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)
+ self.costs.calc(data.costs, x, u)
+ data.cost = data.costs.cost
+
+ def calcDiff(self, data, x, u=None, recalc=True):
+ q, v = x[:self.state.nq], x[-self.state.nv:]
+ if u is None:
+ u = self.unone
+ if recalc:
+ self.calc(data, x, u)
+ pinocchio.computeJointJacobians(self.state.pinocchio, data.pinocchio, q)
+ # Computing the dynamics derivatives
+ if self.forceAba:
+ 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
+ else:
+ pinocchio.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout)
+ data.Fx = -np.hstack([data.Minv * data.pinocchio.dtau_dq, data.Minv * data.pinocchio.dtau_dv])
+ data.Fu = data.Minv
+ # Computing the cost derivatives
+ self.costs.calcDiff(data.costs, x, u, False)
+
+ def set_armature(self, armature):
+ if armature.size is not self.state.nv:
+ print('The armature dimension is wrong, we cannot set it.')
+ else:
+ self.forceAba = 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)
+ return data
+
+
+class IntegratedActionModelEuler(crocoddyl.ActionModelAbstract):
+ def __init__(self, diffModel, timeStep=1e-3, withCostResiduals=True):
+ crocoddyl.ActionModelAbstract.__init__(self, diffModel.state, diffModel.nv, diffModel.nr)
+ self.differential = diffModel
+ self.withCostResiduals = withCostResiduals
+ self.timeStep = timeStep
+
+ def calc(self, data, x, u=None):
+ nq, dt = self.state.nq, self.timeStep
+ acc, cost = self.differential.calc(data.differential, x, u)
+ if self.withCostResiduals:
+ data.r = data.differential.r
+ data.cost = cost
+ # data.xnext[nq:] = x[nq:] + acc*dt
+ # data.xnext[:nq] = pinocchio.integrate(self.differential.pinocchio,
+ # a2m(x[:nq]),a2m(data.xnext[nq:]*dt)).flat
+ data.dx = np.concatenate([x[nq:] * dt + acc * dt**2, acc * dt])
+ data.xnext[:] = self.differential.state.integrate(x, data.dx)
+
+ return data.xnext, data.cost
+
+ def calcDiff(self, data, x, u=None, recalc=True):
+ nv, dt = self.state.nv, self.timeStep
+ if recalc:
+ self.calc(data, x, u)
+ self.differential.calcDiff(data.differential, x, u, recalc=False)
+ dxnext_dx, dxnext_ddx = self.state.Jintegrate(x, data.dx)
+ da_dx, da_du = data.differential.Fx, data.differential.Fu
+ ddx_dx = np.vstack([da_dx * dt, da_dx])
+ ddx_dx[range(nv), range(nv, 2 * nv)] += 1
+ data.Fx[:, :] = dxnext_dx + dt * np.dot(dxnext_ddx, ddx_dx)
+ ddx_du = np.vstack([da_du * dt, da_du])
+ data.Fu[:, :] = dt * np.dot(dxnext_ddx, ddx_du)
+ data.Lx[:] = data.differential.Lx
+ data.Lu[:] = data.differential.Lu
+ data.Lxx[:] = data.differential.Lxx
+ data.Lxu[:] = data.differential.Lxu
+ data.Luu[:] = data.differential.Luu
+
+
+class StateCostDerived(crocoddyl.CostModelAbstract):
+ def __init__(self, state, activation=None, xref=None, nu=None):
+ activation = activation if activation is not None else crocoddyl.ActivationModelQuad(state.ndx)
+ self.xref = xref if xref is not None else state.zero()
+ crocoddyl.CostModelAbstract.__init__(self, state, activation, nu)
+
+ def calc(self, data, x, u):
+ data.r = self.state.diff(self.xref, x)
+ self.activation.calc(data.activation, data.r)
+ data.cost = data.activation.a
+
+ def calcDiff(self, data, x, u, recalc=True):
+ if recalc:
+ self.calc(data, x, u)
+ data.Rx = self.state.Jdiff(self.xref, x, 'second')[0]
+ self.activation.calcDiff(data.activation, data.r, recalc)
+ data.Lx = data.Rx.T * data.activation.Ar
+ data.Lxx = data.Rx.T * data.activation.Arr * data.Rx
+
+
+class ControlCostDerived(crocoddyl.CostModelAbstract):
+ def __init__(self, state, activation=None, uref=None, nu=None):
+ nu = nu if nu is not None else state.nv
+ activation = activation if activation is not None else crocoddyl.ActivationModelQuad(nu)
+ self.uref = uref if uref is not None else pinocchio.utils.zero(nu)
+ crocoddyl.CostModelAbstract.__init__(self, state, activation, nu)
+
+ def calc(self, data, x, u):
+ data.r = u - self.uref
+ self.activation.calc(data.activation, data.r)
+ data.cost = data.activation.a
+
+ def calcDiff(self, data, x, u, recalc=True):
+ if recalc:
+ self.calc(data, x, u)
+ self.activation.calcDiff(data.activation, data.r, recalc)
+ data.Lu = data.activation.Ar
+ data.Luu = data.activation.Arr
+
+
+class CoMPositionCostDerived(crocoddyl.CostModelAbstract):
+ def __init__(self, state, activation=None, cref=None, nu=None):
+ activation = activation if activation is not None else crocoddyl.ActivationModelQuad(3)
+ crocoddyl.CostModelAbstract.__init__(self, state, activation, nu)
+ self.cref = cref
+
+ def calc(self, data, x, u):
+ data.r = data.pinocchio.com[0] - self.cref
+ self.activation.calc(data.activation, data.r)
+ data.cost = data.activation.a
+
+ def calcDiff(self, data, x, u, recalc=True):
+ if recalc:
+ self.calc(data, x, u)
+ self.activation.calcDiff(data.activation, data.r, recalc)
+ data.Rx = np.hstack([data.pinocchio.Jcom, pinocchio.utils.zero((self.activation.nr, self.state.nv))])
+ data.Lx = np.vstack([data.pinocchio.Jcom.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1))])
+ data.Lxx = np.vstack([
+ np.hstack([
+ data.pinocchio.Jcom.T * data.activation.Arr * data.pinocchio.Jcom,
+ pinocchio.utils.zero((self.state.nv, self.state.nv))
+ ]),
+ pinocchio.utils.zero((self.state.nv, self.state.ndx))
+ ])
+
+
+class FramePlacementCostDerived(crocoddyl.CostModelAbstract):
+ def __init__(self, state, activation=None, Mref=None, nu=None):
+ activation = activation if activation is not None else crocoddyl.ActivationModelQuad(6)
+ crocoddyl.CostModelAbstract.__init__(self, state, activation, nu)
+ self.Mref = Mref
+
+ def calc(self, data, x, u):
+ data.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[self.Mref.frame]
+ data.r = pinocchio.log(data.rMf).vector
+ self.activation.calc(data.activation, data.r)
+ data.cost = data.activation.a
+
+ def calcDiff(self, data, x, u, recalc=True):
+ if recalc:
+ self.calc(data, x, u)
+ pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)
+ data.rJf = pinocchio.Jlog6(data.rMf)
+ data.fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.Mref.frame,
+ pinocchio.ReferenceFrame.LOCAL)
+ data.J = data.rJf * data.fJf
+ self.activation.calcDiff(data.activation, data.r, recalc)
+ data.Rx = np.hstack([data.J, pinocchio.utils.zero((self.activation.nr, self.state.nv))])
+ data.Lx = np.vstack([data.J.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1))])
+ data.Lxx = np.vstack([
+ np.hstack([data.J.T * data.activation.Arr * data.J,
+ pinocchio.utils.zero((self.state.nv, self.state.nv))]),
+ pinocchio.utils.zero((self.state.nv, self.state.ndx))
+ ])
+
+
+class FrameTranslationCostDerived(crocoddyl.CostModelAbstract):
+ def __init__(self, state, activation=None, xref=None, nu=None):
+ activation = activation if activation is not None else crocoddyl.ActivationModelQuad(3)
+ crocoddyl.CostModelAbstract.__init__(self, state, activation, nu)
+ self.xref = xref
+
+ def calc(self, data, x, u):
+ data.r = data.pinocchio.oMf[self.xref.frame].translation - self.xref.oxf
+ self.activation.calc(data.activation, data.r)
+ data.cost = data.activation.a
+
+ def calcDiff(self, data, x, u, recalc=True):
+ if recalc:
+ self.calc(data, x, u)
+ pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)
+ data.R = data.pinocchio.oMf[self.xref.frame].rotation
+ data.J = data.R * pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.xref.frame,
+ pinocchio.ReferenceFrame.LOCAL)[:3, :]
+ self.activation.calcDiff(data.activation, data.r, recalc)
+ data.Rx = np.hstack([data.J, pinocchio.utils.zero((self.activation.nr, self.state.nv))])
+ data.Lx = np.vstack([data.J.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1))])
+ data.Lxx = np.vstack([
+ np.hstack([data.J.T * data.activation.Arr * data.J,
+ pinocchio.utils.zero((self.state.nv, self.state.nv))]),
+ pinocchio.utils.zero((self.state.nv, self.state.ndx))
+ ])
+
+
+class FrameVelocityCostDerived(crocoddyl.CostModelAbstract):
+ def __init__(self, state, activation=None, vref=None, nu=None):
+ activation = activation if activation is not None else crocoddyl.ActivationModelQuad(6)
+ crocoddyl.CostModelAbstract.__init__(self, state, activation, nu)
+ self.vref = vref
+ self.joint = state.pinocchio.frames[vref.frame].parent
+ self.fXj = state.pinocchio.frames[vref.frame].placement.inverse().action
+
+ def calc(self, data, x, u):
+ data.r = (pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.vref.frame) -
+ self.vref.oMf).vector
+ self.activation.calc(data.activation, data.r)
+ data.cost = data.activation.a
+
+ def calcDiff(self, data, x, u, recalc=True):
+ if recalc:
+ self.calc(data, x, u)
+ v_partial_dq, v_partial_dv = pinocchio.getJointVelocityDerivatives(self.state.pinocchio, data.pinocchio,
+ self.joint, pinocchio.ReferenceFrame.LOCAL)
+
+ self.activation.calcDiff(data.activation, data.r, recalc)
+ data.Rx = np.hstack([self.fXj * v_partial_dq, self.fXj * v_partial_dv])
+ data.Lx = data.Rx.T * data.activation.Ar
+ data.Lxx = data.Rx.T * data.activation.Arr * data.Rx
+
+
+class Contact3DDerived(crocoddyl.ContactModelAbstract):
+ def __init__(self, state, xref, gains=[0., 0.]):
+ crocoddyl.ContactModelAbstract.__init__(self, state, 3)
+ self.xref = xref
+ self.gains = gains
+ self.joint = state.pinocchio.frames[xref.frame].parent
+ self.fXj = state.pinocchio.frames[xref.frame].placement.inverse().action
+ v = pinocchio.Motion().Zero()
+ self.vw = v.angular
+ self.vv = v.linear
+ self.Jw = pinocchio.utils.zero((3, state.pinocchio.nv))
+
+ def calc(self, data, x):
+ assert (self.xref.oxf is not None or self.gains[0] == 0.)
+ v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.xref.frame)
+ self.vw = v.angular
+ self.vv = v.linear
+
+ fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.xref.frame,
+ pinocchio.ReferenceFrame.LOCAL)
+ data.Jc = fJf[:3, :]
+ self.Jw = fJf[3:, :]
+
+ data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio,
+ self.xref.frame).linear + pinocchio.utils.cross(self.vw, self.vv)
+ if self.gains[0] != 0.:
+ data.a0 += np.asscalar(self.gains[0]) * (data.pinocchio.oMf[self.xref.frame].translation - self.xref.oxf)
+ if self.gains[1] != 0.:
+ data.a0 += np.asscalar(self.gains[1]) * self.vv
+
+ def calcDiff(self, data, x, recalc=True):
+ if recalc:
+ self.calc(data, x)
+ v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da = pinocchio.getJointAccelerationDerivatives(
+ self.state.pinocchio, data.pinocchio, self.joint, pinocchio.ReferenceFrame.LOCAL)
+
+ 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
+
+ if np.asscalar(self.gains[0]) != 0.:
+ R = data.pinocchio.oMf[self.xref.frame].rotation
+ Aq += 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])
+
+
+class Contact6DDerived(crocoddyl.ContactModelAbstract):
+ def __init__(self, state, Mref, gains=[0., 0.]):
+ crocoddyl.ContactModelAbstract.__init__(self, state, 6)
+ self.Mref = Mref
+ self.gains = gains
+ self.joint = state.pinocchio.frames[Mref.frame].parent
+ self.fXj = state.pinocchio.frames[Mref.frame].placement.inverse().action
+
+ def calc(self, data, x):
+ assert (self.Mref.oMf is not None or self.gains[0] == 0.)
+ data.Jc = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.Mref.frame,
+ pinocchio.ReferenceFrame.LOCAL)
+ data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector
+ if self.gains[0] != 0.:
+ self.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[self.Mref.frame]
+ data.a0 += np.asscalar(self.gains[0]) * pinocchio.log6(self.rMf).vector
+ if self.gains[1] != 0.:
+ v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector
+ data.a0 += np.asscalar(self.gains[1]) * v
+
+ def calcDiff(self, data, x, recalc=True):
+ if recalc:
+ self.calc(data, x)
+ 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)
+
+ if np.asscalar(self.gains[0]) != 0.:
+ Aq += 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])
+
+
+class Impulse3DDerived(crocoddyl.ImpulseModelAbstract):
+ def __init__(self, state, frame):
+ crocoddyl.ImpulseModelAbstract.__init__(self, state, 3)
+ self.frame = frame
+ self.joint = state.pinocchio.frames[frame].parent
+ self.fXj = state.pinocchio.frames[frame].placement.inverse().action
+
+ def calc(self, data, x):
+ data.Jc = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.frame,
+ pinocchio.ReferenceFrame.LOCAL)[:3, :]
+
+ def calcDiff(self, data, x, recalc=True):
+ if recalc:
+ 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
+
+
+class Impulse6DDerived(crocoddyl.ImpulseModelAbstract):
+ def __init__(self, state, frame):
+ crocoddyl.ImpulseModelAbstract.__init__(self, state, 6)
+ self.frame = frame
+ self.joint = state.pinocchio.frames[frame].parent
+ self.fXj = state.pinocchio.frames[frame].placement.inverse().action
+
+ def calc(self, data, x):
+ data.Jc = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.frame,
+ pinocchio.ReferenceFrame.LOCAL)
+
+ def calcDiff(self, data, x, recalc=True):
+ if recalc:
+ 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
+
+
+class DDPDerived(crocoddyl.SolverAbstract):
+ def __init__(self, shootingProblem):
+ 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.alphas = [2**(-n) for n in range(10)]
+ self.th_grad = 1e-12
+
+ self.x_reg = 0
+ self.u_reg = 0
+ self.regFactor = 10
+ self.regMax = 1e9
+ self.regMin = 1e-9
+ self.th_step = .5
+
+ 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)
+
+ return self.cost
+
+ def computeDirection(self, recalc=True):
+ if recalc:
+ self.calc()
+ self.backwardPass()
+ return [np.nan] * (self.problem.T + 1), self.k, self.Vx
+
+ def stoppingCriteria(self):
+ return sum([np.asscalar(q.T * q) for q in self.Qu])
+
+ def expectedImprovement(self):
+ d1 = sum([np.asscalar(q.T * k) for q, k in zip(self.Qu, self.k)])
+ d2 = sum([-np.asscalar(k.T * q * k) for q, k in zip(self.Quu, self.k)])
+ return np.matrix([d1, d2]).T
+
+ def tryStep(self, stepLength=1):
+ self.forwardPass(stepLength)
+ return self.cost - self.cost_try
+
+ 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
+ d = self.expectedImprovement()
+ d1, d2 = np.asscalar(d[0]), np.asscalar(d[1])
+
+ for a in self.alphas:
+ try:
+ self.dV = self.tryStep(a)
+ except ArithmeticError:
+ continue
+ self.dV_exp = a * (d1 + .5 * d2 * a)
+ if d1 < self.th_grad or not self.isFeasible or self.dV > self.th_acceptStep * self.dV_exp:
+ # Accept step
+ self.wasFeasible = self.isFeasible
+ self.setCandidate(self.xs_try, self.us_try, True)
+ 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
+
+ # Warning: no convergence in max iterations
+ return self.xs, self.us, False
+
+ def increaseRegularization(self):
+ self.x_reg *= self.regFactor
+ if self.x_reg > self.regMax:
+ self.x_reg = self.regMax
+ self.u_reg = self.x_reg
+
+ def decreaseRegularization(self):
+ self.x_reg /= self.regFactor
+ if self.x_reg < self.regMin:
+ 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()]
+
+ self.Q = [a2m(np.zeros([m.state.ndx + m.nu, m.state.ndx + m.nu])) for m in self.problem.runningModels]
+ self.q = [a2m(np.zeros([m.state.ndx + m.nu])) for m in self.problem.runningModels]
+ self.Qxx = [Q[:m.state.ndx, :m.state.ndx] for m, Q in zip(self.problem.runningModels, self.Q)]
+ self.Qxu = [Q[:m.state.ndx, m.state.ndx:] for m, Q in zip(self.problem.runningModels, self.Q)]
+ self.Qux = [Qxu.T for m, Qxu in zip(self.problem.runningModels, self.Qxu)]
+ self.Quu = [Q[m.state.ndx:, m.state.ndx:] for m, Q in zip(self.problem.runningModels, self.Q)]
+ self.Qx = [q[:m.state.ndx] for m, q in zip(self.problem.runningModels, self.q)]
+ self.Qu = [q[m.state.ndx:] for m, q in zip(self.problem.runningModels, self.q)]
+
+ self.K = [np.matrix(np.zeros([m.nu, m.state.ndx])) for m in self.problem.runningModels]
+ self.k = [a2m(np.zeros([m.nu])) for m in self.problem.runningModels]
+
+ self.xs_try = [self.problem.x0] + [np.nan * self.problem.x0] * self.problem.T
+ self.us_try = [np.nan] * self.problem.T
+ self.gaps = [a2m(np.zeros(self.problem.runningModels[0].state.ndx))
+ ] + [a2m(np.zeros(m.state.ndx)) for m in self.problem.runningModels]
+
+ def backwardPass(self):
+ self.Vx[-1][:] = self.problem.terminalData.Lx
+ self.Vxx[-1][:, :] = self.problem.terminalData.Lxx
+
+ if self.x_reg != 0:
+ ndx = self.problem.terminalModel.state.ndx
+ self.Vxx[-1][range(ndx), range(ndx)] += self.x_reg
+
+ 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
+
+ self.computeGains(t)
+
+ if self.u_reg == 0:
+ self.Vx[t][:] = self.Qx[t] - self.K[t].T * self.Qu[t]
+ else:
+ self.Vx[t][:] = self.Qx[t] - 2 * self.K[t].T * self.Qu[t] + self.K[t].T * self.Quu[t] * self.k[t]
+ self.Vxx[t][:, :] = self.Qxx[t] - self.Qxu[t] * self.K[t]
+ self.Vxx[t][:, :] = 0.5 * (self.Vxx[t][:, :] + self.Vxx[t][:, :].T) # ensure symmetric
+
+ if self.x_reg != 0:
+ self.Vxx[t][range(model.state.ndx), range(model.state.ndx)] += self.x_reg
+ raiseIfNan(self.Vxx[t], ArithmeticError('backward error'))
+ raiseIfNan(self.Vx[t], ArithmeticError('backward error'))
+
+ def computeGains(self, t):
+ try:
+ if self.Quu[t].shape[0] > 0:
+ Lb = scl.cho_factor(self.Quu[t])
+ self.K[t][:, :] = scl.cho_solve(Lb, self.Qux[t])
+ self.k[t][:] = scl.cho_solve(Lb, self.Qu[t])
+ else:
+ pass
+ except scl.LinAlgError:
+ 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
+ for t, (m, d) in enumerate(zip(self.problem.runningModels, self.problem.runningDatas)):
+ utry[t] = us[t] - self.k[t] * stepLength - np.dot(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
+ xtry[t + 1] = xnext.copy() # not sure copy helpful here.
+ ctry += cost
+ raiseIfNan([ctry, cost], ArithmeticError('forward error'))
+ raiseIfNan(xtry[t + 1], ArithmeticError('forward error'))
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..ffb92cfb87cebffa9d056ed6e827456609e73781
--- /dev/null
+++ b/bindings/python/crocoddyl/utils/biped.py
@@ -0,0 +1,267 @@
+import crocoddyl
+import pinocchio
+import numpy as np
+
+
+class SimpleBipedGaitProblem:
+ """ Defines a simple 3d locomotion problem
+ """
+ def __init__(self, rmodel, rightFoot, leftFoot):
+ self.rmodel = rmodel
+ self.rdata = rmodel.createData()
+ self.state = crocoddyl.StateMultibody(self.rmodel)
+ self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
+ # Getting the frame id for all the legs
+ self.rfId = self.rmodel.getFrameId(rightFoot)
+ self.lfId = self.rmodel.getFrameId(leftFoot)
+ # Defining default state
+ q0 = self.rmodel.referenceConfigurations["half_sitting"]
+ self.rmodel.defaultState = np.concatenate([q0, np.zeros((self.rmodel.nv, 1))])
+ self.firstStep = True
+ # Remove the armature
+ # self.rmodel.armature[6:] = 1.
+
+ def createWalkingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
+ """ Create a shooting problem for a simple walking gait.
+
+ :param x0: initial state
+ :param stepLength: step length
+ :param stepHeight: step height
+ :param timeStep: step time for each knot
+ :param stepKnots: number of knots for step phases
+ :param supportKnots: number of knots for double support phases
+ :return shooting problem
+ """
+ # Compute the current foot positions
+ q0 = x0[:self.state.nq]
+ pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
+ pinocchio.updateFramePlacements(self.rmodel, self.rdata)
+ rfPos0 = self.rdata.oMf[self.rfId].translation
+ lfPos0 = self.rdata.oMf[self.lfId].translation
+ comRef = (rfPos0 + lfPos0) / 2
+ comRef[2] = 0.6185
+
+ # Defining the action models along the time instances
+ loco3dModel = []
+ doubleSupport = [self.createSwingFootModel(timeStep, [self.rfId, self.lfId]) for k in range(supportKnots)]
+
+ # Creating the action models for three steps
+ if self.firstStep is True:
+ rStep = self.createFootstepModels(comRef, [rfPos0], 0.5 * stepLength, stepHeight, timeStep, stepKnots,
+ [self.lfId], [self.rfId])
+ self.firstStep = False
+ else:
+ rStep = self.createFootstepModels(comRef, [rfPos0], stepLength, stepHeight, timeStep, stepKnots,
+ [self.lfId], [self.rfId])
+ lStep = self.createFootstepModels(comRef, [lfPos0], stepLength, stepHeight, timeStep, stepKnots, [self.rfId],
+ [self.lfId])
+
+ # We defined the problem as:
+ loco3dModel += doubleSupport + rStep
+ loco3dModel += doubleSupport + lStep
+
+ 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.
+
+ :param comPos0, initial CoM position
+ :param feetPos0: initial position of the swinging feet
+ :param stepLength: step length
+ :param stepHeight: step height
+ :param timeStep: time step
+ :param numKnots: number of knots for the footstep phase
+ :param supportFootIds: Ids of the supporting feet
+ :param swingFootIds: Ids of the swinging foot
+ :return footstep action models
+ """
+ numLegs = len(supportFootIds) + len(swingFootIds)
+ comPercentage = float(len(swingFootIds)) / numLegs
+
+ # Action models for the foot swing
+ footSwingModel = []
+ for k in range(numKnots):
+ swingFootTask = []
+ for i, p in zip(swingFootIds, feetPos0):
+ # Defining a foot swing task given the step length. The swing task
+ # is decomposed on two phases: swing-up and swing-down. We decide
+ # deliveratively to allocated the same number of nodes (i.e. phKnots)
+ # in each phase. With this, we define a proper z-component for the
+ # swing-leg motion.
+ phKnots = numKnots / 2
+ if k < phKnots:
+ dp = np.matrix([[stepLength * (k + 1) / numKnots, 0., stepHeight * k / phKnots]]).T
+ elif k == phKnots:
+ dp = np.matrix([[stepLength * (k + 1) / numKnots, 0., stepHeight]]).T
+ else:
+ dp = np.matrix(
+ [[stepLength * (k + 1) / numKnots, 0., stepHeight * (1 - float(k - phKnots) / phKnots)]]).T
+ tref = np.asmatrix(p + dp)
+
+ swingFootTask += [crocoddyl.FramePlacement(i, pinocchio.SE3(np.eye(3), tref))]
+
+ 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)
+
+ # Updating the current foot position for next step
+ comPos0 += np.matrix([stepLength * comPercentage, 0., 0.]).T
+ for p in feetPos0:
+ p += np.matrix([[stepLength, 0., 0.]]).T
+ return footSwingModel + [footSwitchModel]
+
+ def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None):
+ """ Action model for a swing foot phase.
+
+ :param timeStep: step duration of the action model
+ :param supportFootIds: Ids of the constrained feet
+ :param comTask: CoM task
+ :param swingFootTask: swinging foot task
+ :return action model for a swing foot phase
+ """
+ # Creating a 6D multi-contact model, and then including the supporting
+ # foot
+ contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
+ for i in supportFootIds:
+ Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity())
+ supportContactModel = \
+ crocoddyl.ContactModel6D(self.state, Mref, self.actuation.nu, np.matrix([0., 0.]).T)
+ contactModel.addContact('contact_' + str(i), supportContactModel)
+
+ # Creating the cost model for a contact phase
+ costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
+ if isinstance(comTask, np.ndarray):
+ comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu)
+ costModel.addCost("comTrack", comTrack, 1e4)
+ if swingFootTask is not None:
+ for i in swingFootTask:
+ footTrack = crocoddyl.CostModelFramePlacement(self.state, i, self.actuation.nu)
+ costModel.addCost("footTrack_" + str(i), footTrack, 1e4)
+
+ stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv)
+ stateReg = crocoddyl.CostModelState(self.state,
+ crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
+ self.rmodel.defaultState, self.actuation.nu)
+ ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
+ costModel.addCost("stateReg", stateReg, 1e-1)
+ costModel.addCost("ctrlReg", ctrlReg, 1e-3)
+
+ # Creating the action model for the KKT dynamics with simpletic Euler
+ # integration scheme
+ dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel,
+ costModel)
+ model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
+ return model
+
+ def createFootSwitchModel(self, supportFootIds, swingFootTask):
+ """ Action model for a foot switch phase.
+
+ :param supportFootIds: Ids of the constrained feet
+ :param swingFootTask: swinging foot task
+ :return action model for a foot switch phase
+ """
+ # Creating a 6D multi-contact model, and then including the supporting
+ # foot
+ contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
+ for i in supportFootIds:
+ Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity())
+ supportContactModel = crocoddyl.ContactModel6D(self.state, Mref, self.actuation.nu, np.matrix([0., 0.]).T)
+ contactModel.addContact('contact_' + str(i), supportContactModel)
+
+ # Creating the cost model for a contact phase
+ costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
+ if swingFootTask is not None:
+ for i in swingFootTask:
+ 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)
+
+ stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv)
+ stateReg = crocoddyl.CostModelState(self.state,
+ crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
+ self.rmodel.defaultState, self.actuation.nu)
+ ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
+ costModel.addCost("stateReg", stateReg, 1e1)
+ costModel.addCost("ctrlReg", ctrlReg, 1e-3)
+
+ # Creating the action model for the KKT dynamics with simpletic Euler
+ # integration scheme
+ dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel,
+ costModel)
+ model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
+ return model
+
+
+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]
+ 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]
+
+ # Plotting the joint positions, velocities and torques
+ plt.figure(figIndex)
+ legJointNames = ['1', '2', '3', '4', '5', '6']
+ # left foot
+ plt.subplot(2, 3, 1)
+ plt.title('joint position [rad]')
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(7, 13))]
+ plt.ylabel('LF')
+ plt.legend()
+ plt.subplot(2, 3, 2)
+ plt.title('joint velocity [rad/s]')
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 6, nq + 12))]
+ plt.ylabel('LF')
+ plt.legend()
+ plt.subplot(2, 3, 3)
+ plt.title('joint torque [Nm]')
+ [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(0, 6))]
+ plt.ylabel('LF')
+ plt.legend()
+
+ # right foot
+ plt.subplot(2, 3, 4)
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(13, 19))]
+ plt.ylabel('RF')
+ plt.xlabel('knots')
+ plt.legend()
+ plt.subplot(2, 3, 5)
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 12, nq + 18))]
+ plt.ylabel('RF')
+ plt.xlabel('knots')
+ plt.legend()
+ plt.subplot(2, 3, 6)
+ [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(6, 12))]
+ plt.ylabel('RF')
+ plt.xlabel('knots')
+ plt.legend()
+
+ plt.figure(figIndex + 1)
+ rdata = rmodel.createData()
+ Cx = []
+ Cy = []
+ for x in xs:
+ q = x[:rmodel.nq]
+ c = pinocchio.centerOfMass(rmodel, rdata, q)
+ Cx.append(np.asscalar(c[0]))
+ Cy.append(np.asscalar(c[1]))
+ plt.plot(Cx, Cy)
+ plt.title('CoM position')
+ plt.xlabel('x [m]')
+ plt.ylabel('y [m]')
+ plt.grid(True)
+ if show:
+ plt.show()
diff --git a/bindings/python/crocoddyl/utils/quadruped.py b/bindings/python/crocoddyl/utils/quadruped.py
new file mode 100644
index 0000000000000000000000000000000000000000..eb9dd4841d015baeaf9e648285f27c8e634f82e4
--- /dev/null
+++ b/bindings/python/crocoddyl/utils/quadruped.py
@@ -0,0 +1,521 @@
+import crocoddyl
+import pinocchio
+import numpy as np
+
+
+class SimpleQuadrupedalGaitProblem:
+ def __init__(self, rmodel, lfFoot, rfFoot, lhFoot, rhFoot):
+ self.rmodel = rmodel
+ self.rdata = rmodel.createData()
+ self.state = crocoddyl.StateMultibody(self.rmodel)
+ self.actuation = crocoddyl.ActuationModelFloatingBase(self.state)
+ # Getting the frame id for all the legs
+ self.lfFootId = self.rmodel.getFrameId(lfFoot)
+ self.rfFootId = self.rmodel.getFrameId(rfFoot)
+ self.lhFootId = self.rmodel.getFrameId(lhFoot)
+ self.rhFootId = self.rmodel.getFrameId(rhFoot)
+ # Defining default state
+ q0 = self.rmodel.referenceConfigurations["half_sitting"]
+ self.rmodel.defaultState = np.concatenate([q0, np.zeros((self.rmodel.nv, 1))])
+ self.firstStep = True
+
+ def createCoMProblem(self, x0, comGoTo, timeStep, numKnots):
+ """ Create a shooting problem for a CoM forward/backward task.
+
+ :param x0: initial state
+ :param comGoTo: initial CoM motion
+ :param timeStep: step time for each knot
+ :param numKnots: number of knots per each phase
+ :return shooting problem
+ """
+ # Compute the current foot positions
+ q0 = x0[:self.rmodel.nq]
+ 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 = []
+
+ # Creating the action model for the CoM task
+ comForwardModels = [
+ self.createSwingFootModel(
+ timeStep,
+ [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ ) for k in range(numKnots)
+ ]
+ comForwardTermModel = self.createSwingFootModel(timeStep,
+ [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ com0 + np.matrix([comGoTo, 0., 0.]).T)
+ comForwardTermModel.differential.costs.costs['comTrack'].weight = 1e6
+
+ comBackwardModels = [
+ self.createSwingFootModel(
+ timeStep,
+ [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ ) for k in range(numKnots)
+ ]
+ comBackwardTermModel = self.createSwingFootModel(timeStep,
+ [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ com0 + np.matrix([-comGoTo, 0., 0.]).T)
+ comBackwardTermModel.differential.costs.costs['comTrack'].weight = 1e6
+
+ # Adding the CoM tasks
+ comModels += comForwardModels + [comForwardTermModel]
+ comModels += comBackwardModels + [comBackwardTermModel]
+
+ # Defining the shooting problem
+ problem = crocoddyl.ShootingProblem(x0, comModels, comModels[-1])
+ return problem
+
+ def createWalkingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
+ """ Create a shooting problem for a simple walking gait.
+
+ :param x0: initial state
+ :param stepLength: step length
+ :param stepHeight: step height
+ :param timeStep: step time for each knot
+ :param stepKnots: number of knots for step phases
+ :param supportKnots: number of knots for double support phases
+ :return shooting problem
+ """
+ # Compute the current foot positions
+ 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
+ comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
+ comRef[2] = 0.5325
+
+ # Defining the action models along the time instances
+ loco3dModel = []
+ doubleSupport = [
+ self.createSwingFootModel(
+ timeStep,
+ [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ ) for k in range(supportKnots)
+ ]
+ if self.firstStep is True:
+ rhStep = self.createFootstepModels(comRef, [rhFootPos0], 0.5 * stepLength, stepHeight, timeStep, stepKnots,
+ [self.lfFootId, self.rfFootId, self.lhFootId], [self.rhFootId])
+ rfStep = self.createFootstepModels(comRef, [rfFootPos0], 0.5 * stepLength, stepHeight, timeStep, stepKnots,
+ [self.lfFootId, self.lhFootId, self.rhFootId], [self.rfFootId])
+ self.firstStep = False
+ else:
+ rhStep = self.createFootstepModels(comRef, [rhFootPos0], stepLength, stepHeight, timeStep, stepKnots,
+ [self.lfFootId, self.rfFootId, self.lhFootId], [self.rhFootId])
+ rfStep = self.createFootstepModels(comRef, [rfFootPos0], stepLength, stepHeight, timeStep, stepKnots,
+ [self.lfFootId, self.lhFootId, self.rhFootId], [self.rfFootId])
+ lhStep = self.createFootstepModels(comRef, [lhFootPos0], stepLength, stepHeight, timeStep, stepKnots,
+ [self.lfFootId, self.rfFootId, self.rhFootId], [self.lhFootId])
+ lfStep = self.createFootstepModels(comRef, [lfFootPos0], stepLength, stepHeight, timeStep, stepKnots,
+ [self.rfFootId, self.lhFootId, self.rhFootId], [self.lfFootId])
+ loco3dModel += doubleSupport + rhStep + rfStep
+ loco3dModel += doubleSupport + lhStep + lfStep
+
+ problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
+ return problem
+
+ def createTrottingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
+ """ Create a shooting problem for a simple trotting gait.
+
+ :param x0: initial state
+ :param stepLength: step length
+ :param stepHeight: step height
+ :param timeStep: step time for each knot
+ :param stepKnots: number of knots for step phases
+ :param supportKnots: number of knots for double support phases
+ :return shooting problem
+ """
+ # Compute the current foot positions
+ 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
+ comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
+ comRef[2] = 0.5325
+
+ # Defining the action models along the time instances
+ loco3dModel = []
+ doubleSupport = [
+ self.createSwingFootModel(
+ timeStep,
+ [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ ) for k in range(supportKnots)
+ ]
+ if self.firstStep is True:
+ rflhStep = self.createFootstepModels(comRef, [rfFootPos0, lhFootPos0], 0.5 * stepLength, stepHeight,
+ timeStep, stepKnots, [self.lfFootId, self.rhFootId],
+ [self.rfFootId, self.lhFootId])
+ self.firstStep = False
+ else:
+ rflhStep = self.createFootstepModels(comRef, [rfFootPos0, lhFootPos0], stepLength, stepHeight, timeStep,
+ stepKnots, [self.lfFootId, self.rhFootId],
+ [self.rfFootId, self.lhFootId])
+ lfrhStep = self.createFootstepModels(comRef, [lfFootPos0, rhFootPos0], stepLength, stepHeight, timeStep,
+ stepKnots, [self.rfFootId, self.lhFootId], [self.lfFootId, self.rhFootId])
+
+ loco3dModel += doubleSupport + rflhStep
+ loco3dModel += doubleSupport + lfrhStep
+
+ problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
+ return problem
+
+ def createPacingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
+ """ Create a shooting problem for a simple pacing gait.
+
+ :param x0: initial state
+ :param stepLength: step length
+ :param stepHeight: step height
+ :param timeStep: step time for each knot
+ :param stepKnots: number of knots for step phases
+ :param supportKnots: number of knots for double support phases
+ :return shooting problem
+ """
+ # Compute the current foot positions
+ 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
+ comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
+ comRef[2] = 0.5325
+
+ # Defining the action models along the time instances
+ loco3dModel = []
+ doubleSupport = [
+ self.createSwingFootModel(
+ timeStep,
+ [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ ) for k in range(supportKnots)
+ ]
+ if self.firstStep is True:
+ rightSteps = self.createFootstepModels(comRef, [rfFootPos0, rhFootPos0], 0.5 * stepLength, stepHeight,
+ timeStep, stepKnots, [self.lfFootId, self.lhFootId],
+ [self.rfFootId, self.rhFootId])
+ self.firstStep = False
+ else:
+ rightSteps = self.createFootstepModels(comRef, [rfFootPos0, rhFootPos0], stepLength, stepHeight, timeStep,
+ stepKnots, [self.lfFootId, self.lhFootId],
+ [self.rfFootId, self.rhFootId])
+ leftSteps = self.createFootstepModels(comRef, [lfFootPos0, lhFootPos0], stepLength, stepHeight, timeStep,
+ stepKnots, [self.rfFootId, self.rhFootId],
+ [self.lfFootId, self.lhFootId])
+
+ loco3dModel += doubleSupport + rightSteps
+ loco3dModel += doubleSupport + leftSteps
+
+ problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
+ return problem
+
+ def createBoundingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
+ """ Create a shooting problem for a simple bounding gait.
+
+ :param x0: initial state
+ :param stepLength: step length
+ :param stepHeight: step height
+ :param timeStep: step time for each knot
+ :param stepKnots: number of knots for step phases
+ :param supportKnots: number of knots for double support phases
+ :return shooting problem
+ """
+ # Compute the current foot positions
+ 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
+ comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
+ comRef[2] = 0.5325
+
+ # Defining the action models along the time instances
+ loco3dModel = []
+ doubleSupport = [
+ self.createSwingFootModel(timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId])
+ for k in range(supportKnots)
+ ]
+ hindSteps = self.createFootstepModels(comRef, [lfFootPos0, rfFootPos0], stepLength, stepHeight, timeStep,
+ stepKnots, [self.lhFootId, self.rhFootId],
+ [self.lfFootId, self.rfFootId])
+ frontSteps = self.createFootstepModels(comRef, [lhFootPos0, rhFootPos0], stepLength, stepHeight, timeStep,
+ stepKnots, [self.lfFootId, self.rfFootId],
+ [self.lhFootId, self.rhFootId])
+
+ loco3dModel += doubleSupport + hindSteps
+ loco3dModel += doubleSupport + frontSteps
+
+ problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
+ return problem
+
+ def createJumpingProblem(self, x0, jumpHeight, timeStep):
+ 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
+ comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
+ comRef[2] = 0.5325
+
+ takeOffKnots = 30
+ flyingKnots = 30
+
+ loco3dModel = []
+ takeOff = [
+ self.createSwingFootModel(
+ timeStep,
+ [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
+ ) for k in range(takeOffKnots)
+ ]
+ flyingPhase = [
+ self.createSwingFootModel(timeStep, [],
+ np.matrix([0., 0., jumpHeight * (k + 1) / flyingKnots]).T + comRef)
+ for k in range(flyingKnots)
+ ]
+
+ loco3dModel += takeOff
+ loco3dModel += flyingPhase
+
+ 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.
+
+ :param comPos0, initial CoM position
+ :param feetPos0: initial position of the swinging feet
+ :param stepLength: step length
+ :param stepHeight: step height
+ :param timeStep: time step
+ :param numKnots: number of knots for the footstep phase
+ :param supportFootIds: Ids of the supporting feet
+ :param swingFootIds: Ids of the swinging foot
+ :return footstep action models
+ """
+ numLegs = len(supportFootIds) + len(swingFootIds)
+ comPercentage = float(len(swingFootIds)) / numLegs
+
+ # Action models for the foot swing
+ footSwingModel = []
+ for k in range(numKnots):
+ swingFootTask = []
+ for i, p in zip(swingFootIds, feetPos0):
+ # Defining a foot swing task given the step length
+ # resKnot = numKnots % 2
+ phKnots = numKnots / 2
+ if k < phKnots:
+ dp = np.matrix([[stepLength * (k + 1) / numKnots, 0., stepHeight * k / phKnots]]).T
+ elif k == phKnots:
+ dp = np.matrix([[stepLength * (k + 1) / numKnots, 0., stepHeight]]).T
+ else:
+ dp = np.matrix(
+ [[stepLength * (k + 1) / numKnots, 0., stepHeight * (1 - float(k - phKnots) / phKnots)]]).T
+ tref = np.asmatrix(p + dp)
+
+ 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)
+
+ # Updating the current foot position for next step
+ comPos0 += np.matrix([stepLength * comPercentage, 0., 0.]).T
+ for p in feetPos0:
+ p += np.matrix([[stepLength, 0., 0.]]).T
+ return footSwingModel + [footSwitchModel]
+
+ def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None):
+ """ Action model for a swing foot phase.
+
+ :param timeStep: step duration of the action model
+ :param supportFootIds: Ids of the constrained feet
+ :param comTask: CoM task
+ :param swingFootTask: swinging foot task
+ :return action model for a swing foot phase
+ """
+ # 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)
+ contactModel.addContact('contact_' + str(i), supportContactModel)
+
+ # Creating the cost model for a contact phase
+ costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
+ if isinstance(comTask, np.ndarray):
+ comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu)
+ costModel.addCost("comTrack", comTrack, 1e4)
+ if swingFootTask is not None:
+ for i in swingFootTask:
+ xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
+ 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)
+ stateReg = crocoddyl.CostModelState(self.state,
+ crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
+ self.rmodel.defaultState, self.actuation.nu)
+ ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
+ costModel.addCost("stateReg", stateReg, 1e-1)
+ costModel.addCost("ctrlReg", ctrlReg, 1e-4)
+
+ # Creating the action model for the KKT dynamics with simpletic Euler
+ # integration scheme
+ dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel,
+ costModel)
+ model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
+ return model
+
+ def createFootSwitchModel(self, supportFootIds, swingFootTask):
+ """ Action model for a foot switch phase.
+
+ :param supportFootIds: Ids of the constrained feet
+ :param swingFootTask: swinging foot task
+ :return action model for a foot switch phase
+ """
+ # 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)
+ contactModel.addContact('contact_' + str(i), supportContactModel)
+
+ # Creating the cost model for a contact phase
+ costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
+ if swingFootTask is not None:
+ for i in swingFootTask:
+ xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
+ footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 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)
+ stateReg = crocoddyl.CostModelState(self.state,
+ crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
+ self.rmodel.defaultState, self.actuation.nu)
+ ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
+ 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,
+ costModel)
+ model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
+ return model
+
+
+def plotSolution(rmodel, xs, us):
+ import matplotlib.pyplot as plt
+ # Getting the state and control trajectories
+ nx, nq, nu = xs[0].shape[0], rmodel.nq, 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]
+
+ # Plotting the joint positions, velocities and torques
+ plt.figure(1)
+ legJointNames = ['HAA', 'HFE', 'KFE']
+ # LF foot
+ plt.subplot(4, 3, 1)
+ plt.title('joint position [rad]')
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(7, 10))]
+ plt.ylabel('LF')
+ plt.legend()
+ plt.subplot(4, 3, 2)
+ plt.title('joint velocity [rad/s]')
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 6, nq + 9))]
+ plt.ylabel('LF')
+ plt.legend()
+ plt.subplot(4, 3, 3)
+ plt.title('joint torque [Nm]')
+ [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(0, 3))]
+ plt.ylabel('LF')
+ plt.legend()
+
+ # LH foot
+ plt.subplot(4, 3, 4)
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(10, 13))]
+ plt.ylabel('LH')
+ plt.legend()
+ plt.subplot(4, 3, 5)
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 9, nq + 12))]
+ plt.ylabel('LH')
+ plt.legend()
+ plt.subplot(4, 3, 6)
+ [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(3, 6))]
+ plt.ylabel('LH')
+ plt.legend()
+
+ # RF foot
+ plt.subplot(4, 3, 7)
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(13, 16))]
+ plt.ylabel('RF')
+ plt.legend()
+ plt.subplot(4, 3, 8)
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 12, nq + 15))]
+ plt.ylabel('RF')
+ plt.legend()
+ plt.subplot(4, 3, 9)
+ [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(6, 9))]
+ plt.ylabel('RF')
+ plt.legend()
+
+ # RH foot
+ plt.subplot(4, 3, 10)
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(16, 19))]
+ plt.ylabel('RH')
+ plt.xlabel('knots')
+ plt.legend()
+ plt.subplot(4, 3, 11)
+ [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 15, nq + 18))]
+ plt.ylabel('RH')
+ plt.xlabel('knots')
+ plt.legend()
+ plt.subplot(4, 3, 12)
+ [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(9, 12))]
+ plt.ylabel('RH')
+ plt.legend()
+ plt.xlabel('knots')
+ plt.show()
+
+ plt.figure(2)
+ rdata = rmodel.createData()
+ Cx = []
+ Cy = []
+ for x in xs:
+ q = x[:rmodel.nq]
+ c = pinocchio.centerOfMass(rmodel, rdata, q)
+ Cx.append(np.asscalar(c[0]))
+ Cy.append(np.asscalar(c[1]))
+ plt.plot(Cx, Cy)
+ plt.title('CoM position')
+ plt.xlabel('x [m]')
+ plt.ylabel('y [m]')
+ plt.grid(True)
+ plt.show()
diff --git a/cmake b/cmake
index 40207e1a74854fa99a42304837f826f1692d4440..0fb087eb8e78db616329de0e4ab55abbe015bbe8 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 40207e1a74854fa99a42304837f826f1692d4440
+Subproject commit 0fb087eb8e78db616329de0e4ab55abbe015bbe8
diff --git a/crocoddyl/CMakeLists.txt b/crocoddyl/CMakeLists.txt
index 13f3de470befdd0d22f51eb34b858152d39c491a..50fdc260ae6a5feb5830bb66e3cab17b2d08c95f 100644
--- a/crocoddyl/CMakeLists.txt
+++ b/crocoddyl/CMakeLists.txt
@@ -26,8 +26,8 @@ SET(${PROJECT_NAME}_PYTHON_FILES
)
FOREACH(python ${${PROJECT_NAME}_PYTHON_FILES})
- PYTHON_BUILD(. ${python})
- INSTALL(FILES ${python} DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME})
+ PYTHON_BUILD(../crocoddyl ${python})
+ # INSTALL(FILES ${python} DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME}/legacy)
ENDFOREACH(python ${${PROJECT_NAME}_PYTHON_FILES})
SET(${PROJECT_NAME}_LOCOMOTION_PYTHON_FILES
@@ -40,5 +40,5 @@ SET(${PROJECT_NAME}_LOCOMOTION_PYTHON_FILES
FOREACH(python ${${PROJECT_NAME}_LOCOMOTION_PYTHON_FILES})
PYTHON_BUILD(locomotion ${python})
- INSTALL(FILES locomotion/${python} DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME}/locomotion)
-ENDFOREACH(python ${${PROJECT_NAME}_LOCOMOTION_PYTHON_FILES})
+ # INSTALL(FILES locomotion/${python} DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME}/legacy/locomotion)
+ENDFOREACH(python ${${PROJECT_NAME}_LOCOMOTION_qPYTHON_FILES})
diff --git a/crocoddyl/action.py b/crocoddyl/action.py
index 98773bf82feaf2ff3381e497497aafe5a2720b8f..ab23022d3b271ef35852d123c608bccba1106a72 100644
--- a/crocoddyl/action.py
+++ b/crocoddyl/action.py
@@ -15,7 +15,6 @@ class ActionModelAbstract:
the dynamics, cost functions and their derivatives. These computations are
mainly carry on inside calc() and calcDiff(), respectively.
"""
-
def __init__(self, State, nu):
""" Construct common variables for action models.
@@ -194,7 +193,6 @@ class ActionDataLQR(ActionDataAbstract):
class ActionModelNumDiff(ActionModelAbstract):
""" Abstract action model that uses NumDiff for derivative computation.
"""
-
def __init__(self, model, withGaussApprox=False):
ActionModelAbstract.__init__(self, model.State, model.nu)
self.ActionDataType = ActionDataNumDiff
diff --git a/crocoddyl/activation.py b/crocoddyl/activation.py
index 11bd684c9f36be96ac96f89f40d534e0840a9b06..80891b63c0970417b87d7b5cd2226c1ce8ee62b6 100644
--- a/crocoddyl/activation.py
+++ b/crocoddyl/activation.py
@@ -8,7 +8,6 @@ class ActivationModelAbstract:
the activation value and its derivatives from it. Activation value and
its derivatives are computed by calc() and calcDiff(), respectively.
"""
-
def __init__(self):
self.ActivationDataType = ActivationDataAbstract
@@ -79,7 +78,6 @@ class ActivationModelInequality(ActivationModelAbstract):
a(r) = (r**2)/2 for r<b_l.
a(r) = 0. for b_l<=r<=b_u
"""
-
def __init__(self, lowerLimit, upperLimit, beta=None):
assert ((lowerLimit <= upperLimit).all())
assert (not np.any(np.isinf(lowerLimit)) and not np.any(np.isinf(upperLimit)) or beta is None)
@@ -155,7 +153,6 @@ class ActivationModelSmoothAbs(ActivationModelAbstract):
c'' = R' [ a''(ri) ] R
= R' [ 1/a**3 ] R
'''
-
def __init__(self):
self.ActivationDataType = ActivationDataSmoothAbs
pass
diff --git a/crocoddyl/actuation.py b/crocoddyl/actuation.py
index dac17e43c6694b652c3e926aff87a55eab5e107d..54551e341be9593d94caecef3c65ff306ea27dfe 100644
--- a/crocoddyl/actuation.py
+++ b/crocoddyl/actuation.py
@@ -46,7 +46,6 @@ class ActuationModelFreeFloating:
This model transforms an actuation u into a joint torque tau.
We implement here the simplest model: tau = S.T*u, where S is constant.
'''
-
def __init__(self, pinocchioModel):
self.pinocchio = pinocchioModel
if (pinocchioModel.joints[1].shortname() != 'JointModelFreeFlyer'):
@@ -86,7 +85,6 @@ class ActuationModelFull:
This model transforms an actuation u into a joint torque tau.
We implement here the trivial model: tau = u
'''
-
def __init__(self, pinocchioModel):
self.pinocchio = pinocchioModel
self.nq = pinocchioModel.nq
diff --git a/crocoddyl/box_ddp.py b/crocoddyl/box_ddp.py
index d08623951f6967b4444b4d5148982a61b003a807..049e22b503d5aa687ba87c5f17f2b511a63adc8b 100644
--- a/crocoddyl/box_ddp.py
+++ b/crocoddyl/box_ddp.py
@@ -12,7 +12,6 @@ class SolverBoxDDP(SolverDDP):
This solver uses quadprogWrapper as the default to solve the QP.
"""
-
def solve(self,
maxiter=100,
init_xs=None,
diff --git a/crocoddyl/box_kkt.py b/crocoddyl/box_kkt.py
index 4a7e2ed481d56c542dd63de1045b3be81a475bb5..1b8a4908cb53a2cc8e034819677b05ebffa19fda 100644
--- a/crocoddyl/box_kkt.py
+++ b/crocoddyl/box_kkt.py
@@ -10,7 +10,6 @@ class SolverBoxKKT(SolverKKT):
This solver uses quadprogWrapper as the default to solve the QP.
"""
-
def solve(self, maxiter=100, init_xs=None, init_us=None, isFeasible=False, qpsolver=None, ul=None, uu=None):
self.qpsolver = qpsolver if qpsolver is not None else quadprogWrapper
self.ul = ul
diff --git a/crocoddyl/cost.py b/crocoddyl/cost.py
index 3ebf78f14466f42f9a5327d4067ab4a86aaf78f6..223992044fd5a60ee2653d9aff722452058805e3 100644
--- a/crocoddyl/cost.py
+++ b/crocoddyl/cost.py
@@ -15,7 +15,6 @@ class CostModelPinocchio:
can be retrieved from Pinocchio data, through the calc and calcDiff
functions, respectively.
"""
-
def __init__(self, pinocchioModel, ncost, withResiduals=True, nu=None):
self.ncost = ncost
self.nq = pinocchioModel.nq
@@ -41,7 +40,6 @@ class CostDataPinocchio:
It stores the data corresponting to the CostModelPinocchio class.
"""
-
def __init__(self, model, pinocchioData):
ncost, nv, ndx, nu = model.ncost, model.nv, model.ndx, model.nu
self.pinocchio = pinocchioData
@@ -72,7 +70,6 @@ class CostDataPinocchio:
class CostModelNumDiff(CostModelPinocchio):
""" Abstract cost model that uses NumDiff for derivative computation.
"""
-
def __init__(self, costModel, State, withGaussApprox=False, reevals=[]):
'''
reevals is a list of lambdas of (pinocchiomodel,pinocchiodata,x,u) to be
@@ -225,7 +222,6 @@ class CostModelFrameTranslation(CostModelPinocchio):
a frame of the robot. Parametrize it with the frame index frameIdx and
the effector desired position ref.
"""
-
def __init__(self, pinocchioModel, frame, ref, nu=None, activation=None):
self.CostDataType = CostDataFrameTranslation
CostModelPinocchio.__init__(self, pinocchioModel, ncost=3, nu=nu)
@@ -273,7 +269,6 @@ class CostModelFrameVelocity(CostModelPinocchio):
end-effector. It assumes that updateFramePlacement and computeForwardKinematicsDerivatives
have been runned.
"""
-
def __init__(self, pinocchioModel, frame, ref=None, nu=None, activation=None):
self.CostDataType = CostDataFrameVelocity
CostModelPinocchio.__init__(self, pinocchioModel, ncost=6)
@@ -322,7 +317,6 @@ class CostModelFrameVelocityLinear(CostModelPinocchio):
end-effector. It assumes that updateFramePlacement and computeForwardKinematicsDerivatives
have been runned.
"""
-
def __init__(self, pinocchioModel, frame, ref=None, nu=None, activation=None):
self.CostDataType = CostDataFrameVelocityLinear
CostModelPinocchio.__init__(self, pinocchioModel, ncost=3)
@@ -371,7 +365,6 @@ class CostModelFramePlacement(CostModelPinocchio):
for a frame of the robot. Parametrize it with the frame index frameIdx and
the effector desired pinocchio::SE3 ref.
"""
-
def __init__(self, pinocchioModel, frame, ref, nu=None, activation=None):
self.CostDataType = CostDataFramePlacement
CostModelPinocchio.__init__(self, pinocchioModel, ncost=6, nu=nu)
@@ -420,7 +413,6 @@ class CostModelFrameRotation(CostModelPinocchio):
The class proposes a model of a cost function orientation (3d) for a frame of the robot.
Parametrize it with the frame index frameIdx and the effector desired rotation matrix.
"""
-
def __init__(self, pinocchioModel, frame, ref, nu=None, activation=None):
self.CostDataType = CostDataFrameRotation
CostModelPinocchio.__init__(self, pinocchioModel, ncost=3, nu=nu)
@@ -470,7 +462,6 @@ class CostModelCoM(CostModelPinocchio):
The class proposes a model of a cost function CoM. It is parametrized with the
desired CoM position.
"""
-
def __init__(self, pinocchioModel, ref, nu=None, activation=None):
self.CostDataType = CostDataCoM
CostModelPinocchio.__init__(self, pinocchioModel, ncost=3, nu=nu)
@@ -513,7 +504,6 @@ class CostModelState(CostModelPinocchio):
It tracks a reference state vector. Generally speaking, the state error lie in the
tangent-space of the state manifold (or more precisely the configuration manifold).
"""
-
def __init__(self, pinocchioModel, State, ref=None, nu=None, activation=None):
self.CostDataType = CostDataState
CostModelPinocchio.__init__(self, pinocchioModel, ncost=State.ndx, nu=nu)
@@ -550,7 +540,6 @@ class CostModelControl(CostModelPinocchio):
It tracks a reference control vector.
"""
-
def __init__(self, pinocchioModel, nu=None, ref=None, activation=None):
self.CostDataType = CostDataControl
nu = nu if nu is not None else pinocchioModel.nv
@@ -593,7 +582,6 @@ class CostModelForce(CostModelPinocchio):
The class proposes a model of a cost function for tracking a reference
value of a 6D force, being given the contact model and its derivatives.
"""
-
def __init__(self, pinocchioModel, contactModel, ncost=6, ref=None, nu=None, activation=None):
self.CostDataType = CostDataForce
CostModelPinocchio.__init__(self, pinocchioModel, ncost=ncost, nu=nu)
@@ -641,7 +629,6 @@ class CostModelForceLinearCone(CostModelPinocchio):
The class proposes a model which implements Af-ref<=0 for the linear conic cost. (The inequality
is implemented by the activation function. By default ref is zero (Af<=0).
"""
-
def __init__(self, pinocchioModel, contactModel, A, ref=None, nu=None, activation=None):
self.CostDataType = CostDataForce
CostModelPinocchio.__init__(self, pinocchioModel, ncost=A.shape[0], nu=nu)
diff --git a/crocoddyl/ddp.py b/crocoddyl/ddp.py
index c3772ecc40a779681ad02c07f95015971b965fac..7ede5e9e6dc435b80408a8365d42838c8316d060 100644
--- a/crocoddyl/ddp.py
+++ b/crocoddyl/ddp.py
@@ -19,7 +19,6 @@ class SolverDDP(SolverAbstract):
along a tuple of optimized control commands U*.
:param shootingProblem: shooting problem (list of action models along trajectory)
"""
-
def __init__(self, shootingProblem):
SolverAbstract.__init__(self, shootingProblem)
diff --git a/crocoddyl/differential_action.py b/crocoddyl/differential_action.py
index 8238c7dbe447bad77bce8f3be77da85d022014b5..fa83b485ef158c98694f588b9f83bde47aa02bad 100644
--- a/crocoddyl/differential_action.py
+++ b/crocoddyl/differential_action.py
@@ -14,7 +14,6 @@ class DifferentialActionModelAbstract:
the dynamics, cost functions and their derivatives. These computations are
mainly carry on inside calc() and calcDiff(), respectively.
"""
-
def __init__(self, nq, nv, nu):
self.nq = nq
self.nv = nv
@@ -167,7 +166,7 @@ class DifferentialActionModelFullyActuated(DifferentialActionModelAbstract):
pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q, v, tauq)
data.Fx[:, :nv] = data.pinocchio.ddq_dq
data.Fx[:, nv:] = data.pinocchio.ddq_dv
- data.Fu[:, :] = data.Minv
+ data.Fu[:, :] = data.pinocchio.Minv
else:
pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q, v, a)
data.Fx[:, :nv] = -np.dot(data.Minv, data.pinocchio.dtau_dq)
@@ -200,7 +199,6 @@ class DifferentialActionModelLQR(DifferentialActionModelAbstract):
is given by
l(x,u) = 1/2 [x,u].T [Lxx Lxu; Lxu.T Luu] [x,u] + [lx,lu].T [x,u]
"""
-
def __init__(self, nq, nu, driftFree=True):
DifferentialActionModelAbstract.__init__(self, nq, nq, nu)
self.DifferentialActionDataType = DifferentialActionDataLQR
diff --git a/crocoddyl/fddp.py b/crocoddyl/fddp.py
index f51329efc4bbef9683ad047b18fffae7097b037d..b209133a66e7b2ea4fed7f6e052ca7b97039e354 100644
--- a/crocoddyl/fddp.py
+++ b/crocoddyl/fddp.py
@@ -24,7 +24,6 @@ class SolverFDDP(SolverAbstract):
searching for a good optimization.
:param shootingProblem: shooting problem (list of action models along trajectory)
"""
-
def __init__(self, shootingProblem):
SolverAbstract.__init__(self, shootingProblem)
diff --git a/crocoddyl/floating_contact.py b/crocoddyl/floating_contact.py
index 692eba26b65169ffbf31fd7e21d10b10caa0f718..7cc2f9aa3ea36db18770a67b760c98ac59e20b2e 100644
--- a/crocoddyl/floating_contact.py
+++ b/crocoddyl/floating_contact.py
@@ -67,7 +67,6 @@ class DifferentialActionModelFloatingInContact(DifferentialActionModelAbstract):
fs = data.contact.forces
pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q, v, a, fs)
- pinocchio.computeForwardKinematicsDerivatives(self.pinocchio, data.pinocchio, q, v, a)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
# [a;-f] = K^-1 [ tau - b, -gamma ]
diff --git a/crocoddyl/impact.py b/crocoddyl/impact.py
index 15415a9111a7801089a86108d1af4e228fb20566..c20a085bb5e86ba07c66b55d921a49f981c8b1ca 100644
--- a/crocoddyl/impact.py
+++ b/crocoddyl/impact.py
@@ -247,7 +247,6 @@ class CostModelImpactWholeBody(CostModelImpactBase):
Penalize the impact on the whole body, i.e. the sum-of-square of ||vnext-v||
with vnext the velocity after impact and v the velocity before impact.
'''
-
def __init__(self, pinocchioModel, activation=None):
self.CostDataType = CostDataImpactWholeBody
CostModelImpactBase.__init__(self, pinocchioModel, ncost=pinocchioModel.nv)
@@ -288,7 +287,6 @@ class CostModelImpactCoM(CostModelImpactBase):
Penalize the impact on the com, i.e. the sum-of-square of ||Jcom*(vnext-v)||
with vnext the velocity after impact and v the velocity before impact.
'''
-
def __init__(self, pinocchioModel, activation=None):
self.CostDataType = CostDataImpactCoM
CostModelImpactBase.__init__(self, pinocchioModel, ncost=3)
diff --git a/crocoddyl/integrated_action.py b/crocoddyl/integrated_action.py
index d6329940fa0f489c763339d2ed550c1d3468c877..d0545938c377403858a9d6d263a3b3be3674cb88 100644
--- a/crocoddyl/integrated_action.py
+++ b/crocoddyl/integrated_action.py
@@ -59,7 +59,6 @@ class IntegratedActionDataEuler:
the calcDiff function needs to be taken into account when considering
this integration scheme.
"""
-
def __init__(self, model):
nx, ndx, nu = model.nx, model.ndx, model.nu
self.differential = model.differential.createData()
@@ -119,7 +118,6 @@ class IntegratedActionModelRK4:
data.dx = (dt/6)(k0+2k1+2k2+k3)
'''
-
def calc(self, data, x, u=None):
nq, dt = self.nq, self.timeStep
@@ -176,7 +174,6 @@ class IntegratedActionModelRK4:
dy2_du = dintegrate_right*dt/2*dk1_du
dy3_du = dintegrate_right*dt*dk2_du
'''
-
def calcDiff(self, data, x, u=None, recalc=True):
ndx, nu, nv, dt = self.ndx, self.nu, self.nv, self.timeStep
if recalc:
diff --git a/crocoddyl/locomotion/contact_sequence_wrapper.py b/crocoddyl/locomotion/contact_sequence_wrapper.py
index 3aec3c05a33a0ce121c021656b3ca1e92dd2d43b..43043284c912bb4fe21367b8c633d92496ab8ea7 100644
--- a/crocoddyl/locomotion/contact_sequence_wrapper.py
+++ b/crocoddyl/locomotion/contact_sequence_wrapper.py
@@ -15,7 +15,6 @@ class ContactSequenceWrapper:
2) end-effector trajectories (swing trajectories)
3) easier accessors for centroidal trajectories and stored in centroidalphi class.
"""
-
def __init__(self, cs, ee_map, ee_splines=None):
self.cs = cs # contact sequence file from multicontact-api
self.ee_map = ee_map # map of contact patches
diff --git a/crocoddyl/qpsolvers.py b/crocoddyl/qpsolvers.py
index 02ba210bef482fcbf91bdc635d2ca2b3e9677b1f..7be4aee5d0bf8d3b26ec292aed67a74538448341 100644
--- a/crocoddyl/qpsolvers.py
+++ b/crocoddyl/qpsolvers.py
@@ -28,7 +28,6 @@ class QPSolution:
dual: value of the lagrangian multipliers.
niter: number of iterations needed to solve the problem.
"""
-
def __init__(self, nx, nc):
self.argmin = zeros(nx)
self.optimum = 0.
diff --git a/crocoddyl/solver.py b/crocoddyl/solver.py
index 4a466380f55f75663f089ac721a576f81e4783d3..dc678808cab9ced592a147d3059425c70dfd6597 100644
--- a/crocoddyl/solver.py
+++ b/crocoddyl/solver.py
@@ -11,7 +11,6 @@ class SolverAbstract:
function is used to define when the search direction and length are computed in each iterate. It also describes
the globalization strategy (i.e. regularization) of the numerical optimization.
"""
-
def __init__(self, problem):
# Setting up the problem and allocating the required solver data
self.problem = problem
diff --git a/crocoddyl/state.py b/crocoddyl/state.py
index 93d9001e79d3d8ca52669b402f1653f9a6fec9c4..becf34c415de0c9bc321b16e1aca1f79bbf8cc6f 100644
--- a/crocoddyl/state.py
+++ b/crocoddyl/state.py
@@ -16,7 +16,6 @@ class StateAbstract:
the points x, x1 and x2 belongs to the manifold M; and dx or x1 [-] x2 lie
on its tangential space.
"""
-
def __init__(self, nx, ndx):
# Setting up the dimension of the state vector and its tangent vector
self.nx = nx
@@ -97,7 +96,6 @@ class StateVector(StateAbstract):
point and its velocity lie in the same space, all Jacobians are described
throught the identity matrix.
"""
-
def __init__(self, nx):
# Euclidean point and its velocity lie in the same space dimension.
StateAbstract.__init__(self, nx, nx)
@@ -173,7 +171,6 @@ class StateNumDiff(StateAbstract):
computation of a custom State. For doing so, we need to construct this class
by passing as an argument our State object.
"""
-
def __init__(self, State):
StateAbstract.__init__(self, State.nx, State.ndx)
self.State = State
@@ -275,7 +272,6 @@ class StatePinocchio(StateAbstract):
Jacobians for the differentiate and integrate operators. Note that this code
can be reused in any robot that is described through its Pinocchio model.
"""
-
def __init__(self, pinocchioModel):
StateAbstract.__init__(self, pinocchioModel.nq + pinocchioModel.nv, 2 * pinocchioModel.nv)
self.model = pinocchioModel
diff --git a/examples/bipedal_walking_from_foot_traj.py b/examples/bipedal_walking_from_foot_traj.py
index 122866e3d8166a1cb34272cadc954041edde13bf..5dc018defba3ed3e7293b8caa172752d39921c54 100644
--- a/examples/bipedal_walking_from_foot_traj.py
+++ b/examples/bipedal_walking_from_foot_traj.py
@@ -1,291 +1,27 @@
import sys
import numpy as np
+
+import crocoddyl
+import example_robot_data
import pinocchio
-from crocoddyl import (ActivationModelWeightedQuad, ActuationModelFreeFloating, CallbackDDPLogger, CallbackDDPVerbose,
- CallbackSolverDisplay, ContactModel6D, ContactModelMultiple, CostModelCoM, CostModelControl,
- CostModelFramePlacement, CostModelFrameVelocity, CostModelState, CostModelSum,
- DifferentialActionModelFloatingInContact, IntegratedActionModelEuler, ShootingProblem,
- SolverDDP, StatePinocchio, a2m, displayTrajectory, loadTalosLegs, m2a)
-from pinocchio.utils import zero
+from crocoddyl.utils.biped import SimpleBipedGaitProblem, plotSolution
-WITHDISPLAY = 'disp' in sys.argv
+WITHDISPLAY = 'display' in sys.argv
WITHPLOT = 'plot' in sys.argv
-
-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]
- X = [0.] * nx
- U = [0.] * nu
- for i in range(nx):
- X[i] = [x[i] for x in xs]
- for i in range(nu):
- U[i] = [u[i] for u in us]
-
- # Plotting the joint positions, velocities and torques
- plt.figure(figIndex)
- legJointNames = ['1', '2', '3', '4', '5', '6']
- # left foot
- plt.subplot(2, 3, 1)
- plt.title('joint position [rad]')
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(7, 13))]
- plt.ylabel('LF')
- plt.legend()
- plt.subplot(2, 3, 2)
- plt.title('joint velocity [rad/s]')
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 6, nq + 12))]
- plt.ylabel('LF')
- plt.legend()
- plt.subplot(2, 3, 3)
- plt.title('joint torque [Nm]')
- [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(0, 6))]
- plt.ylabel('LF')
- plt.legend()
-
- # right foot
- plt.subplot(2, 3, 4)
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(13, 19))]
- plt.ylabel('RF')
- plt.xlabel('knots')
- plt.legend()
- plt.subplot(2, 3, 5)
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 12, nq + 18))]
- plt.ylabel('RF')
- plt.xlabel('knots')
- plt.legend()
- plt.subplot(2, 3, 6)
- [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(6, 12))]
- plt.ylabel('RF')
- plt.xlabel('knots')
- plt.legend()
-
- plt.figure(figIndex + 1)
- rdata = rmodel.createData()
- Cx = []
- Cy = []
- for x in xs:
- q = a2m(x[:rmodel.nq])
- c = pinocchio.centerOfMass(rmodel, rdata, q)
- Cx.append(np.asscalar(c[0]))
- Cy.append(np.asscalar(c[1]))
- plt.plot(Cx, Cy)
- plt.title('CoM position')
- plt.xlabel('x [m]')
- plt.ylabel('y [m]')
- plt.grid(True)
- if show:
- plt.show()
-
-
-class TaskSE3:
- def __init__(self, oXf, frameId):
- self.oXf = oXf
- self.frameId = frameId
-
-
-class SimpleBipedGaitProblem:
- """ Defines a simple 3d locomotion problem
- """
-
- def __init__(self, rmodel, rightFoot, leftFoot):
- self.rmodel = rmodel
- self.rdata = rmodel.createData()
- self.state = StatePinocchio(self.rmodel)
- # Getting the frame id for all the legs
- self.rfId = self.rmodel.getFrameId(rightFoot)
- self.lfId = self.rmodel.getFrameId(leftFoot)
- # Defining default state
- q0 = self.rmodel.referenceConfigurations["half_sitting"]
- self.rmodel.defaultState = \
- np.concatenate([m2a(q0), np.zeros(self.rmodel.nv)])
- self.firstStep = True
- # Remove the armature
- self.rmodel.armature[6:] = 1.
-
- def createWalkingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
- """ Create a shooting problem for a simple walking gait.
-
- :param x0: initial state
- :param stepLength: step length
- :param stepHeight: step height
- :param timeStep: step time for each knot
- :param stepKnots: number of knots for step phases
- :param supportKnots: number of knots for double support phases
- :return shooting problem
- """
- # Compute the current foot positions
- q0 = a2m(x0[:self.rmodel.nq])
- pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
- pinocchio.updateFramePlacements(self.rmodel, self.rdata)
- rfPos0 = self.rdata.oMf[self.rfId].translation
- lfPos0 = self.rdata.oMf[self.lfId].translation
- comRef = m2a(rfPos0 + lfPos0) / 2
- comRef[2] = 0.6185
-
- # Defining the action models along the time instances
- loco3dModel = []
- doubleSupport = [self.createSwingFootModel(timeStep, [self.rfId, self.lfId]) for k in range(supportKnots)]
-
- # Creating the action models for three steps
- if self.firstStep is True:
- rStep = self.createFootstepModels(comRef, [rfPos0], 0.5 * stepLength, stepHeight, timeStep, stepKnots,
- [self.lfId], [self.rfId])
- self.firstStep = False
- else:
- rStep = self.createFootstepModels(comRef, [rfPos0], stepLength, stepHeight, timeStep, stepKnots,
- [self.lfId], [self.rfId])
- lStep = self.createFootstepModels(comRef, [lfPos0], stepLength, stepHeight, timeStep, stepKnots, [self.rfId],
- [self.lfId])
-
- # We defined the problem as:
- loco3dModel += doubleSupport + rStep
- loco3dModel += doubleSupport + lStep
-
- problem = ShootingProblem(x0, loco3dModel, loco3dModel[-1])
- return problem
-
- def createFootstepModels(self, comPos0, feetPos0, stepLength, stepHeight, timeStep, numKnots, supportFootIds,
- swingFootIds):
- """ Action models for a footstep phase.
-
- :param comPos0, initial CoM position
- :param feetPos0: initial position of the swinging feet
- :param stepLength: step length
- :param stepHeight: step height
- :param timeStep: time step
- :param numKnots: number of knots for the footstep phase
- :param supportFootIds: Ids of the supporting feet
- :param swingFootIds: Ids of the swinging foot
- :return footstep action models
- """
- numLegs = len(supportFootIds) + len(swingFootIds)
- comPercentage = float(len(swingFootIds)) / numLegs
-
- # Action models for the foot swing
- footSwingModel = []
- for k in range(numKnots):
- swingFootTask = []
- for i, p in zip(swingFootIds, feetPos0):
- # Defining a foot swing task given the step length. The swing task
- # is decomposed on two phases: swing-up and swing-down. We decide
- # deliveratively to allocated the same number of nodes (i.e. phKnots)
- # in each phase. With this, we define a proper z-component for the
- # swing-leg motion.
- phKnots = numKnots / 2
- if k < phKnots:
- dp = a2m([[stepLength * (k + 1) / numKnots, 0., stepHeight * k / phKnots]])
- elif k == phKnots:
- dp = a2m([[stepLength * (k + 1) / numKnots, 0., stepHeight]])
- else:
- dp = a2m([[stepLength * (k + 1) / numKnots, 0., stepHeight * (1 - float(k - phKnots) / phKnots)]])
- tref = np.asmatrix(p + dp)
-
- swingFootTask += [TaskSE3(pinocchio.SE3(np.eye(3), tref), i)]
-
- comTask = np.array([stepLength * (k + 1) / numKnots, 0., 0.]) * comPercentage + comPos0
- footSwingModel += [
- self.createSwingFootModel(timeStep, supportFootIds, comTask=comTask, swingFootTask=swingFootTask)
- ]
-
- # Action model for the foot switch
- footSwitchModel = \
- self.createFootSwitchModel(supportFootIds, swingFootTask)
-
- # Updating the current foot position for next step
- comPos0 += np.array([stepLength * comPercentage, 0., 0.])
- for p in feetPos0:
- p += a2m([[stepLength, 0., 0.]])
- return footSwingModel + [footSwitchModel]
-
- def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None):
- """ Action model for a swing foot phase.
-
- :param timeStep: step duration of the action model
- :param supportFootIds: Ids of the constrained feet
- :param comTask: CoM task
- :param swingFootTask: swinging foot task
- :return action model for a swing foot phase
- """
- # Creating the action model for floating-base systems. A walker system
- # is by default a floating-base system
- actModel = ActuationModelFreeFloating(self.rmodel)
-
- # Creating a 6D multi-contact model, and then including the supporting
- # foot
- contactModel = ContactModelMultiple(self.rmodel)
- for i in supportFootIds:
- supportContactModel = \
- ContactModel6D(self.rmodel, i, ref=pinocchio.SE3.Identity(),
- gains=[0., 0.])
- contactModel.addContact('contact_' + str(i), supportContactModel)
-
- # Creating the cost model for a contact phase
- costModel = CostModelSum(self.rmodel, actModel.nu)
- if isinstance(comTask, np.ndarray):
- comTrack = CostModelCoM(self.rmodel, comTask, actModel.nu)
- costModel.addCost("comTrack", comTrack, 1e4)
- if swingFootTask is not None:
- for i in swingFootTask:
- footTrack = \
- CostModelFramePlacement(self.rmodel,
- i.frameId,
- i.oXf,
- actModel.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)
- stateReg = CostModelState(self.rmodel,
- self.state,
- self.rmodel.defaultState,
- actModel.nu,
- activation=ActivationModelWeightedQuad(stateWeights**2))
- ctrlReg = CostModelControl(self.rmodel, actModel.nu)
- costModel.addCost("stateReg", stateReg, 1e-1)
- costModel.addCost("ctrlReg", ctrlReg, 1e-3)
-
- # Creating the action model for the KKT dynamics with simpletic Euler
- # integration scheme
- dmodel = DifferentialActionModelFloatingInContact(self.rmodel, actModel, contactModel, costModel)
- model = IntegratedActionModelEuler(dmodel)
- model.timeStep = timeStep
- return model
-
- def createFootSwitchModel(self, supportFootId, swingFootTask):
- """ Action model for a foot switch phase.
-
- :param supportFootIds: Ids of the constrained feet
- :param swingFootTask: swinging foot task
- :return action model for a foot switch phase
- """
- model = self.createSwingFootModel(0., supportFootId, swingFootTask=swingFootTask)
-
- for i in swingFootTask:
- impactFootVelCost = \
- CostModelFrameVelocity(self.rmodel, i.frameId)
- model.differential.costs.addCost('impactVel_' + str(i), impactFootVelCost, 1e4)
- model.differential.costs['impactVel_' + str(i)].weight = 1e6
- model.differential.costs['footTrack_' + str(i)].weight = 1e8
- model.differential.costs['stateReg'].weight = 1e1
- model.differential.costs['ctrlReg'].weight = 1e-3
- return model
-
-
# Creating the lower-body part of Talos
-talos_legs = loadTalosLegs()
-rmodel = talos_legs.model
-rdata = rmodel.createData()
+talos_legs = example_robot_data.loadTalosLegs()
# Defining the initial state of the robot
-q0 = rmodel.referenceConfigurations['half_sitting'].copy()
-v0 = zero(rmodel.nv)
-x0 = m2a(np.concatenate([q0, v0]))
+q0 = talos_legs.model.referenceConfigurations['half_sitting'].copy()
+v0 = pinocchio.utils.zero(talos_legs.model.nv)
+x0 = np.concatenate([q0, v0])
# Setting up the 3d walking problem
rightFoot = 'right_sole_link'
leftFoot = 'left_sole_link'
-gait = SimpleBipedGaitProblem(rmodel, rightFoot, leftFoot)
+gait = SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot)
# Setting up all tasks
GAITPHASES = \
@@ -304,25 +40,22 @@ for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
- ddp[i] = SolverDDP(
+ ddp[i] = crocoddyl.SolverDDP(
gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
- ddp[i].callback = [CallbackDDPLogger(), CallbackDDPVerbose()]
if WITHDISPLAY:
- ddp[i].callback.append(CallbackSolverDisplay(talos_legs, 4, 1, cameraTF))
+ ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(talos_legs, 4, 4, cameraTF)])
+ else:
+ ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the DDP solver
ddp[i].th_stop = 1e-9
- ddp[i].solve(maxiter=1000,
- regInit=.1,
- init_xs=[rmodel.defaultState] * len(ddp[i].models()),
- init_us=[
- m.differential.quasiStatic(d.differential, rmodel.defaultState)
- for m, d in zip(ddp[i].models(), ddp[i].datas())[:-1]
- ])
+ xs = [talos_legs.model.defaultState] * len(ddp[i].models())
+ us = [m.quasicStatic(d, talos_legs.model.defaultState) for m, d in list(zip(ddp[i].models(), ddp[i].datas()))[:-1]]
+ ddp[i].solve(xs, us, 1000, False, 0.1)
# Defining the final state as initial one for the next phase
x0 = ddp[i].xs[-1]
@@ -330,7 +63,7 @@ for i, phase in enumerate(GAITPHASES):
# Display the entire motion
if WITHDISPLAY:
for i, phase in enumerate(GAITPHASES):
- displayTrajectory(talos_legs, ddp[i].xs, ddp[i].models()[0].timeStep)
+ crocoddyl.displayTrajectory(talos_legs, ddp[i].xs, ddp[i].models()[0].dt)
# Plotting the entire motion
if WITHPLOT:
@@ -339,4 +72,4 @@ if WITHPLOT:
for i, phase in enumerate(GAITPHASES):
xs.extend(ddp[i].xs)
us.extend(ddp[i].us)
- plotSolution(rmodel, xs, us)
+ plotSolution(talos_legs.model, xs, us)
diff --git a/examples/jump.py b/examples/jump.py
index 8135af0eb2ff2e00a1838ddaf37e69bd220b41f8..c52dc6cf17e359ece1ab2afe8fb273c4647e0ce2 100644
--- a/examples/jump.py
+++ b/examples/jump.py
@@ -195,10 +195,11 @@ ddp = SolverDDP(problem)
ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()] # CallbackSolverDisplay(robot,rate=5) ]
ddp.th_stop = 1e-4
us0 = [
- m.differential.quasiStatic(d.differential, rmodel.defaultState) for m, d in zip(ddp.models(), ddp.datas())[:imp]
+ m.differential.quasiStatic(d.differential, rmodel.defaultState)
+ for m, d in list(zip(ddp.models(), ddp.datas()))[:imp]
] + [np.zeros(0)] + [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
- for m, d in zip(ddp.models(), ddp.datas())[imp + 1:-1]
+ for m, d in list(zip(ddp.models(), ddp.datas()))[imp + 1:-1]
]
print("*** SOLVE %s ***" % PHASE_NAME)
@@ -226,7 +227,7 @@ ddp.callback = [CallbackDDPLogger(), CallbackDDPVerbose()] # CallbackSolverDisp
us = usddp + [
np.zeros(0) if isinstance(m, ActionModelImpact) else m.differential.quasiStatic(
- d.differential, rmodel.defaultState) for m, d in zip(ddp.models(), ddp.datas())[len(usddp):-1]
+ d.differential, rmodel.defaultState) for m, d in list(zip(ddp.models(), ddp.datas()))[len(usddp):-1]
]
ddp.setCandidate(xs=xsddp + [rmodel.defaultState] * (len(models) - len(xsddp)), us=us)
ddp.th_stop = 5e-4
diff --git a/examples/notebooks/arm_example.py b/examples/notebooks/arm_example.py
index 0aef162c34e668f16545d5bdf8cf7fd28f43c498..9d136b648864a3b97fb1f6e924a9dcfb12dac8d9 100644
--- a/examples/notebooks/arm_example.py
+++ b/examples/notebooks/arm_example.py
@@ -1,58 +1,64 @@
-import numpy as np
-
+import crocoddyl
import pinocchio
-from crocoddyl import *
+import numpy as np
+import example_robot_data
-robot = loadTalosArm()
-robot.initDisplay(loadModel=True)
+robot = example_robot_data.loadTalosArm()
+robot_model = robot.model
-robot.q0.flat[:] = [2, 1.5, -2, 0, 0, 0, 0]
-robot.model.armature[:] = .2
-frameId = robot.model.getFrameId('gripper_left_joint')
-DT = 1e-2
+DT = 1e-3
T = 25
+target = np.array([0.4, 0., .4])
-target = np.array([0.4, 0, .4])
-
-robot.viewer.gui.addSphere('world/point', .1, [1, 0, 0, 1]) # radius = .1, RGBA=1001
-robot.viewer.gui.applyConfiguration('world/point', target.tolist() + [0, 0, 0, 1]) # xyz+quaternion
+robot.initViewer(loadModel=True)
+robot.viewer.gui.addSphere('world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001
+robot.viewer.gui.applyConfiguration('world/point', target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion
robot.viewer.gui.refresh()
# Create the cost functions
-costTrack = CostModelFrameTranslation(robot.model, frame=frameId, ref=target)
-costXReg = CostModelState(robot.model, StatePinocchio(robot.model))
-costUReg = CostModelControl(robot.model)
+Mref = crocoddyl.FrameTranslation(robot_model.getFrameId("gripper_left_joint"), np.matrix(target).T)
+state = crocoddyl.StateMultibody(robot.model)
+goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref)
+xRegCost = crocoddyl.CostModelState(state)
+uRegCost = crocoddyl.CostModelControl(state)
# Create cost model per each action model
-runningCostModel = CostModelSum(robot.model)
-terminalCostModel = CostModelSum(robot.model)
+runningCostModel = crocoddyl.CostModelSum(state)
+terminalCostModel = crocoddyl.CostModelSum(state)
# Then let's added the running and terminal cost functions
-runningCostModel.addCost(name="pos", weight=1, cost=costTrack)
-runningCostModel.addCost(name="xreg", weight=1e-4, cost=costXReg)
-runningCostModel.addCost(name="ureg", weight=1e-7, cost=costUReg)
-terminalCostModel.addCost(name="pos", weight=1000, cost=costTrack)
-terminalCostModel.addCost(name="xreg", weight=1e-4, cost=costXReg)
-terminalCostModel.addCost(name="ureg", weight=1e-7, cost=costUReg)
+runningCostModel.addCost("gripperPose", goalTrackingCost, 1.)
+runningCostModel.addCost("stateReg", xRegCost, 1e-4)
+runningCostModel.addCost("ctrlReg", uRegCost, 1e-7)
+terminalCostModel.addCost("gripperPose", goalTrackingCost, 1000.)
+terminalCostModel.addCost("stateReg", xRegCost, 1e-4)
+terminalCostModel.addCost("ctrlReg", uRegCost, 1e-7)
# Create the action model
-runningModel = DifferentialActionModelFullyActuated(robot.model, runningCostModel)
-terminalModel = DifferentialActionModelFullyActuated(robot.model, terminalCostModel)
+runningModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel), DT)
+terminalModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel))
+#runningModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T
+#terminalModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T
# Create the problem
-x0 = np.concatenate([m2a(robot.q0), np.zeros(robot.model.nv)])
-problem = ShootingProblem(x0, [IntegratedActionModelEuler(runningModel)] * T,
- IntegratedActionModelEuler(terminalModel))
+q0 = np.matrix([2., 1.5, -2., 0., 0., 0., 0.]).T
+x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
+problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
-ddp = SolverDDP(problem)
-ddp.callback = [CallbackDDPVerbose()]
+ddp = crocoddyl.SolverDDP(problem)
+ddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
ddp.solve()
# Visualizing the solution in gepetto-viewer
-for x in ddp.xs:
- robot.display(a2m(x))
+crocoddyl.displayTrajectory(robot, ddp.xs, runningModel.dt)
-print('Finally reached = ', ddp.datas()[T].differential.costs['pos'].pinocchio.oMf[frameId].translation.T)
+robot_data = robot_model.createData()
+xT = ddp.xs[-1]
+pinocchio.forwardKinematics(robot_model, robot_data, xT[:state.nq])
+pinocchio.updateFramePlacements(robot_model, robot_data)
+print('Finally reached = ', robot_data.oMf[robot_model.getFrameId("gripper_left_joint")].translation.T)
diff --git a/examples/notebooks/cartpole_swing_up.ipynb b/examples/notebooks/cartpole_swing_up.ipynb
index bc20e7d1b6067339146853a74e4e68f8444df70b..a18845ffa8fc4f03faf8b1a37500f96111991354 100644
--- a/examples/notebooks/cartpole_swing_up.ipynb
+++ b/examples/notebooks/cartpole_swing_up.ipynb
@@ -48,39 +48,30 @@
"metadata": {},
"outputs": [],
"source": [
- "from crocoddyl import *\n",
+ "import crocoddyl\n",
+ "import pinocchio\n",
"import numpy as np\n",
"\n",
- "class DifferentialActionModelCartpole:\n",
+ "class DifferentialActionModelCartpole(crocoddyl.DifferentialActionModelAbstract):\n",
" def __init__(self):\n",
- " self.State = StateVector(4)\n",
- " self.nq,self.nv = 2,2\n",
- " self.nx = 4\n",
- " self.ndx = 4\n",
- " self.nout = 2\n",
- " self.nu = 1\n",
- " self.ncost = 6\n",
+ " crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(4), 1, 6) # nu = 1; nr = 6\n",
" self.unone = np.zeros(self.nu)\n",
"\n",
" self.m1 = 1.\n",
" self.m2 = .1\n",
" self.l = .5\n",
" self.g = 9.81\n",
- " self.costWeights = [ 1., 1., 0.1, 0.001, .001, 1. ] # sin,1-cos, x,xdot,thdot,f\n",
+ " self.costWeights = [ 1., 1., 0.1, 0.001, 0.001, 1. ] # sin, 1-cos, x, xdot, thdot, f\n",
" \n",
- " def createData(self):\n",
- " return DifferentialActionDataCartpole(self)\n",
- "\n",
- " def calc(model,data,x,u=None):\n",
+ " def calc(self, data, x, u=None):\n",
" if u is None: u=model.unone\n",
" # Getting the state and control variables\n",
- " p,th,xdot,thdot = x\n",
- " f, = u\n",
+ " x, th, xdot, 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",
- " m1,m2,l,g = model.m1,model.m2,model.l,model.g\n",
- " s,c = np.sin(th),np.cos(th)\n",
- " m = m1+m2\n",
+ " m1, m2, l, g = self.m1, self.m2, self.l, self.g\n",
+ " s, c = np.sin(th), np.cos(th)\n",
"\n",
" ###########################################################################\n",
" ############ TODO: Write the dynamics equation of your system #############\n",
@@ -88,40 +79,18 @@
" # Hint:\n",
" # You don't need to implement integration rules for your dynamic system.\n",
" # Remember that DAM implemented action models in continuous-time.\n",
- " mu = m1+m2*s**2\n",
- " pddot = 0 ### Replace this value\n",
- " thddot = 0 ### Replace this value\n",
- " data.xout[:] = [ pddot,thddot ]\n",
- " data.xout[:] = cartpole_dynamics(model,data,x,u)\n",
+ " m = m1 + m2\n",
+ " mu = m1 + m2 * s**2\n",
+ " xddot, thddot = cartpole_dynamics(self, data, x, u) ### Write the cartpole dynamics here\n",
+ " data.xout = np.matrix([ xddot, thddot ]).T\n",
" \n",
" # Computing the cost residual and value\n",
- " data.costResiduals[:] = [ s, 1-c, p, xdot, thdot, f ]\n",
- " data.costResiduals[:] *= model.costWeights\n",
- " data.cost = .5*sum( data.costResiduals**2 )\n",
- " return data.xout,data.cost\n",
+ " data.r = np.matrix(self.costWeights * np.array([ s, 1-c, x, xdot, 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",
" # Advance user might implement the derivatives\n",
- " pass\n",
- " \n",
- "class DifferentialActionDataCartpole:\n",
- " def __init__(self,model):\n",
- " self.cost = np.nan\n",
- " self.xout = np.zeros(model.nout)\n",
- "\n",
- " nx,nu,ndx,nq,nv,nout = model.nx,model.nu,model.State.ndx,model.nq,model.nv,model.nout\n",
- " self.costResiduals = np.zeros([ model.ncost ])\n",
- " self.g = np.zeros([ ndx+nu ])\n",
- " self.L = np.zeros([ ndx+nu,ndx+nu ])\n",
- " self.F = np.zeros([ nout,ndx+nu ])\n",
- "\n",
- " self.Lx = self.g[:ndx]\n",
- " self.Lu = self.g[ndx:]\n",
- " self.Lxx = self.L[:ndx,:ndx]\n",
- " self.Lxu = self.L[:ndx,ndx:]\n",
- " self.Luu = self.L[ndx:,ndx:]\n",
- " self.Fx = self.F[:,:ndx]\n",
- " self.Fu = self.F[:,ndx:]"
+ " pass"
]
},
{
@@ -137,26 +106,7 @@
"metadata": {},
"outputs": [],
"source": [
- "# %load solutions/cartpole_dyn.py\n",
- "# Use this function inside DifferentialActionModel.calc by setting:\n",
- "# data.xout[:] = carpole_dynamics(model,data,x,u)\n",
- "def cartpole_dynamics(model,data,x,u):\n",
- " # Getting the state and control variables\n",
- " x,th,xdot,thdot = x\n",
- " f, = u\n",
- "\n",
- " # Shortname for system parameters\n",
- " m1,m2,l,g = model.m1,model.m2,model.l,model.g\n",
- " s,c = np.sin(th),np.cos(th)\n",
- "\n",
- " # Defining the equation of motions\n",
- " m = m1+m2\n",
- " mu = m1+m2*s**2\n",
- " xddot = (f + m2*c*s*g - m2*l*s*thdot**2 )/mu\n",
- " thddot = (c*f/l + m*g*s/l - m2*c*s*thdot**2 )/mu\n",
- "\n",
- " return [ xddot,thddot ]\n",
- "\n"
+ "# %load solutions/cartpole_dyn.py"
]
},
{
@@ -199,8 +149,7 @@
"# Creating the carpole DAM using NumDiff for computing the derivatives.\n",
"# We specify the withGaussApprox=True to have approximation of the\n",
"# Hessian based on the Jacobian of the cost residuals.\n",
- "cartpoleND = DifferentialActionModelNumDiff(cartpoleDAM,\n",
- " withGaussApprox=True)"
+ "cartpoleND = crocoddyl.DifferentialActionModelNumDiff(cartpoleDAM, True)"
]
},
{
@@ -243,9 +192,7 @@
"################## TODO: Create an IAM with from the DAM ##################\n",
"###########################################################################\n",
"# Hint:\n",
- "# Use IntegratedActionModelEuler\n",
- "timeStep = 5e-2\n",
- "cartpoleIAM = IntegratedActionModelEuler() # ... fix me :) "
+ "# Use IntegratedActionModelEuler"
]
},
{
@@ -266,9 +213,9 @@
"outputs": [],
"source": [
"# Fill the number of knots (T) and the time step (dt)\n",
- "x0 = np.array([ 0., 2., 0., 0. ])\n",
+ "x0 = np.matrix([ 0., 3.14, 0., 0. ]).T\n",
"T = 50\n",
- "problem = ShootingProblem(x0, [ cartpoleIAM ]*T, cartpoleIAM)"
+ "problem = crocoddyl.ShootingProblem(x0, [ cartpoleIAM ]*T, cartpoleIAM)"
]
},
{
@@ -284,7 +231,8 @@
"metadata": {},
"outputs": [],
"source": [
- "xs = problem.rollout([ np.zeros(1)]*T)"
+ "us = [ pinocchio.utils.zero(cartpoleIAM.differential.nu) ]*T\n",
+ "xs = problem.rollout(us)"
]
},
{
@@ -345,7 +293,6 @@
"outputs": [],
"source": [
"# %load solutions/cartpole_ddp.py\n",
- "\n",
"##########################################################################\n",
"################## TODO: Create the DDP solver and run it ###############\n",
"###########################################################################"
@@ -402,8 +349,8 @@
"################## TODO: Tune the weights for each cost ###################\n",
"###########################################################################\n",
"terminalCartpole = DifferentialActionModelCartpole()\n",
- "terminalCartpoleDAM = DifferentialActionModelNumDiff(terminalCartpole,withGaussApprox=True)\n",
- "terminalCartpoleIAM = IntegratedActionModelEuler(terminalCartpoleDAM)\n",
+ "terminalCartpoleDAM = crocoddyl.DifferentialActionModelNumDiff(terminalCartpole, True)\n",
+ "terminalCartpoleIAM = crocoddyl.IntegratedActionModelEuler(terminalCartpoleDAM)\n",
"\n",
"terminalCartpole.costWeights[0] = 0 # fix me :)\n",
"terminalCartpole.costWeights[1] = 0 # fix me :)\n",
@@ -411,7 +358,7 @@
"terminalCartpole.costWeights[3] = 0 # fix me :)\n",
"terminalCartpole.costWeights[4] = 0 # fix me :)\n",
"terminalCartpole.costWeights[5] = 0 # fix me :)\n",
- "problem = ShootingProblem(x0, [ cartpoleIAM ]*T, terminalCartpoleIAM)\n"
+ "problem = crocoddyl.ShootingProblem(x0, [ cartpoleIAM ]*T, terminalCartpoleIAM)"
]
},
{
@@ -421,11 +368,12 @@
"outputs": [],
"source": [
"# Creating the DDP solver\n",
- "ddp = SolverDDP(problem)\n",
- "ddp.callback = [ CallbackDDPVerbose() ]\n",
+ "ddp = crocoddyl.SolverDDP(problem)\n",
+ "ddp.setCallbacks([ crocoddyl.CallbackVerbose() ])\n",
"\n",
"# Solving this problem\n",
- "xs, us, done = ddp.solve(maxiter=300)"
+ "done = ddp.solve([], [], 300)\n",
+ "print done"
]
},
{
diff --git a/examples/notebooks/cartpole_swing_up_sol.ipynb b/examples/notebooks/cartpole_swing_up_sol.ipynb
index ab4268cd66df1328bd4a01b896dc547b34ab8c69..03be544176658bc7f5d61b55a8c9c2bdd8a907b1 100644
--- a/examples/notebooks/cartpole_swing_up_sol.ipynb
+++ b/examples/notebooks/cartpole_swing_up_sol.ipynb
@@ -48,74 +48,45 @@
"metadata": {},
"outputs": [],
"source": [
- "from crocoddyl import *\n",
+ "import crocoddyl\n",
+ "import pinocchio\n",
"import numpy as np\n",
"\n",
- "class DifferentialActionModelCartpole:\n",
+ "class DifferentialActionModelCartpole(crocoddyl.DifferentialActionModelAbstract):\n",
" def __init__(self):\n",
- " self.State = StateVector(4)\n",
- " self.nq,self.nv = 2,2\n",
- " self.nx = 4\n",
- " self.ndx = 4\n",
- " self.nout = 2\n",
- " self.nu = 1\n",
- " self.ncost = 6\n",
+ " crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(4), 1, 6) # nu = 1; nr = 6\n",
" self.unone = np.zeros(self.nu)\n",
"\n",
" self.m1 = 1.\n",
" self.m2 = .1\n",
" self.l = .5\n",
" self.g = 9.81\n",
- " self.costWeights = [ 1., 1., 0.1, 0.001, 0.001, 1. ] # sin,1-cos,x,xdot,thdot,f\n",
+ " self.costWeights = [ 1., 1., 0.1, 0.001, 0.001, 1. ] # sin, 1-cos, x, xdot, thdot, f\n",
" \n",
- " def createData(self):\n",
- " return DifferentialActionDataCartpole(self)\n",
- "\n",
- " def calc(model,data,x,u=None):\n",
+ " 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 = x\n",
- " f, = u\n",
+ " x, th, xdot, 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",
- " m1,m2,l,g = model.m1,model.m2,model.l,model.g\n",
- " s,c = np.sin(th),np.cos(th)\n",
+ " m1, m2, l, g = self.m1, self.m2, self.l, self.g\n",
+ " s, c = np.sin(th), np.cos(th)\n",
"\n",
" # Defining the equation of motions\n",
- " m = m1+m2\n",
- " mu = m1+m2*s**2\n",
- " xddot = (f + m2*c*s*g - m2*l*s*thdot**2 )/mu\n",
- " thddot = (c*f/l + m*g*s/l - m2*c*s*thdot**2 )/mu\n",
- " data.xout[:] = [ xddot,thddot ]\n",
+ " m = m1 + m2\n",
+ " mu = m1 + m2 * s**2\n",
+ " xddot = (f + m2 * c * s * g - m2 * l * s * thdot**2 ) / mu\n",
+ " thddot = (c * f / l + m * g * s / l - m2 * c * s * thdot**2 ) / mu\n",
+ " data.xout = np.matrix([ xddot,thddot ]).T\n",
"\n",
" # Computing the cost residual and value\n",
- " data.costResiduals[:] = [ s, 1-c, x, xdot, thdot, f ]\n",
- " data.costResiduals[:] *= model.costWeights\n",
- " data.cost = .5*sum( data.costResiduals**2 )\n",
- " return data.xout,data.cost\n",
+ " data.r = np.matrix(self.costWeights * np.array([ s, 1-c, x, xdot, 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",
+ " def calcDiff(self,data,x,u=None,recalc=True):\n",
" # Advance user might implement the derivatives\n",
- " pass\n",
- " \n",
- "class DifferentialActionDataCartpole:\n",
- " def __init__(self,model):\n",
- " self.cost = np.nan\n",
- " self.xout = np.zeros(model.nout)\n",
- "\n",
- " nx,nu,ndx,nout = model.nx,model.nu,model.ndx,model.nout\n",
- " self.costResiduals = np.zeros([ model.ncost ])\n",
- " self.g = np.zeros([ ndx+nu ])\n",
- " self.L = np.zeros([ ndx+nu,ndx+nu ])\n",
- " self.F = np.zeros([ nout,ndx+nu ])\n",
- "\n",
- " self.Lx = self.g[:ndx]\n",
- " self.Lu = self.g[ndx:]\n",
- " self.Lxx = self.L[:ndx,:ndx]\n",
- " self.Lxu = self.L[:ndx,ndx:]\n",
- " self.Luu = self.L[ndx:,ndx:]\n",
- " self.Fx = self.F[:,:ndx]\n",
- " self.Fu = self.F[:,ndx:]"
+ " pass"
]
},
{
@@ -133,7 +104,7 @@
"source": [
"cartpoleDAM = DifferentialActionModelCartpole()\n",
"cartpoleData = cartpoleDAM.createData()\n",
- "x = cartpoleDAM.State.rand()\n",
+ "x = cartpoleDAM.state.rand()\n",
"u = np.zeros(1)\n",
"cartpoleDAM.calc(cartpoleData,x,u)"
]
@@ -156,8 +127,7 @@
"outputs": [],
"source": [
"# Creating the cartpole DAM\n",
- "cartpoleND = DifferentialActionModelNumDiff(cartpoleDAM,\n",
- " withGaussApprox=True)"
+ "cartpoleND = crocoddyl.DifferentialActionModelNumDiff(cartpoleDAM, True)"
]
},
{
@@ -187,7 +157,7 @@
"outputs": [],
"source": [
"timeStep = 5e-2\n",
- "cartpoleIAM = IntegratedActionModelEuler(cartpoleND,timeStep)"
+ "cartpoleIAM = crocoddyl.IntegratedActionModelEuler(cartpoleND, timeStep)"
]
},
{
@@ -208,9 +178,9 @@
"outputs": [],
"source": [
"# Fill the number of knots (T) and the time step (dt)\n",
- "x0 = np.array([ 0., 3.14, 0., 0. ])\n",
+ "x0 = np.matrix([ 0., 3.14, 0., 0. ]).T\n",
"T = 50\n",
- "problem = ShootingProblem(x0, [ cartpoleIAM ]*T, cartpoleIAM)"
+ "problem = crocoddyl.ShootingProblem(x0, [ cartpoleIAM ]*T, cartpoleIAM)"
]
},
{
@@ -226,7 +196,8 @@
"metadata": {},
"outputs": [],
"source": [
- "xs = problem.rollout([ np.zeros(1) ]*T)"
+ "us = [ pinocchio.utils.zero(cartpoleIAM.differential.nu) ]*T\n",
+ "xs = problem.rollout(us)"
]
},
{
@@ -287,12 +258,13 @@
"outputs": [],
"source": [
"# Creating the DDP solver\n",
- "ddp = SolverDDP(problem)\n",
- "# ddp.callback = [ CallbackDDPVerbose() ]\n",
+ "ddp = crocoddyl.SolverDDP(problem)\n",
+ "ddp.setCallbacks([crocoddyl.CallbackVerbose()])\n",
"\n",
"# Solving this problem\n",
- "xs, us, done = ddp.solve(maxiter=1000)\n",
- "print done"
+ "done = ddp.solve([], [], 1000)\n",
+ "print done\n",
+ "print ddp.us"
]
},
{
@@ -307,7 +279,7 @@
"%matplotlib inline\n",
"\n",
"# Create animation\n",
- "anim = animateCartpole(xs)"
+ "anim = animateCartpole(ddp.xs)"
]
},
{
@@ -347,8 +319,8 @@
"################## TODO: Tune the weights for each cost ###################\n",
"###########################################################################\n",
"terminalCartpole = DifferentialActionModelCartpole()\n",
- "terminalCartpoleDAM = DifferentialActionModelNumDiff(terminalCartpole,withGaussApprox=True)\n",
- "terminalCartpoleIAM = IntegratedActionModelEuler(terminalCartpoleDAM)\n",
+ "terminalCartpoleDAM = crocoddyl.DifferentialActionModelNumDiff(terminalCartpole, True)\n",
+ "terminalCartpoleIAM = crocoddyl.IntegratedActionModelEuler(terminalCartpoleDAM)\n",
"\n",
"terminalCartpole.costWeights[0] = 100\n",
"terminalCartpole.costWeights[1] = 100\n",
@@ -356,7 +328,7 @@
"terminalCartpole.costWeights[3] = 0.1\n",
"terminalCartpole.costWeights[4] = 0.01\n",
"terminalCartpole.costWeights[5] = 0.0001\n",
- "problem = ShootingProblem(x0, [ cartpoleIAM ]*T, terminalCartpoleIAM)"
+ "problem = crocoddyl.ShootingProblem(x0, [ cartpoleIAM ]*T, terminalCartpoleIAM)"
]
},
{
@@ -366,11 +338,12 @@
"outputs": [],
"source": [
"# Creating the DDP solver\n",
- "ddp = SolverDDP(problem)\n",
- "# ddp.callback = [ CallbackDDPVerbose() ]\n",
+ "ddp = crocoddyl.SolverDDP(problem)\n",
+ "ddp.setCallbacks([ crocoddyl.CallbackVerbose() ])\n",
"\n",
"# Solving this problem\n",
- "xs, us, done = ddp.solve(maxiter=300)"
+ "done = ddp.solve([], [], 300)\n",
+ "print done"
]
},
{
@@ -383,7 +356,7 @@
"%matplotlib inline\n",
"\n",
"# Create animation\n",
- "anim = animateCartpole(xs)"
+ "anim = animateCartpole(ddp.xs)"
]
},
{
diff --git a/examples/notebooks/cartpole_utils.py b/examples/notebooks/cartpole_utils.py
index 638cf72698e88a5e6cbb6bd0adee08adf94f7310..80b289ae4b3e089cef0599a86c6d82f5d117c865 100644
--- a/examples/notebooks/cartpole_utils.py
+++ b/examples/notebooks/cartpole_utils.py
@@ -22,9 +22,9 @@ def animateCartpole(xs, sleep=50):
return patch, line, time_text
def animate(i):
- x_cart = xs[i][0]
+ x_cart = np.asscalar(xs[i][0])
y_cart = 0.
- theta = xs[i][1]
+ theta = np.asscalar(xs[i][1])
patch.set_xy([x_cart - cart_size / 2, y_cart - cart_size / 2])
x_pole = np.cumsum([x_cart, -pole_length * sin(theta)])
y_pole = np.cumsum([y_cart, pole_length * cos(theta)])
diff --git a/examples/notebooks/crocoddyl b/examples/notebooks/crocoddyl
deleted file mode 120000
index 4ee0865649b4993bb6cc6e57008e952bbb429134..0000000000000000000000000000000000000000
--- a/examples/notebooks/crocoddyl
+++ /dev/null
@@ -1 +0,0 @@
-../../crocoddyl/
\ No newline at end of file
diff --git a/examples/notebooks/introduction_to_crocoddyl.ipynb b/examples/notebooks/introduction_to_crocoddyl.ipynb
index e75f13cfe078b6ccf6882b1efa151846f099ba39..deea9e4b56035b1642be7f87a7be089cef7369e5 100644
--- a/examples/notebooks/introduction_to_crocoddyl.ipynb
+++ b/examples/notebooks/introduction_to_crocoddyl.ipynb
@@ -8,7 +8,7 @@
"\n",
"\n",
"## I. Welcome to crocoddyl\n",
- "Crocoddyl is an **optimal control library for robot control under contact sequence**. Its solver is based on an efficient Differential Dynamic Programming (DDP) algorithm. Crocoddyl computes optimal trajectories along to optimal feedback gains. It uses Pinocchio for fast computation of robot dynamics and its analytical derivatives. \n",
+ "Crocoddyl is an **optimal control library for robot control under contact sequence**. Its solver is based on an efficient Differential Dynamic Programming (DDP) algorithm. Crocoddyl computes optimal trajectories along with optimal feedback gains. It uses Pinocchio for fast computation of robot dynamics and its analytical derivatives. \n",
"\n",
"Crocoddyl is focused on multi-contact optimal control problem (MCOP) which as the form:\n",
"\n",
@@ -21,7 +21,7 @@
"$$ \\mathbf{\\dot{x}} = \\mathbf{f}(\\mathbf{x},\\mathbf{u}),$$\n",
"$$ \\mathbf{x}\\in\\mathcal{X}, \\mathbf{u}\\in\\mathcal{U}, \\boldsymbol{\\lambda}\\in\\mathcal{K}.$$\n",
"where\n",
- " - the state $\\mathbf{x}=(\\mathbf{q},\\mathbf{v})$ lies in a manifold, e.g. Lie manifold $\\mathbf{q}\\in SE(3)\\times \\mathbb{R}^{n_j}$,\n",
+ " - the state $\\mathbf{x}=(\\mathbf{q},\\mathbf{v})$ lies in a manifold, e.g. Lie manifold $\\mathbf{q}\\in SE(3)\\times \\mathbb{R}^{n_j}$, $n_j$ being the number of degrees of freedom of the robot.\n",
" - the system has underactuacted dynamics, i.e. $\\mathbf{u}=(\\mathbf{0},\\boldsymbol{\\tau})$,\n",
" - $\\mathcal{X}$, $\\mathcal{U}$ are the state and control admissible sets, and\n",
" - $\\mathcal{K}$ represents the contact constraints.\n",
@@ -37,7 +37,7 @@
"source": [
"# II. Action models\n",
"\n",
- "In crocoddyl, an action model combines dynamics and cost models. Each node, in our optimal control problem, is described through an action model. Every time that we want describe a problem, we need to provide ways of computing the dynamics, cost functions and their derivatives. All these is described inside the action model.\n",
+ "In crocoddyl, an action model combines dynamics and cost models. Each node, in our optimal control problem, is described through an action model. In order to describe a problem, we need to provide ways of computing the dynamics, the cost functions and their derivatives. All these are described inside the action model.\n",
"\n",
"To understand the mathematical aspects behind an action model, let's first get a locally linearize version of our optimal control problem as:\n",
"\n",
@@ -85,11 +85,11 @@
"### Important notes:\n",
" - An action model describes the dynamics and cost functions for a node in our optimal control problem.\n",
" - Action models lie in the discrete time space.\n",
- " - For debugging and prototyping, we have also implemented NumDiff abstractions. These computations depend only in the defining of the dynamics equation and cost functions. However to asses efficiency, crocoddyl uses **analytical derivatives** computed from Pinocchio.\n",
+ " - For debugging and prototyping, we have also implemented numerical differentiation (NumDiff) abstractions. These computations depend only on the definition of the dynamics equation and cost functions. However to asses efficiency, crocoddyl uses **analytical derivatives** computed from Pinocchio.\n",
"\n",
"\n",
"## II.a Differential and Integrated Action Models\n",
- "Optimal control solvers require the time-discrete model of cost and dynamics. However, it's often convenient to implement them in continuous time (e.g. to combine with abstract integration rules). In crocoddyl, this continuous-time action models are called \"Differential Action Model (DAM)\". And together with predefined \"Integrated Action Models (IAM)\", it possible to retrieve the time-discrete action model.\n",
+ "Optimal control solvers require the time-discrete model of the cost and the dynamics. However, it's often convenient to implement them in continuous time (e.g. to combine with abstract integration rules). In crocoddyl, this continuous-time action models are called \"Differential Action Model (DAM)\". And together with predefined \"Integrated Action Models (IAM)\", it possible to retrieve the time-discrete action model.\n",
"\n",
"At the moment, we have:\n",
" - a simpletic Euler and\n",
@@ -149,7 +149,7 @@
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
diff --git a/examples/notebooks/manipulator.ipynb b/examples/notebooks/manipulator.ipynb
index b6c42d9ef7bf15a84026e8dc9206d4d35b7bccb1..7c9d4d82aaac8fd544df1143501a02ded21edfab 100644
--- a/examples/notebooks/manipulator.ipynb
+++ b/examples/notebooks/manipulator.ipynb
@@ -17,64 +17,70 @@
"outputs": [],
"source": [
"# %load arm_example.py\n",
- "from crocoddyl import *\n",
+ "import crocoddyl\n",
"import pinocchio\n",
"import numpy as np\n",
+ "import example_robot_data\n",
"\n",
- "robot = loadTalosArm()\n",
- "robot.initDisplay(loadModel=True)\n",
+ "robot = example_robot_data.loadTalosArm()\n",
+ "robot_model = robot.model\n",
"\n",
- "robot.q0.flat[:] = [ 2,1.5,-2,0,0,0,0 ]\n",
- "robot.model.armature[:] = .2\n",
- "frameId = robot.model.getFrameId('gripper_left_joint')\n",
- "DT = 1e-2\n",
- "T = 25\n",
+ "DT = 1e-3\n",
+ "T = 25\n",
+ "target = np.array([0.4, 0., .4])\n",
"\n",
- "target = np.array([ 0.4,0.,0.4 ])\n",
- "\n",
- "robot.viewer.gui.addSphere('world/point',.1,[1,0,0,1]) # radius = .1, RGBA=1001\n",
- "robot.viewer.gui.applyConfiguration('world/point', target.tolist()+[0,0,0,1] ) # xyz+quaternion\n",
+ "robot.initViewer(loadModel=True)\n",
+ "robot.viewer.gui.addSphere('world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001\n",
+ "robot.viewer.gui.applyConfiguration('world/point', target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion\n",
"robot.viewer.gui.refresh()\n",
"\n",
"# Create the cost functions\n",
- "costTrack = CostModelFrameTranslation(robot.model,frame=frameId,ref=target)\n",
- "costXReg = CostModelState(robot.model,StatePinocchio(robot.model))\n",
- "costUReg = CostModelControl(robot.model)\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 the cost model for the running and terminal action models\n",
- "runningCostModel = CostModelSum(robot.model)\n",
- "terminalCostModel = CostModelSum(robot.model)\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( name=\"pos\", weight = 1, cost = costTrack)\n",
- "runningCostModel.addCost( name=\"xreg\", weight = 1e-4, cost = costXReg)\n",
- "runningCostModel.addCost( name=\"ureg\", weight = 1e-7, cost = costUReg)\n",
- "terminalCostModel.addCost( name=\"pos\", weight = 1000, cost = costTrack)\n",
- "terminalCostModel.addCost( name=\"xreg\", weight = 1e-4, cost = costXReg)\n",
- "terminalCostModel.addCost( name=\"ureg\", weight = 1e-7, cost = costUReg)\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",
"# Create the action model\n",
- "runningModel = DifferentialActionModelFullyActuated(robot.model, runningCostModel)\n",
- "terminalModel = DifferentialActionModelFullyActuated(robot.model, terminalCostModel)\n",
+ "runningModel = crocoddyl.IntegratedActionModelEuler(\n",
+ " crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel), DT)\n",
+ "terminalModel = crocoddyl.IntegratedActionModelEuler(\n",
+ " crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel))\n",
+ "#runningModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T\n",
+ "#terminalModel.differential.armature = 0.2 * np.matrix(np.ones(state.nv)).T\n",
"\n",
"# Create the problem\n",
- "x0 = np.concatenate([ m2a(robot.q0), np.zeros(robot.model.nv)])\n",
- "problem = ShootingProblem(x0, [ IntegratedActionModelEuler(runningModel) ] * T,\n",
- " IntegratedActionModelEuler(terminalModel))\n",
+ "q0 = np.matrix([2., 1.5, -2., 0., 0., 0., 0.]).T\n",
+ "x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])\n",
+ "problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)\n",
"\n",
"# Creating the DDP solver for this OC problem, defining a logger\n",
- "ddp = SolverDDP(problem)\n",
- "cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]\n",
- "ddp.callback = [ CallbackDDPVerbose(), CallbackSolverDisplay(robot,4,1,cameraTF) ]\n",
+ "ddp = crocoddyl.SolverDDP(problem)\n",
+ "ddp.setCallbacks([crocoddyl.CallbackVerbose()])\n",
"\n",
"# Solving it with the DDP algorithm\n",
"ddp.solve()\n",
"\n",
"# Visualizing the solution in gepetto-viewer\n",
- "for x in ddp.xs: robot.display(a2m(x))\n",
+ "crocoddyl.displayTrajectory(robot, ddp.xs, runningModel.dt)\n",
"\n",
- "print('Finally reached = ',\n",
- " ddp.datas()[T].differential.costs['pos'].pinocchio.oMf[frameId].translation.T)\n"
+ "robot_data = robot_model.createData()\n",
+ "xT = ddp.xs[-1]\n",
+ "pinocchio.forwardKinematics(robot_model, robot_data, xT[:state.nq])\n",
+ "pinocchio.updateFramePlacements(robot_model, robot_data)\n",
+ "print('Finally reached = ', robot_data.oMf[robot_model.getFrameId(\"gripper_left_joint\")].translation.T)"
]
},
{
@@ -106,7 +112,7 @@
"metadata": {},
"outputs": [],
"source": [
- "dataTrack = costTrack.createData(robot.data)"
+ "trackData = goalTrackingCost.createData(robot.data)"
]
},
{
@@ -126,10 +132,10 @@
},
"outputs": [],
"source": [
- "pinocchio.updateFramePlacements(robot.model,robot.data)\n",
- "pinocchio.computeJointJacobians(robot.model,robot.data,a2m(x))\n",
- "costTrack.calcDiff(dataTrack,x0,None)\n",
- "print(dataTrack.Lx,dataTrack.Lu)"
+ "pinocchio.updateFramePlacements(robot.model, robot.data)\n",
+ "pinocchio.computeJointJacobians(robot.model, robot.data, xT[:state.nq])\n",
+ "goalTrackingCost.calcDiff(trackData, x0)\n",
+ "print(trackData.Lx, trackData.Lu)"
]
},
{
@@ -174,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, CallbackDDPVerbose 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 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",
"\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."
]
@@ -240,11 +246,11 @@
"metadata": {},
"outputs": [],
"source": [
- "seq0 = [runningmodels[0]]*T + [terminalmodel[0]]\n",
- "seq1 = [runningmodels[1]]*T + [terminalmodel[1]]\n",
- "seq2 = [runningmodels[2]]*T + [terminalmodel[2]]\n",
- "seq3 = [runningmodels[3]]*T \n",
- "problem = ShootingProblem(x0,seq0+seq1+seq2+seq3,terminalmodel[3])"
+ "seq0 = [runningModels[0]]*T + [terminalModels[0]]\n",
+ "seq1 = [runningModels[1]]*T + [terminalModels[1]]\n",
+ "seq2 = [runningModels[2]]*T + [terminalModels[2]]\n",
+ "seq3 = [runningModels[3]]*T \n",
+ "problem = crocoddyl.ShootingProblem(x0,seq0+seq1+seq2+seq3,terminalmodel[3])"
]
},
{
@@ -260,7 +266,7 @@
"metadata": {},
"outputs": [],
"source": [
- "ddp = SolverDDP(problem)\n",
+ "ddp = crocoddyl.SolverDDP(problem)\n",
"ddp.solve()"
]
},
@@ -292,8 +298,9 @@
"outputs": [],
"source": [
"for i in range(1,6):\n",
- " terminalmodel[1].costs['pos'].weight = 10**i\n",
- " ddp.solve(init_xs=ddp.xs,init_us=ddp.us,maxiter=10)\n"
+ " for m in terminalModels:\n",
+ " m.costs.costs['gripperPose'].weight = 10**i\n",
+ " ddp.solve(ddp.xs, ddp.us, 10)"
]
}
],
diff --git a/examples/notebooks/p2p.py b/examples/notebooks/p2p.py
index 4ea1081bd39e0c2c82e1344561b9df19f6e4afac..7724efe32342191d0abcf86f6fe7c3c86ce0c8b2 100644
--- a/examples/notebooks/p2p.py
+++ b/examples/notebooks/p2p.py
@@ -1,41 +1,40 @@
+import crocoddyl
+import pinocchio
import numpy as np
+import example_robot_data
-import pinocchio
-from crocoddyl import *
+robot = example_robot_data.loadTalosArm()
+robot_model = robot.model
-robot = loadTalosArm()
-robot.q0.flat[:] = [2, 1.5, -2, 0, 0, 0, 0]
-robot.model.armature[:] = .2
-frameId = robot.model.getFrameId('gripper_left_joint')
-DT = 1e-2
+DT = 1e-3
T = 15
-State = StatePinocchio(robot.model)
+state = crocoddyl.StateMultibody(robot.model)
ps = [
- np.array([0.4, 0, .4]),
+ np.array([0.4, 0., .4]),
np.array([0.4, 0.4, .4]),
- np.array([0.4, 0.4, 0]),
- np.array([0.4, 0, 0]),
+ np.array([0.4, 0.4, 0.]),
+ np.array([0.4, 0., 0.]),
]
colors = [
- [1, 0, 0, 1],
- [0, 1, 0, 1],
- [0, 0, 1, 1],
- [1, 0, 1, 1],
+ [1., 0., 0., 1.],
+ [0., 1., 0., 1.],
+ [0., 0., 1., 1.],
+ [1., 0., 1., 1.],
]
-robot.initDisplay(loadModel=True)
+robot.initViewer(loadModel=True)
gv = robot.viewer.gui
for i, p in enumerate(ps):
- gv.addSphere('world/point%d' % i, .1, colors[i])
- gv.applyConfiguration('world/point%d' % i, p.tolist() + [0, 0, 0, 1])
+ gv.addSphere('world/point%d' % i, .05, colors[i])
+ gv.applyConfiguration('world/point%d' % i, p.tolist() + [0., 0., 0., 1.])
gv.refresh()
# State and control regularization costs
-costXReg = CostModelState(robot.model, StatePinocchio(robot.model))
-costUReg = CostModelControl(robot.model, nu=robot.model.nv)
+xRegCost = crocoddyl.CostModelState(state)
+uRegCost = crocoddyl.CostModelControl(state)
# Then let's added the running and terminal cost functions per each action
# model
@@ -43,31 +42,34 @@ runningModels = []
terminalModels = []
for p in ps:
# Create the tracking cost
- costTrack = CostModelFrameTranslation(robot.model, frame=frameId, ref=p)
+ Mref = crocoddyl.FrameTranslation(robot_model.getFrameId("gripper_left_joint"), np.matrix(p).T)
+ goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref)
# Create the running action model
- runningCostModel = CostModelSum(robot.model)
- runningCostModel.addCost(name="pos", weight=1, cost=costTrack)
- runningCostModel.addCost(name="xreg", weight=1e-4, cost=costXReg)
- runningCostModel.addCost(name="ureg", weight=1e-7, cost=costUReg)
- runningModels += [DifferentialActionModelFullyActuated(robot.model, runningCostModel)]
+ runningCostModel = crocoddyl.CostModelSum(state)
+ runningCostModel.addCost("gripperPose", goalTrackingCost, 1.)
+ runningCostModel.addCost("xReg", xRegCost, 1e-4)
+ runningCostModel.addCost("uReg", uRegCost, 1e-7)
+ runningModels += [crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel)]
# Create the terminal action model
- terminalCostModel = CostModelSum(robot.model)
- terminalCostModel.addCost(name="pos", weight=1000, cost=costTrack)
- terminalCostModel.addCost(name="xreg", weight=1e-4, cost=costXReg)
- terminalCostModel.addCost(name="ureg", weight=1e-7, cost=costUReg)
- terminalModels += [DifferentialActionModelFullyActuated(robot.model, terminalCostModel)]
-
-x0 = np.concatenate([m2a(robot.q0), np.zeros(robot.model.nv)])
-seqs = [[IntegratedActionModelEuler(runningModel)] * T + [IntegratedActionModelEuler(terminalModel)]
+ terminalCostModel = crocoddyl.CostModelSum(state)
+ terminalCostModel.addCost("gripperPose", goalTrackingCost, 1000.)
+ terminalCostModel.addCost("xreg", xRegCost, 1e-4)
+ terminalCostModel.addCost("uReg", uRegCost, 1e-7)
+ terminalModels += [crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel)]
+
+q0 = np.matrix([2., 1.5, -2., 0., 0., 0., 0.]).T
+x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
+seqs = [[crocoddyl.IntegratedActionModelEuler(runningModel, DT)] * T +
+ [crocoddyl.IntegratedActionModelEuler(terminalModel)]
for runningModel, terminalModel in zip(runningModels, terminalModels)]
-problem = ShootingProblem(x0, sum(seqs, [])[:-1], seqs[-1][-1])
+problem = crocoddyl.ShootingProblem(x0, sum(seqs, [])[:-1], seqs[-1][-1])
# Creating the DDP solver for this OC problem, defining a logger
-ddp = SolverDDP(problem)
+ddp = crocoddyl.SolverDDP(problem)
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
-ddp.callback = [CallbackDDPVerbose()]
+ddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
ddp.solve()
@@ -75,11 +77,15 @@ ddp.solve()
# Penalty if you want.
for i in range(4, 6):
for m in terminalModels:
- m.costs['pos'].weight = 10**i
- ddp.solve(init_xs=ddp.xs, init_us=ddp.us, maxiter=10)
+ m.costs.costs['gripperPose'].weight = 10**i
+ ddp.solve(ddp.xs, ddp.us, 10)
# Visualizing the solution in gepetto-viewer
-CallbackSolverDisplay(robot)(ddp)
+crocoddyl.displayTrajectory(robot, ddp.xs, DT)
+robot_data = robot_model.createData()
for i, s in enumerate(seqs + [1]):
- print ddp.datas()[i * T].differential.costs['pos'].pinocchio.oMf[frameId].translation.T
+ xT = ddp.xs[i * T]
+ pinocchio.forwardKinematics(robot_model, robot_data, xT[:state.nq])
+ pinocchio.updateFramePlacements(robot_model, robot_data)
+ print('Finally reached = ', robot_data.oMf[robot_model.getFrameId("gripper_left_joint")].translation.T)
diff --git a/examples/notebooks/solutions/cartpole_ddp.py b/examples/notebooks/solutions/cartpole_ddp.py
index 3d89e2efe4f6b74276f54ade46ab1de2bad26086..9790d23b4d95560f664f83cccbbfb0f56990b8d5 100644
--- a/examples/notebooks/solutions/cartpole_ddp.py
+++ b/examples/notebooks/solutions/cartpole_ddp.py
@@ -3,9 +3,9 @@
###########################################################################
# Creating the DDP solver
-ddp = SolverDDP(problem)
-# ddp.callback = [ CallbackDDPVerbose() ]
+ddp = crocoddyl.SolverDDP(problem)
+ddp.setCallbacks([crocoddyl.CallbackDDPVerbose()])
# Solving this problem
-xs, us, done = ddp.solve(maxiter=1000)
+done = ddp.solve(1000)
print done
diff --git a/examples/notebooks/solutions/cartpole_dyn.py b/examples/notebooks/solutions/cartpole_dyn.py
index 5dc5f411700c35b5fab88ca2bb35658330207611..390a3ef87bdcda0a9580ee979294d795a002d969 100644
--- a/examples/notebooks/solutions/cartpole_dyn.py
+++ b/examples/notebooks/solutions/cartpole_dyn.py
@@ -1,9 +1,9 @@
# Use this function inside DifferentialActionModel.calc by setting:
-# data.xout[:] = carpole_dynamics(model,data,x,u)
+# 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 = x
- f, = u
+ x, th, xdot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])
+ f = np.asscalar(u[0])
# Shortname for system parameters
m1, m2, l, g = model.m1, model.m2, model.l, model.g
diff --git a/examples/notebooks/solutions/cartpole_integration.py b/examples/notebooks/solutions/cartpole_integration.py
index 6abdca2b144ac31c118a7d4cdb2de9b65d4b0484..a5b208b3598f9108156a2a159e3be88dd1cf6787 100644
--- a/examples/notebooks/solutions/cartpole_integration.py
+++ b/examples/notebooks/solutions/cartpole_integration.py
@@ -4,4 +4,4 @@
# Hint:
# Use IntegratedActionModelEuler
timeStep = 5e-2
-cartpoleIAM = IntegratedActionModelEuler(cartpoleND, timeStep)
+cartpoleIAM = crocoddyl.IntegratedActionModelEuler(cartpoleND, timeStep)
diff --git a/examples/notebooks/unicycle_towards_origin.ipynb b/examples/notebooks/unicycle_towards_origin.ipynb
index cc14bb4246c0ee9dce59d1843974e93d8f41a44a..901ec7084246990757f7ac290d372bc3198e4a9a 100644
--- a/examples/notebooks/unicycle_towards_origin.ipynb
+++ b/examples/notebooks/unicycle_towards_origin.ipynb
@@ -71,8 +71,8 @@
"metadata": {},
"outputs": [],
"source": [
- "from crocoddyl import *\n",
- "model = ActionModelUnicycle()\n",
+ "import crocoddyl\n",
+ "model = crocoddyl.ActionModelUnicycle()\n",
"data = model.createData()"
]
},
@@ -91,10 +91,10 @@
"metadata": {},
"outputs": [],
"source": [
- "model.costWeights = [\n",
+ "model.costWeights = np.matrix([\n",
" 1, # state weight\n",
" 1 # control weight\n",
- "]"
+ "]).T"
]
},
{
@@ -118,9 +118,9 @@
"metadata": {},
"outputs": [],
"source": [
- "x0 = np.array([ -1., -1., 1. ]) #x,y,theta\n",
+ "x0 = np.matrix([ -1., -1., 1. ]).T #x,y,theta\n",
"T = 20\n",
- "problem = ShootingProblem(x0, [ model ]*T, model)"
+ "problem = crocoddyl.ShootingProblem(x0, [ model ] * T, model)"
]
},
{
@@ -138,7 +138,7 @@
"metadata": {},
"outputs": [],
"source": [
- "us = [ np.array([1., 1.]) for t in range(T)]\n",
+ "us = [ np.matrix([1., 1.]).T for t in range(T)]\n",
"xs = problem.rollout(us)"
]
},
@@ -176,8 +176,8 @@
"metadata": {},
"outputs": [],
"source": [
- "ddp = SolverDDP(problem)\n",
- "xs,us,done = ddp.solve()\n",
+ "ddp = crocoddyl.SolverDDP(problem)\n",
+ "done = ddp.solve()\n",
"assert(done)"
]
},
@@ -188,7 +188,7 @@
"outputs": [],
"source": [
"plt.clf()\n",
- "for x in xs: plotUnicycle(x)\n",
+ "for x in ddp.xs: plotUnicycle(x)\n",
"plt.axis([-2,2,-2,2])"
]
},
@@ -205,7 +205,7 @@
"metadata": {},
"outputs": [],
"source": [
- "print xs[-1]"
+ "print ddp.xs[-1]"
]
},
{
diff --git a/examples/notebooks/unicycle_utils.py b/examples/notebooks/unicycle_utils.py
index 7274c9739ba2b6b8f241772d93a7ffda645beb9b..ad5fbcffbabdbbb30c6a4a444223d95e6c71aa69 100644
--- a/examples/notebooks/unicycle_utils.py
+++ b/examples/notebooks/unicycle_utils.py
@@ -4,7 +4,7 @@ import numpy as np
def plotUnicycle(x):
sc, delta = .1, .1
- a, b, th = x[:4]
+ a, b, th = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2])
c, s = np.cos(th), np.sin(th)
refs = [
plt.arrow(a - sc / 2 * c - delta * s, b - sc / 2 * s + delta * c, c * sc, s * sc, head_width=.05),
diff --git a/examples/quadrupedal_walking_balancing.py b/examples/quadrupedal_walking_balancing.py
index 82981ee945fd2ced7817f7a356e458e09507628e..0274bd02634b02003f27849212a839fa52b02acf 100644
--- a/examples/quadrupedal_walking_balancing.py
+++ b/examples/quadrupedal_walking_balancing.py
@@ -1,536 +1,29 @@
import sys
import numpy as np
+
+import crocoddyl
+import example_robot_data
import pinocchio
-from crocoddyl import (ActivationModelWeightedQuad, ActuationModelFreeFloating, CallbackDDPLogger, CallbackDDPVerbose,
- CallbackSolverDisplay, ContactModel3D, ContactModelMultiple, CostModelCoM, CostModelControl,
- CostModelFrameTranslation, CostModelFrameVelocity, CostModelState, CostModelSum,
- DifferentialActionModelFloatingInContact, IntegratedActionModelEuler, ShootingProblem,
- SolverDDP, StatePinocchio, a2m, displayTrajectory, loadHyQ, m2a)
+from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
-WITHDISPLAY = 'disp' in sys.argv
+WITHDISPLAY = 'display' in sys.argv
WITHPLOT = 'plot' in sys.argv
-
-def plotSolution(rmodel, xs, us):
- import matplotlib.pyplot as plt
- # Getting the state and control trajectories
- nx, nq, nu = xs[0].shape[0], rmodel.nq, us[0].shape[0]
- X = [0.] * nx
- U = [0.] * nu
- for i in range(nx):
- X[i] = [x[i] for x in xs]
- for i in range(nu):
- U[i] = [u[i] for u in us]
-
- # Plotting the joint positions, velocities and torques
- plt.figure(1)
- legJointNames = ['HAA', 'HFE', 'KFE']
- # LF foot
- plt.subplot(4, 3, 1)
- plt.title('joint position [rad]')
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(7, 10))]
- plt.ylabel('LF')
- plt.legend()
- plt.subplot(4, 3, 2)
- plt.title('joint velocity [rad/s]')
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 6, nq + 9))]
- plt.ylabel('LF')
- plt.legend()
- plt.subplot(4, 3, 3)
- plt.title('joint torque [Nm]')
- [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(0, 3))]
- plt.ylabel('LF')
- plt.legend()
-
- # LH foot
- plt.subplot(4, 3, 4)
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(10, 13))]
- plt.ylabel('LH')
- plt.legend()
- plt.subplot(4, 3, 5)
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 9, nq + 12))]
- plt.ylabel('LH')
- plt.legend()
- plt.subplot(4, 3, 6)
- [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(3, 6))]
- plt.ylabel('LH')
- plt.legend()
-
- # RF foot
- plt.subplot(4, 3, 7)
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(13, 16))]
- plt.ylabel('RF')
- plt.legend()
- plt.subplot(4, 3, 8)
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 12, nq + 15))]
- plt.ylabel('RF')
- plt.legend()
- plt.subplot(4, 3, 9)
- [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(6, 9))]
- plt.ylabel('RF')
- plt.legend()
-
- # RH foot
- plt.subplot(4, 3, 10)
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(16, 19))]
- plt.ylabel('RH')
- plt.xlabel('knots')
- plt.legend()
- plt.subplot(4, 3, 11)
- [plt.plot(X[k], label=legJointNames[i]) for i, k in enumerate(range(nq + 15, nq + 18))]
- plt.ylabel('RH')
- plt.xlabel('knots')
- plt.legend()
- plt.subplot(4, 3, 12)
- [plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(9, 12))]
- plt.ylabel('RH')
- plt.legend()
- plt.xlabel('knots')
- plt.show()
-
- plt.figure(2)
- rdata = rmodel.createData()
- Cx = []
- Cy = []
- for x in xs:
- q = a2m(x[:rmodel.nq])
- c = pinocchio.centerOfMass(rmodel, rdata, q)
- Cx.append(np.asscalar(c[0]))
- Cy.append(np.asscalar(c[1]))
- plt.plot(Cx, Cy)
- plt.title('CoM position')
- plt.xlabel('x [m]')
- plt.ylabel('y [m]')
- plt.grid(True)
- plt.show()
-
-
-class TaskSE3:
- def __init__(self, oXf, frameId):
- self.oXf = oXf
- self.frameId = frameId
-
-
-class SimpleQuadrupedalGaitProblem:
- def __init__(self, rmodel, lfFoot, rfFoot, lhFoot, rhFoot):
- self.rmodel = rmodel
- self.rdata = rmodel.createData()
- self.state = StatePinocchio(self.rmodel)
- # Getting the frame id for all the legs
- self.lfFootId = self.rmodel.getFrameId(lfFoot)
- self.rfFootId = self.rmodel.getFrameId(rfFoot)
- self.lhFootId = self.rmodel.getFrameId(lhFoot)
- self.rhFootId = self.rmodel.getFrameId(rhFoot)
- # Defining default state
- q0 = self.rmodel.referenceConfigurations["half_sitting"]
- self.rmodel.defaultState = np.concatenate([m2a(q0), np.zeros(self.rmodel.nv)])
- self.firstStep = True
-
- def createCoMProblem(self, x0, comGoTo, timeStep, numKnots):
- """ Create a shooting problem for a CoM forward/backward task.
-
- :param x0: initial state
- :param comGoTo: initial CoM motion
- :param timeStep: step time for each knot
- :param numKnots: number of knots per each phase
- :return shooting problem
- """
- # Compute the current foot positions
- q0 = a2m(x0[:self.rmodel.nq])
- pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
- pinocchio.updateFramePlacements(self.rmodel, self.rdata)
- com0 = m2a(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 = []
-
- # Creating the action model for the CoM task
- comForwardModels = [
- self.createSwingFootModel(
- timeStep,
- [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- ) for k in range(numKnots)
- ]
- comForwardTermModel = self.createSwingFootModel(timeStep,
- [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- com0 + [comGoTo, 0., 0.])
- comForwardTermModel.differential.costs['comTrack'].weight = 1e6
-
- comBackwardModels = [
- self.createSwingFootModel(
- timeStep,
- [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- ) for k in range(numKnots)
- ]
- comBackwardTermModel = self.createSwingFootModel(timeStep,
- [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- com0 + [-comGoTo, 0., 0.])
- comBackwardTermModel.differential.costs['comTrack'].weight = 1e6
-
- # Adding the CoM tasks
- comModels += comForwardModels + [comForwardTermModel]
- comModels += comBackwardModels + [comBackwardTermModel]
-
- # Defining the shooting problem
- problem = ShootingProblem(x0, comModels, comModels[-1])
- return problem
-
- def createWalkingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
- """ Create a shooting problem for a simple walking gait.
-
- :param x0: initial state
- :param stepLength: step length
- :param stepHeight: step height
- :param timeStep: step time for each knot
- :param stepKnots: number of knots for step phases
- :param supportKnots: number of knots for double support phases
- :return shooting problem
- """
- # Compute the current foot positions
- q0 = a2m(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
- comRef = m2a(rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
-
- # Defining the action models along the time instances
- loco3dModel = []
- doubleSupport = [
- self.createSwingFootModel(
- timeStep,
- [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- ) for k in range(supportKnots)
- ]
- if self.firstStep is True:
- rhStep = self.createFootstepModels(comRef, [rhFootPos0], 0.5 * stepLength, stepHeight, timeStep, stepKnots,
- [self.lfFootId, self.rfFootId, self.lhFootId], [self.rhFootId])
- rfStep = self.createFootstepModels(comRef, [rfFootPos0], 0.5 * stepLength, stepHeight, timeStep, stepKnots,
- [self.lfFootId, self.lhFootId, self.rhFootId], [self.rfFootId])
- self.firstStep = False
- else:
- rhStep = self.createFootstepModels(comRef, [rhFootPos0], stepLength, stepHeight, timeStep, stepKnots,
- [self.lfFootId, self.rfFootId, self.lhFootId], [self.rhFootId])
- rfStep = self.createFootstepModels(comRef, [rfFootPos0], stepLength, stepHeight, timeStep, stepKnots,
- [self.lfFootId, self.lhFootId, self.rhFootId], [self.rfFootId])
- lhStep = self.createFootstepModels(comRef, [lhFootPos0], stepLength, stepHeight, timeStep, stepKnots,
- [self.lfFootId, self.rfFootId, self.rhFootId], [self.lhFootId])
- lfStep = self.createFootstepModels(comRef, [lfFootPos0], stepLength, stepHeight, timeStep, stepKnots,
- [self.rfFootId, self.lhFootId, self.rhFootId], [self.lfFootId])
-
- loco3dModel += doubleSupport + rhStep + rfStep
- loco3dModel += doubleSupport + lhStep + lfStep
-
- problem = ShootingProblem(x0, loco3dModel, loco3dModel[-1])
- return problem
-
- def createTrottingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
- """ Create a shooting problem for a simple trotting gait.
-
- :param x0: initial state
- :param stepLength: step length
- :param stepHeight: step height
- :param timeStep: step time for each knot
- :param stepKnots: number of knots for step phases
- :param supportKnots: number of knots for double support phases
- :return shooting problem
- """
- # Compute the current foot positions
- q0 = a2m(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
- comRef = m2a(rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
-
- # Defining the action models along the time instances
- loco3dModel = []
- doubleSupport = [
- self.createSwingFootModel(
- timeStep,
- [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- ) for k in range(supportKnots)
- ]
- if self.firstStep is True:
- rflhStep = self.createFootstepModels(comRef, [rfFootPos0, lhFootPos0], 0.5 * stepLength, stepHeight,
- timeStep, stepKnots, [self.lfFootId, self.rhFootId],
- [self.rfFootId, self.lhFootId])
- self.firstStep = False
- else:
- rflhStep = self.createFootstepModels(comRef, [rfFootPos0, lhFootPos0], stepLength, stepHeight, timeStep,
- stepKnots, [self.lfFootId, self.rhFootId],
- [self.rfFootId, self.lhFootId])
- lfrhStep = self.createFootstepModels(comRef, [lfFootPos0, rhFootPos0], stepLength, stepHeight, timeStep,
- stepKnots, [self.rfFootId, self.lhFootId], [self.lfFootId, self.rhFootId])
-
- loco3dModel += doubleSupport + rflhStep
- loco3dModel += doubleSupport + lfrhStep
-
- problem = ShootingProblem(x0, loco3dModel, loco3dModel[-1])
- return problem
-
- def createPacingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
- """ Create a shooting problem for a simple pacing gait.
-
- :param x0: initial state
- :param stepLength: step length
- :param stepHeight: step height
- :param timeStep: step time for each knot
- :param stepKnots: number of knots for step phases
- :param supportKnots: number of knots for double support phases
- :return shooting problem
- """
- # Compute the current foot positions
- q0 = a2m(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
- comRef = m2a(rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
-
- # Defining the action models along the time instances
- loco3dModel = []
- doubleSupport = [
- self.createSwingFootModel(
- timeStep,
- [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- ) for k in range(supportKnots)
- ]
- if self.firstStep is True:
- rightSteps = self.createFootstepModels(comRef, [rfFootPos0, rhFootPos0], 0.5 * stepLength, stepHeight,
- timeStep, stepKnots, [self.lfFootId, self.lhFootId],
- [self.rfFootId, self.rhFootId])
- self.firstStep = False
- else:
- rightSteps = self.createFootstepModels(comRef, [rfFootPos0, rhFootPos0], stepLength, stepHeight, timeStep,
- stepKnots, [self.lfFootId, self.lhFootId],
- [self.rfFootId, self.rhFootId])
- leftSteps = self.createFootstepModels(comRef, [lfFootPos0, lhFootPos0], stepLength, stepHeight, timeStep,
- stepKnots, [self.rfFootId, self.rhFootId],
- [self.lfFootId, self.lhFootId])
-
- loco3dModel += doubleSupport + rightSteps
- loco3dModel += doubleSupport + leftSteps
-
- problem = ShootingProblem(x0, loco3dModel, loco3dModel[-1])
- return problem
-
- def createBoundingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
- """ Create a shooting problem for a simple bounding gait.
-
- :param x0: initial state
- :param stepLength: step length
- :param stepHeight: step height
- :param timeStep: step time for each knot
- :param stepKnots: number of knots for step phases
- :param supportKnots: number of knots for double support phases
- :return shooting problem
- """
- # Compute the current foot positions
- q0 = a2m(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
- comRef = m2a(rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
-
- # Defining the action models along the time instances
- loco3dModel = []
- doubleSupport = [
- self.createSwingFootModel(timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId])
- for k in range(supportKnots)
- ]
- hindSteps = self.createFootstepModels(comRef, [lfFootPos0, rfFootPos0], stepLength, stepHeight, timeStep,
- stepKnots, [self.lhFootId, self.rhFootId],
- [self.lfFootId, self.rfFootId])
- frontSteps = self.createFootstepModels(comRef, [lhFootPos0, rhFootPos0], stepLength, stepHeight, timeStep,
- stepKnots, [self.lfFootId, self.rfFootId],
- [self.lhFootId, self.rhFootId])
-
- loco3dModel += doubleSupport + hindSteps
- loco3dModel += doubleSupport + frontSteps
-
- problem = ShootingProblem(x0, loco3dModel, loco3dModel[-1])
- return problem
-
- def createJumpingProblem(self, x0, jumpHeight, timeStep):
- 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
- comRef = m2a(rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
- comRef[2] = 0.5325
-
- takeOffKnots = 30
- flyingKnots = 30
-
- loco3dModel = []
- takeOff = [
- self.createSwingFootModel(
- timeStep,
- [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
- ) for k in range(takeOffKnots)
- ]
- flyingPhase = [
- self.createSwingFootModel(timeStep, [],
- np.array([0., 0., jumpHeight * (k + 1) / flyingKnots]) + comRef)
- for k in range(flyingKnots)
- ]
-
- loco3dModel += takeOff
- loco3dModel += flyingPhase
-
- problem = ShootingProblem(x0, loco3dModel, loco3dModel[-1])
- return problem
-
- def createFootstepModels(self, comPos0, feetPos0, stepLength, stepHeight, timeStep, numKnots, supportFootIds,
- swingFootIds):
- """ Action models for a footstep phase.
-
- :param comPos0, initial CoM position
- :param feetPos0: initial position of the swinging feet
- :param stepLength: step length
- :param stepHeight: step height
- :param timeStep: time step
- :param numKnots: number of knots for the footstep phase
- :param supportFootIds: Ids of the supporting feet
- :param swingFootIds: Ids of the swinging foot
- :return footstep action models
- """
- numLegs = len(supportFootIds) + len(swingFootIds)
- comPercentage = float(len(swingFootIds)) / numLegs
-
- # Action models for the foot swing
- footSwingModel = []
- for k in range(numKnots):
- swingFootTask = []
- for i, p in zip(swingFootIds, feetPos0):
- # Defining a foot swing task given the step length
- # resKnot = numKnots % 2
- phKnots = numKnots / 2
- if k < phKnots:
- dp = a2m([[stepLength * (k + 1) / numKnots, 0., stepHeight * k / phKnots]])
- elif k == phKnots:
- dp = a2m([[stepLength * (k + 1) / numKnots, 0., stepHeight]])
- else:
- dp = a2m([[stepLength * (k + 1) / numKnots, 0., stepHeight * (1 - float(k - phKnots) / phKnots)]])
- tref = np.asmatrix(p + dp)
-
- swingFootTask += [TaskSE3(pinocchio.SE3(np.eye(3), tref), i)]
-
- # Adding an action model for this knot
- comTask = np.array([stepLength * (k + 1) / numKnots, 0., 0.]) * comPercentage + comPos0
- footSwingModel += [
- self.createSwingFootModel(timeStep, supportFootIds, comTask=comTask, swingFootTask=swingFootTask)
- ]
- # Action model for the foot switch
- footSwitchModel = self.createFootSwitchModel(supportFootIds, swingFootTask)
-
- # Updating the current foot position for next step
- comPos0 += np.array([stepLength * comPercentage, 0., 0.])
- for p in feetPos0:
- p += a2m([[stepLength, 0., 0.]])
- return footSwingModel + [footSwitchModel]
-
- def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None):
- """ Action model for a swing foot phase.
-
- :param timeStep: step duration of the action model
- :param supportFootIds: Ids of the constrained feet
- :param comTask: CoM task
- :param swingFootTask: swinging foot task
- :return action model for a swing foot phase
- """
- # Creating the action model for floating-base systems. A walker system
- # is by default a floating-base system
- actModel = ActuationModelFreeFloating(self.rmodel)
-
- # Creating a 3D multi-contact model, and then including the supporting
- # foot
- contactModel = ContactModelMultiple(self.rmodel)
- for i in supportFootIds:
- supportContactModel = ContactModel3D(self.rmodel, i, ref=[0., 0., 0.], gains=[0., 0.])
- contactModel.addContact('contact_' + str(i), supportContactModel)
-
- # Creating the cost model for a contact phase
- costModel = CostModelSum(self.rmodel, actModel.nu)
- if isinstance(comTask, np.ndarray):
- comTrack = CostModelCoM(self.rmodel, comTask, actModel.nu)
- costModel.addCost("comTrack", comTrack, 1e4)
- if swingFootTask is not None:
- for i in swingFootTask:
- footTrack = CostModelFrameTranslation(self.rmodel, i.frameId, m2a(i.oXf.translation), actModel.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)
- stateReg = CostModelState(self.rmodel, self.state, self.rmodel.defaultState, actModel.nu,
- ActivationModelWeightedQuad(stateWeights**2))
- ctrlReg = CostModelControl(self.rmodel, actModel.nu)
- costModel.addCost("stateReg", stateReg, 1e-1)
- costModel.addCost("ctrlReg", ctrlReg, 1e-4)
-
- # Creating the action model for the KKT dynamics with simpletic Euler
- # integration scheme
- dmodel = DifferentialActionModelFloatingInContact(self.rmodel, actModel, contactModel, costModel)
- model = IntegratedActionModelEuler(dmodel)
- model.timeStep = timeStep
- return model
-
- def createFootSwitchModel(self, supportFootId, swingFootTask):
- """ Action model for a foot switch phase.
-
- :param supportFootIds: Ids of the constrained feet
- :param swingFootTask: swinging foot task
- :return action model for a foot switch phase
- """
- model = self.createSwingFootModel(0., supportFootId, swingFootTask=swingFootTask)
-
- for i in swingFootTask:
- impactFootVelCost = CostModelFrameVelocity(self.rmodel, i.frameId)
- model.differential.costs.addCost('impactVel_' + str(i), impactFootVelCost, 1e4)
- model.differential.costs['impactVel_' + str(i)].weight = 1e6
- model.differential.costs['footTrack_' + str(i)].weight = 1e7
- model.differential.costs['stateReg'].weight = 1e1
- model.differential.costs['ctrlReg'].weight = 1e-3
- return model
-
-
# Loading the HyQ model
-hyq = loadHyQ()
-if WITHDISPLAY:
- hyq.initDisplay(loadModel=True)
-
-rmodel = hyq.model
-rdata = rmodel.createData()
+hyq = example_robot_data.loadHyQ()
# Defining the initial state of the robot
-q0 = rmodel.referenceConfigurations['half_sitting'].copy()
-v0 = pinocchio.utils.zero(rmodel.nv)
-x0 = m2a(np.concatenate([q0, v0]))
+q0 = hyq.model.referenceConfigurations['half_sitting'].copy()
+v0 = pinocchio.utils.zero(hyq.model.nv)
+x0 = np.concatenate([q0, v0])
# Setting up the 3d walking problem
lfFoot = 'lf_foot'
rfFoot = 'rf_foot'
lhFoot = 'lh_foot'
rhFoot = 'rh_foot'
-gait = SimpleQuadrupedalGaitProblem(rmodel, lfFoot, rfFoot, lhFoot, rhFoot)
+gait = SimpleQuadrupedalGaitProblem(hyq.model, lfFoot, rfFoot, lhFoot, rhFoot)
# Setting up all tasks
GAITPHASES = [{
@@ -578,43 +71,40 @@ for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
- ddp[i] = SolverDDP(
+ ddp[i] = crocoddyl.SolverDDP(
gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
elif key == 'trotting':
# Creating a trotting problem
- ddp[i] = SolverDDP(
+ ddp[i] = crocoddyl.SolverDDP(
gait.createTrottingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
elif key == 'pacing':
# Creating a pacing problem
- ddp[i] = SolverDDP(
+ ddp[i] = crocoddyl.SolverDDP(
gait.createPacingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
elif key == 'bounding':
# Creating a bounding problem
- ddp[i] = SolverDDP(
+ ddp[i] = crocoddyl.SolverDDP(
gait.createBoundingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
elif key == 'jumping':
# Creating a jumping problem
- ddp[i] = SolverDDP(gait.createJumpingProblem(x0, value['jumpHeight'], value['timeStep']))
+ ddp[i] = crocoddyl.SolverDDP(gait.createJumpingProblem(x0, value['jumpHeight'], value['timeStep']))
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
- ddp[i].callback = [CallbackDDPLogger(), CallbackDDPVerbose()]
if WITHDISPLAY:
- ddp[i].callback.append(CallbackSolverDisplay(hyq, 4, 1, cameraTF))
+ ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(hyq, 4, 4, cameraTF)])
+ else:
+ ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the DDP solver
ddp[i].th_stop = 1e-9
- ddp[i].solve(maxiter=1000,
- regInit=.1,
- init_xs=[rmodel.defaultState] * len(ddp[i].models()),
- init_us=[
- m.differential.quasiStatic(d.differential, rmodel.defaultState)
- for m, d in zip(ddp[i].models(), ddp[i].datas())[:-1]
- ])
+ 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)
# Defining the final state as initial one for the next phase
x0 = ddp[i].xs[-1]
@@ -622,7 +112,7 @@ for i, phase in enumerate(GAITPHASES):
# Display the entire motion
if WITHDISPLAY:
for i, phase in enumerate(GAITPHASES):
- displayTrajectory(hyq, ddp[i].xs, ddp[i].models()[0].timeStep)
+ crocoddyl.displayTrajectory(hyq, ddp[i].xs, ddp[i].models()[0].dt)
# Plotting the entire motion
if WITHPLOT:
@@ -631,4 +121,4 @@ if WITHPLOT:
for i, phase in enumerate(GAITPHASES):
xs.extend(ddp[i].xs)
us.extend(ddp[i].us)
- plotSolution(rmodel, xs, us)
+ plotSolution(hyq.model, xs, us)
diff --git a/examples/salto.py b/examples/salto.py
index 4adf2e26e49756774140676eae452d47ca21e52f..f854351b6b9d27d9e6116ba20564b2e943f203c2 100644
--- a/examples/salto.py
+++ b/examples/salto.py
@@ -209,10 +209,11 @@ ddp.alphas = [4**(-n) for n in range(10)]
ddp.callback = [CallbackDDPVerbose()]
ddp.th_stop = 1e-4
us0 = [
- m.differential.quasiStatic(d.differential, rmodel.defaultState) for m, d in zip(ddp.models(), ddp.datas())[:imp]
+ m.differential.quasiStatic(d.differential, rmodel.defaultState)
+ for m, d in list(zip(ddp.models(), ddp.datas()))[:imp]
] + [np.zeros(0)] + [
m.differential.quasiStatic(d.differential, rmodel.defaultState)
- for m, d in zip(ddp.models(), ddp.datas())[imp + 1:-1]
+ for m, d in list(zip(ddp.models(), ddp.datas()))[imp + 1:-1]
]
if PHASE_ITERATIONS[PHASE_NAME] > 0:
@@ -268,7 +269,7 @@ ddp.th_stop = 1e-4
ddp.xs = xsddp + [rmodel.defaultState] * (len(models) - len(xsddp))
ddp.us = usddp + [
np.zeros(0) if isinstance(m, ActionModelImpact) else m.differential.quasiStatic(
- d.differential, rmodel.defaultState) for m, d in zip(ddp.models(), ddp.datas())[len(usddp):-1]
+ d.differential, rmodel.defaultState) for m, d in list(zip(ddp.models(), ddp.datas()))[len(usddp):-1]
]
impact.costs['track30'].weight = 1e6
impact.costs['track16'].weight = 1e6
diff --git a/examples/talos_arm.py b/examples/talos_arm.py
index 58cb45f76943095dac5bbaeecfde0c05b5960aa0..fd23d5f91b0f890ed7677d1a60179ee31b0b2943 100644
--- a/examples/talos_arm.py
+++ b/examples/talos_arm.py
@@ -1,11 +1,9 @@
import sys
-import numpy as np
+import crocoddyl
import pinocchio
-from crocoddyl import (CallbackDDPLogger, CallbackDDPVerbose, CallbackSolverDisplay, CostModelControl,
- CostModelFramePlacement, CostModelState, CostModelSum, DifferentialActionModelFullyActuated,
- IntegratedActionModelEuler, ShootingProblem, SolverDDP, StatePinocchio, loadTalosArm,
- plotDDPConvergence, plotOCSolution)
+import numpy as np
+import example_robot_data
WITHDISPLAY = 'disp' in sys.argv
WITHPLOT = 'plot' in sys.argv
@@ -16,74 +14,67 @@ WITHPLOT = 'plot' in sys.argv
# Finally, we use an Euler sympletic integration scheme.
# First, let's load the Pinocchio model for the Talos arm.
-robot = loadTalosArm()
-rmodel = robot.model
-rdata = rmodel.createData()
+talos_arm = example_robot_data.loadTalosArm()
+robot_model = talos_arm.model
# Create a cost model per the running and terminal action model.
-runningCostModel = CostModelSum(rmodel)
-terminalCostModel = CostModelSum(rmodel)
+state = crocoddyl.StateMultibody(robot_model)
+runningCostModel = crocoddyl.CostModelSum(state)
+terminalCostModel = crocoddyl.CostModelSum(state)
# Note that we need to include a cost model (i.e. set of cost functions) in
# order to fully define the action model for our optimal control problem.
# For this particular example, we formulate three running-cost functions:
# goal-tracking cost, state and control regularization; and one terminal-cost:
# goal cost. First, let's create the common cost functions.
-frameName = 'gripper_left_joint'
-state = StatePinocchio(rmodel)
-SE3ref = pinocchio.SE3(np.eye(3), np.array([[.0], [.0], [.4]]))
-goalTrackingCost = CostModelFramePlacement(rmodel, nu=rmodel.nv, frame=rmodel.getFrameId(frameName), ref=SE3ref)
-xRegCost = CostModelState(rmodel, state, ref=state.zero(), nu=rmodel.nv)
-uRegCost = CostModelControl(rmodel, nu=rmodel.nv)
+Mref = crocoddyl.FramePlacement(robot_model.getFrameId("gripper_left_joint"),
+ pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]])))
+goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref)
+xRegCost = crocoddyl.CostModelState(state)
+uRegCost = crocoddyl.CostModelControl(state)
# Then let's added the running and terminal cost functions
-runningCostModel.addCost(name="pos", weight=1e-3, cost=goalTrackingCost)
-runningCostModel.addCost(name="regx", weight=1e-7, cost=xRegCost)
-runningCostModel.addCost(name="regu", weight=1e-7, cost=uRegCost)
-terminalCostModel.addCost(name="pos", weight=1, cost=goalTrackingCost)
+runningCostModel.addCost("gripperPose", goalTrackingCost, 1e-3)
+runningCostModel.addCost("xReg", xRegCost, 1e-7)
+runningCostModel.addCost("uReg", uRegCost, 1e-7)
+terminalCostModel.addCost("gripperPose", goalTrackingCost, 1)
# Next, we need to create an action model for running and terminal knots. The
# forward dynamics (computed using ABA) are implemented
# inside DifferentialActionModelFullyActuated.
-runningModel = IntegratedActionModelEuler(DifferentialActionModelFullyActuated(rmodel, runningCostModel))
-terminalModel = IntegratedActionModelEuler(DifferentialActionModelFullyActuated(rmodel, terminalCostModel))
-
-# Defining the time duration for running action models and the terminal one
dt = 1e-3
-runningModel.timeStep = dt
+runningModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeFwdDynamics(state, runningCostModel), dt)
+runningModel.differential.armature = np.matrix([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T
+terminalModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeFwdDynamics(state, terminalCostModel), 0.)
+terminalModel.differential.armature = np.matrix([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T
# For this optimal control problem, we define 250 knots (or running action
# models) plus a terminal knot
T = 250
-q0 = [0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]
-x0 = np.hstack([q0, np.zeros(rmodel.nv)])
-problem = ShootingProblem(x0, [runningModel] * T, terminalModel)
+q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T
+x0 = np.concatenate([q0, pinocchio.utils.zero(robot_model.nv)])
+problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
-ddp = SolverDDP(problem)
+ddp = crocoddyl.SolverDDP(problem)
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
-ddp.callback = [CallbackDDPVerbose()]
-if WITHPLOT:
- ddp.callback.append(CallbackDDPLogger())
if WITHDISPLAY:
- ddp.callback.append(CallbackSolverDisplay(robot, 4, 1, cameraTF))
+ ddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackSolverDisplay(talos_arm, 4, 4, cameraTF)])
+else:
+ ddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving it with the DDP algorithm
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.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)
# Visualizing the solution in gepetto-viewer
if WITHDISPLAY:
- from crocoddyl.diagnostic import displayTrajectory
- displayTrajectory(robot, ddp.xs, runningModel.timeStep)
-
-# Printing the reached position
-frame_idx = rmodel.getFrameId(frameName)
-print
-print("The reached pose by the wrist is")
-print(ddp.datas()[-1].differential.pinocchio.oMf[frame_idx])
+ crocoddyl.displayTrajectory(talos_arm, ddp.xs, runningModel.dt)
diff --git a/include/crocoddyl/core/action-base.hpp b/include/crocoddyl/core/action-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0180e8866d1b7204f521672a1afa31d77cbb052a
--- /dev/null
+++ b/include/crocoddyl/core/action-base.hpp
@@ -0,0 +1,138 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTION_BASE_HPP_
+#define CROCODDYL_CORE_ACTION_BASE_HPP_
+
+#include "crocoddyl/core/state-base.hpp"
+#include "crocoddyl/core/utils/math.hpp"
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+
+namespace crocoddyl {
+
+struct ActionDataAbstract; // forward declaration
+
+class ActionModelAbstract {
+ public:
+ ActionModelAbstract(StateAbstract& state, unsigned int const& nu, unsigned int const& nr = 1);
+ virtual ~ActionModelAbstract();
+
+ virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u) = 0;
+ virtual 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) = 0;
+ virtual boost::shared_ptr<ActionDataAbstract> createData();
+
+ void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
+ void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
+
+ void quasicStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<Eigen::VectorXd> u,
+ const Eigen::Ref<const Eigen::VectorXd>& x, unsigned int const& maxiter = 100,
+ const double& tol = 1e-9);
+
+ unsigned int const& get_nu() const;
+ unsigned int const& get_nr() const;
+ StateAbstract& get_state() const;
+
+ protected:
+ unsigned int nu_;
+ unsigned int nr_;
+ StateAbstract& state_;
+ Eigen::VectorXd unone_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u = Eigen::VectorXd()) {
+ if (u.size() == 0) {
+ calc(data, x);
+ } else {
+ calc(data, x, u);
+ }
+ }
+
+ void calcDiff_wrap(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u, const bool& recalc) {
+ calcDiff(data, x, u, recalc);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u) {
+ calcDiff(data, x, u, true);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::VectorXd& x) {
+ calcDiff(data, x, unone_, true);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::VectorXd& x, const bool& recalc) {
+ calcDiff(data, x, unone_, recalc);
+ }
+
+ Eigen::VectorXd quasicStatic_wrap(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::VectorXd& x,
+ unsigned int const& maxiter = 100, const double& tol = 1e-9) {
+ Eigen::VectorXd u(nu_);
+ u.setZero();
+ quasicStatic(data, u, x, maxiter, tol);
+ return u;
+ }
+
+#endif
+};
+
+struct ActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ explicit ActionDataAbstract(Model* const model)
+ : cost(0.),
+ xnext(model->get_state().get_nx()),
+ r(model->get_nr()),
+ Fx(model->get_state().get_ndx(), model->get_state().get_ndx()),
+ Fu(model->get_state().get_ndx(), model->get_nu()),
+ 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()) {
+ xnext.setZero();
+ r.setZero();
+ Fx.setZero();
+ Fu.setZero();
+ Lx.setZero();
+ Lu.setZero();
+ Lxx.setZero();
+ Lxu.setZero();
+ Luu.setZero();
+ }
+
+ const double& get_cost() const { return cost; }
+ const Eigen::VectorXd& get_xnext() const { return xnext; }
+ const Eigen::VectorXd& get_r() const { return r; }
+ const Eigen::VectorXd& get_Lx() const { return Lx; }
+ const Eigen::VectorXd& get_Lu() const { return Lu; }
+ const Eigen::MatrixXd& get_Lxx() const { return Lxx; }
+ const Eigen::MatrixXd& get_Lxu() const { return Lxu; }
+ const Eigen::MatrixXd& get_Luu() const { return Luu; }
+ const Eigen::MatrixXd& get_Fx() const { return Fx; }
+ const Eigen::MatrixXd& get_Fu() const { return Fu; }
+
+ double cost;
+ Eigen::VectorXd xnext;
+ Eigen::VectorXd r;
+ Eigen::MatrixXd Fx;
+ Eigen::MatrixXd Fu;
+ Eigen::VectorXd Lx;
+ Eigen::VectorXd Lu;
+ Eigen::MatrixXd Lxx;
+ Eigen::MatrixXd Lxu;
+ Eigen::MatrixXd Luu;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_ACTION_BASE_HPP_
diff --git a/include/crocoddyl/core/actions/diff-lqr.hpp b/include/crocoddyl/core/actions/diff-lqr.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1de5e0394a288c2fcf343f5e8879b901b8a8c94c
--- /dev/null
+++ b/include/crocoddyl/core/actions/diff-lqr.hpp
@@ -0,0 +1,63 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
+#define CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
+
+#include "crocoddyl/core/diff-action-base.hpp"
+#include "crocoddyl/core/states/euclidean.hpp"
+
+namespace crocoddyl {
+
+struct DifferentialActionDataLQR; // forward declaration
+
+class DifferentialActionModelLQR : public DifferentialActionModelAbstract {
+ public:
+ DifferentialActionModelLQR(unsigned int const& nq, unsigned int const& nu, bool drift_free = true);
+ ~DifferentialActionModelLQR();
+
+ void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u);
+ void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
+ const bool& recalc = true);
+ boost::shared_ptr<DifferentialActionDataAbstract> createData();
+
+ Eigen::MatrixXd Fq_;
+ Eigen::MatrixXd Fv_;
+ Eigen::MatrixXd Fu_;
+ Eigen::VectorXd f0_;
+ Eigen::MatrixXd Lxx_;
+ Eigen::MatrixXd Lxu_;
+ Eigen::MatrixXd Luu_;
+ Eigen::VectorXd lx_;
+ Eigen::VectorXd lu_;
+
+ private:
+ bool drift_free_;
+};
+
+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_;
+ Fu = model->Fu_;
+ Lxx = model->Lxx_;
+ Luu = model->Luu_;
+ Lxu = model->Lxu_;
+ }
+ ~DifferentialActionDataLQR() {}
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
diff --git a/include/crocoddyl/core/actions/lqr.hpp b/include/crocoddyl/core/actions/lqr.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d2908b0e43b24c6c42b79c2918c209f3c1654899
--- /dev/null
+++ b/include/crocoddyl/core/actions/lqr.hpp
@@ -0,0 +1,56 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTIONS_LQR_HPP_
+#define CROCODDYL_CORE_ACTIONS_LQR_HPP_
+
+#include "crocoddyl/core/action-base.hpp"
+#include "crocoddyl/core/states/euclidean.hpp"
+
+namespace crocoddyl {
+
+class ActionModelLQR : public ActionModelAbstract {
+ public:
+ ActionModelLQR(unsigned int const& nx, unsigned int const& nu, bool drift_free = true);
+ ~ActionModelLQR();
+
+ 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();
+
+ Eigen::MatrixXd Fx_;
+ Eigen::MatrixXd Fu_;
+ Eigen::VectorXd f0_;
+ Eigen::MatrixXd Lxx_;
+ Eigen::MatrixXd Lxu_;
+ Eigen::MatrixXd Luu_;
+ Eigen::VectorXd lx_;
+ Eigen::VectorXd lu_;
+
+ private:
+ bool drift_free_;
+};
+
+struct ActionDataLQR : public ActionDataAbstract {
+ template <typename Model>
+ explicit ActionDataLQR(Model* const model) : ActionDataAbstract(model) {
+ // Setting the linear model and quadratic cost here because they are constant
+ Fx = model->Fx_;
+ Fu = model->Fu_;
+ Lxx = model->Lxx_;
+ Luu = model->Luu_;
+ Lxu = model->Lxu_;
+ }
+ ~ActionDataLQR() {}
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_ACTIONS_LQR_HPP_
diff --git a/include/crocoddyl/core/actions/unicycle.hpp b/include/crocoddyl/core/actions/unicycle.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a16ddf9c7a86b7f218b89c450c33bd1196567e4
--- /dev/null
+++ b/include/crocoddyl/core/actions/unicycle.hpp
@@ -0,0 +1,45 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
+#define CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
+
+#include "crocoddyl/core/action-base.hpp"
+#include "crocoddyl/core/states/euclidean.hpp"
+
+namespace crocoddyl {
+
+class ActionModelUnicycle : public ActionModelAbstract {
+ public:
+ ActionModelUnicycle();
+ ~ActionModelUnicycle();
+
+ 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();
+
+ const Eigen::Vector2d& get_cost_weights() const;
+ void set_cost_weights(const Eigen::Vector2d& weights);
+
+ private:
+ Eigen::Vector2d cost_weights_;
+ double dt_;
+};
+
+struct ActionDataUnicycle : public ActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ explicit ActionDataUnicycle(Model* const model) : ActionDataAbstract(model) {}
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_ACTIONS_UNICYCLE_HPP_
diff --git a/include/crocoddyl/core/activation-base.hpp b/include/crocoddyl/core/activation-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5411d708ee05d7158604948ab633f80f30947042
--- /dev/null
+++ b/include/crocoddyl/core/activation-base.hpp
@@ -0,0 +1,68 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTIVATION_BASE_HPP_
+#define CROCODDYL_CORE_ACTIVATION_BASE_HPP_
+
+#include <Eigen/Dense>
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+
+namespace crocoddyl {
+
+struct ActivationDataAbstract; // forward declaration
+
+class ActivationModelAbstract {
+ public:
+ explicit ActivationModelAbstract(unsigned int const& nr);
+ virtual ~ActivationModelAbstract();
+
+ virtual void calc(const boost::shared_ptr<ActivationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& r) = 0;
+ virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& r, const bool& recalc = true) = 0;
+ virtual boost::shared_ptr<ActivationDataAbstract> createData();
+
+ unsigned int const& get_nr() const;
+
+ protected:
+ unsigned int nr_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::VectorXd& r) { calc(data, r); }
+
+ void calcDiff_wrap(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::VectorXd& r,
+ const bool& recalc) {
+ calcDiff(data, r, recalc);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::VectorXd& r) {
+ calcDiff(data, r, true);
+ }
+
+#endif
+};
+
+struct ActivationDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Activation>
+ explicit ActivationDataAbstract(Activation* const activation)
+ : a_value(0.),
+ Ar(Eigen::VectorXd::Zero(activation->get_nr())),
+ Arr(Eigen::MatrixXd::Zero(activation->get_nr(), activation->get_nr())) {}
+
+ double a_value;
+ Eigen::VectorXd Ar;
+ Eigen::MatrixXd Arr;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_ACTIVATION_BASE_HPP_
diff --git a/include/crocoddyl/core/activations/inequality.hpp b/include/crocoddyl/core/activations/inequality.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..23dc98d1db1fc0be8a47573b20caeda2190ae3d7
--- /dev/null
+++ b/include/crocoddyl/core/activations/inequality.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTIVATIONS_INEQUALITY_HPP_
+#define CROCODDYL_CORE_ACTIVATIONS_INEQUALITY_HPP_
+
+// TODO: ActivationModelInequality ActivationDataInequality
+
+#endif // CROCODDYL_CORE_ACTIVATIONS_INEQUALITY_HPP_
diff --git a/include/crocoddyl/core/activations/quadratic.hpp b/include/crocoddyl/core/activations/quadratic.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d282511f1fb05fa1ee82dbbe7ef0b5260124ff5
--- /dev/null
+++ b/include/crocoddyl/core/activations/quadratic.hpp
@@ -0,0 +1,29 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
+#define CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
+
+#include "crocoddyl/core/activation-base.hpp"
+
+namespace crocoddyl {
+
+class ActivationModelQuad : public ActivationModelAbstract {
+ public:
+ explicit ActivationModelQuad(unsigned int const& nr);
+ ~ActivationModelQuad();
+
+ void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& r);
+ void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& r,
+ const bool& recalc = true);
+ boost::shared_ptr<ActivationDataAbstract> createData();
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_ACTIVATIONS_QUADRATIC_HPP_
diff --git a/include/crocoddyl/core/activations/smooth-abs.hpp b/include/crocoddyl/core/activations/smooth-abs.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a7a99eddb68298785aaeff5e15f371b2b7dd4998
--- /dev/null
+++ b/include/crocoddyl/core/activations/smooth-abs.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_
+#define CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_
+
+// TODO: ActivationModelSmoothAbs ActivationDataSmoothAbs
+
+#endif // CROCODDYL_CORE_ACTIVATIONS_SMOOTH_ABS_HPP_
diff --git a/include/crocoddyl/core/activations/weighted-quadratic.hpp b/include/crocoddyl/core/activations/weighted-quadratic.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a3c5206525eeab89e8f47cbf9edd5aa8e3289f98
--- /dev/null
+++ b/include/crocoddyl/core/activations/weighted-quadratic.hpp
@@ -0,0 +1,48 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_
+#define CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_
+
+#include "crocoddyl/core/activation-base.hpp"
+
+namespace crocoddyl {
+
+class ActivationModelWeightedQuad : public ActivationModelAbstract {
+ public:
+ explicit ActivationModelWeightedQuad(const Eigen::VectorXd& weights);
+ ~ActivationModelWeightedQuad();
+
+ void calc(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& r);
+ void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& r,
+ const bool& recalc = true);
+ boost::shared_ptr<ActivationDataAbstract> createData();
+
+ const Eigen::VectorXd& get_weights() const;
+
+ private:
+ Eigen::VectorXd weights_;
+
+#ifndef NDEBUG
+ Eigen::MatrixXd Arr_;
+#endif
+};
+
+struct ActivationDataWeightedQuad : public ActivationDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Activation>
+ explicit ActivationDataWeightedQuad(Activation* const activation)
+ : ActivationDataAbstract(activation), Wr(Eigen::VectorXd::Zero(activation->get_nr())) {}
+
+ Eigen::VectorXd Wr;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_ACTIVATIONS_WEIGHTED_QUADRATIC_HPP_
diff --git a/include/crocoddyl/core/actuation-base.hpp b/include/crocoddyl/core/actuation-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e79221b511e02f67dffade8f2c8382390e797e2
--- /dev/null
+++ b/include/crocoddyl/core/actuation-base.hpp
@@ -0,0 +1,76 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_ACTUATION_BASE_HPP_
+#define CROCODDYL_CORE_ACTUATION_BASE_HPP_
+
+#include <Eigen/Dense>
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+#include "crocoddyl/core/state-base.hpp"
+
+namespace crocoddyl {
+
+struct ActuationDataAbstract; // forward declaration
+
+class ActuationModelAbstract {
+ public:
+ ActuationModelAbstract(StateAbstract& state, unsigned int const& nu);
+ virtual ~ActuationModelAbstract();
+
+ virtual void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u) = 0;
+ virtual void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
+ const bool& recalc = true) = 0;
+ virtual boost::shared_ptr<ActuationDataAbstract> createData();
+
+ const unsigned int& get_nu() const;
+ StateAbstract& get_state() const;
+
+ protected:
+ unsigned int nu_;
+ StateAbstract& state_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u) {
+ calc(data, x, u);
+ }
+
+ void calcDiff_wrap(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u, const bool& recalc = true) {
+ calcDiff(data, x, u, recalc);
+ }
+
+#endif
+};
+
+struct ActuationDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ 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);
+ }
+
+ Eigen::VectorXd a;
+ Eigen::MatrixXd Ax;
+ Eigen::MatrixXd Au;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_ACTUATION_BASE_HPP_
diff --git a/include/crocoddyl/core/diff-action-base.hpp b/include/crocoddyl/core/diff-action-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7894260e47776e6fa269357c476cd96a723a30bd
--- /dev/null
+++ b/include/crocoddyl/core/diff-action-base.hpp
@@ -0,0 +1,167 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
+#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>
+
+namespace crocoddyl {
+
+struct DifferentialActionDataAbstract; // forward declaration
+
+class DifferentialActionModelAbstract {
+ public:
+ DifferentialActionModelAbstract(StateAbstract& state, unsigned int const& nu, unsigned int const& nr = 1);
+ virtual ~DifferentialActionModelAbstract();
+
+ virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u) = 0;
+ virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
+ const bool& recalc = true) = 0;
+ virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
+
+ void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
+ void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x);
+
+ unsigned int const& get_nu() const;
+ unsigned int const& get_nr() const;
+ StateAbstract& get_state() const;
+
+ protected:
+ unsigned int nu_;
+ unsigned int nr_;
+ StateAbstract& state_;
+ Eigen::VectorXd unone_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u = Eigen::VectorXd()) {
+ if (u.size() == 0) {
+ calc(data, x);
+ } else {
+ calc(data, x, u);
+ }
+ }
+
+ void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u, const bool& recalc) {
+ calcDiff(data, x, u, recalc);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u) {
+ calcDiff(data, x, u, true);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x) {
+ calcDiff(data, x, unone_, true);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
+ const bool& recalc) {
+ calcDiff(data, x, unone_, recalc);
+ }
+
+#endif
+};
+
+struct DifferentialActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ 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()),
+ 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()) {
+ xout.setZero();
+ r.setZero();
+ qcur.setZero();
+ vcur.setZero();
+ Fx.setZero();
+ Fu.setZero();
+ Lx.setZero();
+ Lu.setZero();
+ Lxx.setZero();
+ Lxu.setZero();
+ 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 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
+
+#endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
diff --git a/include/crocoddyl/core/integrator/euler.hpp b/include/crocoddyl/core/integrator/euler.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5e7433289c78f1f50a04cb9dd68a45bed868249
--- /dev/null
+++ b/include/crocoddyl/core/integrator/euler.hpp
@@ -0,0 +1,66 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
+#define CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
+
+#include "crocoddyl/core/action-base.hpp"
+#include "crocoddyl/core/diff-action-base.hpp"
+
+namespace crocoddyl {
+
+class IntegratedActionModelEuler : public ActionModelAbstract {
+ public:
+ IntegratedActionModelEuler(DifferentialActionModelAbstract* const model, const double& time_step = 1e-3,
+ const bool& with_cost_residual = true);
+ ~IntegratedActionModelEuler();
+
+ 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();
+
+ DifferentialActionModelAbstract* get_differential() const;
+ const double& get_dt() const;
+ void set_dt(double dt);
+
+ private:
+ DifferentialActionModelAbstract* differential_;
+ double time_step_;
+ double time_step2_;
+ bool with_cost_residual_;
+};
+
+struct IntegratedActionDataEuler : public ActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ explicit IntegratedActionDataEuler(Model* const model) : ActionDataAbstract(model) {
+ differential = model->get_differential()->createData();
+ const unsigned int& ndx = model->get_state().get_ndx();
+ const unsigned int& nu = model->get_nu();
+ dx = Eigen::VectorXd::Zero(ndx);
+ ddx_dx = Eigen::MatrixXd::Zero(ndx, ndx);
+ ddx_du = Eigen::MatrixXd::Zero(ndx, nu);
+ dxnext_dx = Eigen::MatrixXd::Zero(ndx, ndx);
+ dxnext_ddx = Eigen::MatrixXd::Zero(ndx, ndx);
+ }
+ ~IntegratedActionDataEuler() {}
+
+ boost::shared_ptr<DifferentialActionDataAbstract> differential;
+ Eigen::VectorXd dx;
+ Eigen::MatrixXd ddx_dx;
+ Eigen::MatrixXd ddx_du;
+ Eigen::MatrixXd dxnext_dx;
+ Eigen::MatrixXd dxnext_ddx;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
diff --git a/include/crocoddyl/core/integrator/rk4.hpp b/include/crocoddyl/core/integrator/rk4.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0fc6e5da7a9782e42b346d7029f05018ad3a176c
--- /dev/null
+++ b/include/crocoddyl/core/integrator/rk4.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
+#define CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
+
+// TODO: IntegratedActionModelRK4 IntegratedActionDataRK4
+
+#endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
diff --git a/include/crocoddyl/core/numdiff/action.hpp b/include/crocoddyl/core/numdiff/action.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f71cd007d91c6be943bd019f76130805bb86479
--- /dev/null
+++ b/include/crocoddyl/core/numdiff/action.hpp
@@ -0,0 +1,98 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
+#define CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
+
+#include "crocoddyl/core/action-base.hpp"
+#include <vector>
+#include <iostream>
+
+namespace crocoddyl {
+
+class ActionModelNumDiff : public ActionModelAbstract {
+ public:
+ explicit ActionModelNumDiff(ActionModelAbstract& model, bool with_gauss_approx = false);
+ ~ActionModelNumDiff();
+
+ 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();
+
+ ActionModelAbstract& get_model() const;
+ const double& get_disturbance() const;
+ bool get_with_gauss_approx();
+
+ private:
+ /**
+ * @brief Make sure that when we finite difference the Action Model, the user
+ * does not face unknown behaviour because of the finite differencing of a
+ * quaternion around pi. This behaviour might occur if CostModelState and
+ * FloatingInContact differential model are used together.
+ *
+ * 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);
+
+ ActionModelAbstract& model_;
+ bool with_gauss_approx_;
+ double disturbance_;
+};
+
+struct ActionDataNumDiff : public ActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * @brief Construct a new ActionDataNumDiff object
+ *
+ * @tparam Model is the type of the ActionModel.
+ * @param model is the object to compute the numerical differentiation from.
+ */
+ template <typename Model>
+ explicit ActionDataNumDiff(Model* const model)
+ : ActionDataAbstract(model),
+ Rx(model->get_model().get_nr(), model->get_model().get_state().get_ndx()),
+ Ru(model->get_model().get_nr(), model->get_model().get_nu()),
+ dx(model->get_model().get_state().get_ndx()),
+ du(model->get_model().get_nu()),
+ xp(model->get_model().get_state().get_nx()) {
+ Rx.setZero();
+ Ru.setZero();
+ dx.setZero();
+ du.setZero();
+ xp.setZero();
+
+ unsigned int const& ndx = model->get_model().get_state().get_ndx();
+ unsigned int const& nu = model->get_model().get_nu();
+ data_0 = model->get_model().createData();
+ for (unsigned int i = 0; i < ndx; ++i) {
+ data_x.push_back(model->get_model().createData());
+ }
+ for (unsigned int i = 0; i < nu; ++i) {
+ data_u.push_back(model->get_model().createData());
+ }
+ }
+
+ 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;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_NUMDIFF_ACTION_HPP_
diff --git a/include/crocoddyl/core/numdiff/diff-action.hpp b/include/crocoddyl/core/numdiff/diff-action.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..50f5ed4146b81c3633fd69fb4eaad3d205399e4e
--- /dev/null
+++ b/include/crocoddyl/core/numdiff/diff-action.hpp
@@ -0,0 +1,87 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
+#define CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
+
+#include "crocoddyl/core/diff-action-base.hpp"
+#include <vector>
+#include <iostream>
+
+namespace crocoddyl {
+
+class DifferentialActionModelNumDiff : public DifferentialActionModelAbstract {
+ public:
+ explicit DifferentialActionModelNumDiff(DifferentialActionModelAbstract& model, bool with_gauss_approx = false);
+ ~DifferentialActionModelNumDiff();
+
+ void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u);
+ void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
+ const bool& recalc = true);
+ boost::shared_ptr<DifferentialActionDataAbstract> createData();
+
+ DifferentialActionModelAbstract& get_model() const;
+ const double& get_disturbance() const;
+ bool get_with_gauss_approx();
+
+ private:
+ void assertStableStateFD(const Eigen::Ref<const Eigen::VectorXd>& x);
+
+ DifferentialActionModelAbstract& model_;
+ bool with_gauss_approx_;
+ double disturbance_;
+};
+
+struct DifferentialActionDataNumDiff : public DifferentialActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * @brief Construct a new ActionDataNumDiff object
+ *
+ * @tparam Model is the type of the ActionModel.
+ * @param model is the object to compute the numerical differentiation from.
+ */
+ template <typename Model>
+ explicit DifferentialActionDataNumDiff(Model* const model)
+ : DifferentialActionDataAbstract(model),
+ Rx(model->get_model().get_nr(), model->get_model().get_state().get_ndx()),
+ Ru(model->get_model().get_nr(), model->get_model().get_nu()),
+ dx(model->get_model().get_state().get_ndx()),
+ du(model->get_model().get_nu()),
+ xp(model->get_model().get_state().get_nx()) {
+ Rx.setZero();
+ Ru.setZero();
+ dx.setZero();
+ du.setZero();
+ xp.setZero();
+
+ unsigned int const& ndx = model->get_model().get_state().get_ndx();
+ unsigned int const& nu = model->get_model().get_nu();
+ data_0 = model->get_model().createData();
+ for (unsigned int i = 0; i < ndx; ++i) {
+ data_x.push_back(model->get_model().createData());
+ }
+ for (unsigned int i = 0; i < nu; ++i) {
+ data_u.push_back(model->get_model().createData());
+ }
+ }
+
+ Eigen::MatrixXd Rx;
+ Eigen::MatrixXd Ru;
+ Eigen::VectorXd dx;
+ Eigen::VectorXd du;
+ Eigen::VectorXd xp;
+ boost::shared_ptr<DifferentialActionDataAbstract> data_0;
+ std::vector<boost::shared_ptr<DifferentialActionDataAbstract> > data_x;
+ std::vector<boost::shared_ptr<DifferentialActionDataAbstract> > data_u;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_NUMDIFF_DIFF_ACTION_HPP_
diff --git a/include/crocoddyl/core/numdiff/state.hpp b/include/crocoddyl/core/numdiff/state.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..96e3dc3c19d6e45e526828bb64f4519358029e80
--- /dev/null
+++ b/include/crocoddyl/core/numdiff/state.hpp
@@ -0,0 +1,103 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_NUMDIFF_STATE_HPP_
+#define CROCODDYL_CORE_NUMDIFF_STATE_HPP_
+
+#include "crocoddyl/core/state-base.hpp"
+
+namespace crocoddyl {
+
+class StateNumDiff : public StateAbstract {
+ public:
+ explicit StateNumDiff(StateAbstract& state);
+ ~StateNumDiff();
+
+ Eigen::VectorXd zero();
+ Eigen::VectorXd rand();
+ void diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::VectorXd> dxout);
+ void integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::VectorXd> xout);
+ /**
+ * @brief This computes the Jacobian of the diff method by finite
+ * differentiation:
+ * \f{equation}{
+ * Jfirst[:,k] = diff(int(x_1, dx_dist), x_2) - diff(x_1, x_2)/disturbance
+ * \f}
+ * and
+ * \f{equation}{
+ * Jsecond[:,k] = diff(x_1, int(x_2, dx_dist)) - diff(x_1, x_2)/disturbance
+ * \f}
+ *
+ * @param Jfirst
+ * @param Jsecond
+ * @param firstsecond
+ */
+ void Jdiff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond, Jcomponent firstsecond = both);
+ /**
+ * @brief This computes the Jacobian of the integrate method by finite
+ * differentiation:
+ * \f{equation}{
+ * Jfirst[:,k] = diff( int(x, d_x), int( int(x, dx_dist), dx) )/disturbance
+ * \f}
+ * and
+ * \f{equation}{
+ * Jsecond[:,k] = diff( int(x, d_x), int( x, dx + dx_dist) )/disturbance
+ * \f}
+ *
+ * @param Jfirst
+ * @param Jsecond
+ * @param firstsecond
+ */
+ void Jintegrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond = both);
+ const double& get_disturbance() { return disturbance_; }
+
+ private:
+ /**
+ * @brief This is the state we need to compute the numerical differentiation
+ * from.
+ */
+ StateAbstract& state_;
+ /**
+ * @brief This the increment used in the finite differentiation and integration.
+ */
+ double disturbance_;
+ /**
+ * @brief This is the vector containing the small element during the finite
+ * differentiation and integration. This is a temporary variable but used
+ * quiet often. For sake of memory management we allocate it once in the
+ * constructor of this class.
+ */
+ Eigen::VectorXd dx_;
+ /**
+ * @brief This is the result of diff(x0, x1, dx0_) in the finite
+ * differentiation. This is the state difference around which to compute
+ * the jacobians of the finite difference.
+ */
+ Eigen::VectorXd dx0_;
+ /**
+ * @brief This is the result of integrate(x, dx x0_) in the finite
+ * integration. This is the state around which to compute the jacobians of
+ * the finite integrate
+ */
+ Eigen::VectorXd x0_;
+ /**
+ * @brief This is the vector containing the result of an integration. This is
+ * a temporary variable but used quiet often. For sake of memory management we
+ * allocate it once in the constructor of this class.
+ */
+ Eigen::VectorXd tmp_x_;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_NUMDIFF_STATE_HPP_
diff --git a/include/crocoddyl/core/optctrl/shooting.hpp b/include/crocoddyl/core/optctrl/shooting.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2be240f4d8254c0ff8ebb46493daed3bf9731c04
--- /dev/null
+++ b/include/crocoddyl/core/optctrl/shooting.hpp
@@ -0,0 +1,54 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
+#define CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
+
+#include <vector>
+#include "crocoddyl/core/action-base.hpp"
+
+namespace crocoddyl {
+
+class ShootingProblem {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ ShootingProblem(const Eigen::VectorXd& x0, const std::vector<ActionModelAbstract*>& running_models,
+ ActionModelAbstract* const terminal_model);
+ ~ShootingProblem();
+
+ double calc(const std::vector<Eigen::VectorXd>& xs, const std::vector<Eigen::VectorXd>& us);
+ double calcDiff(const std::vector<Eigen::VectorXd>& xs, const std::vector<Eigen::VectorXd>& us);
+ void rollout(const std::vector<Eigen::VectorXd>& us, std::vector<Eigen::VectorXd>& xs);
+ std::vector<Eigen::VectorXd> rollout_us(const std::vector<Eigen::VectorXd>& us);
+
+ unsigned int get_T() const;
+ const Eigen::VectorXd& get_x0() const;
+
+ std::vector<ActionModelAbstract*>& get_runningModels();
+ ActionModelAbstract* get_terminalModel();
+ std::vector<boost::shared_ptr<ActionDataAbstract> >& get_runningDatas();
+ boost::shared_ptr<ActionDataAbstract>& get_terminalData();
+
+ ActionModelAbstract* terminal_model_;
+ boost::shared_ptr<ActionDataAbstract> terminal_data_;
+ std::vector<ActionModelAbstract*> running_models_;
+ std::vector<boost::shared_ptr<ActionDataAbstract> > running_datas_;
+
+ protected:
+ void allocateData();
+ unsigned int T_;
+ Eigen::VectorXd x0_;
+
+ private:
+ double cost_;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
diff --git a/include/crocoddyl/core/solver-base.hpp b/include/crocoddyl/core/solver-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..85e17ce48e278cc8b5815c08a840b8babe3f0d53
--- /dev/null
+++ b/include/crocoddyl/core/solver-base.hpp
@@ -0,0 +1,88 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_SOLVER_BASE_HPP_
+#define CROCODDYL_CORE_SOLVER_BASE_HPP_
+
+#include <vector>
+#include "crocoddyl/core/optctrl/shooting.hpp"
+
+namespace crocoddyl {
+
+class CallbackAbstract; // forward declaration
+static std::vector<Eigen::VectorXd> DEFAULT_VECTOR;
+
+class SolverAbstract {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ explicit SolverAbstract(ShootingProblem& problem);
+ virtual ~SolverAbstract();
+
+ virtual 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& reg_init = 1e-9) = 0;
+ // TODO(cmastalli): computeDirection (polymorphism) returning descent direction and lambdas
+ virtual void computeDirection(const bool& recalc) = 0;
+ virtual double tryStep(const double& step_length = 1) = 0;
+ virtual double stoppingCriteria() = 0;
+ virtual const Eigen::Vector2d& expectedImprovement() = 0;
+ void setCandidate(const std::vector<Eigen::VectorXd>& xs_warm = DEFAULT_VECTOR,
+ const std::vector<Eigen::VectorXd>& us_warm = DEFAULT_VECTOR, const bool& is_feasible = false);
+
+ void setCallbacks(const std::vector<CallbackAbstract*>& callbacks);
+
+ const ShootingProblem& get_problem() const;
+ const std::vector<ActionModelAbstract*>& get_models() const;
+ const std::vector<boost::shared_ptr<ActionDataAbstract> >& get_datas() const;
+ const std::vector<Eigen::VectorXd>& get_xs() const;
+ const std::vector<Eigen::VectorXd>& get_us() const;
+ const bool& get_isFeasible() const;
+ const unsigned int& get_iter() const;
+ const double& get_cost() const;
+ const double& get_stop() const;
+ const Eigen::Vector2d& get_d() const;
+ const double& get_xreg() const;
+ const double& get_ureg() const;
+ const double& get_stepLength() const;
+ const double& get_dV() const;
+ const double& get_dVexp() const;
+
+ protected:
+ ShootingProblem& problem_;
+ std::vector<ActionModelAbstract*> models_;
+ std::vector<boost::shared_ptr<ActionDataAbstract> > datas_;
+ std::vector<Eigen::VectorXd> xs_;
+ std::vector<Eigen::VectorXd> us_;
+ std::vector<CallbackAbstract*> callbacks_;
+ bool is_feasible_;
+ double cost_;
+ double stop_;
+ Eigen::Vector2d d_;
+ double xreg_;
+ double ureg_;
+ double steplength_;
+ double dV_;
+ double dVexp_;
+ double th_acceptstep_;
+ double th_stop_;
+ unsigned int iter_;
+};
+
+class CallbackAbstract {
+ public:
+ CallbackAbstract() {}
+ ~CallbackAbstract() {}
+ virtual void operator()(SolverAbstract& solver) = 0;
+};
+
+bool raiseIfNaN(const double& value);
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_SOLVER_BASE_HPP_
diff --git a/include/crocoddyl/core/solvers/box-ddp.hpp b/include/crocoddyl/core/solvers/box-ddp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a121bc6049869e05a59af00f9fb0e8cc21e5eea8
--- /dev/null
+++ b/include/crocoddyl/core/solvers/box-ddp.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_SOLVERS_BOX_DDP_HPP_
+#define CROCODDYL_CORE_SOLVERS_BOX_DDP_HPP_
+
+// TODO: SolverBoxDDP
+
+#endif // CROCODDYL_CORE_SOLVERS_BOX_DDP_HPP_
diff --git a/include/crocoddyl/core/solvers/box-kkt.hpp b/include/crocoddyl/core/solvers/box-kkt.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad7da9947a6cae807ecc62dbd2204aaeefd99430
--- /dev/null
+++ b/include/crocoddyl/core/solvers/box-kkt.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_SOLVERS_BOX_KKT_HPP_
+#define CROCODDYL_CORE_SOLVERS_BOX_KKT_HPP_
+
+// TODO: SolverBoxKKT
+
+#endif // CROCODDYL_CORE_SOLVERS_BOX_KKT_HPP_
diff --git a/include/crocoddyl/core/solvers/ddp.hpp b/include/crocoddyl/core/solvers/ddp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1cfa952fe08ff03f393541f61a9b78dd6bae278b
--- /dev/null
+++ b/include/crocoddyl/core/solvers/ddp.hpp
@@ -0,0 +1,89 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_
+#define CROCODDYL_CORE_SOLVERS_DDP_HPP_
+
+#include <Eigen/Cholesky>
+#include <vector>
+#include "crocoddyl/core/solver-base.hpp"
+
+namespace crocoddyl {
+
+class SolverDDP : public SolverAbstract {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ explicit SolverDDP(ShootingProblem& problem);
+ ~SolverDDP();
+
+ 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);
+ double stoppingCriteria();
+ const Eigen::Vector2d& expectedImprovement();
+ double calc();
+ void backwardPass();
+ void forwardPass(const double& stepLength);
+
+ const std::vector<Eigen::MatrixXd>& get_Vxx() const;
+ const std::vector<Eigen::VectorXd>& get_Vx() const;
+ const std::vector<Eigen::MatrixXd>& get_Qxx() const;
+ const std::vector<Eigen::MatrixXd>& get_Qxu() const;
+ const std::vector<Eigen::MatrixXd>& get_Quu() const;
+ const std::vector<Eigen::VectorXd>& get_Qx() const;
+ const std::vector<Eigen::VectorXd>& get_Qu() const;
+ const std::vector<Eigen::MatrixXd>& get_K() const;
+ 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();
+
+ protected:
+ double regfactor_;
+ double regmin_;
+ double regmax_;
+
+ double cost_try_;
+ std::vector<Eigen::VectorXd> xs_try_;
+ std::vector<Eigen::VectorXd> us_try_;
+ std::vector<Eigen::VectorXd> dx_;
+
+ // allocate data
+ std::vector<Eigen::MatrixXd> Vxx_;
+ std::vector<Eigen::VectorXd> Vx_;
+ std::vector<Eigen::MatrixXd> Qxx_;
+ std::vector<Eigen::MatrixXd> Qxu_;
+ std::vector<Eigen::MatrixXd> Quu_;
+ std::vector<Eigen::VectorXd> Qx_;
+ std::vector<Eigen::VectorXd> Qu_;
+ std::vector<Eigen::MatrixXd> K_;
+ std::vector<Eigen::VectorXd> k_;
+ std::vector<Eigen::VectorXd> gaps_;
+
+ Eigen::VectorXd xnext_;
+ Eigen::VectorXd x_reg_;
+ Eigen::MatrixXd FxTVxx_p_;
+ std::vector<Eigen::MatrixXd> FuTVxx_p_;
+ Eigen::VectorXd fTVxx_p_;
+ std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;
+ std::vector<Eigen::VectorXd> Quuk_;
+ std::vector<double> alphas_;
+ double th_grad_;
+ double th_step_;
+ bool was_feasible_;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_SOLVERS_DDP_HPP_
diff --git a/include/crocoddyl/core/solvers/fddp.hpp b/include/crocoddyl/core/solvers/fddp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..372d75240619581cb6ea8ddd12678b505c810a02
--- /dev/null
+++ b/include/crocoddyl/core/solvers/fddp.hpp
@@ -0,0 +1,44 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_SOLVERS_FDDP_HPP_
+#define CROCODDYL_CORE_SOLVERS_FDDP_HPP_
+
+#include <Eigen/Cholesky>
+#include <vector>
+#include "crocoddyl/core/solvers/ddp.hpp"
+
+namespace crocoddyl {
+
+class SolverFDDP : public SolverDDP {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ 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);
+
+ protected:
+ double dg_;
+ double dq_;
+ double dv_;
+
+ private:
+ double th_acceptNegStep_;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_SOLVERS_FDDP_HPP_
diff --git a/include/crocoddyl/core/solvers/kkt.hpp b/include/crocoddyl/core/solvers/kkt.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7fa2b302aa82717b08b17fc9778a039c977bbd9e
--- /dev/null
+++ b/include/crocoddyl/core/solvers/kkt.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_SOLVERS_KKT_HPP_
+#define CROCODDYL_CORE_SOLVERS_KKT_HPP_
+
+// TODO: SolverKKT
+
+#endif // CROCODDYL_CORE_SOLVERS_KKT_HPP_
diff --git a/include/crocoddyl/core/solvers/qp.hpp b/include/crocoddyl/core/solvers/qp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7287369bc3f818709bfdf419b5eb0e22b8f84589
--- /dev/null
+++ b/include/crocoddyl/core/solvers/qp.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_SOLVERS_QP_HPP_
+#define CROCODDYL_CORE_SOLVERS_QP_HPP_
+
+// TODO: quadprogWrapper QPSolution
+
+#endif // CROCODDYL_CORE_SOLVERS_QP_HPP_
diff --git a/include/crocoddyl/core/state-base.hpp b/include/crocoddyl/core/state-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b2104b0021ecb67cbf4f474272d560c7f1e93d4
--- /dev/null
+++ b/include/crocoddyl/core/state-base.hpp
@@ -0,0 +1,113 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_STATE_BASE_HPP_
+#define CROCODDYL_CORE_STATE_BASE_HPP_
+
+#include <Eigen/Core>
+#include <vector>
+#include <string>
+
+namespace crocoddyl {
+
+enum Jcomponent { both = 0, first = 1, second = 2 };
+
+inline bool is_a_Jcomponent(Jcomponent firstsecond) {
+ return (firstsecond == first || firstsecond == second || firstsecond == both);
+}
+
+class StateAbstract {
+ public:
+ StateAbstract(unsigned int const& nx, unsigned int const& ndx);
+ virtual ~StateAbstract();
+
+ virtual Eigen::VectorXd zero() = 0;
+ virtual Eigen::VectorXd rand() = 0;
+ virtual void diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::VectorXd> dxout) = 0;
+ virtual void integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::VectorXd> xout) = 0;
+ virtual void Jdiff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond = both) = 0;
+ virtual void Jintegrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond = both) = 0;
+
+ const unsigned int& get_nx() const;
+ const unsigned int& get_ndx() const;
+ const unsigned int& get_nq() const;
+ const unsigned int& get_nv() const;
+
+ protected:
+ unsigned int nx_;
+ unsigned int ndx_;
+ unsigned int nq_;
+ unsigned int nv_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ Eigen::VectorXd diff_wrap(const Eigen::VectorXd& x0, const Eigen::VectorXd& x1) {
+ Eigen::VectorXd dxout = Eigen::VectorXd::Zero(ndx_);
+ diff(x0, x1, dxout);
+ return dxout;
+ }
+ Eigen::VectorXd integrate_wrap(const Eigen::VectorXd& x, const Eigen::VectorXd& dx) {
+ Eigen::VectorXd xout = Eigen::VectorXd::Zero(nx_);
+ integrate(x, dx, xout);
+ return xout;
+ }
+ std::vector<Eigen::MatrixXd> Jdiff_wrap(const Eigen::VectorXd& x0, const Eigen::VectorXd& x1,
+ std::string firstsecond = "both") {
+ Eigen::MatrixXd Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
+ std::vector<Eigen::MatrixXd> Jacs;
+ if (firstsecond == "both") {
+ Jdiff(x0, x1, Jfirst, Jsecond, both);
+ Jacs.push_back(Jfirst);
+ Jacs.push_back(Jsecond);
+ } else if (firstsecond == "first") {
+ Jdiff(x0, x1, Jfirst, Jsecond, first);
+ Jacs.push_back(Jfirst);
+ } else if (firstsecond == "second") {
+ Jdiff(x0, x1, Jfirst, Jsecond, second);
+ Jacs.push_back(Jsecond);
+ } else {
+ Jdiff(x0, x1, Jfirst, Jsecond, both);
+ Jacs.push_back(Jfirst);
+ Jacs.push_back(Jsecond);
+ }
+ return Jacs;
+ }
+ std::vector<Eigen::MatrixXd> Jintegrate_wrap(const Eigen::VectorXd& x, const Eigen::VectorXd& dx,
+ std::string firstsecond = "both") {
+ Eigen::MatrixXd Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
+ std::vector<Eigen::MatrixXd> Jacs;
+ if (firstsecond == "both") {
+ Jintegrate(x, dx, Jfirst, Jsecond, both);
+ Jacs.push_back(Jfirst);
+ Jacs.push_back(Jsecond);
+ } else if (firstsecond == "first") {
+ Jintegrate(x, dx, Jfirst, Jsecond, first);
+ Jacs.push_back(Jfirst);
+ } else if (firstsecond == "second") {
+ Jintegrate(x, dx, Jfirst, Jsecond, second);
+ Jacs.push_back(Jsecond);
+ } else {
+ Jintegrate(x, dx, Jfirst, Jsecond, both);
+ Jacs.push_back(Jfirst);
+ Jacs.push_back(Jsecond);
+ }
+ return Jacs;
+ }
+#endif
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_STATE_BASE_HPP_
diff --git a/include/crocoddyl/core/states/euclidean.hpp b/include/crocoddyl/core/states/euclidean.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..91a164e6587e70d6580ded63ae9cf8c8dded2093
--- /dev/null
+++ b/include/crocoddyl/core/states/euclidean.hpp
@@ -0,0 +1,36 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_STATES_EUCLIDEAN_HPP_
+#define CROCODDYL_CORE_STATES_EUCLIDEAN_HPP_
+
+#include "crocoddyl/core/state-base.hpp"
+
+namespace crocoddyl {
+
+class StateVector : public StateAbstract {
+ public:
+ explicit StateVector(unsigned int const& nx);
+ ~StateVector();
+
+ Eigen::VectorXd zero();
+ Eigen::VectorXd rand();
+ void diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::VectorXd> dxout);
+ void integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::VectorXd> xout);
+ void Jdiff(const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>&,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond, Jcomponent firstsecond = both);
+ void Jintegrate(const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>&,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond = both);
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_STATES_EUCLIDEAN_HPP_
diff --git a/include/crocoddyl/core/states/unicycle.hpp b/include/crocoddyl/core/states/unicycle.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..df0b81b9e76bd1d89191227f78d0f8cab7ec4cc1
--- /dev/null
+++ b/include/crocoddyl/core/states/unicycle.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_STATES_UNICYCLE_HPP_
+#define CROCODDYL_CORE_STATES_UNICYCLE_HPP_
+
+// TODO: StateUnicycle
+
+#endif // CROCODDYL_CORE_STATES_UNICYCLE_HPP_
diff --git a/include/crocoddyl/core/utils/callbacks.hpp b/include/crocoddyl/core/utils/callbacks.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7a59bc9dfe2965a1ecbbc2a9661e28f9fe8177ce
--- /dev/null
+++ b/include/crocoddyl/core/utils/callbacks.hpp
@@ -0,0 +1,32 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_UTILS_CALLBACKS_HPP_
+#define CROCODDYL_CORE_UTILS_CALLBACKS_HPP_
+
+#include <iostream>
+#include <iomanip>
+#include "crocoddyl/core/solver-base.hpp"
+
+namespace crocoddyl {
+
+enum VerboseLevel { _1 = 0, _2 };
+class CallbackVerbose : public CallbackAbstract {
+ public:
+ explicit CallbackVerbose(VerboseLevel level = _1);
+ ~CallbackVerbose();
+
+ void operator()(SolverAbstract& solver);
+
+ private:
+ VerboseLevel level;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_UTILS_CALLBACKS_HPP_
diff --git a/include/crocoddyl/core/utils/math.hpp b/include/crocoddyl/core/utils/math.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e4d56bd93f51b81d1d95519c96b4745464ca4aa9
--- /dev/null
+++ b/include/crocoddyl/core/utils/math.hpp
@@ -0,0 +1,29 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
+#define CROCODDYL_CORE_UTILS_MATH_HPP_
+
+#include <Eigen/Dense>
+#include <algorithm>
+#include <limits>
+
+template <typename MatrixType>
+MatrixType pseudoInverse(const MatrixType& a, double epsilon = std::numeric_limits<double>::epsilon()) {
+ Eigen::JacobiSVD<MatrixType> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ double tolerance =
+ epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
+ return svd.matrixV() *
+ (svd.singularValues().array().abs() > tolerance)
+ .select(svd.singularValues().array().inverse(), 0)
+ .matrix()
+ .asDiagonal() *
+ svd.matrixU().adjoint();
+}
+
+#endif // CROCODDYL_CORE_UTILS_MATH_HPP_
diff --git a/include/crocoddyl/core/utils/motion-display.hpp b/include/crocoddyl/core/utils/motion-display.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/include/crocoddyl/core/utils/robot-loader.hpp b/include/crocoddyl/core/utils/robot-loader.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/include/crocoddyl/core/utils/solution-plot.hpp b/include/crocoddyl/core/utils/solution-plot.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/include/crocoddyl/core/utils/version.hpp b/include/crocoddyl/core/utils/version.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f86bef222faadda89abb35f8a63eb2cb2d22f7d1
--- /dev/null
+++ b/include/crocoddyl/core/utils/version.hpp
@@ -0,0 +1,48 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_CORE_UTILS_VERSION_HPP_
+#define CROCODDYL_CORE_UTILS_VERSION_HPP_
+
+#include <string>
+#include <sstream>
+#include "crocoddyl/config.hh"
+
+namespace crocoddyl {
+
+///
+/// \brief Returns the current version of Crocoddyl as a string using
+/// the following standard:
+/// CROCODDYL_MINOR_VERSION.CROCODDYL_MINOR_VERSION.CROCODDYL_PATCH_VERSION
+///
+inline std::string printVersion(const std::string& delimiter = ".") {
+ std::ostringstream oss;
+ oss << CROCODDYL_MAJOR_VERSION << delimiter << CROCODDYL_MINOR_VERSION << delimiter << CROCODDYL_PATCH_VERSION;
+ return oss.str();
+}
+
+///
+/// \brief Checks if the current version of Crocoddyl is at least the version provided
+/// by the input arguments.
+///
+/// \param[in] major_version Major version to check.
+/// \param[in] minor_version Minor version to check.
+/// \param[in] patch_version Patch version to check.
+///
+/// \returns true if the current version of Pinocchio is greater than the version provided
+/// by the input arguments.
+///
+inline bool checkVersionAtLeast(int major_version, int minor_version, int patch_version) {
+ return CROCODDYL_MAJOR_VERSION > major_version ||
+ (CROCODDYL_MAJOR_VERSION >= major_version &&
+ (CROCODDYL_MINOR_VERSION > minor_version ||
+ (CROCODDYL_MINOR_VERSION >= minor_version && CROCODDYL_PATCH_VERSION >= patch_version)));
+}
+} // namespace crocoddyl
+
+#endif // CROCODDYL_CORE_UTILS_VERSION_HPP_
diff --git a/include/crocoddyl/multibody/actions/contact-fwddyn.hpp b/include/crocoddyl/multibody/actions/contact-fwddyn.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..296839dba6a548089840de87ebf9bee292682923
--- /dev/null
+++ b/include/crocoddyl/multibody/actions/contact-fwddyn.hpp
@@ -0,0 +1,87 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
+#define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
+
+#include "crocoddyl/core/diff-action-base.hpp"
+#include "crocoddyl/multibody/states/multibody.hpp"
+#include "crocoddyl/multibody/actuations/floating-base.hpp"
+#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
+#include "crocoddyl/multibody/costs/cost-sum.hpp"
+#include <pinocchio/multibody/data.hpp>
+
+namespace crocoddyl {
+
+class DifferentialActionModelContactFwdDynamics : public DifferentialActionModelAbstract {
+ public:
+ DifferentialActionModelContactFwdDynamics(StateMultibody& state, ActuationModelFloatingBase& actuation,
+ ContactModelMultiple& contacts, CostModelSum& costs,
+ const double& JMinvJt_damping = 0., const bool& enable_force = false);
+ ~DifferentialActionModelContactFwdDynamics();
+
+ void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u);
+ void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
+ const bool& recalc = true);
+ boost::shared_ptr<DifferentialActionDataAbstract> createData();
+
+ ActuationModelFloatingBase& get_actuation() const;
+ ContactModelMultiple& get_contacts() const;
+ CostModelSum& get_costs() const;
+ pinocchio::Model& get_pinocchio() const;
+ const Eigen::VectorXd& get_armature() const;
+ const double& get_damping_factor() const;
+
+ void set_armature(const Eigen::VectorXd& armature);
+ void set_damping_factor(const double& damping);
+
+ private:
+ ActuationModelFloatingBase& actuation_;
+ ContactModelMultiple& contacts_;
+ CostModelSum& costs_;
+ pinocchio::Model& pinocchio_;
+ bool with_armature_;
+ Eigen::VectorXd armature_;
+ double JMinvJt_damping_;
+ bool enable_force_;
+};
+
+struct DifferentialActionDataContactFwdDynamics : public DifferentialActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ explicit DifferentialActionDataContactFwdDynamics(Model* const model)
+ : DifferentialActionDataAbstract(model),
+ 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()) {
+ actuation = model->get_actuation().createData();
+ contacts = model->get_contacts().createData(&pinocchio);
+ costs = model->get_costs().createData(&pinocchio);
+ shareCostMemory(costs);
+ Kinv.fill(0);
+ Gx.fill(0);
+ Gu.fill(0);
+ }
+
+ pinocchio::Data pinocchio;
+ boost::shared_ptr<ActuationDataAbstract> actuation;
+ boost::shared_ptr<ContactDataMultiple> contacts;
+ boost::shared_ptr<CostDataSum> costs;
+ Eigen::MatrixXd Kinv;
+ Eigen::MatrixXd Gx;
+ Eigen::MatrixXd Gu;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
diff --git a/include/crocoddyl/multibody/actions/free-fwddyn.hpp b/include/crocoddyl/multibody/actions/free-fwddyn.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ddb6b17e291f1bb3de780f64f0d4f31fa287a5cd
--- /dev/null
+++ b/include/crocoddyl/multibody/actions/free-fwddyn.hpp
@@ -0,0 +1,66 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
+#define CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
+
+#include "crocoddyl/core/diff-action-base.hpp"
+#include "crocoddyl/multibody/states/multibody.hpp"
+#include "crocoddyl/multibody/costs/cost-sum.hpp"
+#include <pinocchio/multibody/data.hpp>
+
+namespace crocoddyl {
+
+class DifferentialActionModelFreeFwdDynamics : public DifferentialActionModelAbstract {
+ public:
+ DifferentialActionModelFreeFwdDynamics(StateMultibody& state, CostModelSum& costs);
+ ~DifferentialActionModelFreeFwdDynamics();
+
+ void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u);
+ void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
+ const bool& recalc = true);
+ boost::shared_ptr<DifferentialActionDataAbstract> createData();
+
+ CostModelSum& get_costs() const;
+ pinocchio::Model& get_pinocchio() const;
+ const Eigen::VectorXd& get_armature() const;
+ void set_armature(const Eigen::VectorXd& armature);
+
+ private:
+ CostModelSum& costs_;
+ pinocchio::Model& pinocchio_;
+ bool with_armature_;
+ Eigen::VectorXd armature_;
+};
+
+struct DifferentialActionDataFreeFwdDynamics : public DifferentialActionDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ explicit DifferentialActionDataFreeFwdDynamics(Model* const model)
+ : DifferentialActionDataAbstract(model),
+ pinocchio(pinocchio::Data(model->get_pinocchio())),
+ Minv(model->get_state().get_nv(), model->get_state().get_nv()),
+ u_drift(model->get_nu()) {
+ costs = model->get_costs().createData(&pinocchio);
+ shareCostMemory(costs);
+ Minv.fill(0);
+ u_drift.fill(0);
+ }
+
+ pinocchio::Data pinocchio;
+ boost::shared_ptr<CostDataSum> costs;
+ Eigen::MatrixXd Minv;
+ Eigen::VectorXd u_drift;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_FWDDYN_HPP_
diff --git a/include/crocoddyl/multibody/actions/impulse-fwddyn.hpp b/include/crocoddyl/multibody/actions/impulse-fwddyn.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e51a4d1afe8a0efccc7029d186a56e73542ebf4
--- /dev/null
+++ b/include/crocoddyl/multibody/actions/impulse-fwddyn.hpp
@@ -0,0 +1,14 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_IMPULSE_FWDDYN_HPP_
+#define CROCODDYL_MULTIBODY_IMPULSE_FWDDYN_HPP_
+
+// TODO: ActionModelImpact ActionDataImpact
+
+#endif // CROCODDYL_MULTIBODY_IMPULSE_FWDDYN_HPP_
diff --git a/include/crocoddyl/multibody/actuations/floating-base.hpp b/include/crocoddyl/multibody/actuations/floating-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc2e3f7f6472935509ce09b5b618e0decf859e85
--- /dev/null
+++ b/include/crocoddyl/multibody/actuations/floating-base.hpp
@@ -0,0 +1,36 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
+#define CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
+
+#include "crocoddyl/core/actuation-base.hpp"
+#include "crocoddyl/multibody/states/multibody.hpp"
+
+namespace crocoddyl {
+
+class ActuationModelFloatingBase : public ActuationModelAbstract {
+ public:
+ explicit ActuationModelFloatingBase(StateMultibody& state);
+ ~ActuationModelFloatingBase();
+
+ void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u);
+ void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true);
+ boost::shared_ptr<ActuationDataAbstract> createData();
+
+#ifndef NDEBUG
+ private:
+ Eigen::MatrixXd Au_;
+#endif
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_ACTUATIONS_FLOATING_BASE_HPP_
diff --git a/include/crocoddyl/multibody/actuations/full.hpp b/include/crocoddyl/multibody/actuations/full.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e7c2dfb5c7b0f8b7075edb0a5da93da196d3534f
--- /dev/null
+++ b/include/crocoddyl/multibody/actuations/full.hpp
@@ -0,0 +1,31 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
+#define CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
+
+#include "crocoddyl/core/actuation-base.hpp"
+#include "crocoddyl/multibody/states/multibody.hpp"
+
+namespace crocoddyl {
+
+class ActuationModelFull : public ActuationModelAbstract {
+ public:
+ explicit ActuationModelFull(StateMultibody& state);
+ ~ActuationModelFull();
+
+ void calc(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u);
+ void calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true);
+ boost::shared_ptr<ActuationDataAbstract> createData();
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
diff --git a/include/crocoddyl/multibody/contact-base.hpp b/include/crocoddyl/multibody/contact-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..55b916fa751a3307509aa02645825aba1575f73e
--- /dev/null
+++ b/include/crocoddyl/multibody/contact-base.hpp
@@ -0,0 +1,89 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
+#define CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
+
+#include "crocoddyl/multibody/states/multibody.hpp"
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/spatial/force.hpp>
+
+namespace crocoddyl {
+
+struct ContactDataAbstract; // forward declaration
+
+class ContactModelAbstract {
+ public:
+ ContactModelAbstract(StateMultibody& state, unsigned int const& nc, unsigned int const& nu);
+ ContactModelAbstract(StateMultibody& state, unsigned int const& nc);
+ ~ContactModelAbstract();
+
+ virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data,
+ 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 boost::shared_ptr<ContactDataAbstract> createData(pinocchio::Data* const data);
+
+ StateMultibody& get_state() const;
+ unsigned int const& get_nc() const;
+ unsigned int const& get_nu() const;
+
+ protected:
+ StateMultibody& state_;
+ unsigned int nc_;
+ unsigned int nu_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& x) { calc(data, x); }
+
+ void calcDiff_wrap(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::VectorXd& x,
+ const bool& recalc = true) {
+ calcDiff(data, x, recalc);
+ }
+
+#endif
+};
+
+struct ContactDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ ContactDataAbstract(Model* const model, pinocchio::Data* const data)
+ : pinocchio(data),
+ 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()),
+ f(pinocchio::Force::Zero()) {
+ Jc.fill(0);
+ a0.fill(0);
+ Ax.fill(0);
+ Gx.fill(0);
+ Gu.fill(0);
+ }
+
+ pinocchio::Data* pinocchio;
+ pinocchio::JointIndex joint;
+ Eigen::MatrixXd Jc;
+ Eigen::VectorXd a0;
+ Eigen::MatrixXd Ax;
+ Eigen::MatrixXd Gx;
+ Eigen::MatrixXd Gu;
+ pinocchio::Force f;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
diff --git a/include/crocoddyl/multibody/contacts/contact-3d.hpp b/include/crocoddyl/multibody/contacts/contact-3d.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f7780ba3d0521f8dad3d1cedd85d3d871d50711
--- /dev/null
+++ b/include/crocoddyl/multibody/contacts/contact-3d.hpp
@@ -0,0 +1,94 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
+#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
+
+#include "crocoddyl/multibody/contact-base.hpp"
+#include "crocoddyl/multibody/frames.hpp"
+#include <pinocchio/spatial/motion.hpp>
+#include <pinocchio/multibody/data.hpp>
+
+namespace crocoddyl {
+
+class ContactModel3D : public ContactModelAbstract {
+ public:
+ ContactModel3D(StateMultibody& state, const FrameTranslation& xref, unsigned int const& nu,
+ const Eigen::Vector2d& gains = Eigen::Vector2d::Zero());
+ ContactModel3D(StateMultibody& state, const FrameTranslation& xref,
+ const Eigen::Vector2d& gains = Eigen::Vector2d::Zero());
+ ~ContactModel3D();
+
+ 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);
+ boost::shared_ptr<ContactDataAbstract> createData(pinocchio::Data* const data);
+
+ const FrameTranslation& get_xref() const;
+ const Eigen::Vector2d& get_gains() const;
+
+ private:
+ FrameTranslation xref_;
+ Eigen::Vector2d gains_;
+};
+
+struct ContactData3D : public ContactDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ ContactData3D(Model* const model, pinocchio::Data* const data)
+ : ContactDataAbstract(model, data),
+ jMf(model->get_state().get_pinocchio().frames[model->get_xref().frame].placement),
+ fXj(jMf.inverse().toActionMatrix()),
+ fJf(6, model->get_state().get_nv()),
+ v_partial_dq(6, model->get_state().get_nv()),
+ a_partial_dq(6, model->get_state().get_nv()),
+ a_partial_dv(6, model->get_state().get_nv()),
+ a_partial_da(6, model->get_state().get_nv()),
+ fXjdv_dq(6, model->get_state().get_nv()),
+ fXjda_dq(6, model->get_state().get_nv()),
+ fXjda_dv(6, model->get_state().get_nv()) {
+ joint = model->get_state().get_pinocchio().frames[model->get_xref().frame].parent;
+ fJf.fill(0);
+ v_partial_dq.fill(0);
+ a_partial_dq.fill(0);
+ a_partial_dv.fill(0);
+ a_partial_da.fill(0);
+ fXjdv_dq.fill(0);
+ fXjda_dq.fill(0);
+ fXjda_dv.fill(0);
+ vv.fill(0);
+ vw.fill(0);
+ vv_skew.fill(0);
+ vw_skew.fill(0);
+ oRf.fill(0);
+ }
+
+ pinocchio::SE3 jMf;
+ pinocchio::SE3::ActionMatrixType fXj;
+ pinocchio::Motion v;
+ pinocchio::Motion a;
+ pinocchio::Data::Matrix6x fJf;
+ pinocchio::Data::Matrix6x v_partial_dq;
+ pinocchio::Data::Matrix6x a_partial_dq;
+ pinocchio::Data::Matrix6x a_partial_dv;
+ pinocchio::Data::Matrix6x a_partial_da;
+ pinocchio::Data::Matrix6x fXjdv_dq;
+ pinocchio::Data::Matrix6x fXjda_dq;
+ pinocchio::Data::Matrix6x fXjda_dv;
+ Eigen::Vector3d vv;
+ Eigen::Vector3d vw;
+ Eigen::Matrix3d vv_skew;
+ Eigen::Matrix3d vw_skew;
+ Eigen::Matrix3d oRf;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_3D_HPP_
diff --git a/include/crocoddyl/multibody/contacts/contact-6d.hpp b/include/crocoddyl/multibody/contacts/contact-6d.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b60421b227c0309556815accf4ff9f751e789651
--- /dev/null
+++ b/include/crocoddyl/multibody/contacts/contact-6d.hpp
@@ -0,0 +1,84 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
+#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
+
+#include "crocoddyl/multibody/contact-base.hpp"
+#include "crocoddyl/multibody/frames.hpp"
+#include <pinocchio/spatial/motion.hpp>
+#include <pinocchio/multibody/data.hpp>
+
+namespace crocoddyl {
+
+class ContactModel6D : public ContactModelAbstract {
+ public:
+ ContactModel6D(StateMultibody& state, const FramePlacement& xref, unsigned int const& nu,
+ const Eigen::Vector2d& gains = Eigen::Vector2d::Zero());
+ ContactModel6D(StateMultibody& state, const FramePlacement& xref,
+ const Eigen::Vector2d& gains = Eigen::Vector2d::Zero());
+ ~ContactModel6D();
+
+ 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);
+ boost::shared_ptr<ContactDataAbstract> createData(pinocchio::Data* const data);
+
+ const FramePlacement& get_Mref() const;
+ const Eigen::Vector2d& get_gains() const;
+
+ private:
+ FramePlacement Mref_;
+ Eigen::Vector2d gains_;
+};
+
+struct ContactData6D : public ContactDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ ContactData6D(Model* const model, pinocchio::Data* const data)
+ : ContactDataAbstract(model, data),
+ jMf(model->get_state().get_pinocchio().frames[model->get_Mref().frame].placement),
+ rMf(pinocchio::SE3::Identity()),
+ fXj(jMf.inverse().toActionMatrix()),
+ v_partial_dq(6, model->get_state().get_nv()),
+ a_partial_dq(6, model->get_state().get_nv()),
+ a_partial_dv(6, model->get_state().get_nv()),
+ a_partial_da(6, model->get_state().get_nv()) {
+ joint = model->get_state().get_pinocchio().frames[model->get_Mref().frame].parent;
+ v_partial_dq.fill(0);
+ a_partial_dq.fill(0);
+ a_partial_dv.fill(0);
+ a_partial_da.fill(0);
+ rMf_Jlog6.fill(0);
+
+ vv_skew.fill(0);
+ vw_skew.fill(0);
+ oRf.fill(0);
+ }
+
+ pinocchio::SE3 jMf;
+ pinocchio::SE3 rMf;
+ pinocchio::SE3::ActionMatrixType fXj;
+ pinocchio::Motion v;
+ pinocchio::Motion a;
+ pinocchio::Data::Matrix6x v_partial_dq;
+ pinocchio::Data::Matrix6x a_partial_dq;
+ pinocchio::Data::Matrix6x a_partial_dv;
+ pinocchio::Data::Matrix6x a_partial_da;
+ pinocchio::SE3::Matrix6 rMf_Jlog6;
+
+ Eigen::Matrix3d vv_skew;
+ Eigen::Matrix3d vw_skew;
+ Eigen::Matrix3d oRf;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
diff --git a/include/crocoddyl/multibody/contacts/multiple-contacts.hpp b/include/crocoddyl/multibody/contacts/multiple-contacts.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0462533bf7bf6c6be202ea58d2e4a25943f7d262
--- /dev/null
+++ b/include/crocoddyl/multibody/contacts/multiple-contacts.hpp
@@ -0,0 +1,93 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
+#define CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
+
+#include <string>
+#include <map>
+#include <utility>
+#include "crocoddyl/multibody/contact-base.hpp"
+
+namespace crocoddyl {
+
+struct ContactItem {
+ ContactItem() {}
+ ContactItem(const std::string& name, ContactModelAbstract* contact) : name(name), contact(contact) {}
+
+ std::string name;
+ ContactModelAbstract* contact;
+};
+
+struct ContactDataMultiple; // forward declaration
+
+class ContactModelMultiple {
+ public:
+ typedef std::map<std::string, ContactItem> ContactModelContainer;
+ typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> > ContactDataContainer;
+ typedef pinocchio::container::aligned_vector<pinocchio::Force>::iterator ForceIterator;
+
+ ContactModelMultiple(StateMultibody& state, unsigned int const& nu);
+ ContactModelMultiple(StateMultibody& state);
+ ~ContactModelMultiple();
+
+ void addContact(const std::string& name, ContactModelAbstract* const contact);
+ void removeContact(const std::string& name);
+
+ 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);
+ boost::shared_ptr<ContactDataMultiple> createData(pinocchio::Data* const data);
+
+ StateMultibody& get_state() const;
+ const ContactModelContainer& get_contacts() const;
+ const unsigned int& get_nc() const;
+ const unsigned int& get_nu() const;
+
+ private:
+ StateMultibody& state_;
+ ContactModelContainer contacts_;
+ unsigned int nc_;
+ unsigned int nu_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::VectorXd& x) { calc(data, x); }
+
+ void calcDiff_wrap(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::VectorXd& x,
+ const bool& recalc = true) {
+ calcDiff(data, x, recalc);
+ }
+
+#endif
+};
+
+struct ContactDataMultiple : ContactDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ ContactDataMultiple(Model* const model, pinocchio::Data* const data)
+ : ContactDataAbstract(model, data), fext(model->get_state().get_pinocchio().njoints, pinocchio::Force::Zero()) {
+ for (ContactModelMultiple::ContactModelContainer::const_iterator it = model->get_contacts().begin();
+ it != model->get_contacts().end(); ++it) {
+ const ContactItem& item = it->second;
+ contacts.insert(std::make_pair(item.name, item.contact->createData(data)));
+ }
+ }
+
+ ContactModelMultiple::ContactDataContainer contacts;
+ pinocchio::container::aligned_vector<pinocchio::Force> fext;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
diff --git a/include/crocoddyl/multibody/cost-base.hpp b/include/crocoddyl/multibody/cost-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7371175c98a2e6c363ccf691584283c193c4f9b
--- /dev/null
+++ b/include/crocoddyl/multibody/cost-base.hpp
@@ -0,0 +1,116 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COST_BASE_HPP_
+#define CROCODDYL_MULTIBODY_COST_BASE_HPP_
+
+#include "crocoddyl/multibody/states/multibody.hpp"
+#include "crocoddyl/core/activation-base.hpp"
+#include <pinocchio/multibody/data.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+
+namespace crocoddyl {
+
+struct CostDataAbstract; // forward declaration
+
+class CostModelAbstract {
+ public:
+ CostModelAbstract(StateMultibody& state, ActivationModelAbstract& activation, unsigned int const& nu,
+ const bool& with_residuals = true);
+ CostModelAbstract(StateMultibody& state, ActivationModelAbstract& activation, const bool& with_residuals = true);
+ CostModelAbstract(StateMultibody& state, unsigned int const& nr, unsigned int const& nu,
+ const bool& with_residuals = true);
+ CostModelAbstract(StateMultibody& state, unsigned int const& nr, const bool& with_residuals = true);
+ ~CostModelAbstract();
+
+ virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u) = 0;
+ virtual 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) = 0;
+ virtual boost::shared_ptr<CostDataAbstract> createData(pinocchio::Data* const data);
+
+ void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
+ void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
+
+ StateMultibody& get_state() const;
+ ActivationModelAbstract& get_activation() const;
+ unsigned int const& get_nu() const;
+
+ protected:
+ StateMultibody& state_;
+ ActivationModelAbstract& activation_;
+ unsigned int nu_;
+ bool with_residuals_;
+ Eigen::VectorXd unone_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u = Eigen::VectorXd()) {
+ if (u.size() == 0) {
+ calc(data, x);
+ } else {
+ calc(data, x, u);
+ }
+ }
+
+ void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u, const bool& recalc) {
+ calcDiff(data, x, u, recalc);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u) {
+ calcDiff(data, x, u, true);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x) {
+ calcDiff(data, x, unone_, true);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x, const bool& recalc) {
+ calcDiff(data, x, unone_, recalc);
+ }
+
+#endif
+};
+
+struct CostDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ 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* pinocchio;
+ boost::shared_ptr<ActivationDataAbstract> activation;
+ double cost;
+ Eigen::VectorXd Lx;
+ Eigen::VectorXd Lu;
+ Eigen::MatrixXd Lxx;
+ Eigen::MatrixXd Lxu;
+ Eigen::MatrixXd Luu;
+ Eigen::VectorXd r;
+ Eigen::MatrixXd Rx;
+ Eigen::MatrixXd Ru;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_COST_BASE_HPP_
diff --git a/include/crocoddyl/multibody/costs/com-position.hpp b/include/crocoddyl/multibody/costs/com-position.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..29856486606c258caee998f78a2405c8da6c2bd9
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/com-position.hpp
@@ -0,0 +1,51 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_COM_POSITION_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_COM_POSITION_HPP_
+
+#include "crocoddyl/multibody/cost-base.hpp"
+
+namespace crocoddyl {
+
+class CostModelCoMPosition : public CostModelAbstract {
+ public:
+ CostModelCoMPosition(StateMultibody& state, ActivationModelAbstract& activation, const Eigen::Vector3d& cref,
+ unsigned int const& nu);
+ CostModelCoMPosition(StateMultibody& state, ActivationModelAbstract& activation, const Eigen::Vector3d& cref);
+ CostModelCoMPosition(StateMultibody& state, const Eigen::Vector3d& cref, unsigned int const& nu);
+ CostModelCoMPosition(StateMultibody& state, const Eigen::Vector3d& cref);
+ ~CostModelCoMPosition();
+
+ void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const 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);
+ boost::shared_ptr<CostDataAbstract> createData(pinocchio::Data* const data);
+
+ const Eigen::VectorXd& get_cref() const;
+
+ private:
+ Eigen::VectorXd cref_;
+};
+
+struct CostDataCoMPosition : public CostDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ CostDataCoMPosition(Model* const model, pinocchio::Data* const data)
+ : CostDataAbstract(model, data), Arr_Jcom(3, model->get_state().get_nv()) {
+ Arr_Jcom.fill(0);
+ }
+
+ pinocchio::Data::Matrix3x Arr_Jcom;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_COSTS_COM_POSITION_HPP_
diff --git a/include/crocoddyl/multibody/costs/control.hpp b/include/crocoddyl/multibody/costs/control.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac93d4dd873f75c17712437eb286f7262b0b3e02
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/control.hpp
@@ -0,0 +1,39 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_CONTROL_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_CONTROL_HPP_
+
+#include "crocoddyl/multibody/cost-base.hpp"
+
+namespace crocoddyl {
+
+class CostModelControl : public CostModelAbstract {
+ public:
+ CostModelControl(StateMultibody& state, ActivationModelAbstract& activation, const Eigen::VectorXd& uref);
+ CostModelControl(StateMultibody& state, ActivationModelAbstract& activation);
+ CostModelControl(StateMultibody& state, ActivationModelAbstract& activation, unsigned int const& nu);
+ CostModelControl(StateMultibody& state, const Eigen::VectorXd& uref);
+ explicit CostModelControl(StateMultibody& state);
+ CostModelControl(StateMultibody& state, unsigned int const& nu);
+ ~CostModelControl();
+
+ void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const 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);
+
+ const Eigen::VectorXd& get_uref() const;
+
+ private:
+ Eigen::VectorXd uref_;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_COSTS_CONTROL_HPP_
diff --git a/include/crocoddyl/multibody/costs/cost-sum.hpp b/include/crocoddyl/multibody/costs/cost-sum.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4a6ec8661cbf842e1e128b4cf10991884fe1a7a
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/cost-sum.hpp
@@ -0,0 +1,132 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_
+
+#include <string>
+#include <map>
+#include <utility>
+#include "crocoddyl/multibody/cost-base.hpp"
+
+namespace crocoddyl {
+
+struct CostItem {
+ CostItem() {}
+ CostItem(const std::string& name, CostModelAbstract* cost, const double& weight)
+ : name(name), cost(cost), weight(weight) {}
+
+ std::string name;
+ CostModelAbstract* cost;
+ double weight;
+};
+
+struct CostDataSum; // forward declaration
+
+class CostModelSum {
+ public:
+ typedef std::map<std::string, CostItem> CostModelContainer;
+ typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
+
+ CostModelSum(StateMultibody& state, unsigned int const& nu, const bool& with_residuals = true);
+ explicit CostModelSum(StateMultibody& state, const bool& with_residuals = true);
+ ~CostModelSum();
+
+ void addCost(const std::string& name, CostModelAbstract* const cost, const double& weight);
+ void removeCost(const std::string& name);
+
+ void calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u);
+ void calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true);
+ boost::shared_ptr<CostDataSum> createData(pinocchio::Data* const data);
+
+ void calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
+ void calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
+
+ StateMultibody& get_state() const;
+ const CostModelContainer& get_costs() const;
+ unsigned int const& get_nu() const;
+ unsigned int const& get_nr() const;
+
+ private:
+ StateMultibody& state_;
+ CostModelContainer costs_;
+ unsigned int nu_;
+ unsigned int nr_;
+ bool with_residuals_;
+ Eigen::VectorXd unone_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<CostDataSum>& data, const Eigen::VectorXd& x,
+ const Eigen::VectorXd& u = Eigen::VectorXd()) {
+ if (u.size() == 0) {
+ calc(data, x);
+ } else {
+ calc(data, x, u);
+ }
+ }
+
+ void calcDiff_wrap(const boost::shared_ptr<CostDataSum>& data, const Eigen::VectorXd& x, const Eigen::VectorXd& u,
+ const bool& recalc) {
+ calcDiff(data, x, u, recalc);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<CostDataSum>& data, const Eigen::VectorXd& x, const Eigen::VectorXd& u) {
+ calcDiff(data, x, u, true);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<CostDataSum>& data, const Eigen::VectorXd& x) {
+ calcDiff(data, x, unone_, true);
+ }
+ void calcDiff_wrap(const boost::shared_ptr<CostDataSum>& data, const Eigen::VectorXd& x, const bool& recalc) {
+ calcDiff(data, x, unone_, recalc);
+ }
+
+#endif
+};
+
+struct CostDataSum {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ CostDataSum(Model* const model, pinocchio::Data* const data) : pinocchio(data), cost(0.) {
+ 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);
+ }
+
+ 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::MatrixXd Rx;
+ Eigen::MatrixXd Ru;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_
diff --git a/include/crocoddyl/multibody/costs/frame-force.hpp b/include/crocoddyl/multibody/costs/frame-force.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d47fe5bd0305973800416890dbed87618429372
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/frame-force.hpp
@@ -0,0 +1,15 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_FRAME_FORCE_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_FRAME_FORCE_HPP_
+
+// TODO: CostModelForce CostDataFor
+// TODO: CostModelForceLinearCost CostDataForceCone
+
+#endif // CROCODDYL_MULTIBODY_COSTS_FRAME_FORCE_HPP_
diff --git a/include/crocoddyl/multibody/costs/frame-placement.hpp b/include/crocoddyl/multibody/costs/frame-placement.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..859d7cadb34bdcfaa594a27aba5be6a115d64a50
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/frame-placement.hpp
@@ -0,0 +1,66 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_FRAME_PLACEMENT_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_FRAME_PLACEMENT_HPP_
+
+#include "crocoddyl/multibody/cost-base.hpp"
+#include "crocoddyl/multibody/frames.hpp"
+
+namespace crocoddyl {
+
+class CostModelFramePlacement : public CostModelAbstract {
+ public:
+ CostModelFramePlacement(StateMultibody& state, ActivationModelAbstract& activation, const FramePlacement& Fref,
+ unsigned int const& nu);
+ CostModelFramePlacement(StateMultibody& state, ActivationModelAbstract& activation, const FramePlacement& Fref);
+ CostModelFramePlacement(StateMultibody& state, const FramePlacement& Fref, unsigned int const& nu);
+ CostModelFramePlacement(StateMultibody& state, const FramePlacement& Fref);
+ ~CostModelFramePlacement();
+
+ void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const 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);
+ boost::shared_ptr<CostDataAbstract> createData(pinocchio::Data* const data);
+
+ const FramePlacement& get_Mref() const;
+
+ private:
+ FramePlacement Mref_;
+ pinocchio::SE3 oMf_inv_;
+};
+
+struct CostDataFramePlacement : public CostDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ CostDataFramePlacement(Model* const model, pinocchio::Data* const data)
+ : CostDataAbstract(model, data),
+ J(6, model->get_state().get_nv()),
+ rJf(6, 6),
+ fJf(6, model->get_state().get_nv()),
+ Arr_J(6, model->get_state().get_nv()) {
+ r.fill(0);
+ J.fill(0);
+ rJf.fill(0);
+ fJf.fill(0);
+ Arr_J.fill(0);
+ }
+
+ pinocchio::Motion::Vector6 r;
+ pinocchio::SE3 rMf;
+ pinocchio::Data::Matrix6x J;
+ pinocchio::Data::Matrix6 rJf;
+ pinocchio::Data::Matrix6x fJf;
+ pinocchio::Data::Matrix6x Arr_J;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_COSTS_FRAME_PLACEMENT_HPP_
diff --git a/include/crocoddyl/multibody/costs/frame-translation.hpp b/include/crocoddyl/multibody/costs/frame-translation.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fe99e3c2ad7d91aa3081debff009ae36c96aa343
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/frame-translation.hpp
@@ -0,0 +1,55 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_
+
+#include "crocoddyl/multibody/cost-base.hpp"
+#include "crocoddyl/multibody/frames.hpp"
+
+namespace crocoddyl {
+
+class CostModelFrameTranslation : public CostModelAbstract {
+ public:
+ CostModelFrameTranslation(StateMultibody& state, ActivationModelAbstract& activation, const FrameTranslation& xref,
+ unsigned int const& nu);
+ CostModelFrameTranslation(StateMultibody& state, ActivationModelAbstract& activation, const FrameTranslation& xref);
+ CostModelFrameTranslation(StateMultibody& state, const FrameTranslation& xref, unsigned int const& nu);
+ CostModelFrameTranslation(StateMultibody& state, const FrameTranslation& xref);
+ ~CostModelFrameTranslation();
+
+ void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const 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);
+ boost::shared_ptr<CostDataAbstract> createData(pinocchio::Data* const data);
+
+ const FrameTranslation& get_xref() const;
+
+ private:
+ FrameTranslation xref_;
+};
+
+struct CostDataFrameTranslation : public CostDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ CostDataFrameTranslation(Model* const model, pinocchio::Data* const data)
+ : CostDataAbstract(model, data), J(3, model->get_state().get_nv()), fJf(6, model->get_state().get_nv()) {
+ J.fill(0);
+ fJf.fill(0);
+ }
+
+ pinocchio::Data::Matrix3x J;
+ pinocchio::Data::Matrix6x fJf;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_
diff --git a/include/crocoddyl/multibody/costs/frame-velocity.hpp b/include/crocoddyl/multibody/costs/frame-velocity.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f83c9f01f9187c2c6a499dd09a9ca75bdb606188
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/frame-velocity.hpp
@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_
+
+#include "crocoddyl/multibody/cost-base.hpp"
+#include "crocoddyl/multibody/frames.hpp"
+
+namespace crocoddyl {
+
+class CostModelFrameVelocity : public CostModelAbstract {
+ public:
+ CostModelFrameVelocity(StateMultibody& state, ActivationModelAbstract& activation, const FrameMotion& Fref,
+ unsigned int const& nu);
+ CostModelFrameVelocity(StateMultibody& state, ActivationModelAbstract& activation, const FrameMotion& Fref);
+ CostModelFrameVelocity(StateMultibody& state, const FrameMotion& vref, unsigned int const& nu);
+ CostModelFrameVelocity(StateMultibody& state, const FrameMotion& vref);
+ ~CostModelFrameVelocity();
+
+ void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const 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);
+ boost::shared_ptr<CostDataAbstract> createData(pinocchio::Data* const data);
+
+ const FrameMotion& get_vref() const;
+
+ private:
+ FrameMotion vref_;
+};
+
+struct CostDataFrameVelocity : public CostDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ CostDataFrameVelocity(Model* const model, pinocchio::Data* const data)
+ : CostDataAbstract(model, data),
+ joint(model->get_state().get_pinocchio().frames[model->get_vref().frame].parent),
+ vr(pinocchio::Motion::Zero()),
+ fXj(model->get_state().get_pinocchio().frames[model->get_vref().frame].placement.inverse().toActionMatrix()),
+ v_partial_dq(6, model->get_state().get_nv()),
+ v_partial_dv(6, model->get_state().get_nv()),
+ Arr_Rx(6, model->get_state().get_nv()) {
+ v_partial_dq.fill(0);
+ v_partial_dv.fill(0);
+ Arr_Rx.fill(0);
+ }
+
+ pinocchio::JointIndex joint;
+ pinocchio::Motion vr;
+ pinocchio::SE3::ActionMatrixType fXj;
+ pinocchio::Data::Matrix6x v_partial_dq;
+ pinocchio::Data::Matrix6x v_partial_dv;
+ pinocchio::Data::Matrix6x Arr_Rx;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_
diff --git a/include/crocoddyl/multibody/costs/impulse.hpp b/include/crocoddyl/multibody/costs/impulse.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1e354b1e7379063cadcbc496b574e90fd7943b0d
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/impulse.hpp
@@ -0,0 +1,16 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_IMPULSE_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_IMPULSE_HPP_
+
+// TODO: CostModelImpactBase
+// TODO CostModelImpactCoM CostDataImpactCoM
+// TODO CostModelImpactWholeBody CostDataImpactWholeBody
+
+#endif // CROCODDYL_MULTIBODY_COSTS_IMPULSE_HPP_
diff --git a/include/crocoddyl/multibody/costs/state.hpp b/include/crocoddyl/multibody/costs/state.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..42dbd42bbf31290e794a11e96cb776e9a1a9a16f
--- /dev/null
+++ b/include/crocoddyl/multibody/costs/state.hpp
@@ -0,0 +1,57 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_COSTS_STATE_HPP_
+#define CROCODDYL_MULTIBODY_COSTS_STATE_HPP_
+
+#include "crocoddyl/core/state-base.hpp"
+#include "crocoddyl/multibody/cost-base.hpp"
+
+namespace crocoddyl {
+
+class CostModelState : public CostModelAbstract {
+ public:
+ CostModelState(StateMultibody& state, ActivationModelAbstract& activation, const Eigen::VectorXd& xref,
+ unsigned int const& nu);
+ CostModelState(StateMultibody& state, ActivationModelAbstract& activation, const Eigen::VectorXd& xref);
+ CostModelState(StateMultibody& state, const Eigen::VectorXd& xref, unsigned int const& nu);
+ CostModelState(StateMultibody& state, const Eigen::VectorXd& xref);
+ CostModelState(StateMultibody& state, ActivationModelAbstract& activation, unsigned int const& nu);
+ CostModelState(StateMultibody& state, unsigned int const& nu);
+ CostModelState(StateMultibody& state, ActivationModelAbstract& activation);
+ explicit CostModelState(StateMultibody& state);
+
+ ~CostModelState();
+
+ void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const 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);
+ boost::shared_ptr<CostDataAbstract> createData(pinocchio::Data* const data);
+
+ const Eigen::VectorXd& get_xref() const;
+
+ private:
+ Eigen::VectorXd xref_;
+};
+
+struct CostDataState : public CostDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ CostDataState(Model* const model, pinocchio::Data* const data)
+ : CostDataAbstract(model, data), Arr_Rx(model->get_activation().get_nr(), model->get_state().get_ndx()) {
+ Arr_Rx.fill(0);
+ }
+
+ Eigen::MatrixXd Arr_Rx;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_COSTS_STATE_HPP_
diff --git a/include/crocoddyl/multibody/frames.hpp b/include/crocoddyl/multibody/frames.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fe99ce5475dd93b016b32b2a3b8d66361847c602
--- /dev/null
+++ b/include/crocoddyl/multibody/frames.hpp
@@ -0,0 +1,39 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_FRAMES_HPP_
+#define CROCODDYL_MULTIBODY_FRAMES_HPP_
+
+#include <Eigen/Dense>
+#include <pinocchio/spatial/se3.hpp>
+#include <pinocchio/spatial/motion.hpp>
+
+namespace crocoddyl {
+
+struct FrameTranslation {
+ FrameTranslation(unsigned int const& frame, const Eigen::Vector3d& oxf) : frame(frame), oxf(oxf) {}
+ unsigned int frame;
+ Eigen::Vector3d oxf;
+};
+
+struct FramePlacement {
+ FramePlacement(unsigned int const& frame, const pinocchio::SE3& oMf) : frame(frame), oMf(oMf) {}
+ unsigned int frame;
+ pinocchio::SE3 oMf;
+};
+
+struct FrameMotion {
+ FrameMotion(unsigned int const& frame, const pinocchio::Motion& oMf) : frame(frame), oMf(oMf) {}
+ unsigned int frame;
+ pinocchio::Motion oMf;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_FRAMES_HPP_
diff --git a/include/crocoddyl/multibody/impulse-base.hpp b/include/crocoddyl/multibody/impulse-base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..afab37a3d3cf7e905e8507c8f52579c924f81fad
--- /dev/null
+++ b/include/crocoddyl/multibody/impulse-base.hpp
@@ -0,0 +1,76 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
+#define CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
+
+#include "crocoddyl/multibody/states/multibody.hpp"
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/spatial/force.hpp>
+
+namespace crocoddyl {
+
+struct ImpulseDataAbstract; // forward declaration
+
+class ImpulseModelAbstract {
+ public:
+ ImpulseModelAbstract(StateMultibody& state, unsigned int const& ni);
+ ~ImpulseModelAbstract();
+
+ virtual void calc(const boost::shared_ptr<ImpulseDataAbstract>& data,
+ 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 boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::Data* const data);
+
+ StateMultibody& get_state() const;
+ unsigned int const& get_ni() const;
+
+ protected:
+ StateMultibody& state_;
+ unsigned int ni_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& x) { calc(data, x); }
+
+ void calcDiff_wrap(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::VectorXd& x,
+ const bool& recalc = true) {
+ calcDiff(data, x, recalc);
+ }
+
+#endif
+};
+
+struct ImpulseDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ ImpulseDataAbstract(Model* const model, pinocchio::Data* const data)
+ : pinocchio(data),
+ joint(0),
+ Jc(model->get_ni(), model->get_state().get_nv()),
+ Vq(model->get_ni(), model->get_state().get_nv()),
+ f(pinocchio::Force::Zero()) {
+ Jc.fill(0);
+ Vq.fill(0);
+ }
+
+ pinocchio::Data* pinocchio;
+ pinocchio::JointIndex joint;
+ Eigen::MatrixXd Jc;
+ Eigen::MatrixXd Vq;
+ pinocchio::Force f;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
diff --git a/include/crocoddyl/multibody/impulses/impulse-3d.hpp b/include/crocoddyl/multibody/impulses/impulse-3d.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dd5a16679f067166569cf8b123482aa840abb76
--- /dev/null
+++ b/include/crocoddyl/multibody/impulses/impulse-3d.hpp
@@ -0,0 +1,61 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
+#define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
+
+#include "crocoddyl/multibody/impulse-base.hpp"
+#include <pinocchio/spatial/motion.hpp>
+#include <pinocchio/multibody/data.hpp>
+
+namespace crocoddyl {
+
+class ImpulseModel3D : public ImpulseModelAbstract {
+ public:
+ ImpulseModel3D(StateMultibody& state, unsigned int const& frame);
+ ~ImpulseModel3D();
+
+ 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);
+ boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::Data* const data);
+
+ unsigned int const& get_frame() const;
+
+ private:
+ unsigned int frame_;
+};
+
+struct ImpulseData3D : public ImpulseDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ ImpulseData3D(Model* const model, pinocchio::Data* const data)
+ : ImpulseDataAbstract(model, data),
+ jMf(model->get_state().get_pinocchio().frames[model->get_frame()].placement),
+ fXj(jMf.inverse().toActionMatrix()),
+ fJf(6, model->get_state().get_nv()),
+ v_partial_dq(6, model->get_state().get_nv()),
+ v_partial_dv(6, model->get_state().get_nv()) {
+ joint = model->get_state().get_pinocchio().frames[model->get_frame()].parent;
+ fJf.fill(0);
+ v_partial_dq.fill(0);
+ v_partial_dv.fill(0);
+ }
+
+ pinocchio::SE3 jMf;
+ pinocchio::SE3::ActionMatrixType fXj;
+ pinocchio::Data::Matrix6x fJf;
+ pinocchio::Data::Matrix6x v_partial_dq;
+ pinocchio::Data::Matrix6x v_partial_dv;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
diff --git a/include/crocoddyl/multibody/impulses/impulse-6d.hpp b/include/crocoddyl/multibody/impulses/impulse-6d.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ef3ee71bdb173e6db934d48c5647bcf00e1363e
--- /dev/null
+++ b/include/crocoddyl/multibody/impulses/impulse-6d.hpp
@@ -0,0 +1,61 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
+#define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
+
+#include "crocoddyl/multibody/impulse-base.hpp"
+#include <pinocchio/spatial/motion.hpp>
+#include <pinocchio/multibody/data.hpp>
+
+namespace crocoddyl {
+
+class ImpulseModel6D : public ImpulseModelAbstract {
+ public:
+ ImpulseModel6D(StateMultibody& state, unsigned int const& frame);
+ ~ImpulseModel6D();
+
+ 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);
+ boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::Data* const data);
+
+ unsigned int const& get_frame() const;
+
+ private:
+ unsigned int frame_;
+};
+
+struct ImpulseData6D : public ImpulseDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ ImpulseData6D(Model* const model, pinocchio::Data* const data)
+ : ImpulseDataAbstract(model, data),
+ jMf(model->get_state().get_pinocchio().frames[model->get_frame()].placement),
+ fXj(jMf.inverse().toActionMatrix()),
+ fJf(6, model->get_state().get_nv()),
+ v_partial_dq(6, model->get_state().get_nv()),
+ v_partial_dv(6, model->get_state().get_nv()) {
+ joint = model->get_state().get_pinocchio().frames[model->get_frame()].parent;
+ fJf.fill(0);
+ v_partial_dq.fill(0);
+ v_partial_dv.fill(0);
+ }
+
+ pinocchio::SE3 jMf;
+ pinocchio::SE3::ActionMatrixType fXj;
+ pinocchio::Data::Matrix6x fJf;
+ pinocchio::Data::Matrix6x v_partial_dq;
+ pinocchio::Data::Matrix6x v_partial_dv;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
diff --git a/include/crocoddyl/multibody/impulses/multiple-impulses.hpp b/include/crocoddyl/multibody/impulses/multiple-impulses.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6f903b777b0484d56d1572cb74a5e689789c356f
--- /dev/null
+++ b/include/crocoddyl/multibody/impulses/multiple-impulses.hpp
@@ -0,0 +1,88 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
+#define CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
+
+#include <string>
+#include <map>
+#include <utility>
+#include "crocoddyl/multibody/impulse-base.hpp"
+
+namespace crocoddyl {
+
+struct ImpulseItem {
+ ImpulseItem() {}
+ ImpulseItem(const std::string& name, ImpulseModelAbstract* impulse) : name(name), impulse(impulse) {}
+
+ std::string name;
+ ImpulseModelAbstract* impulse;
+};
+
+struct ImpulseDataMultiple; // forward declaration
+
+class ImpulseModelMultiple {
+ public:
+ typedef std::map<std::string, ImpulseItem> ImpulseModelContainer;
+ typedef std::map<std::string, boost::shared_ptr<ImpulseDataAbstract> > ImpulseDataContainer;
+ typedef pinocchio::container::aligned_vector<pinocchio::Force>::iterator ForceIterator;
+
+ explicit ImpulseModelMultiple(StateMultibody& state);
+ ~ImpulseModelMultiple();
+
+ void addImpulse(const std::string& name, ImpulseModelAbstract* const impulse);
+ void removeImpulse(const std::string& name);
+
+ 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);
+ boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::Data* const data);
+
+ StateMultibody& get_state() const;
+ const ImpulseModelContainer& get_impulses() const;
+ const unsigned int& get_ni() const;
+
+ private:
+ StateMultibody& state_;
+ ImpulseModelContainer impulses_;
+ unsigned int ni_;
+
+#ifdef PYTHON_BINDINGS
+
+ public:
+ void calc_wrap(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::VectorXd& x) { calc(data, x); }
+
+ void calcDiff_wrap(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::VectorXd& x,
+ const bool& recalc = true) {
+ calcDiff(data, x, recalc);
+ }
+
+#endif
+};
+
+struct ImpulseDataMultiple : ImpulseDataAbstract {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ template <typename Model>
+ ImpulseDataMultiple(Model* const model, pinocchio::Data* const data)
+ : ImpulseDataAbstract(model, data), fext(model->get_state().get_pinocchio().njoints, pinocchio::Force::Zero()) {
+ for (ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
+ it != model->get_impulses().end(); ++it) {
+ const ImpulseItem& item = it->second;
+ impulses.insert(std::make_pair(item.name, item.impulse->createData(data)));
+ }
+ }
+
+ ImpulseModelMultiple::ImpulseDataContainer impulses;
+ pinocchio::container::aligned_vector<pinocchio::Force> fext;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
diff --git a/include/crocoddyl/multibody/states/multibody.hpp b/include/crocoddyl/multibody/states/multibody.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2eea72cfa4ce21c29cd16b077f2fa340ca9f78eb
--- /dev/null
+++ b/include/crocoddyl/multibody/states/multibody.hpp
@@ -0,0 +1,52 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
+#define CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
+
+#include "crocoddyl/core/state-base.hpp"
+#include <pinocchio/multibody/model.hpp>
+
+namespace crocoddyl {
+
+class StateMultibody : public StateAbstract {
+ public:
+ explicit StateMultibody(pinocchio::Model& model);
+ ~StateMultibody();
+
+ Eigen::VectorXd zero();
+ Eigen::VectorXd rand();
+ void diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::VectorXd> dxout);
+ void integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::VectorXd> xout);
+ void Jdiff(const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>&,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond, Jcomponent firstsecond = both);
+ void Jintegrate(const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>&,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond = both);
+
+ pinocchio::Model& get_pinocchio() const;
+
+ private:
+ void updateJdiff(const Eigen::Ref<const Eigen::MatrixXd>& J, bool positive = true);
+
+ pinocchio::Model& pinocchio_;
+ Eigen::VectorXd x0_;
+ Eigen::VectorXd dx_;
+ Eigen::VectorXd q0_;
+ Eigen::VectorXd dq0_;
+ Eigen::VectorXd q1_;
+ Eigen::VectorXd dq1_;
+ Eigen::MatrixXd Ji_;
+ Eigen::MatrixXd Jd_;
+};
+
+} // namespace crocoddyl
+
+#endif // CROCODDYL_MULTIBODY_STATES_MULTIBODY_HPP_
diff --git a/setup.cfg b/setup.cfg
index 6fabdb553f5704bdf306904b2a47e6b5808ba5cb..1d4137224e4970e2ae2612209690e0840204d7f0 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -1,6 +1,6 @@
[flake8]
max-line-length = 119
-exclude = examples/notebooks,crocoddyl/__init__.py,crocoddyl/locomotion/__init__.py,cmake,unittest/sandbox
+exclude = examples/notebooks,bindings/python/crocoddyl/__init__.py,crocoddyl/__init__.py,crocoddyl/locomotion/__init__.py,cmake,unittest/python/sandbox
ignore = N802,N803,N806,W504
[yapf]
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4f49c86a7a6212967c26dad1bc72b6fa7731030d
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,60 @@
+SET(${PROJECT_NAME}_SOURCES
+ core/state-base.cpp
+ core/actuation-base.cpp
+ core/action-base.cpp
+ core/diff-action-base.cpp
+ core/activation-base.cpp
+ core/solver-base.cpp
+ core/integrator/euler.cpp
+ core/numdiff/state.cpp
+ core/numdiff/action.cpp
+ core/numdiff/diff-action.cpp
+ core/utils/callbacks.cpp
+ core/optctrl/shooting.cpp
+ core/solvers/ddp.cpp
+ core/solvers/fddp.cpp
+ core/states/euclidean.cpp
+ core/actions/unicycle.cpp
+ core/actions/lqr.cpp
+ core/actions/diff-lqr.cpp
+ core/activations/quadratic.cpp
+ core/activations/weighted-quadratic.cpp
+ multibody/cost-base.cpp
+ multibody/contact-base.cpp
+ multibody/impulse-base.cpp
+ multibody/impulses/multiple-impulses.cpp
+ multibody/impulses/impulse-3d.cpp
+ multibody/impulses/impulse-6d.cpp
+ multibody/states/multibody.cpp
+ multibody/actuations/floating-base.cpp
+ multibody/actuations/full.cpp
+ multibody/costs/cost-sum.cpp
+ multibody/costs/state.cpp
+ multibody/costs/control.cpp
+ multibody/costs/com-position.cpp
+ multibody/costs/frame-placement.cpp
+ multibody/costs/frame-translation.cpp
+ multibody/costs/frame-velocity.cpp
+ multibody/contacts/multiple-contacts.cpp
+ multibody/contacts/contact-3d.cpp
+ multibody/contacts/contact-6d.cpp
+ multibody/actions/free-fwddyn.cpp
+ multibody/actions/contact-fwddyn.cpp
+ )
+
+IF(UNIX)
+ ADD_LIBRARY(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES})
+ SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
+ PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} eigen3)
+ PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} pinocchio)
+ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_SERIALIZATION_LIBRARY})
+
+ if(OPENMP_FOUND)
+ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenMP_CXX_LIBRARIES})
+ ENDIF()
+
+ INSTALL(TARGETS ${PROJECT_NAME} DESTINATION lib)
+ INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/include/
+ DESTINATION include
+ FILES_MATCHING PATTERN "*.h*")
+ENDIF(UNIX)
diff --git a/src/core/action-base.cpp b/src/core/action-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a661bee4d312da394470dc9dcbc42c2ae7ecb45d
--- /dev/null
+++ b/src/core/action-base.cpp
@@ -0,0 +1,60 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/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");
+}
+
+ActionModelAbstract::~ActionModelAbstract() {}
+
+void ActionModelAbstract::calc(const boost::shared_ptr<ActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x) {
+ calc(data, x, unone_);
+}
+
+void ActionModelAbstract::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x) {
+ calcDiff(data, x, unone_);
+}
+
+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(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;
+ }
+ }
+}
+
+boost::shared_ptr<ActionDataAbstract> ActionModelAbstract::createData() {
+ return boost::make_shared<ActionDataAbstract>(this);
+}
+
+const unsigned int& ActionModelAbstract::get_nu() const { return nu_; }
+
+const unsigned int& ActionModelAbstract::get_nr() const { return nr_; }
+
+StateAbstract& ActionModelAbstract::get_state() const { return state_; }
+
+} // namespace crocoddyl
diff --git a/src/core/actions/diff-lqr.cpp b/src/core/actions/diff-lqr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11bc8a5ec033729c5567ac000ee7a782689be185
--- /dev/null
+++ b/src/core/actions/diff-lqr.cpp
@@ -0,0 +1,68 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/diff-lqr.hpp"
+
+namespace crocoddyl {
+
+DifferentialActionModelLQR::DifferentialActionModelLQR(unsigned int const& nq, unsigned int const& nu, bool drift_free)
+ : DifferentialActionModelAbstract(*new StateVector(2 * nq), nu), drift_free_(drift_free) {
+ // TODO(cmastalli): substitute by random (vectors) and random-orthogonal (matrices)
+ Fq_ = Eigen::MatrixXd::Identity(state_.get_nq(), state_.get_nq());
+ Fv_ = Eigen::MatrixXd::Identity(state_.get_nv(), state_.get_nv());
+ Fu_ = Eigen::MatrixXd::Identity(state_.get_nq(), nu_);
+ f0_ = Eigen::VectorXd::Ones(state_.get_nv());
+ Lxx_ = Eigen::MatrixXd::Identity(state_.get_nx(), state_.get_nx());
+ Lxu_ = Eigen::MatrixXd::Identity(state_.get_nx(), nu_);
+ Luu_ = Eigen::MatrixXd::Identity(nu_, nu_);
+ lx_ = Eigen::VectorXd::Ones(state_.get_nx());
+ lu_ = Eigen::VectorXd::Ones(nu_);
+}
+
+DifferentialActionModelLQR::~DifferentialActionModelLQR() {}
+
+void DifferentialActionModelLQR::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");
+
+ const Eigen::VectorXd& q = x.head(state_.get_nq());
+ const Eigen::VectorXd& v = x.tail(state_.get_nv());
+ if (drift_free_) {
+ data->xout = Fq_ * q + Fv_ * v + Fu_ * u;
+ } else {
+ data->xout = Fq_ * q + Fv_ * v + Fu_ * u + f0_;
+ }
+ data->cost = 0.5 * x.dot(Lxx_ * x) + 0.5 * u.dot(Luu_ * u) + x.dot(Lxu_ * u) + lx_.dot(x) + lu_.dot(u);
+}
+
+void DifferentialActionModelLQR::calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& 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");
+
+ if (recalc) {
+ calc(data, x, u);
+ }
+ data->Lx = lx_ + Lxx_ * x + Lxu_ * u;
+ data->Lu = lu_ + Lxu_.transpose() * x + Luu_ * u;
+ data->Fx.leftCols(state_.get_nq()) = Fq_;
+ data->Fx.rightCols(state_.get_nv()) = Fv_;
+ data->Fu = Fu_;
+ data->Lxx = Lxx_;
+ data->Lxu = Lxu_;
+ data->Luu = Luu_;
+}
+
+boost::shared_ptr<DifferentialActionDataAbstract> DifferentialActionModelLQR::createData() {
+ return boost::make_shared<DifferentialActionDataLQR>(this);
+}
+
+} // namespace crocoddyl
diff --git a/src/core/actions/lqr.cpp b/src/core/actions/lqr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..142504504ee74d34d455e421e9cb372cd0060a26
--- /dev/null
+++ b/src/core/actions/lqr.cpp
@@ -0,0 +1,63 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/lqr.hpp"
+
+namespace crocoddyl {
+
+ActionModelLQR::ActionModelLQR(unsigned int const& nx, unsigned int const& nu, bool drift_free)
+ : ActionModelAbstract(*new StateVector(nx), nu, 0), drift_free_(drift_free) {
+ // TODO(cmastalli): substitute by random (vectors) and random-orthogonal (matrices)
+ Fx_ = Eigen::MatrixXd::Identity(nx, nx);
+ Fu_ = Eigen::MatrixXd::Identity(nx, nu);
+ f0_ = Eigen::VectorXd::Ones(nx);
+ Lxx_ = Eigen::MatrixXd::Identity(nx, nx);
+ Lxu_ = Eigen::MatrixXd::Identity(nx, nu);
+ Luu_ = Eigen::MatrixXd::Identity(nu, nu);
+ lx_ = Eigen::VectorXd::Ones(nx);
+ lu_ = Eigen::VectorXd::Ones(nu);
+}
+
+ActionModelLQR::~ActionModelLQR() {
+ // delete state_; //TODO @Carlos this breaks the test_actions c++ unit-test
+}
+
+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");
+
+ if (drift_free_) {
+ data->xnext = Fx_ * x + Fu_ * u;
+ } else {
+ data->xnext = Fx_ * x + Fu_ * u + f0_;
+ }
+ data->cost = 0.5 * x.dot(Lxx_ * x) + 0.5 * u.dot(Luu_ * u) + x.dot(Lxu_ * u) + lx_.dot(x) + lu_.dot(u);
+}
+
+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");
+
+ if (recalc) {
+ calc(data, x, u);
+ }
+ data->Lx = lx_ + Lxx_ * x + Lxu_ * u;
+ data->Lu = lu_ + Lxu_.transpose() * x + Luu_ * u;
+ data->Fx = Fx_;
+ data->Fu = Fu_;
+ data->Lxx = Lxx_;
+ data->Lxu = Lxu_;
+ data->Luu = Luu_;
+}
+
+boost::shared_ptr<ActionDataAbstract> ActionModelLQR::createData() { return boost::make_shared<ActionDataLQR>(this); }
+
+} // namespace crocoddyl
diff --git a/src/core/actions/unicycle.cpp b/src/core/actions/unicycle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f63d66968cfae382055731c178b5ebe9f7c8dc6f
--- /dev/null
+++ b/src/core/actions/unicycle.cpp
@@ -0,0 +1,70 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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"
+
+namespace crocoddyl {
+
+ActionModelUnicycle::ActionModelUnicycle() : ActionModelAbstract(*new StateVector(3), 2, 5), dt_(0.1) {
+ cost_weights_ << 10., 1.;
+}
+
+ActionModelUnicycle::~ActionModelUnicycle() {
+ // delete state_; //TODO @Carlos this breaks the test_actions c++ unit-test
+}
+
+void ActionModelUnicycle::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");
+
+ ActionDataUnicycle* d = static_cast<ActionDataUnicycle*>(data.get());
+ const double& c = std::cos(x[2]);
+ const double& s = std::sin(x[2]);
+ d->xnext << x[0] + c * u[0] * dt_, x[1] + s * u[0] * dt_, x[2] + u[1] * dt_;
+ d->r.head<3>() = cost_weights_[0] * x;
+ d->r.tail<2>() = cost_weights_[1] * u;
+ d->cost = 0.5 * d->r.transpose() * d->r;
+}
+
+void ActionModelUnicycle::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");
+
+ if (recalc) {
+ calc(data, x, u);
+ }
+ ActionDataUnicycle* d = static_cast<ActionDataUnicycle*>(data.get());
+
+ // Cost derivatives
+ const double& w_x = cost_weights_[0] * cost_weights_[0];
+ const double& w_u = cost_weights_[1] * cost_weights_[1];
+ d->Lx = x.cwiseProduct(Eigen::VectorXd::Constant(state_.get_nx(), w_x));
+ d->Lu = u.cwiseProduct(Eigen::VectorXd::Constant(nu_, w_u));
+ d->Lxx.diagonal() << w_x, w_x, w_x;
+ d->Luu.diagonal() << w_u, w_u;
+
+ // Dynamic derivatives
+ const double& c = std::cos(x[2]);
+ const double& s = std::sin(x[2]);
+ d->Fx << 1., 0., -s * u[0] * dt_, 0., 1., c * u[0] * dt_, 0., 0., 1.;
+ d->Fu << c * dt_, 0., s * dt_, 0., 0., dt_;
+}
+
+boost::shared_ptr<ActionDataAbstract> ActionModelUnicycle::createData() {
+ return boost::make_shared<ActionDataUnicycle>(this);
+}
+
+const Eigen::Vector2d& ActionModelUnicycle::get_cost_weights() const { return cost_weights_; }
+
+void ActionModelUnicycle::set_cost_weights(const Eigen::Vector2d& weights) { cost_weights_ = weights; }
+
+} // namespace crocoddyl
diff --git a/src/core/activation-base.cpp b/src/core/activation-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..691e36c1c0d0ac822dffad31bdfea42a14733d0a
--- /dev/null
+++ b/src/core/activation-base.cpp
@@ -0,0 +1,23 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/activation-base.hpp"
+
+namespace crocoddyl {
+
+ActivationModelAbstract::ActivationModelAbstract(unsigned int const& nr) : nr_(nr) {}
+
+ActivationModelAbstract::~ActivationModelAbstract() {}
+
+boost::shared_ptr<ActivationDataAbstract> ActivationModelAbstract::createData() {
+ return boost::make_shared<ActivationDataAbstract>(this);
+}
+
+unsigned int const& ActivationModelAbstract::get_nr() const { return nr_; }
+
+} // namespace crocoddyl
diff --git a/src/core/activations/quadratic.cpp b/src/core/activations/quadratic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dda15a23c17b47a2d7b056ceed3e3f2930abf669
--- /dev/null
+++ b/src/core/activations/quadratic.cpp
@@ -0,0 +1,40 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/activations/quadratic.hpp"
+
+namespace crocoddyl {
+
+ActivationModelQuad::ActivationModelQuad(unsigned int const& nr) : ActivationModelAbstract(nr) {}
+
+ActivationModelQuad::~ActivationModelQuad() {}
+
+void ActivationModelQuad::calc(const boost::shared_ptr<ActivationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& r) {
+ assert(r.size() == nr_ && "r has wrong dimension");
+ data->a_value = 0.5 * r.transpose() * r;
+}
+
+void ActivationModelQuad::calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& r, const bool& recalc) {
+ assert(r.size() == nr_ && "r has wrong dimension");
+ if (recalc) {
+ calc(data, r);
+ }
+ data->Ar = r;
+ // The Hessian has constant values which were set in createData.
+ assert(data->Arr == Eigen::MatrixXd::Identity(nr_, nr_) && "Arr has wrong value");
+}
+
+boost::shared_ptr<ActivationDataAbstract> ActivationModelQuad::createData() {
+ boost::shared_ptr<ActivationDataAbstract> data = boost::make_shared<ActivationDataAbstract>(this);
+ data->Arr.diagonal().fill(1.);
+ return data;
+}
+
+} // namespace crocoddyl
diff --git a/src/core/activations/weighted-quadratic.cpp b/src/core/activations/weighted-quadratic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5e9de585605eb750c745b714deb820182877f241
--- /dev/null
+++ b/src/core/activations/weighted-quadratic.cpp
@@ -0,0 +1,53 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/activations/weighted-quadratic.hpp"
+
+namespace crocoddyl {
+
+ActivationModelWeightedQuad::ActivationModelWeightedQuad(const Eigen::VectorXd& weights)
+ : ActivationModelAbstract((unsigned int)weights.size()), weights_(weights) {}
+
+ActivationModelWeightedQuad::~ActivationModelWeightedQuad() {}
+
+void ActivationModelWeightedQuad::calc(const boost::shared_ptr<ActivationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& r) {
+ assert(r.size() == nr_ && "r has wrong dimension");
+ boost::shared_ptr<ActivationDataWeightedQuad> d = boost::static_pointer_cast<ActivationDataWeightedQuad>(data);
+
+ d->Wr = weights_.cwiseProduct(r);
+ data->a_value = 0.5 * r.transpose() * d->Wr;
+}
+
+void ActivationModelWeightedQuad::calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& r, const bool& recalc) {
+ assert(r.size() == nr_ && "r has wrong dimension");
+ if (recalc) {
+ calc(data, r);
+ }
+
+ boost::shared_ptr<ActivationDataWeightedQuad> d = boost::static_pointer_cast<ActivationDataWeightedQuad>(data);
+ data->Ar = d->Wr;
+ // The Hessian has constant values which were set in createData.
+ assert(data->Arr == Arr_ && "Arr has wrong value");
+}
+
+boost::shared_ptr<ActivationDataAbstract> ActivationModelWeightedQuad::createData() {
+ boost::shared_ptr<ActivationDataWeightedQuad> data = boost::make_shared<ActivationDataWeightedQuad>(this);
+ data->Arr.diagonal() = weights_;
+
+#ifndef NDEBUG
+ Arr_ = data->Arr;
+#endif
+
+ return data;
+}
+
+const Eigen::VectorXd& ActivationModelWeightedQuad::get_weights() const { return weights_; }
+
+} // namespace crocoddyl
diff --git a/src/core/actuation-base.cpp b/src/core/actuation-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8d54546f8facaaafd0f52fe0a2a4f3a37cf1ce91
--- /dev/null
+++ b/src/core/actuation-base.cpp
@@ -0,0 +1,27 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/actuation-base.hpp"
+
+namespace crocoddyl {
+
+ActuationModelAbstract::ActuationModelAbstract(StateAbstract& state, unsigned int const& nu) : nu_(nu), state_(state) {
+ assert(nu_ != 0 && "nu cannot be zero");
+}
+
+ActuationModelAbstract::~ActuationModelAbstract() {}
+
+boost::shared_ptr<ActuationDataAbstract> ActuationModelAbstract::createData() {
+ return boost::make_shared<ActuationDataAbstract>(this);
+}
+
+unsigned int const& ActuationModelAbstract::get_nu() const { return nu_; }
+
+StateAbstract& ActuationModelAbstract::get_state() const { return state_; }
+
+} // namespace crocoddyl
diff --git a/src/core/diff-action-base.cpp b/src/core/diff-action-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..619d1f0f3e2891e977dee80d948352c9da867173
--- /dev/null
+++ b/src/core/diff-action-base.cpp
@@ -0,0 +1,42 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/diff-action-base.hpp"
+
+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");
+}
+
+DifferentialActionModelAbstract::~DifferentialActionModelAbstract() {}
+
+void DifferentialActionModelAbstract::calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x) {
+ calc(data, x, unone_);
+}
+
+void DifferentialActionModelAbstract::calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x) {
+ calcDiff(data, x, unone_);
+}
+
+boost::shared_ptr<DifferentialActionDataAbstract> DifferentialActionModelAbstract::createData() {
+ return boost::make_shared<DifferentialActionDataAbstract>(this);
+}
+
+unsigned int const& DifferentialActionModelAbstract::get_nu() const { return nu_; }
+
+unsigned int const& DifferentialActionModelAbstract::get_nr() const { return nr_; }
+
+StateAbstract& DifferentialActionModelAbstract::get_state() const { return state_; }
+
+} // namespace crocoddyl
diff --git a/src/core/integrator/euler.cpp b/src/core/integrator/euler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e0112407eaf94e47bf36fe032f75cfbe3d8f9b2f
--- /dev/null
+++ b/src/core/integrator/euler.cpp
@@ -0,0 +1,95 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/integrator/euler.hpp"
+
+namespace crocoddyl {
+
+IntegratedActionModelEuler::IntegratedActionModelEuler(DifferentialActionModelAbstract* const model,
+ const double& time_step, const bool& with_cost_residual)
+ : ActionModelAbstract(model->get_state(), model->get_nu(), model->get_nr()),
+ differential_(model),
+ time_step_(time_step),
+ time_step2_(time_step * time_step),
+ with_cost_residual_(with_cost_residual) {}
+
+IntegratedActionModelEuler::~IntegratedActionModelEuler() {}
+
+void IntegratedActionModelEuler::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");
+
+ // Static casting the data
+ boost::shared_ptr<IntegratedActionDataEuler> d = boost::static_pointer_cast<IntegratedActionDataEuler>(data);
+
+ // Computing the acceleration and cost
+ differential_->calc(d->differential, x, u);
+
+ // Computing the next state (discrete time)
+ const Eigen::VectorXd& v = x.tail(differential_->get_state().get_nv());
+ const Eigen::VectorXd& a = d->differential->xout;
+ d->dx << v * time_step_ + a * time_step2_, a * time_step_;
+ differential_->get_state().integrate(x, d->dx, d->xnext);
+
+ // Updating the cost value
+ if (with_cost_residual_) {
+ d->r = d->differential->r;
+ }
+ d->cost = d->differential->cost;
+}
+
+void IntegratedActionModelEuler::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");
+
+ const unsigned int& nv = differential_->get_state().get_nv();
+ if (recalc) {
+ calc(data, x, u);
+ }
+
+ // Static casting the data
+ boost::shared_ptr<IntegratedActionDataEuler> d = boost::static_pointer_cast<IntegratedActionDataEuler>(data);
+
+ // Computing the derivatives for the time-continuous model (i.e. differential model)
+ differential_->calcDiff(d->differential, x, u, false);
+ differential_->get_state().Jintegrate(x, d->dx, d->dxnext_dx, d->dxnext_ddx);
+
+ const Eigen::MatrixXd& da_dx = d->differential->Fx;
+ const Eigen::MatrixXd& da_du = d->differential->Fu;
+ d->ddx_dx << da_dx * time_step_, da_dx;
+ d->ddx_du << da_du * time_step_, da_du;
+ for (unsigned int i = 0; i < nv; ++i) {
+ d->ddx_dx(i, i + nv) += 1.;
+ }
+ 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();
+}
+
+boost::shared_ptr<ActionDataAbstract> IntegratedActionModelEuler::createData() {
+ return boost::make_shared<IntegratedActionDataEuler>(this);
+}
+
+DifferentialActionModelAbstract* IntegratedActionModelEuler::get_differential() const { return differential_; }
+
+const double& IntegratedActionModelEuler::get_dt() const { return time_step_; }
+
+void IntegratedActionModelEuler::set_dt(double dt) {
+ time_step_ = dt;
+ time_step2_ = dt * dt;
+}
+
+} // namespace crocoddyl
diff --git a/src/core/numdiff/action.cpp b/src/core/numdiff/action.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e78443674607ae7406d0461f3b210016681e1954
--- /dev/null
+++ b/src/core/numdiff/action.cpp
@@ -0,0 +1,129 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include "crocoddyl/core/numdiff/action.hpp"
+
+namespace crocoddyl {
+
+ActionModelNumDiff::ActionModelNumDiff(ActionModelAbstract& model, bool with_gauss_approx)
+ : 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() {}
+
+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");
+ 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;
+ data->xnext = data_nd->data_0->xnext;
+}
+
+void ActionModelNumDiff::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");
+ boost::shared_ptr<ActionDataNumDiff> data_nd = boost::static_pointer_cast<ActionDataNumDiff>(data);
+
+ if (recalc) {
+ model_.calc(data_nd->data_0, x, u);
+ }
+ const Eigen::VectorXd& xn0 = data_nd->data_0->xnext;
+ const double& c0 = data_nd->data_0->cost;
+ data->xnext = data_nd->data_0->xnext;
+ data->cost = data_nd->data_0->cost;
+
+ assertStableStateFD(x);
+
+ // Computing the d action(x,u) / dx
+ data_nd->dx.setZero();
+ for (unsigned int ix = 0; ix < state_.get_ndx(); ++ix) {
+ data_nd->dx(ix) = disturbance_;
+ model_.get_state().integrate(x, data_nd->dx, data_nd->xp);
+ model_.calc(data_nd->data_x[ix], data_nd->xp, u);
+
+ const Eigen::VectorXd& xn = data_nd->data_x[ix]->xnext;
+ const double& c = data_nd->data_x[ix]->cost;
+ 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_;
+ data_nd->dx(ix) = 0.0;
+ }
+ data->Fx /= disturbance_;
+
+ // Computing the d action(x,u) / du
+ data_nd->du.setZero();
+ for (unsigned iu = 0; iu < model_.get_nu(); ++iu) {
+ data_nd->du(iu) = disturbance_;
+ model_.calc(data_nd->data_u[iu], x, u + data_nd->du);
+
+ const Eigen::VectorXd& xn = data_nd->data_u[iu]->xnext;
+ const double& c = data_nd->data_u[iu]->cost;
+ 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_;
+ data_nd->du(iu) = 0.0;
+ }
+ data->Fu /= disturbance_;
+
+ if (with_gauss_approx_) {
+ 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;
+ }
+}
+
+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_; }
+
+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())
+ // }
+ // }
+ // }
+ // }
+ // }
+}
+
+boost::shared_ptr<ActionDataAbstract> ActionModelNumDiff::createData() {
+ return boost::make_shared<ActionDataNumDiff>(this);
+}
+
+} // namespace crocoddyl
diff --git a/src/core/numdiff/diff-action.cpp b/src/core/numdiff/diff-action.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5446b30be1660532fc33ce3b858d029e5849b18f
--- /dev/null
+++ b/src/core/numdiff/diff-action.cpp
@@ -0,0 +1,104 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include "crocoddyl/core/numdiff/diff-action.hpp"
+
+namespace crocoddyl {
+
+DifferentialActionModelNumDiff::DifferentialActionModelNumDiff(DifferentialActionModelAbstract& model,
+ bool with_gauss_approx)
+ : DifferentialActionModelAbstract(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");
+}
+
+DifferentialActionModelNumDiff::~DifferentialActionModelNumDiff() {}
+
+void DifferentialActionModelNumDiff::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");
+ DifferentialActionDataNumDiff* data_nd = static_cast<DifferentialActionDataNumDiff*>(data.get());
+ model_.calc(data_nd->data_0, x, u);
+ data->cost = data_nd->data_0->cost;
+ data->xout = data_nd->data_0->xout;
+}
+
+void DifferentialActionModelNumDiff::calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& 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");
+ boost::shared_ptr<DifferentialActionDataNumDiff> data_nd =
+ boost::static_pointer_cast<DifferentialActionDataNumDiff>(data);
+
+ if (recalc) {
+ model_.calc(data_nd->data_0, x, u);
+ }
+ const Eigen::VectorXd& xn0 = data_nd->data_0->xout;
+ const double& c0 = data_nd->data_0->cost;
+ data->xout = data_nd->data_0->xout;
+ data->cost = data_nd->data_0->cost;
+
+ assertStableStateFD(x);
+
+ // Computing the d action(x,u) / dx
+ data_nd->dx.setZero();
+ for (unsigned int ix = 0; ix < state_.get_ndx(); ++ix) {
+ data_nd->dx(ix) = disturbance_;
+ model_.get_state().integrate(x, data_nd->dx, data_nd->xp);
+ model_.calc(data_nd->data_x[ix], data_nd->xp, u);
+
+ const Eigen::VectorXd& xn = data_nd->data_x[ix]->xout;
+ const double& c = data_nd->data_x[ix]->cost;
+ data->Fx.col(ix) = (xn - xn0) / disturbance_;
+
+ data->Lx(ix) = (c - c0) / disturbance_;
+ data_nd->Rx.col(ix) = (data_nd->data_x[ix]->r - data_nd->data_0->r) / disturbance_;
+ data_nd->dx(ix) = 0.0;
+ }
+
+ // Computing the d action(x,u) / du
+ data_nd->du.setZero();
+ for (unsigned iu = 0; iu < model_.get_nu(); ++iu) {
+ data_nd->du(iu) = disturbance_;
+ model_.calc(data_nd->data_u[iu], x, u + data_nd->du);
+
+ const Eigen::VectorXd& xn = data_nd->data_u[iu]->xout;
+ const double& c = data_nd->data_u[iu]->cost;
+ data->Fu.col(iu) = (xn - xn0) / disturbance_;
+
+ data->Lu(iu) = (c - c0) / disturbance_;
+ data_nd->Ru.col(iu) = (data_nd->data_u[iu]->r - data_nd->data_0->r) / disturbance_;
+ data_nd->du(iu) = 0.0;
+ }
+
+ if (with_gauss_approx_) {
+ 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;
+ }
+}
+
+DifferentialActionModelAbstract& DifferentialActionModelNumDiff::get_model() const { return model_; }
+
+const double& DifferentialActionModelNumDiff::get_disturbance() const { return disturbance_; }
+
+bool DifferentialActionModelNumDiff::get_with_gauss_approx() { return with_gauss_approx_; }
+
+void DifferentialActionModelNumDiff::assertStableStateFD(const Eigen::Ref<const Eigen::VectorXd>& /** x */) {
+ // TODO(cmastalli): First we need to do it AMNumDiff and then to replicate it.
+}
+
+boost::shared_ptr<DifferentialActionDataAbstract> DifferentialActionModelNumDiff::createData() {
+ return boost::make_shared<DifferentialActionDataNumDiff>(this);
+}
+
+} // namespace crocoddyl
diff --git a/src/core/numdiff/state.cpp b/src/core/numdiff/state.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d416c27c9cd9ef9f70dbccdd6bb4924ef80daec3
--- /dev/null
+++ b/src/core/numdiff/state.cpp
@@ -0,0 +1,139 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include "crocoddyl/core/numdiff/state.hpp"
+
+namespace crocoddyl {
+
+StateNumDiff::StateNumDiff(StateAbstract& state) : StateAbstract(state.get_nx(), state.get_ndx()), state_(state) {
+ disturbance_ = 1e-6;
+ // disturbance vector
+ dx_.resize(ndx_);
+ dx_.setZero();
+ // State around which to compute the finite integrate-operator jacobians
+ x0_.resize(nx_);
+ x0_.setZero();
+ // State difference around which to compute the finite difference-operator jacobians
+ dx0_.resize(ndx_);
+ dx0_.setZero();
+ // temporary variable needed
+ tmp_x_.resize(nx_);
+ tmp_x_.setZero();
+}
+
+StateNumDiff::~StateNumDiff() {}
+
+Eigen::VectorXd StateNumDiff::zero() { return state_.zero(); }
+
+Eigen::VectorXd StateNumDiff::rand() { return state_.rand(); }
+
+void StateNumDiff::diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::VectorXd> dxout) {
+ assert(x0.size() == nx_ && "x0 has wrong dimension");
+ assert(x1.size() == nx_ && "x1 has wrong dimension");
+ assert(dxout.size() == ndx_ && "output must be pre-allocated");
+ state_.diff(x0, x1, dxout);
+}
+
+void StateNumDiff::integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::VectorXd> xout) {
+ assert(x.size() == nx_ && "x has wrong dimension");
+ assert(dx.size() == ndx_ && "dx has wrong dimension");
+ assert(xout.size() == nx_ && "output must be pre-allocated");
+ state_.integrate(x, dx, xout);
+}
+
+void StateNumDiff::Jdiff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond) {
+ assert(is_a_Jcomponent(firstsecond) && ("firstsecond must be one of the Jcomponent {both, first, second}"));
+ assert(x0.size() == nx_ && "x0 has wrong dimension");
+ assert(x1.size() == nx_ && "x1 has wrong dimension");
+
+ dx_.setZero();
+ diff(x0, x1, dx0_);
+ if (firstsecond == first || firstsecond == both) {
+ assert(Jfirst.rows() == ndx_ && Jfirst.cols() == ndx_ && "Jfirst must be of the good size");
+ Jfirst.setZero();
+ for (unsigned int i = 0; i < ndx_; ++i) {
+ dx_(i) = disturbance_;
+ // tmp_x = int(x0, dx)
+ integrate(x0, dx_, tmp_x_);
+ // Jfirst[:,k] = diff(tmp_x, x1) = diff(int(x0 + dx), x1)
+ diff(tmp_x_, x1, Jfirst.col(i));
+ // Jfirst[:,k] = Jfirst[:,k] - tmp_dx_, or
+ // Jfirst[:,k] = Jfirst[:,k] - diff(x0, x1)
+ Jfirst.col(i) -= dx0_;
+ dx_(i) = 0.0;
+ }
+ Jfirst /= disturbance_;
+ }
+ if (firstsecond == second || firstsecond == both) {
+ assert(Jsecond.rows() == ndx_ && Jsecond.cols() == ndx_ && "Jfirst must be of the good size");
+
+ Jsecond.setZero();
+ for (unsigned int i = 0; i < ndx_; ++i) {
+ dx_(i) = disturbance_;
+ // tmp_x = int(x1 + dx)
+ integrate(x1, dx_, tmp_x_);
+ // Jsecond[:,k] = diff(x0, tmp_x) = diff(x0, int(x1 + dx))
+ diff(x0, tmp_x_, Jsecond.col(i));
+ // Jsecond[:,k] = J[:,k] - tmp_dx_
+ // Jsecond[:,k] = Jsecond[:,k] - diff(x0, x1)
+ Jsecond.col(i) -= dx0_;
+ dx_(i) = 0.0;
+ }
+ Jsecond /= disturbance_;
+ }
+}
+
+void StateNumDiff::Jintegrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond) {
+ assert((firstsecond == first || firstsecond == second || firstsecond == both) &&
+ ("firstsecond must be one of the Jcomponent {both, first, second}"));
+ assert(x.size() == nx_ && "x has wrong dimension");
+ assert(dx.size() == ndx_ && "dx has wrong dimension");
+
+ dx_.setZero();
+ // x0_ = integrate(x, dx)
+ integrate(x, dx, x0_);
+
+ if (firstsecond == first || firstsecond == both) {
+ assert(Jfirst.rows() == ndx_ && Jfirst.cols() == ndx_ && "Jfirst must be of the good size");
+ Jfirst.setZero();
+ for (unsigned int i = 0; i < ndx_; ++i) {
+ dx_(i) = disturbance_;
+ // tmp_x_ = integrate(x, dx_) = integrate(x, disturbance_vector)
+ integrate(x, dx_, tmp_x_);
+ // tmp_x_ = integrate(tmp_x_, dx) = integrate(integrate(x, dx_), dx)
+ integrate(tmp_x_, dx, tmp_x_);
+ // Jfirst[:,i] = diff(x0_, tmp_x_)
+ // Jfirst[:,i] = diff( integrate(x, dx), integrate(integrate(x, dx_), dx))
+ diff(x0_, tmp_x_, Jfirst.col(i));
+ dx_(i) = 0.0;
+ }
+ Jfirst /= disturbance_;
+ }
+ if (firstsecond == second || firstsecond == both) {
+ assert(Jsecond.rows() == ndx_ && Jsecond.cols() == ndx_ && "Jfirst must be of the good size");
+ Jsecond.setZero();
+ for (unsigned int i = 0; i < ndx_; ++i) {
+ dx_(i) = disturbance_;
+ // tmp_x_ = integrate(x, dx + dx_) = integrate(x, dx + disturbance_vector)
+ integrate(x, dx + dx_, tmp_x_);
+ // Jsecond[:,i] = diff(x0_, tmp_x_)
+ // Jsecond[:,i] = diff( integrate(x, dx), integrate(x, dx_ + dx) )
+ diff(x0_, tmp_x_, Jsecond.col(i));
+ dx_(i) = 0.0;
+ }
+ Jsecond /= disturbance_;
+ }
+}
+
+} // namespace crocoddyl
diff --git a/src/core/optctrl/shooting.cpp b/src/core/optctrl/shooting.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b3f185433e3f8ba32bbdba0ec290e15127341bf9
--- /dev/null
+++ b/src/core/optctrl/shooting.cpp
@@ -0,0 +1,117 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/optctrl/shooting.hpp"
+#include <iostream>
+#ifdef WITH_MULTITHREADING
+#include <omp.h>
+#define NUM_THREADS WITH_NTHREADS
+#endif // WITH_MULTITHREADING
+
+namespace crocoddyl {
+
+ShootingProblem::ShootingProblem(const Eigen::VectorXd& x0, const std::vector<ActionModelAbstract*>& running_models,
+ ActionModelAbstract* const terminal_model)
+ : terminal_model_(terminal_model),
+ running_models_(running_models),
+ T_(static_cast<unsigned int>(running_models.size())),
+ x0_(x0),
+ cost_(0.) {
+ assert(x0_.size() == running_models_[0]->get_state().get_nx() && "x0 has wrong dimension");
+ allocateData();
+}
+
+ShootingProblem::~ShootingProblem() {}
+
+double ShootingProblem::calc(const std::vector<Eigen::VectorXd>& xs, const std::vector<Eigen::VectorXd>& us) {
+ assert(xs.size() == T_ + 1 && "Wrong dimension of the state trajectory, it should be T + 1.");
+ assert(us.size() == T_ && "Wrong dimension of the control trajectory, it should be T.");
+
+ cost_ = 0;
+ for (unsigned int i = 0; i < T_; ++i) {
+ ActionModelAbstract* model = running_models_[i];
+ boost::shared_ptr<ActionDataAbstract>& data = running_datas_[i];
+ const Eigen::VectorXd& x = xs[i];
+ const Eigen::VectorXd& u = us[i];
+
+ model->calc(data, x, u);
+ cost_ += data->cost;
+ }
+ terminal_model_->calc(terminal_data_, xs.back());
+ cost_ += terminal_data_->cost;
+ return cost_;
+}
+
+double ShootingProblem::calcDiff(const std::vector<Eigen::VectorXd>& xs, const std::vector<Eigen::VectorXd>& us) {
+ assert(xs.size() == T_ + 1 && "Wrong dimension of the state trajectory, it should be T + 1.");
+ assert(us.size() == T_ && "Wrong dimension of the control trajectory, it should be T.");
+
+ cost_ = 0;
+ unsigned int i;
+
+#ifdef WITH_MULTITHREADING
+ omp_set_num_threads(NUM_THREADS);
+#pragma omp parallel for
+#endif
+ for (i = 0; i < T_; ++i) {
+ running_models_[i]->calcDiff(running_datas_[i], xs[i], us[i]);
+ }
+
+ for (unsigned int i = 0; i < T_; ++i) {
+ cost_ += running_datas_[i]->cost;
+ }
+
+ terminal_model_->calcDiff(terminal_data_, xs.back());
+ cost_ += terminal_data_->cost;
+ return cost_;
+}
+
+void ShootingProblem::rollout(const std::vector<Eigen::VectorXd>& us, std::vector<Eigen::VectorXd>& xs) {
+ assert(us.size() == T_ && "Wrong dimension of the control trajectory, it should be T.");
+
+ xs.resize(T_ + 1);
+ xs[0] = x0_;
+ for (unsigned int i = 0; i < T_; ++i) {
+ ActionModelAbstract* model = running_models_[i];
+ boost::shared_ptr<ActionDataAbstract>& data = running_datas_[i];
+ const Eigen::VectorXd& x = xs[i];
+ const Eigen::VectorXd& u = us[i];
+
+ model->calc(data, x, u);
+ xs[i + 1] = data->get_xnext();
+ }
+ terminal_model_->calc(terminal_data_, xs.back());
+}
+
+std::vector<Eigen::VectorXd> ShootingProblem::rollout_us(const std::vector<Eigen::VectorXd>& us) {
+ std::vector<Eigen::VectorXd> xs;
+ rollout(us, xs);
+ return xs;
+}
+
+unsigned int ShootingProblem::get_T() const { return T_; }
+
+const Eigen::VectorXd& ShootingProblem::get_x0() const { return x0_; }
+
+void ShootingProblem::allocateData() {
+ for (unsigned int i = 0; i < T_; ++i) {
+ ActionModelAbstract* model = running_models_[i];
+ running_datas_.push_back(model->createData());
+ }
+ terminal_data_ = terminal_model_->createData();
+}
+
+std::vector<ActionModelAbstract*>& ShootingProblem::get_runningModels() { return running_models_; }
+
+ActionModelAbstract* ShootingProblem::get_terminalModel() { return terminal_model_; }
+
+std::vector<boost::shared_ptr<ActionDataAbstract> >& ShootingProblem::get_runningDatas() { return running_datas_; }
+
+boost::shared_ptr<ActionDataAbstract>& ShootingProblem::get_terminalData() { return terminal_data_; }
+
+} // namespace crocoddyl
diff --git a/src/core/solver-base.cpp b/src/core/solver-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f39d309f6a23da4ca02f86342b5797cb1f82c38d
--- /dev/null
+++ b/src/core/solver-base.cpp
@@ -0,0 +1,115 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/solver-base.hpp"
+
+namespace crocoddyl {
+
+SolverAbstract::SolverAbstract(ShootingProblem& problem)
+ : problem_(problem),
+ is_feasible_(false),
+ cost_(0.),
+ stop_(0.),
+ xreg_(NAN),
+ ureg_(NAN),
+ steplength_(1.),
+ dV_(0.),
+ dVexp_(0.),
+ th_acceptstep_(0.1),
+ th_stop_(1e-9),
+ iter_(0) {
+ // Allocate common data
+ const unsigned int& T = problem_.get_T();
+ xs_.resize(T + 1);
+ us_.resize(T);
+ models_.resize(T + 1);
+ datas_.resize(T + 1);
+ for (unsigned int t = 0; t < T; ++t) {
+ ActionModelAbstract* model = problem_.running_models_[t];
+ boost::shared_ptr<ActionDataAbstract>& data = problem_.running_datas_[t];
+ const int& nu = model->get_nu();
+
+ xs_[t] = model->get_state().zero();
+ us_[t] = Eigen::VectorXd::Zero(nu);
+ models_[t] = model;
+ datas_[t] = data;
+ }
+ xs_.back() = problem_.terminal_model_->get_state().zero();
+ models_.back() = problem_.terminal_model_;
+ datas_.back() = problem_.terminal_data_;
+}
+
+SolverAbstract::~SolverAbstract() {}
+
+void SolverAbstract::setCandidate(const std::vector<Eigen::VectorXd>& xs_warm,
+ const std::vector<Eigen::VectorXd>& us_warm, const bool& is_feasible) {
+ const unsigned int& T = problem_.get_T();
+
+ if (xs_warm.size() == 0) {
+ for (unsigned int t = 0; t < T; ++t) {
+ xs_[t] = problem_.running_models_[t]->get_state().zero();
+ }
+ xs_.back() = problem_.terminal_model_->get_state().zero();
+ } else {
+ assert(xs_warm.size() == T + 1);
+ std::copy(xs_warm.begin(), xs_warm.end(), xs_.begin());
+ }
+
+ if (us_warm.size() == 0) {
+ for (unsigned int t = 0; t < T; ++t) {
+ const int& nu = problem_.running_models_[t]->get_nu();
+ us_[t] = Eigen::VectorXd::Zero(nu);
+ }
+ } else {
+ assert(us_warm.size() == T);
+ std::copy(us_warm.begin(), us_warm.end(), us_.begin());
+ }
+ is_feasible_ = is_feasible;
+}
+
+void SolverAbstract::setCallbacks(const std::vector<CallbackAbstract*>& callbacks) { callbacks_ = callbacks; }
+
+const ShootingProblem& SolverAbstract::get_problem() const { return problem_; }
+
+const std::vector<ActionModelAbstract*>& SolverAbstract::get_models() const { return models_; }
+
+const std::vector<boost::shared_ptr<ActionDataAbstract> >& SolverAbstract::get_datas() const { return datas_; }
+
+const std::vector<Eigen::VectorXd>& SolverAbstract::get_xs() const { return xs_; }
+
+const std::vector<Eigen::VectorXd>& SolverAbstract::get_us() const { return us_; }
+
+const bool& SolverAbstract::get_isFeasible() const { return is_feasible_; }
+
+const unsigned int& SolverAbstract::get_iter() const { return iter_; }
+
+const double& SolverAbstract::get_cost() const { return cost_; }
+
+const double& SolverAbstract::get_stop() const { return stop_; }
+
+const Eigen::Vector2d& SolverAbstract::get_d() const { return d_; }
+
+const double& SolverAbstract::get_xreg() const { return xreg_; }
+
+const double& SolverAbstract::get_ureg() const { return ureg_; }
+
+const double& SolverAbstract::get_stepLength() const { return steplength_; }
+
+const double& SolverAbstract::get_dV() const { return dV_; }
+
+const double& SolverAbstract::get_dVexp() const { return dVexp_; }
+
+bool raiseIfNaN(const double& value) {
+ if (std::isnan(value) || std::isinf(value) || value >= 1e30) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+} // namespace crocoddyl
diff --git a/src/core/solvers/ddp.cpp b/src/core/solvers/ddp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0fd09beefcb92b395dfbc973f91bb6da3a60a65d
--- /dev/null
+++ b/src/core/solvers/ddp.cpp
@@ -0,0 +1,355 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/solvers/ddp.hpp"
+
+namespace crocoddyl {
+
+SolverDDP::SolverDDP(ShootingProblem& problem)
+ : SolverAbstract(problem),
+ regfactor_(10.),
+ regmin_(1e-9),
+ regmax_(1e9),
+ cost_try_(0.),
+ th_grad_(1e-12),
+ th_step_(0.5),
+ was_feasible_(false) {
+ allocateData();
+
+ const unsigned int& n_alphas = 10;
+ alphas_.resize(n_alphas);
+ for (unsigned int n = 0; n < n_alphas; ++n) {
+ alphas_[n] = 1. / pow(2., static_cast<double>(n));
+ }
+}
+
+SolverDDP::~SolverDDP() {}
+
+bool SolverDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::vector<Eigen::VectorXd>& init_us,
+ const unsigned int& maxiter, const bool& is_feasible, const double& reginit) {
+ setCandidate(init_xs, init_us, is_feasible);
+
+ if (std::isnan(reginit)) {
+ xreg_ = regmin_;
+ ureg_ = regmin_;
+ } else {
+ xreg_ = reginit;
+ ureg_ = reginit;
+ }
+ was_feasible_ = false;
+
+ bool recalc = true;
+ for (iter_ = 0; iter_ < maxiter; ++iter_) {
+ while (true) {
+ try {
+ computeDirection(recalc);
+ } catch (const char* msg) {
+ recalc = false;
+ increaseRegularization();
+ if (xreg_ == regmax_) {
+ return false;
+ } else {
+ continue;
+ }
+ }
+ break;
+ }
+ expectedImprovement();
+
+ // We need to recalculate the derivatives when the step length passes
+ recalc = false;
+ for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
+ steplength_ = *it;
+
+ try {
+ dV_ = tryStep(steplength_);
+ } catch (const char* msg) {
+ continue;
+ }
+ dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
+
+ if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
+ was_feasible_ = is_feasible_;
+ setCandidate(xs_try_, us_try_, true);
+ cost_ = cost_try_;
+ recalc = true;
+ break;
+ }
+ }
+
+ if (steplength_ > th_step_) {
+ decreaseRegularization();
+ }
+ if (steplength_ == alphas_.back()) {
+ increaseRegularization();
+ if (xreg_ == regmax_) {
+ return false;
+ }
+ }
+ stoppingCriteria();
+
+ unsigned int const& n_callbacks = static_cast<unsigned int>(callbacks_.size());
+ for (unsigned int c = 0; c < n_callbacks; ++c) {
+ CallbackAbstract& callback = *callbacks_[c];
+ callback(*this);
+ }
+
+ if (was_feasible_ && stop_ < th_stop_) {
+ return true;
+ }
+ }
+ return false;
+}
+
+void SolverDDP::computeDirection(const bool& recalc) {
+ if (recalc) {
+ calc();
+ }
+ backwardPass();
+}
+
+double SolverDDP::tryStep(const double& steplength) {
+ forwardPass(steplength);
+ return cost_ - cost_try_;
+}
+
+double SolverDDP::stoppingCriteria() {
+ stop_ = 0.;
+ unsigned int const& T = this->problem_.get_T();
+ for (unsigned int t = 0; t < T; ++t) {
+ stop_ += Qu_[t].squaredNorm();
+ }
+ return stop_;
+}
+
+const Eigen::Vector2d& SolverDDP::expectedImprovement() {
+ d_.fill(0);
+ unsigned int const& T = this->problem_.get_T();
+ for (unsigned int t = 0; t < T; ++t) {
+ d_[0] += Qu_[t].dot(k_[t]);
+ d_[1] -= k_[t].dot(Quuk_[t]);
+ }
+ return d_;
+}
+
+double SolverDDP::calc() {
+ cost_ = problem_.calcDiff(xs_, us_);
+ if (!is_feasible_) {
+ const Eigen::VectorXd& x0 = problem_.get_x0();
+ problem_.running_models_[0]->get_state().diff(xs_[0], x0, gaps_[0]);
+
+ unsigned int const& T = problem_.get_T();
+ for (unsigned int t = 0; t < T; ++t) {
+ ActionModelAbstract* model = problem_.running_models_[t];
+ boost::shared_ptr<ActionDataAbstract>& d = problem_.running_datas_[t];
+ model->get_state().diff(xs_[t + 1], d->get_xnext(), gaps_[t + 1]);
+ }
+ }
+ return cost_;
+}
+
+void SolverDDP::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_;
+ }
+
+ 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;
+ }
+
+ 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_;
+ }
+
+ if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
+ throw "backward_error";
+ }
+ if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
+ throw "backward_error";
+ }
+ }
+}
+
+void SolverDDP::forwardPass(const double& steplength) {
+ assert(steplength <= 1. && "Step length has to be <= 1.");
+ assert(steplength >= 0. && "Step length has to be >= 0.");
+ cost_try_ = 0.;
+ unsigned int const& T = problem_.get_T();
+ for (unsigned int t = 0; t < T; ++t) {
+ ActionModelAbstract* m = problem_.running_models_[t];
+ boost::shared_ptr<ActionDataAbstract>& d = problem_.running_datas_[t];
+
+ 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]);
+ xs_try_[t + 1] = d->get_xnext();
+ cost_try_ += d->cost;
+
+ if (raiseIfNaN(cost_try_)) {
+ throw "forward_error";
+ }
+ if (raiseIfNaN(xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
+ throw "forward_error";
+ }
+ }
+
+ ActionModelAbstract* m = problem_.terminal_model_;
+ boost::shared_ptr<ActionDataAbstract>& d = problem_.terminal_data_;
+ m->calc(d, xs_try_.back());
+ cost_try_ += d->cost;
+
+ if (raiseIfNaN(cost_try_)) {
+ throw "forward_error";
+ }
+}
+
+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]);
+}
+
+void SolverDDP::increaseRegularization() {
+ xreg_ *= regfactor_;
+ if (xreg_ > regmax_) {
+ xreg_ = regmax_;
+ }
+ ureg_ = xreg_;
+}
+
+void SolverDDP::decreaseRegularization() {
+ xreg_ /= regfactor_;
+ if (xreg_ < regmin_) {
+ xreg_ = regmin_;
+ }
+ ureg_ = xreg_;
+}
+
+void SolverDDP::allocateData() {
+ const unsigned int& T = problem_.get_T();
+ Vxx_.resize(T + 1);
+ Vx_.resize(T + 1);
+ Qxx_.resize(T);
+ Qxu_.resize(T);
+ Quu_.resize(T);
+ Qx_.resize(T);
+ Qu_.resize(T);
+ K_.resize(T);
+ k_.resize(T);
+ gaps_.resize(T + 1);
+
+ xs_try_.resize(T + 1);
+ us_try_.resize(T);
+ dx_.resize(T);
+
+ FuTVxx_p_.resize(T);
+ Quu_llt_.resize(T);
+ Quuk_.resize(T);
+
+ for (unsigned int t = 0; t < T; ++t) {
+ ActionModelAbstract* model = problem_.running_models_[t];
+ const unsigned int& nx = model->get_state().get_nx();
+ const unsigned int& ndx = model->get_state().get_ndx();
+ const unsigned int& nu = model->get_nu();
+
+ Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
+ Vx_[t] = Eigen::VectorXd::Zero(ndx);
+ Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
+ Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
+ Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
+ Qx_[t] = Eigen::VectorXd::Zero(ndx);
+ Qu_[t] = Eigen::VectorXd::Zero(nu);
+ K_[t] = Eigen::MatrixXd::Zero(nu, ndx);
+ k_[t] = Eigen::VectorXd::Zero(nu);
+ gaps_[t] = Eigen::VectorXd::Zero(ndx);
+
+ if (t == 0) {
+ xs_try_[t] = problem_.get_x0();
+ } else {
+ xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
+ }
+ us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
+ dx_[t] = Eigen::VectorXd::Zero(ndx);
+
+ FuTVxx_p_[t] = Eigen::MatrixXd::Zero(nu, ndx);
+ Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
+ Quuk_[t] = Eigen::VectorXd(nu);
+ }
+ const unsigned int& ndx = problem_.terminal_model_->get_state().get_ndx();
+ Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
+ Vx_.back() = Eigen::VectorXd::Zero(ndx);
+ xs_try_.back() = problem_.terminal_model_->get_state().zero();
+ gaps_.back() = Eigen::VectorXd::Zero(ndx);
+
+ x_reg_ = Eigen::VectorXd::Constant(ndx, xreg_);
+ FxTVxx_p_ = Eigen::MatrixXd::Zero(ndx, ndx);
+ fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
+}
+
+const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx() const { return Vxx_; }
+
+const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx() const { return Vx_; }
+
+const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxx() const { return Qxx_; }
+
+const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxu() const { return Qxu_; }
+
+const std::vector<Eigen::MatrixXd>& SolverDDP::get_Quu() const { return Quu_; }
+
+const std::vector<Eigen::VectorXd>& SolverDDP::get_Qx() const { return Qx_; }
+
+const std::vector<Eigen::VectorXd>& SolverDDP::get_Qu() const { return Qu_; }
+
+const std::vector<Eigen::MatrixXd>& SolverDDP::get_K() const { return K_; }
+
+const std::vector<Eigen::VectorXd>& SolverDDP::get_k() const { return k_; }
+
+const std::vector<Eigen::VectorXd>& SolverDDP::get_gaps() const { return gaps_; }
+
+} // namespace crocoddyl
diff --git a/src/core/solvers/fddp.cpp b/src/core/solvers/fddp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..55e65b2ef28f4df4d8f115bdadc328ff75320aa6
--- /dev/null
+++ b/src/core/solvers/fddp.cpp
@@ -0,0 +1,271 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/solvers/fddp.hpp"
+
+namespace crocoddyl {
+
+SolverFDDP::SolverFDDP(ShootingProblem& problem) : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptNegStep_(2) {}
+
+SolverFDDP::~SolverFDDP() {}
+
+bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::vector<Eigen::VectorXd>& init_us,
+ const unsigned int& maxiter, const bool& is_feasible, const double& reginit) {
+ setCandidate(init_xs, init_us, is_feasible);
+
+ if (std::isnan(reginit)) {
+ xreg_ = regmin_;
+ ureg_ = regmin_;
+ } else {
+ xreg_ = reginit;
+ ureg_ = reginit;
+ }
+ was_feasible_ = false;
+
+ bool recalc = true;
+ for (iter_ = 0; iter_ < maxiter; ++iter_) {
+ while (true) {
+ try {
+ computeDirection(recalc);
+ } catch (const char* msg) {
+ recalc = false;
+ increaseRegularization();
+ if (xreg_ == regmax_) {
+ return false;
+ } else {
+ continue;
+ }
+ }
+ break;
+ }
+ updateExpectedImprovement();
+
+ // We need to recalculate the derivatives when the step length passes
+ recalc = false;
+ for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
+ steplength_ = *it;
+
+ try {
+ dV_ = tryStep(steplength_);
+ } catch (const char* msg) {
+ continue;
+ }
+ expectedImprovement();
+ dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
+
+ 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
+ was_feasible_ = is_feasible_;
+ setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
+ cost_ = cost_try_;
+ recalc = true;
+ break;
+ }
+ }
+ }
+ if (steplength_ > th_step_) {
+ decreaseRegularization();
+ }
+ if (steplength_ == alphas_.back()) {
+ increaseRegularization();
+ if (xreg_ == regmax_) {
+ return false;
+ }
+ }
+ stoppingCriteria();
+
+ unsigned int const& n_callbacks = static_cast<unsigned int>(callbacks_.size());
+ for (unsigned int c = 0; c < n_callbacks; ++c) {
+ CallbackAbstract& callback = *callbacks_[c];
+ callback(*this);
+ }
+
+ if (was_feasible_ && stop_ < th_stop_) {
+ return true;
+ }
+ }
+ 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_;
+ }
+ }
+}
+
+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_;
+ 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_;
+ }
+ }
+ d_[0] = dg_ + dv_;
+ d_[1] = dq_ - 2 * dv_;
+ return d_;
+}
+
+double SolverFDDP::calc() {
+ cost_ = problem_.calcDiff(xs_, us_);
+ if (!is_feasible_) {
+ const Eigen::VectorXd& x0 = problem_.get_x0();
+ problem_.running_models_[0]->get_state().diff(xs_[0], x0, gaps_[0]);
+
+ unsigned int const& T = problem_.get_T();
+ for (unsigned int t = 0; t < T; ++t) {
+ ActionModelAbstract* model = problem_.running_models_[t];
+ boost::shared_ptr<ActionDataAbstract>& d = problem_.running_datas_[t];
+ model->get_state().diff(xs_[t + 1], d->get_xnext(), gaps_[t + 1]);
+ }
+ } else if (!was_feasible_) {
+ for (std::vector<Eigen::VectorXd>::iterator it = gaps_.begin(); it != gaps_.end(); ++it) {
+ it->setZero();
+ }
+ }
+ 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.");
+ cost_try_ = 0.;
+ xnext_ = problem_.get_x0();
+ unsigned int const& T = problem_.get_T();
+ for (unsigned int t = 0; t < T; ++t) {
+ ActionModelAbstract* m = problem_.running_models_[t];
+ boost::shared_ptr<ActionDataAbstract>& d = problem_.running_datas_[t];
+ if ((is_feasible_) || (steplength == 1)) {
+ xs_try_[t] = xnext_;
+ } else {
+ m->get_state().integrate(xnext_, gaps_[t] * (steplength - 1), xs_try_[t]);
+ }
+ 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]);
+ cost_try_ += d->cost;
+
+ if (raiseIfNaN(cost_try_)) {
+ throw "forward_error";
+ }
+ if (raiseIfNaN(d->get_xnext().lpNorm<Eigen::Infinity>())) {
+ throw "forward_error";
+ }
+ }
+
+ ActionModelAbstract* m = problem_.terminal_model_;
+ boost::shared_ptr<ActionDataAbstract>& d = problem_.terminal_data_;
+
+ if ((is_feasible_) || (steplength == 1)) {
+ xs_try_.back() = problem_.running_datas_.back()->get_xnext();
+ } else {
+ m->get_state().integrate(problem_.running_datas_.back()->get_xnext(), gaps_.back() * (steplength - 1),
+ xs_try_.back());
+ }
+ m->calc(d, xs_try_.back());
+ cost_try_ += d->cost;
+
+ if (raiseIfNaN(cost_try_)) {
+ throw "forward_error";
+ }
+}
+
+} // namespace crocoddyl
diff --git a/src/core/state-base.cpp b/src/core/state-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f07fbd33249ef705e686a37913c47ba5dd08b967
--- /dev/null
+++ b/src/core/state-base.cpp
@@ -0,0 +1,28 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/state-base.hpp"
+
+namespace crocoddyl {
+
+StateAbstract::StateAbstract(unsigned int const& nx, unsigned int const& ndx) : nx_(nx), ndx_(ndx) {
+ nv_ = ndx / 2;
+ nq_ = nx_ - nv_;
+}
+
+StateAbstract::~StateAbstract() {}
+
+const unsigned int& StateAbstract::get_nx() const { return nx_; }
+
+const unsigned int& StateAbstract::get_ndx() const { return ndx_; }
+
+const unsigned int& StateAbstract::get_nq() const { return nq_; }
+
+const unsigned int& StateAbstract::get_nv() const { return nv_; }
+
+} // namespace crocoddyl
diff --git a/src/core/states/euclidean.cpp b/src/core/states/euclidean.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a3c8eb84b3361cc0efa2443fbd564b8dc1f811a0
--- /dev/null
+++ b/src/core/states/euclidean.cpp
@@ -0,0 +1,71 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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"
+
+namespace crocoddyl {
+
+StateVector::StateVector(unsigned int const& nx) : StateAbstract(nx, nx) {}
+
+StateVector::~StateVector() {}
+
+Eigen::VectorXd StateVector::zero() { return Eigen::VectorXd::Zero(nx_); }
+
+Eigen::VectorXd StateVector::rand() { return Eigen::VectorXd::Random(nx_); }
+
+void StateVector::diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::VectorXd> dxout) {
+ assert(x0.size() == nx_ && "x0 has wrong dimension");
+ assert(x1.size() == nx_ && "x1 has wrong dimension");
+ assert(dxout.size() == ndx_ && "output must be pre-allocated");
+ dxout = x1 - x0;
+}
+
+void StateVector::integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::VectorXd> xout) {
+ assert(x.size() == nx_ && "x has wrong dimension");
+ assert(dx.size() == ndx_ && "dx has wrong dimension");
+ assert(xout.size() == nx_ && "Output must be pre-allocated");
+ xout = x + dx;
+}
+
+void StateVector::Jdiff(const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>&,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond) {
+ assert((firstsecond == first || firstsecond == second || firstsecond == both) &&
+ ("firstsecond must be one of the Jcomponent {both, first, second}"));
+ if (firstsecond == first || firstsecond == both) {
+ assert(Jfirst.rows() == ndx_ && Jfirst.cols() == ndx_ && "Jfirst must be of the good size");
+ Jfirst.setZero();
+ Jfirst.diagonal() = Eigen::VectorXd::Constant(ndx_, -1.);
+ }
+ if (firstsecond == second || firstsecond == both) {
+ assert(Jsecond.rows() == ndx_ && Jsecond.cols() == ndx_ && "Jfirst must be of the good size");
+ Jsecond.setZero();
+ Jsecond.diagonal() = Eigen::VectorXd::Constant(ndx_, 1.);
+ }
+}
+
+void StateVector::Jintegrate(const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>&,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond) {
+ assert((firstsecond == first || firstsecond == second || firstsecond == both) &&
+ ("firstsecond must be one of the Jcomponent {both, first, second}"));
+ if (firstsecond == first || firstsecond == both) {
+ assert(Jfirst.rows() == ndx_ && Jfirst.cols() == ndx_ && "Jfirst must be of the good size");
+ Jfirst.setZero();
+ Jfirst.diagonal() = Eigen::VectorXd::Constant(ndx_, 1.);
+ }
+ if (firstsecond == second || firstsecond == both) {
+ assert(Jsecond.rows() == ndx_ && Jsecond.cols() == ndx_ && "Jfirst must be of the good size");
+ Jsecond.setZero();
+ Jsecond.diagonal() = Eigen::VectorXd::Constant(ndx_, 1.);
+ }
+}
+
+} // namespace crocoddyl
diff --git a/src/core/utils/callbacks.cpp b/src/core/utils/callbacks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd76fe9e7d8b5e73285e63ab262b7e40f4c27079
--- /dev/null
+++ b/src/core/utils/callbacks.cpp
@@ -0,0 +1,69 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/utils/callbacks.hpp"
+
+namespace crocoddyl {
+
+CallbackVerbose::CallbackVerbose(VerboseLevel level) : CallbackAbstract(), level(level) {}
+
+CallbackVerbose::~CallbackVerbose() {}
+
+void CallbackVerbose::operator()(SolverAbstract& solver) {
+ if (solver.get_iter() % 10 == 0) {
+ switch (level) {
+ case _1: {
+ std::cout << "iter \t cost \t stop \t grad \t xreg";
+ std::cout << " \t ureg \t step \t feas\n";
+ break;
+ }
+ case _2: {
+ std::cout << "iter \t cost \t stop \t grad \t xreg";
+ std::cout << " \t ureg \t step \t feas \tdV-exp \t dV\n";
+ break;
+ }
+ default: {
+ std::cout << "iter \t cost \t stop \t grad \t xreg";
+ std::cout << " \t ureg \t step \t feas\n";
+ }
+ }
+ }
+
+ switch (level) {
+ case _1: {
+ std::cout << std::setw(4) << solver.get_iter() << " ";
+ std::cout << std::scientific << std::setprecision(5) << solver.get_cost() << " ";
+ std::cout << solver.get_stop() << " " << -solver.get_d()[1] << " ";
+ std::cout << solver.get_xreg() << " " << solver.get_ureg() << " ";
+ std::cout << std::fixed << std::setprecision(4) << solver.get_stepLength() << " ";
+ std::cout << solver.get_isFeasible() << '\n';
+ break;
+ }
+ case _2: {
+ std::cout << std::setw(4) << solver.get_iter() << " ";
+ std::cout << std::scientific << std::setprecision(5) << solver.get_cost() << " ";
+ std::cout << solver.get_stop() << " " << -solver.get_d()[1] << " ";
+ std::cout << solver.get_xreg() << " " << solver.get_ureg() << " ";
+ std::cout << std::fixed << std::setprecision(4) << solver.get_stepLength() << " ";
+ std::cout << solver.get_isFeasible() << " ";
+ std::cout << std::scientific << std::setprecision(5) << solver.get_dV() << " ";
+ std::cout << solver.get_dVexp() << '\n';
+ break;
+ }
+ default: {
+ std::cout << std::setw(4) << solver.get_iter() << " ";
+ std::cout << std::scientific << std::setprecision(5) << solver.get_cost() << " ";
+ std::cout << solver.get_stop() << " " << -solver.get_d()[1] << " ";
+ std::cout << solver.get_xreg() << " " << solver.get_ureg() << " ";
+ std::cout << std::fixed << std::setprecision(4) << solver.get_stepLength() << " ";
+ std::cout << solver.get_isFeasible() << '\n';
+ }
+ }
+}
+
+} // namespace crocoddyl
diff --git a/src/multibody/actions/contact-fwddyn.cpp b/src/multibody/actions/contact-fwddyn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e75681d31ea118e6e6a87471fbd86ed60c2e4403
--- /dev/null
+++ b/src/multibody/actions/contact-fwddyn.cpp
@@ -0,0 +1,151 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/contact-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 {
+
+DifferentialActionModelContactFwdDynamics::DifferentialActionModelContactFwdDynamics(
+ StateMultibody& state, ActuationModelFloatingBase& actuation, ContactModelMultiple& contacts, CostModelSum& costs,
+ const double& JMinvJt_damping, const bool& enable_force)
+ : DifferentialActionModelAbstract(state, actuation.get_nu(), costs.get_nr()),
+ actuation_(actuation),
+ contacts_(contacts),
+ costs_(costs),
+ pinocchio_(state.get_pinocchio()),
+ with_armature_(true),
+ armature_(Eigen::VectorXd::Zero(state.get_nv())),
+ JMinvJt_damping_(fabs(JMinvJt_damping)),
+ enable_force_(enable_force) {
+ assert(contacts_.get_nu() == nu_ && "Contacts doesn't have the same control dimension");
+ assert(costs_.get_nu() == nu_ && "Costs doesn't have the same control dimension");
+}
+
+DifferentialActionModelContactFwdDynamics::~DifferentialActionModelContactFwdDynamics() {}
+
+void DifferentialActionModelContactFwdDynamics::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");
+
+ DifferentialActionDataContactFwdDynamics* d = static_cast<DifferentialActionDataContactFwdDynamics*>(data.get());
+ d->qcur = x.head(state_.get_nq());
+ d->vcur = 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::updateFramePlacements(pinocchio_, d->pinocchio);
+
+ if (!with_armature_) {
+ d->pinocchio.M.diagonal() += armature_;
+ }
+ actuation_.calc(d->actuation, x, u);
+ contacts_.calc(d->contacts, x);
+
+#ifndef NDEBUG
+ Eigen::FullPivLU<Eigen::MatrixXd> Jc_lu(d->contacts->Jc);
+
+ if (Jc_lu.rank() < d->contacts->Jc.rows()) {
+ assert(JMinvJt_damping_ > 0. && "It is needed a damping factor since the contact Jacobian is not full-rank");
+ }
+#endif
+
+ pinocchio::forwardDynamics(pinocchio_, d->pinocchio, d->qcur, d->vcur, d->actuation->a, d->contacts->Jc,
+ d->contacts->a0, JMinvJt_damping_, false);
+ d->xout = d->pinocchio.ddq;
+ contacts_.updateLagrangian(d->contacts, d->pinocchio.lambda_c);
+
+ // Computing the cost value and residuals
+ costs_.calc(d->costs, x, u);
+ d->cost = d->costs->cost;
+}
+
+void DifferentialActionModelContactFwdDynamics::calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& 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");
+
+ DifferentialActionDataContactFwdDynamics* d = static_cast<DifferentialActionDataContactFwdDynamics*>(data.get());
+ unsigned int const& nv = state_.get_nv();
+ unsigned int const& nc = contacts_.get_nc();
+ 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::getKKTContactDynamicMatrixInverse(pinocchio_, d->pinocchio, d->contacts->Jc, d->Kinv);
+
+ actuation_.calcDiff(d->actuation, x, u, false);
+ contacts_.calcDiff(d->contacts, 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, nc);
+ Eigen::Block<Eigen::MatrixXd> f_partial_dtau = d->Kinv.bottomLeftCorner(nc, nv);
+ Eigen::Block<Eigen::MatrixXd> f_partial_da = d->Kinv.bottomRightCorner(nc, nc);
+
+ 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;
+
+ // 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);
+ }
+ costs_.calcDiff(d->costs, x, u, false);
+}
+
+boost::shared_ptr<DifferentialActionDataAbstract> DifferentialActionModelContactFwdDynamics::createData() {
+ return boost::make_shared<DifferentialActionDataContactFwdDynamics>(this);
+}
+
+pinocchio::Model& DifferentialActionModelContactFwdDynamics::get_pinocchio() const { return pinocchio_; }
+
+ActuationModelFloatingBase& DifferentialActionModelContactFwdDynamics::get_actuation() const { return actuation_; }
+
+ContactModelMultiple& DifferentialActionModelContactFwdDynamics::get_contacts() const { return contacts_; }
+
+CostModelSum& DifferentialActionModelContactFwdDynamics::get_costs() const { return costs_; }
+
+const Eigen::VectorXd& DifferentialActionModelContactFwdDynamics::get_armature() const { return armature_; }
+
+const double& DifferentialActionModelContactFwdDynamics::get_damping_factor() const { return JMinvJt_damping_; }
+
+void DifferentialActionModelContactFwdDynamics::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 DifferentialActionModelContactFwdDynamics::set_damping_factor(const double& damping) {
+ JMinvJt_damping_ = damping;
+}
+
+} // namespace crocoddyl
diff --git a/src/multibody/actions/free-fwddyn.cpp b/src/multibody/actions/free-fwddyn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac3d7b9ca3b1e3072485a395a8ec298c2098bf11
--- /dev/null
+++ b/src/multibody/actions/free-fwddyn.cpp
@@ -0,0 +1,116 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/free-fwddyn.hpp"
+#include <pinocchio/algorithm/aba.hpp>
+#include <pinocchio/algorithm/aba-derivatives.hpp>
+#include <pinocchio/algorithm/rnea-derivatives.hpp>
+#include <pinocchio/algorithm/compute-all-terms.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/algorithm/jacobian.hpp>
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/cholesky.hpp>
+
+namespace crocoddyl {
+
+DifferentialActionModelFreeFwdDynamics::DifferentialActionModelFreeFwdDynamics(StateMultibody& state,
+ CostModelSum& costs)
+ : DifferentialActionModelAbstract(state, state.get_pinocchio().nv, costs.get_nr()),
+ costs_(costs),
+ pinocchio_(state.get_pinocchio()),
+ with_armature_(true),
+ armature_(Eigen::VectorXd::Zero(state.get_nv())) {}
+
+DifferentialActionModelFreeFwdDynamics::~DifferentialActionModelFreeFwdDynamics() {}
+
+void DifferentialActionModelFreeFwdDynamics::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");
+
+ DifferentialActionDataFreeFwdDynamics* d = static_cast<DifferentialActionDataFreeFwdDynamics*>(data.get());
+ d->qcur = x.head(state_.get_nq());
+ d->vcur = 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);
+ } else {
+ pinocchio::computeAllTerms(pinocchio_, d->pinocchio, d->qcur, d->vcur);
+ d->pinocchio.M.diagonal() += armature_;
+ pinocchio::cholesky::decompose(pinocchio_, d->pinocchio);
+ d->Minv.setZero();
+ pinocchio::cholesky::computeMinv(pinocchio_, d->pinocchio, d->Minv);
+ d->u_drift = u - d->pinocchio.nle;
+ d->xout.noalias() = d->Minv * d->u_drift;
+ }
+
+ // Computing the cost value and residuals
+ pinocchio::forwardKinematics(pinocchio_, d->pinocchio, d->qcur, d->vcur);
+ pinocchio::updateFramePlacements(pinocchio_, d->pinocchio);
+ costs_.calc(d->costs, x, u);
+ d->cost = d->costs->cost;
+}
+
+void DifferentialActionModelFreeFwdDynamics::calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& 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");
+
+ DifferentialActionDataFreeFwdDynamics* d = static_cast<DifferentialActionDataFreeFwdDynamics*>(data.get());
+ const unsigned int& nv = state_.get_nv();
+ 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);
+ }
+
+ // Computing the dynamics derivatives
+ if (with_armature_) {
+ pinocchio::computeABADerivatives(pinocchio_, d->pinocchio, d->qcur, d->vcur, 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);
+ 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;
+ d->Fx.rightCols(nv) *= -1.;
+ d->Fu = d->Minv;
+ }
+
+ // Computing the cost derivatives
+ costs_.calcDiff(d->costs, x, u, false);
+}
+
+boost::shared_ptr<DifferentialActionDataAbstract> DifferentialActionModelFreeFwdDynamics::createData() {
+ return boost::make_shared<DifferentialActionDataFreeFwdDynamics>(this);
+}
+
+pinocchio::Model& DifferentialActionModelFreeFwdDynamics::get_pinocchio() const { return pinocchio_; }
+
+CostModelSum& DifferentialActionModelFreeFwdDynamics::get_costs() const { return costs_; }
+
+const Eigen::VectorXd& DifferentialActionModelFreeFwdDynamics::get_armature() const { return armature_; }
+
+void DifferentialActionModelFreeFwdDynamics::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;
+ }
+}
+
+} // namespace crocoddyl
diff --git a/src/multibody/actuations/floating-base.cpp b/src/multibody/actuations/floating-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9389f107c1d831f3bcd31fb3c5b4db2b36b90cb5
--- /dev/null
+++ b/src/multibody/actuations/floating-base.cpp
@@ -0,0 +1,54 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/actuations/floating-base.hpp"
+
+namespace crocoddyl {
+
+ActuationModelFloatingBase::ActuationModelFloatingBase(StateMultibody& state)
+ : ActuationModelAbstract(state, state.get_nv() - 6) {
+ pinocchio::JointModelFreeFlyer ff_joint;
+ assert(state.get_pinocchio().joints[1].shortname() == ff_joint.shortname() &&
+ "The first joint has to be free-flyer");
+ if (state.get_pinocchio().joints[1].shortname() != ff_joint.shortname()) {
+ std::cout << "Warning: the first joint has to be a free-flyer" << std::endl;
+ }
+}
+
+ActuationModelFloatingBase::~ActuationModelFloatingBase() {}
+
+void ActuationModelFloatingBase::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.tail(nu_) = u;
+}
+
+void ActuationModelFloatingBase::calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
+ if (recalc) {
+ 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");
+}
+
+boost::shared_ptr<ActuationDataAbstract> ActuationModelFloatingBase::createData() {
+ boost::shared_ptr<ActuationDataAbstract> data = boost::make_shared<ActuationDataAbstract>(this);
+ data->Au.diagonal(-6).fill(1.);
+
+#ifndef NDEBUG
+ Au_ = data->Au;
+#endif
+
+ return data;
+}
+
+} // namespace crocoddyl
diff --git a/src/multibody/actuations/full.cpp b/src/multibody/actuations/full.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7e02c5e13ef31b26322052eb432246010f431a9
--- /dev/null
+++ b/src/multibody/actuations/full.cpp
@@ -0,0 +1,47 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/actuations/full.hpp"
+
+namespace crocoddyl {
+
+ActuationModelFull::ActuationModelFull(StateMultibody& state) : ActuationModelAbstract(state, state.get_nv()) {
+ pinocchio::JointModelFreeFlyer ff_joint;
+ assert(state.get_pinocchio().joints[1].shortname() != ff_joint.shortname() &&
+ "The first joint cannot be free-flyer");
+ if (state.get_pinocchio().joints[1].shortname() == ff_joint.shortname()) {
+ std::cout << "Warning: the first joint cannot be a free-flyer" << std::endl;
+ }
+}
+
+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;
+}
+
+void ActuationModelFull::calcDiff(const boost::shared_ptr<ActuationDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
+ if (recalc) {
+ 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");
+}
+
+boost::shared_ptr<ActuationDataAbstract> ActuationModelFull::createData() {
+ boost::shared_ptr<ActuationDataAbstract> data = boost::make_shared<ActuationDataAbstract>(this);
+ data->Au.diagonal().fill(1);
+ return data;
+}
+
+} // namespace crocoddyl
diff --git a/src/multibody/contact-base.cpp b/src/multibody/contact-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d8494bd51ecc6778ebb9199e137826d90053048c
--- /dev/null
+++ b/src/multibody/contact-base.cpp
@@ -0,0 +1,39 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/contact-base.hpp"
+
+namespace crocoddyl {
+
+ContactModelAbstract::ContactModelAbstract(StateMultibody& state, unsigned int const& nc, unsigned int const& nu)
+ : state_(state), nc_(nc), nu_(nu) {}
+
+ContactModelAbstract::ContactModelAbstract(StateMultibody& state, unsigned int const& nc)
+ : state_(state), nc_(nc), nu_(state.get_nv()) {}
+
+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;
+}
+
+boost::shared_ptr<ContactDataAbstract> ContactModelAbstract::createData(pinocchio::Data* const data) {
+ return boost::make_shared<ContactDataAbstract>(this, data);
+}
+
+StateMultibody& ContactModelAbstract::get_state() const { return state_; }
+
+unsigned int const& ContactModelAbstract::get_nc() const { return nc_; }
+
+unsigned int const& ContactModelAbstract::get_nu() const { return nu_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/contacts/contact-3d.cpp b/src/multibody/contacts/contact-3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..edaf95cc3d7916d7f70b3e4975ce37e76c77bf48
--- /dev/null
+++ b/src/multibody/contacts/contact-3d.cpp
@@ -0,0 +1,89 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/contacts/contact-3d.hpp"
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics-derivatives.hpp>
+
+namespace crocoddyl {
+
+ContactModel3D::ContactModel3D(StateMultibody& state, const FrameTranslation& xref, unsigned int const& nu,
+ const Eigen::Vector2d& gains)
+ : ContactModelAbstract(state, 3, nu), xref_(xref), gains_(gains) {}
+
+ContactModel3D::ContactModel3D(StateMultibody& state, const FrameTranslation& xref, const Eigen::Vector2d& gains)
+ : ContactModelAbstract(state, 3), xref_(xref), gains_(gains) {}
+
+ContactModel3D::~ContactModel3D() {}
+
+void ContactModel3D::calc(const boost::shared_ptr<ContactDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>&) {
+ ContactData3D* d = static_cast<ContactData3D*>(data.get());
+ d->v = pinocchio::getFrameVelocity(state_.get_pinocchio(), *d->pinocchio, xref_.frame);
+ d->vw = d->v.angular();
+ d->vv = d->v.linear();
+
+ pinocchio::getFrameJacobian(state_.get_pinocchio(), *d->pinocchio, xref_.frame, pinocchio::LOCAL, d->fJf);
+ d->Jc = d->fJf.topRows<3>();
+
+ d->a = pinocchio::getFrameAcceleration(state_.get_pinocchio(), *d->pinocchio, xref_.frame);
+ d->a0 = d->a.linear() + d->vw.cross(d->vv);
+
+ if (gains_[0] != 0.) {
+ d->a0 += gains_[0] * (d->pinocchio->oMf[xref_.frame].translation() - xref_.oxf);
+ }
+ if (gains_[1] != 0.) {
+ d->a0 += gains_[1] * d->vv;
+ }
+}
+
+void ContactModel3D::calcDiff(const boost::shared_ptr<ContactDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const bool& recalc) {
+ if (recalc) {
+ calc(data, x);
+ }
+
+ ContactData3D* d = static_cast<ContactData3D*>(data.get());
+ 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();
+ pinocchio::skew(d->vv, d->vv_skew);
+ pinocchio::skew(d->vw, d->vw_skew);
+ 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->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>();
+
+ if (gains_[0] != 0.) {
+ d->oRf = d->pinocchio->oMf[xref_.frame].rotation();
+ d->Ax.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;
+ }
+}
+
+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");
+ ContactData3D* d = static_cast<ContactData3D*>(data.get());
+ data->f = d->jMf.act(pinocchio::Force(lambda, Eigen::Vector3d::Zero()));
+}
+
+boost::shared_ptr<ContactDataAbstract> ContactModel3D::createData(pinocchio::Data* const data) {
+ return boost::make_shared<ContactData3D>(this, data);
+}
+
+const FrameTranslation& ContactModel3D::get_xref() const { return xref_; }
+
+const Eigen::Vector2d& ContactModel3D::get_gains() const { return gains_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/contacts/contact-6d.cpp b/src/multibody/contacts/contact-6d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..65e0f63b118a00f56f90774b7b78ab0e8bee411b
--- /dev/null
+++ b/src/multibody/contacts/contact-6d.cpp
@@ -0,0 +1,82 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// 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/contacts/contact-6d.hpp"
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics-derivatives.hpp>
+
+namespace crocoddyl {
+
+ContactModel6D::ContactModel6D(StateMultibody& state, const FramePlacement& Mref, unsigned int const& nu,
+ const Eigen::Vector2d& gains)
+ : ContactModelAbstract(state, 6, nu), Mref_(Mref), gains_(gains) {}
+
+ContactModel6D::ContactModel6D(StateMultibody& state, const FramePlacement& Mref, const Eigen::Vector2d& gains)
+ : ContactModelAbstract(state, 6), Mref_(Mref), gains_(gains) {}
+
+ContactModel6D::~ContactModel6D() {}
+
+void ContactModel6D::calc(const boost::shared_ptr<ContactDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>&) {
+ ContactData6D* d = static_cast<ContactData6D*>(data.get());
+
+ pinocchio::getFrameJacobian(state_.get_pinocchio(), *d->pinocchio, Mref_.frame, pinocchio::LOCAL, d->Jc);
+
+ d->a = pinocchio::getFrameAcceleration(state_.get_pinocchio(), *d->pinocchio, Mref_.frame);
+ d->a0 = d->a.toVector();
+
+ if (gains_[0] != 0.) {
+ d->rMf = Mref_.oMf.inverse() * d->pinocchio->oMf[Mref_.frame];
+ d->a0 += gains_[0] * pinocchio::log6(d->rMf).toVector();
+ }
+ if (gains_[1] != 0.) {
+ d->v = pinocchio::getFrameVelocity(state_.get_pinocchio(), *d->pinocchio, Mref_.frame);
+ d->a0 += gains_[1] * d->v.toVector();
+ }
+}
+
+void ContactModel6D::calcDiff(const boost::shared_ptr<ContactDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const bool& recalc) {
+ if (recalc) {
+ calc(data, x);
+ }
+
+ ContactData6D* d = static_cast<ContactData6D*>(data.get());
+ 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;
+
+ if (gains_[0] != 0.) {
+ pinocchio::Jlog6(d->rMf, d->rMf_Jlog6);
+ d->Ax.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;
+ }
+}
+
+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");
+ ContactData6D* d = static_cast<ContactData6D*>(data.get());
+ data->f = d->jMf.act(pinocchio::Force(lambda));
+}
+
+boost::shared_ptr<ContactDataAbstract> ContactModel6D::createData(pinocchio::Data* const data) {
+ return boost::make_shared<ContactData6D>(this, data);
+}
+
+const FramePlacement& ContactModel6D::get_Mref() const { return Mref_; }
+
+const Eigen::Vector2d& ContactModel6D::get_gains() const { return gains_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/contacts/multiple-contacts.cpp b/src/multibody/contacts/multiple-contacts.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..441760c13beafc8757a85744785188e2715edb99
--- /dev/null
+++ b/src/multibody/contacts/multiple-contacts.cpp
@@ -0,0 +1,148 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/contacts/multiple-contacts.hpp"
+
+namespace crocoddyl {
+
+ContactModelMultiple::ContactModelMultiple(StateMultibody& state, unsigned int const& nu)
+ : state_(state), nc_(0), nu_(nu) {}
+
+ContactModelMultiple::ContactModelMultiple(StateMultibody& state) : state_(state), nc_(0), nu_(state.get_nv()) {}
+
+ContactModelMultiple::~ContactModelMultiple() {}
+
+void ContactModelMultiple::addContact(const std::string& name, ContactModelAbstract* const contact) {
+ assert(contact->get_nu() == nu_ && "Contact item doesn't have the same control dimension");
+ std::pair<ContactModelContainer::iterator, bool> ret =
+ contacts_.insert(std::make_pair(name, ContactItem(name, contact)));
+ if (ret.second == false) {
+ std::cout << "Warning: this contact item already existed, we cannot add it" << std::endl;
+ } else {
+ nc_ += contact->get_nc();
+ }
+}
+
+void ContactModelMultiple::removeContact(const std::string& name) {
+ ContactModelContainer::iterator it = contacts_.find(name);
+ if (it != contacts_.end()) {
+ nc_ -= it->second.contact->get_nc();
+ contacts_.erase(it);
+ } else {
+ std::cout << "Warning: this contact item doesn't exist, we cannot remove it" << std::endl;
+ }
+}
+
+void ContactModelMultiple::calc(const boost::shared_ptr<ContactDataMultiple>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x) {
+ assert(data->contacts.size() == contacts_.size() && "it doesn't match the number of contact datas and models");
+ unsigned int nc = 0;
+
+ unsigned int const& nv = state_.get_nv();
+ ContactModelContainer::iterator it_m, end_m;
+ ContactDataContainer::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;
+ assert(it_m->first == it_d->first && "it doesn't match the contact name between data and model");
+
+ m_i.contact->calc(d_i, x);
+ unsigned int const& nc_i = m_i.contact->get_nc();
+ data->a0.segment(nc, nc_i) = d_i->a0;
+ data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
+ nc += nc_i;
+ }
+}
+
+void ContactModelMultiple::calcDiff(const boost::shared_ptr<ContactDataMultiple>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const bool& recalc) {
+ assert(data->contacts.size() == contacts_.size() && "it doesn't match the number of contact datas and models");
+ if (recalc) {
+ calc(data, x);
+ }
+ unsigned int nc = 0;
+
+ unsigned int const& ndx = state_.get_ndx();
+ ContactModelContainer::iterator it_m, end_m;
+ ContactDataContainer::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;
+ assert(it_m->first == it_d->first && "it doesn't match the contact name between data and model");
+
+ 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;
+ 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");
+ assert(data->contacts.size() == contacts_.size() && "it doesn't match the number of contact datas and models");
+ unsigned int nc = 0;
+
+ for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
+ *it = pinocchio::Force::Zero();
+ }
+
+ ContactModelContainer::iterator it_m, end_m;
+ ContactDataContainer::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;
+ 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));
+ 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) {
+ 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(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;
+ 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;
+ 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);
+ nc += nc_i;
+ }
+}
+
+boost::shared_ptr<ContactDataMultiple> ContactModelMultiple::createData(pinocchio::Data* const data) {
+ return boost::make_shared<ContactDataMultiple>(this, data);
+}
+
+StateMultibody& ContactModelMultiple::get_state() const { return state_; }
+
+const ContactModelMultiple::ContactModelContainer& ContactModelMultiple::get_contacts() const { return contacts_; }
+
+const unsigned int& ContactModelMultiple::get_nc() const { return nc_; }
+
+const unsigned int& ContactModelMultiple::get_nu() const { return nu_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/cost-base.cpp b/src/multibody/cost-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be4ad1090e03fa0dbfbc01416928b438f0e35b2e
--- /dev/null
+++ b/src/multibody/cost-base.cpp
@@ -0,0 +1,67 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/cost-base.hpp"
+#include "crocoddyl/core/activations/quadratic.hpp"
+
+namespace crocoddyl {
+
+CostModelAbstract::CostModelAbstract(StateMultibody& state, ActivationModelAbstract& activation,
+ unsigned int const& nu, const bool& with_residuals)
+ : state_(state),
+ activation_(activation),
+ nu_(nu),
+ with_residuals_(with_residuals),
+ unone_(Eigen::VectorXd::Zero(nu)) {}
+
+CostModelAbstract::CostModelAbstract(StateMultibody& state, ActivationModelAbstract& activation,
+ const bool& with_residuals)
+ : state_(state),
+ activation_(activation),
+ nu_(state.get_nv()),
+ with_residuals_(with_residuals),
+ unone_(Eigen::VectorXd::Zero(state.get_nv())) {}
+
+CostModelAbstract::CostModelAbstract(StateMultibody& state, unsigned int const& nr, const unsigned int& nu,
+ const bool& with_residuals)
+ : state_(state),
+ activation_(*new ActivationModelQuad(nr)),
+ nu_(nu),
+ with_residuals_(with_residuals),
+ unone_(Eigen::VectorXd::Zero(nu)) {}
+
+CostModelAbstract::CostModelAbstract(StateMultibody& state, unsigned int const& nr, const bool& with_residuals)
+ : state_(state),
+ activation_(*new ActivationModelQuad(nr)),
+ nu_(state.get_nv()),
+ with_residuals_(with_residuals),
+ unone_(Eigen::VectorXd::Zero(state.get_nv())) {}
+
+CostModelAbstract::~CostModelAbstract() {}
+
+void CostModelAbstract::calc(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x) {
+ calc(data, x, unone_);
+}
+
+void CostModelAbstract::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x) {
+ calcDiff(data, x, unone_);
+}
+
+boost::shared_ptr<CostDataAbstract> CostModelAbstract::createData(pinocchio::Data* const data) {
+ return boost::make_shared<CostDataAbstract>(this, data);
+}
+
+StateMultibody& CostModelAbstract::get_state() const { return state_; }
+
+ActivationModelAbstract& CostModelAbstract::get_activation() const { return activation_; }
+
+unsigned int const& CostModelAbstract::get_nu() const { return nu_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/costs/com-position.cpp b/src/multibody/costs/com-position.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..32c3d8f215dc4666f270a6647de51837c1f77936
--- /dev/null
+++ b/src/multibody/costs/com-position.cpp
@@ -0,0 +1,67 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/costs/com-position.hpp"
+
+namespace crocoddyl {
+
+CostModelCoMPosition::CostModelCoMPosition(StateMultibody& state, ActivationModelAbstract& activation,
+ const Eigen::Vector3d& cref, unsigned int const& nu)
+ : CostModelAbstract(state, activation, nu), cref_(cref) {
+ assert(activation_.get_nr() == 3 && "activation::nr is not equals to 3");
+}
+
+CostModelCoMPosition::CostModelCoMPosition(StateMultibody& state, ActivationModelAbstract& activation,
+ const Eigen::Vector3d& cref)
+ : CostModelAbstract(state, activation), cref_(cref) {
+ assert(activation_.get_nr() == 3 && "activation::nr is not equals to 3");
+}
+
+CostModelCoMPosition::CostModelCoMPosition(StateMultibody& state, const Eigen::Vector3d& cref, unsigned int const& nu)
+ : CostModelAbstract(state, 3, nu), cref_(cref) {}
+
+CostModelCoMPosition::CostModelCoMPosition(StateMultibody& state, const Eigen::Vector3d& cref)
+ : CostModelAbstract(state, 3), cref_(cref) {}
+
+CostModelCoMPosition::~CostModelCoMPosition() {}
+
+void CostModelCoMPosition::calc(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>&) {
+ // Compute the cost residual give the reference CoMPosition position
+ data->r = data->pinocchio->com[0] - cref_;
+
+ // Compute the cost
+ activation_.calc(data->activation, data->r);
+ data->cost = data->activation->a_value;
+}
+
+void CostModelCoMPosition::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
+ if (recalc) {
+ calc(data, x, u);
+ }
+
+ CostDataCoMPosition* d = static_cast<CostDataCoMPosition*>(data.get());
+
+ // Compute the derivatives of the frame placement
+ unsigned int const& nv = state_.get_nv();
+ activation_.calcDiff(data->activation, data->r, recalc);
+ data->Rx.leftCols(nv) = data->pinocchio->Jcom;
+ data->Lx.head(nv).noalias() = data->pinocchio->Jcom.transpose() * data->activation->Ar;
+ d->Arr_Jcom.noalias() = data->activation->Arr * data->pinocchio->Jcom;
+ data->Lxx.topLeftCorner(nv, nv).noalias() = data->pinocchio->Jcom.transpose() * d->Arr_Jcom;
+}
+
+boost::shared_ptr<CostDataAbstract> CostModelCoMPosition::createData(pinocchio::Data* const data) {
+ return boost::make_shared<CostDataCoMPosition>(this, data);
+}
+
+const Eigen::VectorXd& CostModelCoMPosition::get_cref() const { return cref_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/costs/control.cpp b/src/multibody/costs/control.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0349c86695b1c808a24905614de01f706b56245d
--- /dev/null
+++ b/src/multibody/costs/control.cpp
@@ -0,0 +1,62 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/costs/control.hpp"
+
+namespace crocoddyl {
+
+CostModelControl::CostModelControl(StateMultibody& state, ActivationModelAbstract& activation,
+ const Eigen::VectorXd& uref)
+ : CostModelAbstract(state, activation, (const unsigned)uref.size()), uref_(uref) {
+ assert(activation.get_nr() == (const unsigned)uref.size() && "activation::nr is not equals to nu");
+}
+
+CostModelControl::CostModelControl(StateMultibody& state, ActivationModelAbstract& activation)
+ : CostModelAbstract(state, activation), uref_(Eigen::VectorXd::Zero(activation.get_nr())) {}
+
+CostModelControl::CostModelControl(StateMultibody& state, ActivationModelAbstract& activation, const unsigned int& nu)
+ : CostModelAbstract(state, activation, nu), uref_(Eigen::VectorXd::Zero(nu)) {
+ assert(activation.get_nr() == nu_ && "activation::nr is not equals to nu");
+}
+
+CostModelControl::CostModelControl(StateMultibody& state, const Eigen::VectorXd& uref)
+ : CostModelAbstract(state, (unsigned int)uref.size(), (unsigned int)uref.size()), uref_(uref) {}
+
+CostModelControl::CostModelControl(StateMultibody& state)
+ : CostModelAbstract(state, state.get_nv()), uref_(Eigen::VectorXd::Zero(state.get_nv())) {}
+
+CostModelControl::CostModelControl(StateMultibody& state, unsigned int const& nu)
+ : CostModelAbstract(state, nu, nu), uref_(Eigen::VectorXd::Zero(nu)) {}
+
+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(u.size() == nu_ && "u has wrong dimension");
+
+ data->r = u - uref_;
+ activation_.calc(data->activation, data->r);
+ data->cost = data->activation->a_value;
+}
+
+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(u.size() == nu_ && "u has wrong dimension");
+
+ if (recalc) {
+ calc(data, x, u);
+ }
+ activation_.calcDiff(data->activation, data->r, recalc);
+ data->Lu = data->activation->Ar;
+ data->Luu.diagonal() = data->activation->Arr.diagonal();
+}
+
+const Eigen::VectorXd& CostModelControl::get_uref() const { return uref_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/costs/cost-sum.cpp b/src/multibody/costs/cost-sum.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4d7bcc1b73d59d82fc42cd22df0856b0f1c096a0
--- /dev/null
+++ b/src/multibody/costs/cost-sum.cpp
@@ -0,0 +1,127 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/costs/cost-sum.hpp"
+
+namespace crocoddyl {
+
+CostModelSum::CostModelSum(StateMultibody& state, unsigned int const& nu, const bool& with_residuals)
+ : state_(state), nu_(nu), nr_(0), with_residuals_(with_residuals) {}
+
+CostModelSum::CostModelSum(StateMultibody& state, const bool& with_residuals)
+ : state_(state), nu_(state.get_nv()), nr_(0), with_residuals_(with_residuals) {}
+
+CostModelSum::~CostModelSum() {}
+
+void CostModelSum::addCost(const std::string& name, CostModelAbstract* const cost, const double& weight) {
+ assert(cost->get_nu() == nu_ && "Cost item doesn't have the same control dimension");
+ std::pair<CostModelContainer::iterator, bool> ret =
+ costs_.insert(std::make_pair(name, CostItem(name, cost, weight)));
+ if (ret.second == false) {
+ std::cout << "Warning: this cost item already existed, we cannot add it" << std::endl;
+ } else {
+ nr_ += cost->get_activation().get_nr();
+ }
+}
+
+void CostModelSum::removeCost(const std::string& name) {
+ CostModelContainer::iterator it = costs_.find(name);
+ if (it != costs_.end()) {
+ nr_ -= it->second.cost->get_activation().get_nr();
+ costs_.erase(it);
+ } else {
+ std::cout << "Warning: this cost item doesn't exist, we cannot remove it" << std::endl;
+ }
+}
+
+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(data->costs.size() == costs_.size() && "it doesn't match the number of cost datas and models");
+ data->cost = 0.;
+ unsigned int nr = 0;
+
+ CostModelContainer::iterator it_m, end_m;
+ CostDataContainer::iterator it_d, end_d;
+ for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(), end_d = data->costs.end();
+ it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
+ const CostItem& m_i = it_m->second;
+ boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
+ assert(it_m->first == it_d->first && "it doesn't match the cost name between data and model");
+
+ m_i.cost->calc(d_i, x, u);
+ data->cost += m_i.weight * d_i->cost;
+ if (with_residuals_) {
+ unsigned int const& nr_i = m_i.cost->get_activation().get_nr();
+ data->r.segment(nr, nr_i) = sqrt(m_i.weight) * d_i->r;
+ nr += nr_i;
+ }
+ }
+}
+
+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(data->costs.size() == costs_.size() && "it doesn't match the number of cost datas and models");
+ if (recalc) {
+ calc(data, x, u);
+ }
+ unsigned int nr = 0;
+ data->Lx.fill(0);
+ data->Lu.fill(0);
+ data->Lxx.fill(0);
+ data->Lxu.fill(0);
+ data->Luu.fill(0);
+
+ unsigned int const& ndx = state_.get_ndx();
+ CostModelContainer::iterator it_m, end_m;
+ CostDataContainer::iterator it_d, end_d;
+ for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(), end_d = data->costs.end();
+ it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
+ const CostItem& m_i = it_m->second;
+ boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
+ assert(it_m->first == it_d->first && "it doesn't match the cost name between data and model");
+
+ m_i.cost->calcDiff(d_i, x, u);
+ data->Lx += m_i.weight * d_i->Lx;
+ data->Lu += m_i.weight * d_i->Lu;
+ data->Lxx += m_i.weight * d_i->Lxx;
+ data->Lxu += m_i.weight * d_i->Lxu;
+ data->Luu += m_i.weight * d_i->Luu;
+ if (with_residuals_) {
+ const unsigned int& nr_i = m_i.cost->get_activation().get_nr();
+ data->Rx.block(nr, 0, nr_i, ndx) = sqrt(m_i.weight) * d_i->Rx;
+ data->Ru.block(nr, 0, nr_i, nu_) = sqrt(m_i.weight) * d_i->Ru;
+ nr += nr_i;
+ }
+ }
+}
+
+boost::shared_ptr<CostDataSum> CostModelSum::createData(pinocchio::Data* const data) {
+ return boost::make_shared<CostDataSum>(this, data);
+}
+
+void CostModelSum::calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const Eigen::VectorXd>& x) {
+ calc(data, x, unone_);
+}
+
+void CostModelSum::calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const Eigen::VectorXd>& x) {
+ calcDiff(data, x, unone_);
+}
+
+StateMultibody& CostModelSum::get_state() const { return state_; }
+
+const CostModelSum::CostModelContainer& CostModelSum::get_costs() const { return costs_; }
+
+unsigned int const& CostModelSum::get_nu() const { return nu_; }
+
+unsigned int const& CostModelSum::get_nr() const { return nr_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/costs/frame-placement.cpp b/src/multibody/costs/frame-placement.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5dfd1f3113ae228a76036b8d6c2ca23f7c5d8ead
--- /dev/null
+++ b/src/multibody/costs/frame-placement.cpp
@@ -0,0 +1,80 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/costs/frame-placement.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+namespace crocoddyl {
+
+CostModelFramePlacement::CostModelFramePlacement(StateMultibody& state, ActivationModelAbstract& activation,
+ const FramePlacement& Mref, unsigned int const& nu)
+ : CostModelAbstract(state, activation, nu), Mref_(Mref), oMf_inv_(Mref.oMf.inverse()) {
+ assert(activation_.get_nr() == 6 && "activation::nr is not equals to 6");
+}
+
+CostModelFramePlacement::CostModelFramePlacement(StateMultibody& state, ActivationModelAbstract& activation,
+ const FramePlacement& Mref)
+ : CostModelAbstract(state, activation), Mref_(Mref), oMf_inv_(Mref.oMf.inverse()) {
+ assert(activation_.get_nr() == 6 && "activation::nr is not equals to 6");
+}
+
+CostModelFramePlacement::CostModelFramePlacement(StateMultibody& state, const FramePlacement& Mref,
+ unsigned int const& nu)
+ : CostModelAbstract(state, 6, nu), Mref_(Mref), oMf_inv_(Mref.oMf.inverse()) {}
+
+CostModelFramePlacement::CostModelFramePlacement(StateMultibody& state, const FramePlacement& Mref)
+ : CostModelAbstract(state, 6), Mref_(Mref), oMf_inv_(Mref.oMf.inverse()) {}
+
+CostModelFramePlacement::~CostModelFramePlacement() {}
+
+void CostModelFramePlacement::calc(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>&,
+ const Eigen::Ref<const Eigen::VectorXd>&) {
+ CostDataFramePlacement* d = static_cast<CostDataFramePlacement*>(data.get());
+
+ // Compute the frame placement w.r.t. the reference frame
+ d->rMf = oMf_inv_ * d->pinocchio->oMf[Mref_.frame];
+ d->r = pinocchio::log6(d->rMf);
+ data->r = d->r; // this is needed because we overwrite it
+
+ // Compute the cost
+ activation_.calc(d->activation, d->r);
+ d->cost = d->activation->a_value;
+}
+
+void CostModelFramePlacement::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
+ if (recalc) {
+ calc(data, x, u);
+ }
+ // Update the frame placements
+ CostDataFramePlacement* d = static_cast<CostDataFramePlacement*>(data.get());
+ pinocchio::updateFramePlacements(state_.get_pinocchio(), *d->pinocchio);
+
+ // // Compute the frame Jacobian at the error point
+ pinocchio::Jlog6(d->rMf, d->rJf);
+ pinocchio::getFrameJacobian(state_.get_pinocchio(), *d->pinocchio, Mref_.frame, pinocchio::LOCAL, d->fJf);
+ d->J.noalias() = d->rJf * d->fJf;
+
+ // Compute the derivatives of the frame placement
+ unsigned int const& nv = state_.get_nv();
+ activation_.calcDiff(data->activation, data->r, recalc);
+ data->Rx.leftCols(nv) = d->J;
+ data->Lx.head(nv).noalias() = d->J.transpose() * data->activation->Ar;
+ d->Arr_J.noalias() = data->activation->Arr * d->J;
+ data->Lxx.topLeftCorner(nv, nv).noalias() = d->J.transpose() * d->Arr_J;
+}
+
+boost::shared_ptr<CostDataAbstract> CostModelFramePlacement::createData(pinocchio::Data* const data) {
+ return boost::make_shared<CostDataFramePlacement>(this, data);
+}
+
+const FramePlacement& CostModelFramePlacement::get_Mref() const { return Mref_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/costs/frame-translation.cpp b/src/multibody/costs/frame-translation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e4f5a18100df18825753696ee7a1b08ae332309
--- /dev/null
+++ b/src/multibody/costs/frame-translation.cpp
@@ -0,0 +1,74 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/costs/frame-translation.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+namespace crocoddyl {
+
+CostModelFrameTranslation::CostModelFrameTranslation(StateMultibody& state, ActivationModelAbstract& activation,
+ const FrameTranslation& xref, unsigned int const& nu)
+ : CostModelAbstract(state, activation, nu), xref_(xref) {
+ assert(activation_.get_nr() == 3 && "activation::nr is not equals to 3");
+}
+
+CostModelFrameTranslation::CostModelFrameTranslation(StateMultibody& state, ActivationModelAbstract& activation,
+ const FrameTranslation& xref)
+ : CostModelAbstract(state, activation), xref_(xref) {
+ assert(activation_.get_nr() == 3 && "activation::nr is not equals to 3");
+}
+
+CostModelFrameTranslation::CostModelFrameTranslation(StateMultibody& state, const FrameTranslation& xref,
+ unsigned int const& nu)
+ : CostModelAbstract(state, 3, nu), xref_(xref) {}
+
+CostModelFrameTranslation::CostModelFrameTranslation(StateMultibody& state, const FrameTranslation& xref)
+ : CostModelAbstract(state, 3), xref_(xref) {}
+
+CostModelFrameTranslation::~CostModelFrameTranslation() {}
+
+void CostModelFrameTranslation::calc(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>&,
+ const Eigen::Ref<const Eigen::VectorXd>&) {
+ // Compute the frame translation w.r.t. the reference frame
+ data->r = data->pinocchio->oMf[xref_.frame].translation() - xref_.oxf;
+
+ // Compute the cost
+ activation_.calc(data->activation, data->r);
+ data->cost = data->activation->a_value;
+}
+
+void CostModelFrameTranslation::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
+ if (recalc) {
+ calc(data, x, u);
+ }
+ // Update the frame placements
+ CostDataFrameTranslation* d = static_cast<CostDataFrameTranslation*>(data.get());
+ pinocchio::updateFramePlacements(state_.get_pinocchio(), *d->pinocchio);
+
+ // Compute the frame Jacobian at the error point
+ pinocchio::getFrameJacobian(state_.get_pinocchio(), *d->pinocchio, xref_.frame, pinocchio::LOCAL, d->fJf);
+ d->J = d->pinocchio->oMf[xref_.frame].rotation() * d->fJf.topRows<3>();
+
+ // Compute the derivatives of the frame placement
+ unsigned int const& nv = state_.get_nv();
+ activation_.calcDiff(d->activation, d->r, recalc);
+ d->Rx.leftCols(nv) = d->J;
+ d->Lx.head(nv) = d->J.transpose() * d->activation->Ar;
+ d->Lxx.topLeftCorner(nv, nv) = d->J.transpose() * d->activation->Arr * d->J;
+}
+
+boost::shared_ptr<CostDataAbstract> CostModelFrameTranslation::createData(pinocchio::Data* const data) {
+ return boost::make_shared<CostDataFrameTranslation>(this, data);
+}
+
+const FrameTranslation& CostModelFrameTranslation::get_xref() const { return xref_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/costs/frame-velocity.cpp b/src/multibody/costs/frame-velocity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6740b79d484e256f08af428e07be389202059eb4
--- /dev/null
+++ b/src/multibody/costs/frame-velocity.cpp
@@ -0,0 +1,76 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/costs/frame-velocity.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+#include <pinocchio/algorithm/kinematics-derivatives.hpp>
+
+namespace crocoddyl {
+
+CostModelFrameVelocity::CostModelFrameVelocity(StateMultibody& state, ActivationModelAbstract& activation,
+ const FrameMotion& vref, unsigned int const& nu)
+ : CostModelAbstract(state, activation, nu), vref_(vref) {
+ assert(activation_.get_nr() == 6 && "activation::nr is not equals to 6");
+}
+
+CostModelFrameVelocity::CostModelFrameVelocity(StateMultibody& state, ActivationModelAbstract& activation,
+ const FrameMotion& vref)
+ : CostModelAbstract(state, activation), vref_(vref) {
+ assert(activation_.get_nr() == 6 && "activation::nr is not equals to 6");
+}
+
+CostModelFrameVelocity::CostModelFrameVelocity(StateMultibody& state, const FrameMotion& vref, unsigned int const& nu)
+ : CostModelAbstract(state, 6, nu), vref_(vref) {}
+
+CostModelFrameVelocity::CostModelFrameVelocity(StateMultibody& state, const FrameMotion& vref)
+ : CostModelAbstract(state, 6), vref_(vref) {}
+
+CostModelFrameVelocity::~CostModelFrameVelocity() {}
+
+void CostModelFrameVelocity::calc(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>&, const Eigen::Ref<const Eigen::VectorXd>&) {
+ CostDataFrameVelocity* d = static_cast<CostDataFrameVelocity*>(data.get());
+
+ // Compute the frame velocity w.r.t. the reference frame
+ d->vr = pinocchio::getFrameVelocity(state_.get_pinocchio(), *data->pinocchio, vref_.frame) - vref_.oMf;
+ data->r = d->vr.toVector();
+
+ // Compute the cost
+ activation_.calc(data->activation, data->r);
+ data->cost = data->activation->a_value;
+}
+
+void CostModelFrameVelocity::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc) {
+ if (recalc) {
+ calc(data, x, u);
+ }
+
+ // Get the partial derivatives of the local frame velocity
+ CostDataFrameVelocity* d = static_cast<CostDataFrameVelocity*>(data.get());
+ pinocchio::getJointVelocityDerivatives(state_.get_pinocchio(), *data->pinocchio, d->joint, pinocchio::LOCAL,
+ d->v_partial_dq, d->v_partial_dv);
+
+ // Compute the derivatives of the frame velocity
+ unsigned int const& nv = state_.get_nv();
+ activation_.calcDiff(data->activation, data->r, recalc);
+ data->Rx.leftCols(nv).noalias() = d->fXj * d->v_partial_dq;
+ data->Rx.rightCols(nv).noalias() = d->fXj * d->v_partial_dv;
+ data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
+ d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
+ data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
+}
+
+boost::shared_ptr<CostDataAbstract> CostModelFrameVelocity::createData(pinocchio::Data* const data) {
+ return boost::make_shared<CostDataFrameVelocity>(this, data);
+}
+
+const FrameMotion& CostModelFrameVelocity::get_vref() const { return vref_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/costs/state.cpp b/src/multibody/costs/state.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a6483fbccbdf8ff108d1c84fae0c8dc680d757c
--- /dev/null
+++ b/src/multibody/costs/state.cpp
@@ -0,0 +1,95 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/costs/state.hpp"
+
+namespace crocoddyl {
+
+CostModelState::CostModelState(StateMultibody& state, ActivationModelAbstract& activation, const Eigen::VectorXd& xref,
+ unsigned int const& nu)
+ : CostModelAbstract(state, activation, nu), xref_(xref) {
+ assert(xref_.size() == state_.get_nx() && "CostModelState: reference is not dimension nx");
+ assert(activation_.get_nr() == state_.get_ndx() && "CostModelState: nr is not equals to ndx");
+}
+
+CostModelState::CostModelState(StateMultibody& state, ActivationModelAbstract& activation, const Eigen::VectorXd& xref)
+ : CostModelAbstract(state, activation), xref_(xref) {
+ assert(xref_.size() == state_.get_nx() && "CostModelState: reference is not dimension nx");
+ assert(activation_.get_nr() == state_.get_ndx() && "CostModelState: nr is not equals to ndx");
+}
+
+CostModelState::CostModelState(StateMultibody& state, const Eigen::VectorXd& xref, unsigned int const& nu)
+ : CostModelAbstract(state, state.get_ndx(), nu), xref_(xref) {
+ assert(xref_.size() == state_.get_nx() && "CostModelState: reference is not dimension nx");
+ assert(activation_.get_nr() == state_.get_ndx() && "CostModelState: nr is not equals to ndx");
+}
+
+CostModelState::CostModelState(StateMultibody& state, const Eigen::VectorXd& xref)
+ : CostModelAbstract(state, state.get_ndx()), xref_(xref) {
+ assert(xref_.size() == state_.get_nx() && "CostModelState: reference is not dimension nx");
+ assert(activation_.get_nr() == state_.get_ndx() && "CostModelState: nr is not equals to ndx");
+}
+
+CostModelState::CostModelState(StateMultibody& state, ActivationModelAbstract& activation, unsigned int const& nu)
+ : CostModelAbstract(state, activation, nu), xref_(state.zero()) {
+ assert(xref_.size() == state_.get_nx() && "CostModelState: reference is not dimension nx");
+ assert(activation_.get_nr() == state_.get_ndx() && "CostModelState: nr is not equals to ndx");
+}
+
+CostModelState::CostModelState(StateMultibody& state, unsigned int const& nu)
+ : CostModelAbstract(state, state.get_ndx(), nu), xref_(state.zero()) {
+ assert(xref_.size() == state_.get_nx() && "CostModelState: reference is not dimension nx");
+ assert(activation_.get_nr() == state_.get_ndx() && "CostModelState: nr is not equals to ndx");
+}
+
+CostModelState::CostModelState(StateMultibody& state, ActivationModelAbstract& activation)
+ : CostModelAbstract(state, activation), xref_(state.zero()) {
+ assert(xref_.size() == state_.get_nx() && "CostModelState: reference is not dimension nx");
+ assert(activation_.get_nr() == state_.get_ndx() && "CostModelState: nr is not equals to ndx");
+}
+
+CostModelState::CostModelState(StateMultibody& state)
+ : CostModelAbstract(state, state.get_ndx()), xref_(state.zero()) {
+ assert(xref_.size() == state_.get_nx() && "CostModelState: reference is not dimension nx");
+ assert(activation_.get_nr() == state_.get_ndx() && "CostModelState: nr is not equals to ndx");
+}
+
+CostModelState::~CostModelState() {}
+
+void CostModelState::calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>&) {
+ assert(x.size() == state_.get_nx() && "CostModelState::calc: x has wrong dimension");
+
+ state_.diff(xref_, x, data->r);
+ activation_.calc(data->activation, data->r);
+ data->cost = data->activation->a_value;
+}
+
+void CostModelState::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(x.size() == state_.get_nx() && "x has wrong dimension");
+
+ CostDataState* d = static_cast<CostDataState*>(data.get());
+ if (recalc) {
+ calc(data, x, u);
+ }
+ state_.Jdiff(xref_, x, data->Rx, data->Rx, second);
+ activation_.calcDiff(data->activation, data->r, recalc);
+ data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
+ d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
+ data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
+}
+
+boost::shared_ptr<CostDataAbstract> CostModelState::createData(pinocchio::Data* const data) {
+ return boost::make_shared<CostDataState>(this, data);
+}
+
+const Eigen::VectorXd& CostModelState::get_xref() const { return xref_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/impulse-base.cpp b/src/multibody/impulse-base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4c7700347339b24a540b30c387b84dc781262ea6
--- /dev/null
+++ b/src/multibody/impulse-base.cpp
@@ -0,0 +1,25 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/impulse-base.hpp"
+
+namespace crocoddyl {
+
+ImpulseModelAbstract::ImpulseModelAbstract(StateMultibody& state, unsigned int const& ni) : state_(state), ni_(ni) {}
+
+ImpulseModelAbstract::~ImpulseModelAbstract() {}
+
+boost::shared_ptr<ImpulseDataAbstract> ImpulseModelAbstract::createData(pinocchio::Data* const data) {
+ return boost::make_shared<ImpulseDataAbstract>(this, data);
+}
+
+StateMultibody& ImpulseModelAbstract::get_state() const { return state_; }
+
+unsigned int const& ImpulseModelAbstract::get_ni() const { return ni_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/impulses/impulse-3d.cpp b/src/multibody/impulses/impulse-3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..99264d76b05f5fdeb4abab5cd53c7af4360d22fa
--- /dev/null
+++ b/src/multibody/impulses/impulse-3d.cpp
@@ -0,0 +1,53 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/impulses/impulse-3d.hpp"
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics-derivatives.hpp>
+
+namespace crocoddyl {
+
+ImpulseModel3D::ImpulseModel3D(StateMultibody& state, unsigned int const& frame)
+ : ImpulseModelAbstract(state, 3), frame_(frame) {}
+
+ImpulseModel3D::~ImpulseModel3D() {}
+
+void ImpulseModel3D::calc(const boost::shared_ptr<ImpulseDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>&) {
+ ImpulseData3D* d = static_cast<ImpulseData3D*>(data.get());
+
+ pinocchio::getFrameJacobian(state_.get_pinocchio(), *d->pinocchio, frame_, pinocchio::LOCAL, d->fJf);
+ d->Jc = d->fJf.topRows<3>();
+}
+
+void ImpulseModel3D::calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const bool& recalc) {
+ if (recalc) {
+ calc(data, x);
+ }
+
+ 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;
+}
+
+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");
+ ImpulseData3D* d = static_cast<ImpulseData3D*>(data.get());
+ data->f = d->jMf.act(pinocchio::Force(lambda, Eigen::Vector3d::Zero()));
+}
+
+boost::shared_ptr<ImpulseDataAbstract> ImpulseModel3D::createData(pinocchio::Data* const data) {
+ return boost::make_shared<ImpulseData3D>(this, data);
+}
+
+unsigned int const& ImpulseModel3D::get_frame() const { return frame_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/impulses/impulse-6d.cpp b/src/multibody/impulses/impulse-6d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..473f73d9037d7e7d15a5a3a7b3299dd55da3c6fb
--- /dev/null
+++ b/src/multibody/impulses/impulse-6d.cpp
@@ -0,0 +1,52 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/impulses/impulse-6d.hpp"
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics-derivatives.hpp>
+
+namespace crocoddyl {
+
+ImpulseModel6D::ImpulseModel6D(StateMultibody& state, unsigned int const& frame)
+ : ImpulseModelAbstract(state, 6), frame_(frame) {}
+
+ImpulseModel6D::~ImpulseModel6D() {}
+
+void ImpulseModel6D::calc(const boost::shared_ptr<ImpulseDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>&) {
+ ImpulseData6D* d = static_cast<ImpulseData6D*>(data.get());
+
+ pinocchio::getFrameJacobian(state_.get_pinocchio(), *d->pinocchio, frame_, pinocchio::LOCAL, d->Jc);
+}
+
+void ImpulseModel6D::calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const bool& recalc) {
+ if (recalc) {
+ calc(data, x);
+ }
+
+ 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;
+}
+
+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");
+ ImpulseData6D* d = static_cast<ImpulseData6D*>(data.get());
+ data->f = d->jMf.act(pinocchio::Force(lambda, Eigen::Vector3d::Zero()));
+}
+
+boost::shared_ptr<ImpulseDataAbstract> ImpulseModel6D::createData(pinocchio::Data* const data) {
+ return boost::make_shared<ImpulseData6D>(this, data);
+}
+
+unsigned int const& ImpulseModel6D::get_frame() const { return frame_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/impulses/multiple-impulses.cpp b/src/multibody/impulses/multiple-impulses.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dda0d1bb80061ecb1bb7e37c42206b78d5daa31f
--- /dev/null
+++ b/src/multibody/impulses/multiple-impulses.cpp
@@ -0,0 +1,117 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/impulses/multiple-impulses.hpp"
+
+namespace crocoddyl {
+
+ImpulseModelMultiple::ImpulseModelMultiple(StateMultibody& state) : state_(state), ni_(0) {}
+
+ImpulseModelMultiple::~ImpulseModelMultiple() {}
+
+void ImpulseModelMultiple::addImpulse(const std::string& name, ImpulseModelAbstract* const impulse) {
+ std::pair<ImpulseModelContainer::iterator, bool> ret =
+ impulses_.insert(std::make_pair(name, ImpulseItem(name, impulse)));
+ if (ret.second == false) {
+ std::cout << "Warning: this impulse item already existed, we cannot add it" << std::endl;
+ } else {
+ ni_ += impulse->get_ni();
+ }
+}
+
+void ImpulseModelMultiple::removeImpulse(const std::string& name) {
+ ImpulseModelContainer::iterator it = impulses_.find(name);
+ if (it != impulses_.end()) {
+ ni_ -= it->second.impulse->get_ni();
+ impulses_.erase(it);
+ } else {
+ std::cout << "Warning: this impulse item doesn't exist, we cannot remove it" << std::endl;
+ }
+}
+
+void ImpulseModelMultiple::calc(const boost::shared_ptr<ImpulseDataMultiple>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x) {
+ assert(data->impulses.size() == impulses_.size() && "it doesn't match the number of impulse datas and models");
+ unsigned int ni = 0;
+
+ unsigned int const& nv = state_.get_nv();
+ ImpulseModelContainer::iterator it_m, end_m;
+ ImpulseDataContainer::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;
+ 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");
+
+ m_i.impulse->calc(d_i, x);
+ unsigned int const& ni_i = m_i.impulse->get_ni();
+ data->Jc.block(ni, 0, ni_i, nv) = d_i->Jc;
+ ni += ni_i;
+ }
+}
+
+void ImpulseModelMultiple::calcDiff(const boost::shared_ptr<ImpulseDataMultiple>& data,
+ const Eigen::Ref<const Eigen::VectorXd>& x, const bool& recalc) {
+ assert(data->impulses.size() == impulses_.size() && "it doesn't match the number of impulse datas and models");
+ if (recalc) {
+ calc(data, x);
+ }
+ unsigned int ni = 0;
+
+ unsigned int const& nv = state_.get_nv();
+ ImpulseModelContainer::iterator it_m, end_m;
+ ImpulseDataContainer::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;
+ 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");
+
+ 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;
+ 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");
+ assert(data->impulses.size() == impulses_.size() && "it doesn't match the number of impulse datas and models");
+ unsigned int ni = 0;
+
+ for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
+ *it = pinocchio::Force::Zero();
+ }
+
+ ImpulseModelContainer::iterator it_m, end_m;
+ ImpulseDataContainer::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;
+ 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();
+ m_i.impulse->updateLagrangian(d_i, lambda.segment(ni, ni_i));
+ data->fext[d_i->joint] = d_i->f;
+ ni += ni_i;
+ }
+}
+
+boost::shared_ptr<ImpulseDataMultiple> ImpulseModelMultiple::createData(pinocchio::Data* const data) {
+ return boost::make_shared<ImpulseDataMultiple>(this, data);
+}
+
+StateMultibody& ImpulseModelMultiple::get_state() const { return state_; }
+
+const ImpulseModelMultiple::ImpulseModelContainer& ImpulseModelMultiple::get_impulses() const { return impulses_; }
+
+const unsigned int& ImpulseModelMultiple::get_ni() const { return ni_; }
+
+} // namespace crocoddyl
diff --git a/src/multibody/states/multibody.cpp b/src/multibody/states/multibody.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fbd78b798ad3aabdc4d47f02af5ced11a2c573d2
--- /dev/null
+++ b/src/multibody/states/multibody.cpp
@@ -0,0 +1,181 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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/states/multibody.hpp"
+#include <pinocchio/algorithm/joint-configuration.hpp>
+
+namespace crocoddyl {
+
+StateMultibody::StateMultibody(pinocchio::Model& model)
+ : StateAbstract(model.nq + model.nv, 2 * model.nv),
+ pinocchio_(model),
+ x0_(Eigen::VectorXd::Zero(model.nq + model.nv)),
+ dx_(Eigen::VectorXd::Zero(2 * model.nv)),
+ q0_(Eigen::VectorXd::Zero(model.nq)),
+ dq0_(Eigen::VectorXd::Zero(model.nv)),
+ q1_(Eigen::VectorXd::Zero(model.nq)),
+ dq1_(Eigen::VectorXd::Zero(model.nv)),
+ Ji_(Eigen::MatrixXd::Zero(model.nv, model.nv)),
+ Jd_(Eigen::MatrixXd::Zero(model.nv, model.nv)) {
+ x0_.head(nq_) = pinocchio::neutral(pinocchio_);
+}
+
+StateMultibody::~StateMultibody() {}
+
+Eigen::VectorXd StateMultibody::zero() { return x0_; }
+
+Eigen::VectorXd StateMultibody::rand() {
+ Eigen::VectorXd xrand = Eigen::VectorXd::Random(nx_);
+ xrand.head(nq_) = pinocchio::randomConfiguration(pinocchio_);
+ return xrand;
+}
+
+void StateMultibody::diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::VectorXd> dxout) {
+ assert(x0.size() == nx_ && "x0 has wrong dimension");
+ assert(x1.size() == nx_ && "x1 has wrong dimension");
+ assert(dxout.size() == ndx_ && "Output must be pre-allocated");
+
+ q0_ = x0.head(nq_);
+ q1_ = x1.head(nq_);
+ dq0_ = x0.tail(nv_);
+ dq1_ = x1.tail(nv_);
+ pinocchio::difference(pinocchio_, q0_, q1_, dxout.head(nv_));
+ dxout.tail(nv_) = dq1_ - dq0_;
+}
+
+void StateMultibody::integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
+ Eigen::Ref<Eigen::VectorXd> xout) {
+ assert(x.size() == nx_ && "x has wrong dimension");
+ assert(dx.size() == ndx_ && "dx has wrong dimension");
+ assert(xout.size() == nx_ && "Output must be pre-allocated");
+
+ q0_ = x.head(nq_);
+ dq0_ = dx.head(nv_);
+ pinocchio::integrate(pinocchio_, q0_, dq0_, q1_);
+ xout.head(nq_) = q1_;
+
+ dq0_ = x.tail(nv_);
+ dq1_ = dx.tail(nv_);
+ xout.tail(nv_) = dq0_ + dq1_;
+}
+
+void StateMultibody::Jdiff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
+ Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
+ Jcomponent firstsecond) {
+ assert(x0.size() == nx_ && "x0 has wrong dimension");
+ assert(x1.size() == nx_ && "x1 has wrong dimension");
+ assert(is_a_Jcomponent(firstsecond) && ("firstsecond must be one of the Jcomponent {both, first, second}"));
+
+ if (firstsecond == first) {
+ assert(Jfirst.rows() == ndx_ && Jfirst.cols() == ndx_ && "Jfirst must be of the good size");
+
+ diff(x1, x0, dx_);
+ q1_ = x1.head(nq_);
+ dq1_ = dx_.head(nv_);
+ pinocchio::dIntegrate(pinocchio_, q1_, dq1_, Ji_, pinocchio::ARG1);
+ updateJdiff(Ji_, false);
+
+ Jfirst.setZero();
+ Jfirst.topLeftCorner(nv_, nv_) = Jd_;
+ Jfirst.bottomRightCorner(nv_, nv_).diagonal() = -Eigen::VectorXd::Ones(nv_);
+ } else if (firstsecond == second) {
+ assert(Jsecond.rows() == ndx_ && Jsecond.cols() == ndx_ && "Jsecond must be of the good size");
+
+ diff(x0, x1, dx_);
+ q0_ = x0.head(nq_);
+ dq0_ = dx_.head(nv_);
+ pinocchio::dIntegrate(pinocchio_, q0_, dq0_, Ji_, pinocchio::ARG1);
+ updateJdiff(Ji_);
+
+ Jsecond.setZero();
+ Jsecond.topLeftCorner(nv_, nv_) = Jd_;
+ Jsecond.bottomRightCorner(nv_, nv_).diagonal() = Eigen::VectorXd::Ones(nv_);
+ } else { // computing both
+ assert(Jfirst.rows() == ndx_ && Jfirst.cols() == ndx_ && "Jfirst must be of the good size");
+ assert(Jsecond.rows() == ndx_ && Jsecond.cols() == ndx_ && "Jsecond must be of the good size");
+
+ // Computing Jfirst
+ diff(x1, x0, dx_);
+ q1_ = x1.head(nq_);
+ dq1_ = dx_.head(nv_);
+ pinocchio::dIntegrate(pinocchio_, q1_, dq1_, Ji_, pinocchio::ARG1);
+ updateJdiff(Ji_, false);
+
+ Jfirst.setZero();
+ Jfirst.topLeftCorner(nv_, nv_) = Jd_;
+ Jfirst.bottomRightCorner(nv_, nv_).diagonal() = -Eigen::VectorXd::Ones(nv_);
+
+ // Computing Jsecond
+ diff(x0, x1, dx_);
+ q0_ = x0.head(nq_);
+ dq0_ = dx_.head(nv_);
+ pinocchio::dIntegrate(pinocchio_, q0_, dq0_, Ji_, pinocchio::ARG1);
+ updateJdiff(Ji_);
+
+ Jsecond.setZero();
+ Jsecond.topLeftCorner(nv_, nv_) = Jd_;
+ Jsecond.bottomRightCorner(nv_, nv_).diagonal() = Eigen::VectorXd::Ones(nv_);
+ }
+}
+
+void StateMultibody::Jintegrate(const Eigen::Ref<const Eigen::VectorXd>& x,
+ const Eigen::Ref<const Eigen::VectorXd>& dx, Eigen::Ref<Eigen::MatrixXd> Jfirst,
+ Eigen::Ref<Eigen::MatrixXd> Jsecond, Jcomponent firstsecond) {
+ assert(x.size() == nx_ && "x has wrong dimension");
+ assert(dx.size() == ndx_ && "dx has wrong dimension");
+ assert((firstsecond == first || firstsecond == second || firstsecond == both) &&
+ ("firstsecond must be one of the Jcomponent {both, first, second}"));
+
+ q0_ = x.head(nq_);
+ dq0_ = dx.head(nv_);
+ if (firstsecond == first) {
+ assert(Jfirst.rows() == ndx_ && Jfirst.cols() == ndx_ && "Jfirst must be of the good size");
+
+ pinocchio::dIntegrate(pinocchio_, q0_, dq0_, Ji_, pinocchio::ARG0);
+ Jfirst.setZero();
+ Jfirst.topLeftCorner(nv_, nv_) = Ji_;
+ Jfirst.bottomRightCorner(nv_, nv_).diagonal() = Eigen::VectorXd::Ones(nv_);
+ } else if (firstsecond == second) {
+ assert(Jsecond.rows() == ndx_ && Jsecond.cols() == ndx_ && "Jsecond must be of the good size");
+
+ pinocchio::dIntegrate(pinocchio_, q0_, dq0_, Ji_, pinocchio::ARG1);
+ Jsecond.setZero();
+ Jsecond.topLeftCorner(nv_, nv_) = Ji_;
+ Jsecond.bottomRightCorner(nv_, nv_).diagonal() = Eigen::VectorXd::Ones(nv_);
+ } else { // computing both
+ assert(Jfirst.rows() == ndx_ && Jfirst.cols() == ndx_ && "Jfirst must be of the good size");
+ assert(Jsecond.rows() == ndx_ && Jsecond.cols() == ndx_ && "Jsecond must be of the good size");
+
+ // Computing Jfirst
+ pinocchio::dIntegrate(pinocchio_, q0_, dq0_, Ji_, pinocchio::ARG0);
+ Jfirst.setZero();
+ Jfirst.topLeftCorner(nv_, nv_) = Ji_;
+ Jfirst.bottomRightCorner(nv_, nv_).diagonal() = Eigen::VectorXd::Ones(nv_);
+
+ // Computing Jsecond
+ pinocchio::dIntegrate(pinocchio_, q0_, dq0_, Ji_, pinocchio::ARG1);
+ Jsecond.setZero();
+ Jsecond.topLeftCorner(nv_, nv_) = Ji_;
+ Jsecond.bottomRightCorner(nv_, nv_).diagonal() = Eigen::VectorXd::Ones(nv_);
+ }
+}
+
+pinocchio::Model& StateMultibody::get_pinocchio() const { return pinocchio_; }
+
+void StateMultibody::updateJdiff(const Eigen::Ref<const Eigen::MatrixXd>& Jdq, bool positive) {
+ if (positive) {
+ Jd_.diagonal() = Jdq.diagonal();
+ Jd_.block<6, 6>(0, 0) = Jdq.block<6, 6>(0, 0).inverse();
+ } else {
+ Jd_.diagonal() = -Jdq.diagonal();
+ Jd_.block<6, 6>(0, 0) = -Jdq.block<6, 6>(0, 0).inverse();
+ }
+}
+
+} // namespace crocoddyl
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index 7c4e93ace86c9c2b56d82bf67cd49f37d954e2f7..0b2a967a970d8c6714806fc8e35f000251573e96 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -1,23 +1,62 @@
-SET(${PROJECT_NAME}_PYTHON_TESTS
- actions
- activation
- actuation
- armature
- boxsolvers
- contacts
- costs
- ddp_contact
- dse3
- dynamic_derivatives
- impact
- locomotion
- quadruped
- rk4
- robots
- solvers
- state
+ADD_SUBDIRECTORY(python)
+
+SET(${PROJECT_NAME}_CPP_TESTS
+ ## without KKT
+ # testutils
+ test_state
+ # test_robots
+ # test_rk4
+ # test_quadruped
+ # test_locomotion
+ # test_impact
+ # test_dynamic_derivatives
+ # test_dse3
+ # test_ddp_contact
+ # test_armature
+ # test_actuation
+ # test_activation
+ test_actions
+
+ ## with KKT
+ # test_solvers
+ # test_costs
+ # test_contacts
+ # test_boxsolvers
)
-FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
- ADD_PYTHON_UNIT_TEST("py-${TEST}" "unittest/test_${TEST}.py" ".")
-ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
+MACRO(ADD_TEST_CFLAGS target flag)
+ SET_PROPERTY(TARGET ${target} APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}")
+ENDMACRO(ADD_TEST_CFLAGS)
+
+MACRO(ADD_CPP_UNIT_TEST NAME PKGS)
+ set(unittest_name ${NAME})
+
+ ADD_UNIT_TEST(${unittest_name} ${NAME}.cpp)
+
+ SET_TARGET_PROPERTIES(${unittest_name} PROPERTIES LINKER_LANGUAGE CXX)
+
+ ADD_TEST_CFLAGS(${unittest_name} "-DBOOST_TEST_DYN_LINK")
+
+ SET(MODULE_NAME "${NAME}Test")
+ STRING(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME})
+ ADD_TEST_CFLAGS(${unittest_name} "-DTEST_MODULE_NAME=${MODULE_NAME}")
+
+ FOREACH(PKG ${PKGS})
+ PKG_CONFIG_USE_DEPENDENCY(${unittest_name} ${PKG})
+ ENDFOREACH(PKG)
+
+ TARGET_LINK_LIBRARIES(${unittest_name} ${PROJECT_NAME})
+ TARGET_LINK_LIBRARIES(${unittest_name} ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
+
+ ADD_TEST_CFLAGS(${unittest_name} '-DCROCODDYL_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
+ENDMACRO(ADD_CPP_UNIT_TEST NAME PKGS)
+
+
+FOREACH(TEST ${${PROJECT_NAME}_CPP_TESTS})
+ ADD_CPP_UNIT_TEST(${TEST} "eigen3;pinocchio")
+ENDFOREACH(TEST ${${PROJECT_NAME}_CPP_TESTS})
+
+
+IF(BUILD_PYTHON_INTERFACE)
+ ADD_SUBDIRECTORY(bindings)
+ENDIF(BUILD_PYTHON_INTERFACE)
\ No newline at end of file
diff --git a/unittest/bindings/CMakeLists.txt b/unittest/bindings/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ce41362a40ab2b9bc117b83a60f992d1e239badd
--- /dev/null
+++ b/unittest/bindings/CMakeLists.txt
@@ -0,0 +1,14 @@
+SET(${PROJECT_NAME}_PYTHON_BINDINGS_TESTS
+ states
+ actuations
+ actions
+ shooting
+ solvers
+ costs
+ contacts
+ impulses
+ )
+
+FOREACH(TEST ${${PROJECT_NAME}_PYTHON_BINDINGS_TESTS})
+ ADD_PYTHON_UNIT_TEST("pybinds-${TEST}" "unittest/bindings/test_${TEST}.py" "bindings/python")
+ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_BINDINGS_TESTS})
diff --git a/unittest/bindings/test_actions.py b/unittest/bindings/test_actions.py
new file mode 100644
index 0000000000000000000000000000000000000000..785fbf287aff4533f1fe07a367eaf4729904b0ca
--- /dev/null
+++ b/unittest/bindings/test_actions.py
@@ -0,0 +1,121 @@
+import sys
+import unittest
+from random import randint
+
+import numpy as np
+
+import crocoddyl
+import pinocchio
+from crocoddyl.utils import DifferentialFreeFwdDynamicsDerived, DifferentialLQRDerived, LQRDerived, UnicycleDerived
+
+
+class ActionModelAbstractTestCase(unittest.TestCase):
+ MODEL = None
+ MODEL_DER = None
+
+ def setUp(self):
+ state = self.MODEL.state
+ self.x = state.rand()
+ self.u = np.matrix(np.random.rand(self.MODEL.nu)).T
+ self.DATA = self.MODEL.createData()
+ self.DATA_DER = self.MODEL_DER.createData()
+
+ def test_calc(self):
+ # Run calc for both action models
+ self.MODEL.calc(self.DATA, self.x, self.u)
+ self.MODEL_DER.calc(self.DATA_DER, self.x, self.u)
+ # Checking the cost value and its residual
+ self.assertAlmostEqual(self.DATA.cost, self.DATA_DER.cost, 10, "Wrong cost value.")
+ self.assertTrue(np.allclose(self.DATA.r, self.DATA_DER.r, atol=1e-9), "Wrong cost residuals.")
+
+ if isinstance(self.MODEL, crocoddyl.ActionModelAbstract):
+ # Checking the dimension of the next state
+ self.assertEqual(self.DATA.xnext.shape, self.DATA_DER.xnext.shape, "Wrong next state dimension.")
+ # Checking the next state value
+ self.assertTrue(np.allclose(self.DATA.xnext, self.DATA_DER.xnext, atol=1e-9), "Wrong next state.")
+ elif isinstance(self.MODEL, crocoddyl.DifferentialActionModelAbstract):
+ # Checking the dimension of the next state
+ self.assertEqual(self.DATA.xout.shape, self.DATA_DER.xout.shape, "Wrong next state dimension.")
+ # Checking the next state value
+ self.assertTrue(np.allclose(self.DATA.xout, self.DATA_DER.xout, atol=1e-9), "Wrong next state.")
+
+ def test_calcDiff(self):
+ # Run calcDiff for both action models
+ self.MODEL.calcDiff(self.DATA, self.x, self.u)
+ self.MODEL_DER.calcDiff(self.DATA_DER, self.x, self.u)
+ # Checking the next state value
+ if isinstance(self.MODEL, crocoddyl.ActionModelAbstract):
+ self.assertTrue(np.allclose(self.DATA.xnext, self.DATA_DER.xnext, atol=1e-9), "Wrong next state.")
+ elif isinstance(self.MODEL, crocoddyl.DifferentialActionModelAbstract):
+ self.assertTrue(np.allclose(self.DATA.xout, self.DATA_DER.xout, atol=1e-9), "Wrong next state.")
+ # Checking the Jacobians of the dynamic
+ self.assertTrue(np.allclose(self.DATA.Fx, self.DATA_DER.Fx, atol=1e-9), "Wrong Fx.")
+ self.assertTrue(np.allclose(self.DATA.Fu, self.DATA_DER.Fu, atol=1e-9), "Wrong Fu.")
+ # Checking the Jacobians and Hessians of the cost
+ self.assertTrue(np.allclose(self.DATA.Lx, self.DATA_DER.Lx, atol=1e-9), "Wrong Lx.")
+ self.assertTrue(np.allclose(self.DATA.Lu, self.DATA_DER.Lu, atol=1e-9), "Wrong Lu.")
+ self.assertTrue(np.allclose(self.DATA.Lxx, self.DATA_DER.Lxx, atol=1e-9), "Wrong Lxx.")
+ self.assertTrue(np.allclose(self.DATA.Lxu, self.DATA_DER.Lxu, atol=1e-9), "Wrong Lxu.")
+ self.assertTrue(np.allclose(self.DATA.Luu, self.DATA_DER.Luu, atol=1e-9), "Wrong Luu.")
+
+
+class UnicycleTest(ActionModelAbstractTestCase):
+ MODEL = crocoddyl.ActionModelUnicycle()
+ MODEL_DER = UnicycleDerived()
+
+
+class LQRTest(ActionModelAbstractTestCase):
+ NX = randint(1, 21)
+ NU = randint(1, NX)
+ MODEL = crocoddyl.ActionModelLQR(NX, NU)
+ MODEL_DER = LQRDerived(NX, NU)
+
+
+class DifferentialLQRTest(ActionModelAbstractTestCase):
+ NX = randint(1, 21)
+ NU = randint(1, NX)
+ MODEL = crocoddyl.DifferentialActionModelLQR(NX, NU)
+ MODEL_DER = DifferentialLQRDerived(NX, NU)
+
+
+class FreeFwdDynamicsTest(ActionModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
+ STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+ COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv)
+ COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1.)
+ COST_SUM.addCost(
+ 'frTrack',
+ crocoddyl.CostModelFramePlacement(
+ STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.)
+ MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM)
+ MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM)
+
+
+class FreeFwdDynamicsWithArmatureTest(ActionModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
+ STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+ COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv)
+ COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1.)
+ COST_SUM.addCost(
+ 'frTrack',
+ crocoddyl.CostModelFramePlacement(
+ STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.)
+ MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM)
+ MODEL.armature = 0.1 * np.matrix(np.ones(ROBOT_MODEL.nv)).T
+ MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM)
+ MODEL_DER.set_armature(0.1 * np.matrix(np.ones(ROBOT_MODEL.nv)).T)
+
+
+if __name__ == '__main__':
+ test_classes_to_run = [
+ UnicycleTest, LQRTest, DifferentialLQRTest, FreeFwdDynamicsTest, FreeFwdDynamicsWithArmatureTest
+ ]
+ loader = unittest.TestLoader()
+ suites_list = []
+ for test_class in test_classes_to_run:
+ suite = loader.loadTestsFromTestCase(test_class)
+ suites_list.append(suite)
+ big_suite = unittest.TestSuite(suites_list)
+ runner = unittest.TextTestRunner()
+ results = runner.run(big_suite)
+ sys.exit(not results.wasSuccessful())
diff --git a/unittest/bindings/test_actuations.py b/unittest/bindings/test_actuations.py
new file mode 100644
index 0000000000000000000000000000000000000000..48eeaee211c6b9ceecf04ada8dfa9432e22c6188
--- /dev/null
+++ b/unittest/bindings/test_actuations.py
@@ -0,0 +1,62 @@
+import sys
+import unittest
+
+import numpy as np
+
+import crocoddyl
+import pinocchio
+from crocoddyl.utils import FreeFloatingActuationDerived, FullActuationDerived
+
+
+class ActuationModelAbstractTestCase(unittest.TestCase):
+ STATE = None
+ ACTUATION = None
+ ACTUATION_DER = None
+
+ def setUp(self):
+ self.x = self.STATE.rand()
+ self.u = pinocchio.utils.rand(self.ACTUATION.nu)
+ self.DATA = self.ACTUATION.createData()
+ self.DATA_DER = self.ACTUATION_DER.createData()
+
+ def test_calc(self):
+ # Run calc for both action models
+ 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.")
+
+ 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.")
+
+
+class FloatingBaseActuationTest(ActuationModelAbstractTestCase):
+ STATE = crocoddyl.StateMultibody(pinocchio.buildSampleModelHumanoidRandom())
+
+ ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE)
+ ACTUATION_DER = FreeFloatingActuationDerived(STATE)
+
+
+class FullActuationTest(ActuationModelAbstractTestCase):
+ STATE = crocoddyl.StateMultibody(pinocchio.buildSampleModelManipulator())
+
+ ACTUATION = crocoddyl.ActuationModelFull(STATE)
+ ACTUATION_DER = FullActuationDerived(STATE)
+
+
+if __name__ == '__main__':
+ test_classes_to_run = [FloatingBaseActuationTest, FullActuationTest]
+ loader = unittest.TestLoader()
+ suites_list = []
+ for test_class in test_classes_to_run:
+ suite = loader.loadTestsFromTestCase(test_class)
+ suites_list.append(suite)
+ big_suite = unittest.TestSuite(suites_list)
+ runner = unittest.TextTestRunner()
+ results = runner.run(big_suite)
+ sys.exit(not results.wasSuccessful())
diff --git a/unittest/bindings/test_contacts.py b/unittest/bindings/test_contacts.py
new file mode 100644
index 0000000000000000000000000000000000000000..b4814aba3af22a1e9c71991356e2676a11cfabab
--- /dev/null
+++ b/unittest/bindings/test_contacts.py
@@ -0,0 +1,142 @@
+import sys
+import unittest
+
+import numpy as np
+
+import crocoddyl
+import pinocchio
+from crocoddyl.utils import Contact3DDerived, Contact6DDerived
+
+
+class ContactModelAbstractTestCase(unittest.TestCase):
+ ROBOT_MODEL = None
+ ROBOT_STATE = None
+ CONTACT = None
+ CONTACT_DER = None
+
+ def setUp(self):
+ self.x = self.ROBOT_STATE.rand()
+ self.robot_data = self.ROBOT_MODEL.createData()
+ self.data = self.CONTACT.createData(self.robot_data)
+ self.data_der = self.CONTACT_DER.createData(self.robot_data)
+
+ nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
+ pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+ pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+
+ def test_nc_dimension(self):
+ self.assertEqual(self.CONTACT.nc, self.CONTACT_DER.nc, "Wrong nc.")
+
+ def test_calc(self):
+ # Run calc for both action models
+ self.CONTACT.calc(self.data, self.x)
+ self.CONTACT_DER.calc(self.data_der, self.x)
+ # Checking the cost value and its residual
+ self.assertTrue(np.allclose(self.data.Jc, self.data_der.Jc, atol=1e-9), "Wrong contact Jacobian (Jc).")
+ self.assertTrue(np.allclose(self.data.a0, self.data_der.a0, atol=1e-9), "Wrong drift acceleration (a0).")
+
+ def test_calcDiff(self):
+ # Run calc for both action models
+ 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).")
+
+
+class ContactModelMultipleAbstractTestCase(unittest.TestCase):
+ ROBOT_MODEL = None
+ ROBOT_STATE = None
+ CONTACT = None
+
+ def setUp(self):
+ self.x = self.ROBOT_STATE.rand()
+ self.robot_data = self.ROBOT_MODEL.createData()
+
+ self.contacts = crocoddyl.ContactModelMultiple(self.ROBOT_STATE)
+ self.contacts.addContact("myContact", self.CONTACT)
+
+ self.data = self.CONTACT.createData(self.robot_data)
+ self.data_multiple = self.contacts.createData(self.robot_data)
+
+ nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
+ pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+ pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+
+ def test_nc_dimension(self):
+ self.assertEqual(self.CONTACT.nc, self.contacts.nc, "Wrong nc.")
+
+ def test_calc(self):
+ # Run calc for both action models
+ self.CONTACT.calc(self.data, self.x)
+ self.contacts.calc(self.data_multiple, self.x)
+ # Checking the cost value and its residual
+ self.assertTrue(np.allclose(self.data.Jc, self.data_multiple.Jc, atol=1e-9), "Wrong contact Jacobian (Jc).")
+ self.assertTrue(np.allclose(self.data.a0, self.data_multiple.a0, atol=1e-9), "Wrong drift acceleration (a0).")
+
+ def test_calcDiff(self):
+ # Run calc for both action models
+ 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).")
+
+
+class Contact3DTest(ContactModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ gains = pinocchio.utils.rand(2)
+ xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random().translation)
+ CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains)
+ CONTACT_DER = Contact3DDerived(ROBOT_STATE, xref, gains)
+
+
+class Contact3DMultipleTest(ContactModelMultipleAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ gains = pinocchio.utils.rand(2)
+ xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random().translation)
+ CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains)
+
+
+class Contact6DTest(ContactModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ gains = pinocchio.utils.rand(2)
+ Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random())
+ CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
+ CONTACT_DER = Contact6DDerived(ROBOT_STATE, Mref, gains)
+
+
+class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ gains = pinocchio.utils.rand(2)
+ Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random())
+ CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
+
+
+if __name__ == '__main__':
+ test_classes_to_run = [Contact3DTest, Contact3DMultipleTest, Contact6DTest, Contact6DMultipleTest]
+ loader = unittest.TestLoader()
+ suites_list = []
+ for test_class in test_classes_to_run:
+ suite = loader.loadTestsFromTestCase(test_class)
+ suites_list.append(suite)
+ big_suite = unittest.TestSuite(suites_list)
+ runner = unittest.TextTestRunner()
+ results = runner.run(big_suite)
+ sys.exit(not results.wasSuccessful())
diff --git a/unittest/bindings/test_costs.py b/unittest/bindings/test_costs.py
new file mode 100644
index 0000000000000000000000000000000000000000..0eb27e739f718485cc7a151aa1750da42ce658d5
--- /dev/null
+++ b/unittest/bindings/test_costs.py
@@ -0,0 +1,236 @@
+import sys
+import unittest
+
+import numpy as np
+
+import crocoddyl
+import pinocchio
+from crocoddyl.utils import (CoMPositionCostDerived, ControlCostDerived, FramePlacementCostDerived,
+ FrameTranslationCostDerived, FrameVelocityCostDerived, StateCostDerived)
+
+
+class CostModelAbstractTestCase(unittest.TestCase):
+ ROBOT_MODEL = None
+ ROBOT_STATE = None
+ COST = None
+ COST_DER = None
+
+ def setUp(self):
+ self.robot_data = self.ROBOT_MODEL.createData()
+ self.x = self.ROBOT_STATE.rand()
+ self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv)
+
+ self.data = self.COST.createData(self.robot_data)
+ self.data_der = self.COST_DER.createData(self.robot_data)
+
+ nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
+ pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:])
+ pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+ pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data, self.x[:nq])
+ pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data, self.x[:nq], False)
+
+ def test_dimensions(self):
+ self.assertEqual(self.COST.state.nx, self.COST_DER.state.nx, "Wrong nx.")
+ self.assertEqual(self.COST.state.ndx, self.COST_DER.state.ndx, "Wrong ndx.")
+ self.assertEqual(self.COST.nu, self.COST_DER.nu, "Wrong nu.")
+ self.assertEqual(self.COST.state.nq, self.COST_DER.state.nq, "Wrong nq.")
+ self.assertEqual(self.COST.state.nv, self.COST_DER.state.nv, "Wrong nv.")
+ self.assertEqual(self.COST.activation.nr, self.COST_DER.activation.nr, "Wrong nr.")
+
+ def test_calc(self):
+ # Run calc for both action models
+ self.COST.calc(self.data, self.x, self.u)
+ self.COST_DER.calc(self.data_der, self.x, self.u)
+ # Checking the cost value and its residual
+ self.assertAlmostEqual(self.data.cost, self.data_der.cost, 10, "Wrong cost value.")
+ self.assertTrue(np.allclose(self.data.r, self.data_der.r, atol=1e-9), "Wrong cost residuals.")
+
+ def test_calcDiff(self):
+ # Run calc for both action models
+ self.COST.calcDiff(self.data, self.x, self.u)
+ self.COST_DER.calcDiff(self.data_der, self.x, self.u)
+ # Checking the cost value and its residual
+ self.assertAlmostEqual(self.data.cost, self.data_der.cost, 10, "Wrong cost value.")
+ self.assertTrue(np.allclose(self.data.r, self.data_der.r, atol=1e-9), "Wrong cost residuals.")
+ # Checking the Jacobians and Hessians of the cost
+ self.assertTrue(np.allclose(self.data.Lx, self.data_der.Lx, atol=1e-9), "Wrong Lx.")
+ self.assertTrue(np.allclose(self.data.Lu, self.data_der.Lu, atol=1e-9), "Wrong Lu.")
+ self.assertTrue(np.allclose(self.data.Lxx, self.data_der.Lxx, atol=1e-9), "Wrong Lxx.")
+ self.assertTrue(np.allclose(self.data.Lxu, self.data_der.Lxu, atol=1e-9), "Wrong Lxu.")
+ self.assertTrue(np.allclose(self.data.Luu, self.data_der.Luu, atol=1e-9), "Wrong Luu.")
+
+
+class CostModelSumTestCase(unittest.TestCase):
+ ROBOT_MODEL = None
+ ROBOT_STATE = None
+ COST = None
+
+ def setUp(self):
+ self.robot_data = self.ROBOT_MODEL.createData()
+ self.x = self.ROBOT_STATE.rand()
+ self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv)
+
+ self.cost_sum = crocoddyl.CostModelSum(self.ROBOT_STATE)
+ self.cost_sum.addCost('myCost', self.COST, 1.)
+
+ self.data = self.COST.createData(self.robot_data)
+ self.data_sum = self.cost_sum.createData(self.robot_data)
+
+ nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
+ pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:])
+ pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+ pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data, self.x[:nq])
+ pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data, self.x[:nq], False)
+
+ def test_dimensions(self):
+ self.assertEqual(self.COST.state.nx, self.cost_sum.state.nx, "Wrong nx.")
+ self.assertEqual(self.COST.state.ndx, self.cost_sum.state.ndx, "Wrong ndx.")
+ self.assertEqual(self.COST.nu, self.cost_sum.nu, "Wrong nu.")
+ self.assertEqual(self.COST.state.nq, self.cost_sum.state.nq, "Wrong nq.")
+ self.assertEqual(self.COST.state.nv, self.cost_sum.state.nv, "Wrong nv.")
+ self.assertEqual(self.COST.activation.nr, self.cost_sum.nr, "Wrong nr.")
+
+ def test_calc(self):
+ # Run calc for both action models
+ self.COST.calc(self.data, self.x, self.u)
+ self.cost_sum.calc(self.data_sum, self.x, self.u)
+ # Checking the cost value and its residual
+ self.assertAlmostEqual(self.data.cost, self.data_sum.cost, 10, "Wrong cost value.")
+ self.assertTrue(np.allclose(self.data.r, self.data_sum.r, atol=1e-9), "Wrong cost residuals.")
+
+ def test_calcDiff(self):
+ # Run calc for both action models
+ self.COST.calcDiff(self.data, self.x, self.u)
+ self.cost_sum.calcDiff(self.data_sum, self.x, self.u)
+ # Checking the cost value and its residual
+ self.assertAlmostEqual(self.data.cost, self.data_sum.cost, 10, "Wrong cost value.")
+ self.assertTrue(np.allclose(self.data.r, self.data_sum.r, atol=1e-9), "Wrong cost residuals.")
+ # Checking the Jacobians and Hessians of the cost
+ self.assertTrue(np.allclose(self.data.Lx, self.data_sum.Lx, atol=1e-9), "Wrong Lx.")
+ self.assertTrue(np.allclose(self.data.Lu, self.data_sum.Lu, atol=1e-9), "Wrong Lu.")
+ self.assertTrue(np.allclose(self.data.Lxx, self.data_sum.Lxx, atol=1e-9), "Wrong Lxx.")
+ self.assertTrue(np.allclose(self.data.Lxu, self.data_sum.Lxu, atol=1e-9), "Wrong Lxu.")
+ self.assertTrue(np.allclose(self.data.Luu, self.data_sum.Luu, atol=1e-9), "Wrong Luu.")
+
+ def test_removeCost(self):
+ self.cost_sum.removeCost("myCost")
+ self.assertEqual(len(self.cost_sum.costs), 0, "The number of cost items should be zero")
+
+
+class StateCostTest(CostModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ COST = crocoddyl.CostModelState(ROBOT_STATE)
+ COST_DER = StateCostDerived(ROBOT_STATE)
+
+
+class StateCostSumTest(CostModelSumTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ COST = crocoddyl.CostModelState(ROBOT_STATE)
+
+
+class ControlCostTest(CostModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ COST = crocoddyl.CostModelControl(ROBOT_STATE)
+ COST_DER = ControlCostDerived(ROBOT_STATE)
+
+
+class ControlCostSumTest(CostModelSumTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ COST = crocoddyl.CostModelControl(ROBOT_STATE)
+
+
+class CoMPositionCostTest(CostModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ cref = pinocchio.utils.rand(3)
+ COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
+ COST_DER = CoMPositionCostDerived(ROBOT_STATE, cref=cref)
+
+
+class CoMPositionCostSumTest(CostModelSumTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ cref = pinocchio.utils.rand(3)
+ COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
+
+
+class FramePlacementCostTest(CostModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random())
+ COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
+ COST_DER = FramePlacementCostDerived(ROBOT_STATE, Mref=Mref)
+
+
+class FramePlacementCostSumTest(CostModelSumTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random())
+ COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
+
+
+class FrameTranslationCostTest(CostModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.utils.rand(3))
+ COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
+ COST_DER = FrameTranslationCostDerived(ROBOT_STATE, xref=xref)
+
+
+class FrameTranslationCostSumTest(CostModelSumTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.utils.rand(3))
+ COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
+
+
+class FrameVelocityCostTest(CostModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.Motion.Random())
+ COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
+ COST_DER = FrameVelocityCostDerived(ROBOT_STATE, vref=vref)
+
+
+class FrameVelocityCostSumTest(CostModelSumTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.Motion.Random())
+ COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
+
+
+if __name__ == '__main__':
+ test_classes_to_run = [
+ StateCostTest, StateCostSumTest, ControlCostTest, ControlCostSumTest, CoMPositionCostTest,
+ CoMPositionCostSumTest, FramePlacementCostTest, FramePlacementCostSumTest, FrameTranslationCostTest,
+ FrameTranslationCostSumTest, FrameVelocityCostTest, FrameVelocityCostSumTest
+ ]
+ loader = unittest.TestLoader()
+ suites_list = []
+ for test_class in test_classes_to_run:
+ suite = loader.loadTestsFromTestCase(test_class)
+ suites_list.append(suite)
+ big_suite = unittest.TestSuite(suites_list)
+ runner = unittest.TextTestRunner()
+ results = runner.run(big_suite)
+ sys.exit(not results.wasSuccessful())
diff --git a/unittest/bindings/test_impulses.py b/unittest/bindings/test_impulses.py
new file mode 100644
index 0000000000000000000000000000000000000000..c1c5d8e2fd34d2d337fdc4bd762df4b3e78cc9ec
--- /dev/null
+++ b/unittest/bindings/test_impulses.py
@@ -0,0 +1,134 @@
+import sys
+import unittest
+
+import numpy as np
+
+import crocoddyl
+import pinocchio
+from crocoddyl.utils import Impulse3DDerived, Impulse6DDerived
+
+
+class ImpulseModelAbstractTestCase(unittest.TestCase):
+ ROBOT_MODEL = None
+ ROBOT_STATE = None
+ IMPULSE = None
+ IMPULSE_DER = None
+
+ def setUp(self):
+ self.x = self.ROBOT_STATE.rand()
+ self.robot_data = self.ROBOT_MODEL.createData()
+ self.data = self.IMPULSE.createData(self.robot_data)
+ self.data_der = self.IMPULSE_DER.createData(self.robot_data)
+
+ nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
+ pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+ pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+
+ def test_ni_dimension(self):
+ self.assertEqual(self.IMPULSE.ni, self.IMPULSE_DER.ni, "Wrong ni.")
+
+ def test_calc(self):
+ # Run calc for both action models
+ self.IMPULSE.calc(self.data, self.x)
+ self.IMPULSE_DER.calc(self.data_der, self.x)
+ # Checking the cost value and its residual
+ self.assertTrue(np.allclose(self.data.Jc, self.data_der.Jc, atol=1e-9), "Wrong contact Jacobian (Jc).")
+
+ def test_calcDiff(self):
+ # Run calc for both action models
+ 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).")
+
+
+class ImpulseModelMultipleAbstractTestCase(unittest.TestCase):
+ ROBOT_MODEL = None
+ ROBOT_STATE = None
+ IMPULSE = None
+
+ def setUp(self):
+ self.x = self.ROBOT_STATE.rand()
+ self.robot_data = self.ROBOT_MODEL.createData()
+
+ self.impulses = crocoddyl.ImpulseModelMultiple(self.ROBOT_STATE)
+ self.impulses.addImpulse("myImpulse", self.IMPULSE)
+
+ self.data = self.IMPULSE.createData(self.robot_data)
+ self.data_multiple = self.impulses.createData(self.robot_data)
+
+ nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
+ pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+ pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
+ pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
+ pinocchio.utils.zero(nv))
+
+ def test_ni_dimension(self):
+ self.assertEqual(self.IMPULSE.ni, self.impulses.ni, "Wrong ni.")
+
+ def test_calc(self):
+ # Run calc for both action models
+ self.IMPULSE.calc(self.data, self.x)
+ self.impulses.calc(self.data_multiple, self.x)
+ # Checking the cost value and its residual
+ self.assertTrue(np.allclose(self.data.Jc, self.data_multiple.Jc, atol=1e-9), "Wrong contact Jacobian (Jc).")
+
+ def test_calcDiff(self):
+ # Run calc for both action models
+ 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).")
+
+
+class Impulse3DTest(ImpulseModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ # gains = pinocchio.utils.rand(2)
+ frame = ROBOT_MODEL.getFrameId('rleg5_joint')
+ IMPULSE = crocoddyl.ImpulseModel3D(ROBOT_STATE, frame)
+ IMPULSE_DER = Impulse3DDerived(ROBOT_STATE, frame)
+
+
+class Impulse3DMultipleTest(ImpulseModelMultipleAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ gains = pinocchio.utils.rand(2)
+ IMPULSE = crocoddyl.ImpulseModel3D(ROBOT_STATE, ROBOT_MODEL.getFrameId('rleg5_joint'))
+
+
+class Impulse6DTest(ImpulseModelAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ frame = ROBOT_MODEL.getFrameId('rleg5_joint')
+ IMPULSE = crocoddyl.ImpulseModel6D(ROBOT_STATE, frame)
+ IMPULSE_DER = Impulse6DDerived(ROBOT_STATE, frame)
+
+
+class Impulse6DMultipleTest(ImpulseModelMultipleAbstractTestCase):
+ ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
+
+ IMPULSE = crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('rleg5_joint'))
+
+
+if __name__ == '__main__':
+ test_classes_to_run = [Impulse3DTest, Impulse3DMultipleTest, Impulse6DTest, Impulse6DMultipleTest]
+ loader = unittest.TestLoader()
+ suites_list = []
+ for test_class in test_classes_to_run:
+ suite = loader.loadTestsFromTestCase(test_class)
+ suites_list.append(suite)
+ big_suite = unittest.TestSuite(suites_list)
+ runner = unittest.TextTestRunner()
+ results = runner.run(big_suite)
+ sys.exit(not results.wasSuccessful())
diff --git a/unittest/bindings/test_shooting.py b/unittest/bindings/test_shooting.py
new file mode 100644
index 0000000000000000000000000000000000000000..fa394f03e60c6fb60a1dd9c57143ea0d62f39170
--- /dev/null
+++ b/unittest/bindings/test_shooting.py
@@ -0,0 +1,75 @@
+import sys
+import unittest
+from random import randint
+
+import numpy as np
+
+import crocoddyl
+from crocoddyl.utils import UnicycleDerived
+
+
+class ShootingProblemTestCase(unittest.TestCase):
+ MODEL = None
+ MODEL_DER = None
+
+ def setUp(self):
+ self.T = randint(1, 101)
+ state = self.MODEL.state
+ self.xs = []
+ self.us = []
+ self.xs.append(state.rand())
+ for i in range(self.T):
+ self.xs.append(state.rand())
+ self.us.append(np.matrix(np.random.rand(self.MODEL.nu)).T)
+ self.PROBLEM = crocoddyl.ShootingProblem(self.xs[0], [self.MODEL] * self.T, self.MODEL)
+ self.PROBLEM_DER = crocoddyl.ShootingProblem(self.xs[0], [self.MODEL_DER] * self.T, self.MODEL_DER)
+
+ def test_number_of_nodes(self):
+ self.assertEqual(self.T, self.PROBLEM.T, "Wrong number of nodes")
+
+ def test_calc(self):
+ # Running calc functions
+ cost = self.PROBLEM.calc(self.xs, self.us)
+ costDer = self.PROBLEM_DER.calc(self.xs, self.us)
+ self.assertAlmostEqual(cost, costDer, 10, "Wrong cost value")
+ for d1, d2 in zip(self.PROBLEM.runningDatas, self.PROBLEM_DER.runningDatas):
+ self.assertTrue(np.allclose(d1.xnext, d2.xnext, atol=1e-9), "Next state doesn't match.")
+
+ def test_calcDiff(self):
+ # Running calc functions
+ cost = self.PROBLEM.calcDiff(self.xs, self.us)
+ costDer = self.PROBLEM_DER.calcDiff(self.xs, self.us)
+ self.assertAlmostEqual(cost, costDer, 10, "Wrong cost value")
+ for d1, d2 in zip(self.PROBLEM.runningDatas, self.PROBLEM_DER.runningDatas):
+ self.assertTrue(np.allclose(d1.xnext, d2.xnext, atol=1e-9), "Next state doesn't match.")
+ self.assertTrue(np.allclose(d1.Lx, d2.Lx, atol=1e-9), "Lx doesn't match.")
+ self.assertTrue(np.allclose(d1.Lu, d2.Lu, atol=1e-9), "Lu doesn't match.")
+ self.assertTrue(np.allclose(d1.Lxx, d2.Lxx, atol=1e-9), "Lxx doesn't match.")
+ self.assertTrue(np.allclose(d1.Lxu, d2.Lxu, atol=1e-9), "Lxu doesn't match.")
+ self.assertTrue(np.allclose(d1.Luu, d2.Luu, atol=1e-9), "Luu doesn't match.")
+ self.assertTrue(np.allclose(d1.Fx, d2.Fx, atol=1e-9), "Fx doesn't match.")
+ self.assertTrue(np.allclose(d1.Fu, d2.Fu, atol=1e-9), "Fu doesn't match.")
+
+ def test_rollout(self):
+ xs = self.PROBLEM.rollout(self.us)
+ xsDer = self.PROBLEM_DER.rollout(self.us)
+ for x1, x2 in zip(xs, xsDer):
+ self.assertTrue(np.allclose(x1, x2, atol=1e-9), "The rollout state doesn't match.")
+
+
+class UnicycleShootingTest(ShootingProblemTestCase):
+ MODEL = crocoddyl.ActionModelUnicycle()
+ MODEL_DER = UnicycleDerived()
+
+
+if __name__ == '__main__':
+ test_classes_to_run = [UnicycleShootingTest]
+ loader = unittest.TestLoader()
+ suites_list = []
+ for test_class in test_classes_to_run:
+ suite = loader.loadTestsFromTestCase(test_class)
+ suites_list.append(suite)
+ big_suite = unittest.TestSuite(suites_list)
+ runner = unittest.TextTestRunner()
+ results = runner.run(big_suite)
+ sys.exit(not results.wasSuccessful())
diff --git a/unittest/bindings/test_solvers.py b/unittest/bindings/test_solvers.py
new file mode 100644
index 0000000000000000000000000000000000000000..5dd124032916773054b0c44612a1f0450889d849
--- /dev/null
+++ b/unittest/bindings/test_solvers.py
@@ -0,0 +1,131 @@
+import sys
+import unittest
+from random import randint
+
+import numpy as np
+
+import crocoddyl
+import pinocchio
+from crocoddyl.utils import DDPDerived
+
+
+class SolverAbstractTestCase(unittest.TestCase):
+ MODEL = None
+ SOLVER = None
+ SOLVER_DER = None
+
+ def setUp(self):
+ # Set up the solvers
+ self.T = randint(1, 21)
+ state = self.MODEL.state
+ self.xs = []
+ self.us = []
+ self.xs.append(state.rand())
+ for i in range(self.T):
+ self.xs.append(state.rand())
+ self.us.append(pinocchio.utils.rand(self.MODEL.nu))
+ self.PROBLEM = crocoddyl.ShootingProblem(self.xs[0], [self.MODEL] * self.T, self.MODEL)
+ self.PROBLEM_DER = crocoddyl.ShootingProblem(self.xs[0], [self.MODEL] * self.T, self.MODEL)
+ self.solver = self.SOLVER(self.PROBLEM)
+ self.solver_der = self.SOLVER_DER(self.PROBLEM_DER)
+
+ def test_number_of_nodes(self):
+ # Check the number of nodes
+ self.assertEqual(self.T, self.solver.problem.T, "Wrong number of nodes in SOLVER")
+ self.assertEqual(self.T, self.solver_der.problem.T, "Wrong number of nodes in SOLVER_DER")
+
+ def test_solve(self):
+ # Run maximum 10 iterations in order to boost test analysis
+ self.solver.solve([], [], 10)
+ self.solver_der.solve([], [], 10)
+ for x1, x2 in zip(self.solver.xs, self.solver_der.xs):
+ self.assertTrue(np.allclose(x1, x2, atol=1e-9), "xs doesn't match.")
+ for u1, u2 in zip(self.solver.us, self.solver_der.us):
+ self.assertTrue(np.allclose(u1, u2, atol=1e-9), "us doesn't match.")
+ for k1, k2 in zip(self.solver.k, self.solver_der.k):
+ self.assertTrue(np.allclose(k1, k2, atol=1e-9), "k doesn't match.")
+
+ def test_compute_search_direction(self):
+ # Compute the direction
+ self.solver.computeDirection()
+ self.solver_der.computeDirection()
+ # Check the LQ model of the Hamiltonian
+ for qx1, qx2 in zip(self.solver.Qx, self.solver_der.Qx):
+ self.assertTrue(np.allclose(qx1, qx2, atol=1e-9), "Qx doesn't match.")
+ for qu1, qu2 in zip(self.solver.Qu, self.solver_der.Qu):
+ self.assertTrue(np.allclose(qu1, qu2, atol=1e-9), "Qu doesn't match.")
+ for qxx1, qxx2 in zip(self.solver.Qxx, self.solver_der.Qxx):
+ self.assertTrue(np.allclose(qxx1, qxx2, atol=1e-9), "Qxx doesn't match.")
+ for qxu1, qxu2 in zip(self.solver.Qxu, self.solver_der.Qxu):
+ self.assertTrue(np.allclose(qxu1, qxu2, atol=1e-9), "Quu doesn't match.")
+ for quu1, quu2 in zip(self.solver.Quu, self.solver_der.Quu):
+ self.assertTrue(np.allclose(qx1, qx2, atol=1e-9), "Quu doesn't match.")
+ for vx1, vx2 in zip(self.solver.Vx, self.solver_der.Vx):
+ self.assertTrue(np.allclose(vx1, vx2, atol=1e-9), "Vx doesn't match.")
+ for vxx1, vxx2 in zip(self.solver.Vxx, self.solver_der.Vxx):
+ self.assertTrue(np.allclose(vxx1, vxx2, atol=1e-9), "Vxx doesn't match.")
+
+ def test_try_step(self):
+ # Try a full step and check the improvement in the cost
+ self.solver.computeDirection()
+ self.solver_der.computeDirection()
+ cost = self.solver.tryStep()
+ costDer = self.solver_der.tryStep()
+ self.assertAlmostEqual(cost, costDer, 10, "Wrong cost value for full step")
+ # Try a half step and check the improvement in the cost
+ cost = self.solver.tryStep(0.5)
+ costDer = self.solver_der.tryStep(0.5)
+ self.assertAlmostEqual(cost, costDer, 10, "Wrong cost value for half step")
+
+ def test_stopping_criteria(self):
+ # Run 2 iteration in order to boost test analysis
+ self.solver.solve([], [], 2)
+ self.solver_der.solve([], [], 2)
+ # Compute and check the stopping criteria
+ stop = self.solver.stoppingCriteria()
+ stopDer = self.solver_der.stoppingCriteria()
+ self.assertAlmostEqual(stop, stopDer, 10, "Wrong stopping value")
+
+ def test_expected_improvement(self):
+ # Run 2 iteration in order to boost test analysis
+ self.solver.solve([], [], 2)
+ self.solver_der.solve([], [], 2)
+ expImp = self.solver.expectedImprovement()
+ expImpDer = self.solver_der.expectedImprovement()
+ self.assertTrue(np.allclose(expImp, expImpDer, atol=1e-9), "Expected improvement doesn't match.")
+
+
+class UnicycleDDPTest(SolverAbstractTestCase):
+ MODEL = crocoddyl.ActionModelUnicycle()
+ SOLVER = crocoddyl.SolverDDP
+ SOLVER_DER = DDPDerived
+
+
+class ManipulatorDDPTest(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.SolverDDP
+ SOLVER_DER = DDPDerived
+
+
+if __name__ == '__main__':
+ test_classes_to_run = [UnicycleDDPTest, ManipulatorDDPTest]
+ loader = unittest.TestLoader()
+ suites_list = []
+ for test_class in test_classes_to_run:
+ suite = loader.loadTestsFromTestCase(test_class)
+ suites_list.append(suite)
+ big_suite = unittest.TestSuite(suites_list)
+ runner = unittest.TextTestRunner()
+ results = runner.run(big_suite)
+ sys.exit(not results.wasSuccessful())
diff --git a/unittest/bindings/test_states.py b/unittest/bindings/test_states.py
new file mode 100644
index 0000000000000000000000000000000000000000..ac59912c0c7ffe1d2b6958a9a019ef920d7dd319
--- /dev/null
+++ b/unittest/bindings/test_states.py
@@ -0,0 +1,117 @@
+import sys
+import unittest
+from random import randint
+
+import numpy as np
+
+import crocoddyl
+import pinocchio
+from crocoddyl.utils import StateMultibodyDerived, StateVectorDerived
+
+
+class StateAbstractTestCase(unittest.TestCase):
+ NX = None
+ NDX = None
+ STATE = None
+ STATE_DER = None
+
+ def test_state_dimension(self):
+ # Checking the state dimensions
+ self.assertEqual(self.STATE.nx, self.STATE_DER.nx, "Wrong nx value.")
+ self.assertEqual(self.STATE.ndx, self.STATE_DER.ndx, "Wrong ndx value.")
+ self.assertEqual(self.STATE.nq, self.STATE_DER.nq, "Wrong nq value.")
+ self.assertEqual(self.STATE.nv, self.STATE_DER.nv, "Wrong nv value.")
+
+ # Checking the dimension of zero and random states
+ self.assertEqual(self.STATE.zero().shape, (self.NX, 1), "Wrong dimension of zero state.")
+ self.assertEqual(self.STATE.rand().shape, (self.NX, 1), "Wrong dimension of random state.")
+
+ def test_python_derived_diff(self):
+ x0 = self.STATE.rand()
+ x1 = self.STATE.rand()
+
+ # Checking that both diff functions agree
+ self.assertTrue(np.allclose(self.STATE.diff(x0, x1), self.STATE_DER.diff(x0, x1), atol=1e-9),
+ "state.diff() function doesn't agree with Python bindings.")
+
+ def test_python_derived_integrate(self):
+ x = self.STATE.rand()
+ dx = self.STATE.rand()[:self.STATE.ndx]
+
+ # Checking that both integrate functions agree
+ self.assertTrue(np.allclose(self.STATE.integrate(x, dx), self.STATE_DER.integrate(x, dx), atol=1e-9),
+ "state.integrate() function doesn't agree with Python bindings.")
+
+ def test_python_derived_Jdiff(self):
+ x0 = self.STATE.rand()
+ x1 = self.STATE.rand()
+
+ # Checking that both Jdiff functions agree
+ J1, J2 = self.STATE.Jdiff(x0, x1, "both")
+ J1d, J2d = self.STATE_DER.Jdiff(x0, x1, "both")
+ if self.STATE.__class__ == crocoddyl.libcrocoddyl_pywrap.StateMultibody:
+ nv = self.STATE.nv
+ self.assertTrue(np.allclose(J1[nv:, nv:], J1d[nv:, nv:], atol=1e-9),
+ "state.Jdiff()[0] function doesn't agree with Python bindings.")
+ self.assertTrue(np.allclose(J2[nv:, nv:], J2d[nv:, nv:], atol=1e-9),
+ "state.Jdiff()[1] function doesn't agree with Python bindings.")
+ else:
+ self.assertTrue(np.allclose(J1, J1d, atol=1e-9),
+ "state.Jdiff()[0] function doesn't agree with Python bindings.")
+ self.assertTrue(np.allclose(J2, J2d, atol=1e-9),
+ "state.Jdiff()[1] function doesn't agree with Python bindings.")
+
+ def test_python_derived_Jintegrate(self):
+ x = self.STATE.rand()
+ dx = self.STATE.rand()[:self.STATE.ndx]
+
+ # Checking that both Jintegrate functions agree
+ J1, J2 = self.STATE.Jintegrate(x, dx, "both")
+ J1d, J2d = self.STATE_DER.Jintegrate(x, dx, "both")
+ if self.STATE.__class__ == crocoddyl.libcrocoddyl_pywrap.StateMultibody:
+ nv = self.STATE.nv
+ self.assertTrue(np.allclose(J1[nv:, nv:], J1d[nv:, nv:], atol=1e-9),
+ "state.Jintegrate()[0] function doesn't agree with Python bindings.")
+ self.assertTrue(np.allclose(J2[nv:, nv:], J2d[nv:, nv:], atol=1e-9),
+ "state.Jintegrate()[1] function doesn't agree with Python bindings.")
+ else:
+ self.assertTrue(np.allclose(J1, J1d, atol=1e-9),
+ "state.Jintegrate()[0] function doesn't agree with Python bindings.")
+ self.assertTrue(np.allclose(J2, J2d, atol=1e-9),
+ "state.Jintegrate()[1] function doesn't agree with Python bindings.")
+
+
+class StateVectorTest(StateAbstractTestCase):
+ NX = randint(1, 101)
+ NDX = StateAbstractTestCase.NX
+ STATE = crocoddyl.StateVector(NX)
+ STATE_DER = StateVectorDerived(NX)
+
+
+class StateMultibodyManipulatorTest(StateAbstractTestCase):
+ MODEL = pinocchio.buildSampleModelManipulator()
+ NX = MODEL.nq + MODEL.nv
+ NDX = 2 * MODEL.nv
+ STATE = crocoddyl.StateMultibody(MODEL)
+ STATE_DER = StateMultibodyDerived(MODEL)
+
+
+class StateMultibodyHumanoidTest(StateAbstractTestCase):
+ MODEL = pinocchio.buildSampleModelHumanoidRandom()
+ NX = MODEL.nq + MODEL.nv
+ NDX = 2 * MODEL.nv
+ STATE = crocoddyl.StateMultibody(MODEL)
+ STATE_DER = StateMultibodyDerived(MODEL)
+
+
+if __name__ == '__main__':
+ test_classes_to_run = [StateVectorTest, StateMultibodyManipulatorTest, StateMultibodyHumanoidTest]
+ loader = unittest.TestLoader()
+ suites_list = []
+ for test_class in test_classes_to_run:
+ suite = loader.loadTestsFromTestCase(test_class)
+ suites_list.append(suite)
+ big_suite = unittest.TestSuite(suites_list)
+ runner = unittest.TextTestRunner()
+ results = runner.run(big_suite)
+ sys.exit(not results.wasSuccessful())
diff --git a/unittest/python/CMakeLists.txt b/unittest/python/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4686560f43281dedeeca228e6b4e1814d3b625b5
--- /dev/null
+++ b/unittest/python/CMakeLists.txt
@@ -0,0 +1,23 @@
+SET(${PROJECT_NAME}_PYTHON_TESTS
+ actions
+ activation
+ actuation
+ armature
+ boxsolvers
+ contacts
+ costs
+ ddp_contact
+ dse3
+ dynamic_derivatives
+ impact
+ locomotion
+ quadruped
+ rk4
+ robots
+ solvers
+ state
+)
+
+FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
+ ADD_PYTHON_UNIT_TEST("py-${TEST}" "unittest/python/test_${TEST}.py" ".")
+ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
diff --git a/unittest/all.py b/unittest/python/all.py
similarity index 100%
rename from unittest/all.py
rename to unittest/python/all.py
diff --git a/unittest/data/test_locomotion.xml b/unittest/python/data/test_locomotion.xml
similarity index 100%
rename from unittest/data/test_locomotion.xml
rename to unittest/python/data/test_locomotion.xml
diff --git a/unittest/sandbox/sympy_ddp_1step.py b/unittest/python/sandbox/sympy_ddp_1step.py
similarity index 100%
rename from unittest/sandbox/sympy_ddp_1step.py
rename to unittest/python/sandbox/sympy_ddp_1step.py
diff --git a/unittest/test_actions.py b/unittest/python/test_actions.py
similarity index 100%
rename from unittest/test_actions.py
rename to unittest/python/test_actions.py
diff --git a/unittest/test_activation.py b/unittest/python/test_activation.py
similarity index 100%
rename from unittest/test_activation.py
rename to unittest/python/test_activation.py
diff --git a/unittest/test_actuation.py b/unittest/python/test_actuation.py
similarity index 99%
rename from unittest/test_actuation.py
rename to unittest/python/test_actuation.py
index de26949efdfb397e5f476c3690435c44322a0fbe..c3309ca974ccb079c3026bb9d0a0a9df43335a71 100644
--- a/unittest/test_actuation.py
+++ b/unittest/python/test_actuation.py
@@ -8,7 +8,6 @@ from testutils import NUMDIFF_MODIFIER, assertNumDiff
class DifferentialActionModelActuated:
'''Unperfect class written to validate the actuation model. Do not use except for tests. '''
-
def __init__(self, pinocchioModel, actuationModel):
self.pinocchio = pinocchioModel
self.actuation = actuationModel
diff --git a/unittest/test_armature.py b/unittest/python/test_armature.py
similarity index 100%
rename from unittest/test_armature.py
rename to unittest/python/test_armature.py
diff --git a/unittest/test_boxsolvers.py b/unittest/python/test_boxsolvers.py
similarity index 100%
rename from unittest/test_boxsolvers.py
rename to unittest/python/test_boxsolvers.py
diff --git a/unittest/test_contacts.py b/unittest/python/test_contacts.py
similarity index 100%
rename from unittest/test_contacts.py
rename to unittest/python/test_contacts.py
diff --git a/unittest/test_costs.py b/unittest/python/test_costs.py
similarity index 94%
rename from unittest/test_costs.py
rename to unittest/python/test_costs.py
index f2d134798e820ddaf4d57c8069d3f2de91a7f7ba..503ffd97ffb734438d837d9d5570a56d0234943b 100644
--- a/unittest/test_costs.py
+++ b/unittest/python/test_costs.py
@@ -42,9 +42,9 @@ costModelND = CostModelNumDiff(
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[
- lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(
- x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x, u: pinocchio.computeJointJacobians(
- m, d, a2m(x[:rmodel.nq])), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
+ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])),
+ lambda m, d, x, u: pinocchio.computeJointJacobians(m, d, a2m(x[:rmodel.nq])),
+ lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
@@ -81,9 +81,9 @@ costModelND = CostModelNumDiff(
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[
- lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(
- x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x, u: pinocchio.computeJointJacobians(
- m, d, a2m(x[:rmodel.nq])), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
+ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])),
+ lambda m, d, x, u: pinocchio.computeJointJacobians(m, d, a2m(x[:rmodel.nq])),
+ lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
@@ -119,9 +119,9 @@ costModelND = CostModelNumDiff(
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[
- lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x,
- u: pinocchio.computeForwardKinematicsDerivatives(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:]), zero(rmodel.nv)
- ), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
+ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])),
+ lambda m, d, x, u: pinocchio.computeForwardKinematicsDerivatives(m, d, a2m(x[:rmodel.nq]), a2m(x[
+ rmodel.nq:]), zero(rmodel.nv)), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
@@ -157,9 +157,9 @@ costModelND = CostModelNumDiff(
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[
- lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x,
- u: pinocchio.computeForwardKinematicsDerivatives(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:]), zero(rmodel.nv)
- ), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
+ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])),
+ lambda m, d, x, u: pinocchio.computeForwardKinematicsDerivatives(m, d, a2m(x[:rmodel.nq]), a2m(x[
+ rmodel.nq:]), zero(rmodel.nv)), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
@@ -198,9 +198,9 @@ costModelND = CostModelNumDiff(
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[
- lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(
- x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x, u: pinocchio.computeJointJacobians(
- m, d, a2m(x[:rmodel.nq])), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
+ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])),
+ lambda m, d, x, u: pinocchio.computeJointJacobians(m, d, a2m(x[:rmodel.nq])),
+ lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
@@ -397,9 +397,9 @@ costModelND = CostModelNumDiff(
StatePinocchio(rmodel),
withGaussApprox=True,
reevals=[
- lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(
- x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x, u: pinocchio.computeJointJacobians(
- m, d, a2m(x[:rmodel.nq])), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
+ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])),
+ lambda m, d, x, u: pinocchio.computeJointJacobians(m, d, a2m(x[:rmodel.nq])),
+ lambda m, d, x, u: pinocchio.updateFramePlacements(m, d)
])
costDataND = costModelND.createData(rdata)
costModelND.calcDiff(costDataND, x, u)
diff --git a/unittest/test_ddp_contact.py b/unittest/python/test_ddp_contact.py
similarity index 100%
rename from unittest/test_ddp_contact.py
rename to unittest/python/test_ddp_contact.py
diff --git a/unittest/test_dse3.py b/unittest/python/test_dse3.py
similarity index 100%
rename from unittest/test_dse3.py
rename to unittest/python/test_dse3.py
diff --git a/unittest/test_dynamic_derivatives.py b/unittest/python/test_dynamic_derivatives.py
similarity index 100%
rename from unittest/test_dynamic_derivatives.py
rename to unittest/python/test_dynamic_derivatives.py
diff --git a/unittest/test_impact.py b/unittest/python/test_impact.py
similarity index 100%
rename from unittest/test_impact.py
rename to unittest/python/test_impact.py
diff --git a/unittest/test_locomotion.py b/unittest/python/test_locomotion.py
similarity index 100%
rename from unittest/test_locomotion.py
rename to unittest/python/test_locomotion.py
diff --git a/unittest/test_quadruped.py b/unittest/python/test_quadruped.py
similarity index 100%
rename from unittest/test_quadruped.py
rename to unittest/python/test_quadruped.py
diff --git a/unittest/test_rk4.py b/unittest/python/test_rk4.py
similarity index 100%
rename from unittest/test_rk4.py
rename to unittest/python/test_rk4.py
diff --git a/unittest/test_robots.py b/unittest/python/test_robots.py
similarity index 100%
rename from unittest/test_robots.py
rename to unittest/python/test_robots.py
diff --git a/unittest/test_solvers.py b/unittest/python/test_solvers.py
similarity index 99%
rename from unittest/test_solvers.py
rename to unittest/python/test_solvers.py
index 0f59365fbfa51ceaf38f6e5672df2f90df4b3ac7..706deda981aabcd6010dddcfcbb282e15a94c823 100644
--- a/unittest/test_solvers.py
+++ b/unittest/python/test_solvers.py
@@ -758,7 +758,7 @@ for t in range(T):
fddp.updateExpectedImprovement()
d1, d2 = fddp.expectedImprovement()
d1kkt, d2kkt = kkt.expectedImprovement()
-assert (abs(d1 - d1kkt) < 1e-14 and abs(d2 - d2kkt) < 1e-14)
+assert (abs(d1 - d1kkt) < 1e-12 and abs(d2 - d2kkt) < 1e-12)
# -------------------------------------------------------------------
# ------------- test expected improvement against gaps --------------
@@ -784,7 +784,7 @@ for t in range(T):
fddp.updateExpectedImprovement()
d1, d2 = fddp.expectedImprovement()
d1kkt, d2kkt = kkt.expectedImprovement()
-assert (abs(d1 - d1kkt) < 1e-14 and abs(d2 - d2kkt) < 1e-14)
+assert (abs(d1 - d1kkt) < 1e-12 and abs(d2 - d2kkt) < 1e-12)
if __name__ == '__main__':
unittest.main()
diff --git a/unittest/test_state.py b/unittest/python/test_state.py
similarity index 99%
rename from unittest/test_state.py
rename to unittest/python/test_state.py
index 1e3d422119ba2dda158d4169636ac9e25b347ba5..908be23827362df05628a6b02d2338f87e1b4dc3 100644
--- a/unittest/test_state.py
+++ b/unittest/python/test_state.py
@@ -1,6 +1,5 @@
import unittest
from random import randint
-
import numpy as np
from crocoddyl import StateNumDiff, StatePinocchio, StateUnicycle, StateVector
from testutils import NUMDIFF_MODIFIER, assertNumDiff
diff --git a/unittest/testutils.py b/unittest/python/testutils.py
similarity index 96%
rename from unittest/testutils.py
rename to unittest/python/testutils.py
index 97587073de173ab6b43fd95bd5ef3d4f42d7326e..3a73e7bc22799246456dd53b22d58824fe11f267 100644
--- a/unittest/testutils.py
+++ b/unittest/python/testutils.py
@@ -7,7 +7,7 @@ NUMDIFF_MODIFIER = 1e4
class NumDiffException(Exception):
- """Raised when the NumDiff values are too hight"""
+ """Raised when the NumDiff values are too high"""
pass
diff --git a/unittest/test_actions.cpp b/unittest/test_actions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c8f9f5b80662e823b2e7788f9c7d6c5dc1a8c2e1
--- /dev/null
+++ b/unittest/test_actions.cpp
@@ -0,0 +1,118 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#define BOOST_TEST_NO_MAIN
+#define BOOST_TEST_ALTERNATIVE_INIT_API
+#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;
+
+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();
+
+ // Generating random state and control vectors
+ Eigen::VectorXd x = model.get_state().rand();
+ Eigen::VectorXd u = Eigen::VectorXd::Random(model.get_nu());
+
+ // Getting the state dimension from calc() call
+ model.calc(data, x, u);
+
+ 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();
+ data->cost = nan("");
+
+ // Getting the cost value computed by calc()
+ Eigen::VectorXd x = model.get_state().rand();
+ Eigen::VectorXd u = Eigen::VectorXd::Random(model.get_nu());
+ model.calc(data, x, u);
+
+ // Checking that calc returns a cost value
+ 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);
+ boost::shared_ptr<crocoddyl::ActionDataAbstract> data_num_diff = model_num_diff.createData();
+
+ // Generating random values for the state and control
+ Eigen::VectorXd x = model.get_state().rand();
+ Eigen::VectorXd u = Eigen::VectorXd::Random(model.get_nu());
+
+ // Computing the action derivatives
+ model.calcDiff(data, x, u);
+ model_num_diff.calcDiff(data_num_diff, x, u);
+
+ // Checking the partial derivatives against NumDiff
+ double tol = num_diff_modifier * model_num_diff.get_disturbance();
+ BOOST_CHECK((data->Fx - data_num_diff->Fx).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((data->Fu - data_num_diff->Fu).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((data->Lx - data_num_diff->Lx).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((data->Lu - data_num_diff->Lu).isMuchSmallerThan(1.0, tol));
+ if (model_num_diff.get_with_gauss_approx()) {
+ 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));
+ }
+}
+
+//____________________________________________________________________________//
+
+void register_action_model_lqr_unit_tests() {
+ int nx = 80;
+ int nu = 40;
+ bool driftfree = true;
+ double num_diff_modifier = 1e4;
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_construct_data, crocoddyl::ActionModelLQR(nx, nu, driftfree))));
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, crocoddyl::ActionModelLQR(nx, nu, driftfree))));
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, crocoddyl::ActionModelLQR(nx, nu, driftfree))));
+ framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(
+ &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_activation.cpp b/unittest/test_activation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6820c6c19ad01da5b01cf0769b39bd4d0456c6d8
--- /dev/null
+++ b/unittest/test_activation.cpp
@@ -0,0 +1,127 @@
+/**
+ * @file test_activation.cpp
+ * @author Maximilien Naveau (maximilien.naveau@gmail.com)
+ * @license License BSD-2-Clause
+ * @copyright Copyright (c) 2019, New York University, Max Planck Gesellshaft and LAAS.
+ * @date 2019-06-18
+ */
+
+#include <boost/test/unit_test.hpp>
+// #include "crocoddyl/core/"
+
+BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
+
+BOOST_AUTO_TEST_CASE(test_activation) {}
+
+BOOST_AUTO_TEST_SUITE_END()
+
+// import numpy as np
+// from crocoddyl import ActivationModelQuad, ActivationModelSmoothAbs, ActivationModelWeightedQuad
+// from crocoddyl.utils import EPS
+// from testutils import assertNumDiff
+
+// # Comment:
+// '''
+// c = sum( a(ri) )
+// c' = sum( [a(ri)]' ) = sum( ri' a'(ri) ) = R' [ a'(ri) ]_i
+// c'' = R' [a'(ri) ]_i' = R' [a''(ri) ] R
+
+// ex
+// a(x) = x**2/x
+// a'(x) = x
+// a''(x) = 1
+
+// sum(a(ri)) = sum(ri**2/2) = .5*r'r
+// sum(ri' a'(ri)) = sum(ri' ri) = R' r
+// sum(ri' a''(ri) ri') = R' r
+// c'' = R'R
+// '''
+
+// # - ------------------------------
+// # --- Dim 1 ----------------------
+// h = np.sqrt(2 * EPS)
+
+// def df(am, ad, x):
+// return (am.calc(ad, x + h) - am.calc(ad, x)) / h
+
+// def ddf(am, ad, x):
+// return (am.calcDiff(ad, x + h)[0] - am.calcDiff(ad, x)[0]) / h
+
+// am = ActivationModelQuad()
+// ad = am.createData()
+// x = np.random.rand(1)
+
+// am.calc(ad, x)
+// assertNumDiff(df(am, ad, x),
+// am.calcDiff(ad, x)[0], 1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
+// assertNumDiff(ddf(am, ad, x),
+// am.calcDiff(ad, x)[1], 1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
+
+// am = ActivationModelWeightedQuad(np.random.rand(1))
+// ad = am.createData()
+// assertNumDiff(df(am, ad, x),
+// am.calcDiff(ad, x)[0], 1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
+// assertNumDiff(ddf(am, ad, x),
+// am.calcDiff(ad, x)[1], 1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
+
+// am = ActivationModelSmoothAbs()
+// ad = am.createData()
+// assertNumDiff(df(am, ad, x),
+// am.calcDiff(ad, x)[0], 1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
+// assertNumDiff(ddf(am, ad, x),
+// am.calcDiff(ad, x)[1], 1e-6) # threshold was 1e-6, is now 1e-6 (see assertNumDiff.__doc__)
+
+// # - ------------------------------
+// # --- Dim N ----------------------
+
+// def df(am, ad, x):
+// dx = x * 0
+// J = np.zeros([len(x), len(x)])
+// for i, _ in enumerate(x):
+// dx[i] = h
+// J[:, i] = (am.calc(ad, x + dx) - am.calc(ad, x)) / h
+// dx[i] = 0
+// return J
+
+// def ddf(am, ad, x):
+// dx = x * 0
+// J = np.zeros([len(x), len(x)])
+// for i, _ in enumerate(x):
+// dx[i] = h
+// J[:, i] = (am.calcDiff(ad, x + dx)[0] - am.calcDiff(ad, x)[0]) / h
+// dx[i] = 0
+// return J
+// return
+
+// x = np.random.rand(3)
+
+// am = ActivationModelQuad()
+// ad = am.createData()
+// J = df(am, ad, x)
+// H = ddf(am, ad, x)
+// assertNumDiff(np.diag(J.diagonal()), J, 5e-8) # threshold was 1e-9, is now 5e-8 (see assertNumDiff.__doc__)
+// assertNumDiff(np.diag(H.diagonal()), H, 5e-8) # threshold was 1e-9, is now 5e-8 (see assertNumDiff.__doc__)
+// assertNumDiff(df(am, ad, x).diagonal(),
+// am.calcDiff(ad, x)[0],
+// np.sqrt(2 * EPS)) # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
+// assertNumDiff(ddf(am, ad, x).diagonal(),
+// am.calcDiff(ad, x)[1][:, 0],
+// np.sqrt(2 * EPS)) # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
+
+// am = ActivationModelWeightedQuad(np.random.rand(len(x)))
+// ad = am.createData()
+// assertNumDiff(df(am, ad, x).diagonal(),
+// am.calcDiff(ad, x)[0],
+// np.sqrt(2 * EPS)) # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
+// assertNumDiff(ddf(am, ad, x).diagonal(),
+// am.calcDiff(ad, x)[1][:, 0],
+// np.sqrt(2 * EPS)) # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
+
+// am = ActivationModelSmoothAbs()
+// ad = am.createData()
+// assertNumDiff(df(am, ad, x).diagonal(),
+// am.calcDiff(ad, x)[0],
+// np.sqrt(2 * EPS)) # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
+// assertNumDiff(ddf(am, ad, x).diagonal(),
+// am.calcDiff(ad, x)[1][:, 0],
+// np.sqrt(2 * EPS)) # threshold was 1e-6, is now 2.11e-8 (see assertNumDiff.__doc__)
diff --git a/unittest/test_state.cpp b/unittest/test_state.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a70b680b238c008b99fdc1e182ad181b9555fb31
--- /dev/null
+++ b/unittest/test_state.cpp
@@ -0,0 +1,328 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#define BOOST_TEST_NO_MAIN
+#define BOOST_TEST_ALTERNATIVE_INIT_API
+#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>
+
+using namespace boost::unit_test;
+
+//____________________________________________________________________________//
+
+void test_state_dimension(crocoddyl::StateAbstract& state, int nx) {
+ // Checking the dimension of zero and random states
+ BOOST_CHECK(state.zero().size() == nx);
+ BOOST_CHECK(state.rand().size() == nx);
+}
+
+//____________________________________________________________________________//
+
+void test_integrate_against_difference(crocoddyl::StateAbstract& state) {
+ // Generating random states
+ 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 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) {
+ // Generating random states
+ 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);
+
+ // Checking that both states agree
+ BOOST_CHECK((dxd - dx).isMuchSmallerThan(1.0, 1e-9));
+}
+
+//____________________________________________________________________________//
+
+void test_Jdiff_firstsecond(crocoddyl::StateAbstract& state) {
+ // Generating random values for the initial and terminal states
+ 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);
+
+ // 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);
+
+ 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) {
+ // Generating random values for the initial and terminal states
+ 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);
+
+ // 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);
+
+ 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) {
+ // Generating random values for the initial and terminal states
+ Eigen::VectorXd x1 = state.rand();
+ Eigen::VectorXd x2 = state.rand();
+
+ // Get the 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());
+ 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());
+ 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) {
+ // Generating random values for the initial and terminal states
+ 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);
+
+ // 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());
+ 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());
+ 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) {
+ // Generating random values for the initial and terminal states
+ 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);
+
+ // 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());
+ state_num_diff.Jdiff(x1, x2, Jdiff_num_1, Jdiff_num_2);
+
+ // Checking the partial derivatives against NumDiff
+ // The previous tolerance was 10*disturbance
+ double tol = num_diff_modifier * state_num_diff.get_disturbance();
+ BOOST_CHECK((Jdiff_1 - Jdiff_num_1).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((Jdiff_2 - Jdiff_num_2).isMuchSmallerThan(1.0, tol));
+}
+
+//____________________________________________________________________________//
+
+void test_Jintegrate_against_numdiff(crocoddyl::StateAbstract& state, double num_diff_modifier) {
+ // 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());
+
+ // 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);
+
+ // 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());
+ state_num_diff.Jintegrate(x, dx, Jint_num_1, Jint_num_2);
+
+ // Checking the partial derivatives against NumDiff
+ // The previous tolerance was 10*disturbance
+ double tol = num_diff_modifier * state_num_diff.get_disturbance();
+ BOOST_CHECK((Jint_1 - Jint_num_1).isMuchSmallerThan(1.0, tol));
+ BOOST_CHECK((Jint_2 - Jint_num_2).isMuchSmallerThan(1.0, tol));
+}
+
+//____________________________________________________________________________//
+
+void test_Jdiff_and_Jintegrate_are_inverses(crocoddyl::StateAbstract& 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);
+
+ // 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);
+
+ // Checking that Jdiff and Jintegrate are inverses
+ Eigen::MatrixXd dX_dDX = Jdx;
+ Eigen::MatrixXd dDX_dX = J2;
+ BOOST_CHECK((dX_dDX - dDX_dX.inverse()).isMuchSmallerThan(1.0, 1e-9));
+}
+
+//____________________________________________________________________________//
+
+void test_velocity_from_Jintegrate_Jdiff(crocoddyl::StateAbstract& 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());
+ 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);
+
+ // 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);
+ BOOST_CHECK((dX_dDX * eps - x2_eps / h).isMuchSmallerThan(1.0, 1e-3));
+
+ // Checking the velocity computed from Jdiff
+ 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);
+ J1.setZero();
+ J2.setZero();
+ 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)));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_integrate_against_difference, crocoddyl::StateVector(nx))));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_difference_against_integrate, crocoddyl::StateVector(nx))));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, crocoddyl::StateVector(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_Jdiff_num_diff_firstsecond, crocoddyl::StateVector(nx))));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, crocoddyl::StateVector(nx))));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, crocoddyl::StateVector(nx), num_diff_modifier)));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_Jintegrate_against_numdiff, crocoddyl::StateVector(nx), num_diff_modifier)));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, crocoddyl::StateVector(nx))));
+
+ framework::master_test_suite().add(
+ BOOST_TEST_CASE(boost::bind(&test_velocity_from_Jintegrate_Jdiff, crocoddyl::StateVector(nx))));
+}
+
+//____________________________________________________________________________//
+
+bool init_function() {
+ // Here we test the state_vector
+ register_state_vector_unit_tests();
+ return true;
+}
+
+//____________________________________________________________________________//
+
+int main(int argc, char** argv) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }