diff --git a/.github/workflows/windows-conda-clang.yml b/.github/workflows/windows-conda-clang.yml
index 47ac1b674d..0a40d7ffb1 100644
--- a/.github/workflows/windows-conda-clang.yml
+++ b/.github/workflows/windows-conda-clang.yml
@@ -2,6 +2,9 @@ name: Build Pinocchio for Windows (CLANG) via Conda
on:
pull_request:
push:
+ branches:
+ - master
+ - devel
jobs:
build:
diff --git a/.github/workflows/windows-conda-v142.yml b/.github/workflows/windows-conda-v142.yml
index f3a2c769ed..f077544c08 100644
--- a/.github/workflows/windows-conda-v142.yml
+++ b/.github/workflows/windows-conda-v142.yml
@@ -44,6 +44,7 @@ jobs:
set Boost_ROOT=
set BOOST_ROOT_1_69_0=
set BOOST_ROOT_1_72_0=
+ set PATH=%PATH:C:\hostedtoolcache\windows\Boost\1.72.0;=%
call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64
diff --git a/.gitignore b/.gitignore
index ff84451138..a523851b7f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,4 +4,4 @@ Xcode*
*.pyc
coverage*
.travis
-.vscode*
+.vscode*
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f24462a37e..08fb0d4f5e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,6 +63,7 @@ OPTION(BUILD_ADVANCED_TESTING "Build the advanced tests (multiprecision, etc.) o
# --- OPTIONAL DEPENDENCIES -------------------------
OPTION(BUILD_WITH_URDF_SUPPORT "Build the library with the URDF format support" ON)
+OPTION(BUILD_WITH_SDF_SUPPORT "Build the library with the SDF format support" OFF)
OPTION(BUILD_WITH_COLLISION_SUPPORT "Build the library with the collision support (required HPP-FCL)" OFF)
OPTION(BUILD_WITH_AUTODIFF_SUPPORT "Build the library with the automatic differentiation support (via CppAD)" OFF)
OPTION(BUILD_WITH_CASADI_SUPPORT "Build the library with the support of CASADI" OFF)
@@ -71,6 +72,7 @@ OPTION(BUILD_WITH_OPENMP_SUPPORT "Build the library with the OpenMP support" OFF
cmake_dependent_option(LINK_PYTHON_INTERFACE_TO_OPENM "Link OpenMP to the Python interface" ON BUILD_WITH_OPENMP_SUPPORT OFF)
OPTION(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
+OPTION(CHECK_RUNTIME_MALLOC "Check if some memory allocations are performed at runtime" OFF)
IF(BUILD_WITH_COLLISION_SUPPORT)
SET(BUILD_WITH_HPP_FCL_SUPPORT TRUE)
@@ -83,10 +85,16 @@ IF(BUILD_WITH_CODEGEN_SUPPORT)
ENDIF()
IF(INITIALIZE_WITH_NAN)
- MESSAGE (STATUS "Initialize with NaN all the Eigen entries.")
+ MESSAGE(STATUS "Initialize with NaN all the Eigen entries.")
ADD_DEFINITIONS(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
ENDIF(INITIALIZE_WITH_NAN)
+IF(CHECK_RUNTIME_MALLOC)
+ MESSAGE(STATUS "Check if some memory allocations are performed at runtime.")
+ ADD_DEFINITIONS(-DPINOCCHIO_EIGEN_CHECK_MALLOC)
+ ADD_DEFINITIONS(-DEIGEN_RUNTIME_NO_MALLOC)
+ENDIF(CHECK_RUNTIME_MALLOC)
+
MACRO(TAG_LIBRARY_VERSION target)
SET_TARGET_PROPERTIES(${target} PROPERTIES SOVERSION ${PROJECT_VERSION})
ENDMACRO(TAG_LIBRARY_VERSION)
@@ -116,6 +124,17 @@ IF(BUILD_WITH_URDF_SUPPORT)
ENDIF(${urdfdom_VERSION} VERSION_GREATER "0.4.2")
ENDIF(BUILD_WITH_URDF_SUPPORT)
+IF(BUILD_WITH_SDF_SUPPORT)
+ SET(CMAKE_CXX_STANDARD 11)
+ ADD_PROJECT_DEPENDENCY(SDFormat)
+ INCLUDE_DIRECTORIES(${SDFormat_INCLUDE_DIRS})
+ LINK_DIRECTORIES(${SDFormat_LIBRARY_DIRS})
+ ADD_DEFINITIONS(-DPINOCCHIO_WITH_SDF)
+ LIST(APPEND CFLAGS_DEPENDENCIES "-DPINOCCHIO_WITH_SDF")
+ENDIF(BUILD_WITH_SDF_SUPPORT)
+
+
+
IF(BUILD_WITH_AUTODIFF_SUPPORT)
# Check first CppADCodeGen
IF(BUILD_WITH_CODEGEN_SUPPORT)
@@ -187,6 +206,8 @@ ENDIF(BUILD_WITH_HPP_FCL_SUPPORT)
ADD_DEFINITIONS(-DBOOST_MPL_LIMIT_LIST_SIZE=30)
ADD_DEFINITIONS(-DBOOST_MPL_LIMIT_VECTOR_SIZE=30)
+LIST(APPEND CFLAGS_DEPENDENCIES "-DBOOST_MPL_LIMIT_LIST_SIZE=30")
+
# Path to boost headers
INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIRS})
@@ -208,6 +229,15 @@ IF(NOT BUILD_WITH_URDF_SUPPORT)
)
ENDIF(NOT BUILD_WITH_URDF_SUPPORT)
+IF(NOT BUILD_WITH_SDF_SUPPORT)
+ LIST(REMOVE_ITEM HEADERS
+ ${PROJECT_SOURCE_DIR}/src/parsers/sdf.hpp
+ ${PROJECT_SOURCE_DIR}/src/parsers/sdf/model.hxx
+ ${PROJECT_SOURCE_DIR}/src/parsers/sdf/geometry.hxx
+ )
+ENDIF(NOT BUILD_WITH_SDF_SUPPORT)
+
+
IF(NOT BUILD_WITH_HPP_FCL_SUPPORT)
LIST(REMOVE_ITEM HEADERS
${PROJECT_SOURCE_DIR}/src/spatial/fcl-pinocchio-conversions.hpp
@@ -227,10 +257,12 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/pool")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/visitor")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/sdf")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/utils")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/serialization")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm/parallel")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm/utils")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/container")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/codegen")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/autodiff")
diff --git a/README.md b/README.md
index 18c5f4b6a2..761d6a4bc9 100644
--- a/README.md
+++ b/README.md
@@ -31,6 +31,39 @@ If you want to directly dive into **Pinocchio**, only one single line is suffici
conda install pinocchio -c conda-forge
+## Introducing Pinocchio 3
+**Pinocchio3** is released for development under the branch [pinocchio3-preview](https://github.com/stack-of-tasks/pinocchio/tree/pinocchio3-preview) on the main github repository.
+With **Pinocchio3**, multiple new features are introduced in **Pinocchio**, such as:
+ - Sparse Solution of Constrained Dynamics (Published in Robotics: Science and Systems 2021)
+ - Constrained Dynamics Derivatives (In pre-publishing stages)
+ - Constraint Models for handling loop constraints.
+ - Full casadi support in python and C++
+ - Increased support of CppAD and CppADCodeGen
+ - New SDF parser.
+ - and much more...
+
+**Pinocchio** developers are highly encouraged to check out the new features. However, please keep in mind that this remains a **development** branch, and thus the API between 2.9.x and 2.9.{x+1} could change without backward compatibility.
+
+The new constrained dynamics algorithm can be cited by the following publication:
+
+```bibtex
+@inproceedings{carpentier:hal-03271811,
+ TITLE = {{Proximal and Sparse Resolution of Constrained Dynamic Equations}},
+ AUTHOR = {Carpentier, Justin and Budhiraja, Rohan and Mansard, Nicolas},
+ URL = {https://hal.inria.fr/hal-03271811},
+ BOOKTITLE = {{Robotics: Science and Systems 2021}},
+ ADDRESS = {Austin / Virtual, United States},
+ YEAR = {2021},
+ MONTH = Jul,
+ PDF = {https://hal.inria.fr/hal-03271811/file/rss-proximal-and-sparse.pdf},
+ HAL_ID = {hal-03271811},
+ HAL_VERSION = {v1},
+}
+
+```
+
+
+
## Pinocchio main features
**Pinocchio** is fast:
diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt
index 40754f324b..8627b922eb 100644
--- a/benchmark/CMakeLists.txt
+++ b/benchmark/CMakeLists.txt
@@ -84,3 +84,8 @@ ENDIF(URDFDOM_FOUND AND hpp-fcl_FOUND)
# timings-jacobian
#
ADD_BENCH(timings-jacobian TRUE)
+
+# timings-constrained-dynamics
+#
+ADD_BENCH(timings-contact-dynamics TRUE)
+ADD_BENCH(timings-constrained-dynamics-derivatives TRUE)
diff --git a/benchmark/timings-cg.cpp b/benchmark/timings-cg.cpp
index f23635a97a..4cdedb344e 100644
--- a/benchmark/timings-cg.cpp
+++ b/benchmark/timings-cg.cpp
@@ -8,6 +8,9 @@
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/algorithm/contact-dynamics-derivatives.hpp"
+#include "pinocchio/algorithm/contact-dynamics.hpp"
+#include "pinocchio/algorithm/contact-info.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
@@ -62,6 +65,21 @@ int main(int argc, const char ** argv)
std::cout << "--" << std::endl;
pinocchio::Data data(model);
+
+ const std::string RF = "RLEG_ANKLE_R";
+ const std::string LF = "LLEG_ANKLE_R";
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
+
+ RigidConstraintModel ci_RF(CONTACT_6D,model.getFrameId(RF),WORLD);
+ RigidConstraintModel ci_LF(CONTACT_6D,model.getFrameId(LF),WORLD);
+ contact_models_6D6D.push_back(ci_RF);
+ contact_models_6D6D.push_back(ci_LF);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D6D;
+
+ RigidConstraintData cd_RF(ci_RF); contact_datas_6D6D.push_back(cd_RF);
+ RigidConstraintData cd_LF(ci_LF); contact_datas_6D6D.push_back(cd_LF);
+
VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
CodeGenRNEA rnea_code_gen(model);
@@ -87,7 +105,11 @@ int main(int argc, const char ** argv)
CodeGenABADerivatives aba_derivatives_code_gen(model);
aba_derivatives_code_gen.initLib();
aba_derivatives_code_gen.loadLib();
-
+
+ CodeGenConstraintDynamicsDerivatives constraint_dynamics_derivatives_code_gen(model, contact_models_6D6D);
+ constraint_dynamics_derivatives_code_gen.initLib();
+ constraint_dynamics_derivatives_code_gen.loadLib();
+
pinocchio::container::aligned_vector qs (NBT);
pinocchio::container::aligned_vector qdots (NBT);
pinocchio::container::aligned_vector qddots (NBT);
@@ -191,6 +213,27 @@ int main(int argc, const char ** argv)
aba_derivatives_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
}
std::cout << "ABA partial derivatives code gen = \t\t"; timer.toc(std::cout,NBT);
+
+
+ pinocchio::initConstraintDynamics(model, data, contact_models_6D6D);
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ pinocchio::constraintDynamics(model, data, qs[_smooth],qdots[_smooth],qddots[_smooth],
+ contact_models_6D6D, contact_datas_6D6D);
+ pinocchio::computeConstraintDynamicsDerivatives(model, data,
+ contact_models_6D6D, contact_datas_6D6D);
+ }
+ std::cout << "contact dynamics derivatives 6D,6D = \t\t"; timer.toc(std::cout,NBT);
+
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ constraint_dynamics_derivatives_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
+ }
+ std::cout << "contact dynamics derivatives 6D,6D code gen = \t\t"; timer.toc(std::cout,NBT);
+
return 0;
}
diff --git a/benchmark/timings-constrained-dynamics-derivatives.cpp b/benchmark/timings-constrained-dynamics-derivatives.cpp
new file mode 100644
index 0000000000..34b03e05b1
--- /dev/null
+++ b/benchmark/timings-constrained-dynamics-derivatives.cpp
@@ -0,0 +1,159 @@
+//
+// Copyright (c) 2019-2020 LAAS-CNRS INRIA
+//
+
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/constrained-dynamics.hpp"
+#include "pinocchio/algorithm/aba-derivatives.hpp"
+#include "pinocchio/algorithm/contact-dynamics.hpp"
+#include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include
+
+#include "pinocchio/utils/timer.hpp"
+
+int main(int argc, const char ** argv)
+{
+ using namespace Eigen;
+ using namespace pinocchio;
+
+ PinocchioTicToc timer(PinocchioTicToc::US);
+#ifdef NDEBUG
+ const int NBT = 1000*100;
+#else
+ const int NBT = 1;
+ std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+#endif
+
+ // Build model
+ Model model;
+
+ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
+ if(argc>1) filename = argv[1];
+ bool with_ff = true;
+
+ if(argc>2)
+ {
+ const std::string ff_option = argv[2];
+ if(ff_option == "-no-ff")
+ with_ff = false;
+ }
+
+ if( filename == "HS")
+ buildModels::humanoidRandom(model,true);
+ else
+ if(with_ff)
+ pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+ // pinocchio::urdf::buildModel(filename,JointModelRX(),model);
+ else
+ pinocchio::urdf::buildModel(filename,model);
+
+ const std::string RA = "RARM_LINK6";
+ const JointIndex RA_id = model.frames[model.getFrameId(RA)].parent;
+ const std::string LA = "LARM_LINK6";
+ const JointIndex LA_id = model.frames[model.getFrameId(LA)].parent;
+ const std::string RF = "RLEG_LINK6";
+ const JointIndex RF_id = model.frames[model.getFrameId(RF)].parent;
+ const std::string LF = "LLEG_LINK6";
+ const JointIndex LF_id = model.frames[model.getFrameId(LF)].parent;
+
+ RigidConstraintModel ci_RF_6D(CONTACT_6D,RF_id,LOCAL);
+ RigidConstraintData cd_RF_6D(ci_RF_6D);
+ //RigidConstraintModel ci_RF_3D(CONTACT_3D,model.getJointId(RF),WORLD);
+
+ RigidConstraintModel ci_LF_6D(CONTACT_6D,LF_id,LOCAL);
+ RigidConstraintData cd_LF_6D(ci_LF_6D);
+ // RigidConstraintModel ci_LF_3D(CONTACT_3D,model.getJointId(LF),WORLD);
+
+ //RigidConstraintModel ci_RA_3D(CONTACT_3D,model.getJointId(RA),WORLD);
+ //RigidConstraintModel ci_LA_3D(CONTACT_3D,model.getJointId(LA),WORLD);
+
+ // Define contact infos structure
+ const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_empty;
+
+ cholesky::ContactCholeskyDecomposition contact_chol_empty(model,contact_models_empty);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D;
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D;
+ contact_models_6D.push_back(ci_RF_6D);
+ contact_data_6D.push_back(cd_RF_6D);
+
+ cholesky::ContactCholeskyDecomposition contact_chol_6D(model,contact_models_6D);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
+ contact_models_6D6D.push_back(ci_RF_6D);
+ contact_models_6D6D.push_back(ci_LF_6D);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D6D;
+ contact_data_6D6D.push_back(cd_RF_6D);
+ contact_data_6D6D.push_back(cd_LF_6D);
+
+ cholesky::ContactCholeskyDecomposition contact_chol_6D6D(model,contact_models_6D6D);
+
+ std::cout << "nq = " << model.nq << std::endl;
+ std::cout << "nv = " << model.nv << std::endl;
+ std::cout << "--" << std::endl;
+
+ Data data(model);
+ VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
+
+ for(size_t i=0;i
+
+#include "pinocchio/utils/timer.hpp"
+
+int main(int argc, const char ** argv)
+{
+ using namespace Eigen;
+ using namespace pinocchio;
+
+ PinocchioTicToc timer(PinocchioTicToc::US);
+#ifdef NDEBUG
+ const int NBT = 1000*100;
+#else
+ const int NBT = 1;
+ std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+#endif
+
+ // Build model
+ Model model;
+
+ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
+ if(argc>1) filename = argv[1];
+ bool with_ff = true;
+
+ if(argc>2)
+ {
+ const std::string ff_option = argv[2];
+ if(ff_option == "-no-ff")
+ with_ff = false;
+ }
+
+ if( filename == "HS")
+ buildModels::humanoidRandom(model,true);
+ else
+ if(with_ff)
+ pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+ // pinocchio::urdf::buildModel(filename,JointModelRX(),model);
+ else
+ pinocchio::urdf::buildModel(filename,model);
+
+ const std::string RA = "RARM_LINK6";
+ const JointIndex RA_id = model.frames[model.getFrameId(RA)].parent;
+ const std::string LA = "LARM_LINK6";
+ const JointIndex LA_id = model.frames[model.getFrameId(LA)].parent;
+ const std::string RF = "RLEG_LINK6";
+ const JointIndex RF_id = model.frames[model.getFrameId(RF)].parent;
+ const std::string LF = "LLEG_LINK6";
+ const JointIndex LF_id = model.frames[model.getFrameId(LF)].parent;
+
+ RigidConstraintModel ci_RF_6D(CONTACT_6D,RF_id,LOCAL);
+ RigidConstraintModel ci_RF_3D(CONTACT_3D,RF_id,LOCAL);
+
+ RigidConstraintModel ci_LF_6D(CONTACT_6D,LF_id,LOCAL);
+ RigidConstraintModel ci_LF_3D(CONTACT_3D,LF_id,LOCAL);
+
+ RigidConstraintModel ci_RA_3D(CONTACT_3D,RA_id,LOCAL);
+ RigidConstraintModel ci_LA_3D(CONTACT_3D,LA_id,LOCAL);
+
+ // Define contact infos structure
+ static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
+ static PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_empty;
+ cholesky::ContactCholeskyDecomposition contact_chol_empty(model,contact_models_empty);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D;
+ contact_models_6D.push_back(ci_RF_6D);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D;
+ contact_data_6D.push_back(RigidConstraintData(ci_RF_6D));
+ cholesky::ContactCholeskyDecomposition contact_chol_6D(model,contact_models_6D);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
+ contact_models_6D6D.push_back(ci_RF_6D);
+ contact_models_6D6D.push_back(ci_LF_6D);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D6D;
+ contact_data_6D6D.push_back(RigidConstraintData(ci_RF_6D));
+ contact_data_6D6D.push_back(RigidConstraintData(ci_LF_6D));
+ cholesky::ContactCholeskyDecomposition contact_chol_6D6D(model,contact_models_6D6D);
+
+ ProximalSettings prox_settings;
+ prox_settings.max_iter = 10;
+ prox_settings.mu = 1e8;
+
+ std::cout << "nq = " << model.nq << std::endl;
+ std::cout << "nv = " << model.nv << std::endl;
+ std::cout << "--" << std::endl;
+
+ Data data(model);
+ VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
+
+ static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
+
+ for(size_t i=0;i(0));
+ total_time += timer.toc(timer.DEFAULT_UNIT);
+
+ forwardDynamics(model,data,qs[_smooth], qdots[_smooth], taus[_smooth], J, gamma);
+
+ timer.tic();
+ cholesky::decompose(model,data);
+ getKKTContactDynamicMatrixInverse(model,data,J,MJtJ_inv);
+ total_time += timer.toc(timer.DEFAULT_UNIT);
+ }
+ std::cout << "KKTContactDynamicMatrixInverse {6D} = \t\t" << (total_time/NBT)
+ << " " << timer.unitName(timer.DEFAULT_UNIT) <(0));
+ getJointJacobian(model,data,ci_LF_6D.joint1_id,ci_LF_6D.reference_frame,J.middleRows<6>(6));
+
+ forwardDynamics(model,data,qs[_smooth], qdots[_smooth], taus[_smooth], J, gamma);
+
+ timer.tic();
+ cholesky::decompose(model,data);
+ getKKTContactDynamicMatrixInverse(model,data,J,MJtJ_inv);
+ total_time += timer.toc(timer.DEFAULT_UNIT);
+ }
+ std::cout << "KKTContactDynamicMatrixInverse {6D,6D} = \t\t" << (total_time/NBT)
+ << " " << timer.unitName(timer.DEFAULT_UNIT) <(0));
+ getJointJacobian(model,data,ci_LF_6D.joint1_id,ci_LF_6D.reference_frame,J.middleRows<6>(6));
+ forwardDynamics(model,data,taus[_smooth],J,gamma);
+ }
+ std::cout << "constrainedDynamics {6D,6D} = \t\t"; timer.toc(std::cout,NBT);
+
+ std::cout << "--" << std::endl;
+
+ return 0;
+}
+
diff --git a/benchmark/timings-derivatives.cpp b/benchmark/timings-derivatives.cpp
index 8e763cfce1..7ed21bafc4 100644
--- a/benchmark/timings-derivatives.cpp
+++ b/benchmark/timings-derivatives.cpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2018-2020 CNRS INRIA
+// Copyright (c) 2018-2021 CNRS INRIA
//
#include "pinocchio/algorithm/joint-configuration.hpp"
@@ -147,6 +147,7 @@ int main(int argc, const char ** argv)
pinocchio::urdf::buildModel(filename,model);
std::cout << "nq = " << model.nq << std::endl;
std::cout << "nv = " << model.nv << std::endl;
+ std::cout << "--" << std::endl;
Data data(model);
VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
@@ -177,7 +178,7 @@ int main(int argc, const char ** argv)
{
forwardKinematics(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
}
- std::cout << "FK= \t\t"; timer.toc(std::cout,NBT);
+ std::cout << "FK= \t\t\t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
@@ -191,7 +192,7 @@ int main(int argc, const char ** argv)
{
rnea(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
}
- std::cout << "RNEA= \t\t"; timer.toc(std::cout,NBT);
+ std::cout << "RNEA= \t\t\t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
@@ -207,14 +208,14 @@ int main(int argc, const char ** argv)
rnea_fd(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth],
drnea_dq,drnea_dv,drnea_da);
}
- std::cout << "RNEA finite differences= \t\t"; timer.toc(std::cout,NBT/100);
+ std::cout << "RNEA finite differences= \t"; timer.toc(std::cout,NBT/100);
timer.tic();
SMOOTH(NBT)
{
aba(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
}
- std::cout << "ABA= \t\t"; timer.toc(std::cout,NBT);
+ std::cout << "ABA= \t\t\t\t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
@@ -222,8 +223,8 @@ int main(int argc, const char ** argv)
computeABADerivatives(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],
daba_dq,daba_dv,daba_dtau);
}
- std::cout << "ABA derivatives= \t\t"; timer.toc(std::cout,NBT);
-
+ std::cout << "ABA derivatives(q,v,tau)= \t"; timer.toc(std::cout,NBT);
+
timer.tic();
SMOOTH(NBT)
{
@@ -237,8 +238,8 @@ int main(int argc, const char ** argv)
{
computeMinverse(model,data,qs[_smooth]);
}
- std::cout << "M.inverse() from ABA = \t\t"; timer.toc(std::cout,NBT);
-
+ std::cout << "M.inverse(q) = \t\t\t"; timer.toc(std::cout,NBT);
+
MatrixXd Minv(model.nv,model.nv); Minv.setZero();
timer.tic();
SMOOTH(NBT)
diff --git a/benchmark/timings-impulse-dynamics-derivatives.cpp b/benchmark/timings-impulse-dynamics-derivatives.cpp
new file mode 100644
index 0000000000..23b40fc7d7
--- /dev/null
+++ b/benchmark/timings-impulse-dynamics-derivatives.cpp
@@ -0,0 +1,139 @@
+//
+// Copyright (c) 2020 LAAS-CNRS, INRIA
+//
+
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/impulse-dynamics.hpp"
+#include "pinocchio/algorithm/impulse-dynamics-derivatives.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include
+
+#include "pinocchio/utils/timer.hpp"
+
+int main(int argc, const char ** argv)
+{
+ using namespace Eigen;
+ using namespace pinocchio;
+
+ PinocchioTicToc timer(PinocchioTicToc::US);
+#ifdef NDEBUG
+ const int NBT = 1000*100;
+#else
+ const int NBT = 1;
+ std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+#endif
+
+ // Build model
+ Model model;
+
+ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
+ if(argc>1) filename = argv[1];
+ bool with_ff = true;
+
+ if(argc>2)
+ {
+ const std::string ff_option = argv[2];
+ if(ff_option == "-no-ff")
+ with_ff = false;
+ }
+
+ if( filename == "HS")
+ buildModels::humanoidRandom(model,true);
+ else
+ if(with_ff)
+ pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+ // pinocchio::urdf::buildModel(filename,JointModelRX(),model);
+ else
+ pinocchio::urdf::buildModel(filename,model);
+
+ const std::string RA = "RARM_LINK6";
+ const std::string LA = "LARM_LINK6";
+ const std::string RF = "RLEG_LINK6";
+ const std::string LF = "LLEG_LINK6";
+
+ RigidConstraintModel ci_RF_6D(CONTACT_6D,model.getFrameId(RF),LOCAL);
+ RigidConstraintData cd_RF_6D(ci_RF_6D);
+
+ RigidConstraintModel ci_LF_6D(CONTACT_6D,model.getFrameId(LF),LOCAL);
+ RigidConstraintData cd_LF_6D(ci_LF_6D);
+
+ // Define contact infos structure
+ const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_infos_empty;
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty;
+
+ cholesky::ContactCholeskyDecomposition contact_chol_empty(model,contact_infos_empty);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_infos_6D;
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D;
+ contact_infos_6D.push_back(ci_RF_6D); contact_datas_6D.push_back(cd_RF_6D);
+
+ cholesky::ContactCholeskyDecomposition contact_chol_6D(model,contact_infos_6D);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_infos_6D6D;
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D6D;
+ contact_infos_6D6D.push_back(ci_RF_6D); contact_datas_6D6D.push_back(cd_RF_6D);
+ contact_infos_6D6D.push_back(ci_LF_6D); contact_datas_6D6D.push_back(cd_LF_6D);
+
+ cholesky::ContactCholeskyDecomposition contact_chol_6D6D(model,contact_infos_6D6D);
+
+ std::cout << "nq = " << model.nq << std::endl;
+ std::cout << "nv = " << model.nv << std::endl;
+ std::cout << "--" << std::endl;
+
+ Data data(model);
+ VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
+
+ for(size_t i=0;i
+
+#include "pinocchio/utils/timer.hpp"
+
+int main(int argc, const char ** argv)
+{
+ using namespace Eigen;
+ using namespace pinocchio;
+
+ PinocchioTicToc timer(PinocchioTicToc::US);
+#ifdef NDEBUG
+ const int NBT = 1000*100;
+#else
+ const int NBT = 1;
+ std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+#endif
+
+ // Build model
+ Model model;
+
+ std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
+ if(argc>1) filename = argv[1];
+ bool with_ff = true;
+
+ if(argc>2)
+ {
+ const std::string ff_option = argv[2];
+ if(ff_option == "-no-ff")
+ with_ff = false;
+ }
+
+ if( filename == "HS")
+ buildModels::humanoidRandom(model,true);
+ else
+ if(with_ff)
+ pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+ // pinocchio::urdf::buildModel(filename,JointModelRX(),model);
+ else
+ pinocchio::urdf::buildModel(filename,model);
+
+ const std::string RA = "RARM_LINK6";
+ const std::string LA = "LARM_LINK6";
+ const std::string RF = "RLEG_LINK6";
+ const std::string LF = "LLEG_LINK6";
+
+ RigidConstraintModel ci_RF_6D(CONTACT_6D,model.getFrameId(RF),WORLD);
+ RigidConstraintModel ci_RF_3D(CONTACT_3D,model.getFrameId(RF),WORLD);
+
+ RigidConstraintModel ci_LF_6D(CONTACT_6D,model.getFrameId(LF),WORLD);
+ RigidConstraintModel ci_LF_3D(CONTACT_3D,model.getFrameId(LF),WORLD);
+
+ RigidConstraintModel ci_RA_3D(CONTACT_3D,model.getFrameId(RA),WORLD);
+ RigidConstraintModel ci_LA_3D(CONTACT_3D,model.getFrameId(LA),WORLD);
+
+ // Define contact infos structure
+ static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
+ static PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_empty;
+ cholesky::ContactCholeskyDecomposition contact_chol_empty(model,contact_models_empty);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D;
+ contact_models_6D.push_back(ci_RF_6D);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D;
+ contact_data_6D.push_back(RigidConstraintData(ci_RF_6D));
+ cholesky::ContactCholeskyDecomposition contact_chol_6D(model,contact_models_6D);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
+ contact_models_6D6D.push_back(ci_RF_6D);
+ contact_models_6D6D.push_back(ci_LF_6D);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D6D;
+ contact_data_6D6D.push_back(RigidConstraintData(ci_RF_6D));
+ contact_data_6D6D.push_back(RigidConstraintData(ci_LF_6D));
+ cholesky::ContactCholeskyDecomposition contact_chol_6D6D(model,contact_models_6D6D);
+
+ ProximalSettings prox_settings;
+ prox_settings.max_iter = 10;
+ prox_settings.mu = 1e8;
+
+ std::cout << "nq = " << model.nq << std::endl;
+ std::cout << "nv = " << model.nv << std::endl;
+ std::cout << "--" << std::endl;
+
+ Data data(model);
+ VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
+
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
+ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
+
+ static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
+
+ for(size_t i=0;i(0));
+ getFrameJacobian(model,data,ci_LF_6D.frame_id,ci_LF_6D.reference_frame,J.middleRows<6>(6));
+ impulseDynamics(model,data,qdots[_smooth],J,r_coeffs[(Eigen::Index)_smooth]);
+ }
+ std::cout << "constrained impulseDynamics {6D,6D} = \t\t"; timer.toc(std::cout,NBT);
+
+
+ std::cout << "--" << std::endl;
+
+ return 0;
+}
+
diff --git a/benchmark/timings.cpp b/benchmark/timings.cpp
index d0386bbc5e..9cb659a9b9 100644
--- a/benchmark/timings.cpp
+++ b/benchmark/timings.cpp
@@ -200,21 +200,23 @@ int main(int argc, const char ** argv)
else
pinocchio::urdf::buildModel(filename,model);
std::cout << "nq = " << model.nq << std::endl;
+ std::cout << "nv = " << model.nv << std::endl;
+ std::cout << "--" << std::endl;
-
-
pinocchio::Data data(model);
const VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qs (NBT);
PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qdots (NBT);
PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qddots (NBT);
+ PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) taus (NBT);
for(size_t i=0;i Mldlt(data.M);
@@ -281,43 +276,43 @@ int main(int argc, const char ** argv)
Mldlt.compute(data.M);
total += timer.toc(timer.DEFAULT_UNIT);
}
- std::cout << "Dense Eigen Cholesky = \t" << (total/NBT)
- << " " << timer.unitName(timer.DEFAULT_UNIT) <= 2.6.2")
- ADD_LIBRARY(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
IF(BUILD_WITH_OPENMP_SUPPORT)
- TARGET_COMPILE_OPTIONS(${PYWRAP} PRIVATE ${OpenMP_CXX_FLAGS})
- TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_OPENMP)
- TARGET_INCLUDE_DIRECTORIES(${PYWRAP} SYSTEM PRIVATE ${OpenMP_CXX_INCLUDE_DIR})
+ TARGET_COMPILE_OPTIONS(${PYTHON_LIB_NAME} PRIVATE ${OpenMP_CXX_FLAGS})
+ TARGET_COMPILE_DEFINITIONS(${PYTHON_LIB_NAME} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_OPENMP)
+ TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} SYSTEM PRIVATE ${OpenMP_CXX_INCLUDE_DIR})
IF(LINK_PYTHON_INTERFACE_TO_OPENM)
- TARGET_LINK_LIBRARIES(${PYWRAP} PRIVATE ${OpenMP_CXX_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PRIVATE ${OpenMP_CXX_LIBRARIES})
ENDIF(LINK_PYTHON_INTERFACE_TO_OPENM)
ENDIF(BUILD_WITH_OPENMP_SUPPORT)
- ADD_DEPENDENCIES(python ${PYWRAP})
+ ADD_DEPENDENCIES(python ${PYTHON_LIB_NAME})
- SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES PREFIX "") # Remove lib prefix for the target
-
+ SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES PREFIX "") # Remove lib prefix for the target
# Do not report:
# -Wconversion as the BOOST_PYTHON_FUNCTION_OVERLOADS implicitly converts.
# -Wcomment as latex equations have multi-line comments.
IF(NOT WIN32)
- TARGET_COMPILE_OPTIONS(${PYWRAP} PRIVATE -Wno-conversion -Wno-comment)
+ TARGET_COMPILE_OPTIONS(${PYTHON_LIB_NAME} PRIVATE -Wno-conversion -Wno-comment)
ENDIF(NOT WIN32)
+ SET(PINOCCHIO_PYTHON_CONTEXT_FILE_VALUE "pinocchio/bindings/python/context/${scalar_name}.hpp")
+ TARGET_COMPILE_OPTIONS(${PYTHON_LIB_NAME} PRIVATE -DPINOCCHIO_PYTHON_CONTEXT_FILE="${PINOCCHIO_PYTHON_CONTEXT_FILE_VALUE}" -DPINOCCHIO_PYTHON_MODULE_NAME=${PYTHON_LIB_NAME})
- SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES VERSION ${PROJECT_VERSION})
+ SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES VERSION ${PROJECT_VERSION})
IF(BUILD_WITH_COMMIT_VERSION)
- TAG_LIBRARY_VERSION(${PYWRAP})
+ TAG_LIBRARY_VERSION(${PYTHON_LIB_NAME})
ENDIF(BUILD_WITH_COMMIT_VERSION)
- ADD_HEADER_GROUP(${PYWRAP}_HEADERS)
- ADD_SOURCE_GROUP(${PYWRAP}_SOURCES)
+ ADD_HEADER_GROUP(${PYTHON_LIB_NAME}_HEADERS)
+ ADD_SOURCE_GROUP(${PYTHON_LIB_NAME}_SOURCES)
- TARGET_LINK_LIBRARIES(${PYWRAP} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy)
- TARGET_LINK_BOOST_PYTHON(${PYWRAP} PUBLIC)
+ TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy)
+ TARGET_LINK_BOOST_PYTHON(${PYTHON_LIB_NAME} PUBLIC)
IF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
- TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_HPP_FCL_PYTHON_BINDINGS)
+ TARGET_COMPILE_DEFINITIONS(${PYTHON_LIB_NAME} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_HPP_FCL_PYTHON_BINDINGS)
ENDIF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
IF(WIN32)
- TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DNOMINMAX)
- TARGET_LINK_LIBRARIES(${PYWRAP} PUBLIC ${PYTHON_LIBRARY})
+ TARGET_COMPILE_DEFINITIONS(${PYTHON_LIB_NAME} PRIVATE -DNOMINMAX)
+ TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY})
ENDIF(WIN32)
- IF(IS_ABSOLUTE ${PYTHON_SITELIB})
- SET(${PYWRAP}_INSTALL_DIR ${PYTHON_SITELIB}/${PROJECT_NAME})
- ELSE()
- SET(${PYWRAP}_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}/${PROJECT_NAME})
+ IF(NOT ${scalar_name} STREQUAL "default")
+ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/bindings/python/${PROJECT_NAME}/${scalar_name}")
+ SET(python_file "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/pinocchio/${scalar_name}/__init__.py")
+ PYTHON_BUILD(${PROJECT_NAME}/${scalar_name} "__init__.py")
+ INSTALL(FILES
+ ${python_file}
+ DESTINATION ${PINOCCHIO_PYTHON_INSTALL_DIR}/${scalar_name})
ENDIF()
- SET_TARGET_PROPERTIES(${PYWRAP}
+ SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME}
PROPERTIES
PREFIX ""
SUFFIX ${PYTHON_EXT_SUFFIX}
- OUTPUT_NAME "${PYWRAP}"
+ OUTPUT_NAME "${PYTHON_LIB_NAME}"
LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bindings/python/${PROJECT_NAME}"
)
INSTALL(
- TARGETS ${PYWRAP}
+ TARGETS ${PYTHON_LIB_NAME}
EXPORT ${TARGETS_EXPORT_NAME}
- DESTINATION ${${PYWRAP}_INSTALL_DIR}
+ DESTINATION ${PINOCCHIO_PYTHON_INSTALL_DIR}
)
+
+ENDFUNCTION()
+IF(BUILD_PYTHON_INTERFACE)
+ INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
+ ADD_CUSTOM_TARGET(python)
+ SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
+
+ SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 2.6.2")
+ IF(IS_ABSOLUTE ${PYTHON_SITELIB})
+ SET(PINOCCHIO_PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/${PROJECT_NAME})
+ ELSE()
+ SET(PINOCCHIO_PYTHON_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}/${PROJECT_NAME})
+ ENDIF()
+
+ PINOCCHIO_PYTHON_BINDINGS_SPECIFIC_TYPE(default)
+ SET(PYTHON_LIB_NAME "${PYWRAP}_default")
+
+ # IF(BUILD_WITH_AUTODIFF_SUPPORT)
+ # PINOCCHIO_PYTHON_BINDINGS_SPECIFIC_TYPE(cppad cppad)
+ # TARGET_INCLUDE_DIRECTORIES(${PYWRAP}_cppad SYSTEM PUBLIC ${cppad_INCLUDE_DIR})
+ # TARGET_LINK_LIBRARIES(${PYWRAP}_cppad PUBLIC ${cppad_LIBRARY})
+ # ENDIF(BUILD_WITH_AUTODIFF_SUPPORT)
+
+ IF(BUILD_WITH_CASADI_SUPPORT)
+ PINOCCHIO_PYTHON_BINDINGS_SPECIFIC_TYPE(casadi)
+ TARGET_LINK_LIBRARIES(${PYWRAP}_casadi PUBLIC casadi)
+ ENDIF(BUILD_WITH_CASADI_SUPPORT)
+
# --- INSTALL SCRIPTS
SET(PYTHON_FILES
__init__.py
@@ -199,12 +233,13 @@ IF(BUILD_PYTHON_INTERFACE)
explog.py
shortcuts.py
)
-
+
FOREACH(python ${PYTHON_FILES})
PYTHON_BUILD(${PROJECT_NAME} ${python})
INSTALL(FILES
"${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/pinocchio/${python}"
- DESTINATION ${${PYWRAP}_INSTALL_DIR})
+ DESTINATION ${PINOCCHIO_PYTHON_INSTALL_DIR}
+ )
ENDFOREACH(python)
# --- INSTALL VISUALIZATION SCRIPTS
@@ -220,14 +255,14 @@ IF(BUILD_PYTHON_INTERFACE)
PYTHON_BUILD(${PROJECT_NAME}/visualize ${python})
INSTALL(FILES
"${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/pinocchio/visualize/${python}"
- DESTINATION ${${PYWRAP}_INSTALL_DIR}/visualize)
+ DESTINATION ${PINOCCHIO_PYTHON_INSTALL_DIR}/visualize)
ENDFOREACH(python)
# --- PACKAGING --- #
# Format string
- SET(_PKG_CONFIG_PYWRAP_LIBDIR ${${PYWRAP}_INSTALL_DIR})
- SET(_PKG_CONFIG_PYWRAP_BINDIR ${${PYWRAP}_INSTALL_DIR})
+ SET(_PKG_CONFIG_PYWRAP_LIBDIR ${PINOCCHIO_PYTHON_INSTALL_DIR})
+ SET(_PKG_CONFIG_PYWRAP_BINDIR ${PINOCCHIO_PYTHON_INSTALL_DIR})
SET(_PKG_CONFIG_PYWRAP_CONFLICTS)
SET(_PKG_CONFIG_PYWRAP_REQUIRES "${PROJECT_NAME}")
FOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
@@ -248,11 +283,11 @@ IF(BUILD_PYTHON_INTERFACE)
ENDFOREACH(cflags ${CFLAGS_DEPENDENCIES})
CONFIGURE_FILE(
- "${CMAKE_CURRENT_SOURCE_DIR}/pinocchiopy.pc.cmake"
- "${CMAKE_CURRENT_BINARY_DIR}/pinocchiopy.pc")
+ "${CMAKE_CURRENT_SOURCE_DIR}/pinocchiopy.pc.cmake"
+ "${CMAKE_CURRENT_BINARY_DIR}/pinocchiopy.pc")
INSTALL(
- FILES "${CMAKE_CURRENT_BINARY_DIR}/pinocchiopy.pc"
- DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
+ FILES "${CMAKE_CURRENT_BINARY_DIR}/pinocchiopy.pc"
+ DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
ENDIF(BUILD_PYTHON_INTERFACE)
diff --git a/bindings/python/algorithm/algorithms.hpp b/bindings/python/algorithm/algorithms.hpp
index e7aee512db..e7a31ffcc2 100644
--- a/bindings/python/algorithm/algorithms.hpp
+++ b/bindings/python/algorithm/algorithms.hpp
@@ -23,7 +23,9 @@ namespace pinocchio
void exposeFramesAlgo();
void exposeEnergy();
void exposeKinematics();
- void exposeDynamics();
+ void exposeConstraintDynamics();
+ void exposeConstraintDynamicsDerivatives();
+ void exposeContactDynamics();
void exposeCAT();
void exposeJacobian();
void exposeGeometryAlgo();
@@ -31,13 +33,15 @@ namespace pinocchio
void exposeRegressor();
void exposeCholesky();
void exposeModelAlgo();
+ void exposeImpulseDynamics();
void exposeRNEADerivatives();
void exposeABADerivatives();
void exposeKinematicsDerivatives();
void exposeFramesDerivatives();
void exposeCentroidalDerivatives();
-
+ void exposeImpulseDynamicsDerivatives();
+
void exposeAlgorithms();
} // namespace python
diff --git a/bindings/python/algorithm/contact-cholesky.hpp b/bindings/python/algorithm/contact-cholesky.hpp
new file mode 100644
index 0000000000..c8c440ef77
--- /dev/null
+++ b/bindings/python/algorithm/contact-cholesky.hpp
@@ -0,0 +1,154 @@
+//
+// Copyright (c) 2020 INRIA
+//
+
+#ifndef __pinocchio_python_algorithm_contact_cholesky_hpp__
+#define __pinocchio_python_algorithm_contact_cholesky_hpp__
+
+#include
+#include "pinocchio/algorithm/contact-cholesky.hpp"
+
+#include "pinocchio/bindings/python/utils/macros.hpp"
+#include "pinocchio/bindings/python/utils/std-vector.hpp"
+#include "pinocchio/bindings/python/utils/comparable.hpp"
+
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::ContactCholeskyDecomposition)
+
+namespace pinocchio
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ template
+ struct ContactCholeskyDecompositionPythonVisitor
+ : public boost::python::def_visitor< ContactCholeskyDecompositionPythonVisitor >
+ {
+ typedef ContactCholeskyDecomposition Self;
+ typedef typename ContactCholeskyDecomposition::Scalar Scalar;
+ typedef typename ContactCholeskyDecomposition::RigidConstraintModel RigidConstraintModel;
+ typedef typename ContactCholeskyDecomposition::RigidConstraintData RigidConstraintData;
+ typedef typename ContactCholeskyDecomposition::Matrix Matrix;
+ typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) RigidConstraintModelVector;
+ typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) RigidConstraintDataVector;
+
+ typedef pinocchio::python::context::Model Model;
+ typedef pinocchio::python::context::Data Data;
+
+ public:
+
+ template
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def(bp::init<>(bp::arg("self"),"Default constructor."))
+ .def(bp::init(bp::args("self","model"),"Constructor from a model."))
+ .def(bp::init((bp::arg("self"),bp::arg("model"),bp::arg("contact_models")),
+ "Constructor from a model and a collection of RigidConstraintModels."))
+
+ .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self,U,"")
+ .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self,D,"")
+ .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self,Dinv,"")
+
+ .def("size", &Self::size, bp::arg("self"), "Size of the decomposition.")
+ .def("constraintDim", &Self::constraintDim, bp::arg("self"),
+ "Returns the total dimension of the constraints contained in the Cholesky factorization.")
+ .def("numContacts", &Self::numContacts, bp::arg("self"),
+ "Returns the number of contacts associated to this decomposition.")
+ .def("getConstraintSparsityPattern", &Self::getConstraintSparsityPattern,
+ bp::args("self", "constraint_id"),
+ "Returns the associated sparsity of the constraints.",
+ bp::return_value_policy())
+ .def("getLoopSparsityPattern", &Self::getLoopSparsityPattern,
+ bp::args("self", "constraint_id"),
+ "Returns the sparsity of the loop constraints (indexes that connect c1 with c2)",
+ bp::return_value_policy())
+ .def("getJoint1SparsityPattern", &Self::getJoint1SparsityPattern,
+ bp::args("self", "constraint_id"),
+ "Returns the associated sparsity introduced because of first joint",
+ bp::return_value_policy())
+ .def("getJoint2SparsityPattern", &Self::getJoint2SparsityPattern,
+ bp::args("self", "constraint_id"),
+ "Returns the associated sparsity introduces because of second joint.",
+ bp::return_value_policy())
+ .def("matrix",
+ (Matrix (Self::*)(void) const)&Self::matrix,
+ bp::arg("self"),
+ "Returns the matrix resulting from the decomposition.")
+ .def("compute",
+ (void (*)(Self & self, const Model &, Data &, const RigidConstraintModelVector &, RigidConstraintDataVector &, const Scalar))&compute,
+ bp::args("self","model","data","contact_models","contact_datas","mu"),
+ "Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix\n"
+ "related to the system mass matrix and the Jacobians of the contact patches contained in\n"
+ "the vector of RigidConstraintModel named contact_models. The decomposition is regularized with a factor mu.\n")
+
+ .def("compute",
+ (void (*)(Self & self, const Model &, Data &, const RigidConstraintModelVector &, RigidConstraintDataVector &))&compute,
+ bp::args("self","model","data","contact_models","contact_datas"),
+ "Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix\n"
+ "related to the system mass matrix and the Jacobians of the contact patches contained in\n"
+ "the vector of RigidConstraintModel named contact_models.")
+
+ .def("getInverseOperationalSpaceInertiaMatrix",
+ (Matrix (Self::*)(void) const)&Self::getInverseOperationalSpaceInertiaMatrix,
+ bp::arg("self"),
+ "Returns the Inverse of the Operational Space Inertia Matrix resulting from the decomposition.",
+ bp::return_value_policy())
+
+ .def("getOperationalSpaceInertiaMatrix",
+ (Matrix (Self::*)(void) const)&Self::getOperationalSpaceInertiaMatrix,
+ bp::arg("self"),
+ "Returns the Operational Space Inertia Matrix resulting from the decomposition.",
+ bp::return_value_policy())
+
+ .def("solve",
+ &solve,
+ bp::args("self","matrix"),
+ "Computes the solution of \f$ A x = b \f$ where self corresponds to the Cholesky decomposition of A.",
+ bp::return_value_policy())
+
+ .def("inverse",
+ (Matrix (Self::*)(void) const)&Self::inverse,
+ bp::arg("self"),
+ "Returns the inverse matrix resulting from the decomposition.")
+
+ .def("getMassMatrixChoeslkyDecomposition",
+ &Self::template getMassMatrixChoeslkyDecomposition,
+ bp::arg("self"),
+ "Retrieves the Cholesky decomposition of the Mass Matrix contained in the current decomposition.")
+
+ .def(ComparableVisitor::value>())
+ ;
+ }
+
+ static void expose()
+ {
+ bp::class_("ContactCholeskyDecomposition",
+ "Contact information container for contact dynamic algorithms.",
+ bp::no_init)
+ .def(ContactCholeskyDecompositionPythonVisitor())
+ ;
+
+ }
+
+ template
+ static Matrix solve(const Self & self, const MatrixType & mat)
+ {
+ return self.solve(mat);
+ }
+
+ static void compute(Self & self, const Model & model, Data & data, const RigidConstraintModelVector & contact_models, RigidConstraintDataVector & contact_datas, const Scalar mu)
+ {
+ self.compute(model,data,contact_models,contact_datas,mu);
+ }
+
+ static void compute(Self & self, const Model & model, Data & data, const RigidConstraintModelVector & contact_models, RigidConstraintDataVector & contact_datas)
+ {
+ self.compute(model,data,contact_models,contact_datas);
+ }
+ };
+
+ } // namespace python
+} // namespace pinocchio
+
+#endif // ifndef __pinocchio_python_algorithm_contact_cholesky_hpp__
diff --git a/bindings/python/algorithm/contact-info.hpp b/bindings/python/algorithm/contact-info.hpp
new file mode 100644
index 0000000000..a9a55b4699
--- /dev/null
+++ b/bindings/python/algorithm/contact-info.hpp
@@ -0,0 +1,208 @@
+//
+// Copyright (c) 2019-2020 INRIA
+//
+
+#ifndef __pinocchio_python_algorithm_contact_info_hpp__
+#define __pinocchio_python_algorithm_contact_info_hpp__
+
+#include
+
+#include "pinocchio/algorithm/contact-info.hpp"
+#include "pinocchio/bindings/python/utils/macros.hpp"
+#include "pinocchio/bindings/python/utils/comparable.hpp"
+
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::RigidConstraintModel)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::RigidConstraintData)
+
+namespace pinocchio
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ template
+ struct BaumgarteCorrectorParametersPythonVisitor
+ : public boost::python::def_visitor< BaumgarteCorrectorParametersPythonVisitor >
+ {
+ typedef typename BaumgarteCorrectorParameters::Scalar Scalar;
+ typedef BaumgarteCorrectorParameters Self;
+
+ public:
+
+ template
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def(bp::init<>(bp::arg("self"),
+ "Default constructor."))
+
+ .PINOCCHIO_ADD_PROPERTY(Self,Kp,"Proportional corrector value.")
+ .PINOCCHIO_ADD_PROPERTY(Self,Kd,"Damping corrector value.")
+ .def(ComparableVisitor::value>())
+ ;
+ }
+
+ static void expose()
+ {
+ bp::class_("BaumgarteCorrectorParameters",
+ "Paramaters of the Baumgarte Corrector.",
+ bp::no_init)
+ .def(BaumgarteCorrectorParametersPythonVisitor())
+ ;
+
+ }
+ };
+
+ template
+ struct RigidConstraintModelPythonVisitor
+ : public boost::python::def_visitor< RigidConstraintModelPythonVisitor >
+ {
+ typedef typename RigidConstraintModel::Scalar Scalar;
+ typedef typename RigidConstraintModel::SE3 SE3;
+ typedef RigidConstraintModel Self;
+ typedef typename RigidConstraintModel::ContactData ContactData;
+ typedef typename RigidConstraintModel::BaumgarteCorrectorParameters BaumgarteCorrectorParameters;
+
+ public:
+
+ template
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def(bp::init<>(bp::arg("self"),
+ "Default constructor."))
+ .def(bp::init >
+ ((bp::arg("self"),
+ bp::arg("contact_type"),
+ bp::arg("joint1_id"),
+ bp::arg("joint1_placement"),
+ bp::arg("joint2_id"),
+ bp::arg("joint2_placement"),
+ bp::arg("reference_frame")),
+ "Contructor from a given ContactType, joint index and placement for the two joints implied in the constraint."))
+ .def(bp::init >
+ ((bp::arg("self"),
+ bp::arg("contact_type"),
+ bp::arg("joint1_id"),
+ bp::arg("joint1_placement"),
+ bp::arg("reference_frame")),
+ "Contructor from a given ContactType, joint index and placement only for the first joint implied in the constraint."))
+ .def(bp::init >
+ ((bp::arg("self"),
+ bp::arg("contact_type"),
+ bp::arg("joint1_id"),
+ bp::arg("reference_frame")),
+ "Contructor from a given ContactType and joint index. The base joint is taken as 0 in the constraint."))
+ .PINOCCHIO_ADD_PROPERTY(Self,name,
+ "Name of the contact.")
+ .PINOCCHIO_ADD_PROPERTY(Self,type,
+ "Type of the contact.")
+ .PINOCCHIO_ADD_PROPERTY(Self,joint1_id,
+ "Index of first parent joint in the model tree.")
+ .PINOCCHIO_ADD_PROPERTY(Self,joint2_id,
+ "Index of second parent joint in the model tree.")
+ .PINOCCHIO_ADD_PROPERTY(Self,joint1_placement,
+ "Relative placement with respect to the frame of joint1.")
+ .PINOCCHIO_ADD_PROPERTY(Self,joint2_placement,
+ "Relative placement with respect to the frame of joint2.")
+ .PINOCCHIO_ADD_PROPERTY(Self,reference_frame,"Reference frame where the constraint is expressed (WORLD, LOCAL_WORLD_ALIGNED or LOCAL).")
+ .PINOCCHIO_ADD_PROPERTY(Self,desired_contact_placement,"Desired contact placement.")
+ .PINOCCHIO_ADD_PROPERTY(Self,desired_contact_velocity,"Desired contact spatial velocity.")
+ .PINOCCHIO_ADD_PROPERTY(Self,desired_contact_acceleration,"Desired contact spatial acceleration.")
+ .PINOCCHIO_ADD_PROPERTY(Self,corrector,"Corrector parameters.")
+
+ .def("size", &RigidConstraintModel::size, "Size of the contact")
+
+ .def("createData",
+ &RigidConstraintModelPythonVisitor::createData,
+ "Create a Data object for the given model.")
+
+ .def(ComparableVisitor::value>())
+ ;
+ }
+
+ static void expose()
+ {
+ bp::class_("RigidConstraintModel",
+ "Rigid contact model for contact dynamic algorithms.",
+ bp::no_init)
+ .def(RigidConstraintModelPythonVisitor())
+ ;
+
+ BaumgarteCorrectorParametersPythonVisitor::expose();
+
+ }
+
+ static ContactData createData(const Self & self)
+ {
+ return ContactData(self);
+ }
+ };
+
+ template
+ struct RigidConstraintDataPythonVisitor
+ : public boost::python::def_visitor< RigidConstraintDataPythonVisitor >
+ {
+ typedef typename RigidConstraintData::Scalar Scalar;
+ typedef RigidConstraintData Self;
+ typedef typename RigidConstraintData::ContactModel ContactModel;
+
+ public:
+
+ template
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def(bp::init(bp::args("self","contact_model"),
+ "Default constructor."))
+
+ .PINOCCHIO_ADD_PROPERTY(Self,contact_force,
+ "Constraint force.")
+ .PINOCCHIO_ADD_PROPERTY(Self,oMc1,
+ "Placement of the constraint frame 1 with respect to the WORLD frame.")
+ .PINOCCHIO_ADD_PROPERTY(Self,oMc2,
+ "Placement of the constraint frame 2 with respect to the WORLD frame.")
+ .PINOCCHIO_ADD_PROPERTY(Self,c1Mc2,
+ "Relative displacement between the two frames.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact_placement_error,
+ "Current contact placement error between the two contact Frames.\n"
+ "This corresponds to the relative placement between the two contact Frames seen as a Motion error.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact1_velocity,
+ "Current contact Spatial velocity of the constraint 1.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact2_velocity,
+ "Current contact Spatial velocity of the constraint 2.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact_velocity_error,
+ "Current contact Spatial velocity error between the two contact Frames.\n"
+ "This corresponds to the relative velocity between the two contact Frames.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact_acceleration,
+ "Current contact Spatial acceleration.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact_acceleration_desired,
+ "Desired contact acceleration.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact_acceleration_error,
+ "Current contact spatial error (due to the integration step).")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact1_acceleration_drift,
+ "Current contact drift acceleration (acceleration only due to the Coriolis and centrifugal effects) of the contact frame 1.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact2_acceleration_drift,
+ "Current contact drift acceleration (acceleration only due to the Coriolis and centrifugal effects) of the contact frame 2.")
+ .PINOCCHIO_ADD_PROPERTY(Self,contact_acceleration_deviation,
+ "Contact deviation from the reference acceleration (a.k.a the error).")
+
+ .def(ComparableVisitor::value>())
+ ;
+ }
+
+ static void expose()
+ {
+ bp::class_("RigidConstraintData",
+ "Rigid contact data associated to a RigidConstraintModel for contact dynamic algorithms.",
+ bp::no_init)
+ .def(RigidConstraintDataPythonVisitor())
+ ;
+
+ }
+ };
+
+ } // namespace python
+} // namespace pinocchio
+
+#endif // ifndef __pinocchio_python_algorithm_contact_info_hpp__
diff --git a/bindings/python/algorithm/expose-aba-derivatives.cpp b/bindings/python/algorithm/expose-aba-derivatives.cpp
index fa0b9bbb95..7cda7f314c 100644
--- a/bindings/python/algorithm/expose-aba-derivatives.cpp
+++ b/bindings/python/algorithm/expose-aba-derivatives.cpp
@@ -3,6 +3,7 @@
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/bindings/python/utils/namespace.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/bindings/python/utils/eigen.hpp"
@@ -14,27 +15,27 @@ namespace pinocchio
{
namespace bp = boost::python;
- bp::tuple computeABADerivativesDefault(const Model & model, Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & tau)
+ typedef PINOCCHIO_ALIGNED_STD_VECTOR(context::Force) ForceAlignedVector;
+
+ bp::tuple computeABADerivatives(const context::Model & model, context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & tau)
{
- computeABADerivatives(model,data,q,v,tau);
+ pinocchio::computeABADerivatives(model,data,q,v,tau);
make_symmetric(data.Minv);
return bp::make_tuple(make_ref(data.ddq_dq),
make_ref(data.ddq_dv),
make_ref(data.Minv));
}
-
- typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector;
-
- bp::tuple computeABADerivatives_fext(const Model & model, Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & tau,
+
+ bp::tuple computeABADerivatives_fext(const context::Model & model, context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & tau,
const ForceAlignedVector & fext)
{
- computeABADerivatives(model,data,q,v,tau,fext);
+ pinocchio::computeABADerivatives(model,data,q,v,tau,fext);
make_symmetric(data.Minv);
return bp::make_tuple(make_ref(data.ddq_dq),
make_ref(data.ddq_dv),
@@ -46,7 +47,7 @@ namespace pinocchio
using namespace Eigen;
bp::def("computeABADerivatives",
- computeABADerivativesDefault,
+ computeABADerivatives,
bp::args("model","data","q","v","tau"),
"Computes the ABA derivatives, store the result in data.ddq_dq, data.ddq_dv and data.Minv (aka ddq_dtau)\n"
"which correspond to the partial derivatives of the joint acceleration vector output with respect to the joint configuration,\n"
@@ -74,6 +75,7 @@ namespace pinocchio
"\ttau: the joint torque vector (size model.nv)\n"
"\tfext: list of external forces expressed in the local frame of the joints (size model.njoints)\n\n"
"Returns: (ddq_dq, ddq_dv, ddq_da)");
+
}
} // namespace python
} // namespace pinocchio
diff --git a/bindings/python/algorithm/expose-aba.cpp b/bindings/python/algorithm/expose-aba.cpp
index dd6f38be53..b640f7c649 100644
--- a/bindings/python/algorithm/expose-aba.cpp
+++ b/bindings/python/algorithm/expose-aba.cpp
@@ -3,6 +3,8 @@
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/bindings/python/utils/namespace.hpp"
+
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/bindings/python/utils/eigen.hpp"
@@ -11,45 +13,55 @@ namespace pinocchio
namespace python
{
- const Data::RowMatrixXs &
- computeMinverse_proxy(const Model & model, Data & data, const Eigen::VectorXd & q)
+ const context::Data::RowMatrixXs &
+ computeMinverse_proxy(const context::Model & model, context::Data & data, const context::VectorXs & q)
{
computeMinverse(model,data,q);
make_symmetric(data.Minv);
return data.Minv;
}
-
+
void exposeABA()
{
- using namespace Eigen;
-
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
+
bp::def("aba",
- &aba,
- bp::args("Model","Data",
- "Joint configuration q (size Model::nq)",
- "Joint velocity v (size Model::nv)",
- "Joint torque tau (size Model::nv)"),
- "Compute ABA, store the result in Data::ddq and return it.",
+ &aba,
+ bp::args("model","data","q","v","tau"),
+ "Compute ABA, store the result in data.ddq and return it.\n"
+ "Parameters:\n"
+ "\t model: Model of the kinematic tree\n"
+ "\t data: Data related to the kinematic tree\n"
+ "\t q: joint configuration (size model.nq)\n"
+ "\t tau: joint velocity (size model.nv)\n"
+ "\t v: joint torque (size model.nv)",
bp::return_value_policy());
bp::def("aba",
- &aba,
- bp::args("Model","Data",
- "Joint configuration q (size Model::nq)",
- "Joint velocity v (size Model::nv)",
- "Joint torque tau (size Model::nv)",
- "Vector of external forces expressed in the local frame of each joint (size Model::njoints)"),
- "Compute ABA with external forces, store the result in Data::ddq and return it.",
+ &aba,
+ bp::args("model","data","q","v","tau","fext"),
+ "Compute ABA with external forces, store the result in data.ddq and return it.\n"
+ "Parameters:\n"
+ "\t model: Model of the kinematic tree\n"
+ "\t data: Data related to the kinematic tree\n"
+ "\t q: joint configuration (size model.nq)\n"
+ "\t v: joint velocity (size model.nv)\n"
+ "\t tau: joint torque (size model.nv)\n"
+ "\t fext: vector of external forces expressed in the local frame of the joint (size model.njoints)",
bp::return_value_policy());
bp::def("computeMinverse",
&computeMinverse_proxy,
- bp::args("Model","Data",
- "Joint configuration q (size Model::nq)"),
- "Computes the inverse of the joint space inertia matrix using a variant of the Articulated Body algorithm.\n"
- "The result is stored in data.Minv.",
- bp::return_value_policy());
-
+ bp::args("model","data","q"),
+ "Computes the inverse of the joint space inertia matrix using an extension of the Articulated Body algorithm.\n"
+ "The result is stored in data.Minv.\n"
+ "Parameters:\n"
+ "\t model: Model of the kinematic tree\n"
+ "\t data: Data related to the kinematic tree\n"
+ "\t q: joint configuration (size model.nq)",
+ bp::return_value_policy());
}
} // namespace python
diff --git a/bindings/python/algorithm/expose-algorithms.cpp b/bindings/python/algorithm/expose-algorithms.cpp
index 652091b706..09cf3ffb6f 100644
--- a/bindings/python/algorithm/expose-algorithms.cpp
+++ b/bindings/python/algorithm/expose-algorithms.cpp
@@ -20,7 +20,10 @@ namespace pinocchio
exposeFramesAlgo();
exposeEnergy();
exposeKinematics();
- exposeDynamics();
+
+ exposeConstraintDynamics();
+ exposeConstraintDynamicsDerivatives();
+ exposeContactDynamics();
exposeCAT();
exposeJacobian();
exposeGeometryAlgo();
@@ -28,6 +31,7 @@ namespace pinocchio
exposeRegressor();
exposeCholesky();
exposeModelAlgo();
+ exposeImpulseDynamics();
// expose derivative version of the algorithms
exposeRNEADerivatives();
@@ -35,6 +39,7 @@ namespace pinocchio
exposeKinematicsDerivatives();
exposeFramesDerivatives();
exposeCentroidalDerivatives();
+ exposeImpulseDynamicsDerivatives();
}
} // namespace python
diff --git a/bindings/python/algorithm/expose-cat.cpp b/bindings/python/algorithm/expose-cat.cpp
index b4b17bc479..d8c0eb1976 100644
--- a/bindings/python/algorithm/expose-cat.cpp
+++ b/bindings/python/algorithm/expose-cat.cpp
@@ -9,10 +9,10 @@ namespace pinocchio
{
namespace python
{
- static void computeAllTerms_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v)
+ static void computeAllTerms_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v)
{
data.M.fill(0);
computeAllTerms(model,data,q,v);
diff --git a/bindings/python/algorithm/expose-centroidal-derivatives.cpp b/bindings/python/algorithm/expose-centroidal-derivatives.cpp
index e6a2ed9a19..00c4ad96eb 100644
--- a/bindings/python/algorithm/expose-centroidal-derivatives.cpp
+++ b/bindings/python/algorithm/expose-centroidal-derivatives.cpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2019 CNRS
+// Copyright (c) 2019-2020 CNRS INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
@@ -12,13 +12,13 @@ namespace pinocchio
namespace python
{
- bp::tuple computeCentroidalDynamicsDerivatives_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & a)
+ bp::tuple computeCentroidalDynamicsDerivatives_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & a)
{
- typedef Data::Matrix6x Matrix6x;
+ typedef context::Data::Matrix6x Matrix6x;
Matrix6x partialh_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
@@ -30,10 +30,10 @@ namespace pinocchio
return bp::make_tuple(partialh_dq, partial_dq,partial_dv,partial_da);
}
- bp::tuple getCentroidalDynamicsDerivatives_proxy(const Model & model,
- Data & data)
+ bp::tuple getCentroidalDynamicsDerivatives_proxy(const context::Model & model,
+ context::Data & data)
{
- typedef Data::Matrix6x Matrix6x;
+ typedef context::Data::Matrix6x Matrix6x;
Matrix6x partialh_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
@@ -50,7 +50,7 @@ namespace pinocchio
bp::def("computeCentroidalDynamicsDerivatives",
computeCentroidalDynamicsDerivatives_proxy,
- bp::args("Model","Data",
+ bp::args("model","data",
"q: configuration vector (size model.nq)",
"v: velocity vector (size model.nv)",
"a: acceleration vector (size model.nv)"),
@@ -59,7 +59,7 @@ namespace pinocchio
bp::def("getCentroidalDynamicsDerivatives",
getCentroidalDynamicsDerivatives_proxy,
- bp::args("Model","Data"),
+ bp::args("model","data"),
"Retrive the analytical derivatives of the centroidal dynamics\n"
"from the RNEA derivatives.\n"
"pinocchio.computeRNEADerivatives should have been called first.");
diff --git a/bindings/python/algorithm/expose-centroidal.cpp b/bindings/python/algorithm/expose-centroidal.cpp
index c26ea47c08..a430239389 100644
--- a/bindings/python/algorithm/expose-centroidal.cpp
+++ b/bindings/python/algorithm/expose-centroidal.cpp
@@ -12,48 +12,50 @@ namespace pinocchio
void exposeCentroidal()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
bp::def("computeCentroidalMomentum",
- &computeCentroidalMomentum,
+ &computeCentroidalMomentum,
bp::args("model","data"),
"Computes the Centroidal momentum, a.k.a. the total momentum of the system expressed around the center of mass.",
bp::return_value_policy());
bp::def("computeCentroidalMomentum",
- &computeCentroidalMomentum,
+ &computeCentroidalMomentum,
bp::args("model","data","q","v"),
"Computes the Centroidal momentum, a.k.a. the total momentum of the system expressed around the center of mass.",
bp::return_value_policy());
bp::def("computeCentroidalMomentumTimeVariation",
- &computeCentroidalMomentumTimeVariation,
+ &computeCentroidalMomentumTimeVariation,
bp::args("model","data"),
"Computes the Centroidal momentum and its time derivatives, a.k.a. the total momentum of the system and its time derivative expressed around the center of mass.",
bp::return_value_policy());
bp::def("computeCentroidalMomentumTimeVariation",
- &computeCentroidalMomentumTimeVariation,
+ &computeCentroidalMomentumTimeVariation,
bp::args("model","data","q","v","a"),
"Computes the Centroidal momentum and its time derivatives, a.k.a. the total momentum of the system and its time derivative expressed around the center of mass.",
bp::return_value_policy());
bp::def("ccrba",
- &ccrba,
+ &ccrba,
bp::args("model","data","q","v"),
"Computes the centroidal mapping, the centroidal momentum and the Centroidal Composite Rigid Body Inertia, puts the result in Data and returns the centroidal mapping."
"For the same price, it also computes the total joint jacobians (data.J).",
bp::return_value_policy());
bp::def("computeCentroidalMap",
- &computeCentroidalMap,
+ &computeCentroidalMap,
bp::args("model","data","q"),
"Computes the centroidal mapping, puts the result in Data.Ag and returns the centroidal mapping.\n"
"For the same price, it also computes the total joint jacobians (data.J).",
bp::return_value_policy());
bp::def("dccrba",
- dccrba,
+ dccrba,
bp::args("model","data","q","v"),
"Computes the time derivative of the centroidal momentum matrix Ag in terms of q and v.\n"
"For the same price, it also computes the centroidal momentum matrix (data.Ag), the total joint jacobians (data.J) "
@@ -61,7 +63,7 @@ namespace pinocchio
bp::return_value_policy());
bp::def("computeCentroidalMapTimeVariation",
- computeCentroidalMapTimeVariation,
+ computeCentroidalMapTimeVariation,
bp::args("model","data","q","v"),
"Computes the time derivative of the centroidal momentum matrix Ag, puts the result in Data.Ag and returns the centroidal mapping.\n"
"For the same price, it also computes the centroidal momentum matrix (data.Ag), the total joint jacobians (data.J) "
diff --git a/bindings/python/algorithm/expose-cholesky.cpp b/bindings/python/algorithm/expose-cholesky.cpp
index ceaef1f477..bf2720906d 100644
--- a/bindings/python/algorithm/expose-cholesky.cpp
+++ b/bindings/python/algorithm/expose-cholesky.cpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2019 CNRS INRIA
+// Copyright (c) 2019-2020 CNRS INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
@@ -17,24 +17,28 @@ namespace pinocchio
using namespace pinocchio::cholesky;
{
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
+
// using the cholesky scope
bp::scope current_scope = getOrCreatePythonNamespace("cholesky");
bp::def("decompose",
- &decompose,
+ &decompose,
bp::args("Model","Data"),
"Computes the Cholesky decomposition of the joint space inertia matrix M contained in data.\n"
"The upper triangular part of data.M should have been filled first by calling crba, or any related algorithms.",
bp::return_value_policy());
bp::def("solve",
- &solve,
+ &solve,
bp::args("Model","Data","v"),
"Returns the solution \f$x\f$ of \f$ M x = y \f$ using the Cholesky decomposition stored in data given the entry \f$ y \f$.",
bp::return_value_policy());
bp::def("computeMinv",
- &pinocchio::cholesky::computeMinv,
+ &pinocchio::cholesky::computeMinv,
bp::args("Model","Data"),
"Returns the inverse of the inverse of the joint space inertia matrix using the results of the Cholesky decomposition\n"
"performed by cholesky.decompose. The result is stored in data.Minv.",
diff --git a/bindings/python/algorithm/expose-com.cpp b/bindings/python/algorithm/expose-com.cpp
index 50c5ab0338..4dfb8d2903 100644
--- a/bindings/python/algorithm/expose-com.cpp
+++ b/bindings/python/algorithm/expose-com.cpp
@@ -17,39 +17,39 @@ namespace pinocchio
BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMassNoUpdate_overload,jacobianCenterOfMass,2,3)
- static SE3::Vector3
- com_0_proxy(const Model& model,
- Data & data,
- const Eigen::VectorXd & q,
+ static context::SE3::Vector3
+ com_0_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
bool computeSubtreeComs = true)
{
return centerOfMass(model,data,q,computeSubtreeComs);
}
- static SE3::Vector3
- com_1_proxy(const Model& model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
+ static context::SE3::Vector3
+ com_1_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
bool computeSubtreeComs = true)
{
return centerOfMass(model,data,q,v,computeSubtreeComs);
}
- static SE3::Vector3
- com_2_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & a,
+ static context::SE3::Vector3
+ com_2_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & a,
bool computeSubtreeComs = true)
{
return centerOfMass(model,data,q,v,a,computeSubtreeComs);
}
- static const Data::Vector3 &
- com_level_proxy(const Model & model,
- Data & data,
+ static const context::Data::Vector3 &
+ com_level_proxy(const context::Model & model,
+ context::Data & data,
KinematicLevel kinematic_level,
bool computeSubtreeComs = true)
{
@@ -57,51 +57,51 @@ namespace pinocchio
}
static void
- com_level_proxy_deprecated_signature(const Model & model,
- Data & data,
+ com_level_proxy_deprecated_signature(const context::Model & model,
+ context::Data & data,
int kinematic_level,
bool computeSubtreeComs = true)
{
centerOfMass(model,data,static_cast(kinematic_level),computeSubtreeComs);
}
- static const Data::Vector3 &
- com_default_proxy(const Model & model,
- Data & data,
+ static const context::Data::Vector3 &
+ com_default_proxy(const context::Model & model,
+ context::Data & data,
bool computeSubtreeComs = true)
{
return centerOfMass(model,data,computeSubtreeComs);
}
- static Data::Matrix3x
- jacobian_subtree_com_kinematics_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- Model::JointIndex jointId)
+ static context::Data::Matrix3x
+ jacobian_subtree_com_kinematics_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ context::Model::JointIndex jointId)
{
- Data::Matrix3x J(3,model.nv); J.setZero();
+ context::Data::Matrix3x J(3,model.nv); J.setZero();
jacobianSubtreeCenterOfMass(model, data, q, jointId, J);
return J;
}
- static Data::Matrix3x
- jacobian_subtree_com_proxy(const Model & model,
- Data & data,
- Model::JointIndex jointId)
+ static context::Data::Matrix3x
+ jacobian_subtree_com_proxy(const context::Model & model,
+ context::Data & data,
+ context::Model::JointIndex jointId)
{
- Data::Matrix3x J(3,model.nv); J.setZero();
+ context::Data::Matrix3x J(3,model.nv); J.setZero();
jacobianSubtreeCenterOfMass(model, data, jointId, J);
return J;
}
- static Data::Matrix3x
- get_jacobian_subtree_com_proxy(const Model & model,
- Data & data,
- Model::JointIndex jointId)
+ static context::Data::Matrix3x
+ get_jacobian_subtree_com_proxy(const context::Model & model,
+ context::Data & data,
+ context::Model::JointIndex jointId)
{
- Data::Matrix3x J(3,model.nv); J.setZero();
+ context::Data::Matrix3x J(3,model.nv); J.setZero();
getJacobianSubtreeCenterOfMass(model, data, jointId, J);
return J;
@@ -120,20 +120,22 @@ namespace pinocchio
void exposeCOM()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
bp::def("computeTotalMass",
- (double (*)(const Model &))&computeTotalMass,
+ (Scalar (*)(const context::Model &))&computeTotalMass,
bp::args("model"),
"Compute the total mass of the model and return it.");
bp::def("computeTotalMass",
- (double (*)(const Model &, Data &))&computeTotalMass,
+ (Scalar (*)(const context::Model &, context::Data &))&computeTotalMass,
bp::args("model","data"),
"Compute the total mass of the model, put it in data.mass[0] and return it.");
bp::def("computeSubtreeMasses",
- (void (*)(const Model &, Data &))&computeSubtreeMasses,
+ (void (*)(const context::Model &, context::Data &))&computeSubtreeMasses,
bp::args("model","data"),
"Compute the mass of each kinematic subtree and store it in the vector data.mass.");
@@ -142,7 +144,7 @@ namespace pinocchio
com_0_overload(bp::args("model","data",
"q",
"compute_subtree_coms"),
- "Compute the center of mass, putting the result in Data and return it."
+ "Compute the center of mass, putting the result in context::Data and return it."
"If compute_subtree_coms is True, the algorithm also computes the center of mass of the subtrees."
)[bp::return_value_policy()]
);
@@ -153,7 +155,7 @@ namespace pinocchio
bp::args("model","data",
"q","v",
"compute_subtree_coms"),
- "Computes the center of mass position and velocity by storing the result in Data. It returns the center of mass position expressed in the WORLD frame.\n"
+ "Computes the center of mass position and velocity by storing the result in context::Data. It returns the center of mass position expressed in the WORLD frame.\n"
"If compute_subtree_coms is True, the algorithm also computes the center of mass of the subtrees."
)[bp::return_value_policy()]
);
@@ -164,7 +166,7 @@ namespace pinocchio
bp::args("model","data",
"q","v","a",
"compute_subtree_coms"),
- "Computes the center of mass position, velocity and acceleration by storing the result in Data. It returns the center of mass position expressed in the WORLD frame.\n"
+ "Computes the center of mass position, velocity and acceleration by storing the result in context::Data. It returns the center of mass position expressed in the WORLD frame.\n"
"If compute_subtree_coms is True, the algorithm also computes the center of mass of the subtrees."
)[bp::return_value_policy()]
);
@@ -172,7 +174,7 @@ namespace pinocchio
bp::def("centerOfMass",
com_level_proxy_deprecated_signature,
com_level_overload_deprecated_signature(
- bp::args("Model","Data",
+ bp::args("context::Model","context::Data",
"kinematic_level",
"computeSubtreeComs If true, the algorithm computes also the center of mass of the subtrees"
),
@@ -204,21 +206,21 @@ namespace pinocchio
);
bp::def("jacobianCenterOfMass",
- (const Data::Matrix3x & (*)(const Model &, Data &, const Eigen::MatrixBase &, bool))&jacobianCenterOfMass,
+ (const context::Data::Matrix3x & (*)(const context::Model &, context::Data &, const Eigen::MatrixBase &, bool))&jacobianCenterOfMass,
jacobianCenterOfMassUpdate_overload(bp::args("model",
"data",
"q",
"compute_subtree_coms"),
- "Computes the Jacobian of the center of mass, puts the result in Data and return it.\n"
+ "Computes the Jacobian of the center of mass, puts the result in context::Data and return it.\n"
"If compute_subtree_coms is True, the algorithm also computes the center of mass of the subtrees.")[
bp::return_value_policy()]);
bp::def("jacobianCenterOfMass",
- (const Data::Matrix3x & (*)(const Model &, Data &, bool))&jacobianCenterOfMass,
+ (const context::Data::Matrix3x & (*)(const context::Model &, context::Data &, bool))&jacobianCenterOfMass,
jacobianCenterOfMassNoUpdate_overload(bp::args("model",
"data",
"compute_subtree_coms"),
- "Computes the Jacobian of the center of mass, puts the result in Data and return it.\n"
+ "Computes the Jacobian of the center of mass, puts the result in context::Data and return it.\n"
"If compute_subtree_coms is True, the algorithm also computes the center of mass of the subtrees.")[
bp::return_value_policy()]);
@@ -229,9 +231,9 @@ namespace pinocchio
"subtree_root_joint_id"),
"Computes the Jacobian of the CoM of the given subtree (subtree_root_joint_id) expressed in the WORLD frame, according to the given joint configuration.");
bp::def("jacobianSubtreeCoMJacobian",jacobian_subtree_com_kinematics_proxy,
- bp::args("Model, the model of the kinematic tree",
- "Data, the data associated to the model where the results are stored",
- "Joint configuration q (size Model::nq)",
+ bp::args("context::Model, the model of the kinematic tree",
+ "context::Data, the data associated to the model where the results are stored",
+ "Joint configuration q (size context::Model::nq)",
"Subtree root ID, the index of the subtree root joint."),
"Computes the Jacobian of the CoM of the given subtree expressed in the world frame, according to the given joint configuration.",
deprecated_function<>("This function is now deprecated. It has been renamed jacobianSubtreeCenterOfMass."));
@@ -243,8 +245,8 @@ namespace pinocchio
"Computes the Jacobian of the CoM of the given subtree (subtree_root_joint_id) expressed in the WORLD frame, according to the given entries in data.");
bp::def("jacobianSubtreeCoMJacobian",jacobian_subtree_com_proxy,
- bp::args("Model, the model of the kinematic tree",
- "Data, the data associated to the model where the results are stored",
+ bp::args("context::Model, the model of the kinematic tree",
+ "context::Data, the data associated to the model where the results are stored",
"Subtree root ID, the index of the subtree root joint."),
"Computes the Jacobian of the CoM of the given subtree expressed in the world frame, according to the given entries in data.",
deprecated_function<>("This function is now deprecated. It has been renamed jacobianSubtreeCenterOfMass."));
diff --git a/bindings/python/algorithm/expose-constrained-dynamics-derivatives.cpp b/bindings/python/algorithm/expose-constrained-dynamics-derivatives.cpp
new file mode 100644
index 0000000000..2bb5bc6546
--- /dev/null
+++ b/bindings/python/algorithm/expose-constrained-dynamics-derivatives.cpp
@@ -0,0 +1,51 @@
+//
+// Copyright (c) 2021 INRIA
+//
+
+#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp"
+#include "pinocchio/bindings/python/utils/eigen.hpp"
+
+namespace bp = boost::python;
+
+namespace pinocchio
+{
+ namespace python
+ {
+ typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintModel) RigidConstraintModelVector;
+ typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintData) RigidConstraintDataVector;
+
+ bp::tuple computeConstraintDynamicsDerivatives_proxy(const context::Model & model,
+ context::Data & data,
+ const RigidConstraintModelVector & contact_models,
+ RigidConstraintDataVector & contact_datas)
+ {
+ pinocchio::computeConstraintDynamicsDerivatives(model, data,
+ contact_models, contact_datas);
+ return bp::make_tuple(make_ref(data.ddq_dq),
+ make_ref(data.ddq_dv),
+ make_ref(data.ddq_dtau),
+ make_ref(data.dlambda_dq),
+ make_ref(data.dlambda_dv),
+ make_ref(data.dlambda_dtau));
+ }
+
+
+ void exposeConstraintDynamicsDerivatives()
+ {
+ using namespace Eigen;
+
+ typedef Eigen::aligned_allocator RigidConstraintModelAllocator;
+
+ bp::def("computeConstraintDynamicsDerivatives",
+ computeConstraintDynamicsDerivatives_proxy,bp::args("model","data",
+ "contact_models",
+ "contact_datas"),
+ "Computes the derivatives of the forward dynamics with kinematic constraints (given in the list of Contact information).\n"
+ "Assumes that constraintDynamics has been called first. See constraintDynamics for more details.\n"
+ "This function returns derivatives of joint acceleration (ddq) and contact forces (lambda_c) of the system.\n"
+ "The output is a tuple with ddq_dq, ddq_dv, ddq_da, dlambda_dq, dlambda_dv, dlambda_da");
+ }
+ }
+}
+
diff --git a/bindings/python/algorithm/expose-constrained-dynamics.cpp b/bindings/python/algorithm/expose-constrained-dynamics.cpp
new file mode 100644
index 0000000000..ac8e9c291c
--- /dev/null
+++ b/bindings/python/algorithm/expose-constrained-dynamics.cpp
@@ -0,0 +1,97 @@
+//
+// Copyright (c) 2020-2021 INRIA
+//
+
+#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/bindings/python/algorithm/contact-info.hpp"
+#include "pinocchio/bindings/python/algorithm/proximal.hpp"
+#include "pinocchio/bindings/python/algorithm/contact-cholesky.hpp"
+
+#include "pinocchio/bindings/python/utils/std-vector.hpp"
+#include "pinocchio/bindings/python/utils/registration.hpp"
+
+#include "pinocchio/algorithm/constrained-dynamics.hpp"
+
+namespace bp = boost::python;
+
+namespace pinocchio
+{
+ namespace python
+ {
+ typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintModel) RigidConstraintModelVector;
+ typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintData) RigidConstraintDataVector;
+
+ static const context::VectorXs constraintDynamics_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & tau,
+ const RigidConstraintModelVector & contact_models,
+ RigidConstraintDataVector & contact_datas,
+ context::ProximalSettings & prox_settings)
+ {
+ return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
+ }
+
+ static const context::VectorXs constraintDynamics_proxy_default(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & tau,
+ const RigidConstraintModelVector & contact_models,
+ RigidConstraintDataVector & contact_datas)
+ {
+ return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas);
+ }
+
+
+ void exposeConstraintDynamics()
+ {
+ using namespace Eigen;
+
+ // Expose type of contacts
+ if(!register_symbolic_link_to_registered_type())
+ {
+ bp::enum_("ContactType")
+ .value("CONTACT_3D",CONTACT_3D)
+ .value("CONTACT_6D",CONTACT_6D)
+ .value("CONTACT_UNDEFINED",CONTACT_UNDEFINED)
+ ;
+ }
+
+ ContactCholeskyDecompositionPythonVisitor::expose();
+
+ RigidConstraintModelPythonVisitor::expose();
+ RigidConstraintDataPythonVisitor::expose();
+
+ typedef Eigen::aligned_allocator RigidConstraintModelAllocator;
+ StdVectorPythonVisitor::expose("StdVec_RigidConstraintModel");
+
+ typedef Eigen::aligned_allocator RigidConstraintDataAllocator;
+ StdVectorPythonVisitor::expose("StdVec_RigidConstraintData");
+
+ ProximalSettingsPythonVisitor::expose();
+
+ bp::def("initConstraintDynamics",
+ &initConstraintDynamics,
+ bp::args("model","data","contact_models"),
+ "This function allows to allocate the memory before hand for contact dynamics algorithms.\n"
+ "This allows to avoid online memory allocation when running these algorithms.");
+
+ bp::def("constraintDynamics",
+ constraintDynamics_proxy,
+ bp::args("model","data","q","v","tau","contact_models","contact_datas","prox_settings"),
+ "Computes the forward dynamics with contact constraints according to a given list of Contact information.\n"
+ "When using constraintDynamics for the first time, you should call first initConstraintDynamics to initialize the internal memory used in the algorithm.\n"
+ "This function returns joint acceleration of the system. The contact forces are stored in the list data.contact_forces.");
+
+ bp::def("constraintDynamics",
+ constraintDynamics_proxy_default,
+ bp::args("model","data","q","v","tau","contact_models","contact_datas"),
+ "Computes the forward dynamics with contact constraints according to a given list of Contact information.\n"
+ "When using constraintDynamics for the first time, you should call first initConstraintDynamics to initialize the internal memory used in the algorithm.\n"
+ "This function returns joint acceleration of the system. The contact forces are stored in the list data.contact_forces.");
+ }
+ }
+}
+
diff --git a/bindings/python/algorithm/expose-contact-dynamics.cpp b/bindings/python/algorithm/expose-contact-dynamics.cpp
index 78ab6adc8a..3645a5837a 100644
--- a/bindings/python/algorithm/expose-contact-dynamics.cpp
+++ b/bindings/python/algorithm/expose-contact-dynamics.cpp
@@ -1,8 +1,10 @@
//
-// Copyright (c) 2015-2020 CNRS, INRIA
+// Copyright (c) 2015-2020 CNRS INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/bindings/python/algorithm/contact-info.hpp"
+#include "pinocchio/bindings/python/algorithm/proximal.hpp"
#include "pinocchio/algorithm/contact-dynamics.hpp"
namespace pinocchio
@@ -10,64 +12,64 @@ namespace pinocchio
namespace python
{
- static const Eigen::VectorXd forwardDynamics_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & tau,
- const Eigen::MatrixXd & J,
- const Eigen::VectorXd & gamma,
- const double inv_damping = 0.0)
+ static const context::VectorXs forwardDynamics_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & tau,
+ const context::MatrixXs & J,
+ const context::VectorXs & gamma,
+ const context::Scalar inv_damping = 0.0)
{
return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 8)
- static const Eigen::VectorXd forwardDynamics_proxy_no_q(const Model & model,
- Data & data,
- const Eigen::VectorXd & tau,
- const Eigen::MatrixXd & J,
- const Eigen::VectorXd & gamma,
- const double inv_damping = 0.0)
+ static const context::VectorXs forwardDynamics_proxy_no_q(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & tau,
+ const context::MatrixXs & J,
+ const context::VectorXs & gamma,
+ const context::Scalar inv_damping = 0.0)
{
return forwardDynamics(model, data, tau, J, gamma, inv_damping);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads_no_q, forwardDynamics_proxy_no_q, 5, 6)
- static const Eigen::VectorXd impulseDynamics_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v_before,
- const Eigen::MatrixXd & J,
- const double r_coeff = 0.,
- const double inv_damping = 0.)
+ static const context::VectorXs impulseDynamics_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v_before,
+ const context::MatrixXs & J,
+ const context::Scalar r_coeff = 0.,
+ const context::Scalar inv_damping = 0.)
{
return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads, impulseDynamics_proxy, 5, 7)
- static const Eigen::VectorXd impulseDynamics_proxy_no_q(const Model & model,
- Data & data,
- const Eigen::VectorXd & v_before,
- const Eigen::MatrixXd & J,
- const double r_coeff = 0.,
- const double inv_damping = 0.)
+ static const context::VectorXs impulseDynamics_proxy_no_q(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & v_before,
+ const context::MatrixXs & J,
+ const context::Scalar r_coeff = 0.,
+ const context::Scalar inv_damping = 0.)
{
return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads_no_q, impulseDynamics_proxy_no_q, 4, 6)
- static Eigen::MatrixXd computeKKTContactDynamicMatrixInverse_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::MatrixXd & J,
- const double mu = 0)
+ static context::MatrixXs computeKKTContactDynamicMatrixInverse_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::MatrixXs & J,
+ const context::Scalar mu = 0)
{
- Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
+ context::MatrixXs KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv, mu);
return KKTMatrix_inv;
}
@@ -75,81 +77,61 @@ namespace pinocchio
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload,
computeKKTContactDynamicMatrixInverse_proxy, 4, 5)
- static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model,
- Data & data,
- const Eigen::MatrixXd & J)
+ static const context::MatrixXs getKKTContactDynamicMatrixInverse_proxy(const context::Model & model,
+ context::Data & data,
+ const context::MatrixXs & J)
{
- Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
- getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
- return KKTMatrix_inv;
+ context::MatrixXs MJtJ_inv(model.nv+J.rows(), model.nv+J.rows());
+ getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
+ return MJtJ_inv;
}
- void exposeDynamics()
+ void exposeContactDynamics()
{
using namespace Eigen;
bp::def("forwardDynamics",
&forwardDynamics_proxy,
forwardDynamics_overloads(
- bp::args("Model","Data",
- "Joint configuration q (size Model::nq)",
- "Joint velocity v (size Model::nv)",
- "Joint torque tau (size Model::nv)",
- "Contact Jacobian J (size nb_constraint * Model::nv)",
- "Contact drift gamma (size nb_constraint)",
- "(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank."),
- "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c."
- " Internally, pinocchio.computeAllTerms is called."
+ bp::args("model","data","q","v","tau","constraint_jacobian","constraint_drift","damping"),
+ "Solves the constrained dynamics problem with contacts, puts the result in context::Data::ddq and return it. The contact forces are stored in data.lambda_c.\n"
+ "Note: internally, pinocchio.computeAllTerms is called."
));
bp::def("forwardDynamics",
&forwardDynamics_proxy_no_q,
forwardDynamics_overloads_no_q(
- bp::args("Model","Data",
- "Joint torque tau (size Model::nv)",
- "Contact Jacobian J (size nb_constraint * Model::nv)",
- "Contact drift gamma (size nb_constraint)",
- "(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank."),
- "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c."
- " Assumes pinocchio.computeAllTerms has been called."
+ bp::args("model","data","tau","constraint_jacobian","constraint_drift","damping"),
+ "Solves the forward dynamics problem with contacts, puts the result in context::Data::ddq and return it. The contact forces are stored in data.lambda_c.\n"
+ "Note: this function assumes that pinocchio.computeAllTerms has been called first."
));
bp::def("impulseDynamics",
&impulseDynamics_proxy,
impulseDynamics_overloads(
- bp::args("Model","Data",
- "Joint configuration q (size Model::nq)",
- "Joint velocity before impact v_before (size Model::nv)",
- "Contact Jacobian J (size nb_constraint * Model::nv)",
- "Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)",
- "Damping factor when J is rank deficient."
- ),
- "Solves the impact dynamics problem with contacts, store the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c."
- " Internally, pinocchio.crba is called."
+ bp::args("model","data","q","v_before","constraint_jacobian","restitution_coefficient","damping"),
+ "Solves the impact dynamics problem with contacts, store the result in context::Data::dq_after and return it. The contact impulses are stored in data.impulse_c.\n"
+ "Note: internally, pinocchio.crba is called."
));
bp::def("impulseDynamics",
&impulseDynamics_proxy_no_q,
impulseDynamics_overloads_no_q(
- bp::args("Model","Data",
- "Joint velocity before impact v_before (size Model::nv)",
- "Contact Jacobian J (size nb_constraint * Model::nv)",
- "Coefficient of restitution r_coeff (0 = rigid impact; 1 = fully elastic impact)",
- "Damping factor when J is rank deficient."),
- "Solves the impact dynamics problem with contacts, store the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c."
- " Assumes pinocchio.crba has been called."
+ bp::args("model","data","v_before","constraint_jacobian","restitution_coefficient","damping"),
+ "Solves the impact dynamics problem with contacts, store the result in context::Data::dq_after and return it. The contact impulses are stored in data.impulse_c.\n"
+ "Note: this function assumes that pinocchio.crba has been called first."
));
-
+
bp::def("computeKKTContactDynamicMatrixInverse",
computeKKTContactDynamicMatrixInverse_proxy,
- computeKKTContactDynamicMatrixInverse_overload(bp::args("model","data","q","J","damping"),
+ computeKKTContactDynamicMatrixInverse_overload(bp::args("model","data","q","constraint_jacobian","damping"),
"Computes the inverse of the constraint matrix [[M J^T], [J 0]]."));
bp::def("getKKTContactDynamicMatrixInverse",
getKKTContactDynamicMatrixInverse_proxy,
- bp::args("Model","Data",
- "Contact Jacobian J(size nb_constraint * Model::nv)"),
- "Computes the inverse of the constraint matrix [[M JT], [J 0]]. forward/impulseDynamics must be called first. The jacobian should be the same that was provided to forward/impulseDynamics.");
+ bp::args("model","data","constraint_jacobian"),
+ "Computes the inverse of the constraint matrix [[M Jt], [J 0]].\n forwardDynamics or impulseDynamics must have been called first.\n"
+ "Note: the constraint Jacobian should be the same that was provided to forwardDynamics or impulseDynamics.");
}
} // namespace python
diff --git a/bindings/python/algorithm/expose-crba.cpp b/bindings/python/algorithm/expose-crba.cpp
index a07cb4cb42..d965073895 100644
--- a/bindings/python/algorithm/expose-crba.cpp
+++ b/bindings/python/algorithm/expose-crba.cpp
@@ -3,20 +3,20 @@
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/bindings/python/utils/eigen.hpp"
#include "pinocchio/algorithm/crba.hpp"
namespace pinocchio
{
namespace python
{
- static Eigen::MatrixXd crba_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q)
+ static context::MatrixXs crba_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q)
{
data.M.fill(0);
crba(model,data,q);
- data.M.triangularView()
- = data.M.transpose().triangularView();
+ make_symmetric(data.M);
return data.M;
}
diff --git a/bindings/python/algorithm/expose-energy.cpp b/bindings/python/algorithm/expose-energy.cpp
index 94068b553f..4893383f2a 100644
--- a/bindings/python/algorithm/expose-energy.cpp
+++ b/bindings/python/algorithm/expose-energy.cpp
@@ -12,31 +12,47 @@ namespace pinocchio
void exposeEnergy()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
bp::def("computeKineticEnergy",
- &computeKineticEnergy,
+ &computeKineticEnergy,
bp::args("model","data","q","v"),
- "Computes the forward kinematics and the kinematic energy of the model for the "
+ "Computes the forward kinematics and the kinematic energy of the system for the "
"given joint configuration and velocity given as input. The result is accessible through data.kinetic_energy.");
bp::def("computeKineticEnergy",
- &computeKineticEnergy,
+ &computeKineticEnergy,
bp::args("model","data"),
- "Computes the kinematic energy of the model for the "
+ "Computes the kinematic energy of the system for the "
"given joint placement and velocity stored in data. The result is accessible through data.kinetic_energy.");
bp::def("computePotentialEnergy",
- &computePotentialEnergy,
+ &computePotentialEnergy,
bp::args("model","data","q"),
- "Computes the potential energy of the model for the "
+ "Computes the potential energy of the system for the "
"given the joint configuration given as input. The result is accessible through data.potential_energy.");
bp::def("computePotentialEnergy",
- &computePotentialEnergy,
+ &computePotentialEnergy,
bp::args("model","data"),
- "Computes the potential energy of the model for the "
+ "Computes the potential energy of the system for the "
"given joint placement stored in data. The result is accessible through data.potential_energy.");
+
+ bp::def("computeMechanicalEnergy",
+ &computeMechanicalEnergy,
+ bp::args("model","data","q","v"),
+ "Computes the forward kinematics and the kinematic energy of the system for the "
+ "given joint configuration and velocity given as input. The result is accessible through data.mechanical_energy.\n"
+ "A byproduct of this function is the computation of both data.kinetic_energy and data.potential_energy too.");
+
+ bp::def("computeMechanicalEnergy",
+ &computeMechanicalEnergy,
+ bp::args("model","data"),
+ "Computes the mechanical energy of the system for the "
+ "given joint placement and velocity stored in data. The result is accessible through data.mechanical_energy.\n"
+ "A byproduct of this function is the computation of both data.kinetic_energy and data.potential_energy too.");
}
} // namespace python
diff --git a/bindings/python/algorithm/expose-frames-derivatives.cpp b/bindings/python/algorithm/expose-frames-derivatives.cpp
index 151f93a310..2dd0f784b0 100644
--- a/bindings/python/algorithm/expose-frames-derivatives.cpp
+++ b/bindings/python/algorithm/expose-frames-derivatives.cpp
@@ -2,23 +2,23 @@
// Copyright (c) 2020 INRIA
//
+#include
+
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/frames-derivatives.hpp"
-#include
-
namespace pinocchio
{
namespace python
{
namespace bp = boost::python;
- bp::tuple getFrameVelocityDerivatives_proxy(const Model & model,
- Data & data,
- const Model::FrameIndex frame_id,
- ReferenceFrame rf)
+ bp::tuple getFrameVelocityDerivatives_proxy1(const context::Model & model,
+ context::Data & data,
+ const context::Model::FrameIndex frame_id,
+ ReferenceFrame rf)
{
- typedef Data::Matrix6x Matrix6x;
+ typedef context::Data::Matrix6x Matrix6x;
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
@@ -28,13 +28,30 @@ namespace pinocchio
return bp::make_tuple(partial_dq,partial_dv);
}
+
+ bp::tuple getFrameVelocityDerivatives_proxy2(const context::Model & model,
+ context::Data & data,
+ const context::Model::JointIndex joint_id,
+ const context::SE3 & placement,
+ ReferenceFrame rf)
+ {
+ typedef context::Data::Matrix6x Matrix6x;
+
+ Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
+ Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
+
+ getFrameVelocityDerivatives(model,data,joint_id,placement,rf,
+ partial_dq,partial_dv);
+
+ return bp::make_tuple(partial_dq,partial_dv);
+ }
- bp::tuple getFrameAccelerationDerivatives_proxy(const Model & model,
- Data & data,
- const Model::FrameIndex frame_id,
- ReferenceFrame rf)
+ bp::tuple getFrameAccelerationDerivatives_proxy1(const context::Model & model,
+ context::Data & data,
+ const context::Model::FrameIndex frame_id,
+ ReferenceFrame rf)
{
- typedef Data::Matrix6x Matrix6x;
+ typedef context::Data::Matrix6x Matrix6x;
Matrix6x v_partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x a_partial_dq(Matrix6x::Zero(6,model.nv));
@@ -48,16 +65,36 @@ namespace pinocchio
return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
}
+ bp::tuple getFrameAccelerationDerivatives_proxy2(const context::Model & model,
+ context::Data & data,
+ const context::Model::JointIndex joint_id,
+ const context::SE3 & placement,
+ ReferenceFrame rf)
+ {
+ typedef context::Data::Matrix6x Matrix6x;
+
+ Matrix6x v_partial_dq(Matrix6x::Zero(6,model.nv));
+ Matrix6x a_partial_dq(Matrix6x::Zero(6,model.nv));
+ Matrix6x a_partial_dv(Matrix6x::Zero(6,model.nv));
+ Matrix6x a_partial_da(Matrix6x::Zero(6,model.nv));
+
+ getFrameAccelerationDerivatives(model,data,joint_id,placement,rf,
+ v_partial_dq,a_partial_dq,
+ a_partial_dv,a_partial_da);
+
+ return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
+ }
+
void exposeFramesDerivatives()
{
using namespace Eigen;
bp::def("getFrameVelocityDerivatives",
- getFrameVelocityDerivatives_proxy,
+ getFrameVelocityDerivatives_proxy1,
bp::args("model","data","frame_id","reference_frame"),
"Computes the partial derivatives of the spatial velocity of a given frame with respect to\n"
"the joint configuration and velocity and returns them as a tuple.\n"
- "The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
+ "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
"You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
@@ -65,18 +102,46 @@ namespace pinocchio
"\tframe_id: index of the frame\n"
"\treference_frame: reference frame in which the resulting derivatives are expressed\n");
+ bp::def("getFrameVelocityDerivatives",
+ getFrameVelocityDerivatives_proxy2,
+ bp::args("model","data","joint_id","placement","reference_frame"),
+ "Computes the partial derivatives of the spatial velocity of a frame given by its relative placement, with respect to\n"
+ "the joint configuration and velocity and returns them as a tuple.\n"
+ "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
+ "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
+ "Parameters:\n"
+ "\tmodel: model of the kinematic tree\n"
+ "\tdata: data related to the model\n"
+ "\tjoint_id: index of the joint\n"
+ "\tplacement: placement of the Frame w.r.t. the joint frame.\n"
+ "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
+
bp::def("getFrameAccelerationDerivatives",
- getFrameAccelerationDerivatives_proxy,
+ getFrameAccelerationDerivatives_proxy1,
bp::args("model","data","frame_id","reference_frame"),
"Computes the partial derivatives of the spatial acceleration of a given frame with respect to\n"
"the joint configuration, velocity and acceleration and returns them as a tuple.\n"
- "The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
+ "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
"You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
"\tdata: data related to the model\n"
"\tframe_id: index of the frame\n"
"\treference_frame: reference frame in which the resulting derivatives are expressed\n");
+
+ bp::def("getFrameAccelerationDerivatives",
+ getFrameAccelerationDerivatives_proxy2,
+ bp::args("model","data","joint_id","placement","reference_frame"),
+ "Computes the partial derivatives of the spatial acceleration of a frame given by its relative placement, with respect to\n"
+ "the joint configuration, velocity and acceleration and returns them as a tuple.\n"
+ "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
+ "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
+ "Parameters:\n"
+ "\tmodel: model of the kinematic tree\n"
+ "\tdata: data related to the model\n"
+ "\tjoint_id: index of the joint\n"
+ "\tplacement: placement of the Frame w.r.t. the joint frame.\n"
+ "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
}
diff --git a/bindings/python/algorithm/expose-frames.cpp b/bindings/python/algorithm/expose-frames.cpp
index f7e395a839..d240183567 100644
--- a/bindings/python/algorithm/expose-frames.cpp
+++ b/bindings/python/algorithm/expose-frames.cpp
@@ -10,57 +10,123 @@ namespace pinocchio
namespace python
{
- static Data::Matrix6x get_frame_jacobian_proxy(const Model & model,
- Data & data,
- const Model::FrameIndex frame_id,
- ReferenceFrame rf)
+ static context::Data::Matrix6x get_frame_jacobian_proxy1(const context::Model & model,
+ context::Data & data,
+ const context::Data::FrameIndex frame_id,
+ ReferenceFrame rf = LOCAL)
{
- Data::Matrix6x J(6,model.nv); J.setZero();
+ context::Data::Matrix6x J(6,model.nv); J.setZero();
getFrameJacobian(model, data, frame_id, rf, J);
return J;
}
+
+ static context::Data::Matrix6x get_frame_jacobian_proxy2(const context::Model & model,
+ context::Data & data,
+ const context::Data::JointIndex joint_id,
+ const context::SE3 & placement,
+ ReferenceFrame rf = LOCAL)
+ {
+ context::Data::Matrix6x J(6,model.nv); J.setZero();
+ getFrameJacobian(model, data, joint_id, placement, rf, J);
+ return J;
+ }
+
+
+ static context::Data::Motion get_frame_velocity_proxy1(const context::Model & model,
+ context::Data & data,
+ const context::Data::FrameIndex frame_id,
+ ReferenceFrame rf = LOCAL)
+ {
+ return getFrameVelocity(model, data, frame_id, rf);
+ }
+
+ static context::Data::Motion get_frame_velocity_proxy2(const context::Model & model,
+ context::Data & data,
+ const context::Data::JointIndex joint_id,
+ const context::SE3 & placement,
+ ReferenceFrame rf = LOCAL)
+ {
+ return getFrameVelocity(model, data, joint_id, placement, rf);
+ }
+
+
+ static context::Data::Motion get_frame_acceleration_proxy1(const context::Model & model,
+ context::Data & data,
+ const context::Data::FrameIndex frame_id,
+ ReferenceFrame rf = LOCAL)
+ {
+ return getFrameAcceleration(model, data, frame_id, rf);
+ }
+
+ static context::Data::Motion get_frame_acceleration_proxy2(const context::Model & model,
+ context::Data & data,
+ const context::Data::JointIndex joint_id,
+ const context::SE3 & placement,
+ ReferenceFrame rf = LOCAL)
+ {
+ return getFrameAcceleration(model, data, joint_id, placement, rf);
+ }
+
+
+ static context::Data::Motion get_frame_classical_acceleration_proxy1(const context::Model & model,
+ context::Data & data,
+ const context::Data::FrameIndex frame_id,
+ ReferenceFrame rf = LOCAL)
+ {
+ return getFrameClassicalAcceleration(model, data, frame_id, rf);
+ }
+
+ static context::Data::Motion get_frame_classical_acceleration_proxy2(const context::Model & model,
+ context::Data & data,
+ const context::Data::JointIndex joint_id,
+ const context::SE3 & placement,
+ ReferenceFrame rf = LOCAL)
+ {
+ return getFrameClassicalAcceleration(model, data, joint_id, placement, rf);
+ }
+
- static Data::Matrix6x compute_frame_jacobian_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- Model::FrameIndex frame_id)
+ static context::Data::Matrix6x compute_frame_jacobian_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ context::Data::FrameIndex frame_id)
{
- Data::Matrix6x J(6,model.nv); J.setZero();
+ context::Data::Matrix6x J(6,model.nv); J.setZero();
computeFrameJacobian(model, data, q, frame_id, J);
return J;
}
- static Data::Matrix6x compute_frame_jacobian_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- Model::FrameIndex frame_id,
- ReferenceFrame reference_frame)
+ static context::Data::Matrix6x compute_frame_jacobian_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ context::Data::FrameIndex frame_id,
+ ReferenceFrame reference_frame)
{
- Data::Matrix6x J(6,model.nv); J.setZero();
+ context::Data::Matrix6x J(6,model.nv); J.setZero();
computeFrameJacobian(model, data, q, frame_id, reference_frame, J);
return J;
}
- static Data::Matrix6x get_frame_jacobian_time_variation_proxy(const Model & model,
- Data & data,
- Model::FrameIndex jointId,
- ReferenceFrame rf)
+ static context::Data::Matrix6x get_frame_jacobian_time_variation_proxy(const context::Model & model,
+ context::Data & data,
+ context::Data::FrameIndex jointId,
+ ReferenceFrame rf)
{
- Data::Matrix6x dJ(6,model.nv); dJ.setZero();
+ context::Data::Matrix6x dJ(6,model.nv); dJ.setZero();
getFrameJacobianTimeVariation(model,data,jointId,rf,dJ);
return dJ;
}
- static Data::Matrix6x frame_jacobian_time_variation_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Model::FrameIndex frame_id,
- const ReferenceFrame rf)
+ static context::Data::Matrix6x frame_jacobian_time_variation_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::Data::FrameIndex frame_id,
+ const ReferenceFrame rf)
{
computeJointJacobiansTimeVariation(model,data,q,v);
updateFramePlacements(model,data);
@@ -68,59 +134,90 @@ namespace pinocchio
return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
}
- BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameVelocity_overload, (getFrameVelocity), 3, 4)
- BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameAcceleration_overload, (getFrameAcceleration), 3, 4)
- BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameClassicalAcceleration_overload, (getFrameClassicalAcceleration), 3, 4)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameVelocity_overload_proxy1, get_frame_velocity_proxy1, 3, 4)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameAcceleration_overload_proxy1, get_frame_acceleration_proxy1, 3, 4)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameClassicalAcceleration_overload_proxy1, get_frame_classical_acceleration_proxy1, 3, 4)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameVelocity_overload_proxy2, get_frame_velocity_proxy2, 4, 5)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameAcceleration_overload_proxy2, get_frame_acceleration_proxy2, 4, 5)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameClassicalAcceleration_overload_proxy2, get_frame_classical_acceleration_proxy2, 4, 5)
+
+
void exposeFramesAlgo()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
bp::def("updateFramePlacements",
- &updateFramePlacements,
+ &updateFramePlacements,
bp::args("model","data"),
"Computes the placements of all the operational frames according to the current joint placement stored in data"
"and puts the results in data.");
bp::def("updateFramePlacement",
- &updateFramePlacement,
+ &updateFramePlacement,
bp::args("model","data","frame_id"),
"Computes the placement of the given operational frame (frame_id) according to the current joint placement stored in data, stores the results in data and returns it.",
bp::return_value_policy());
bp::def("getFrameVelocity",
- &getFrameVelocity,
- getFrameVelocity_overload(
+ &get_frame_velocity_proxy1,
+ getFrameVelocity_overload_proxy1(
bp::args("model","data","frame_id","reference_frame"),
"Returns the spatial velocity of the frame expressed in the coordinate system given by reference_frame.\n"
"forwardKinematics(model,data,q,v[,a]) should be called first to compute the joint spatial velocity stored in data.v"));
+ bp::def("getFrameVelocity",
+ &get_frame_velocity_proxy2,
+ getFrameVelocity_overload_proxy2(
+ bp::args("model","data","joint_id","placement","reference_frame"),
+ "Returns the spatial velocity of the frame expressed in the coordinate system given by reference_frame.\n"
+ "forwardKinematics(model,data,q,v[,a]) should be called first to compute the joint spatial velocity stored in data.v"));
+
+
bp::def("getFrameAcceleration",
- &getFrameAcceleration,
- getFrameAcceleration_overload(
+ &get_frame_acceleration_proxy1,
+ getFrameAcceleration_overload_proxy1(
bp::args("model","data","frame_id","reference_frame"),
"Returns the spatial acceleration of the frame expressed in the coordinate system given by reference_frame.\n"
"forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
+ bp::def("getFrameAcceleration",
+ &get_frame_acceleration_proxy2,
+ getFrameAcceleration_overload_proxy2(
+ bp::args("model","data","joint_id","placement","reference_frame"),
+ "Returns the spatial acceleration of the frame expressed in the coordinate system given by reference_frame.\n"
+ "forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
+
+
bp::def("getFrameClassicalAcceleration",
- &getFrameClassicalAcceleration,
- getFrameClassicalAcceleration_overload(
+ &get_frame_classical_acceleration_proxy1,
+ getFrameClassicalAcceleration_overload_proxy1(
bp::args("model","data","frame_id","reference_frame"),
"Returns the \"classical\" acceleration of the frame expressed in the coordinate system given by reference_frame.\n"
"forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
+ bp::def("getFrameClassicalAcceleration",
+ &get_frame_classical_acceleration_proxy2,
+ getFrameClassicalAcceleration_overload_proxy2(
+ bp::args("model","data","frame_id","reference_frame"),
+ "Returns the \"classical\" acceleration of the frame expressed in the coordinate system given by reference_frame.\n"
+ "forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
+
+
bp::def("framesForwardKinematics",
- &framesForwardKinematics,
+ &framesForwardKinematics,
bp::args("model","data","q"),
"Calls first the forwardKinematics(model,data,q) and then update the Frame placement quantities (data.oMf).");
bp::def("computeFrameJacobian",
- (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &, Model::FrameIndex, ReferenceFrame))&compute_frame_jacobian_proxy,
+ (context::Data::Matrix6x (*)(const context::Model &, context::Data &, const context::VectorXs &, context::Data::FrameIndex, ReferenceFrame))&compute_frame_jacobian_proxy,
bp::args("model","data","q","frame_id","reference_frame"),
"Computes the Jacobian of the frame given by its frame_id in the coordinate system given by reference_frame.\n");
bp::def("computeFrameJacobian",
- (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &, Model::FrameIndex))&compute_frame_jacobian_proxy,
+ (context::Data::Matrix6x (*)(const context::Model &, context::Data &, const context::VectorXs &, context::Data::FrameIndex))&compute_frame_jacobian_proxy,
bp::args("model","data","q","frame_id"),
"Computes the Jacobian of the frame given by its frame_id.\n"
"The columns of the Jacobian are expressed in the coordinates system of the Frame itself.\n"
@@ -128,13 +225,20 @@ namespace pinocchio
"where v is the joint velocity.");
bp::def("getFrameJacobian",
- &get_frame_jacobian_proxy,
+ &get_frame_jacobian_proxy1,
bp::args("model","data","frame_id","reference_frame"),
- "Computes the Jacobian of the frame given by its ID either in the local or the world frames.\n"
- "The columns of the Jacobian are expressed in the LOCAL frame coordinates system.\n"
- "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
- "where v is the joint velocity.\n"
- "computeJointJacobians(model,data,q) and updateFramePlacements(model,data) must have been called first.");
+ "Computes the Jacobian of the frame given by its ID either in the LOCAL, LOCAL_WORLD_ALIGNED or the WORLD coordinates systems.\n"
+ "In other words, the velocity of the frame vF expressed in the reference frame is given by J*v,"
+ "where v is the joint velocity vector.\n"
+ "remarks: computeJointJacobians(model,data,q) must have been called first.");
+
+ bp::def("getFrameJacobian",
+ &get_frame_jacobian_proxy2,
+ bp::args("model","data","joint_id","placement","reference_frame"),
+ "Computes the Jacobian of the frame given by its placement with respect to the Joint frame and expressed the solution either in the LOCAL, LOCAL_WORLD_ALIGNED or the WORLD coordinates systems.\n"
+ "In other words, the velocity of the frame vF expressed in the reference frame is given by J*v,"
+ "where v is the joint velocity vector.\n\n"
+ "remarks: computeJointJacobians(model,data,q) must have been called first.");
bp::def("frameJacobianTimeVariation",&frame_jacobian_time_variation_proxy,
bp::args("model","data","q","v","frame_id","reference_frame"),
diff --git a/bindings/python/algorithm/expose-impulse-dynamics-derivatives.cpp b/bindings/python/algorithm/expose-impulse-dynamics-derivatives.cpp
new file mode 100644
index 0000000000..271fca8025
--- /dev/null
+++ b/bindings/python/algorithm/expose-impulse-dynamics-derivatives.cpp
@@ -0,0 +1,50 @@
+//
+// Copyright (c) 2020 CNRS INRIA
+//
+
+#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/bindings/python/algorithm/contact-info.hpp"
+#include "pinocchio/bindings/python/algorithm/proximal.hpp"
+#include "pinocchio/bindings/python/algorithm/contact-cholesky.hpp"
+
+#include "pinocchio/bindings/python/utils/std-vector.hpp"
+
+#include "pinocchio/algorithm/impulse-dynamics-derivatives.hpp"
+
+namespace bp = boost::python;
+
+namespace pinocchio
+{
+ namespace python
+ {
+
+ typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintModel) RigidConstraintModelVector;
+ typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintData) RigidConstraintDataVector;
+
+ static void impulseDynamicsDerivatives_proxy(const context::Model & model,
+ context::Data & data,
+ const RigidConstraintModelVector & contact_models,
+ RigidConstraintDataVector & contact_datas,
+ const context::Scalar & r_coeff =0.0,
+ const context::Scalar & mu = 0.0)
+ {
+ computeImpulseDynamicsDerivatives(model, data, contact_models,
+ contact_datas, r_coeff, mu);
+ return;
+ }
+
+ BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamicsDerivatives_overloads,
+ impulseDynamicsDerivatives_proxy, 4, 6)
+
+
+ void exposeImpulseDynamicsDerivatives()
+ {
+ bp::def("computeImpulseDynamicsDerivatives",
+ impulseDynamicsDerivatives_proxy,
+ impulseDynamicsDerivatives_overloads(bp::args("model","data","contact_models","contact_datas","r_coeff","mu"),
+ "Computes the impulse dynamics derivatives with contact constraints according to a given list of Contact information.\n"
+ "impulseDynamics should have been called before."));
+ }
+ }
+}
+
diff --git a/bindings/python/algorithm/expose-impulse-dynamics.cpp b/bindings/python/algorithm/expose-impulse-dynamics.cpp
new file mode 100644
index 0000000000..42d07dc4f8
--- /dev/null
+++ b/bindings/python/algorithm/expose-impulse-dynamics.cpp
@@ -0,0 +1,50 @@
+//
+// Copyright (c) 2020 CNRS INRIA
+//
+
+#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/bindings/python/algorithm/contact-info.hpp"
+#include "pinocchio/bindings/python/algorithm/proximal.hpp"
+#include "pinocchio/bindings/python/algorithm/contact-cholesky.hpp"
+
+#include "pinocchio/bindings/python/utils/std-vector.hpp"
+
+#include "pinocchio/algorithm/impulse-dynamics.hpp"
+
+namespace bp = boost::python;
+
+namespace pinocchio
+{
+ namespace python
+ {
+
+ typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintModel) RigidConstraintModelVector;
+ typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RigidConstraintData) RigidConstraintDataVector;
+
+ static const context::VectorXs impulseDynamics_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const RigidConstraintModelVector & contact_models,
+ RigidConstraintDataVector & contact_datas,
+ const context::Scalar & r_coeff =0.0,
+ const context::Scalar & mu = 0.0)
+ {
+ return impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, mu);
+ }
+
+ BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads, impulseDynamics_proxy, 6, 8)
+
+
+ void exposeImpulseDynamics()
+ {
+ bp::def("impulseDynamics",
+ impulseDynamics_proxy,
+ impulseDynamics_overloads(bp::args("model","data","q","v","contact_models","contact_datas","r_coeff","mu"),
+ "Computes the impulse dynamics with contact constraints according to a given list of Contact information.\n"
+ "When using impulseDynamics for the first time, you should call first initConstraintDynamics to initialize the internal memory used in the algorithm.\n"
+ "This function returns the after-impulse velocity of the system. The impulses acting on the contacts are stored in the list data.contact_forces."));
+ }
+ }
+}
+
diff --git a/bindings/python/algorithm/expose-jacobian.cpp b/bindings/python/algorithm/expose-jacobian.cpp
index a47ed4e43a..b86a3c0fd4 100644
--- a/bindings/python/algorithm/expose-jacobian.cpp
+++ b/bindings/python/algorithm/expose-jacobian.cpp
@@ -10,37 +10,37 @@ namespace pinocchio
namespace python
{
- static Data::Matrix6x
- compute_jacobian_proxy(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- Model::JointIndex jointId)
+ static context::Data::Matrix6x
+ compute_jacobian_proxy(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ JointIndex jointId)
{
- Data::Matrix6x J(6,model.nv); J.setZero();
+ context::Data::Matrix6x J(6,model.nv); J.setZero();
computeJointJacobian(model,data,q,jointId,J);
return J;
}
- static Data::Matrix6x
- get_jacobian_proxy(const Model & model,
- Data & data,
- Model::JointIndex jointId,
+ static context::Data::Matrix6x
+ get_jacobian_proxy(const context::Model & model,
+ context::Data & data,
+ JointIndex jointId,
ReferenceFrame rf)
{
- Data::Matrix6x J(6,model.nv); J.setZero();
+ context::Data::Matrix6x J(6,model.nv); J.setZero();
getJointJacobian(model,data,jointId,rf,J);
return J;
}
- static Data::Matrix6x
- get_jacobian_time_variation_proxy(const Model & model,
- Data & data,
- Model::JointIndex jointId,
+ static context::Data::Matrix6x
+ get_jacobian_time_variation_proxy(const context::Model & model,
+ context::Data & data,
+ JointIndex jointId,
ReferenceFrame rf)
{
- Data::Matrix6x dJ(6,model.nv); dJ.setZero();
+ context::Data::Matrix6x dJ(6,model.nv); dJ.setZero();
getJointJacobianTimeVariation(model,data,jointId,rf,dJ);
return dJ;
@@ -48,10 +48,12 @@ namespace pinocchio
void exposeJacobian()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
bp::def("computeJointJacobians",
- &computeJointJacobians,
+ &computeJointJacobians,
bp::args("model","data","q"),
"Computes the full model Jacobian, i.e. the stack of all the motion subspaces expressed in the coordinate world frame.\n"
"The result is accessible through data.J. This function computes also the forward kinematics of the model.\n\n"
@@ -62,7 +64,7 @@ namespace pinocchio
bp::return_value_policy());
bp::def("computeJointJacobians",
- &computeJointJacobians,
+ &computeJointJacobians,
bp::args("model","data"),
"Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.\n"
"The result is accessible through data.J. This function assumes that forward kinematics (pinocchio.forwardKinematics) has been called first.\n\n"
@@ -71,7 +73,8 @@ namespace pinocchio
"\tdata: data related to the model\n",
bp::return_value_policy());
- bp::def("computeJointJacobian",compute_jacobian_proxy,
+ bp::def("computeJointJacobian",
+ compute_jacobian_proxy,
bp::args("model","data","q","joint_id"),
"Computes the Jacobian of a specific joint frame expressed in the local frame of the joint according to the given input configuration.\n\n"
"Parameters:\n"
@@ -80,7 +83,8 @@ namespace pinocchio
"\tq: the joint configuration vector (size model.nq)\n"
"\tjoint_id: index of the joint\n");
- bp::def("getJointJacobian",get_jacobian_proxy,
+ bp::def("getJointJacobian",
+ get_jacobian_proxy,
bp::args("model","data","joint_id","reference_frame"),
"Computes the jacobian of a given given joint according to the given entries in data.\n"
"If reference_frame is set to LOCAL, it returns the Jacobian expressed in the local coordinate system of the joint.\n"
@@ -92,7 +96,8 @@ namespace pinocchio
"\tjoint_id: index of the joint\n"
"\treference_frame: reference frame in which the resulting derivatives are expressed\n");
- bp::def("computeJointJacobiansTimeVariation",computeJointJacobiansTimeVariation,
+ bp::def("computeJointJacobiansTimeVariation",
+ computeJointJacobiansTimeVariation,
bp::args("model","data","q","v"),
"Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depends both on q and v. It also computes the joint Jacobian of the model (similar to computeJointJacobians)."
"The result is accessible through data.dJ and data.J.\n\n"
@@ -103,7 +108,8 @@ namespace pinocchio
"\tv: the joint velocity vector (size model.nv)\n",
bp::return_value_policy());
- bp::def("getJointJacobianTimeVariation",get_jacobian_time_variation_proxy,
+ bp::def("getJointJacobianTimeVariation",
+ get_jacobian_time_variation_proxy,
bp::args("model","data","joint_id","reference_frame"),
"Computes the Jacobian time variation of a specific joint expressed in the requested frame provided by the value of reference_frame."
"You have to call computeJointJacobiansTimeVariation first. This function also computes the full model Jacobian contained in data.J.\n"
diff --git a/bindings/python/algorithm/expose-joints.cpp b/bindings/python/algorithm/expose-joints.cpp
index b84f169122..47e89871f1 100644
--- a/bindings/python/algorithm/expose-joints.cpp
+++ b/bindings/python/algorithm/expose-joints.cpp
@@ -12,25 +12,25 @@ namespace pinocchio
BOOST_PYTHON_FUNCTION_OVERLOADS(isNormalized_overload,isNormalized,2,3)
- static Eigen::VectorXd normalize_proxy(const Model & model,
- const Eigen::VectorXd & config)
+ static context::VectorXs normalize_proxy(const context::Model & model,
+ const context::VectorXs & config)
{
- Eigen::VectorXd q(config);
+ context::VectorXs q(config);
normalize(model,q);
return q;
}
- static Eigen::VectorXd randomConfiguration_proxy(const Model & model)
+ static context::VectorXs randomConfiguration_proxy(const context::Model & model)
{
return randomConfiguration(model);
}
- bp::tuple dIntegrate_proxy(const Model & model,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v)
+ bp::tuple dIntegrate_proxy(const context::Model & model,
+ const context::VectorXs & q,
+ const context::VectorXs & v)
{
- Eigen::MatrixXd J0(Eigen::MatrixXd::Zero(model.nv,model.nv));
- Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(model.nv,model.nv));
+ context::MatrixXs J0(context::MatrixXs::Zero(model.nv,model.nv));
+ context::MatrixXs J1(context::MatrixXs::Zero(model.nv,model.nv));
dIntegrate(model,q,v,J0,ARG0);
dIntegrate(model,q,v,J1,ARG1);
@@ -38,24 +38,24 @@ namespace pinocchio
return bp::make_tuple(J0,J1);
}
- Eigen::MatrixXd dIntegrate_arg_proxy(const Model & model,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const ArgumentPosition arg)
+ context::MatrixXs dIntegrate_arg_proxy(const context::Model & model,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const ArgumentPosition arg)
{
- Eigen::MatrixXd J(Eigen::MatrixXd::Zero(model.nv,model.nv));
+ context::MatrixXs J(context::MatrixXs::Zero(model.nv,model.nv));
dIntegrate(model,q,v,J,arg, SETTO);
return J;
}
- bp::tuple dDifference_proxy(const Model & model,
- const Eigen::VectorXd & q1,
- const Eigen::VectorXd & q2)
+ bp::tuple dDifference_proxy(const context::Model & model,
+ const context::VectorXs & q1,
+ const context::VectorXs & q2)
{
- Eigen::MatrixXd J0(Eigen::MatrixXd::Zero(model.nv,model.nv));
- Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(model.nv,model.nv));
+ context::MatrixXs J0(context::MatrixXs::Zero(model.nv,model.nv));
+ context::MatrixXs J1(context::MatrixXs::Zero(model.nv,model.nv));
dDifference(model,q1,q2,J0,ARG0);
dDifference(model,q1,q2,J1,ARG1);
@@ -63,12 +63,12 @@ namespace pinocchio
return bp::make_tuple(J0,J1);
}
- Eigen::MatrixXd dDifference_arg_proxy(const Model & model,
- const Eigen::VectorXd & q1,
- const Eigen::VectorXd & q2,
- const ArgumentPosition arg)
+ context::MatrixXs dDifference_arg_proxy(const context::Model & model,
+ const context::VectorXs & q1,
+ const context::VectorXs & q2,
+ const ArgumentPosition arg)
{
- Eigen::MatrixXd J(Eigen::MatrixXd::Zero(model.nv,model.nv));
+ context::MatrixXs J(context::MatrixXs::Zero(model.nv,model.nv));
dDifference(model,q1,q2,J,arg);
@@ -77,10 +77,12 @@ namespace pinocchio
void exposeJointsAlgo()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
bp::def("integrate",
- &integrate,
+ &integrate,
bp::args("model","q","v"),
"Integrate the joint configuration vector q with a tangent vector v during one unit time.\n"
"This is the canonical integrator of a Configuration Space composed of Lie groups, such as most robots.\n\n"
@@ -111,7 +113,7 @@ namespace pinocchio
"\targument_position: either pinocchio.ArgumentPosition.ARG0 or pinocchio.ArgumentPosition.ARG1, depending on the desired Jacobian value.\n");
bp::def("interpolate",
- &interpolate,
+ &interpolate,
bp::args("model","q1","q2","alpha"),
"Interpolate between two given joint configuration vectors q1 and q2.\n\n"
"Parameters:\n"
@@ -121,7 +123,7 @@ namespace pinocchio
"\talpha: the interpolation coefficient in [0,1]\n");
bp::def("difference",
- &difference,
+ &difference,
bp::args("model","q1","q2"),
"Difference between two joint configuration vectors, i.e. the tangent vector that must be integrated during one unit time"
"to go from q1 to q2.\n\n"
@@ -131,7 +133,7 @@ namespace pinocchio
"\tq2: the terminal joint configuration vector (size model.nq)\n");
bp::def("squaredDistance",
- &squaredDistance,
+ &squaredDistance,
bp::args("model","q1","q2"),
"Squared distance vector between two joint configuration vectors.\n\n"
"Parameters:\n"
@@ -140,7 +142,7 @@ namespace pinocchio
"\tq2: the terminal joint configuration vector (size model.nq)\n");
bp::def("distance",
- &distance,
+ &distance,
bp::args("model","q1","q2"),
"Distance between two joint configuration vectors.\n\n"
"Parameters:\n"
@@ -177,7 +179,7 @@ namespace pinocchio
"\tmodel: model of the kinematic tree\n");
bp::def("randomConfiguration",
- &randomConfiguration,
+ &randomConfiguration,
bp::args("model","lower_bound","upper_bound"),
"Generate a random configuration in the bounds given by the Joint lower and upper limits arguments.\n\n"
"Parameters:\n"
@@ -186,7 +188,7 @@ namespace pinocchio
"\tupper_bound: the upper bound on the joint configuration vectors (size model.nq)\n");
bp::def("neutral",
- &neutral,
+ &neutral,
bp::arg("model"),
"Returns the neutral configuration vector associated to the model.\n\n"
"Parameters:\n"
@@ -200,8 +202,9 @@ namespace pinocchio
"\tmodel: model of the kinematic tree\n"
"\tq: a joint configuration vector to normalize (size model.nq)\n");
+#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
bp::def("isSameConfiguration",
- &isSameConfiguration,
+ &isSameConfiguration,
bp::args("model","q1","q2","prec"),
"Return true if two configurations are equivalent within the given precision provided by prec.\n\n"
"Parameters:\n"
@@ -211,7 +214,7 @@ namespace pinocchio
"\tprec: requested accuracy for the comparison\n");
bp::def("isNormalized",
- &isNormalized,
+ &isNormalized,
isNormalized_overload(
bp::args("model","q","prec"),
"Check whether a configuration vector is normalized within the given precision provided by prec.\n\n"
@@ -219,8 +222,8 @@ namespace pinocchio
"\tmodel: model of the kinematic tree\n"
"\tq: a joint configuration vector (size model.nq)\n"
"\tprec: requested accuracy for the check\n"
- )
- );
+ ));
+#endif
}
} // namespace python
diff --git a/bindings/python/algorithm/expose-kinematic-regressor.cpp b/bindings/python/algorithm/expose-kinematic-regressor.cpp
index 908082067b..5a7bf717c8 100644
--- a/bindings/python/algorithm/expose-kinematic-regressor.cpp
+++ b/bindings/python/algorithm/expose-kinematic-regressor.cpp
@@ -12,10 +12,11 @@ namespace pinocchio
void exposeKinematicRegressor()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ enum { Options = context::Options };
bp::def("computeJointKinematicRegressor",
- (Data::Matrix6x (*)(const Model &, const Data &, const JointIndex, const ReferenceFrame, const SE3 &))&computeJointKinematicRegressor,
+ (context::Data::Matrix6x (*)(const context::Model &, const context::Data &, const JointIndex, const ReferenceFrame, const context::SE3 &))&computeJointKinematicRegressor,
bp::args("model","data","joint_id","reference_frame","placement"),
"Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree to the placement variation of the frame rigidly attached to the joint and given by its placement w.r.t. to the joint frame.\n\n"
"Parameters:\n"
@@ -26,7 +27,7 @@ namespace pinocchio
"\tplacement: relative placement to the joint frame\n");
bp::def("computeJointKinematicRegressor",
- (Data::Matrix6x (*)(const Model &, const Data &, const JointIndex, const ReferenceFrame))&computeJointKinematicRegressor,
+ (context::Data::Matrix6x (*)(const context::Model &, const context::Data &, const JointIndex, const ReferenceFrame))&computeJointKinematicRegressor,
bp::args("model","data","joint_id","reference_frame"),
"Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree to the placement variation of the joint given as input.\n\n"
"Parameters:\n"
@@ -36,7 +37,7 @@ namespace pinocchio
"\treference_frame: reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD)\n");
bp::def("computeFrameKinematicRegressor",
- (Data::Matrix6x (*)(const Model &, Data &, const FrameIndex, const ReferenceFrame))&computeFrameKinematicRegressor,
+ (context::Data::Matrix6x (*)(const context::Model &, context::Data &, const FrameIndex, const ReferenceFrame))&computeFrameKinematicRegressor,
bp::args("model","data","frame_id","reference_frame"),
"Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree to the placement variation of the frame given as input.\n\n"
"Parameters:\n"
diff --git a/bindings/python/algorithm/expose-kinematics-derivatives.cpp b/bindings/python/algorithm/expose-kinematics-derivatives.cpp
index af254fa889..1667ed00a5 100644
--- a/bindings/python/algorithm/expose-kinematics-derivatives.cpp
+++ b/bindings/python/algorithm/expose-kinematics-derivatives.cpp
@@ -14,12 +14,12 @@ namespace pinocchio
{
namespace bp = boost::python;
- bp::tuple getJointVelocityDerivatives_proxy(const Model & model,
- Data & data,
- const Model::JointIndex jointId,
+ bp::tuple getJointVelocityDerivatives_proxy(const context::Model & model,
+ context::Data & data,
+ const JointIndex jointId,
ReferenceFrame rf)
{
- typedef Data::Matrix6x Matrix6x;
+ typedef context::Data::Matrix6x Matrix6x;
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
@@ -29,13 +29,30 @@ namespace pinocchio
return bp::make_tuple(partial_dq,partial_dv);
}
+
+ bp::tuple getPointVelocityDerivatives_proxy(const context::Model & model,
+ context::Data & data,
+ const JointIndex joint_id,
+ const context::SE3 & placement,
+ ReferenceFrame rf)
+ {
+ typedef context::Data::Matrix3x Matrix3x;
+
+ Matrix3x v_partial_dq(Matrix3x::Zero(3,model.nv));
+ Matrix3x v_partial_dv(Matrix3x::Zero(3,model.nv));
+
+ getPointVelocityDerivatives(model,data,joint_id,placement,rf,
+ v_partial_dq,v_partial_dv);
+
+ return bp::make_tuple(v_partial_dq,v_partial_dv);
+ }
- bp::tuple getJointAccelerationDerivatives_proxy(const Model & model,
- Data & data,
- const Model::JointIndex jointId,
+ bp::tuple getJointAccelerationDerivatives_proxy(const context::Model & model,
+ context::Data & data,
+ const JointIndex jointId,
ReferenceFrame rf)
{
- typedef Data::Matrix6x Matrix6x;
+ typedef context::Data::Matrix6x Matrix6x;
Matrix6x v_partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x a_partial_dq(Matrix6x::Zero(6,model.nv));
@@ -49,22 +66,42 @@ namespace pinocchio
return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
}
- Data::Matrix3x getCoMVelocityDerivatives_proxy(const Model & model,
- Data & data)
+ bp::tuple getPointClassicAccelerationDerivatives_proxy(const context::Model & model,
+ context::Data & data,
+ const JointIndex joint_id,
+ const context::SE3 & placement,
+ ReferenceFrame rf)
+ {
+ typedef context::Data::Matrix3x Matrix3x;
+
+ Matrix3x v_partial_dq(Matrix3x::Zero(3,model.nv));
+ Matrix3x a_partial_dq(Matrix3x::Zero(3,model.nv));
+ Matrix3x a_partial_dv(Matrix3x::Zero(3,model.nv));
+ Matrix3x a_partial_da(Matrix3x::Zero(3,model.nv));
+
+ getPointClassicAccelerationDerivatives(model,data,joint_id,placement,rf,
+ v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
+
+ return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
+ }
+
+ context::Data::Matrix3x getCoMVelocityDerivatives_proxy(const context::Model & model,
+ context::Data & data)
{
- typedef Data::Matrix3x Matrix3x;
+ typedef context::Data::Matrix3x Matrix3x;
Matrix3x partial_dq(Matrix3x::Zero(3,model.nv));
getCenterOfMassVelocityDerivatives(model,data,partial_dq);
return partial_dq;
}
-
void exposeKinematicsDerivatives()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
bp::def("computeForwardKinematicsDerivatives",
- &computeForwardKinematicsDerivatives,
+ &computeForwardKinematicsDerivatives,
bp::args("model","data","q","v","a"),
"Computes all the terms required to compute the derivatives of the placement, spatial velocity and acceleration\n"
"for any joint of the model.\n"
@@ -81,7 +118,7 @@ namespace pinocchio
bp::args("model","data","joint_id","reference_frame"),
"Computes the partial derivatives of the spatial velocity of a given joint with respect to\n"
"the joint configuration and velocity and returns them as a tuple.\n"
- "The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
+ "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
"You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
@@ -89,12 +126,38 @@ namespace pinocchio
"\tjoint_id: index of the joint\n"
"\treference_frame: reference frame in which the resulting derivatives are expressed\n");
+ bp::def("getPointVelocityDerivatives",
+ getPointVelocityDerivatives_proxy,
+ bp::args("model","data","joint_id","placement","reference_frame"),
+ "Computes the partial derivatives of the velocity of a point given by its placement information w.r.t. the joint frame and returns them as a tuple.\n"
+ "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
+ "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
+ "Parameters:\n"
+ "\tmodel: model of the kinematic tree\n"
+ "\tdata: data related to the model\n"
+ "\tjoint_id: index of the joint\n"
+ "\tplacement: relative placement of the point w.r.t. the joint frame\n"
+ "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
+
+ bp::def("getPointClassicAccelerationDerivatives",
+ getPointClassicAccelerationDerivatives_proxy,
+ bp::args("model","data","joint_id","placement","reference_frame"),
+ "Computes the partial derivatives of the classic acceleration of a point given by its placement information w.r.t. the joint frame and returns them as a tuple.\n"
+ "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
+ "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
+ "Parameters:\n"
+ "\tmodel: model of the kinematic tree\n"
+ "\tdata: data related to the model\n"
+ "\tjoint_id: index of the joint\n"
+ "\tplacement: relative placement of the point w.r.t. the joint frame\n"
+ "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
+
bp::def("getJointAccelerationDerivatives",
getJointAccelerationDerivatives_proxy,
bp::args("model","data","joint_id","reference_frame"),
"Computes the partial derivatives of the spatial acceleration of a given joint with respect to\n"
"the joint configuration, velocity and acceleration and returns them as a tuple.\n"
- "The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
+ "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
"You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
@@ -115,7 +178,5 @@ namespace pinocchio
}
-
-
} // namespace python
} // namespace pinocchio
diff --git a/bindings/python/algorithm/expose-kinematics.cpp b/bindings/python/algorithm/expose-kinematics.cpp
index 8371c962e9..be0ca4497b 100644
--- a/bindings/python/algorithm/expose-kinematics.cpp
+++ b/bindings/python/algorithm/expose-kinematics.cpp
@@ -10,15 +10,18 @@ namespace pinocchio
namespace python
{
- BOOST_PYTHON_FUNCTION_OVERLOADS(getVelocity_overload, (getVelocity), 3, 4)
- BOOST_PYTHON_FUNCTION_OVERLOADS(getAcceleration_overload, (getAcceleration), 3, 4)
- BOOST_PYTHON_FUNCTION_OVERLOADS(getClassicalAcceleration_overload, (getClassicalAcceleration), 3, 4)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getVelocity_overload, (getVelocity), 3, 4)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getAcceleration_overload, (getAcceleration), 3, 4)
+ BOOST_PYTHON_FUNCTION_OVERLOADS(getClassicalAcceleration_overload, (getClassicalAcceleration), 3, 4)
void exposeKinematics()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
+
bp::def("updateGlobalPlacements",
- &updateGlobalPlacements,
+ &updateGlobalPlacements,
bp::args("model","data"),
"Updates the global placements of all joint frames of the kinematic "
"tree and store the results in data according to the relative placements of the joints.\n\n"
@@ -27,28 +30,28 @@ namespace pinocchio
"\tdata: data related to the model\n");
bp::def("getVelocity",
- &getVelocity,
+ &getVelocity,
getVelocity_overload(
bp::args("model","data","joint_id","reference_frame"),
"Returns the spatial velocity of the joint expressed in the coordinate system given by reference_frame.\n"
"forwardKinematics(model,data,q,v[,a]) should be called first to compute the joint spatial velocity stored in data.v"));
bp::def("getAcceleration",
- &getAcceleration,
+ &getAcceleration,
getAcceleration_overload(
bp::args("model","data","joint_id","reference_frame"),
"Returns the spatial acceleration of the joint expressed in the coordinate system given by reference_frame.\n"
"forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
bp::def("getClassicalAcceleration",
- &getClassicalAcceleration,
+ &getClassicalAcceleration,
getClassicalAcceleration_overload(
bp::args("model","data","joint_id","reference_frame"),
"Returns the \"classical\" acceleration of the joint expressed in the coordinate system given by reference_frame.\n"
"forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
bp::def("forwardKinematics",
- &forwardKinematics,
+ &forwardKinematics,
bp::args("model","data","q"),
"Compute the global placements of all the joints of the kinematic "
"tree and store the results in data.\n\n"
@@ -58,7 +61,7 @@ namespace pinocchio
"\tq: the joint configuration vector (size model.nq)\n");
bp::def("forwardKinematics",
- &forwardKinematics,
+ &forwardKinematics,
bp::args("model","data","q","v"),
"Compute the global placements and local spatial velocities of all the joints of the kinematic "
"tree and store the results in data.\n\n"
@@ -69,7 +72,7 @@ namespace pinocchio
"\tv: the joint velocity vector (size model.nv)\n");
bp::def("forwardKinematics",
- &forwardKinematics,
+ &forwardKinematics,
bp::args("model","data","q","v","a"),
"Compute the global placements, local spatial velocities and spatial accelerations of all the joints of the kinematic "
"tree and store the results in data.\n\n"
diff --git a/bindings/python/algorithm/expose-regressor.cpp b/bindings/python/algorithm/expose-regressor.cpp
index 861e8d21f1..abc8f539ff 100644
--- a/bindings/python/algorithm/expose-regressor.cpp
+++ b/bindings/python/algorithm/expose-regressor.cpp
@@ -10,30 +10,32 @@ namespace pinocchio
namespace python
{
- Eigen::MatrixXd bodyRegressor_proxy(const Motion & v, const Motion & a)
+ context::MatrixXs bodyRegressor_proxy(const context::Motion & v, const context::Motion & a)
{
return bodyRegressor(v,a);
}
- Eigen::MatrixXd jointBodyRegressor_proxy(const Model & model, Data & data, const JointIndex jointId)
+ context::MatrixXs jointBodyRegressor_proxy(const context::Model & model, context::Data & data, const JointIndex jointId)
{
return jointBodyRegressor(model,data,jointId);
}
- Eigen::MatrixXd frameBodyRegressor_proxy(const Model & model, Data & data, const FrameIndex frameId)
+ context::MatrixXs frameBodyRegressor_proxy(const context::Model & model, context::Data & data, const FrameIndex frameId)
{
return frameBodyRegressor(model,data,frameId);
}
void exposeRegressor()
{
- using namespace Eigen;
-
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
+
bp::def("computeStaticRegressor",
- &computeStaticRegressor,
+ &computeStaticRegressor,
bp::args("model","data","q"),
"Compute the static regressor that links the inertia parameters of the system to its center of mass position,\n"
- "store the result in Data and return it.\n\n"
+ "store the result in context::Data and return it.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
"\tdata: data related to the model\n"
@@ -71,11 +73,11 @@ namespace pinocchio
"\tframe_id: index of the frame\n");
bp::def("computeJointTorqueRegressor",
- &computeJointTorqueRegressor,
+ &computeJointTorqueRegressor,
bp::args("model","data","q","v","a"),
"Compute the joint torque regressor that links the joint torque "
"to the dynamic parameters of each link according to the current the robot motion,\n"
- "store the result in Data and return it.\n\n"
+ "store the result in context::Data and return it.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
"\tdata: data related to the model\n"
diff --git a/bindings/python/algorithm/expose-rnea-derivatives.cpp b/bindings/python/algorithm/expose-rnea-derivatives.cpp
index c4a7ebaec4..297a5095c3 100644
--- a/bindings/python/algorithm/expose-rnea-derivatives.cpp
+++ b/bindings/python/algorithm/expose-rnea-derivatives.cpp
@@ -12,31 +12,34 @@ namespace pinocchio
{
namespace bp = boost::python;
- typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector;
+ typedef PINOCCHIO_ALIGNED_STD_VECTOR(context::Force) ForceAlignedVector;
- Data::MatrixXs computeGeneralizedGravityDerivatives(const Model & model, Data & data,
- const Eigen::VectorXd & q)
+ context::Data::MatrixXs computeGeneralizedGravityDerivatives(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q)
{
- Data::MatrixXs res(model.nv,model.nv);
+ context::Data::MatrixXs res(model.nv,model.nv);
res.setZero();
pinocchio::computeGeneralizedGravityDerivatives(model,data,q,res);
return res;
}
- Data::MatrixXs computeStaticTorqueDerivatives(const Model & model, Data & data,
- const Eigen::VectorXd & q,
- const ForceAlignedVector & fext)
+ context::Data::MatrixXs computeStaticTorqueDerivatives(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const ForceAlignedVector & fext)
{
- Data::MatrixXs res(model.nv,model.nv);
+ context::Data::MatrixXs res(model.nv,model.nv);
res.setZero();
pinocchio::computeStaticTorqueDerivatives(model,data,q,fext,res);
return res;
}
- bp::tuple computeRNEADerivatives(const Model & model, Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & a)
+ bp::tuple computeRNEADerivatives(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & a)
{
pinocchio::computeRNEADerivatives(model,data,q,v,a);
make_symmetric(data.M);
@@ -45,10 +48,11 @@ namespace pinocchio
make_ref(data.M));
}
- bp::tuple computeRNEADerivatives_fext(const Model & model, Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & a,
+ bp::tuple computeRNEADerivatives_fext(const context::Model & model,
+ context::Data & data,
+ const context::VectorXs & q,
+ const context::VectorXs & v,
+ const context::VectorXs & a,
const ForceAlignedVector & fext)
{
pinocchio::computeRNEADerivatives(model,data,q,v,a,fext);
@@ -60,8 +64,6 @@ namespace pinocchio
void exposeRNEADerivatives()
{
- using namespace Eigen;
-
bp::def("computeGeneralizedGravityDerivatives",
computeGeneralizedGravityDerivatives,
bp::args("model","data","q"),
diff --git a/bindings/python/algorithm/expose-rnea.cpp b/bindings/python/algorithm/expose-rnea.cpp
index a91eedfe0a..0d3a63158f 100644
--- a/bindings/python/algorithm/expose-rnea.cpp
+++ b/bindings/python/algorithm/expose-rnea.cpp
@@ -13,10 +13,12 @@ namespace pinocchio
void exposeRNEA()
{
- using namespace Eigen;
+ typedef context::Scalar Scalar;
+ typedef context::VectorXs VectorXs;
+ enum { Options = context::Options };
bp::def("rnea",
- &rnea,
+ &rnea,
bp::args("model","data","q","v","a"),
"Compute the RNEA, store the result in Data and return it.\n\n"
"Parameters:\n"
@@ -28,7 +30,7 @@ namespace pinocchio
bp::return_value_policy());
bp::def("rnea",
- &rnea,
+ &rnea,
bp::args("model","data","q","v","a","fext"),
"Compute the RNEA with external forces, store the result in Data and return it.\n\n"
"Parameters:\n"
@@ -41,7 +43,7 @@ namespace pinocchio
bp::return_value_policy());
bp::def("nonLinearEffects",
- &nonLinearEffects,
+ &nonLinearEffects,
bp::args("model","data","q","v"),
"Compute the Non Linear Effects (coriolis, centrifugal and gravitational effects), store the result in Data and return it.\n\n"
"Parameters:\n"
@@ -52,7 +54,7 @@ namespace pinocchio
bp::return_value_policy());
bp::def("computeGeneralizedGravity",
- &computeGeneralizedGravity,
+ &computeGeneralizedGravity,
bp::args("model","data","q"),
"Compute the generalized gravity contribution g(q) of the Lagrangian dynamics, store the result in data.g and return it.\n\n"
"Parameters:\n"
@@ -62,7 +64,7 @@ namespace pinocchio
bp::return_value_policy());
bp::def("computeStaticTorque",
- &computeStaticTorque,
+ &computeStaticTorque,
bp::args("model","data","q","fext"),
"Computes the generalized static torque contribution g(q) - J.T f_ext of the Lagrangian dynamics, store the result in data.tau and return it.\n\n"
"Parameters:\n"
@@ -73,7 +75,7 @@ namespace pinocchio
bp::return_value_policy());
bp::def("computeCoriolisMatrix",
- &computeCoriolisMatrix,
+ &computeCoriolisMatrix,
bp::args("model","data","q","v"),
"Compute the Coriolis Matrix C(q,v) of the Lagrangian dynamics, store the result in data.C and return it.\n\n"
"Parameters:\n"
@@ -84,7 +86,7 @@ namespace pinocchio
bp::return_value_policy());
bp::def("getCoriolisMatrix",
- &getCoriolisMatrix,
+ &getCoriolisMatrix,
bp::args("model","data"),
"Retrives the Coriolis Matrix C(q,v) of the Lagrangian dynamics after calling one of the derivative algorithms, store the result in data.C and return it.\n\n"
"Parameters:\n"
diff --git a/bindings/python/algorithm/proximal.hpp b/bindings/python/algorithm/proximal.hpp
new file mode 100644
index 0000000000..6c2b395ac1
--- /dev/null
+++ b/bindings/python/algorithm/proximal.hpp
@@ -0,0 +1,61 @@
+//
+// Copyright (c) 2019-2020 INRIA
+//
+
+#ifndef __pinocchio_python_algorithm_proximal_hpp__
+#define __pinocchio_python_algorithm_proximal_hpp__
+
+#include
+#include "pinocchio/algorithm/proximal.hpp"
+
+namespace pinocchio
+{
+ namespace python
+ {
+ namespace bp = boost::python;
+
+ template
+ struct ProximalSettingsPythonVisitor
+ : public boost::python::def_visitor< ProximalSettingsPythonVisitor >
+ {
+ typedef typename ProximalSettings::Scalar Scalar;
+
+ public:
+
+ template
+ void visit(PyClass& cl) const
+ {
+ cl
+ .def(bp::init<>("Default constructor."))
+ .def(bp::init
+ ((bp::arg("accuracy"),
+ bp::arg("mu"),
+ bp::arg("max_iter")),
+ "Structure containing all the settings paramters for the proximal algorithms."))
+
+ .add_property("accuracy",&ProximalSettings::accuracy,"Minimum residual accuracy.")
+ .add_property("mu",&ProximalSettings::mu,"Regularization parameter of the Proximal algorithms.")
+ .add_property("max_iter",&ProximalSettings::max_iter,"Maximal number of iterations.")
+
+ .add_property("residual",&ProximalSettings::residual,
+ "Final residual when the algorithm has converged or reached the maximal number of allowed iterations.")
+ .add_property("iter",&ProximalSettings::iter,
+ "Final number of iteration of the algorithm when it has converged or reached the maximal number of allowed iterations.")
+ ;
+ }
+
+ static void expose()
+ {
+ bp::class_("ProximalSettings",
+ "Structure containing all the settings paramters for the Proximal algorithms.",
+ bp::no_init)
+ .def(ProximalSettingsPythonVisitor())
+ ;
+
+ }
+ };
+
+ } // namespace python
+} // namespace pinocchio
+
+#endif // ifndef __pinocchio_python_algorithm_proximal_hpp__
diff --git a/bindings/python/context.hpp b/bindings/python/context.hpp
new file mode 100644
index 0000000000..38a829933a
--- /dev/null
+++ b/bindings/python/context.hpp
@@ -0,0 +1,18 @@
+//
+// Copyright (c) 2020 INRIA
+//
+
+#ifndef __pinocchio_python_context_hpp__
+#define __pinocchio_python_context_hpp__
+
+#define PINOCCHIO_PYTHON_SCALAR_TYPE_DEFAULT double
+
+#define PINOCCHIO_PYTHON_CONTEXT_FILE_DEFAULT "pinocchio/bindings/python/context/default.hpp"
+
+#ifndef PINOCCHIO_PYTHON_CONTEXT_FILE
+#define PINOCCHIO_PYTHON_CONTEXT_FILE PINOCCHIO_PYTHON_CONTEXT_FILE_DEFAULT
+#endif
+
+#include PINOCCHIO_PYTHON_CONTEXT_FILE
+
+#endif // #ifndef __pinocchio_python_context_hpp__
diff --git a/bindings/python/context/casadi.hpp b/bindings/python/context/casadi.hpp
new file mode 100644
index 0000000000..85cab1f01c
--- /dev/null
+++ b/bindings/python/context/casadi.hpp
@@ -0,0 +1,415 @@
+//
+// Copyright (c) 2020 INRIA
+//
+
+#ifndef __pinocchio_python_context_casadi_hpp__
+#define __pinocchio_python_context_casadi_hpp__
+
+#include "pinocchio/autodiff/casadi.hpp"
+
+#define PINOCCHIO_PYTHON_SCALAR_TYPE ::casadi::SX
+#include "pinocchio/bindings/python/context/generic.hpp"
+#undef PINOCCHIO_PYTHON_SCALAR_TYPE
+
+#define PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
+#define PINOCCHIO_PYTHON_NO_SERIALIZATION
+
+#include
+#include
+#include
+#include
+
+namespace eigenpy {
+
+namespace bp = boost::python;
+
+namespace casadi
+{
+
+struct CasadiType
+{
+ static PyTypeObject * getSXType()
+ {
+ return reinterpret_cast(getInstance().casadi_SX_type.ptr());
+ }
+
+private:
+
+ static const CasadiType & getInstance()
+ {
+ static CasadiType elt;
+ return elt;
+ }
+
+ CasadiType()
+ {
+ casadi_module = bp::import("casadi");
+ casadi_SX_type = casadi_module.attr("SX");
+ Py_INCREF(casadi_module.ptr());
+ }
+
+ ~CasadiType()
+ {
+ casadi_SX_type.~object();
+// casadi_module.~object();
+ }
+
+ bp::object casadi_module;
+ bp::object casadi_SX_type;
+};
+
+} // namespace casadi
+
+template
+struct EigenFromPy,::casadi::Matrix >
+{
+ typedef _Scalar Scalar;
+ typedef ::casadi::Matrix CasadiMatrix;
+ typedef Eigen::Matrix<_Scalar,Rows,Cols,Options,MaxRows,MaxCols> MatType;
+
+ /// \brief Determine if pyObj can be converted into a MatType object
+ static void* convertible(PyObject* pyObj);
+
+ /// \brief Allocate memory and copy pyObj in the new storage
+ static void construct(PyObject* pyObj,
+ bp::converter::rvalue_from_python_stage1_data* memory);
+
+ static void registration();
+};
+
+template
+void* EigenFromPy,::casadi::Matrix >::convertible(PyObject * pyObj)
+{
+ if(std::strcmp(pyObj->ob_type->tp_name,CasadiMatrix::type_name().c_str()) != 0)
+ return 0;
+
+#define RETURN_VALUE(value) \
+ { \
+ Py_DECREF(reinterpret_cast(casadi_matrix_swig_obj)); \
+ return value; \
+ }
+
+ eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj);
+ if(casadi_matrix_swig_obj == NULL)
+ RETURN_VALUE(0);
+
+ CasadiMatrix * casadi_matrix_ptr = reinterpret_cast(casadi_matrix_swig_obj->ptr);
+ const CasadiMatrix & casadi_matrix = *casadi_matrix_ptr;
+
+ const casadi_int
+ R = casadi_matrix.rows(),
+ C = casadi_matrix.columns(),
+ size = casadi_matrix.numel();
+
+ const int ndim = (R == 0 || C == 0) ? 0 : (R == 1 || C == 1) ? 1 : 2;
+
+ if(MatType::IsVectorAtCompileTime)
+ {
+ const Eigen::DenseIndex size_at_compile_time
+ = MatType::IsRowMajor
+ ? MatType::ColsAtCompileTime
+ : MatType::RowsAtCompileTime;
+
+ switch(ndim)
+ {
+ case 0:
+ RETURN_VALUE(0);
+ case 1:
+ {
+ if(size_at_compile_time != Eigen::Dynamic)
+ {
+ // check that the sizes at compile time matche
+ if(size == size_at_compile_time)
+ {
+ if(MatType::ColsAtCompileTime != C || MatType::RowsAtCompileTime != R)
+ {
+ RETURN_VALUE(0);
+ }
+ else
+ {
+ RETURN_VALUE(pyObj);
+ }
+ }
+ else
+ RETURN_VALUE(0);
+ }
+ else // This is a dynamic MatType
+ RETURN_VALUE(pyObj);
+ }
+ case 2:
+ {
+ assert(R > 1 && C > 1);
+ RETURN_VALUE(0);
+ }
+ default:
+ RETURN_VALUE(0);
+ }
+ }
+ else // this is a matrix
+ {
+ if(ndim == 1) // We can always convert a vector into a matrix
+ RETURN_VALUE(pyObj);
+
+ if(ndim == 2)
+ {
+ if( (MatType::RowsAtCompileTime!=R)
+ && (MatType::RowsAtCompileTime!=Eigen::Dynamic) )
+ RETURN_VALUE(0);
+ if( (MatType::ColsAtCompileTime!=C)
+ && (MatType::ColsAtCompileTime!=Eigen::Dynamic) )
+ RETURN_VALUE(0);
+ }
+ }
+
+ RETURN_VALUE(pyObj);
+#undef RETURN_VALUE
+}
+
+template
+void EigenFromPy