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,::casadi::Matrix >::construct(PyObject * pyObj, + bp::converter::rvalue_from_python_stage1_data* memory) +{ + eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj); + assert(casadi_matrix_swig_obj != NULL); + + 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(); + + bp::converter::rvalue_from_python_storage* storage = reinterpret_cast*> + (reinterpret_cast(memory)); + + // Allocate memory + void * storage_ptr = storage->storage.bytes; + MatType * eigen_matrix_ptr = ::eigenpy::details::init_matrix_or_array::run(R,C,storage_ptr); + + // Copy element to matrix + pinocchio::casadi::copy(casadi_matrix,*eigen_matrix_ptr); + + memory->convertible = storage->storage.bytes; + Py_DECREF(reinterpret_cast(casadi_matrix_swig_obj)); +} + +template +void EigenFromPy,::casadi::Matrix >::registration() +{ + bp::converter::registry::push_back + (reinterpret_cast(&EigenFromPy::convertible), + &EigenFromPy::construct,bp::type_id()); +} + +template +struct EigenToPy > +{ + typedef ::casadi::Matrix<_Scalar> CasadiMatrix; + + static PyObject* convert(typename boost::add_reference::type>::type mat) + { + assert( (mat.rows()(casadi::CasadiType::getSXType()), + NULL); + + eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(casadi_matrix_py_ptr); + assert(casadi_matrix_swig_obj != NULL); + + CasadiMatrix * casadi_matrix_obj_ptr = reinterpret_cast(casadi_matrix_swig_obj->ptr); + pinocchio::casadi::copy(mat.derived(),*casadi_matrix_obj_ptr); + + Py_DECREF(reinterpret_cast(casadi_matrix_swig_obj)); + return casadi_matrix_py_ptr; + } + +}; + +template +struct EigenToPy< Eigen::Ref,::casadi::Matrix<_Scalar> > +{ + typedef ::casadi::Matrix<_Scalar> CasadiMatrix; + + static PyObject* convert(const Eigen::Ref & mat) + { + assert( (mat.rows()(casadi::CasadiType::getSXType()), + NULL); + + eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(casadi_matrix_py_ptr); + assert(casadi_matrix_swig_obj != NULL); + + CasadiMatrix * casadi_matrix_obj_ptr = reinterpret_cast(casadi_matrix_swig_obj->ptr); + pinocchio::casadi::copy(mat.derived(),*casadi_matrix_obj_ptr); + + Py_DECREF(reinterpret_cast(casadi_matrix_swig_obj)); + return casadi_matrix_py_ptr; + } +}; + +namespace internal { + + template<> + inline npy_bool SpecialMethods::nonzero(void * ip, void * array) + { + + typedef pinocchio::python::context::Scalar Scalar; + PyArrayObject * py_array = static_cast(array); + if(py_array == NULL || PyArray_ISBEHAVED_RO(py_array)) + { + const Scalar & value = *static_cast(ip); + return (npy_bool)(value.is_zero()); + } + else + { + Scalar tmp_value; + PyArray_DESCR(py_array)->f->copyswap(&tmp_value, ip, PyArray_ISBYTESWAPPED(py_array), + array); + return (npy_bool)(tmp_value.is_zero()); + } + } + +} // namespace internal + +} // namespace eigenpy + +namespace pinocchio { namespace python { + + template + struct CasadiMatrixToPython + { + + static PyObject* convert(CasadiMatrix const & x) + { + PyObject * casadi_matrix_py_ptr = PyObject_CallObject(reinterpret_cast(eigenpy::casadi::CasadiType::getSXType()), + NULL); + eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(casadi_matrix_py_ptr); + assert(casadi_matrix_swig_obj != NULL); + + CasadiMatrix * casadi_matrix_obj_ptr = reinterpret_cast(casadi_matrix_swig_obj->ptr); + *casadi_matrix_obj_ptr = x; + + Py_DECREF(reinterpret_cast(casadi_matrix_swig_obj)); + return casadi_matrix_py_ptr; + } + + static PyTypeObject const* get_pytype() + { + return ::eigenpy::casadi::CasadiType::getSXType(); + } + + static void registration() + { + boost::python::to_python_converter(); + } + }; + + template + struct CasadiMatrixFromPython + { + struct Extractor + { + static CasadiMatrix & execute(PyObject* /*pyObj*/) + { + throw std::runtime_error("Should never be called"); + } + }; + + static void registration() + { + boost::python::converter::registry::insert + ( &extract + , boost::python::detail::extractor_type_id(&Extractor::execute) +#ifndef BOOST_PYTHON_NO_PY_SIGNATURES + , &get_pytype +#endif + ); + } + + private: + static void* extract(PyObject* pyObj) + { + if(!PyObject_TypeCheck(pyObj,::eigenpy::casadi::CasadiType::getSXType())) + return 0; + + eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj); + return casadi_matrix_swig_obj->ptr; + + } +#ifndef BOOST_PYTHON_NO_PY_SIGNATURES + static PyTypeObject const * get_pytype() + { + return ::eigenpy::casadi::CasadiType::getSXType(); + } +#endif + }; + +// template +// struct CasadiMatrixFromPython +// { +// static void* convertible(PyObject * pyObj) +// { +// if(PyFloat_Check(pyObj)) +// return pyObj; +// if(std::strcmp(pyObj->ob_type->tp_name,CasadiMatrix::type_name().c_str()) != 0) +// return 0; +// +// return pyObj; +// } +// static void construct(PyObject * pyObj, +// boost::python::converter::rvalue_from_python_stage1_data * memory) +// { +// eigenpy::PySwigObject * casadi_matrix_swig_obj = eigenpy::get_PySwigObject(pyObj); +// assert(casadi_matrix_swig_obj != NULL); +// +// CasadiMatrix * casadi_matrix_ptr = reinterpret_cast(casadi_matrix_swig_obj->ptr); +// const CasadiMatrix & casadi_matrix = *casadi_matrix_ptr; +// +// bp::converter::rvalue_from_python_storage* storage = reinterpret_cast*> +// (reinterpret_cast(memory)); +// +// // Allocate memory +// void * storage_ptr = storage->storage.bytes; +// CasadiMatrix * casadi_matrix_cpp = new (storage_ptr) CasadiMatrix(casadi_matrix); +// +// memory->convertible = storage->storage.bytes; +// Py_DECREF(reinterpret_cast(casadi_matrix_swig_obj)); +// } +// }; + + inline boost::python::object getScalarType() + { + namespace bp = boost::python; + + PyObject * pyObj = reinterpret_cast(::eigenpy::casadi::CasadiType::getSXType()); + bp::object scalar_type(bp::handle<>(bp::borrowed(pyObj))); + + return scalar_type; + } + + inline void exposeSpecificTypeFeatures() + { + typedef pinocchio::python::context::Scalar Scalar; + CasadiMatrixToPython::registration(); + CasadiMatrixFromPython::registration(); + boost::python::implicitly_convertible(); + boost::python::implicitly_convertible(); + boost::python::implicitly_convertible(); + boost::python::implicitly_convertible(); + boost::python::implicitly_convertible(); + }; + +}} + +namespace pinocchio { namespace python { namespace internal { + + template struct has_operator_equal; + + template + struct has_operator_equal<::casadi::Matrix > : boost::false_type + {}; +}}} + +#endif // #ifndef __pinocchio_python_context_casadi_hpp__ diff --git a/bindings/python/context/cppad.hpp b/bindings/python/context/cppad.hpp new file mode 100644 index 0000000000..5765d25a34 --- /dev/null +++ b/bindings/python/context/cppad.hpp @@ -0,0 +1,44 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef __pinocchio_python_context_cppad_hpp__ +#define __pinocchio_python_context_cppad_hpp__ + +#include "pinocchio/autodiff/cppad.hpp" + +#define PINOCCHIO_PYTHON_SCALAR_TYPE ::CppAD::AD +#include "pinocchio/bindings/python/context/generic.hpp" +#undef PINOCCHIO_PYTHON_SCALAR_TYPE + +#define PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS +#define PINOCCHIO_PYTHON_NO_SERIALIZATION + +#include + +namespace pinocchio +{ + namespace python + { + inline void exposeSpecificTypeFeatures() { }; + + } +} + +namespace pinocchio +{ + namespace python + { + namespace internal + { + + template struct has_operator_equal; + + template + struct has_operator_equal< ::CppAD::AD > : boost::false_type {}; + + } + } +} + +#endif // #ifndef __pinocchio_python_context_cppad_hpp__ diff --git a/bindings/python/context/default.hpp b/bindings/python/context/default.hpp new file mode 100644 index 0000000000..89544b7638 --- /dev/null +++ b/bindings/python/context/default.hpp @@ -0,0 +1,24 @@ +// +// Copyright (c) 2020-2021 INRIA +// + +#ifndef __pinocchio_python_context_default_hpp__ +#define __pinocchio_python_context_default_hpp__ + +#define PINOCCHIO_PYTHON_SCALAR_TYPE PINOCCHIO_SCALAR_TYPE +#include "pinocchio/bindings/python/context/generic.hpp" +#include + +namespace pinocchio { namespace python { + + inline void exposeSpecificTypeFeatures() {}; + + inline boost::python::object getScalarType() + { + namespace bp = boost::python; + return bp::object(bp::handle<>(bp::borrowed(reinterpret_cast(&PyFloat_Type)))); + } +}} + +#undef PINOCCHIO_PYTHON_SCALAR_TYPE +#endif // #ifndef __pinocchio_python_context_default_hpp__ diff --git a/bindings/python/context/generic.hpp b/bindings/python/context/generic.hpp new file mode 100644 index 0000000000..1bb72a0af2 --- /dev/null +++ b/bindings/python/context/generic.hpp @@ -0,0 +1,50 @@ +#ifndef __pinocchio_python_context_generic_hpp__ +#define __pinocchio_python_context_generic_hpp__ + +#include "pinocchio/fwd.hpp" +#include "pinocchio/spatial/fwd.hpp" +#include "pinocchio/multibody/fwd.hpp" +#include "pinocchio/algorithm/fwd.hpp" + +#include + +namespace pinocchio { +namespace python { + +namespace context { + +typedef PINOCCHIO_PYTHON_SCALAR_TYPE Scalar; +enum { Options = 0 }; + +// Eigen +EIGENPY_MAKE_TYPEDEFS_ALL_SIZES(Scalar,Options,s); +typedef Eigen::Matrix Matrix1s; +typedef Eigen::Quaternion Quaternion; +typedef Eigen::AngleAxis AngleAxis; + +// Spatial +typedef SE3Tpl SE3; +typedef MotionTpl Motion; +typedef ForceTpl Force; +typedef InertiaTpl Inertia; + +// Multibody +typedef FrameTpl Frame; +typedef ModelTpl Model; +typedef DataTpl Data; + +typedef JointCollectionDefaultTpl JointCollectionDefault; + +typedef JointModelTpl JointModel; +typedef JointDataTpl JointData; + +// Algorithm +typedef ProximalSettingsTpl ProximalSettings; +typedef cholesky::ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; + +typedef RigidConstraintModelTpl RigidConstraintModel; +typedef RigidConstraintDataTpl RigidConstraintData; + +}}} + +#endif // #ifndef __pinocchio_python_context_generic_hpp__ diff --git a/bindings/python/fwd.hpp b/bindings/python/fwd.hpp index 0907bb4dd8..9015dd53f1 100644 --- a/bindings/python/fwd.hpp +++ b/bindings/python/fwd.hpp @@ -3,10 +3,10 @@ // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_python_python_hpp__ -#define __pinocchio_python_python_hpp__ +#ifndef __pinocchio_python_fwd_hpp__ +#define __pinocchio_python_fwd_hpp__ -#include "pinocchio/fwd.hpp" +#include "pinocchio/bindings/python/context.hpp" #include namespace pinocchio @@ -24,6 +24,8 @@ namespace pinocchio // Expose math module void exposeRpy(); + void exposeEigenTypes(); + void exposeConversions(); // Expose multibody classes void exposeJoints(); @@ -52,4 +54,4 @@ namespace pinocchio } // namespace python } // namespace pinocchio -#endif // ifndef __pinocchio_python_python_hpp__ +#endif // ifndef __pinocchio_python_fwd_hpp__ diff --git a/bindings/python/math/expose-eigen-types.cpp b/bindings/python/math/expose-eigen-types.cpp new file mode 100644 index 0000000000..fab5c010e7 --- /dev/null +++ b/bindings/python/math/expose-eigen-types.cpp @@ -0,0 +1,84 @@ +// +// Copyright (c) 2020 INRIA +// + +#include + +#include "pinocchio/bindings/python/fwd.hpp" +#include "pinocchio/bindings/python/utils/registration.hpp" +#include "pinocchio/bindings/python/utils/std-vector.hpp" + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::Quaternion); +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::AngleAxis); + +namespace pinocchio +{ + namespace python + { + + namespace bp = boost::python; + + namespace internal + { + template::value> + struct exposeTypeAlgo + { + static void run() {}; + }; + + template + struct exposeTypeAlgo + { + static void run() + { + eigenpy::exposeType(); + eigenpy::exposeType(); + }; + }; + + template + void exposeType() + { + exposeTypeAlgo::run(); + } + } + + void exposeEigenTypes() + { +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS + if(! register_symbolic_link_to_registered_type()) + eigenpy::expose(); + if(! register_symbolic_link_to_registered_type()) + eigenpy::expose(); +#endif + + StdContainerFromPythonList< std::vector >::register_converter(); + + typedef Eigen::Matrix Matrix6s; + typedef Eigen::Matrix Vector6s; + typedef Eigen::Matrix Matrix6xs; + typedef Eigen::Matrix Matrix3xs; + + internal::exposeType(); + eigenpy::enableEigenPySpecific(); + eigenpy::enableEigenPySpecific(); + eigenpy::enableEigenPySpecific(); + eigenpy::enableEigenPySpecific(); + eigenpy::enableEigenPySpecific(); + } + + } // namespace python +} // namespace pinocchio + diff --git a/bindings/python/module.cpp b/bindings/python/module.cpp index 25b5cdb084..ac6e991049 100644 --- a/bindings/python/module.cpp +++ b/bindings/python/module.cpp @@ -12,6 +12,7 @@ #include "pinocchio/bindings/python/utils/registration.hpp" #include "pinocchio/bindings/python/utils/std-vector.hpp" +#include "pinocchio/spatial/cartesian-axis.hpp" #include "pinocchio/bindings/python/serialization/serialization.hpp" #include @@ -22,7 +23,7 @@ namespace bp = boost::python; using namespace pinocchio::python; -BOOST_PYTHON_MODULE(pinocchio_pywrap) +BOOST_PYTHON_MODULE(PINOCCHIO_PYTHON_MODULE_NAME) { bp::docstring_options module_docstring_options(true,true,false); @@ -33,22 +34,18 @@ BOOST_PYTHON_MODULE(pinocchio_pywrap) // Enable warnings bp::import("warnings"); - if(! register_symbolic_link_to_registered_type()) - eigenpy::exposeQuaternion(); - if(! register_symbolic_link_to_registered_type()) - eigenpy::exposeAngleAxis(); + eigenpy::enableEigenPy(); + exposeEigenTypes(); + exposeSpecificTypeFeatures(); - StdContainerFromPythonList< std::vector >::register_converter(); + bp::scope().attr("ScalarType") = getScalarType(); - typedef Eigen::Matrix Matrix6d; - typedef Eigen::Matrix Vector6d; - typedef Eigen::Matrix Matrix6x; - typedef Eigen::Matrix Matrix3x; - - eigenpy::enableEigenPySpecific(); - eigenpy::enableEigenPySpecific(); - eigenpy::enableEigenPySpecific(); - eigenpy::enableEigenPySpecific(); + bp::scope().attr("XAxis") + = bp::object(bp::handle<>(eigenpy::EigenToPy::convert(pinocchio::XAxis::vector()))); + bp::scope().attr("YAxis") + = bp::object(bp::handle<>(eigenpy::EigenToPy::convert(pinocchio::YAxis::vector()))); + bp::scope().attr("ZAxis") + = bp::object(bp::handle<>(eigenpy::EigenToPy::convert(pinocchio::ZAxis::vector()))); exposeSE3(); exposeForce(); @@ -60,34 +57,43 @@ BOOST_PYTHON_MODULE(pinocchio_pywrap) exposeSkew(); exposeLieGroups(); - bp::enum_< ::pinocchio::ReferenceFrame >("ReferenceFrame") - .value("WORLD",::pinocchio::WORLD) - .value("LOCAL",::pinocchio::LOCAL) - .value("LOCAL_WORLD_ALIGNED",::pinocchio::LOCAL_WORLD_ALIGNED) - .export_values() - ; + if(!register_symbolic_link_to_registered_type<::pinocchio::ReferenceFrame>()) + { + bp::enum_< ::pinocchio::ReferenceFrame >("ReferenceFrame") + .value("WORLD",::pinocchio::WORLD) + .value("LOCAL",::pinocchio::LOCAL) + .value("LOCAL_WORLD_ALIGNED",::pinocchio::LOCAL_WORLD_ALIGNED) + .export_values() + ; + } - bp::enum_< ::pinocchio::KinematicLevel >("KinematicLevel") - .value("POSITION",::pinocchio::POSITION) - .value("VELOCITY",::pinocchio::VELOCITY) - .value("ACCELERATION",::pinocchio::ACCELERATION) - .export_values() - ; - - bp::enum_< ::pinocchio::ArgumentPosition>("ArgumentPosition") - .value("ARG0",::pinocchio::ARG0) - .value("ARG1",::pinocchio::ARG1) - .value("ARG2",::pinocchio::ARG2) - .value("ARG3",::pinocchio::ARG3) - .value("ARG4",::pinocchio::ARG4) - .export_values() - ; + if(!register_symbolic_link_to_registered_type<::pinocchio::KinematicLevel>()) + { + bp::enum_< ::pinocchio::KinematicLevel >("KinematicLevel") + .value("POSITION",::pinocchio::POSITION) + .value("VELOCITY",::pinocchio::VELOCITY) + .value("ACCELERATION",::pinocchio::ACCELERATION) + .export_values() + ; + } + + if(!register_symbolic_link_to_registered_type<::pinocchio::ArgumentPosition>()) + { + bp::enum_< ::pinocchio::ArgumentPosition>("ArgumentPosition") + .value("ARG0",::pinocchio::ARG0) + .value("ARG1",::pinocchio::ARG1) + .value("ARG2",::pinocchio::ARG2) + .value("ARG3",::pinocchio::ARG3) + .value("ARG4",::pinocchio::ARG4) + .export_values() + ; + } exposeModel(); exposeFrame(); exposeData(); exposeGeometry(); - + exposeAlgorithms(); exposeParsers(); exposeSerialization(); diff --git a/bindings/python/multibody/data.hpp b/bindings/python/multibody/data.hpp index 546b74d0f9..02abaac3ca 100644 --- a/bindings/python/multibody/data.hpp +++ b/bindings/python/multibody/data.hpp @@ -14,13 +14,15 @@ #include #include +#include "pinocchio/bindings/python/fwd.hpp" +#include "pinocchio/bindings/python/utils/macros.hpp" #include "pinocchio/bindings/python/serialization/serializable.hpp" #include "pinocchio/bindings/python/utils/std-vector.hpp" #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" -EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Data) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::Data) namespace pinocchio { @@ -66,29 +68,25 @@ namespace pinocchio } }; + template struct DataPythonVisitor - : public boost::python::def_visitor< DataPythonVisitor > + : public boost::python::def_visitor< DataPythonVisitor > { - typedef Data::Matrix6x Matrix6x; - typedef Data::Matrix3x Matrix3x; - typedef Data::Vector3 Vector3; + typedef typename Data::Matrix6 Matrix6; + typedef typename Data::Matrix6x Matrix6x; + typedef typename Data::Matrix3x Matrix3x; + typedef typename Data::Vector3 Vector3; public: -#define ADD_DATA_PROPERTY(NAME,DOC) \ - def_readwrite(#NAME, \ - &Data::NAME, \ - DOC) +#define ADD_DATA_PROPERTY(NAME,DOC) \ + PINOCCHIO_ADD_PROPERTY(Data,NAME,DOC) -#define ADD_DATA_PROPERTY_READONLY(NAME,DOC) \ - def_readonly(#NAME, \ - &Data::NAME, \ - DOC) +#define ADD_DATA_PROPERTY_READONLY(NAME,DOC) \ + PINOCCHIO_ADD_PROPERTY_READONLY(Data,NAME,DOC) -#define ADD_DATA_PROPERTY_READONLY_BYVALUE(NAME,DOC) \ - add_property(#NAME, \ - make_getter(&Data::NAME,bp::return_value_policy()), \ - DOC) +#define ADD_DATA_PROPERTY_READONLY_BYVALUE(NAME,DOC) \ + PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Data,NAME,DOC) /* --- Exposing C++ API to python through the handler ----------------- */ @@ -97,21 +95,24 @@ namespace pinocchio { cl .def(bp::init<>(bp::arg("self"),"Default constructor.")) - .def(bp::init(bp::arg("model"),"Constructs a data structure from a given model.")) + .def(bp::init(bp::arg("model"),"Constructs a data structure from a given model.")) - .ADD_DATA_PROPERTY(a,"Joint spatial acceleration") + .ADD_DATA_PROPERTY(joints,"Vector of JointData associated to each JointModel stored in the related model.") + .ADD_DATA_PROPERTY(a,"Vector of joint accelerations expressed in the local frame of the joint.") .ADD_DATA_PROPERTY(oa, "Joint spatial acceleration expressed at the origin of the world frame.") .ADD_DATA_PROPERTY(a_gf, "Joint spatial acceleration containing also the contribution of the gravity acceleration") .ADD_DATA_PROPERTY(oa_gf,"Joint spatial acceleration containing also the contribution of the gravity acceleration, but expressed at the origin of the world frame.") - .ADD_DATA_PROPERTY(v,"Joint spatial velocity expressed in the joint frame.") - .ADD_DATA_PROPERTY(ov,"Joint spatial velocity expressed at the origin of the world frame.") + .ADD_DATA_PROPERTY(v,"Vector of joint velocities expressed in the local frame of the joint.") + .ADD_DATA_PROPERTY(ov,"Vector of joint velocities expressed at the origin of the world.") - .ADD_DATA_PROPERTY(f,"Joint spatial force expresssed in the joint frame.") - .ADD_DATA_PROPERTY(of,"Joint spatial force expresssed at the origin of the world frame.") + .ADD_DATA_PROPERTY(f,"Vector of body forces expressed in the local frame of the joint.") + .ADD_DATA_PROPERTY(of,"Vector of body forces expressed at the origin of the world.") + .ADD_DATA_PROPERTY(of_augmented,"Vector of body forces expressed at the origin of the world, in the context of lagrangian formulation") .ADD_DATA_PROPERTY(h,"Vector of spatial momenta expressed in the local frame of the joint.") + .ADD_DATA_PROPERTY(oh,"Vector of spatial momenta expressed at the origin of the world.") .ADD_DATA_PROPERTY(oMi,"Body absolute placement (wrt world)") .ADD_DATA_PROPERTY(oMf,"frames absolute placement (wrt world)") .ADD_DATA_PROPERTY(liMi,"Body relative placement (wrt parent)") @@ -119,6 +120,9 @@ namespace pinocchio .ADD_DATA_PROPERTY(nle,"Non Linear Effects (output of nle algorithm)") .ADD_DATA_PROPERTY(ddq,"Joint accelerations (output of ABA)") .ADD_DATA_PROPERTY(Ycrb,"Inertia of the sub-tree composit rigid body") + .ADD_DATA_PROPERTY(oYcrb,"Composite Rigid Body Inertia of the sub-tree expressed in the WORLD coordinate system.") + .ADD_DATA_PROPERTY(Yaba,"Articulated Body Inertia of the sub-tree") + .ADD_DATA_PROPERTY(oYaba,"Articulated Body Inertia of the sub-tree expressed in the WORLD coordinate system.") .ADD_DATA_PROPERTY(M,"The joint space inertia matrix") .ADD_DATA_PROPERTY(Minv,"The inverse of the joint space inertia matrix") .ADD_DATA_PROPERTY(C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v") @@ -150,24 +154,46 @@ namespace pinocchio .ADD_DATA_PROPERTY(acom,"CoM acceleration of the subtree starting at joint index i.") .ADD_DATA_PROPERTY(mass,"Mass of the subtree starting at joint index i.") .ADD_DATA_PROPERTY(Jcom,"Jacobian of center of mass.") + + .ADD_DATA_PROPERTY(dAdq,"Variation of the spatial acceleration set with respect to the joint configuration.") + .ADD_DATA_PROPERTY(dAdv,"Variation of the spatial acceleration set with respect to the joint velocity.") + .ADD_DATA_PROPERTY(dHdq,"Variation of the spatial momenta set with respect to the joint configuration.") + .ADD_DATA_PROPERTY(dFdq,"Variation of the force set with respect to the joint configuration.") + .ADD_DATA_PROPERTY(dFdv,"Variation of the force set with respect to the joint velocity.") + .ADD_DATA_PROPERTY(dFda,"Variation of the force set with respect to the joint acceleration.") .ADD_DATA_PROPERTY(dtau_dq,"Partial derivative of the joint torque vector with respect to the joint configuration.") .ADD_DATA_PROPERTY(dtau_dv,"Partial derivative of the joint torque vector with respect to the joint velocity.") .ADD_DATA_PROPERTY(ddq_dq,"Partial derivative of the joint acceleration vector with respect to the joint configuration.") .ADD_DATA_PROPERTY(ddq_dv,"Partial derivative of the joint acceleration vector with respect to the joint velocity.") - + .ADD_DATA_PROPERTY(ddq_dtau,"Partial derivative of the joint acceleration vector with respect to the joint torque.") + .ADD_DATA_PROPERTY(dvc_dq,"Partial derivative of the constraint velocity vector with respect to the joint configuration.") + + .ADD_DATA_PROPERTY(dac_dq,"Partial derivative of the contact acceleration vector with respect to the joint configuration.") + .ADD_DATA_PROPERTY(dac_dv,"Partial derivative of the contact acceleration vector vector with respect to the joint velocity.") + .ADD_DATA_PROPERTY(dac_da,"Partial derivative of the contact acceleration vector vector with respect to the joint acceleration.") + + .ADD_DATA_PROPERTY(osim,"Operational space inertia matrix.") + + .ADD_DATA_PROPERTY_READONLY_BYVALUE(dlambda_dq,"Partial derivative of the contact force vector with respect to the joint configuration.") + .ADD_DATA_PROPERTY_READONLY_BYVALUE(dlambda_dv,"Partial derivative of the contact force vector with respect to the joint velocity.") + .ADD_DATA_PROPERTY_READONLY_BYVALUE(dlambda_dtau,"Partial derivative of the contact force vector with respect to the torque.") .ADD_DATA_PROPERTY(kinetic_energy,"Kinetic energy in [J] computed by computeKineticEnergy") .ADD_DATA_PROPERTY(potential_energy,"Potential energy in [J] computed by computePotentialEnergy") + .ADD_DATA_PROPERTY(mechanical_energy,"Mechanical energy in [J] of the system computed by computeMechanicalEnergy") .ADD_DATA_PROPERTY(lambda_c,"Lagrange Multipliers linked to contact forces") .ADD_DATA_PROPERTY(impulse_c,"Lagrange Multipliers linked to contact impulses") + .ADD_DATA_PROPERTY(contact_chol,"Contact Cholesky decomposition.") .ADD_DATA_PROPERTY(dq_after,"Generalized velocity after the impact.") .ADD_DATA_PROPERTY(staticRegressor,"Static regressor.") .ADD_DATA_PROPERTY(jointTorqueRegressor,"Joint torque regressor.") - + +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def(bp::self == bp::self) .def(bp::self != bp::self) +#endif ; } @@ -180,21 +206,28 @@ namespace pinocchio bp::no_init) .def(DataPythonVisitor()) .def(CopyableVisitor()) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION .def(SerializableVisitor()) - .def_pickle(PickleData()); + .def_pickle(PickleData()) +#endif + ; typedef PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) StdVec_Vector3; typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) StdVec_Matrix6x; + typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) StdVec_Matrix6; - StdAlignedVectorPythonVisitor::expose("StdVec_Vector3") - .def(details::overload_base_get_item_for_std_vector()); - serialize::vector_type>(); - - StdAlignedVectorPythonVisitor::expose("StdVec_Matrix6x") - .def(details::overload_base_get_item_for_std_vector()); - serialize::vector_type>(); - - StdVectorPythonVisitor::expose("StdVec_Int"); + StdAlignedVectorPythonVisitor::expose("StdVec_Vector3", + details::overload_base_get_item_for_std_vector()); + + StdAlignedVectorPythonVisitor::expose("StdVec_Matrix6x", + details::overload_base_get_item_for_std_vector()); + StdAlignedVectorPythonVisitor::expose("StdVec_Matrix6", + details::overload_base_get_item_for_std_vector()); + StdVectorPythonVisitor,true>::expose("StdVec_int"); +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + serialize::vector_type>(); + serialize::vector_type>(); +#endif serialize::vector_type>(); } @@ -202,4 +235,8 @@ namespace pinocchio }} // namespace pinocchio::python +#undef ADD_DATA_PROPERTY +#undef ADD_DATA_PROPERTY_READONLY +#undef ADD_DATA_PROPERTY_READONLY_BYVALUE + #endif // ifndef __pinocchio_python_multibody_data_hpp__ diff --git a/bindings/python/multibody/expose-data.cpp b/bindings/python/multibody/expose-data.cpp index ae587a41b7..b9e86024ff 100644 --- a/bindings/python/multibody/expose-data.cpp +++ b/bindings/python/multibody/expose-data.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2016 CNRS +// Copyright (c) 2015-2020 CNRS INRIA // #include "pinocchio/bindings/python/fwd.hpp" @@ -12,7 +12,7 @@ namespace pinocchio void exposeData() { - DataPythonVisitor::expose(); + DataPythonVisitor::expose(); } } // namespace python diff --git a/bindings/python/multibody/expose-frame.cpp b/bindings/python/multibody/expose-frame.cpp index f548e73fc3..646bb1ca6c 100644 --- a/bindings/python/multibody/expose-frame.cpp +++ b/bindings/python/multibody/expose-frame.cpp @@ -17,9 +17,11 @@ namespace pinocchio void exposeFrame() { - FramePythonVisitor::expose(); - StdAlignedVectorPythonVisitor::expose("StdVec_Frame"); - serialize::vector_type>(); + FramePythonVisitor::expose(); + StdAlignedVectorPythonVisitor::expose("StdVec_Frame"); +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + serialize::vector_type>(); +#endif } } // namespace python diff --git a/bindings/python/multibody/expose-liegroups.cpp b/bindings/python/multibody/expose-liegroups.cpp index 485d622fb6..fecc578f73 100644 --- a/bindings/python/multibody/expose-liegroups.cpp +++ b/bindings/python/multibody/expose-liegroups.cpp @@ -16,17 +16,17 @@ namespace python namespace bp = boost::python; template -CartesianProductOperationVariantTpl +CartesianProductOperationVariantTpl makeLieGroup() { - return CartesianProductOperationVariantTpl (LgType()); + return CartesianProductOperationVariantTpl (LgType()); } -CartesianProductOperationVariantTpl +CartesianProductOperationVariantTpl makeRn(int n) { - return CartesianProductOperationVariantTpl ( - VectorSpaceOperationTpl(n)); + return CartesianProductOperationVariantTpl ( + VectorSpaceOperationTpl(n)); } @@ -34,21 +34,21 @@ makeRn(int n) void exposeLieGroups() { LieGroupPythonVisitor< - CartesianProductOperationVariantTpl + CartesianProductOperationVariantTpl >::expose("LieGroup"); { // Switch the scope to the submodule, add methods and classes. bp::scope submoduleScope = getOrCreatePythonNamespace("liegroups"); - bp::def("R1", makeLieGroup >); - bp::def("R2", makeLieGroup >); - bp::def("R3", makeLieGroup >); + bp::def("R1", makeLieGroup >); + bp::def("R2", makeLieGroup >); + bp::def("R3", makeLieGroup >); bp::def("Rn", makeRn); - bp::def("SO2", makeLieGroup >); - bp::def("SO3", makeLieGroup >); - bp::def("SE2", makeLieGroup >); - bp::def("SE3", makeLieGroup >); + bp::def("SO2", makeLieGroup >); + bp::def("SO3", makeLieGroup >); + bp::def("SE2", makeLieGroup >); + bp::def("SE3", makeLieGroup >); } } } // namespace python diff --git a/bindings/python/multibody/expose-model.cpp b/bindings/python/multibody/expose-model.cpp index ac32a7a0d1..b3077e8ff9 100644 --- a/bindings/python/multibody/expose-model.cpp +++ b/bindings/python/multibody/expose-model.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2021 CNRS INRIA // #include "pinocchio/bindings/python/fwd.hpp" @@ -12,7 +12,7 @@ namespace pinocchio void exposeModel() { - ModelPythonVisitor::expose(); + ModelPythonVisitor::expose(); } } // namespace python diff --git a/bindings/python/multibody/fcl/expose-fcl.cpp b/bindings/python/multibody/fcl/expose-fcl.cpp index 2e83339e80..80b7e5540a 100644 --- a/bindings/python/multibody/fcl/expose-fcl.cpp +++ b/bindings/python/multibody/fcl/expose-fcl.cpp @@ -19,7 +19,7 @@ namespace pinocchio { namespace bp = boost::python; bp::import("hppfcl"); - + using namespace ::hpp::fcl; // Register implicit conversion SE3 <=> ::hpp::fcl::Transform3f @@ -34,8 +34,7 @@ namespace pinocchio serialize(); serialize(); serialize(); - serialize(); - + serialize(); serialize< BVHModel >(); serialize< BVHModel >(); serialize< BVHModel >(); diff --git a/bindings/python/multibody/frame.hpp b/bindings/python/multibody/frame.hpp index a80132a779..4f5edeb1a8 100644 --- a/bindings/python/multibody/frame.hpp +++ b/bindings/python/multibody/frame.hpp @@ -2,13 +2,16 @@ // Copyright (c) 2016-2021 CNRS INRIA // -#ifndef __pinocchio_python_frame_hpp__ -#define __pinocchio_python_frame_hpp__ +#ifndef __pinocchio_python_multibody_frame_hpp__ +#define __pinocchio_python_multibody_frame_hpp__ #include "pinocchio/multibody/fwd.hpp" #include "pinocchio/multibody/frame.hpp" + +#include "pinocchio/bindings/python/utils/cast.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" #include "pinocchio/bindings/python/utils/printable.hpp" +#include "pinocchio/bindings/python/utils/registration.hpp" namespace pinocchio { @@ -16,48 +19,60 @@ namespace pinocchio { namespace bp = boost::python; + template struct FramePythonVisitor - : public bp::def_visitor< FramePythonVisitor > + : public boost::python::def_visitor< FramePythonVisitor > { + typedef typename Frame::SE3 SE3; + typedef typename Frame::Inertia Inertia; + template - void visit(PyClass& cl) const + void visit(PyClass & cl) const { cl - .def(bp::init<>(bp::arg("self"),"Default constructor")) - .def(bp::init(bp::args("self","other"),"Copy constructor")) + .def(bp::init<>(bp::arg("self"),"Default constructor")) + .def(bp::init(bp::args("self","other"),"Copy constructor")) .def(bp::init< const std::string&,const JointIndex, const FrameIndex, const SE3&, FrameType, bp::optional > ((bp::arg("name"),bp::arg("parent_joint"), bp::args("parent_frame"), bp::arg("placement"), bp::arg("type"), bp::arg("inertia")), "Initialize from a given name, type, parent joint index, parent frame index and placement wrt parent joint and an spatial inertia object.")) + .def(bp::init((bp::arg("self"),bp::arg("clone")),"Copy constructor")) - .def_readwrite("name", &Frame::name, "name of the frame") - .def_readwrite("parent", &Frame::parent, "id of the parent joint") - .def_readwrite("previousFrame", &Frame::previousFrame, "id of the previous frame") - .def_readwrite("placement", - &Frame::placement, - "placement in the parent joint local frame") - .def_readwrite("type", &Frame::type, "Type of the frame") - .def_readwrite("inertia", &Frame::inertia,"Inertia information attached to the frame.") - - .def(bp::self == bp::self) - .def(bp::self != bp::self) - ; + .def_readwrite("name", &Frame::name, "name of the frame") + .def_readwrite("parent", &Frame::parent, "id of the parent joint") + .def_readwrite("previousFrame", &Frame::previousFrame, "id of the previous frame") + .def_readwrite("placement", + &Frame::placement, + "placement in the parent joint local frame") + .def_readwrite("type", &Frame::type, "type of the frame") + .def_readwrite("inertia", &Frame::inertia,"Inertia information attached to the frame.") + +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS + .def(bp::self == bp::self) + .def(bp::self != bp::self) +#endif + ; } static void expose() { - bp::enum_("FrameType") - .value("OP_FRAME",OP_FRAME) - .value("JOINT",JOINT) - .value("FIXED_JOINT",FIXED_JOINT) - .value("BODY",BODY) - .value("SENSOR",SENSOR) - .export_values() - ; + if(!register_symbolic_link_to_registered_type()) + { + bp::enum_("FrameType") + .value("OP_FRAME",OP_FRAME) + .value("JOINT",JOINT) + .value("FIXED_JOINT",FIXED_JOINT) + .value("BODY",BODY) + .value("SENSOR",SENSOR) + .export_values() + ; + } bp::class_("Frame", "A Plucker coordinate frame related to a parent joint inside a kinematic tree.\n\n", bp::no_init ) .def(FramePythonVisitor()) + .def(CastVisitor()) + .def(ExposeConstructorByCastVisitor()) .def(CopyableVisitor()) .def(PrintableVisitor()) .def_pickle(Pickle()) @@ -94,4 +109,4 @@ namespace pinocchio } // namespace python } // namespace pinocchio -#endif // ifndef __pinocchio_python_frame_hpp__ +#endif // ifndef __pinocchio_python_multibody_frame_hpp__ diff --git a/bindings/python/multibody/geometry-data.hpp b/bindings/python/multibody/geometry-data.hpp index 099efd74fc..e29d4aa3df 100644 --- a/bindings/python/multibody/geometry-data.hpp +++ b/bindings/python/multibody/geometry-data.hpp @@ -13,6 +13,7 @@ #include "pinocchio/bindings/python/utils/copyable.hpp" #include "pinocchio/bindings/python/utils/deprecation.hpp" #include "pinocchio/bindings/python/utils/std-vector.hpp" +#include "pinocchio/bindings/python/utils/registration.hpp" #include "pinocchio/bindings/python/serialization/serializable.hpp" EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::GeometryData) @@ -31,6 +32,8 @@ namespace pinocchio { static void expose() { + if(!register_symbolic_link_to_registered_type()) + { bp::class_ ("CollisionPair", "Pair of ordered index defining a pair of collisions", bp::no_init) @@ -46,9 +49,13 @@ namespace pinocchio .def(bp::self != bp::self) .def_readwrite("first",&CollisionPair::first) .def_readwrite("second",&CollisionPair::second); - + StdVectorPythonVisitor::expose("StdVec_CollisionPair"); +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION serialize< std::vector >(); +#endif + } + } }; // struct CollisionPairPythonVisitor @@ -137,14 +144,17 @@ namespace pinocchio /* --- Expose --------------------------------------------------------- */ static void expose() { - bp::class_("GeometryData", - "Geometry data linked to a Geometry Model and a Data struct.", - bp::no_init) - .def(GeometryDataPythonVisitor()) - .def(PrintableVisitor()) - .def(CopyableVisitor()) - .def(SerializableVisitor()) - ; + if(!register_symbolic_link_to_registered_type()) + { + bp::class_("GeometryData", + "Geometry data linked to a Geometry Model and a Data struct.", + bp::no_init) + .def(GeometryDataPythonVisitor()) + .def(PrintableVisitor()) + .def(CopyableVisitor()) + .def(SerializableVisitor()) + ; + } } diff --git a/bindings/python/multibody/geometry-model.hpp b/bindings/python/multibody/geometry-model.hpp index a7754a68c3..8413714028 100644 --- a/bindings/python/multibody/geometry-model.hpp +++ b/bindings/python/multibody/geometry-model.hpp @@ -9,6 +9,8 @@ #include "pinocchio/bindings/python/utils/printable.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" +#include "pinocchio/bindings/python/utils/registration.hpp" + #include "pinocchio/multibody/geometry.hpp" EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::GeometryModel) @@ -100,13 +102,16 @@ namespace pinocchio /* --- Expose --------------------------------------------------------- */ static void expose() { + if(!register_symbolic_link_to_registered_type()) + { bp::class_("GeometryModel", "Geometry model containing the collision or visual geometries associated to a model.", bp::no_init) .def(GeometryModelPythonVisitor()) .def(PrintableVisitor()) .def(CopyableVisitor()) - ; + ; + } } protected: diff --git a/bindings/python/multibody/geometry-object.hpp b/bindings/python/multibody/geometry-object.hpp index 0661601d2b..a840671775 100644 --- a/bindings/python/multibody/geometry-object.hpp +++ b/bindings/python/multibody/geometry-object.hpp @@ -8,6 +8,8 @@ #include #include #include +#include "pinocchio/bindings/python/utils/registration.hpp" + #include "pinocchio/multibody/geometry.hpp" @@ -94,18 +96,24 @@ namespace pinocchio static void expose() { - bp::class_("GeometryObject", - "A wrapper on a collision geometry including its parent joint, parent frame, placement in parent joint's frame.\n\n", - bp::no_init - ) - .def(GeometryObjectPythonVisitor()) - ; - - bp::enum_("GeometryType") - .value("VISUAL",VISUAL) - .value("COLLISION",COLLISION) - .export_values() - ; + if(!register_symbolic_link_to_registered_type()) + { + bp::class_("GeometryObject", + "A wrapper on a collision geometry including its parent joint, parent frame, placement in parent joint's frame.\n\n", + bp::no_init + ) + .def(GeometryObjectPythonVisitor()) + ; + } + + if(!register_symbolic_link_to_registered_type()) + { + bp::enum_("GeometryType") + .value("VISUAL",VISUAL) + .value("COLLISION",COLLISION) + .export_values() + ; + } } }; diff --git a/bindings/python/multibody/joint/expose-joints.cpp b/bindings/python/multibody/joint/expose-joints.cpp index 5bf6505e8e..b963e8af34 100644 --- a/bindings/python/multibody/joint/expose-joints.cpp +++ b/bindings/python/multibody/joint/expose-joints.cpp @@ -5,8 +5,8 @@ #include "pinocchio/bindings/python/fwd.hpp" #include "pinocchio/bindings/python/multibody/joint/joint-derived.hpp" #include "pinocchio/bindings/python/multibody/joint/joints-variant.hpp" -#include "pinocchio/bindings/python/multibody/joint/joint.hpp" - +#include "pinocchio/bindings/python/multibody/joint/joint-model.hpp" +#include "pinocchio/bindings/python/multibody/joint/joint-data.hpp" #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" namespace pinocchio @@ -14,13 +14,26 @@ namespace pinocchio namespace python { - void exposeJoints() + static void exposeJointVariants() { + typedef context::JointCollectionDefault::JointModelVariant JointModelVariant; boost::mpl::for_each(JointModelExposer()); + bp::to_python_converter >(); + + typedef context::JointCollectionDefault::JointDataVariant JointDataVariant; boost::mpl::for_each(JointDataExposer()); + bp::to_python_converter >(); + } + + void exposeJoints() + { + exposeJointVariants(); + + JointModelPythonVisitor::expose(); + StdAlignedVectorPythonVisitor::expose("StdVec_JointModelVector"); - JointModelPythonVisitor::expose(); - StdAlignedVectorPythonVisitor::expose("StdVec_JointModelVector"); + JointDataPythonVisitor::expose(); + StdAlignedVectorPythonVisitor::expose("StdVec_JointDataVector"); } } // namespace python diff --git a/bindings/python/multibody/joint/joint-data.hpp b/bindings/python/multibody/joint/joint-data.hpp new file mode 100644 index 0000000000..5f9cffd0aa --- /dev/null +++ b/bindings/python/multibody/joint/joint-data.hpp @@ -0,0 +1,39 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_python_multibody_joint_joint_data_hpp__ +#define __pinocchio_python_multibody_joint_joint_data_hpp__ + +#include + +#include "pinocchio/multibody/joint/joint-generic.hpp" +#include "pinocchio/bindings/python/multibody/joint/joint-derived.hpp" +#include "pinocchio/bindings/python/utils/printable.hpp" + +namespace pinocchio +{ + namespace python + { + namespace bp = boost::python; + + template + struct JointDataPythonVisitor + { + + static void expose() + { + bp::class_("JointData", + "Generic Joint Data", + bp::no_init) + .def(bp::init(bp::args("self","joint_data"))) + .def(JointDataBasePythonVisitor()) + .def(PrintableVisitor()) + ; + } + + }; + +}} // namespace pinocchio::python + +#endif // ifndef __pinocchio_python_multibody_joint_joint_data_hpp__ diff --git a/bindings/python/multibody/joint/joint-derived.hpp b/bindings/python/multibody/joint/joint-derived.hpp index af58dfd381..f8f73718f7 100644 --- a/bindings/python/multibody/joint/joint-derived.hpp +++ b/bindings/python/multibody/joint/joint-derived.hpp @@ -2,12 +2,13 @@ // Copyright (c) 2015-2021 CNRS INRIA // -#ifndef __pinocchio_python_joint_variants_hpp__ -#define __pinocchio_python_joint_variants_hpp__ +#ifndef __pinocchio_python_multibody_joint_joint_derived_hpp__ +#define __pinocchio_python_multibody_joint_joint_derived_hpp__ #include #include +#include "pinocchio/bindings/python/fwd.hpp" #include "pinocchio/multibody/joint/joint-collection.hpp" namespace pinocchio @@ -17,13 +18,15 @@ namespace pinocchio namespace bp = boost::python; template - struct JointModelDerivedPythonVisitor - : public boost::python::def_visitor< JointModelDerivedPythonVisitor > + struct JointModelBasePythonVisitor + : public boost::python::def_visitor< JointModelBasePythonVisitor > { public: + typedef typename JointModelDerived::JointDataDerived JointDataDerived; + template - void visit(PyClass& cl) const + void visit(PyClass & cl) const { cl .def(bp::init<>(bp::arg("self"))) @@ -35,15 +38,25 @@ namespace pinocchio .add_property("nv",&get_nv) .def("setIndexes", &JointModelDerived::setIndexes, - bp::args("self","id","idx_q","idx_v")) + bp::args("self","joint_id","idx_q","idx_v")) + .def("shortname", + &JointModelDerived::shortname, + bp::arg("self")) + .def("classname",&JointModelDerived::classname) + .staticmethod("classname") + .def("calc",&calc0, + bp::args("self","jdata","q")) + .def("calc",&calc1, + bp::args("self","jdata","q","v")) .def("hasSameIndexes", &JointModelDerived::template hasSameIndexes, bp::args("self","other"), "Check if this has same indexes than other.") - .def("shortname",&JointModelDerived::shortname,bp::arg("self")) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION .def(bp::self == bp::self) .def(bp::self != bp::self) +#endif ; } @@ -57,20 +70,27 @@ namespace pinocchio { return self.nq(); } static int get_nv(const JointModelDerived & self) { return self.nv(); } - + static void calc0(const JointModelDerived & self, JointDataDerived & jdata, + const context::VectorXs & q) + { self.calc(jdata,q); } + static void calc1(const JointModelDerived & self, JointDataDerived & jdata, + const context::VectorXs & q, const context::VectorXs & v) + { self.calc(jdata,q,v); } }; template - struct JointDataDerivedPythonVisitor - : public boost::python::def_visitor< JointDataDerivedPythonVisitor > + struct JointDataBasePythonVisitor + : public boost::python::def_visitor< JointDataBasePythonVisitor > { public: template - void visit(PyClass& cl) const + void visit(PyClass & cl) const { cl // All are add_properties cause ReadOnly + .add_property("joint_q",&get_joint_q) + .add_property("joint_v",&get_joint_v) .add_property("S",&get_S) .add_property("M",&get_M) .add_property("v",&get_v) @@ -78,15 +98,25 @@ namespace pinocchio .add_property("U",&get_U) .add_property("Dinv",&get_Dinv) .add_property("UDinv",&get_UDinv) - .def("shortname",&JointDataDerived::shortname) + .def("shortname", + &JointDataDerived::shortname, + bp::arg("self")) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION .def(bp::self == bp::self) .def(bp::self != bp::self) +#endif ; } - static typename JointDataDerived::Constraint_t get_S(const JointDataDerived & self) - { return self.S_accessor(); } + static typename JointDataDerived::ConfigVector_t get_joint_q(const JointDataDerived & self ) + { return self.joint_q_accessor(); } + static typename JointDataDerived::TangentVector_t get_joint_v(const JointDataDerived & self ) + { return self.joint_v_accessor(); } +// static typename JointDataDerived::Constraint_t get_S(const JointDataDerived & self) +// { return self.S_accessor(); } + static typename JointDataDerived::Constraint_t::DenseBase get_S(const JointDataDerived & self) + { return self.S_accessor().matrix(); } static typename JointDataDerived::Transformation_t get_M(const JointDataDerived & self) { return self.M_accessor(); } static typename JointDataDerived::Motion_t get_v(const JointDataDerived & self) @@ -107,4 +137,4 @@ namespace pinocchio }} // namespace pinocchio::python -#endif // ifndef __pinocchio_python_joint_variants_hpp__ +#endif // ifndef __pinocchio_python_multibody_joint_joint_derived_hpp__ diff --git a/bindings/python/multibody/joint/joint-model.hpp b/bindings/python/multibody/joint/joint-model.hpp new file mode 100644 index 0000000000..ede3a50cd2 --- /dev/null +++ b/bindings/python/multibody/joint/joint-model.hpp @@ -0,0 +1,39 @@ +// +// Copyright (c) 2015-2021 CNRS INRIA +// + +#ifndef __pinocchio_python_multibody_joint_joint_model_hpp__ +#define __pinocchio_python_multibody_joint_joint_model_hpp__ + +#include + +#include "pinocchio/multibody/joint/joint-generic.hpp" +#include "pinocchio/bindings/python/multibody/joint/joint-derived.hpp" +#include "pinocchio/bindings/python/utils/printable.hpp" + +namespace pinocchio +{ + namespace python + { + namespace bp = boost::python; + + template + struct JointModelPythonVisitor + { + + static void expose() + { + bp::class_("JointModel", + "Generic Joint Model", + bp::no_init) + .def(bp::init(bp::args("self","other"))) + .def(JointModelBasePythonVisitor()) + .def(PrintableVisitor()) + ; + } + + }; + +}} // namespace pinocchio::python + +#endif // ifndef __pinocchio_python_multibody_joint_joint_model_hpp__ diff --git a/bindings/python/multibody/joint/joint.hpp b/bindings/python/multibody/joint/joint.hpp deleted file mode 100644 index 3f78d837b2..0000000000 --- a/bindings/python/multibody/joint/joint.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// -// Copyright (c) 2015-2021 CNRS INRIA -// - -#ifndef __pinocchio_python_multibody_joint_joint_hpp__ -#define __pinocchio_python_multibody_joint_joint_hpp__ - -#include - -#include "pinocchio/multibody/joint/joint-generic.hpp" -#include "pinocchio/bindings/python/utils/printable.hpp" - -namespace pinocchio -{ - namespace python - { - namespace bp = boost::python; - - struct JointModelPythonVisitor - : public boost::python::def_visitor< JointModelPythonVisitor > - { - - template - void visit(PyClass& cl) const - { - cl - .def(bp::init<>(bp::arg("self"))) - // All are add_properties cause ReadOnly - .add_property("id",&getId) - .add_property("idx_q",&getIdx_q) - .add_property("idx_v",&getIdx_v) - .add_property("nq",&getNq) - .add_property("nv",&getNv) - .def("setIndexes", - &JointModel::setIndexes, - bp::args("self","id","idx_q","idx_v")) - .def("hasSameIndexes", - &JointModel::hasSameIndexes, - bp::args("self","other"), - "Check if this has same indexes than other.") - .def("shortname",&JointModel::shortname,bp::arg("self")) - - .def(bp::self == bp::self) - .def(bp::self != bp::self) - ; - } - - static JointIndex getId( const JointModel & self ) { return self.id(); } - static int getIdx_q(const JointModel & self) {return self.idx_q();} - static int getIdx_v(const JointModel & self) {return self.idx_v();} - static int getNq(const JointModel & self) {return self.nq();} - static int getNv(const JointModel & self) {return self.nv();} - - static void expose() - { - bp::class_("JointModel", - "Generic Joint Model", - bp::no_init) - .def(bp::init(bp::args("self","other"))) - .def(JointModelPythonVisitor()) - .def(PrintableVisitor()) - ; - } - - }; - -}} // namespace pinocchio::python - -#endif // ifndef __pinocchio_python_multibody_joint_joint_hpp__ diff --git a/bindings/python/multibody/joint/joints-models.hpp b/bindings/python/multibody/joint/joints-models.hpp index fb8ca05664..218ca9bd0e 100644 --- a/bindings/python/multibody/joint/joints-models.hpp +++ b/bindings/python/multibody/joint/joints-models.hpp @@ -32,7 +32,7 @@ namespace pinocchio inline bp::class_& expose_joint_model (bp::class_ & cl) { return cl - .def(bp::init (bp::args("self","x", "y", "z"), + .def(bp::init (bp::args("self","x", "y", "z"), "Init JointModelRevoluteUnaligned from the components x, y, z of the axis")) .def(bp::init (bp::args("self","axis"), "Init JointModelRevoluteUnaligned from an axis with x-y-z components")) @@ -46,13 +46,13 @@ namespace pinocchio inline bp::class_& expose_joint_model (bp::class_ & cl) { return cl - .def(bp::init (bp::args("self","x", "y", "z"), - "Init JointModelPrismaticUnaligned from the components x, y, z of the axis")) - .def(bp::init (bp::args("self","axis"), - "Init JointModelPrismaticUnaligned from an axis with x-y-z components")) - .def_readwrite("axis",&JointModelPrismaticUnaligned::axis, - "Translation axis of the JointModelPrismaticUnaligned.") - ; + .def(bp::init (bp::args("self","x", "y", "z"), + "Init JointModelPrismaticUnaligned from the components x, y, z of the axis")) + .def(bp::init (bp::args("self","axis"), + "Init JointModelPrismaticUnaligned from an axis with x-y-z components")) + .def_readwrite("axis",&JointModelPrismaticUnaligned::axis, + "Translation axis of the JointModelPrismaticUnaligned.") + ; } // specialization for JointModelComposite @@ -143,8 +143,10 @@ namespace pinocchio )[bp::return_internal_reference<>()] ) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION .def(bp::self == bp::self) .def(bp::self != bp::self) +#endif ; } diff --git a/bindings/python/multibody/joint/joints-variant.hpp b/bindings/python/multibody/joint/joints-variant.hpp index c011adbf20..3b5a1bd557 100644 --- a/bindings/python/multibody/joint/joints-variant.hpp +++ b/bindings/python/multibody/joint/joints-variant.hpp @@ -42,10 +42,10 @@ namespace pinocchio bp::class_(T::classname().c_str(), T::classname().c_str(), bp::init<>()) - .def(JointDataDerivedPythonVisitor()) + .def(JointDataBasePythonVisitor()) .def(PrintableVisitor()) ); - bp::implicitly_convertible(); + bp::implicitly_convertible(); } }; @@ -58,10 +58,10 @@ namespace pinocchio bp::class_(T::classname().c_str(), T::classname().c_str(), bp::no_init) - .def(JointModelDerivedPythonVisitor()) + .def(JointModelBasePythonVisitor()) .def(PrintableVisitor()) ); - bp::implicitly_convertible(); + bp::implicitly_convertible(); } }; diff --git a/bindings/python/multibody/liegroups.hpp b/bindings/python/multibody/liegroups.hpp index 412d61996a..d0e137a2b7 100644 --- a/bindings/python/multibody/liegroups.hpp +++ b/bindings/python/multibody/liegroups.hpp @@ -21,9 +21,9 @@ namespace bp = boost::python; template struct LieGroupWrapperTpl { - typedef Eigen::Matrix ConfigVector_t; - typedef Eigen::Matrix TangentVector_t; - typedef Eigen::Matrix JacobianMatrix_t; + typedef Eigen::Matrix ConfigVector_t; + typedef Eigen::Matrix TangentVector_t; + typedef Eigen::Matrix JacobianMatrix_t; static ConfigVector_t integrate(const LieGroupType& lg, const ConfigVector_t& q, const TangentVector_t& v) @@ -178,7 +178,7 @@ struct LieGroupPythonVisitor template void visit(PyClass& cl) const { - typedef Eigen::Matrix ConfigVector_t; + typedef Eigen::Matrix ConfigVector_t; typedef LieGroupWrapperTpl LieGroupWrapper; cl @@ -213,7 +213,9 @@ struct LieGroupPythonVisitor .def(bp::self * bp::self) .def(bp::self *= bp::self) +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def(bp::self == bp::self) +#endif ; } diff --git a/bindings/python/multibody/model.hpp b/bindings/python/multibody/model.hpp index 8f4c4cff64..393e399c57 100644 --- a/bindings/python/multibody/model.hpp +++ b/bindings/python/multibody/model.hpp @@ -16,6 +16,7 @@ #include #include "pinocchio/algorithm/check.hpp" +#include "pinocchio/bindings/python/utils/cast.hpp" #include "pinocchio/bindings/python/serialization/serializable.hpp" #include "pinocchio/bindings/python/utils/printable.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" @@ -23,7 +24,7 @@ #include "pinocchio/bindings/python/utils/pickle-map.hpp" #include "pinocchio/bindings/python/utils/std-vector.hpp" -EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Model) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::Model) namespace pinocchio { @@ -31,10 +32,6 @@ namespace pinocchio { namespace bp = boost::python; - BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getFrameId_overload,Model::getFrameId,1,2) - BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(existFrame_overload,Model::existFrame,1,2) - BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addJointFrame_overload,Model::addJointFrame,1,2) - template struct PickleModel : bp::pickle_suite { @@ -82,6 +79,8 @@ namespace pinocchio typedef typename Model::Index Index; typedef typename Model::JointIndex JointIndex; + typedef typename Model::JointModel JointModel; + typedef typename Model::JointModel::JointModelVariant JointModelVariant; typedef typename Model::FrameIndex FrameIndex; typedef typename Model::IndexVector IndexVector; @@ -94,8 +93,12 @@ namespace pinocchio typedef typename Model::Data Data; typedef typename Model::VectorXs VectorXs; + + BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addFrame_overload,Model::addFrame,1,2) + BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getFrameId_overload,Model::getFrameId,1,2) + BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(existFrame_overload,Model::existFrame,1,2) + BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addJointFrame_overload,Model::addJointFrame,1,2) - BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addFrame_overload,Model::addFrame,1,2) public: @@ -106,6 +109,7 @@ namespace pinocchio cl .def(bp::init<>(bp::arg("self"), "Default constructor. Constructs an empty model.")) + .def(bp::init((bp::arg("self"),bp::arg("clone")),"Copy constructor")) // Class Members .add_property("nq", &Model::nq) @@ -121,10 +125,13 @@ namespace pinocchio .add_property("idx_vs",&Model::idx_vs) .add_property("nvs",&Model::nvs) .add_property("parents",&Model::parents) + .add_property("children",&Model::children) .add_property("names",&Model::names) .def_readwrite("name",&Model::name) .def_readwrite("referenceConfigurations", &Model::referenceConfigurations) + .def_readwrite("armature",&Model::armature, + "Armature vector.") .def_readwrite("rotorInertia",&Model::rotorInertia, "Vector of rotor inertia parameters.") .def_readwrite("rotorGearRatio",&Model::rotorGearRatio, @@ -199,12 +206,13 @@ namespace pinocchio .def("createData", &ModelPythonVisitor::createData,bp::arg("self"), "Create a Data object for the given model.") - .def("check",(bool (Model::*)(const Data &) const) &Model::check,bp::args("self","data"), "Check consistency of data wrt model.") - + +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def(bp::self == bp::self) .def(bp::self != bp::self) +#endif ; } @@ -281,15 +289,18 @@ namespace pinocchio typedef typename Model::ConfigVectorMap ConfigVectorMap; typedef bp::map_indexing_suite map_indexing_suite; StdVectorPythonVisitor::expose("StdVec_Index"); - serialize< std::vector >(); StdVectorPythonVisitor::expose("StdVec_IndexVector"); - serialize< std::vector >(); StdVectorPythonVisitor::expose("StdVec_StdString"); - serialize< std::vector >(); StdVectorPythonVisitor::expose("StdVec_Bool"); - serialize< std::vector >(); StdVectorPythonVisitor::expose("StdVec_Double"); + +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + serialize< std::vector >(); + serialize< std::vector >(); + serialize< std::vector >(); + serialize< std::vector >(); serialize< std::vector >(); +#endif bp::class_("StdMap_String_VectorXd") .def(map_indexing_suite()) .def_pickle(PickleMap()) @@ -299,10 +310,14 @@ namespace pinocchio "Articulated Rigid Body model", bp::no_init) .def(ModelPythonVisitor()) - .def(SerializableVisitor()) + .def(CastVisitor()) + .def(ExposeConstructorByCastVisitor()) .def(PrintableVisitor()) .def(CopyableVisitor()) - .def_pickle(PickleModel()) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + .def(SerializableVisitor()) + .def_pickle(PickleModel()) +#endif ; } }; diff --git a/bindings/python/parsers/expose-parsers.cpp b/bindings/python/parsers/expose-parsers.cpp index 34bf0ed097..69f1921ad4 100644 --- a/bindings/python/parsers/expose-parsers.cpp +++ b/bindings/python/parsers/expose-parsers.cpp @@ -3,6 +3,7 @@ // #include "pinocchio/bindings/python/parsers/urdf.hpp" +#include "pinocchio/bindings/python/parsers/sdf.hpp" #include "pinocchio/bindings/python/parsers/srdf.hpp" #include "pinocchio/bindings/python/parsers/sample-models.hpp" @@ -13,6 +14,7 @@ namespace pinocchio void exposeParsers() { + exposeSDFParser(); exposeURDFParser(); exposeSRDFParser(); exposeSampleModels(); diff --git a/bindings/python/parsers/sdf.hpp b/bindings/python/parsers/sdf.hpp new file mode 100644 index 0000000000..d3ebd5a7ae --- /dev/null +++ b/bindings/python/parsers/sdf.hpp @@ -0,0 +1,23 @@ +// +// Copyright (c) 2020 CNRS +// + +#ifndef __pinocchio_python_parsers_sdf_hpp__ +#define __pinocchio_python_parsers_sdf_hpp__ + +namespace pinocchio +{ + namespace python + { + void exposeSDFModel(); + void exposeSDFGeometry(); + + inline void exposeSDFParser() + { + exposeSDFModel(); + exposeSDFGeometry(); + } + } +} + +#endif // ifndef __pinocchio_python_parsers_sdf_hpp__ diff --git a/bindings/python/parsers/sdf/geometry.cpp b/bindings/python/parsers/sdf/geometry.cpp new file mode 100644 index 0000000000..a32cf5b239 --- /dev/null +++ b/bindings/python/parsers/sdf/geometry.cpp @@ -0,0 +1,56 @@ +// +// Copyright (c) 2020-2021 CNRS INRIA +// + +#include "pinocchio/parsers/sdf.hpp" +#include "pinocchio/bindings/python/parsers/sdf.hpp" + +#include + +namespace pinocchio +{ + namespace python + { + + namespace bp = boost::python; + +#ifdef PINOCCHIO_WITH_SDF +#ifdef PINOCCHIO_WITH_HPP_FCL + + GeometryModel + buildGeomFromSdf(Model & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const std::string & filename, + const GeometryType type, + const std::string & packageDir) + { + GeometryModel geometry_model; + const std::vector dirs(1,packageDir); + pinocchio::sdf::buildGeom(model,contact_models, + filename,type,geometry_model,dirs); + + return geometry_model; + } +#endif // #ifdef PINOCCHIO_WITH_HPP_FCL +#endif // #ifdef PINOCCHIO_WITH_SDF + + void exposeSDFGeometry() + { +#ifdef PINOCCHIO_WITH_SDF +#ifdef PINOCCHIO_WITH_HPP_FCL + bp::def("buildGeomFromSdf", + static_cast (pinocchio::python::buildGeomFromSdf), + bp::args("model","contact_models","urdf_filename","geom_type","package_dir" ), + "Parse the URDF file given as input looking for the geometry of the given input model and\n" + "return a GeometryModel containing either the collision geometries (GeometryType.COLLISION) or the visual geometries (GeometryType.VISUAL).\n" + "Parameters:\n" + "\tmodel: model of the robot\n" + "\turdf_filename: path to the URDF file containing the model of the robot\n" + "\tgeom_type: type of geometry to extract from the URDF file (either the VISUAL for display or the COLLISION for collision detection).\n" + "\tpackage_dir: path pointing to the folder containing the meshes of the robot\n" + ); +#endif // #ifdef PINOCCHIO_WITH_HPP_FCL +#endif + } + } +} diff --git a/bindings/python/parsers/sdf/model.cpp b/bindings/python/parsers/sdf/model.cpp new file mode 100644 index 0000000000..4ac523a3a2 --- /dev/null +++ b/bindings/python/parsers/sdf/model.cpp @@ -0,0 +1,60 @@ +// +// Copyright (c) 2021 CNRS +// + +#include "pinocchio/parsers/sdf.hpp" +#include "pinocchio/bindings/python/parsers/sdf.hpp" + +#include +#include + +namespace pinocchio +{ + namespace python + { + + namespace bp = boost::python; +#ifdef PINOCCHIO_WITH_SDF +#ifdef PINOCCHIO_WITH_HPP_FCL + bp::tuple buildModelFromSdf(const std::string & filename) + { + Model model; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + ::pinocchio::sdf::buildModel(filename, model, contact_models); + return bp::make_tuple(model,contact_models); + } + + bp::tuple buildModelFromSdf(const std::string & filename, + const bp::object & root_joint_object) + { + JointModelVariant root_joint = bp::extract(root_joint_object)(); + Model model; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + pinocchio::sdf::buildModel(filename, root_joint, model, contact_models); + return bp::make_tuple(model,contact_models); + } +#endif // #ifdef PINOCCHIO_WITH_HPP_FCL +#endif + + void exposeSDFModel() + { + +#ifdef PINOCCHIO_WITH_SDF +#ifdef PINOCCHIO_WITH_HPP_FCL + bp::def("buildModelFromSdf", + static_cast (pinocchio::python::buildModelFromSdf), + bp::args("sdf_filename"), + "Parse the SDF file given in input and return a pinocchio Model." + ); + + bp::def("buildModelFromSdf", + static_cast (pinocchio::python::buildModelFromSdf), + bp::args("sdf_filename","root_joint"), + "Append to a given model a SDF structure given by its filename and the root joint." + ); +#endif // #ifdef PINOCCHIO_WITH_HPP_FCL +#endif + + } + } +} diff --git a/bindings/python/parsers/srdf.cpp b/bindings/python/parsers/srdf.cpp index d90e1bc9dc..d6c68eeac8 100644 --- a/bindings/python/parsers/srdf.cpp +++ b/bindings/python/parsers/srdf.cpp @@ -92,6 +92,7 @@ namespace pinocchio loadRotorParameters_overload(bp::args("model","srdf_filename","verbose"), "Load the rotor parameters of a given model from a SRDF file.\n" "Results are stored in model.rotorInertia and model.rotorGearRatio." + "This function also fills the armature of the model." "Parameters:\n" "\tmodel: model of the robot\n" "\tsrdf_filename: path to the SRDF file containing the rotor parameters\n" diff --git a/bindings/python/parsers/urdf.hpp b/bindings/python/parsers/urdf.hpp index 8856594867..f59625eabf 100644 --- a/bindings/python/parsers/urdf.hpp +++ b/bindings/python/parsers/urdf.hpp @@ -9,11 +9,13 @@ namespace pinocchio { namespace python { + void exposeConsoleBridge(); void exposeURDFModel(); void exposeURDFGeometry(); inline void exposeURDFParser() { + exposeConsoleBridge(); exposeURDFModel(); exposeURDFGeometry(); } diff --git a/bindings/python/parsers/urdf/console-bridge.cpp b/bindings/python/parsers/urdf/console-bridge.cpp new file mode 100644 index 0000000000..2da9c4fd9b --- /dev/null +++ b/bindings/python/parsers/urdf/console-bridge.cpp @@ -0,0 +1,41 @@ +// +// Copyright (c) 2020 INRIA +// + +#include +#include "pinocchio/bindings/python/parsers/urdf.hpp" +#include "pinocchio/bindings/python/utils/registration.hpp" + +#ifdef PINOCCHIO_WITH_URDFDOM + #include +#endif + +namespace pinocchio +{ + namespace python + { + void exposeConsoleBridge() + { + namespace bp = boost::python; + +#ifdef PINOCCHIO_WITH_URDFDOM + + // fix CONSOLE_BRIDGE warning level + ::console_bridge::setLogLevel(::console_bridge::CONSOLE_BRIDGE_LOG_ERROR); + + typedef ::console_bridge::LogLevel LogLevel; + if(!register_symbolic_link_to_registered_type()) + { + bp::enum_("LogLevel") + .value("CONSOLE_BRIDGE_LOG_DEBUG",::console_bridge::CONSOLE_BRIDGE_LOG_DEBUG) + .value("CONSOLE_BRIDGE_LOG_INFO" ,::console_bridge::CONSOLE_BRIDGE_LOG_INFO ) + .value("CONSOLE_BRIDGE_LOG_WARN" ,::console_bridge::CONSOLE_BRIDGE_LOG_WARN ) + .value("CONSOLE_BRIDGE_LOG_ERROR",::console_bridge::CONSOLE_BRIDGE_LOG_ERROR) + .value("CONSOLE_BRIDGE_LOG_NONE" ,::console_bridge::CONSOLE_BRIDGE_LOG_NONE ) + ; + } + +#endif + } + } +} diff --git a/bindings/python/pinocchio/__init__.py b/bindings/python/pinocchio/__init__.py index 1354d404a4..44b4d484c6 100644 --- a/bindings/python/pinocchio/__init__.py +++ b/bindings/python/pinocchio/__init__.py @@ -3,9 +3,9 @@ # import numpy -from .pinocchio_pywrap import * +from .pinocchio_pywrap_default import * -from .pinocchio_pywrap import __version__, __raw_version__ +from .pinocchio_pywrap_default import __version__, __raw_version__ from . import utils from .explog import exp, log @@ -13,10 +13,13 @@ # Manually register submodules import sys, inspect -submodules = inspect.getmembers(pinocchio_pywrap, inspect.ismodule) +submodules = inspect.getmembers(pinocchio_pywrap_default, inspect.ismodule) for module_info in submodules: sys.modules['pinocchio.' + module_info[0]] = module_info[1] +sys.modules['pinocchio.rpy'] = rpy +sys.modules['pinocchio.cholesky'] = cholesky + if WITH_HPP_FCL: try: import hppfcl diff --git a/bindings/python/pinocchio/casadi/__init__.py b/bindings/python/pinocchio/casadi/__init__.py new file mode 100644 index 0000000000..5b0e58de89 --- /dev/null +++ b/bindings/python/pinocchio/casadi/__init__.py @@ -0,0 +1,30 @@ +# +# Copyright (c) 2020 INRIA +# + +from ..pinocchio_pywrap_casadi import * + +from ..pinocchio_pywrap_casadi import __version__, __raw_version__ + +from .. import utils +from ..explog import exp, log + +# Manually register submodules +import sys +sys.modules['pinocchio.casadi.rpy'] = rpy +sys.modules['pinocchio.casadi.cholesky'] = cholesky + +if WITH_HPP_FCL: + try: + import hppfcl + from hppfcl import Contact, StdVec_Contact, CollisionResult, StdVec_CollisionResult, DistanceResult, StdVec_DistanceResult, CollisionGeometry, MeshLoader, CachedMeshLoader + WITH_HPP_FCL_BINDINGS = True + except ImportError: + WITH_HPP_FCL_BINDINGS = False +else: + WITH_HPP_FCL_BINDINGS = False + +from ..robot_wrapper import RobotWrapper +from ..deprecated import * +from ..shortcuts import * +from .. import visualize diff --git a/bindings/python/pinocchio/cppad/__init__.py b/bindings/python/pinocchio/cppad/__init__.py new file mode 100644 index 0000000000..6ddb456631 --- /dev/null +++ b/bindings/python/pinocchio/cppad/__init__.py @@ -0,0 +1,30 @@ +# +# Copyright (c) 2020-2021 INRIA +# + +from ..pinocchio_pywrap_cppad import * + +from ..pinocchio_pywrap_cppad import __version__, __raw_version__ + +from .. import utils +from ..explog import exp, log + +# Manually register submodules +import sys +sys.modules['pinocchio.cppad.rpy'] = rpy +sys.modules['pinocchio.cppad.cholesky'] = cholesky + +if WITH_HPP_FCL: + try: + import hppfcl + from hppfcl import Contact, StdVec_Contact, CollisionResult, StdVec_CollisionResult, DistanceResult, StdVec_DistanceResult, CollisionGeometry, MeshLoader, CachedMeshLoader + WITH_HPP_FCL_BINDINGS = True + except ImportError: + WITH_HPP_FCL_BINDINGS = False +else: + WITH_HPP_FCL_BINDINGS = False + +from ..robot_wrapper import RobotWrapper +from ..deprecated import * +from ..shortcuts import * +from .. import visualize diff --git a/bindings/python/pinocchio/deprecated.py b/bindings/python/pinocchio/deprecated.py index 14c289f164..8cb9536330 100644 --- a/bindings/python/pinocchio/deprecated.py +++ b/bindings/python/pinocchio/deprecated.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2018-2020 CNRS INRIA +# Copyright (c) 2018-2021 CNRS INRIA # ## In this file, are reported some deprecated functions that are still maintained until the next important future releases ## @@ -8,7 +8,7 @@ import warnings as _warnings -from . import pinocchio_pywrap as pin +from . import pinocchio_pywrap_default as pin from .deprecation import deprecated, DeprecatedWarning # This function is only deprecated when using a specific signature. Therefore, it needs special care diff --git a/bindings/python/pinocchio/explog.py b/bindings/python/pinocchio/explog.py index ef8075d908..3ce114dab0 100644 --- a/bindings/python/pinocchio/explog.py +++ b/bindings/python/pinocchio/explog.py @@ -7,7 +7,7 @@ import numpy as np -from . import pinocchio_pywrap as pin +from . import pinocchio_pywrap_default as pin def exp(x): if isinstance(x, pin.Motion): diff --git a/bindings/python/pinocchio/robot_wrapper.py b/bindings/python/pinocchio/robot_wrapper.py index 1f8f376323..ab7efc9775 100644 --- a/bindings/python/pinocchio/robot_wrapper.py +++ b/bindings/python/pinocchio/robot_wrapper.py @@ -2,7 +2,7 @@ # Copyright (c) 2015-2020 CNRS INRIA # -from . import pinocchio_pywrap as pin +from . import pinocchio_pywrap_default as pin from . import utils from .deprecation import deprecated from .shortcuts import buildModelsFromUrdf, createDatas diff --git a/bindings/python/pinocchio/romeo_wrapper.py b/bindings/python/pinocchio/romeo_wrapper.py index 39ba3a10e8..a0d977b371 100644 --- a/bindings/python/pinocchio/romeo_wrapper.py +++ b/bindings/python/pinocchio/romeo_wrapper.py @@ -4,7 +4,7 @@ import numpy as np -from . import pinocchio_pywrap as pin +from . import pinocchio_pywrap_default as pin from .robot_wrapper import RobotWrapper diff --git a/bindings/python/pinocchio/shortcuts.py b/bindings/python/pinocchio/shortcuts.py index 836477565a..726205ea1f 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -4,7 +4,7 @@ ## In this file, some shortcuts are provided ## -from . import pinocchio_pywrap as pin +from . import pinocchio_pywrap_default as pin from . import WITH_HPP_FCL, WITH_HPP_FCL_BINDINGS nle = pin.nonLinearEffects diff --git a/bindings/python/pinocchio/utils.py b/bindings/python/pinocchio/utils.py index 465fd8375a..a7d8ba4ffd 100644 --- a/bindings/python/pinocchio/utils.py +++ b/bindings/python/pinocchio/utils.py @@ -9,8 +9,8 @@ import numpy as np import numpy.linalg as npl -from . import pinocchio_pywrap as pin -from .pinocchio_pywrap.rpy import matrixToRpy, rpyToMatrix, rotate +from . import pinocchio_pywrap_default as pin +from .pinocchio_pywrap_default.rpy import matrixToRpy, rpyToMatrix, rotate from .deprecation import deprecated diff --git a/bindings/python/pinocchio/visualize/base_visualizer.py b/bindings/python/pinocchio/visualize/base_visualizer.py index 70ed006a83..3aabf21290 100644 --- a/bindings/python/pinocchio/visualize/base_visualizer.py +++ b/bindings/python/pinocchio/visualize/base_visualizer.py @@ -1,4 +1,4 @@ -from .. import pinocchio_pywrap as pin +from .. import pinocchio_pywrap_default as pin from ..shortcuts import buildModelsFromUrdf, createDatas import time diff --git a/bindings/python/pinocchio/visualize/gepetto_visualizer.py b/bindings/python/pinocchio/visualize/gepetto_visualizer.py index 86bde561a6..65c13fd1c9 100644 --- a/bindings/python/pinocchio/visualize/gepetto_visualizer.py +++ b/bindings/python/pinocchio/visualize/gepetto_visualizer.py @@ -1,6 +1,7 @@ -from .. import pinocchio_pywrap as pin +from .. import pinocchio_pywrap_default as pin from ..shortcuts import buildModelsFromUrdf, createDatas from ..utils import npToTuple +from numpy.linalg import norm from . import BaseVisualizer @@ -79,6 +80,20 @@ def loadPrimitive(self, meshName, geometry_object): return gui.addSphere(meshName, geom.radius, npToTuple(meshColor)) elif isinstance(geom, hppfcl.Cone): return gui.addCone(meshName, geom.radius, 2. * geom.halfLength, npToTuple(meshColor)) + elif isinstance(geom, hppfcl.Plane) or isinstance(geom, hppfcl.Halfspace): + res = gui.createGroup(meshName) + if not res: + return False + planeName = meshName + "/plane" + res = gui.addFloor(planeName) + if not res: + return False + normal = geom.n + rot = pin.Quaternion.FromTwoVectors(normal,pin.ZAxis) + alpha = geom.d / norm(normal,2)**2 + trans = alpha * normal + plane_offset = pin.SE3(rot,trans) + gui.applyConfiguration(planeName,pin.SE3ToXYZQUATtuple(plane_offset)) elif isinstance(geom, hppfcl.Convex): pts = [ npToTuple(geom.points(geom.polygons(f)[i])) for f in range(geom.num_polygons) for i in range(3) ] gui.addCurve(meshName, pts, npToTuple(meshColor)) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 6bf31d2a12..aa9b6a5d87 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -1,4 +1,5 @@ -from .. import pinocchio_pywrap as pin +from .. import pinocchio_pywrap_default as pin +from ..shortcuts import buildModelsFromUrdf, createDatas from ..utils import npToTuple from . import BaseVisualizer @@ -161,6 +162,7 @@ def loadMesh(self, geometry_object): def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None): """Load a single geometry object""" + import meshcat.geometry viewer_name = self.getViewerNodeName(geometry_object, geometry_type) @@ -254,12 +256,16 @@ def displayCollisions(self,visibility): """Set whether to display collision objects or not. WARNING: Plotting collision meshes is not yet available for MeshcatVisualizer.""" # TODO + import warnings warnings.warn("Plotting collision meshes is not available for MeshcatVisualizer", category=UserWarning, stacklevel=2) + pass def displayVisuals(self,visibility): """Set whether to display visual objects or not WARNING: Visual meshes are always plotted for MeshcatVisualizer""" # TODO + import warnings warnings.warn("Visual meshes are always plotted for MeshcatVisualizer", category=UserWarning, stacklevel=2) + pass __all__ = ['MeshcatVisualizer'] diff --git a/bindings/python/pinocchio/visualize/panda3d_visualizer.py b/bindings/python/pinocchio/visualize/panda3d_visualizer.py index fa55d09c7f..2e3c91feb7 100644 --- a/bindings/python/pinocchio/visualize/panda3d_visualizer.py +++ b/bindings/python/pinocchio/visualize/panda3d_visualizer.py @@ -1,6 +1,6 @@ import warnings -from .. import pinocchio_pywrap as pin +from .. import pinocchio_pywrap_default as pin from ..utils import npToTuple from .base_visualizer import BaseVisualizer diff --git a/bindings/python/serialization/serializable.hpp b/bindings/python/serialization/serializable.hpp index 87db663661..f3e3df78ac 100755 --- a/bindings/python/serialization/serializable.hpp +++ b/bindings/python/serialization/serializable.hpp @@ -25,6 +25,7 @@ namespace pinocchio template void visit(PyClass& cl) const { +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION cl .def("saveToText",&Derived::saveToText, bp::arg("filename"),"Saves *this inside a text file.") @@ -58,8 +59,10 @@ namespace pinocchio .def("loadFromBinary",(void (Derived::*)(serialization::StaticBuffer &))&Derived::loadFromBinary, bp::args("self","buffer"),"Loads *this from a static binary buffer.") ; - serialize(); +#else + PINOCCHIO_UNUSED_VARIABLE(cl); +#endif } }; diff --git a/bindings/python/serialization/serialization.cpp b/bindings/python/serialization/serialization.cpp index 740bbd9524..0ec4bdf119 100644 --- a/bindings/python/serialization/serialization.cpp +++ b/bindings/python/serialization/serialization.cpp @@ -32,24 +32,30 @@ namespace pinocchio bp::scope current_scope = getOrCreatePythonNamespace("serialization"); typedef boost::asio::streambuf StreamBuffer; - bp::class_("StreamBuffer", - "Stream buffer to save/load serialized objects in binary mode.", - bp::init<>(bp::arg("self"),"Default constructor.")) -// .def("capacity",&StreamBuffer::capacity,"Get the current capacity of the StreamBuffer.") - .def("size",&StreamBuffer::size,"Get the size of the input sequence.") - .def("max_size",&StreamBuffer::max_size,"Get the maximum size of the StreamBuffer.") - .def("prepare",prepare_proxy,"Reserve data.",bp::return_internal_reference<>()) - ; + if(!eigenpy::register_symbolic_link_to_registered_type()) + { + bp::class_("StreamBuffer", + "Stream buffer to save/load serialized objects in binary mode.", + bp::init<>(bp::arg("self"),"Default constructor.")) + // .def("capacity",&StreamBuffer::capacity,"Get the current capacity of the StreamBuffer.") + .def("size",&StreamBuffer::size,"Get the size of the input sequence.") + .def("max_size",&StreamBuffer::max_size,"Get the maximum size of the StreamBuffer.") + .def("prepare",prepare_proxy,"Reserve data.",bp::return_internal_reference<>()) + ; + } typedef pinocchio::serialization::StaticBuffer StaticBuffer; - bp::class_("StaticBuffer", - "Static buffer to save/load serialized objects in binary mode with pre-allocated memory.", - bp::init(bp::args("self","size"),"Default constructor from a given size capacity.")) - .def("size",&StaticBuffer::size,bp::arg("self"), - "Get the size of the input sequence.") - .def("reserve",&StaticBuffer::resize,bp::arg("new_size"), - "Increase the capacity of the vector to a value that's greater or equal to new_size.") - ; + if(!eigenpy::register_symbolic_link_to_registered_type()) + { + bp::class_("StaticBuffer", + "Static buffer to save/load serialized objects in binary mode with pre-allocated memory.", + bp::init(bp::args("self","size"),"Default constructor from a given size capacity.")) + .def("size",&StaticBuffer::size,bp::arg("self"), + "Get the size of the input sequence.") + .def("reserve",&StaticBuffer::resize,bp::arg("new_size"), + "Increase the capacity of the vector to a value that's greater or equal to new_size.") + ; + } bp::def("buffer_copy",buffer_copy, bp::args("dest","source"), diff --git a/bindings/python/spatial/classic-acceleration.hpp b/bindings/python/spatial/classic-acceleration.hpp new file mode 100644 index 0000000000..8761c45a50 --- /dev/null +++ b/bindings/python/spatial/classic-acceleration.hpp @@ -0,0 +1,32 @@ +// +// Copyright (c) 2019-2020 INRIA +// + +#include "pinocchio/bindings/python/fwd.hpp" +#include "pinocchio/spatial/classic-acceleration.hpp" + +namespace pinocchio +{ + namespace python + { + namespace bp = boost::python; + + void exposeClassicAcceleration() + { + bp::def("classicAcceleration", + &classicAcceleration, + bp::args("spatial_velocity", + "spatial_acceleration"), + "Computes the classic acceleration from a given spatial velocity and spatial acceleration."); + + bp::def("classicAcceleration", + &classicAcceleration, + bp::args("spatial_velocity", + "spatial_acceleration", + "placement"), + "Computes the classic acceleration of a frame B, given the spatial velocity and spatial acceleration of a frame A,\n" + "and the relative placement A^M_B."); + } + + } // namespace python +} // namespace pinocchio diff --git a/bindings/python/spatial/explog.hpp b/bindings/python/spatial/explog.hpp index ad54e8e303..b8a7927674 100644 --- a/bindings/python/spatial/explog.hpp +++ b/bindings/python/spatial/explog.hpp @@ -94,6 +94,20 @@ namespace pinocchio { return log3(R); } + + template + Eigen::Matrix + log3_proxy(const Matrix3Like & R, Eigen::Ref theta) + { + return log3(R,theta.coeffRef(0,0)); + } + + template + Eigen::Matrix + log3_proxy_fix(const Matrix3Like & R, Scalar & theta) + { + return log3(R,theta); + } template MotionTpl diff --git a/bindings/python/spatial/expose-SE3.cpp b/bindings/python/spatial/expose-SE3.cpp index df7c3ea2ff..66a1aa0609 100644 --- a/bindings/python/spatial/expose-SE3.cpp +++ b/bindings/python/spatial/expose-SE3.cpp @@ -17,9 +17,11 @@ namespace pinocchio void exposeSE3() { - SE3PythonVisitor::expose(); - StdAlignedVectorPythonVisitor::expose("StdVec_SE3"); - serialize::vector_type>(); + SE3PythonVisitor::expose(); + StdAlignedVectorPythonVisitor::expose("StdVec_SE3"); +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + serialize::vector_type>(); +#endif } } // namespace python diff --git a/bindings/python/spatial/expose-explog.cpp b/bindings/python/spatial/expose-explog.cpp index f5e035685a..5bf54e88ed 100644 --- a/bindings/python/spatial/expose-explog.cpp +++ b/bindings/python/spatial/expose-explog.cpp @@ -1,11 +1,13 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#include "pinocchio/bindings/python/spatial/explog.hpp" #include +#include "pinocchio/bindings/python/context.hpp" +#include "pinocchio/bindings/python/spatial/explog.hpp" + namespace pinocchio { namespace python @@ -15,23 +17,35 @@ namespace pinocchio void exposeExplog() { - bp::def("exp3",&exp3_proxy, - bp::arg("Angular velocity (vector of size 3)"), + bp::def("exp3",&exp3_proxy, + bp::arg("w"), "Exp: so3 -> SO3. Return the integral of the input" - " angular velocity during time 1."); + " vector w during time 1. This is also known as the Rodrigues formula."); - bp::def("Jexp3",&Jexp3_proxy, - bp::arg("v: Angular velocity (vector of size 3)"), - "Jacobian of exp(R) which maps from the tangent of SO(3) at exp(v) to" + bp::def("Jexp3",&Jexp3_proxy, + bp::arg("w"), + "Jacobian of exp(v) which maps from the tangent of SO(3) at R = exp(v) to" " the tangent of SO(3) at Identity."); - bp::def("log3",&log3_proxy, - bp::arg("Rotation matrix (matrix of size 3x3))"), - "Log: SO3 -> so3. Pseudo-inverse of log from SO3" - " -> { v in so3, ||v|| < 2pi }.Exp: so3 -> SO3."); + bp::def("log3",&log3_proxy, + bp::arg("R"), + "Log: SO3 -> so3 is the pseudo-inverse of Exp: so3 -> SO3. Log maps from SO3" + " -> { v in so3, ||v|| < 2pi }."); - bp::def("Jlog3",&Jlog3_proxy, - bp::arg("Rotation matrix R (matrix of size 3x3)"), + bp::def("log3",&log3_proxy, + bp::args("R","theta"), + "Log: SO3 -> so3 is the pseudo-inverse of Exp: so3 -> SO3. Log maps from SO3" + " -> { v in so3, ||v|| < 2pi }.\n" + "It also returns the angle of rotation theta around the rotation axis."); + + bp::def("log3",&log3_proxy_fix, + bp::args("R","theta"), + "Log: SO3 -> so3 is the pseudo-inverse of Exp: so3 -> SO3. Log maps from SO3" + " -> { v in so3, ||v|| < 2pi }.\n" + "It also returns the angle of rotation theta around the rotation axis."); + + bp::def("Jlog3",&Jlog3_proxy, + bp::arg("R"), "Jacobian of log(R) which maps from the tangent of SO(3) at R to" " the tangent of SO(3) at Identity."); @@ -40,38 +54,38 @@ namespace pinocchio "Vector v to be multiplied to the hessian", "v^T * H where H is the Hessian of log(R)"); - bp::def("exp6",&exp6_proxy, - bp::arg("Spatial velocity (Motion)"), + bp::def("exp6",&exp6_proxy, + bp::arg("motion"), "Exp: se3 -> SE3. Return the integral of the input" " spatial velocity during time 1."); - bp::def("exp6",&exp6_proxy, - bp::arg("Spatial velocity (vector 6x1)"), + bp::def("exp6",&exp6_proxy, + bp::arg("v"), "Exp: se3 -> SE3. Return the integral of the input" " spatial velocity during time 1."); - bp::def("Jexp6",&Jexp6_proxy, - bp::arg("v: Spatial velocity (Motion)"), + bp::def("Jexp6",&Jexp6_proxy, + bp::arg("motion"), "Jacobian of exp(v) which maps from the tangent of SE(3) at exp(v) to" " the tangent of SE(3) at Identity."); - bp::def("Jexp6",&Jexp6_proxy, - bp::arg("v: Spatial velocity (vector 6x1)"), + bp::def("Jexp6",&Jexp6_proxy, + bp::arg("v"), "Jacobian of exp(v) which maps from the tangent of SE(3) at exp(v) to" " the tangent of SE(3) at Identity."); - bp::def("log6",(Motion (*)(const SE3 &))&log6, - bp::arg("Spatial transform (SE3)"), + bp::def("log6",(context::Motion (*)(const context::SE3 &))&log6, + bp::arg("M"), "Log: SE3 -> se3. Pseudo-inverse of exp from SE3" " -> { v,w in se3, ||w|| < 2pi }."); - bp::def("log6",&log6_proxy, - bp::arg("Homegenious matrix (matrix 4x4)"), - "Log: SE3 -> se3. Pseudo-inverse of exp from SE3" + bp::def("log6",&log6_proxy, + bp::arg("homegeneous_matrix"), + "Log: SE3 -> se3. Pseudo-inverse of Exp: so3 -> SO3. Log maps from SE3" " -> { v,w in se3, ||w|| < 2pi }."); - bp::def("Jlog6",&Jlog6_proxy, - bp::arg("Spatial transform M (SE3)"), + bp::def("Jlog6",&Jlog6_proxy, + bp::arg("M"), "Jacobian of log(M) which maps from the tangent of SE(3) at M to" " the tangent of SE(3) at Identity."); diff --git a/bindings/python/spatial/expose-force.cpp b/bindings/python/spatial/expose-force.cpp index 77320d1af4..fc9625b42b 100644 --- a/bindings/python/spatial/expose-force.cpp +++ b/bindings/python/spatial/expose-force.cpp @@ -17,9 +17,11 @@ namespace pinocchio void exposeForce() { - ForcePythonVisitor::expose(); - StdAlignedVectorPythonVisitor::expose("StdVec_Force"); - serialize::vector_type>(); + ForcePythonVisitor::expose(); + StdAlignedVectorPythonVisitor::expose("StdVec_Force"); +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + serialize::vector_type>(); +#endif } } // namespace python diff --git a/bindings/python/spatial/expose-inertia.cpp b/bindings/python/spatial/expose-inertia.cpp index bf960eff66..138a218de7 100644 --- a/bindings/python/spatial/expose-inertia.cpp +++ b/bindings/python/spatial/expose-inertia.cpp @@ -17,9 +17,11 @@ namespace pinocchio void exposeInertia() { - InertiaPythonVisitor::expose(); - StdAlignedVectorPythonVisitor::expose("StdVec_Inertia"); - serialize::vector_type>(); + InertiaPythonVisitor::expose(); + StdAlignedVectorPythonVisitor::expose("StdVec_Inertia"); +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + serialize::vector_type>(); +#endif } } // namespace python diff --git a/bindings/python/spatial/expose-motion.cpp b/bindings/python/spatial/expose-motion.cpp index c1ca298c46..2f5df50e5a 100644 --- a/bindings/python/spatial/expose-motion.cpp +++ b/bindings/python/spatial/expose-motion.cpp @@ -7,9 +7,9 @@ #include "pinocchio/bindings/python/fwd.hpp" #include "pinocchio/bindings/python/spatial/motion.hpp" -#include "pinocchio/bindings/python/serialization/serialization.hpp" +#include "pinocchio/bindings/python/spatial/classic-acceleration.hpp" #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" - +#include "pinocchio/bindings/python/serialization/serialization.hpp" namespace pinocchio { namespace python @@ -17,9 +17,12 @@ namespace pinocchio void exposeMotion() { - MotionPythonVisitor::expose(); - StdAlignedVectorPythonVisitor::expose("StdVec_Motion"); - serialize::vector_type>(); + MotionPythonVisitor::expose(); + StdAlignedVectorPythonVisitor::expose("StdVec_Motion"); +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + serialize::vector_type>(); +#endif + exposeClassicAcceleration(); } } // namespace python diff --git a/bindings/python/spatial/expose-skew.cpp b/bindings/python/spatial/expose-skew.cpp index ef380c1a96..524812d578 100644 --- a/bindings/python/spatial/expose-skew.cpp +++ b/bindings/python/spatial/expose-skew.cpp @@ -36,8 +36,8 @@ namespace pinocchio void exposeSkew() { - typedef SE3::Matrix3 Matrix3; - typedef SE3::Vector3 Vector3; + typedef context::SE3::Matrix3 Matrix3; + typedef context::SE3::Vector3 Vector3; bp::def("skew",&skew, bp::arg("u"), diff --git a/bindings/python/spatial/force.hpp b/bindings/python/spatial/force.hpp index 3e8ca9da1d..cf34d3bcaa 100644 --- a/bindings/python/spatial/force.hpp +++ b/bindings/python/spatial/force.hpp @@ -12,10 +12,12 @@ #include "pinocchio/spatial/se3.hpp" #include "pinocchio/spatial/force.hpp" + +#include "pinocchio/bindings/python/utils/cast.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" #include "pinocchio/bindings/python/utils/printable.hpp" -EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Force) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::Force) namespace pinocchio { @@ -50,7 +52,7 @@ namespace pinocchio struct ForcePythonVisitor : public boost::python::def_visitor< ForcePythonVisitor > { - enum { Options = traits::Options }; + enum { Options = traits::Options }; typedef typename Force::Vector6 Vector6; typedef typename Force::Vector3 Vector3; @@ -68,7 +70,7 @@ namespace pinocchio ((bp::arg("self"),bp::arg("linear"),bp::arg("angular")), "Initialize from linear and angular components of a Wrench vector (don't mix the order).")) .def(bp::init((bp::args("self","array")),"Init from a vector 6 [force,torque]")) - .def(bp::init((bp::args("self","other")),"Copy constructor.")) + .def(bp::init((bp::arg("self"),bp::arg("clone")),"Copy constructor")) .add_property("linear", bp::make_function(&ForcePythonVisitor::getLinear, @@ -99,19 +101,25 @@ namespace pinocchio .def("setRandom",&ForcePythonVisitor::setRandom,bp::arg("self"), "Set the linear and angular components of *this to random values.") + .def("dot",(Scalar (Force::*)(const MotionDense &) const) &Force::dot, + bp::args("self","m"),"Dot product between *this and a Motion m.") + .def(bp::self + bp::self) .def(bp::self += bp::self) .def(bp::self - bp::self) .def(bp::self -= bp::self) .def(-bp::self) - + +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def(bp::self == bp::self) .def(bp::self != bp::self) +#endif .def(bp::self * Scalar()) .def(Scalar() * bp::self) .def(bp::self / Scalar()) +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def("isApprox", &call::isApprox, isApproxForce_overload(bp::args("self","other","prec"), @@ -121,6 +129,7 @@ namespace pinocchio &call::isZero, isZero_overload(bp::args("self","prec"), "Returns true if *this is approximately equal to the zero Force, within the precision given by prec.")) +#endif .def("Random",&Force::Random,"Returns a random Force.") .staticmethod("Random") @@ -129,18 +138,29 @@ namespace pinocchio .def("__array__",bp::make_function((typename Force::ToVectorReturnType (Force::*)())&Force::toVector, bp::return_internal_reference<>())) - +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION .def_pickle(Pickle()) +#endif ; } static void expose() { + typedef pinocchio::ForceBase ForceBase; + bp::objects::register_dynamic_id(); + bp::objects::register_conversion(false); + + typedef pinocchio::ForceDense ForceDense; + bp::objects::register_dynamic_id(); + bp::objects::register_conversion(false); + bp::class_("Force", "Force vectors, in se3* == F^6.\n\n" "Supported operations ...", bp::no_init) .def(ForcePythonVisitor()) + .def(CastVisitor()) + .def(ExposeConstructorByCastVisitor()) .def(CopyableVisitor()) .def(PrintableVisitor()) ; diff --git a/bindings/python/spatial/inertia.hpp b/bindings/python/spatial/inertia.hpp index fe8b43b959..13484d531c 100644 --- a/bindings/python/spatial/inertia.hpp +++ b/bindings/python/spatial/inertia.hpp @@ -12,10 +12,12 @@ #include #include "pinocchio/spatial/inertia.hpp" + +#include "pinocchio/bindings/python/utils/cast.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" #include "pinocchio/bindings/python/utils/printable.hpp" -EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Inertia) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::Inertia) namespace pinocchio { @@ -42,21 +44,25 @@ namespace pinocchio return self.isZero(prec); } }; - - BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxInertia_overload,call::isApprox,2,3) - BOOST_PYTHON_FUNCTION_OVERLOADS(isZero_overload,call::isZero,1,2) template struct InertiaPythonVisitor : public boost::python::def_visitor< InertiaPythonVisitor > { - + enum { Options = Inertia::Options }; typedef typename Inertia::Scalar Scalar; typedef typename Inertia::Vector3 Vector3; typedef typename Inertia::Matrix3 Matrix3; typedef typename Inertia::Vector6 Vector6; typedef typename Inertia::Matrix6 Matrix6; - + + typedef Eigen::Matrix VectorXs; + typedef MotionTpl Motion; + typedef ForceTpl Force; + + BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxInertia_overload,call::isApprox,2,3) + BOOST_PYTHON_FUNCTION_OVERLOADS(isZero_overload,call::isZero,1,2) + public: template @@ -68,7 +74,9 @@ namespace pinocchio bp::default_call_policies(), (bp::arg("mass"),bp::arg("lever"),bp::arg("inertia"))), "Initialize from mass, lever and 3d inertia.") - .def(bp::init((bp::arg("self"),bp::arg("other")),"Copy constructor.")) + + .def(bp::init<>(bp::arg("self"),"Default constructor.")) + .def(bp::init((bp::arg("self"),bp::arg("clone")),"Copy constructor")) .add_property("mass", &InertiaPythonVisitor::getMass, @@ -85,9 +93,9 @@ namespace pinocchio "Rotational part of the Spatial Inertia, i.e. a symmetric matrix representing the rotational inertia around the center of mass.") .def("matrix",&Inertia::matrix,bp::arg("self")) - .def("se3Action",&Inertia::se3Action, + .def("se3Action",&Inertia::template se3Action, bp::args("self","M"),"Returns the result of the action of M on *this.") - .def("se3ActionInverse",&Inertia::se3ActionInverse, + .def("se3ActionInverse",&Inertia::template se3ActionInverse, bp::args("self","M"),"Returns the result of the action of the inverse of M on *this.") .def("setIdentity",&Inertia::setIdentity,bp::arg("self"), @@ -100,21 +108,30 @@ namespace pinocchio .def(bp::self + bp::self) .def(bp::self * bp::other() ) .add_property("np",&Inertia::matrix) - .def("vxiv",&Inertia::vxiv,bp::args("self","v"),"Returns the result of v x Iv.") - .def("vtiv",&Inertia::vtiv,bp::args("self","v"),"Returns the result of v.T * Iv.") - .def("vxi",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::vxi, + .def("vxiv", + &Inertia::template vxiv, + bp::args("self","v"), + "Returns the result of v x Iv.") + .def("vtiv", + &Inertia::template vtiv, + bp::args("self","v"), + "Returns the result of v.T * Iv.") + .def("vxi",(Matrix6 (Inertia::*)(const MotionDense &) const)&Inertia::template vxi, bp::args("self","v"), "Returns the result of v x* I, a 6x6 matrix.") - .def("ivx",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::ivx, + .def("ivx",(Matrix6 (Inertia::*)(const MotionDense &) const)&Inertia::template ivx, bp::args("self","v"), "Returns the result of I vx, a 6x6 matrix.") - .def("variation",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::variation, + .def("variation",(Matrix6 (Inertia::*)(const MotionDense &) const)&Inertia::template variation, bp::args("self","v"), "Returns the time derivative of the inertia.") +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def(bp::self == bp::self) .def(bp::self != bp::self) - +#endif + +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def("isApprox", call::isApprox, isApproxInertia_overload(bp::args("self","other","prec"), @@ -124,6 +141,7 @@ namespace pinocchio call::isZero, isZero_overload(bp::args("self","prec"), "Returns true if *this is approximately equal to the zero Inertia, within the precision given by prec.")) +#endif .def("Identity",&Inertia::Identity,"Returns the identity Inertia.") .staticmethod("Identity") @@ -137,7 +155,7 @@ namespace pinocchio "\nThe parameters are given as v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T " "where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter" ) - .def("FromDynamicParameters",&Inertia::template FromDynamicParameters, + .def("FromDynamicParameters",&Inertia::template FromDynamicParameters, bp::args("dynamic_parameters"), "Builds and inertia matrix from a vector of dynamic parameters." "\nThe parameters are given as dynamic_parameters = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T " @@ -163,8 +181,9 @@ namespace pinocchio .staticmethod("FromBox") .def("__array__",&Inertia::matrix) - +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION .def_pickle(Pickle()) +#endif ; } @@ -177,7 +196,7 @@ namespace pinocchio // static void setInertia(Inertia & self, const Vector6 & minimal_inertia) { self.inertia().data() = minimal_inertia; } static void setInertia(Inertia & self, const Matrix3 & symmetric_inertia) { - assert(symmetric_inertia.isApprox(symmetric_inertia.transpose())); + assert(check_expression_if_real(isZero(symmetric_inertia - symmetric_inertia.transpose()))); self.inertia().data() << symmetric_inertia(0,0), symmetric_inertia(1,0), @@ -187,21 +206,23 @@ namespace pinocchio symmetric_inertia(2,2); } - static Eigen::VectorXd toDynamicParameters_proxy(const Inertia & self) + static VectorXs toDynamicParameters_proxy(const Inertia & self) { return self.toDynamicParameters(); } - static Inertia* makeFromMCI(const double & mass, + static Inertia* makeFromMCI(const Scalar & mass, const Vector3 & lever, const Matrix3 & inertia) { +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS if(! inertia.isApprox(inertia.transpose()) ) throw eigenpy::Exception("The 3d inertia should be symmetric."); - if( (Eigen::Vector3d::UnitX().transpose()*inertia*Eigen::Vector3d::UnitX()<0) - || (Eigen::Vector3d::UnitY().transpose()*inertia*Eigen::Vector3d::UnitY()<0) - || (Eigen::Vector3d::UnitZ().transpose()*inertia*Eigen::Vector3d::UnitZ()<0) ) + if( (Vector3::UnitX().transpose()*inertia*Vector3::UnitX()<0) + || (Vector3::UnitY().transpose()*inertia*Vector3::UnitY()<0) + || (Vector3::UnitZ().transpose()*inertia*Vector3::UnitZ()<0) ) throw eigenpy::Exception("The 3d inertia should be positive."); +#endif return new Inertia(mass,lever,inertia); } @@ -210,8 +231,10 @@ namespace pinocchio bp::class_("Inertia", "This class represenses a sparse version of a Spatial Inertia and its is defined by its mass, its center of mass location and the rotational inertia expressed around this center of mass.\n\n" "Supported operations ...", - bp::init<>(bp::arg("self"),"Default constructor.")) + bp::no_init) .def(InertiaPythonVisitor()) + .def(CastVisitor()) + .def(ExposeConstructorByCastVisitor()) .def(CopyableVisitor()) .def(PrintableVisitor()) ; diff --git a/bindings/python/spatial/motion.hpp b/bindings/python/spatial/motion.hpp index 3410a14acd..dcb1c8c671 100644 --- a/bindings/python/spatial/motion.hpp +++ b/bindings/python/spatial/motion.hpp @@ -9,14 +9,17 @@ #include #include #include +#include #include "pinocchio/spatial/se3.hpp" #include "pinocchio/spatial/motion.hpp" #include "pinocchio/spatial/force.hpp" + +#include "pinocchio/bindings/python/utils/cast.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" #include "pinocchio/bindings/python/utils/printable.hpp" -EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Motion) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::Motion) namespace pinocchio { @@ -54,7 +57,7 @@ namespace pinocchio enum { Options = traits::Options }; typedef typename Motion::Scalar Scalar; - typedef ForceTpl::Options> Force; + typedef ForceTpl Force; typedef typename Motion::Vector6 Vector6; typedef typename Motion::Vector3 Vector3; @@ -72,7 +75,7 @@ namespace pinocchio ((bp::arg("self"),bp::arg("linear"),bp::arg("angular")), "Initialize from linear and angular components of a Motion vector (don't mix the order).")) .def(bp::init((bp::arg("self"),bp::arg("array")),"Init from a vector 6 [linear velocity, angular velocity]")) - .def(bp::init((bp::arg("self"),bp::arg("other")),"Copy constructor.")) + .def(bp::init((bp::arg("self"),bp::arg("clone")),"Copy constructor")) .add_property("linear", bp::make_function(&MotionPythonVisitor::getLinear, @@ -106,6 +109,9 @@ namespace pinocchio .def("setRandom",&MotionPythonVisitor::setRandom,bp::arg("self"), "Set the linear and angular components of *this to random values.") + .def("dot",(Scalar (Motion::*)(const ForceBase &) const) &Motion::dot, + bp::args("self","f"),"Dot product between *this and a Force f.") + .def("cross",(Motion (Motion::*)(const Motion &) const) &Motion::cross, bp::args("self","m"),"Action of *this onto another Motion m. Returns ¨*this x m.") .def("cross",(Force (Motion::*)(const Force &) const) &Motion::cross, @@ -119,13 +125,16 @@ namespace pinocchio .def(bp::self ^ bp::self) .def(bp::self ^ Force()) +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def(bp::self == bp::self) .def(bp::self != bp::self) +#endif .def(bp::self * Scalar()) .def(Scalar() * bp::self) .def(bp::self / Scalar()) +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def("isApprox", call::isApprox, isApproxMotion_overload(bp::args("self","other","prec"), @@ -135,6 +144,7 @@ namespace pinocchio call::isZero, isZero_overload(bp::args("self","prec"), "Returns true if *this is approximately equal to the zero Motion, within the precision given by prec.")) +#endif .def("Random",&Motion::Random,"Returns a random Motion.") .staticmethod("Random") @@ -143,18 +153,29 @@ namespace pinocchio .def("__array__",bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector, bp::return_internal_reference<>())) - +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION .def_pickle(Pickle()) +#endif ; } static void expose() { + typedef pinocchio::MotionBase MotionBase; + bp::objects::register_dynamic_id(); + bp::objects::register_conversion(false); + + typedef pinocchio::MotionDense MotionDense; + bp::objects::register_dynamic_id(); + bp::objects::register_conversion(false); + bp::class_("Motion", "Motion vectors, in se3 == M^6.\n\n" "Supported operations ...", bp::no_init) .def(MotionPythonVisitor()) + .def(CastVisitor()) + .def(ExposeConstructorByCastVisitor()) .def(CopyableVisitor()) .def(PrintableVisitor()) ; diff --git a/bindings/python/spatial/se3.hpp b/bindings/python/spatial/se3.hpp index ed2c6c4f75..92281d4f52 100644 --- a/bindings/python/spatial/se3.hpp +++ b/bindings/python/spatial/se3.hpp @@ -16,10 +16,11 @@ #include "pinocchio/spatial/inertia.hpp" #include "pinocchio/spatial/explog.hpp" +#include "pinocchio/bindings/python/utils/cast.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" #include "pinocchio/bindings/python/utils/printable.hpp" -EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::SE3) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::python::context::SE3) namespace pinocchio { @@ -50,10 +51,15 @@ namespace pinocchio : public boost::python::def_visitor< SE3PythonVisitor > { typedef typename SE3::Scalar Scalar; + enum { Options = SE3::Options }; typedef typename SE3::Matrix3 Matrix3; typedef typename SE3::Vector3 Vector3; typedef typename SE3::Matrix4 Matrix4; typedef typename SE3::Quaternion Quaternion; + + typedef MotionTpl Motion; + typedef ForceTpl Force; + typedef InertiaTpl Inertia; public: @@ -68,10 +74,10 @@ namespace pinocchio ((bp::arg("self"),bp::arg("quat"),bp::arg("translation")), "Initialize from a quaternion and a translation vector.")) .def(bp::init((bp::arg("self"),bp::arg("int")),"Init to identity.")) - .def(bp::init((bp::arg("self"),bp::arg("other")), "Copy constructor.")) .def(bp::init ((bp::arg("self"),bp::arg("array")), "Initialize from an homogeneous matrix.")) + .def(bp::init((bp::arg("self"),bp::arg("clone")),"Copy constructor")) .add_property("rotation", bp::make_function((typename SE3::AngularRef (SE3::*)()) &SE3::rotation,bp::return_internal_reference<>()), @@ -133,7 +139,8 @@ namespace pinocchio bp::args("self","inertia"), "Returns the result of *this onto a Force.") .def("actInv", (Inertia (SE3::*)(const Inertia &) const) &SE3::actInv, bp::args("self","inertia"), "Returns the result of the inverse of *this onto an Inertia.") - + +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def("isApprox", call::isApprox, isApproxSE3_overload(bp::args("self","other","prec"), @@ -143,6 +150,7 @@ namespace pinocchio &SE3::isIdentity, isIdentity_overload(bp::args("self","prec"), "Returns true if *this is approximately equal to the identity placement, within the precision given by prec.")) +#endif .def("__invert__",&SE3::inverse,"Returns the inverse of *this.") .def(bp::self * bp::self) @@ -152,8 +160,10 @@ namespace pinocchio .def("__mul__",&__mul__) .add_property("np",&SE3::toHomogeneousMatrix) +#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS .def(bp::self == bp::self) .def(bp::self != bp::self) +#endif .def("Identity",&SE3::Identity,"Returns the identity transformation.") .staticmethod("Identity") @@ -172,7 +182,9 @@ namespace pinocchio .def("__array__",&SE3::toHomogeneousMatrix) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION .def_pickle(Pickle()) +#endif ; } @@ -182,6 +194,8 @@ namespace pinocchio "SE3 transformation defined by a 3d vector and a rotation matrix.", bp::init<>(bp::arg("self"),"Default constructor.")) .def(SE3PythonVisitor()) + .def(CastVisitor()) + .def(ExposeConstructorByCastVisitor()) .def(CopyableVisitor()) .def(PrintableVisitor()) ; diff --git a/bindings/python/utils/cast.hpp b/bindings/python/utils/cast.hpp new file mode 100644 index 0000000000..bd81801945 --- /dev/null +++ b/bindings/python/utils/cast.hpp @@ -0,0 +1,83 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_python_utils_cast_hpp__ +#define __pinocchio_python_utils_cast_hpp__ + +#include "pinocchio/bindings/python/fwd.hpp" + +#include + +namespace pinocchio +{ + namespace python + { + + namespace bp = boost::python; + + /// + /// \brief Add the Python method cast + /// + template + struct CastVisitor : public bp::def_visitor< CastVisitor > + { + template + void visit(PyClass & cl) const + { + cl.def("cast",&C::template cast,"Returns a cast of *this."); + } + + }; + + template + struct ExposeConstructorByCastVisitor : public bp::def_visitor< ExposeConstructorByCastVisitor<_From,_To> > + { + template + void visit(PyClass & /*cl*/) const + { + expose_constructor<_From,_To>(); + expose_constructor<_To,_From>(); + } + + protected: + + template + static void expose_constructor() + { + if(eigenpy::check_registration() && eigenpy::check_registration()) + { + const bp::type_info to_info = bp::type_id(); + const bp::converter::registration* to_reg = bp::converter::registry::query(to_info); + bp::object to_class_obj(bp::handle<>(bp::borrowed(to_reg->get_class_object()))); + const std::string to_module_name = bp::extract(to_class_obj.attr("__module__")); + const std::string to_class_name = bp::extract(to_class_obj.attr("__name__")); + + const bp::type_info from_info = bp::type_id(); + const bp::converter::registration* from_reg = bp::converter::registry::query(from_info); + bp::object from_class_obj(bp::handle<>(bp::borrowed(from_reg->get_class_object()))); + const std::string from_module_name = bp::extract(from_class_obj.attr("__module__")); + const std::string from_class_name = bp::extract(from_class_obj.attr("__name__")); + + const std::string to_full_class_name = to_module_name + "." + to_class_name; + const std::string from_full_class_name = from_module_name + "." + from_class_name; + std::stringstream to_doc; + to_doc << "Copy constructor from " << from_full_class_name; + to_doc << " -> " << to_full_class_name; + bp::objects::add_to_namespace(to_class_obj,"__init__", + bp::make_constructor(&ExposeConstructorByCastVisitor::constructor,bp::default_call_policies(),bp::arg("clone")),to_doc.str().c_str()); + } + } + + template + static To * constructor(const From & clone) + { + typedef typename To::Scalar NewScalar; + return new To(clone.template cast()); + } + + }; + } // namespace python +} // namespace pinocchio + +#endif // ifndef __pinocchio_python_utils_cast_hpp__ diff --git a/bindings/python/utils/comparable.hpp b/bindings/python/utils/comparable.hpp new file mode 100644 index 0000000000..5a11b466d9 --- /dev/null +++ b/bindings/python/utils/comparable.hpp @@ -0,0 +1,43 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_python_utils_comparable_hpp__ +#define __pinocchio_python_utils_comparable_hpp__ + +#include + +namespace pinocchio +{ + namespace python + { + + namespace bp = boost::python; + + /// + /// \brief Add the Python method == and != to allow a comparison of this. + /// + template + struct ComparableVisitor + : public bp::def_visitor< ComparableVisitor > + { + template + void visit(PyClass & cl) const + { + cl + .def(bp::self == bp::self) + .def(bp::self != bp::self); + } + }; + + template + struct ComparableVisitor + : public bp::def_visitor< ComparableVisitor > + { + template + void visit(PyClass &) const {} + }; + } // namespace python +} // namespace pinocchio + +#endif // ifndef __pinocchio_python_utils_comparable_hpp__ diff --git a/bindings/python/utils/conversions.cpp b/bindings/python/utils/conversions.cpp index 8b07ba2bc9..eafa5caeaf 100644 --- a/bindings/python/utils/conversions.cpp +++ b/bindings/python/utils/conversions.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2019-2020 CNRS INRIA +// Copyright (c) 2019-2021 CNRS INRIA // #include "pinocchio/bindings/python/fwd.hpp" @@ -10,68 +10,79 @@ namespace pinocchio namespace python { namespace bp = boost::python; - typedef SE3::Scalar Scalar; - typedef Eigen::Matrix VectorXd; - typedef Eigen::Matrix Vector7d; - typedef Eigen::Map< SE3::Quaternion> QuatMap; - typedef Eigen::Map QuatConstMap; - - VectorXd SE3ToXYZQUAT(const SE3& M) + + struct XYZQUATConverter { - Vector7d res; - res.head<3>() = M.translation(); - QuatMap (res.tail<4>().data()) = M.rotation(); - return res; - } + typedef context::Scalar Scalar; + typedef context::SE3 SE3; + typedef context::VectorXs VectorXs; + enum { Options = context::Options }; + typedef Eigen::Matrix Vector7s; + typedef Eigen::Map< SE3::Quaternion> QuatMap; + typedef Eigen::Map QuatConstMap; - bp::tuple SE3ToXYZQUATtuple(const SE3& M) - { - SE3::Quaternion q (M.rotation()); - return bp::make_tuple ( - M.translation()(0), M.translation()(1), M.translation()(2), - q.x(), q.y(), q.z(), q.w()); - } + static VectorXs fromSE3(const SE3 & M) + { + Vector7s res; + res.head<3>() = M.translation(); + QuatMap (res.tail<4>().data()) = M.rotation(); + return res; + } - template - SE3 XYZQUATToSE3_bp(const TupleOrList& v) - { - //bp::extract to_double; - SE3::Quaternion q ( - (Scalar)bp::extract(v[6]), - (Scalar)bp::extract(v[3]), - (Scalar)bp::extract(v[4]), - (Scalar)bp::extract(v[5])); - SE3::Vector3 t ( - (Scalar)bp::extract(v[0]), - (Scalar)bp::extract(v[1]), - (Scalar)bp::extract(v[2])); - return SE3 (q.matrix(), t); - } + static bp::tuple fromSE3tuple(const SE3 & M) + { + const SE3::Quaternion q(M.rotation()); + return bp::make_tuple ( + M.translation()(0), M.translation()(1), M.translation()(2), + q.x(), q.y(), q.z(), q.w()); + } - template - SE3 XYZQUATToSE3_ei(const Vector7Like& v) - { - PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector7Like, v, 7, 1); - QuatConstMap q (v.template tail<4>().data()); - return SE3 (q.matrix(), v.template head<3>()); - } - + template + static SE3 toSE3fromTupleOrList(const TupleOrList & v) + { + //bp::extract to_double; + SE3::Quaternion q( + (Scalar)bp::extract(v[6]), + (Scalar)bp::extract(v[3]), + (Scalar)bp::extract(v[4]), + (Scalar)bp::extract(v[5])); + SE3::Vector3 t( + (Scalar)bp::extract(v[0]), + (Scalar)bp::extract(v[1]), + (Scalar)bp::extract(v[2])); + return SE3(q.matrix(), t); + } + + template + static SE3 toSE3(const Vector7Like & v) + { + PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector7Like, v, 7, 1); + QuatConstMap q(v.template tail<4>().data()); + return SE3(q.matrix(), v.template head<3>()); + } + + static void expose() + { + const char* doc1 = "Convert the input SE3 object to a numpy array."; + bp::def("SE3ToXYZQUAT" , fromSE3 , "M", doc1); + const char* doc1_tuple = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w]."; + bp::def("SE3ToXYZQUATtuple", fromSE3tuple, "M", doc1_tuple); + + const char* doc2 = "Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element."; + bp::def("XYZQUATToSE3", + static_cast (toSE3fromTupleOrList), + bp::arg("tuple"),doc2); + bp::def("XYZQUATToSE3", + static_cast (toSE3fromTupleOrList), + bp::arg("list"),doc2); + bp::def("XYZQUATToSE3", static_cast (toSE3), + bp::arg("array"),doc2); + } + }; + void exposeConversions() { - const char* doc1 = "Convert the input SE3 object to a numpy array."; - bp::def("SE3ToXYZQUAT" , SE3ToXYZQUAT , "M", doc1); - const char* doc1_tuple = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w]."; - bp::def("SE3ToXYZQUATtuple", SE3ToXYZQUATtuple, "M", doc1_tuple); - - const char* doc2 = "Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element."; - bp::def("XYZQUATToSE3", - static_cast (XYZQUATToSE3_bp), - bp::arg("tuple"),doc2); - bp::def("XYZQUATToSE3", - static_cast (XYZQUATToSE3_bp), - bp::arg("list"),doc2); - bp::def("XYZQUATToSE3", static_cast (XYZQUATToSE3_ei), - bp::arg("array"),doc2); + XYZQUATConverter::expose(); } } // namespace python diff --git a/bindings/python/utils/macros.hpp b/bindings/python/utils/macros.hpp new file mode 100644 index 0000000000..2e991489d4 --- /dev/null +++ b/bindings/python/utils/macros.hpp @@ -0,0 +1,30 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_python_utils_macros_hpp__ +#define __pinocchio_python_utils_macros_hpp__ + +#define PINOCCHIO_ADD_PROPERTY(STRUCT_NAME,PROPERTY_NAME,DOC) \ + def_readwrite(#PROPERTY_NAME, \ + &STRUCT_NAME::PROPERTY_NAME, \ + DOC) + +#define PINOCCHIO_ADD_PROPERTY_READONLY(STRUCT_NAME,PROPERTY_NAME,DOC) \ + def_readonly(#PROPERTY_NAME, \ + &STRUCT_NAME::PROPERTY_NAME, \ + DOC) + +#define PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(STRUCT_NAME,PROPERTY_NAME,DOC) \ + add_property(#PROPERTY_NAME, \ + make_getter(&STRUCT_NAME::PROPERTY_NAME, \ + ::boost::python::return_value_policy< ::boost::python::return_by_value>()), \ + DOC) + +#define PINOCCHIO_ADD_STATIC_PROPERTY_READONLY_BYVALUE(STRUCT_NAME,PROPERTY_NAME,DOC) \ + add_static_property(#PROPERTY_NAME, \ + make_getter(&STRUCT_NAME::PROPERTY_NAME, \ + ::boost::python::return_value_policy< ::boost::python::return_by_value>()), \ + DOC) + +#endif // ifndef __pinocchio_python_utils_macros_hpp__ diff --git a/bindings/python/utils/namespace.hpp b/bindings/python/utils/namespace.hpp index bb187e0be6..a8051ea16f 100644 --- a/bindings/python/utils/namespace.hpp +++ b/bindings/python/utils/namespace.hpp @@ -11,7 +11,8 @@ namespace pinocchio { namespace python { - + namespace bp = boost::python; + /// /// \brief Helper to create or simply return an existing namespace in Python /// @@ -19,10 +20,8 @@ namespace pinocchio /// /// \returns The submodule related to the namespace name. /// - inline boost::python::object getOrCreatePythonNamespace(const std::string & submodule_name) + inline bp::object getOrCreatePythonNamespace(const std::string & submodule_name) { - namespace bp = boost::python; - bp::scope current_scope; std::string current_scope_name(bp::extract(current_scope.attr("__name__"))); std::string complete_submodule_name = current_scope_name + "." + submodule_name; diff --git a/bindings/python/utils/pickle-vector.hpp b/bindings/python/utils/pickle-vector.hpp index 02dabb96d9..30356c02e4 100644 --- a/bindings/python/utils/pickle-vector.hpp +++ b/bindings/python/utils/pickle-vector.hpp @@ -35,7 +35,11 @@ namespace pinocchio { VecType & o = boost::python::extract(op)(); boost::python::stl_input_iterator begin(tup[0]), end; - o.insert(o.begin(),begin,end); + while(begin != end) + { + o.push_back(*begin); + ++begin; + } } } }; diff --git a/bindings/python/utils/registration.hpp b/bindings/python/utils/registration.hpp index bf32d68ca3..797bb38851 100644 --- a/bindings/python/utils/registration.hpp +++ b/bindings/python/utils/registration.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2019 INRIA +// Copyright (c) 2019-2020 INRIA // #ifndef __pinocchio_python_utils_registration_hpp__ @@ -23,7 +23,7 @@ namespace pinocchio { const bp::type_info info = bp::type_id(); const bp::converter::registration* reg = bp::converter::registry::query(info); - bp::handle<> class_obj(reg->get_class_object()); + bp::handle<> class_obj(bp::borrowed(reg->get_class_object())); bp::scope().attr(reg->get_class_object()->tp_name) = bp::object(class_obj); return true; } @@ -35,4 +35,3 @@ namespace pinocchio } // namespace pinocchio #endif // ifndef __pinocchio_python_utils_registration_hpp__ - diff --git a/bindings/python/utils/std-aligned-vector.hpp b/bindings/python/utils/std-aligned-vector.hpp index c228fdb098..9ffac27aa2 100644 --- a/bindings/python/utils/std-aligned-vector.hpp +++ b/bindings/python/utils/std-aligned-vector.hpp @@ -29,29 +29,48 @@ namespace pinocchio /// template struct StdAlignedVectorPythonVisitor - : public ::boost::python::vector_indexing_suite,NoProxy> + : public ::boost::python::vector_indexing_suite,NoProxy, internal::contains_vector_derived_policies,NoProxy> > , public StdContainerFromPythonList< container::aligned_vector > { typedef container::aligned_vector vector_type; - typedef StdContainerFromPythonList FromPythonListConverter; + typedef StdContainerFromPythonList FromPythonListConverter; - static ::boost::python::class_ expose(const std::string & class_name, - const std::string & doc_string = "") + static void expose(const std::string & class_name, + const std::string & doc_string = "") + { + expose(class_name,doc_string,EmptyPythonVisitor()); + } + + template + static void expose(const std::string & class_name, + const boost::python::def_visitor & visitor) + { + expose(class_name,"",visitor); + } + + template + static void expose(const std::string & class_name, + const std::string & doc_string, + const boost::python::def_visitor & visitor) { namespace bp = boost::python; - - bp::class_ cl(class_name.c_str(),doc_string.c_str()); - cl - .def(StdAlignedVectorPythonVisitor()) - .def("tolist",&FromPythonListConverter::tolist,bp::arg("self"), - "Returns the aligned_vector as a Python list.") - .def_pickle(PickleVector()); - - // Register conversion - if(EnableFromPythonListConverter) - FromPythonListConverter::register_converter(); - - return cl; + if(!register_symbolic_link_to_registered_type()) + { + bp::class_ cl(class_name.c_str(),doc_string.c_str()); + cl + .def(StdAlignedVectorPythonVisitor()) + .def("tolist",&FromPythonListConverter::tolist,bp::arg("self"), + "Returns the aligned_vector as a Python list.") + .def(visitor) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + .def_pickle(PickleVector()) +#endif + ; + + // Register conversion + if(EnableFromPythonListConverter) + FromPythonListConverter::register_converter(); + } } }; } // namespace python diff --git a/bindings/python/utils/std-vector.hpp b/bindings/python/utils/std-vector.hpp index b41d295495..96db3a59e9 100644 --- a/bindings/python/utils/std-vector.hpp +++ b/bindings/python/utils/std-vector.hpp @@ -8,20 +8,78 @@ #include #include #include +#include #include #include #include #include "pinocchio/bindings/python/utils/pickle-vector.hpp" +#include "pinocchio/bindings/python/utils/registration.hpp" namespace pinocchio { namespace python { + // Forward declaration + template + struct StdContainerFromPythonList; + namespace details { + /// \brief Check if a PyObject can be converted to an std::vector. + template + bool from_python_list(PyObject * obj_ptr,T *) + { + namespace bp = ::boost::python; + + // Check if it is a list + if(!PyList_Check(obj_ptr)) return false; + + // Retrieve the underlying list + bp::object bp_obj(bp::handle<>(bp::borrowed(obj_ptr))); + bp::list bp_list(bp_obj); + bp::ssize_t list_size = bp::len(bp_list); + + // Check if all the elements contained in the current vector is of type T + for(bp::ssize_t k = 0; k < list_size; ++k) + { + bp::extract elt(bp_list[k]); + if(!elt.check()) return false; + } + + return true; + } + + template + struct build_list + { + static ::boost::python::list run(vector_type & vec) + { + namespace bp = ::boost::python; + + bp::list bp_list; + for(size_t k = 0; k < vec.size(); ++k) + { + bp_list.append(boost::ref(vec[k])); + } + return bp_list; + } + }; + + template + struct build_list + { + static ::boost::python::list run(vector_type & vec) + { + namespace bp = ::boost::python; + + typedef bp::iterator iterator; + return bp::list(iterator()(vec)); + } + }; + template struct overload_base_get_item_for_std_vector : public boost::python::def_visitor< overload_base_get_item_for_std_vector > @@ -80,17 +138,87 @@ namespace pinocchio return index_type(); } }; + } // namespace details + +}} // namespace pinocchio::python + +namespace boost { namespace python { namespace converter { + + template + struct reference_arg_from_python &> + : arg_lvalue_from_python_base + { + typedef std::vector vector_type; + typedef vector_type & ref_vector_type; + typedef ref_vector_type result_type; + + reference_arg_from_python(PyObject* py_obj) + : arg_lvalue_from_python_base(converter::get_lvalue_from_python(py_obj, + registered::converters)) + , m_data(NULL) + , m_source(py_obj) + , vec_ptr(NULL) + { + if(result() != 0) // we have found a lvalue converter + return; + + // Check if py_obj is a py_list, which can then be converted to an std::vector + bool is_convertible = ::pinocchio::python::details::from_python_list(py_obj,(Type*)(0)); + if(!is_convertible) + return; + + typedef ::pinocchio::python::StdContainerFromPythonList Constructor; + Constructor::construct(py_obj,&m_data.stage1); + + void * & m_result = const_cast(result()); + m_result = m_data.stage1.convertible; + vec_ptr = reinterpret_cast(m_data.storage.bytes); + } + + result_type operator()() const + { + return ::boost::python::detail::void_ptr_to_reference(result(), + (result_type(*)())0); } + + ~reference_arg_from_python() + { + if(m_data.stage1.convertible == m_data.storage.bytes) + { + // Copy back the reference + const vector_type & vec = *vec_ptr; + list bp_list(handle<>(borrowed(m_source))); + for(size_t i = 0; i < vec.size(); ++i) + { + Type & elt = extract(bp_list[i]); + elt = vec[i]; + } + } + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + vector_type * vec_ptr; + }; + +}}} // boost::python::converter + +namespace pinocchio +{ + namespace python + { /// /// \brief Register the conversion from a Python list to a std::vector /// /// \tparam vector_type A std container (e.g. std::vector or std::list) /// - template + template struct StdContainerFromPythonList { typedef typename vector_type::value_type T; + typedef typename vector_type::allocator_type Allocator; /// \brief Check if obj_ptr can be converted static void* convertible(PyObject* obj_ptr) @@ -155,6 +283,84 @@ namespace pinocchio return python_list; } }; + + namespace internal + { + +// template struct has_operator_equal; + template + struct has_operator_equal : + boost::mpl::if_,T>::type,has_operator_equal< NumericalBase >, + typename boost::mpl::if_,T>::type,has_operator_equal< Eigen::EigenBase >, + boost::true_type>::type > + {}; + + + template + struct has_operator_equal< std::vector > : has_operator_equal + {}; + + template<> + struct has_operator_equal : boost::true_type + {}; + + template + struct has_operator_equal< Eigen::EigenBase > : has_operator_equal + {}; + + template + struct has_operator_equal< NumericalBase > : has_operator_equal::Scalar> + {}; + + template >::value > + struct contains_algo; + + template + struct contains_algo + { + template + static bool run(Container & container, key_type const & key) + { + return std::find(container.begin(), container.end(), key) + != container.end(); + } + }; + + template + struct contains_algo + { + template + static bool run(Container & container, key_type const & key) + { + for(size_t k = 0; k < container.size(); ++k) + { + if(&container[k] == &key) + return true; + } + return false; + } + }; + + template + struct contains_vector_derived_policies + : public ::boost::python::vector_indexing_suite > + { + typedef typename Container::value_type key_type; + + static bool contains(Container & container, key_type const & key) + { + return contains_algo::run(container,key); + } + }; + } + + struct EmptyPythonVisitor + : public ::boost::python::def_visitor + { + template + void visit(classT &) const + {} + }; /// /// \brief Expose an std::vector from a type given as template argument. @@ -168,29 +374,49 @@ namespace pinocchio /// template, bool NoProxy = false, bool EnableFromPythonListConverter = true> struct StdVectorPythonVisitor - : public ::boost::python::vector_indexing_suite, NoProxy> - , public StdContainerFromPythonList< std::vector > + : public ::boost::python::vector_indexing_suite, NoProxy, internal::contains_vector_derived_policies,NoProxy> > + , public StdContainerFromPythonList< std::vector,NoProxy> { typedef std::vector vector_type; - typedef StdContainerFromPythonList FromPythonListConverter; + typedef StdContainerFromPythonList FromPythonListConverter; - static ::boost::python::class_ expose(const std::string & class_name, - const std::string & doc_string = "") + static void expose(const std::string & class_name, + const std::string & doc_string = "") + { + expose(class_name,doc_string,EmptyPythonVisitor()); + } + + template + static void expose(const std::string & class_name, + const boost::python::def_visitor & visitor) + { + expose(class_name,"",visitor); + } + + template + static void expose(const std::string & class_name, + const std::string & doc_string, + const boost::python::def_visitor & visitor) { namespace bp = boost::python; - bp::class_ cl(class_name.c_str(),doc_string.c_str()); - cl - .def(StdVectorPythonVisitor()) - .def("tolist",&FromPythonListConverter::tolist,bp::arg("self"), - "Returns the std::vector as a Python list.") - .def_pickle(PickleVector()); - - // Register conversion - if(EnableFromPythonListConverter) - FromPythonListConverter::register_converter(); - - return cl; + if(!register_symbolic_link_to_registered_type()) + { + bp::class_ cl(class_name.c_str(),doc_string.c_str()); + cl + .def(StdVectorPythonVisitor()) + .def("tolist",&FromPythonListConverter::tolist,bp::arg("self"), + "Returns the std::vector as a Python list.") + .def(visitor) +#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION + .def_pickle(PickleVector()) +#endif + ; + + // Register conversion + if(EnableFromPythonListConverter) + FromPythonListConverter::register_converter(); + } } }; diff --git a/bindings/python/utils/version.cpp b/bindings/python/utils/version.cpp index eff9b5dc57..8e84b8110d 100644 --- a/bindings/python/utils/version.cpp +++ b/bindings/python/utils/version.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2018 CNRS, INRIA +// Copyright (c) 2018 CNRS INRIA // #include "pinocchio/bindings/python/utils/constant.hpp" diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in index 7caa1e723c..d1b61dd758 100644 --- a/doc/Doxyfile.extra.in +++ b/doc/Doxyfile.extra.in @@ -397,6 +397,12 @@ VERBATIM_HEADERS = YES ALPHABETICAL_INDEX = YES +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 4 + #--------------------------------------------------------------------------- # configuration options related to the HTML output #--------------------------------------------------------------------------- diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index eafb34cf66..e96f40c91d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -83,7 +83,8 @@ IF(BUILD_PYTHON_INTERFACE) LIST(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES sample-model-viewer display-shapes - simulation-pendulum + simulation-inverted-pendulum + #simulation-contact-dynamics ) IF(BUILD_WITH_URDF_SUPPORT) LIST(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES diff --git a/examples/capsule-approximation.py b/examples/capsule-approximation.py index d8c868b205..cc459c256c 100644 --- a/examples/capsule-approximation.py +++ b/examples/capsule-approximation.py @@ -150,5 +150,5 @@ def set_transform(origin, a, b): # Example for a whole URDF model # This path refers to Pinocchio source code but you can define your own directory here. pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") - urdf_filename = pinocchio_model_dir + "models/others/robots/ur_description/urdf/ur5_gripper.urdf" + urdf_filename = pinocchio_model_dir + "models/example-robot-data/robots/ur_description/urdf/ur5_gripper.urdf" parse_urdf(urdf_filename, "ur5_gripper_with_capsules.urdf") diff --git a/examples/contact-cholesky.py b/examples/contact-cholesky.py new file mode 100644 index 0000000000..278d97e756 --- /dev/null +++ b/examples/contact-cholesky.py @@ -0,0 +1,37 @@ +import pinocchio as pin +import numpy as np + +from numpy.linalg import norm, inv +from os.path import join, dirname, abspath +from math import sqrt + +pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models") +urdf_filename = pinocchio_model_dir + '/others/robots/anymal_b_simple_description/robots/anymal.urdf' +model = pin.buildModelFromUrdf(urdf_filename) +data = model.createData() + +q0 = pin.neutral(model) + +# Add feet frames +feet_names = ["LH_FOOT","RH_FOOT","LF_FOOT","RF_FOOT"] +feet_frame_ids = [] +for foot_name in feet_names: + + frame_id = model.getFrameId(foot_name) + feet_frame_ids.append(frame_id) + +contact_models = [] +for fid in feet_frame_ids: + frame = model.frames[fid] + cmodel = pin.RigidConstraintModel(pin.ContactType.CONTACT_3D,frame.parent,frame.placement,pin.LOCAL_WORLD_ALIGNED) + contact_models.append(cmodel) + +contact_data = [cmodel.createData() for cmodel in contact_models] + +pin.initConstraintDynamics(model,data,contact_models) +pin.crba(model,data,q0) + +data.contact_chol.compute(model,data,contact_models,contact_data) + +delassus_matrix = data.contact_chol.getInverseOperationalSpaceInertiaMatrix() +delassus_matrix_inv = data.contact_chol.getOperationalSpaceInertiaMatrix() \ No newline at end of file diff --git a/examples/inverse-kinematics.cpp b/examples/inverse-kinematics.cpp index b9b16fc6bf..15151978e9 100644 --- a/examples/inverse-kinematics.cpp +++ b/examples/inverse-kinematics.cpp @@ -1,3 +1,5 @@ +#include + #include "pinocchio/parsers/sample-models.hpp" #include "pinocchio/spatial/explog.hpp" #include "pinocchio/algorithm/kinematics.hpp" diff --git a/examples/overview-simple.cpp b/examples/overview-simple.cpp index 535cbe7ba9..65b5650d12 100644 --- a/examples/overview-simple.cpp +++ b/examples/overview-simple.cpp @@ -1,3 +1,5 @@ +#include + #include "pinocchio/parsers/sample-models.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/rnea.hpp" diff --git a/examples/simulation-contact-dynamics.py b/examples/simulation-contact-dynamics.py new file mode 100644 index 0000000000..e4c4c804a3 --- /dev/null +++ b/examples/simulation-contact-dynamics.py @@ -0,0 +1,127 @@ +import pinocchio as pin + +import math +import numpy as np +import sys +import os +from os.path import dirname, join, abspath +import time + +from pinocchio.visualize import MeshcatVisualizer + +# Load the URDF model. +# Conversion with str seems to be necessary when executing this file with ipython +pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models") + +# Load the URDF model. +# Conversion with str seems to be necessary when executing this file with ipython +pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models") +model_path = join(pinocchio_model_dir,"example-robot-data/robots") +mesh_dir = pinocchio_model_dir +urdf_filename = "talos_reduced.urdf" +urdf_model_path = join(join(model_path,"talos_data/robots"),urdf_filename) +srdf_filename = "talos.srdf" +srdf_full_path = join(join(model_path,"talos_data/srdf"),srdf_filename) + +model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()) + +viz = MeshcatVisualizer(model, collision_model, visual_model) + +# Start a new MeshCat server and client. +# Note: the server can also be started separately using the "meshcat-server" command in a terminal: +# this enables the server to remain active after the current script ends. +# +# Option open=True pens the visualizer. +# Note: the visualizer can also be opened seperately by visiting the provided URL. +try: + viz.initViewer(open=False) +except ImportError as err: + print("Error while initializing the viewer. It seems you should install Python meshcat") + print(err) + sys.exit(0) + +# Load the robot in the viewer. +viz.loadViewerModel() + +# Display a robot configuration. +pin.loadReferenceConfigurations(model,srdf_full_path) +q0 = model.referenceConfigurations["half_sitting"] +q_ref = pin.integrate(model,q0,0.1*np.random.rand((model.nv))) +viz.display(q0) + +feet_name = ["left_sole_link","right_sole_link"] +frame_ids = [ model.getFrameId(frame_name) for frame_name in feet_name ] + +v0 = np.zeros((model.nv)) +v_ref = v0.copy() +data_sim = model.createData() +data_control = model.createData() + +contact_models = [] +contact_datas = [] + + +for frame_id in frame_ids: + frame = model.frames[frame_id] + contact_model = pin.RigidConstraintModel(pin.ContactType.CONTACT_6D,frame.parent,frame.placement) + + contact_models.append(contact_model) + contact_datas.append(contact_model.createData()) + +num_constraints = len(frame_ids) +contact_dim = 6 * num_constraints + +pin.initConstraintDynamics(model,data_sim,contact_models) + +t = 0 +dt = 5e-3 + +S = np.zeros((model.nv-6,model.nv)) +S.T[6:,:] = np.eye(model.nv-6) +Kp_posture = 30. +Kv_posture = 0.05*math.sqrt(Kp_posture) + +q = q0.copy() +v = v0.copy() +tau = np.zeros((model.nv)) + +T = 3 + +while t <= T: + print("t:",t) + t += dt + + tic = time.time() + J_constraint = np.zeros((contact_dim,model.nv)) + pin.computeJointJacobians(model,data_control,q) + constraint_index = 0 + for k in range(num_constraints): + contact_model = contact_models[k] + J_constraint[constraint_index:constraint_index+6,:] = pin.getFrameJacobian(model,data_control,contact_model.joint1_id,contact_model.joint1_placement,contact_model.reference_frame) + constraint_index += 6 + + A = np.vstack((S,J_constraint)) + b = pin.rnea(model,data_control,q,v,np.zeros((model.nv))) + + sol = np.linalg.lstsq(A.T,b,rcond=None)[0] + tau = np.concatenate((np.zeros((6)),sol[:model.nv-6])) + + tau[6:] += -Kp_posture*(pin.difference(model,q_ref,q))[6:] - Kv_posture*(v - v_ref)[6:] + + prox_settings = pin.ProximalSettings(1e-12,1e-12,10) + a = pin.constraintDynamics(model,data_sim,q,v,tau,contact_models,contact_datas,prox_settings) + print("a:",a.T) + print("v:",v.T) + print("constraint:",np.linalg.norm(J_constraint@a)) + print("iter:",prox_settings.iter) + + v += a * dt + q = pin.integrate(model,q,v*dt) + + viz.display(q) + elapsed_time = time.time() - tic + + time.sleep(max(0,dt - elapsed_time)) + #input() + + diff --git a/examples/simulation-inverted-pendulum.py b/examples/simulation-inverted-pendulum.py new file mode 100644 index 0000000000..e3a9756de2 --- /dev/null +++ b/examples/simulation-inverted-pendulum.py @@ -0,0 +1,96 @@ +import pinocchio as pin +import hppfcl as fcl +import numpy as np +import math +import time +import sys + +N = 10 # number of pendulums +model = pin.Model() +geom_model = pin.GeometryModel() + +parent_id = 0 +joint_placement = pin.SE3.Identity() +body_mass = 1. +body_radius = 0.1 + +shape0 = fcl.Sphere(body_radius) +geom0_obj = pin.GeometryObject("base", 0, shape0, pin.SE3.Identity()) +geom0_obj.meshColor = np.array([1.,0.1,0.1,1.]) +geom_model.addGeometryObject(geom0_obj) + +for k in range(N): + joint_name = "joint_" + str(k+1) + joint_id = model.addJoint(parent_id,pin.JointModelRY(),joint_placement,joint_name) + + body_inertia = pin.Inertia.FromSphere(body_mass,body_radius) + body_placement = joint_placement.copy() + body_placement.translation[2] = 1. + model.appendBodyToJoint(joint_id,body_inertia,body_placement) + + geom1_name = "ball_" + str(k+1) + shape1 = fcl.Sphere(body_radius) + geom1_obj = pin.GeometryObject(geom1_name, joint_id, shape1, body_placement) + geom1_obj.meshColor = np.ones((4)) + geom_model.addGeometryObject(geom1_obj) + + geom2_name = "bar_" + str(k+1) + shape2 = fcl.Cylinder(body_radius/4.,body_placement.translation[2]) + shape2_placement = body_placement.copy() + shape2_placement.translation[2] /= 2. + + geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2, shape2_placement) + geom2_obj.meshColor = np.array([0.,0.,0.,1.]) + geom_model.addGeometryObject(geom2_obj) + + parent_id = joint_id + joint_placement = body_placement.copy() + +from pinocchio.visualize import GepettoVisualizer + +visual_model = geom_model +viz = GepettoVisualizer(model, geom_model, visual_model) + +# Initialize the viewer. +try: + viz.initViewer() +except ImportError as err: + print("Error while initializing the viewer. It seems you should install gepetto-viewer") + print(err) + sys.exit(0) + +try: + viz.loadViewerModel("pinocchio") +except AttributeError as err: + print("Error while loading the viewer model. It seems you should start gepetto-viewer") + print(err) + sys.exit(0) + +# Display a robot configuration. +q0 = pin.neutral(model) +viz.display(q0) + +# Play a bit with the simulation +dt = 0.01 +T = 5 + +N = math.floor(T/dt) + +model.lowerPositionLimit.fill(-math.pi) +model.upperPositionLimit.fill(+math.pi) +q = pin.randomConfiguration(model) +v = np.zeros((model.nv)) + +t = 0. +data_sim = model.createData() +for k in range(N): + tau_control = np.zeros((model.nv)) + a = pin.aba(model,data_sim,q,v,tau_control) # Forward dynamics + + v += a*dt + #q += v*dt + q = pin.integrate(model,q,v*dt) + + viz.display(q) + time.sleep(dt) + t += dt diff --git a/package.xml b/package.xml index 1178db3e98..6f49d656fa 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ pinocchio - 2.6.1 + 2.9.0 A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ba9eb1676a..6784378321 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -10,6 +10,12 @@ IF(BUILD_WITH_URDF_SUPPORT) parsers/urdf/utils.cpp ) ENDIF() +IF(BUILD_WITH_SDF_SUPPORT) + LIST(APPEND ${PROJECT_NAME}_SOURCES + parsers/sdf/model.cpp + parsers/sdf/geometry.cpp + ) +ENDIF() LIST(APPEND ${PROJECT_NAME}_SOURCES utils/file-explorer.cpp) @@ -57,6 +63,10 @@ IF(urdfdom_FOUND) TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM PUBLIC ${urdfdom_INCLUDE_DIRS}) ENDIF(urdfdom_FOUND) +IF(SDFormat_FOUND) + TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC ${SDFormat_LIBRARIES}) +ENDIF(SDFormat_FOUND) + IF(hpp-fcl_FOUND) TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC hpp-fcl::hpp-fcl) ENDIF(hpp-fcl_FOUND) diff --git a/src/algorithm/aba-derivatives.hpp b/src/algorithm/aba-derivatives.hpp index 5b950aab34..87b3dd14c9 100644 --- a/src/algorithm/aba-derivatives.hpp +++ b/src/algorithm/aba-derivatives.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2018 CNRS, INRIA +// Copyright (c) 2018-2020 CNRS INRIA // -#ifndef __pinocchio_aba_derivatives_hpp__ -#define __pinocchio_aba_derivatives_hpp__ +#ifndef __pinocchio_algorithm_aba_derivatives_hpp__ +#define __pinocchio_algorithm_aba_derivatives_hpp__ #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" @@ -150,4 +150,4 @@ namespace pinocchio /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/algorithm/aba-derivatives.hxx" -#endif // ifndef __pinocchio_aba_derivatives_hpp__ +#endif // ifndef __pinocchio_algorithm_aba_derivatives_hpp__ diff --git a/src/algorithm/aba-derivatives.hxx b/src/algorithm/aba-derivatives.hxx index 80a6d91733..8df1725e55 100644 --- a/src/algorithm/aba-derivatives.hxx +++ b/src/algorithm/aba-derivatives.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2018-2020 CNRS INRIA +// Copyright (c) 2018-2021 CNRS INRIA // #ifndef __pinocchio_algorithm_aba_derivatives_hxx__ @@ -35,31 +35,31 @@ namespace pinocchio { typedef typename Model::JointIndex JointIndex; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; typename Data::Motion & ov = data.ov[i]; jmodel.calc(jdata.derived(),q.derived(),v.derived()); data.liMi[i] = model.jointPlacements[i]*jdata.M(); - data.v[i] = jdata.v(); - if(parent > 0) - { data.oMi[i] = data.oMi[parent] * data.liMi[i]; - data.v[i] += data.liMi[i].actInv(data.v[parent]); - } else data.oMi[i] = data.liMi[i]; - ov = data.oMi[i].act(data.v[i]); - data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); - data.Yaba[i] = model.inertias[i].matrix(); + ov = data.oMi[i].act(jdata.v()); + if(parent > 0) + ov += data.ov[parent]; + + data.oa_gf[i] = data.oMi[i].act(jdata.c()); + if(parent > 0) + data.oa_gf[i] += (data.ov[parent] ^ ov); + data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]); - + data.oYaba[i] = data.oYcrb[i].matrix(); + data.oh[i] = data.oYcrb[i] * ov; data.of[i] = ov.cross(data.oh[i]); - data.f[i] = data.oMi[i].actInv(data.of[i]); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; ColsBlock J_cols = jmodel.jointCols(data.J); @@ -87,20 +87,32 @@ namespace pinocchio const Eigen::MatrixBase & Minv) { typedef typename Model::JointIndex JointIndex; + typedef typename Data::Inertia Inertia; + typedef typename Data::Force Force; + typedef typename Data::Matrix6x Matrix6x; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; - typename Data::Inertia::Matrix6 & Ia = data.Yaba[i]; - jmodel.calc_aba(jdata.derived(), Ia, parent > 0); + typename Inertia::Matrix6 & Ia = data.oYaba[i]; - typename Data::Matrix6x & Fcrb = data.Fcrb[0]; - typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); + Matrix6x & Fcrb = data.Fcrb[0]; typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - - ColsBlock U_cols = jmodel.jointCols(data.IS); - forceSet::se3Action(data.oMi[i],jdata.U(),U_cols); // expressed in the world frame + Force & fi = data.of[i]; + + typedef typename SizeDepType::template ColsReturn::Type ColBlock; + const ColBlock J_cols = jmodel.jointCols(data.J); + + jmodel.jointVelocitySelector(data.u).noalias() -= J_cols.transpose()*fi.toVector(); + + jdata.U().noalias() = Ia * J_cols; + jdata.StU().noalias() = J_cols.transpose() * jdata.U(); + + jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + + internal::PerformStYSInversion::run(jdata.StU(),jdata.Dinv()); + jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,Minv); @@ -108,7 +120,6 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if(nv_children > 0) { - ColsBlock J_cols = jmodel.jointCols(data.J); ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); @@ -117,25 +128,22 @@ namespace pinocchio if(parent > 0) { - FcrbTmp.leftCols(data.nvSubtree[i]).noalias() - = U_cols * Minv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); - Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]) += FcrbTmp.leftCols(data.nvSubtree[i]); + Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias() += jdata.U() * Minv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]);; } } else // This a leaf of the kinematic tree { Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias() - = U_cols * Minv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); + = jdata.U() * Minv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); } - - jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i]; if (parent > 0) { - typename Data::Force & pa = data.f[i]; - pa.toVector() += Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); - data.Yaba[parent] += internal::SE3actOn::run(data.liMi[i], Ia); - data.f[parent] += data.liMi[i].act(pa); + Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); + + fi.toVector().noalias() += Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + data.oYaba[parent] += Ia; + data.of[parent] += fi; } } @@ -161,41 +169,35 @@ namespace pinocchio MatrixType & Minv) { typedef typename Model::JointIndex JointIndex; + typedef typename Data::Matrix6x Matrix6x; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; typename Data::Motion & ov = data.ov[i]; typename Data::Motion & oa = data.oa[i]; - typename Data::Motion & oa_gf = data.oa_gf[i]; typename Data::Force & of = data.of[i]; + typename Data::Motion & oa_gf = data.oa_gf[i]; + + typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + ColsBlock J_cols = jmodel.jointCols(data.J); - data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); + oa_gf += data.oa_gf[parent]; jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.a_gf[i].toVector(); + jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * oa_gf.toVector(); - data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); - oa_gf = data.oMi[i].act(data.a_gf[i]); + oa_gf.toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq); oa = oa_gf + model.gravity; of = data.oYcrb[i] * oa_gf + ov.cross(data.oh[i]); - typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); - - typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock UDinv_cols = jmodel.jointCols(data.UDinv); - forceSet::se3Action(data.oMi[i],jdata.UDinv(),UDinv_cols); // expressed in the world frame - MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,Minv); if(parent > 0) { - FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias() - = UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); - Minv_.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()) - -= FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); + Minv_.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias() + -= jdata.UDinv().transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());; } - ColsBlock J_cols = jmodel.jointCols(data.J); data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() = J_cols * Minv_.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); if(parent > 0) @@ -251,14 +253,15 @@ namespace pinocchio Data & data) { typedef typename Model::JointIndex JointIndex; + typedef Eigen::Matrix MatrixNV6; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; - typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) StdY(jmodel.nv(),6); - typename Data::MatrixXs & rnea_partial_dq = data.dtau_dq; - typename Data::MatrixXs & rnea_partial_dv = data.dtau_dv; + typename Data::RowMatrixXs & rnea_partial_dq = data.dtau_dq; + typename Data::RowMatrixXs & rnea_partial_dv = data.dtau_dv; typedef typename SizeDepType::template ColsReturn::Type ColsBlock; @@ -268,10 +271,11 @@ namespace pinocchio ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); + ColsBlock dFda_cols = jmodel.jointCols(data.dFda); // dtau/dv motionSet::inertiaAction(data.oYcrb[i],dAdv_cols,dFdv_cols); - dFdv_cols += data.doYcrb[i] * J_cols; + dFdv_cols.noalias() += data.doYcrb[i] * J_cols; rnea_partial_dv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() = J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); @@ -286,19 +290,19 @@ namespace pinocchio motionSet::act(J_cols,data.of[i],dFdq_cols); + motionSet::inertiaAction(data.oYcrb[i],J_cols,dFda_cols); if(parent > 0) { - lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv())); for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) - rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j); + rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = dFda_cols.transpose() * data.dAdq.col(j); for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) - rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdv.col(j); + rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = dFda_cols.transpose() * data.dAdv.col(j); - M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.doYcrb[i]; + StdY.noalias() = J_cols.transpose() * data.doYcrb[i]; for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) - rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.dVdq.col(j); + rnea_partial_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += StdY * data.dVdq.col(j); for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) - rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.J.col(j); + rnea_partial_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += StdY * data.J.col(j); } if(parent>0) @@ -309,8 +313,6 @@ namespace pinocchio } // Restore the status of dAdq_cols (remove gravity) - PINOCCHIO_CHECK_INPUT_ARGUMENT(isZero(model.gravity.angular()), - "The gravity must be a pure force vector, no angular part"); for(Eigen::DenseIndex k =0; k < jmodel.nv(); ++k) { MotionRef m_in(J_cols.col(k)); @@ -318,15 +320,6 @@ namespace pinocchio m_out.linear() += model.gravity.linear().cross(m_in.angular()); } } - - template - static void lhsInertiaMult(const typename Data::Inertia & Y, - const Eigen::MatrixBase & J, - const Eigen::MatrixBase & F) - { - Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F); - motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose()); - } }; template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, @@ -349,11 +342,12 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.rows(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.rows(), model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(isZero(model.gravity.angular()), + "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); typedef typename ModelTpl::JointIndex JointIndex; - data.a_gf[0] = -model.gravity; data.oa_gf[0] = -model.gravity; data.u = tau; @@ -419,11 +413,12 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.rows(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.rows(), model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(isZero(model.gravity.angular()), + "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); typedef typename ModelTpl::JointIndex JointIndex; - data.a_gf[0] = -model.gravity; data.oa_gf[0] = -model.gravity; data.u = tau; @@ -436,7 +431,7 @@ namespace pinocchio { Pass1::run(model.joints[i],data.joints[i], typename Pass1::ArgsType(model,data,q.derived(),v.derived())); - data.f[i] -= fext[i]; + data.of[i] -= data.oMi[i].act(fext[i]); } data.Fcrb[0].setZero(); @@ -468,8 +463,7 @@ namespace pinocchio PINOCCHIO_EIGEN_CONST_CAST(MatrixType1,aba_partial_dq).noalias() = -Minv_*data.dtau_dq; PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,aba_partial_dv).noalias() = -Minv_*data.dtau_dv; } - - + } // namespace pinocchio #endif // ifndef __pinocchio_algorithm_aba_derivatives_hxx__ diff --git a/src/algorithm/aba.hpp b/src/algorithm/aba.hpp index 986ff2e7b2..d046239a34 100644 --- a/src/algorithm/aba.hpp +++ b/src/algorithm/aba.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2016-2018 CNRS +// Copyright (c) 2016-2020 CNRS // -#ifndef __pinocchio_aba_hpp__ -#define __pinocchio_aba_hpp__ +#ifndef __pinocchio_algorithm_aba_hpp__ +#define __pinocchio_algorithm_aba_hpp__ #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" @@ -11,8 +11,10 @@ namespace pinocchio { + /// /// \brief The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model. + /// This is the original implementation, considering all quantities to be expressed in the LOCAL coordinate systems of the joint frames. /// /// \tparam JointCollection Collection of Joint types. /// \tparam ConfigVectorType Type of the joint configuration vector. @@ -38,7 +40,8 @@ namespace pinocchio const Eigen::MatrixBase & tau); /// - /// \brief The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model. + /// \brief The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model and the external forces. + /// This is the original implementation, considering all quantities to be expressed in the LOCAL coordinate systems of the joint frames. /// /// \tparam JointCollection Collection of Joint types. /// \tparam ConfigVectorType Type of the joint configuration vector. @@ -65,7 +68,7 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & tau, const container::aligned_vector & fext); - + /// /// \brief Computes the inverse of the joint space inertia matrix using Articulated Body formulation. /// \remarks Only the upper triangular part of the matrix is filled. @@ -93,4 +96,4 @@ namespace pinocchio /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/algorithm/aba.hxx" -#endif // ifndef __pinocchio_aba_hpp__ +#endif // ifndef __pinocchio_algorithm_aba_hpp__ diff --git a/src/algorithm/aba.hxx b/src/algorithm/aba.hxx index 1846b7aaaf..5111103064 100644 --- a/src/algorithm/aba.hxx +++ b/src/algorithm/aba.hxx @@ -1,9 +1,9 @@ // -// Copyright (c) 2016-2020 CNRS, INRIA +// Copyright (c) 2016-2021 CNRS INRIA // -#ifndef __pinocchio_aba_hxx__ -#define __pinocchio_aba_hxx__ +#ifndef __pinocchio_algorithm_aba_hxx__ +#define __pinocchio_algorithm_aba_hxx__ #include "pinocchio/spatial/act-on-set.hpp" #include "pinocchio/multibody/visitor.hpp" @@ -13,47 +13,7 @@ namespace pinocchio { - template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> - struct AbaForwardStep1 - : public fusion::JointUnaryVisitorBase< AbaForwardStep1 > - { - typedef ModelTpl Model; - typedef DataTpl Data; - - typedef boost::fusion::vector ArgsType; - - template - static void algo(const pinocchio::JointModelBase & jmodel, - pinocchio::JointDataBase & jdata, - const Model & model, - Data & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v) - { - typedef typename Model::JointIndex JointIndex; - - const JointIndex & i = jmodel.id(); - jmodel.calc(jdata.derived(),q.derived(),v.derived()); - - const JointIndex & parent = model.parents[i]; - data.liMi[i] = model.jointPlacements[i] * jdata.M(); - - data.v[i] = jdata.v(); - if (parent>0) - data.v[i] += data.liMi[i].actInv(data.v[parent]); - data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); - - data.Yaba[i] = model.inertias[i].matrix(); - data.f[i] = model.inertias[i].vxiv(data.v[i]); // -f_ext - } - - }; - namespace internal { @@ -114,9 +74,50 @@ namespace pinocchio return res; } }; + } // namespace internal + + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> + struct AbaForwardStep1 + : public fusion::JointUnaryVisitorBase< AbaForwardStep1 > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const pinocchio::JointModelBase & jmodel, + pinocchio::JointDataBase & jdata, + const Model & model, + Data & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v) + { + typedef typename Model::JointIndex JointIndex; + + const JointIndex i = jmodel.id(); + jmodel.calc(jdata.derived(),q.derived(),v.derived()); + + const JointIndex & parent = model.parents[i]; + data.liMi[i] = model.jointPlacements[i] * jdata.M(); + + data.v[i] = jdata.v(); + if (parent>0) + data.v[i] += data.liMi[i].actInv(data.v[parent]); + + data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); + + data.Yaba[i] = model.inertias[i].matrix(); + data.h[i] = model.inertias[i] * data.v[i]; + data.f[i] = data.v[i].cross(data.h[i]); // -f_ext + } + + }; - } - template class JointCollectionTpl> struct AbaBackwardStep : public fusion::JointUnaryVisitorBase< AbaBackwardStep > @@ -125,7 +126,7 @@ namespace pinocchio typedef DataTpl Data; typedef boost::fusion::vector ArgsType; + Data &> ArgsType; template static void algo(const JointModelBase & jmodel, @@ -137,13 +138,15 @@ namespace pinocchio typedef typename Data::Inertia Inertia; typedef typename Data::Force Force; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; typename Inertia::Matrix6 & Ia = data.Yaba[i]; jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i]; - jmodel.calc_aba(jdata.derived(), Ia, parent > 0); - + jmodel.calc_aba(jdata.derived(), + jmodel.jointVelocitySelector(model.armature), + Ia, parent > 0); + if (parent > 0) { Force & pa = data.f[i]; @@ -154,9 +157,7 @@ namespace pinocchio } }; - - - + template class JointCollectionTpl> struct AbaForwardStep2 : public fusion::JointUnaryVisitorBase< AbaForwardStep2 > @@ -165,7 +166,7 @@ namespace pinocchio typedef DataTpl Data; typedef boost::fusion::vector ArgsType; + Data &> ArgsType; template static void algo(const pinocchio::JointModelBase & jmodel, @@ -175,13 +176,17 @@ namespace pinocchio { typedef typename Model::JointIndex JointIndex; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); jmodel.jointVelocitySelector(data.ddq).noalias() = jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.a_gf[i].toVector(); data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); + + data.a[i] = data.a_gf[i]; + data.a[i].linear() += data.oMi[i].rotation().transpose() * model.gravity.linear(); + data.f[i] = model.inertias[i] * data.a_gf[i] + data.v[i].cross(data.h[i]); } }; @@ -197,12 +202,13 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size"); - PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size");; typedef typename ModelTpl::JointIndex JointIndex; data.v[0].setZero(); data.a_gf[0] = -model.gravity; + data.f[0].setZero(); data.u = tau; typedef AbaForwardStep1 Pass1; @@ -218,7 +224,7 @@ namespace pinocchio Pass2::run(model.joints[i],data.joints[i], typename Pass2::ArgsType(model,data)); } - + typedef AbaForwardStep2 Pass3; for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) { @@ -226,6 +232,12 @@ namespace pinocchio typename Pass3::ArgsType(model,data)); } + for(JointIndex i=(JointIndex)model.njoints-1;i>0; --i) + { + const JointIndex parent = model.parents[i]; + data.f[parent] += data.liMi[i].act(data.f[i]); + } + return data.ddq; } @@ -272,6 +284,12 @@ namespace pinocchio typename Pass3::ArgsType(model,data)); } + for(JointIndex i=(JointIndex)model.njoints-1;i>0; --i) + { + const JointIndex parent = model.parents[i]; + data.f[parent] += data.liMi[i].act(data.f[i]); + } + return data.ddq; } @@ -311,7 +329,8 @@ namespace pinocchio ColsBlock J_cols = jmodel.jointCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - data.Yaba[i] = model.inertias[i].matrix(); + data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); + data.oYaba[i] = data.oYcrb[i].matrix(); } }; @@ -335,26 +354,29 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Inertia Inertia; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; - typename Inertia::Matrix6 & Ia = data.Yaba[i]; + typename Inertia::Matrix6 & Ia = data.oYaba[i]; typename Data::RowMatrixXs & Minv = data.Minv; typename Data::Matrix6x & Fcrb = data.Fcrb[0]; - typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); - - jmodel.calc_aba(jdata.derived(), Ia, parent > 0); - typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + + ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock U_cols = jmodel.jointCols(data.IS); - forceSet::se3Action(data.oMi[i],jdata.U(),U_cols); // expressed in the world frame + jdata.U().noalias() = Ia * J_cols; + jdata.StU().noalias() = J_cols.transpose() * jdata.U(); + + // Account for the rotor inertia contribution + jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + + internal::PerformStYSInversion::run(jdata.StU(),jdata.Dinv()); + jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),jmodel.nv()) = jdata.Dinv(); const int nv_children = data.nvSubtree[i] - jmodel.nv(); if(nv_children > 0) { - ColsBlock J_cols = jmodel.jointCols(data.J); ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); @@ -363,21 +385,24 @@ namespace pinocchio if(parent > 0) { - FcrbTmp.leftCols(data.nvSubtree[i]).noalias() - = U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); - Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]) += FcrbTmp.leftCols(data.nvSubtree[i]); + Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias() + += jdata.U() * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]);; } } else { Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias() - = U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); + = jdata.U() * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); } if(parent > 0) - data.Yaba[parent] += internal::SE3actOn::run(data.liMi[i], Ia); + { + Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); + data.oYaba[parent] += Ia; + } } }; + template class JointCollectionTpl> struct ComputeMinverseForwardStep2 @@ -397,27 +422,24 @@ namespace pinocchio { typedef typename Model::JointIndex JointIndex; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; typename Data::RowMatrixXs & Minv = data.Minv; - typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); - + typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock UDinv_cols = jmodel.jointCols(data.UDinv); - forceSet::se3Action(data.oMi[i],jdata.UDinv(),UDinv_cols); // expressed in the world frame ColsBlock J_cols = jmodel.jointCols(data.J); if(parent > 0) { - FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias() - = UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); - Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()) - -= FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); + Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias() + -= jdata.UDinv().transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); } - data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() = J_cols * Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); + data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() + = J_cols * Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); if(parent > 0) - data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); + data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) + += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); } }; @@ -485,4 +507,4 @@ namespace pinocchio /// @endcond -#endif // ifndef __pinocchio_aba_hxx__ +#endif // ifndef __pinocchio_algorithm_aba_hxx__ diff --git a/src/algorithm/center-of-mass-derivatives.hxx b/src/algorithm/center-of-mass-derivatives.hxx index fe7c8a221e..4f251a74bf 100644 --- a/src/algorithm/center-of-mass-derivatives.hxx +++ b/src/algorithm/center-of-mass-derivatives.hxx @@ -36,6 +36,8 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; + typedef Eigen::Matrix Matrix6NV; + const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; @@ -46,8 +48,7 @@ namespace pinocchio Motion vpc = (parent>0) ? (data.v[i]-(Motion)jdata.v()) : Motion::Zero(); vpc.linear() -= data.vcom[i]; // vpc = v_{parent+c} = [ v_parent+vc; w_parent ] - typename SizeDepType::template ColsReturn::Type vxS = - SizeDepType::middleCols(data.M6tmp,0,jmodel.nv()); + Matrix6NV vxS(6,jmodel.nv()); vxS = vpc.cross(jdata.S()); dvcom_dqi.noalias() = (data.mass[i]/data.mass[0])*data.oMi[i].rotation() diff --git a/src/algorithm/center-of-mass.hpp b/src/algorithm/center-of-mass.hpp index 5741664c16..5958131452 100644 --- a/src/algorithm/center-of-mass.hpp +++ b/src/algorithm/center-of-mass.hpp @@ -305,7 +305,7 @@ namespace pinocchio /// /// \return The jacobian of the CoM expressed in the world frame (matrix 3 x model.nv). /// - /// \remark This extraction of inertial quantities is only valid for free-floating base systems. + /// \remarks This extraction of inertial quantities is only valid for free-floating base systems. /// template class JointCollectionTpl> inline const typename DataTpl::Matrix3x & diff --git a/src/algorithm/center-of-mass.hxx b/src/algorithm/center-of-mass.hxx index f84c6c500e..3a7d392fa5 100644 --- a/src/algorithm/center-of-mass.hxx +++ b/src/algorithm/center-of-mass.hxx @@ -195,7 +195,7 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); - return data.com[0] = data.liMi[1].act(data.Ycrb[1].lever()); + return data.com[0] = data.liMi[1].act(data.oYcrb[1].lever()); } /* --- JACOBIAN ---------------------------------------------------------- */ @@ -409,7 +409,8 @@ namespace pinocchio typename Pass2::ArgsType(model,data,Jcom_subtree,computeSubtreeComs)); } - PINOCCHIO_CHECK_INPUT_ARGUMENT(data.mass[rootSubtreeId] > 0., "The mass of the subtree is not positive."); + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real(data.mass[rootSubtreeId] > 0.), + "The mass of the subtree is not positive."); const Scalar mass_inv_subtree = Scalar(1)/data.mass[rootSubtreeId]; typename Data::Vector3 & com_subtree = data.com[rootSubtreeId]; if(!computeSubtreeComs) @@ -485,22 +486,11 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); - typedef DataTpl Data; - typedef typename Data::SE3 SE3; - - const SE3 & oM1 = data.liMi[1]; - // Extract the total mass of the system. - data.mass[0] = data.M(0,0); - - // As the 6 first rows of M*a are a wrench, we just need to multiply by the - // relative rotation between the first joint and the world - const typename SE3::Matrix3 oR1_over_m (oM1.rotation() / data.M(0,0)); + data.mass[0] = data.oYcrb[0].mass(); + data.com[0] = data.oYcrb[0].lever(); - // I don't know why, but the colwise multiplication is much more faster - // than the direct Eigen multiplication - for(long k=0; k().col(k); + data.Jcom.noalias() = data.Ag.template middleRows<3>(Force::LINEAR) / data.mass[0]; return data.Jcom; } diff --git a/src/algorithm/centroidal-derivatives.hxx b/src/algorithm/centroidal-derivatives.hxx index c6c8c2c8d9..2b415649c9 100644 --- a/src/algorithm/centroidal-derivatives.hxx +++ b/src/algorithm/centroidal-derivatives.hxx @@ -176,24 +176,15 @@ namespace pinocchio motionSet::act(J_cols, data.oh[i], dHdq_cols); motionSet::inertiaAction(data.oYcrb[i], dVdq_cols, dHdq_cols); } - - template - static void lhsInertiaMult(const typename Data::Inertia & Y, - const Eigen::MatrixBase & J, - const Eigen::MatrixBase & F) - { - Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F); - motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose()); - } }; // struct CentroidalDynDerivativesBackwardStep namespace { // TODO: should be moved to ForceSet template - inline void translateForceSet(const Eigen::MatrixBase & Fin, - const Eigen::MatrixBase & v3, - const Eigen::MatrixBase & Fout) + void translateForceSet(const Eigen::MatrixBase & Fin, + const Eigen::MatrixBase & v3, + const Eigen::MatrixBase & Fout) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeIn,6,Eigen::Dynamic) EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3) @@ -211,6 +202,21 @@ namespace pinocchio fout.angular().noalias() = fin.angular() - v3.cross(fin.linear()); } } + + template + void translateForceSet(const Eigen::MatrixBase & F, + const Eigen::MatrixBase & v3) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLike,6,Eigen::Dynamic) + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3) + + for(Eigen::DenseIndex k = 0; k < F.cols(); ++k) + { + typedef ForceRef ForceType; + ForceType f(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,F).col(k)); + f.angular() -= v3.cross(f.linear()); + } + } } // internal namespace template class JointCollectionTpl, @@ -288,7 +294,8 @@ namespace pinocchio translateForceSet(data.dHdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike0,dh_dq)); translateForceSet(data.dFdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq)); translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv)); - translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da)); + translateForceSet(data.dFda,com,data.Ag); + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da) = data.Ag; } template class JointCollectionTpl> @@ -406,7 +413,8 @@ namespace pinocchio translateForceSet(data.dHdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike0,dh_dq)); translateForceSet(Ftmp,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq)); translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv)); - translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da)); + translateForceSet(data.dFda,com,data.Ag); + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da) = data.Ag; } } // namespace pinocchio diff --git a/src/algorithm/centroidal.hxx b/src/algorithm/centroidal.hxx index 9bd4b95f4c..4e9018a98e 100644 --- a/src/algorithm/centroidal.hxx +++ b/src/algorithm/centroidal.hxx @@ -173,7 +173,7 @@ namespace pinocchio typedef Eigen::Block Block3x; const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); - for (long i = 0; i #ifndef PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE -#define PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE 5 + #define PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE 5 #endif namespace pinocchio diff --git a/src/algorithm/check.hxx b/src/algorithm/check.hxx index cd6b133119..e856a50dd1 100644 --- a/src/algorithm/check.hxx +++ b/src/algorithm/check.hxx @@ -113,7 +113,7 @@ namespace pinocchio CHECK_DATA( data.U.cols() == model.nv ); CHECK_DATA( data.U.rows() == model.nv ); CHECK_DATA( data.D.size() == model.nv ); - CHECK_DATA( data.tmp.size() == model.nv ); + CHECK_DATA( data.tmp.size() >= model.nv ); CHECK_DATA( data.J.cols() == model.nv ); CHECK_DATA( data.Jcom.cols() == model.nv ); CHECK_DATA( data.torque_residual.size() == model.nv ); diff --git a/src/algorithm/chol.m b/src/algorithm/chol.m index aa9ec44ef1..303c2d1dc3 100644 --- a/src/algorithm/chol.m +++ b/src/algorithm/chol.m @@ -1,19 +1,6 @@ // // Copyright (c) 2015 CNRS // -// This file is part of Pinocchio -// Pinocchio is free software: you can redistribute it -// and/or modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation, either version -// 3 of the License, or (at your option) any later version. -// -// Pinocchio is distributed in the hope that it will be -// useful, but WITHOUT ANY WARRANTY; without even the implied warranty -// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Lesser Public License for more details. You should have -// received a copy of the GNU Lesser General Public License along with -// Pinocchio If not, see -// . parent = [ 0 1 2 3 2 5 2 7 8 9 8 11 ]; %parent = [ 0 1 2 3 2 5 2 7 8 9 10 8 12 13 ]; @@ -116,4 +103,4 @@ end -norm(U*diag(D)*U' - M) \ No newline at end of file +norm(U*diag(D)*U' - M) diff --git a/src/algorithm/cholesky.hpp b/src/algorithm/cholesky.hpp index 3116b203d1..e3b3f6b754 100644 --- a/src/algorithm/cholesky.hpp +++ b/src/algorithm/cholesky.hpp @@ -150,11 +150,11 @@ namespace pinocchio /// /// \return A reference to the result of \f$ U^{-1}v \f$ stored in v. /// - /// \remark The result is similar to the code data.U.triangularView ().solveInPlace(v). + /// \remarks The result is similar to the code data.U.triangularView ().solveInPlace(v). /// template class JointCollectionTpl, typename Mat> Mat & Uiv(const ModelTpl & model, - const DataTpl & data , + const DataTpl & data, const Eigen::MatrixBase & v); /// @@ -168,11 +168,11 @@ namespace pinocchio /// /// \return A reference to the result of \f$ U^{-\top}v \f$ stored in v. /// - /// \remark The result is similar to the code data.U.triangularView ().transpose().solveInPlace(v). + /// \remarks The result is similar to the code data.U.triangularView ().transpose().solveInPlace(v). /// template class JointCollectionTpl, typename Mat> Mat & Utiv(const ModelTpl & model, - const DataTpl & data , + const DataTpl & data, const Eigen::MatrixBase & v); /// @@ -188,7 +188,7 @@ namespace pinocchio /// template class JointCollectionTpl, typename Mat> Mat & solve(const ModelTpl & model, - const DataTpl & data , + const DataTpl & data, const Eigen::MatrixBase & v); /// diff --git a/src/algorithm/cholesky.hxx b/src/algorithm/cholesky.hxx index 0167c331a9..1a8ded5991 100644 --- a/src/algorithm/cholesky.hxx +++ b/src/algorithm/cholesky.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // #ifndef __pinocchio_cholesky_hxx__ @@ -441,18 +441,18 @@ namespace pinocchio { Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); internal::solve::run(model,data,m_); - return m_.derived(); + return m_; } namespace internal { - template class JointCollectionTpl, typename Mat> - Mat & Miunit(const ModelTpl & model, - const DataTpl & data, - const int col, - const Eigen::MatrixBase & v) + template class JointCollectionTpl, typename VectorLike> + VectorLike & Miunit(const ModelTpl & model, + const DataTpl & data, + const int col, + const Eigen::MatrixBase & v) { - EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike); PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); @@ -463,26 +463,26 @@ namespace pinocchio const typename Data::MatrixXs & U = data.U; const std::vector & nvt = data.nvSubtree_fromRow; - Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); + VectorLike & v_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike,v); + v_[col] = (typename VectorLike::Scalar)1; const int last_col = std::min(col-1,model.nv-2); // You can start from nv-2 (no child in nv-1) v_.tail(model.nv - col - 1).setZero(); - v_[col] = 1.; for( int k=last_col;k>=0;--k ) { - int nvt_max = std::min(col,nvt[(size_t)k]-1); + const int nvt_max = std::min(col,nvt[(size_t)k]-1); v_[k] = -U.row(k).segment(k+1,nvt_max).dot(v_.segment(k+1,nvt_max)); } v_.head(col+1).array() *= data.Dinv.head(col+1).array(); - for( int k=0;k() + = Minv_.transpose().template triangularView(); - return Minv_.derived(); + return Minv_; } } // namespace cholesky diff --git a/src/algorithm/compute-all-terms.hxx b/src/algorithm/compute-all-terms.hxx index f716d75317..c3e134401f 100644 --- a/src/algorithm/compute-all-terms.hxx +++ b/src/algorithm/compute-all-terms.hxx @@ -187,6 +187,9 @@ namespace pinocchio data.dhg = data.f[0]; data.dhg.angular() += data.dhg.linear().cross(data.com[0]); + + // Add the armature contribution + data.M.diagonal() += model.armature; // JCoM data.Jcom = data.Ag.template middleRows<3>(Force::LINEAR)/data.mass[0]; @@ -199,8 +202,7 @@ namespace pinocchio data.g.noalias() = -data.Ag.template middleRows<3>(Force::LINEAR).transpose() * model.gravity.linear(); // Energy - computeKineticEnergy(model, data); - computePotentialEnergy(model, data); + computeMechanicalEnergy(model, data); } } // namespace pinocchio diff --git a/src/algorithm/constrained-dynamics-derivatives.hpp b/src/algorithm/constrained-dynamics-derivatives.hpp new file mode 100644 index 0000000000..5c02f9143a --- /dev/null +++ b/src/algorithm/constrained-dynamics-derivatives.hpp @@ -0,0 +1,40 @@ +// +// Copyright (c) 2020 CNRS INRIA +// + +#ifndef __pinocchio_algorithm_constrained_dynamics_derivatives_hpp__ +#define __pinocchio_algorithm_constrained_dynamics_derivatives_hpp__ + +#include "pinocchio/algorithm/contact-info.hpp" + +namespace pinocchio +{ + + template class JointCollectionTpl, class ContactModelAllocator, class ContactDataAllocator, typename MatrixType1, typename MatrixType2, typename MatrixType3, typename MatrixType4, typename MatrixType5, typename MatrixType6> + inline void computeConstraintDynamicsDerivatives(const ModelTpl & model, + DataTpl & data, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_data, + const Eigen::MatrixBase & ddq_partial_dq, + const Eigen::MatrixBase & ddq_partial_dv, + const Eigen::MatrixBase & ddq_partial_dtau, + const Eigen::MatrixBase & lambda_partial_dq, + const Eigen::MatrixBase & lambda_partial_dv, + const Eigen::MatrixBase & lambda_partial_dtau); + + template class JointCollectionTpl, class ContactModelAllocator, class ContactDataAllocator> + inline void computeConstraintDynamicsDerivatives(const ModelTpl & model, + DataTpl & data, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_data) + { + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data, + data.ddq_dq, data.ddq_dv, data.ddq_dtau, + data.dlambda_dq, data.dlambda_dv, data.dlambda_dtau); + }; + +} // namespace pinocchio + +#include "pinocchio/algorithm/constrained-dynamics-derivatives.hxx" + +#endif // ifndef __pinocchio_algorithm_constrained_dynamics_derivatives_hpp__ diff --git a/src/algorithm/constrained-dynamics-derivatives.hxx b/src/algorithm/constrained-dynamics-derivatives.hxx new file mode 100644 index 0000000000..e307ea1508 --- /dev/null +++ b/src/algorithm/constrained-dynamics-derivatives.hxx @@ -0,0 +1,873 @@ +// +// Copyright (c) 2020 CNRS INRIA +// + +#ifndef __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__ +#define __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__ + +#include "pinocchio/algorithm/check.hpp" +#include "pinocchio/algorithm/rnea-derivatives.hpp" +#include "pinocchio/algorithm/kinematics-derivatives.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/frames-derivatives.hpp" +#include "pinocchio/algorithm/contact-cholesky.hpp" +#include "pinocchio/algorithm/utils/motion.hpp" + +namespace pinocchio +{ + + template class JointCollectionTpl, bool ContactMode> + struct ComputeConstraintDynamicsDerivativesForwardStep + : public fusion::JointUnaryVisitorBase< ComputeConstraintDynamicsDerivativesForwardStep > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data) + { + typedef typename Model::JointIndex JointIndex; + typedef typename Data::Motion Motion; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + + typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + + if(ContactMode) + { + const Motion & ov = data.ov[i]; + ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + + motionSet::motionAction(ov,J_cols,dJ_cols); + // TODO: make more efficient + data.v[i] = data.oMi[i].actInv(data.ov[i]); + + if(parent > 0) + { + motionSet::motionAction(data.ov[parent],J_cols,dVdq_cols); + } + else + dVdq_cols.setZero(); + + // computes variation of inertias + data.doYcrb[i] = data.oinertias[i].variation(ov); + typedef ComputeRNEADerivativesForwardStep RNEAForwardStepType; + RNEAForwardStepType::addForceCrossMatrix(data.oh[i],data.doYcrb[i]); + Motion & oa = data.oa[i]; + Motion & oa_gf = data.oa_gf[i]; + ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + const typename Data::TangentVectorType& a = data.ddq; + data.a[i] = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + if(parent > 0) + data.a[i] += data.liMi[i].actInv(data.a[parent]); + oa = data.oMi[i].act(data.a[i]); + oa_gf = oa - model.gravity; // add gravity contribution + data.of[i] = data.oinertias[i] * oa_gf + ov.cross(data.oh[i]); + motionSet::motionAction(data.oa_gf[parent],J_cols,dAdq_cols); + dAdv_cols = dJ_cols; + if(parent > 0) + { + motionSet::motionAction(data.ov[parent],dVdq_cols,dAdq_cols); + dAdv_cols.noalias() += dVdq_cols; + } + } + else + { + Motion & odv = data.oa[i]; + Motion & odvparent = data.oa[parent]; + const typename Data::TangentVectorType& dimpulse = data.ddq; + //Temporary calculation of J(dq_after) + odv = J_cols * jmodel.jointVelocitySelector(dimpulse); + if(parent > 0) + odv += odvparent; + motionSet::motionAction(odvparent,J_cols,dAdq_cols); + data.of[i] = data.oinertias[i] * odv; + } + } + }; + + template class JointCollectionTpl, bool ContactMode> + struct ComputeContactDynamicDerivativesBackwardStep + : public fusion::JointUnaryVisitorBase > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + const Model & model, + Data & data) + { + typedef typename Model::JointIndex JointIndex; + typedef Eigen::Matrix MatrixNV6; + typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); + ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + + typename Data::RowMatrixXs & dtau_dq = data.dtau_dq; + + // Temporary variables + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) StdY(jmodel.nv(),6); + + motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols); + // dtau/dq + if(parent>0) + { + if(ContactMode) + { + dFdq_cols.noalias() += data.doYcrb[i] * dVdq_cols; + StdY.noalias() = J_cols.transpose() * data.doYcrb[i]; + for(int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(typename Model::Index)j]) + { + dtau_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() + = dFda_cols.transpose() * data.dAdq.col(j) + + StdY * data.dVdq.col(j); + } + } + else + { + for(int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(typename Model::Index)j]) + { + dtau_dq.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() + = dFda_cols.transpose() * data.dAdq.col(j); + } + } + } + + dtau_dq.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() + = J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]); + motionSet::act(J_cols,data.of[i],dFdq_cols); + + if(ContactMode) + { + ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); + + typename Data::RowMatrixXs & dtau_dv = data.dtau_dv; + dFdv_cols.noalias() = data.doYcrb[i] * J_cols; + motionSet::inertiaAction(data.oYcrb[i],dAdv_cols,dFdv_cols); + + dtau_dv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() + = J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); + if(parent > 0) + { + for(int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(typename Model::Index)j]) + { + dtau_dv.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() + = dFda_cols.transpose() * data.dAdv.col(j) + + StdY * data.J.col(j); + } + data.doYcrb[parent] += data.doYcrb[i]; + } + // Restore the status of dAdq_cols (remove gravity) + for(Eigen::DenseIndex k =0; k < jmodel.nv(); ++k) + { + typedef typename ColsBlock::ColXpr ColType; + MotionRef min(J_cols.col(k)); + MotionRef mout(dAdq_cols.col(k)); + mout.linear() += model.gravity.linear().cross(min.angular()); + } + } + + if(parent > 0) + data.of[parent] += data.of[i]; + } + }; + + template class JointCollectionTpl, class ContactModelAllocator, class ContactDataAllocator, typename MatrixType1, typename MatrixType2, typename MatrixType3, typename MatrixType4, + typename MatrixType5, typename MatrixType6> + inline void computeConstraintDynamicsDerivatives(const ModelTpl & model, + DataTpl & data, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_data, + const Eigen::MatrixBase & ddq_partial_dq, + const Eigen::MatrixBase & ddq_partial_dv, + const Eigen::MatrixBase & ddq_partial_dtau, + const Eigen::MatrixBase & lambda_partial_dq, + const Eigen::MatrixBase & lambda_partial_dv, + const Eigen::MatrixBase & lambda_partial_dtau) + { + const Eigen::DenseIndex & nc = data.contact_chol.constraintDim(); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(contact_data.size() == contact_models.size(), + "contact_data and contact_models do not have the same size"); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dq.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dq.rows() == model.nv); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dv.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dv.rows() == model.nv); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dtau.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dtau.rows() == model.nv); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dq.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dq.rows() == nc); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dv.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dv.rows() == nc); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dtau.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dtau.rows() == nc); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real( + model.gravity.angular()[0] == Scalar(0) + && model.gravity.angular()[1] == Scalar(0) + && model.gravity.angular()[2] == Scalar(0)), + "The gravity must be a pure force vector, no angular part"); + + assert(model.check(data) && "data is not consistent with model."); + data.dtau_dq.setZero(); + data.dtau_dv.setZero(); + data.dac_dq.setZero(); + data.dac_dv.setZero(); + + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef RigidConstraintModelTpl RigidConstraintModel; + typedef RigidConstraintDataTpl RigidConstraintData; + + typedef typename ModelTpl::JointIndex JointIndex; + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; + typedef typename Data::Force Force; + typedef typename Data::Vector3 Vector3; + data.oa_gf[0] = -model.gravity; + + //TODO: Temp variable + Force of_temp, of_temp2; + Motion a_temp; + SE3 se3_temp; + + typedef ComputeConstraintDynamicsDerivativesForwardStep Pass1; + for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) + { + Pass1::run(model.joints[i],data.joints[i], + typename Pass1::ArgsType(model,data)); + } + + // Add the contribution of the external forces. + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_data[k]; + + // TODO: Temporary variable + const SE3 & oMc1 = cdata.oMc1; + Force & of1 = data.of[cmodel.joint1_id]; + const SE3 & oMc2 = cdata.oMc2; + Force & of2 = data.of[cmodel.joint2_id]; + + switch(cmodel.reference_frame) + { + case LOCAL: { + switch(cmodel.type) { + case CONTACT_6D: { + if(cmodel.joint1_id > 0) { + of1 -= oMc1.act(cdata.contact_force); + } + if(cmodel.joint2_id > 0) { + of_temp = oMc1.act(cdata.contact_force); + of2 += of_temp; + } + break; + } + case CONTACT_3D: { + of_temp.linear().noalias() = oMc1.rotation()*cdata.contact_force.linear(); + + if(cmodel.joint1_id > 0) { + of1.linear().noalias() -= of_temp.linear(); + of1.angular().noalias() -= oMc1.translation().cross(of_temp.linear()); + } + if(cmodel.joint2_id > 0) { + of2.linear() += of_temp.linear(); + of2.angular().noalias() += oMc2.translation().cross(of_temp.linear()); + } + break; + } + default: { + assert(false && "must never happen"); + break; + } + } + break; + } + case LOCAL_WORLD_ALIGNED: { + switch(cmodel.type) { + case CONTACT_6D: { + if(cmodel.joint1_id > 0) { + of1 -= cdata.contact_force; + of1.angular().noalias() -= oMc1.translation().cross(cdata.contact_force.linear()); + } + if(cmodel.joint2_id > 0) { + of2 += cdata.contact_force; + of2.angular().noalias() += oMc1.translation().cross(cdata.contact_force.linear()); + } + break; + } + case CONTACT_3D: { + if(cmodel.joint1_id > 0) { + of1.linear() -= cdata.contact_force.linear(); + of1.angular().noalias() -= oMc1.translation().cross(cdata.contact_force.linear()); + } + if(cmodel.joint2_id > 0) { + of2.linear() += cdata.contact_force.linear(); + of2.angular().noalias() += oMc2.translation().cross(cdata.contact_force.linear()); + } + break; + } + default: { + assert(false && "must never happen"); + break; + } + } + break; + } + default: + assert(false && "Should never happen"); + break; + } + } + + //Backward Pass + typedef ComputeContactDynamicDerivativesBackwardStep Pass2; + for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) + { + Pass2::run(model.joints[i], + typename Pass2::ArgsType(model,data)); + } + + // Compute the contact frame partial derivatives + typename Data::SE3::Matrix6 Jlog; + Eigen::DenseIndex current_row_sol_id = 0; + const Eigen::DenseIndex constraint_dim = data.contact_chol.constraintDim(); + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_data[k]; + typedef typename Data::ContactCholeskyDecomposition ContactCholeskyDecomposition; + typedef typename ContactCholeskyDecomposition::IndexVector IndexVector; + typedef typename ContactCholeskyDecomposition::BooleanVector BooleanVector; + + switch(cmodel.type) + { + case CONTACT_6D: + { + typedef typename SizeDepType<6>::template RowsReturn::Type RowsBlock; + RowsBlock contact_dvc_dq = SizeDepType<6>::middleRows(data.dvc_dq, + current_row_sol_id); + RowsBlock contact_dac_dq = SizeDepType<6>::middleRows(data.dac_dq, + current_row_sol_id); + RowsBlock contact_dac_dv = SizeDepType<6>::middleRows(data.dac_dv, + current_row_sol_id); + RowsBlock contact_dac_da = SizeDepType<6>::middleRows(data.dac_da, + current_row_sol_id); + + + if(cmodel.joint1_id >0) { + getFrameAccelerationDerivatives(model, data, + cmodel.joint1_id, + cmodel.joint1_placement, + cmodel.reference_frame, + contact_dvc_dq, + contact_dac_dq, + contact_dac_dv, + contact_dac_da); + } + + if(cmodel.joint2_id >0) { + //TODO: Memory assignment here + typename Data::Matrix6x j2_dvc_dq(6,model.nv), j2_dac_dq(6,model.nv), j2_dac_dv(6,model.nv), j2_dac_da(6,model.nv); + j2_dvc_dq.setZero(); j2_dac_dq.setZero(); j2_dac_dv.setZero(); j2_dac_da.setZero(); + se3_temp = cmodel.joint2_placement * cdata.c1Mc2.inverse(); + getFrameAccelerationDerivatives(model, data, + cmodel.joint2_id, + se3_temp, + cmodel.reference_frame, + j2_dvc_dq, + j2_dac_dq, + j2_dac_dv, + j2_dac_da); + + //TODO: This is only in case reference_frame is LOCAL + contact_dvc_dq -= j2_dvc_dq; + contact_dac_dq -= j2_dac_dq; + contact_dac_dv -= j2_dac_dv; + contact_dac_da -= j2_dac_da; + } + //END TODO + + break; + } + case CONTACT_3D: + { + typedef typename SizeDepType<3>::template RowsReturn::Type RowsBlock; + + RowsBlock contact_dvc_dq = SizeDepType<3>::middleRows(data.dvc_dq, + current_row_sol_id); + RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq, + current_row_sol_id); + RowsBlock contact_dac_dv = SizeDepType<3>::middleRows(data.dac_dv, + current_row_sol_id); + RowsBlock contact_dac_da = SizeDepType<3>::middleRows(data.dac_da, + current_row_sol_id); + + if(cmodel.joint1_id >0) { + getPointClassicAccelerationDerivatives(model, data, + cmodel.joint1_id, + cmodel.joint1_placement, + cmodel.reference_frame, + contact_dvc_dq, + contact_dac_dq, + contact_dac_dv, + contact_dac_da); + } + if(cmodel.joint2_id >0) { + + //TODO: Memory assignment here + typename Data::Matrix3x j2_dvc_dq(3,model.nv), j2_dac_dq(3,model.nv), j2_dac_dv(3,model.nv), j2_dac_da(3,model.nv); + + j2_dvc_dq.setZero(); j2_dac_dq.setZero(); j2_dac_dv.setZero(); j2_dac_da.setZero(); + se3_temp.translation() = cmodel.joint2_placement.translation(); + se3_temp.rotation() = cmodel.joint2_placement.rotation() * cdata.c1Mc2.inverse().rotation(); + getPointClassicAccelerationDerivatives(model, data, + cmodel.joint2_id, + se3_temp, + cmodel.reference_frame, + j2_dvc_dq, + j2_dac_dq, + j2_dac_dv, + j2_dac_da); + + //TODO: This is only in case reference_frame is LOCAL + contact_dvc_dq -= j2_dvc_dq; + contact_dac_dq -= j2_dac_dq; + contact_dac_dv -= j2_dac_dv; + contact_dac_da -= j2_dac_da; + } + //END TODO: Memory assignment here + break; + } + default: + assert(false && "must never happen"); + break; + } + + const IndexVector & colwise_sparsity = data.contact_chol.getLoopSparsityPattern(k); + const BooleanVector& joint2_indexes = data.contact_chol.getJoint2SparsityPattern(k).tail(model.nv); + assert(colwise_sparsity.size() > 0 && "Must never happened, the sparsity pattern is empty"); + + //Derivative of closed loop kinematic tree + if(cmodel.joint2_id >0 ) + { + switch(cmodel.type) + { + case CONTACT_6D: + { + //TODO: THIS IS FOR THE LOCAL FRAME ONLY + const Motion& o_acc_c2 = data.oa[cmodel.joint2_id]; + typedef typename SizeDepType<6>::template RowsReturn::Type RowsBlock; + const RowsBlock contact_dvc_dq = SizeDepType<6>::middleRows(data.dvc_dq,current_row_sol_id); + RowsBlock contact_dac_dq = SizeDepType<6>::middleRows(data.dac_dq,current_row_sol_id); + RowsBlock contact_dac_dv = SizeDepType<6>::middleRows(data.dac_dv,current_row_sol_id); + const RowsBlock contact_dac_da = SizeDepType<6>::middleRows(data.dac_da,current_row_sol_id); + const typename Model::JointIndex joint2_id = cmodel.joint2_id; + const Eigen::DenseIndex colRef2 = + nv(model.joints[joint2_id])+idx_v(model.joints[joint2_id])-1; + + switch(cmodel.reference_frame) { + case LOCAL: { + of_temp = cdata.oMc1.act(cdata.contact_force); + break; + } + case LOCAL_WORLD_ALIGNED: { + of_temp = cdata.contact_force; + of_temp.angular().noalias() += cdata.oMc1.translation().cross(cdata.contact_force.linear()); + break; + } + default: { + assert(false && "must never happen"); + break; + } + } + + // d./dq + for(Eigen::DenseIndex k = 0; k < colwise_sparsity.size(); ++k) + { + const Eigen::DenseIndex col_id = colwise_sparsity[k] - constraint_dim; + const MotionRef J_col(data.J.col(col_id)); + motionSet::motionAction(o_acc_c2, data.J.col(col_id), a_temp.toVector()); + + switch(cmodel.reference_frame) { + case LOCAL: { + if(joint2_indexes[col_id]) { + contact_dac_dq.col(col_id).noalias() += cdata.oMc1.actInv(a_temp).toVector(); + } + else { + contact_dac_dq.col(col_id).noalias() -= cdata.oMc1.actInv(a_temp).toVector(); + } + break; + } + case LOCAL_WORLD_ALIGNED: { + // Do nothing + break; + } + default: { + assert(false && "must never happen"); + break; + } + } + + motionSet::act(data.J.col(col_id), of_temp, of_temp2.toVector()); + for(Eigen::DenseIndex j=colRef2;j>=0;j=data.parents_fromRow[(size_t)j]) + { + if(joint2_indexes[col_id]) { + data.dtau_dq(j,col_id) -= data.J.col(j).transpose() * of_temp2.toVector(); + } + else { + data.dtau_dq(j,col_id) += data.J.col(j).transpose() * of_temp2.toVector(); + } + } + } + break; + } + case CONTACT_3D: + { + + typedef typename SizeDepType<3>::template RowsReturn::Type RowsBlock; + RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq,current_row_sol_id); + const typename Model::JointIndex joint2_id = cmodel.joint2_id; + const Eigen::DenseIndex colRef2 = + nv(model.joints[joint2_id])+idx_v(model.joints[joint2_id])-1; + + switch(cmodel.reference_frame) { + case LOCAL: { + of_temp.linear().noalias() = cdata.oMc1.rotation()*cdata.contact_force.linear(); + const Motion& c2_acc_c2 = getFrameClassicalAcceleration(model, data, + cmodel.joint2_id, + cmodel.joint2_placement, + cmodel.reference_frame); + a_temp.angular().noalias() = cdata.oMc2.rotation() * c2_acc_c2.linear(); + break; + } + case LOCAL_WORLD_ALIGNED: { + of_temp.linear() = cdata.contact_force.linear(); + break; + } + default: { + assert(false && "must never happen"); + break; + } + } + + // d./dq + for(Eigen::DenseIndex k = 0; k < colwise_sparsity.size(); ++k) + { + const Eigen::DenseIndex col_id = colwise_sparsity[k] - constraint_dim; + const MotionRef J_col(data.J.col(col_id)); + + switch(cmodel.reference_frame) { + case LOCAL: { + a_temp.linear().noalias() = a_temp.angular().cross(J_col.angular()); + if(joint2_indexes[col_id]) { + contact_dac_dq.col(col_id).noalias() += cdata.oMc1.rotation().transpose() * a_temp.linear(); + } + else { + contact_dac_dq.col(col_id).noalias() -= cdata.oMc1.rotation().transpose() * a_temp.linear(); + } + break; + } + case LOCAL_WORLD_ALIGNED: { + // Do nothing + break; + } + default: { + assert(false && "must never happen"); + break; + } + } + + of_temp2.linear().noalias() = of_temp.linear().cross(J_col.angular()); + for(Eigen::DenseIndex j=colRef2;j>=0;j=data.parents_fromRow[(size_t)j]) + { + const MotionRef J2_col(data.J.col(j)); + //Temporary assignment + of_temp2.angular().noalias() = J2_col.linear() - cdata.oMc2.translation().cross(J2_col.angular()); + if(joint2_indexes[col_id]) { + data.dtau_dq(j,col_id) += of_temp2.angular().transpose() * of_temp2.linear(); + } + else { + data.dtau_dq(j,col_id) -= of_temp2.angular().transpose() * of_temp2.linear(); + } + } + } + break; + } + default: + { + assert(false && "must never happen"); + break; + } + } + } + + // Add the contribution of the corrector + if(check_expression_if_real(cmodel.corrector.Kp != Scalar(0))) + { + Jlog6(cdata.c1Mc2.inverse(),Jlog); + + switch(cmodel.type) + { + case CONTACT_6D: + { + typedef typename SizeDepType<6>::template RowsReturn::Type RowsBlock; + const RowsBlock contact_dvc_dq = SizeDepType<6>::middleRows(data.dvc_dq,current_row_sol_id); + RowsBlock contact_dac_dq = SizeDepType<6>::middleRows(data.dac_dq,current_row_sol_id); + RowsBlock contact_dac_dv = SizeDepType<6>::middleRows(data.dac_dv,current_row_sol_id); + const RowsBlock contact_dac_da = SizeDepType<6>::middleRows(data.dac_da,current_row_sol_id); + + // d./dq + for(Eigen::DenseIndex k = 0; k < colwise_sparsity.size(); ++k) + { + const Eigen::DenseIndex row_id = colwise_sparsity[k] - constraint_dim; + contact_dac_dq.col(row_id).noalias() += cmodel.corrector.Kd * contact_dvc_dq.col(row_id); + contact_dac_dq.col(row_id).noalias() += cmodel.corrector.Kp * Jlog * contact_dac_da.col(row_id); + } + + // d./dv + for(Eigen::DenseIndex k = 0; k < colwise_sparsity.size(); ++k) + { + const Eigen::DenseIndex row_id = colwise_sparsity[k] - constraint_dim; + contact_dac_dv.col(row_id).noalias() += cmodel.corrector.Kd * contact_dac_da.col(row_id); + } + break; + } + case CONTACT_3D: + { + typedef typename SizeDepType<3>::template RowsReturn::Type RowsBlock; + const RowsBlock contact_dvc_dq = SizeDepType<3>::middleRows(data.dvc_dq,current_row_sol_id); + RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq,current_row_sol_id); + RowsBlock contact_dac_dv = SizeDepType<3>::middleRows(data.dac_dv,current_row_sol_id); + const RowsBlock contact_dac_da = SizeDepType<3>::middleRows(data.dac_da,current_row_sol_id); + + // d./dq + for(Eigen::DenseIndex k = 0; k < colwise_sparsity.size(); ++k) + { + const Eigen::DenseIndex row_id = colwise_sparsity[k] - constraint_dim; + + const MotionRef J_col(data.J.col(row_id)); + contact_dac_dq.col(row_id).noalias() += cmodel.corrector.Kd * contact_dvc_dq.col(row_id); + contact_dac_dq.col(row_id).noalias() -= cmodel.corrector.Kp * (cdata.oMc1.rotation().transpose()*J_col.angular()).cross(cdata.contact_placement_error.linear()); + contact_dac_dq.col(row_id).noalias() += cmodel.corrector.Kp * contact_dac_da.col(row_id); + } + // d./dv + for(Eigen::DenseIndex k = 0; k < colwise_sparsity.size(); ++k) + { + const Eigen::DenseIndex row_id = colwise_sparsity[k] - constraint_dim; + contact_dac_dv.col(row_id).noalias() += cmodel.corrector.Kd * contact_dac_da.col(row_id); + } + break; + } + default: + assert(false && "must never happen"); + break; + } + } + + current_row_sol_id += cmodel.size(); + } + + data.contact_chol.getOperationalSpaceInertiaMatrix(data.osim); + data.contact_chol.getInverseMassMatrix(data.Minv); + + //Temporary: dlambda_dv stores J*Minv + typename Data::MatrixXs & JMinv = data.dlambda_dv; + + JMinv.noalias() = data.dac_da * data.Minv; + PINOCCHIO_EIGEN_CONST_CAST(MatrixType6,lambda_partial_dtau).noalias() = -data.osim * JMinv; //OUTPUT + + MatrixType3 & ddq_partial_dtau_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,ddq_partial_dtau); + ddq_partial_dtau_.noalias() = JMinv.transpose() * lambda_partial_dtau; + ddq_partial_dtau_ += data.Minv; //OUTPUT + + data.dac_dq.noalias() -= JMinv * data.dtau_dq; + data.dac_dv.noalias() -= JMinv * data.dtau_dv; + + PINOCCHIO_EIGEN_CONST_CAST(MatrixType4,lambda_partial_dq).noalias() = -data.osim * data.dac_dq; //OUTPUT + PINOCCHIO_EIGEN_CONST_CAST(MatrixType5,lambda_partial_dv).noalias() = -data.osim * data.dac_dv; //OUTPUT + + current_row_sol_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + + const typename Model::JointIndex joint1_id = cmodel.joint1_id; + const typename Model::JointIndex joint2_id = cmodel.joint2_id; + const Eigen::DenseIndex colRef = nv(model.joints[joint1_id]) + +idx_v(model.joints[joint1_id])-1; + + switch(cmodel.type) + { + case CONTACT_6D: + { + + typedef typename SizeDepType<6>::template RowsReturn::Type RowsBlock; + typedef typename SizeDepType<6>::template RowsReturn::ConstType ConstRowsBlock; + + //TODO: replace with contact_model::nc + RowsBlock contact_dac_da = SizeDepType<6>::middleRows(data.dac_da, + current_row_sol_id); + + ConstRowsBlock contact_dlambda_dq = SizeDepType<6>::middleRows(lambda_partial_dq, + current_row_sol_id); + ConstRowsBlock contact_dlambda_dv = SizeDepType<6>::middleRows(lambda_partial_dv, + current_row_sol_id); + + //TODO: Sparsity in dac_da with loop joints? + + data.dtau_dq.noalias() -= contact_dac_da.transpose() * contact_dlambda_dq; + data.dtau_dv.noalias() -= contact_dac_da.transpose() * contact_dlambda_dv; + + //END TODO + + /* + + for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j]) + { + data.dtau_dq.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dq; + data.dtau_dv.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dv; + } + */ + break; + } + case CONTACT_3D: + { + + typedef typename SizeDepType<3>::template RowsReturn::Type RowsBlock; + typedef typename SizeDepType<3>::template RowsReturn::ConstType ConstRowsBlock; + + RowsBlock contact_dac_da = SizeDepType<3>::middleRows(data.dac_da, + current_row_sol_id); + + ConstRowsBlock contact_dlambda_dq = SizeDepType<3>::middleRows(lambda_partial_dq, + current_row_sol_id); + ConstRowsBlock contact_dlambda_dv = SizeDepType<3>::middleRows(lambda_partial_dv, + current_row_sol_id); + + + //TODO: Sparsity in dac_da with loop joints? + + data.dtau_dq.noalias() -= contact_dac_da.transpose() * contact_dlambda_dq; + data.dtau_dv.noalias() -= contact_dac_da.transpose() * contact_dlambda_dv; + + //END TODO + /* + + + + for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j]) + { + data.dtau_dq.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dq; + data.dtau_dv.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dv; + } + */ + break; + } + + default: + assert(false && "must never happen"); + break; + } + current_row_sol_id += cmodel.size(); + } + + PINOCCHIO_EIGEN_CONST_CAST(MatrixType1,ddq_partial_dq).noalias() = -data.Minv*data.dtau_dq; //OUTPUT + PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,ddq_partial_dv).noalias() = -data.Minv*data.dtau_dv; //OUTPUT + + MatrixType4& dfc_dq = PINOCCHIO_EIGEN_CONST_CAST(MatrixType4,lambda_partial_dq); + typedef typename SizeDepType<6>::template RowsReturn::Type Rows6Block; + typedef typename SizeDepType<3>::template RowsReturn::Type Rows3Block; + + current_row_sol_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_data[k]; + const typename Model::JointIndex joint1_id = cmodel.joint1_id; + const int colRef = nv(model.joints[joint1_id])+idx_v(model.joints[joint1_id])-1; + + switch(cmodel.reference_frame) + { + case LOCAL: + break; + case LOCAL_WORLD_ALIGNED: + { + const Force & of = cdata.contact_force; + switch(cmodel.type) + { + case CONTACT_6D: + { + Rows6Block contact_dfc_dq = SizeDepType<6>::middleRows(dfc_dq, current_row_sol_id); + for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j]) + { + typedef typename Rows6Block::ColXpr ColType; + typedef typename Rows6Block::ColXpr ColTypeOut; + const MotionRef J_col(data.J.col(j)); + ForceRef fout(contact_dfc_dq.col(j)); + fout.linear().noalias() += J_col.angular().cross(of.linear()); + fout.angular().noalias() += J_col.angular().cross(of.angular()); + } + break; + } + case CONTACT_3D: + { + Rows3Block contact_dfc_dq = SizeDepType<3>::middleRows(dfc_dq, current_row_sol_id); + for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j]) + { + typedef typename Data::Matrix6x::ColXpr ColType; + const MotionRef J_col(data.J.col(j)); + contact_dfc_dq.col(j).noalias() += J_col.angular().cross(of.linear()); + } + break; + } + default: + assert(false && "must never happen"); + break; + } + break; + } + default: + assert(false && "must never happen"); + break; + } + current_row_sol_id += cmodel.size(); + } + + } + +} // namespace pinocchio + +#endif // ifndef __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__ diff --git a/src/algorithm/constrained-dynamics.hpp b/src/algorithm/constrained-dynamics.hpp new file mode 100644 index 0000000000..550e1d5026 --- /dev/null +++ b/src/algorithm/constrained-dynamics.hpp @@ -0,0 +1,122 @@ +// +// Copyright (c) 2019-2021 INRIA +// + +#ifndef __pinocchio_algorithm_constrained_dynamics_hpp__ +#define __pinocchio_algorithm_constrained_dynamics_hpp__ + +#include "pinocchio/algorithm/constrained-dynamics.hpp" +#include "pinocchio/algorithm/contact-info.hpp" + +#include "pinocchio/algorithm/proximal.hpp" + +namespace pinocchio +{ + + /// + /// \brief Init the forward dynamics data according to the contact information contained in contact_models. + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType1 Type of the joint velocity vector. + /// \tparam TangentVectorType2 Type of the joint torque vector. + /// \tparam Allocator Allocator class for the std::vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] contact_models Vector of contact information related to the problem. + /// + template class JointCollectionTpl, class Allocator> + inline void + initConstraintDynamics(const ModelTpl & model, + DataTpl & data, + const std::vector,Allocator> & contact_models); + + /// + /// \brief Computes the forward dynamics with contact constraints according to a given list of Contact information. + /// When using forwardDynamics for the first time, you should call first initConstraintDynamics to initialize the internal memory used in the algorithm. + /// + /// \note It computes the following problem:
+ ///
\f$ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ % + /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$

+ /// where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints), + /// \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \gamma \f$ is the constraint drift. + /// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed. + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType1 Type of the joint velocity vector. + /// \tparam TangentVectorType2 Type of the joint torque vector. + /// \tparam Allocator Allocator class for the std::vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] q The joint configuration (size model.nq). + /// \param[in] v The joint velocity (size model.nv). + /// \param[in] tau The joint torque vector (size model.nv). + /// \param[in] contact_models Vector of contact information related to the problem. + /// \param[in] settings Proximal settings (mu, accuracy and maximal number of iterations). + /// + /// \note A hint: a typical value for mu is 1e-12 when two contact constraints are redundant. + /// + /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector. + /// + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, class ContactModelAllocator, class ContactDataAllocator> + inline const typename DataTpl::TangentVectorType & + constraintDynamics(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_datas, + ProximalSettingsTpl & settings); + + /// + /// \brief Computes the forward dynamics with contact constraints according to a given list of Contact information. + /// When using forwardDynamics for the first time, you should call first initConstraintDynamics to initialize the internal memory used in the algorithm. + /// + /// \note It computes the following problem:
+ ///
\f$ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ % + /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$

+ /// where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without constraints), + /// \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \gamma \f$ is the constraint drift. + /// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed. + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType1 Type of the joint velocity vector. + /// \tparam TangentVectorType2 Type of the joint torque vector. + /// \tparam Allocator Allocator class for the std::vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] q The joint configuration (size model.nq). + /// \param[in] v The joint velocity (size model.nv). + /// \param[in] tau The joint torque vector (size model.nv). + /// \param[in] contact_models Vector of contact information related to the problem. + /// \param[in] mu Damping factor for cholesky decomposition. Set to zero if constraints are full rank. + /// + /// \note A hint: a typical value for mu is 1e-12 when two contact constraints are redundant. + /// + /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector. + /// + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, class ContactModelAllocator, class ContactDataAllocator> + inline const typename DataTpl::TangentVectorType & + constraintDynamics(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_datas) + { + ProximalSettingsTpl settings; + return constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,settings); + } + +} // namespace pinocchio + +#include "pinocchio/algorithm/constrained-dynamics.hxx" + +#endif // ifndef __pinocchio_algorithm_constrained_dynamics_hpp__ diff --git a/src/algorithm/constrained-dynamics.hxx b/src/algorithm/constrained-dynamics.hxx new file mode 100644 index 0000000000..2faf74d403 --- /dev/null +++ b/src/algorithm/constrained-dynamics.hxx @@ -0,0 +1,446 @@ +// +// Copyright (c) 2019-2020 INRIA CNRS +// + +#ifndef __pinocchio_algorithm_constraint_dynamics_hxx__ +#define __pinocchio_algorithm_constraint_dynamics_hxx__ + +#include "pinocchio/spatial/classic-acceleration.hpp" +#include "pinocchio/spatial/explog.hpp" + +#include "pinocchio/algorithm/check.hpp" +#include "pinocchio/algorithm/aba.hpp" +#include "pinocchio/algorithm/contact-cholesky.hxx" + +#include + +namespace pinocchio +{ + + template class JointCollectionTpl, class Allocator> + inline void initConstraintDynamics(const ModelTpl & model, + DataTpl & data, + const std::vector,Allocator> & contact_models) + { + data.contact_chol.allocate(model,contact_models); + data.primal_dual_contact_solution.resize(data.contact_chol.size()); + data.primal_rhs_contact.resize(data.contact_chol.constraintDim()); + + data.lambda_c.resize(data.contact_chol.constraintDim()); + data.lambda_c_prox.resize(data.contact_chol.constraintDim()); + data.impulse_c.resize(data.contact_chol.constraintDim()); + + // TODO: should be moved elsewhere + data.dlambda_dq.resize(data.contact_chol.constraintDim(), model.nv); + data.dlambda_dv.resize(data.contact_chol.constraintDim(), model.nv); + data.dlambda_dtau.resize(data.contact_chol.constraintDim(), model.nv); + data.dvc_dq.resize(data.contact_chol.constraintDim(), model.nv); + data.dac_dq.resize(data.contact_chol.constraintDim(), model.nv); + data.dac_dv.resize(data.contact_chol.constraintDim(), model.nv); + data.dac_da.resize(data.contact_chol.constraintDim(), model.nv); + data.osim.resize(data.contact_chol.constraintDim(), data.contact_chol.constraintDim()); + + data.lambda_c.setZero(); + data.lambda_c_prox.setZero(); + data.dlambda_dq.setZero(); + data.dlambda_dv.setZero(); + data.dlambda_dtau.setZero(); + data.dvc_dq.setZero(); + data.dac_dq.setZero(); + data.dac_dv.setZero(); + data.dac_da.setZero(); + data.osim.setZero(); + } + + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, bool ContactMode> + struct ContactAndImpulseDynamicsForwardStep + : public fusion::JointUnaryVisitorBase< ContactAndImpulseDynamicsForwardStep > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v) + { + typedef typename Model::JointIndex JointIndex; + typedef typename Data::Motion Motion; + typedef typename Data::Force Force; + typedef typename Data::Inertia Inertia; + + const JointIndex & i = jmodel.id(); + const JointIndex & parent = model.parents[i]; + + Motion & ov = data.ov[i]; + Inertia & oinertias = data.oinertias[i]; + + jmodel.calc(jdata.derived(),q.derived(),v.derived()); + + data.liMi[i] = model.jointPlacements[i]*jdata.M(); + if(parent > 0) + data.oMi[i] = data.oMi[parent] * data.liMi[i]; + else + data.oMi[i] = data.liMi[i]; + + ov = data.oMi[i].act(jdata.v()); + if(parent > 0) + ov += data.ov[parent]; + + jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + oinertias = data.oMi[i].act(model.inertias[i]); + data.oYcrb[i] = data.oinertias[i]; + if(ContactMode) + { + Force & oh = data.oh[i]; + Force & of = data.of[i]; + Motion & oa = data.oa[i]; + Motion & oa_gf = data.oa_gf[i]; + oh = oinertias * ov; + oa = data.oMi[i].act(jdata.c()); + if(parent > 0) + { + oa += (data.ov[parent] ^ ov); + oa += data.oa[parent]; + } + oa_gf = oa - model.gravity; // add gravity contribution + of = oinertias * oa_gf + ov.cross(oh); + } + } + }; + + template class JointCollectionTpl, bool ContactMode> + struct ContactAndImpulseDynamicsBackwardStep + : public fusion::JointUnaryVisitorBase< ContactAndImpulseDynamicsBackwardStep > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + const Model & model, + Data & data) + { + typedef typename Model::JointIndex JointIndex; + typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + + ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + const ColsBlock J_cols = jmodel.jointCols(data.J); + motionSet::inertiaAction(data.oYcrb[i],J_cols,dFda_cols); + + data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() + = J_cols.transpose()*data.dFda.middleCols(jmodel.idx_v(),data.nvSubtree[i]); + data.oYcrb[parent] += data.oYcrb[i]; + + if(ContactMode) + { + jmodel.jointVelocitySelector(data.nle).noalias() + = J_cols.transpose()*data.of[i].toVector(); + data.of[parent] += data.of[i]; + } + } + }; + + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, class ContactModelAllocator, class ContactDataAllocator> + inline const typename DataTpl::TangentVectorType & + constraintDynamics(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_datas, + ProximalSettingsTpl & settings) + { + typedef ModelTpl Model; + typedef DataTpl Data; + typedef typename Data::Motion Motion; + + typedef RigidConstraintModelTpl RigidConstraintModel; + typedef std::vector VectorRigidConstraintModel; + typedef RigidConstraintDataTpl RigidConstraintData; + + assert(model.check(data) && "data is not consistent with model."); + PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, + "The joint configuration vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, + "The joint velocity vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, + "The joint torque vector is not of right size"); + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real(settings.mu >= Scalar(0)), + "mu has to be positive"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(contact_models.size(),contact_datas.size(), + "The contact models and data do not have the same vector size."); + + // Check that all the frames are related to LOCAL or LOCAL_WORLD_ALIGNED reference frames + for(typename VectorRigidConstraintModel::const_iterator cm_it = contact_models.begin(); + cm_it != contact_models.end(); ++cm_it) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(cm_it->reference_frame != WORLD, + "Contact model with name " + cm_it->name + " has reference_frame equals to WORLD. " + "constraintDynamics is only operating from LOCAL or LOCAL_WORLD_ALIGNED reference frames.") + } + + typename Data::TangentVectorType & a = data.ddq; + typename Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + typename Data::VectorXs & primal_dual_contact_solution = data.primal_dual_contact_solution; + typename Data::VectorXs & primal_rhs_contact = data.primal_rhs_contact; + + data.oYcrb[0].setZero(); + data.of[0].setZero(); + data.oa[0].setZero(); + data.ov[0].setZero(); + typedef ContactAndImpulseDynamicsForwardStep Pass1; + for(JointIndex i=1;i<(JointIndex) model.njoints;++i) + { + Pass1::run(model.joints[i],data.joints[i], + typename Pass1::ArgsType(model,data,q.derived(),v.derived())); + } + + typedef ContactAndImpulseDynamicsBackwardStep Pass2; + for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i) + { + Pass2::run(model.joints[i], + typename Pass2::ArgsType(model,data)); + } + + // Add the armature contribution + data.M.diagonal() += model.armature; + + // Retrieve the Centroidal Momemtum map + typedef typename Data::Force Force; + typedef Eigen::Block Block3x; + + data.com[0] = data.oYcrb[0].lever(); + + data.Ag = data.dFda; + const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); + Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); + for(long i = 0; i 0) + vc1 = oMc1.actInv(data.ov[joint1_id]); + else + vc1.setZero(); + if(joint2_id > 0) + vc2 = oMc2.actInv(data.ov[joint2_id]); + else + vc2.setZero(); + + // Compute placement and velocity errors + if(contact_model.type == CONTACT_6D) + { + contact_data.contact_placement_error = -log6(c1Mc2); + contact_data.contact_velocity_error.toVector() = (vc1 - c1Mc2.act(vc2)).toVector(); + } + else + { + contact_data.contact_placement_error.linear() = -c1Mc2.translation(); + contact_data.contact_placement_error.angular().setZero(); + + contact_data.contact_velocity_error.linear() = vc1.linear() - c1Mc2.rotation()*vc2.linear(); + contact_data.contact_velocity_error.angular().setZero(); + } + + if(check_expression_if_real(corrector.Kp == Scalar(0) && corrector.Kd == Scalar(0))) + { + contact_acceleration_error.setZero(); + } + else + { + if(contact_model.type == CONTACT_6D) + contact_acceleration_error.toVector().noalias() = + - corrector.Kp /* * Jexp6(contact_data.contact_placement_error) */ * contact_data.contact_placement_error.toVector() + - corrector.Kd * contact_data.contact_velocity_error.toVector(); + else + { + contact_acceleration_error.linear().noalias() = + - corrector.Kp * contact_data.contact_placement_error.linear() + - corrector.Kd * contact_data.contact_velocity_error.linear(); + contact_acceleration_error.angular().setZero(); + } + } + + switch(contact_model.reference_frame) + { + case LOCAL_WORLD_ALIGNED: + { + // LINEAR + coriolis_centrifugal_acc1.linear().noalias() + = data.oa[joint1_id].linear() + data.oa[joint1_id].angular().cross(oMc1.translation()); + if(contact_model.type == CONTACT_3D) + coriolis_centrifugal_acc1.linear() += data.ov[joint1_id].angular().cross(data.ov[joint1_id].linear() + data.ov[joint1_id].angular().cross(oMc1.translation())); + // ANGULAR + coriolis_centrifugal_acc1.angular() = data.oa[joint1_id].angular(); + + // LINEAR + if(contact_model.type == CONTACT_3D) + { + coriolis_centrifugal_acc2.linear().noalias() + = data.oa[joint2_id].linear() + data.oa[joint2_id].angular().cross(oMc2.translation()) + + data.ov[joint2_id].angular().cross(data.ov[joint2_id].linear() + data.ov[joint2_id].angular().cross(oMc2.translation())); + coriolis_centrifugal_acc2.angular().setZero(); + } + else + { + coriolis_centrifugal_acc2.linear() = data.oa[joint2_id].linear() + data.oa[joint2_id].angular().cross(oMc1.translation()); + coriolis_centrifugal_acc2.angular() = data.oa[joint2_id].angular(); + } + + contact_acceleration_error.linear() = oMc1.rotation() * contact_acceleration_error.linear(); + contact_acceleration_error.angular() = oMc1.rotation() * contact_acceleration_error.angular(); + break; + } + case LOCAL: + { + coriolis_centrifugal_acc1 = oMc1.actInv(data.oa[joint1_id]); + if(contact_model.type == CONTACT_3D) + { + coriolis_centrifugal_acc1.linear() += vc1.angular().cross(vc1.linear()); + coriolis_centrifugal_acc1.angular().setZero(); + } + + if(contact_model.type == CONTACT_3D) + { + coriolis_centrifugal_acc2.linear().noalias() + = oMc1.rotation().transpose()*(data.oa[joint2_id].linear() + data.oa[joint2_id].angular().cross(oMc2.translation()) + + data.ov[joint2_id].angular().cross(data.ov[joint2_id].linear() + data.ov[joint2_id].angular().cross(oMc2.translation()))); + coriolis_centrifugal_acc2.angular().setZero(); + } + else + coriolis_centrifugal_acc2 = oMc1.actInv(data.oa[joint2_id]); + break; + } + default: + assert(false && "must never happened"); + break; + } + + contact_data.contact_acceleration = coriolis_centrifugal_acc2; + switch(contact_model.type) + { + case CONTACT_3D: + primal_rhs_contact.segment(current_row_id,contact_dim) + = -coriolis_centrifugal_acc1.linear() + coriolis_centrifugal_acc2.linear() + contact_acceleration_error.linear() + contact_acceleration_desired.linear(); + break; + case CONTACT_6D: + primal_rhs_contact.segment(current_row_id,contact_dim) + = -coriolis_centrifugal_acc1.toVector() + coriolis_centrifugal_acc2.toVector() + contact_acceleration_error.toVector() + contact_acceleration_desired.toVector(); + break; + default: + assert(false && "must never happened"); + break; + } + + current_row_id += contact_dim; + } + + // Solve the system +// Scalar primal_infeasibility = Scalar(0); + int it = 1; + data.lambda_c_prox.setZero(); + for(; it <= settings.max_iter; ++it) + { + primal_dual_contact_solution.head(contact_chol.constraintDim()) = primal_rhs_contact + data.lambda_c_prox * settings.mu; + primal_dual_contact_solution.tail(model.nv) = tau - data.nle; + contact_chol.solveInPlace(primal_dual_contact_solution); + settings.residual = (primal_dual_contact_solution.head(contact_chol.constraintDim()) + data.lambda_c_prox).template lpNorm(); + if(check_expression_if_real(settings.residual <= settings.accuracy)) // In the case where Scalar is not double, this will iterate for max_it. + break; + data.lambda_c_prox = -primal_dual_contact_solution.head(contact_chol.constraintDim()); + } + settings.iter = it; + + // Retrieve the joint space acceleration + a = primal_dual_contact_solution.tail(model.nv); + data.lambda_c = -primal_dual_contact_solution.head(contact_chol.constraintDim()); + + // Retrieve the contact forces + Eigen::DenseIndex current_row_sol_id = 0; + for(size_t contact_id = 0; contact_id < contact_models.size(); ++contact_id) + { + const RigidConstraintModel & contact_model = contact_models[contact_id]; + RigidConstraintData & contact_data = contact_datas[contact_id]; + typename RigidConstraintData::Force & fext = contact_data.contact_force; + const int contact_dim = contact_model.size(); + + switch(contact_model.type) + { + case CONTACT_3D: + { + fext.linear() = -primal_dual_contact_solution.template segment<3>(current_row_sol_id); + fext.angular().setZero(); + break; + } + case CONTACT_6D: + { + typedef typename Data::VectorXs::template FixedSegmentReturnType<6>::Type Segment6d; + const ForceRef f_sol(primal_dual_contact_solution.template segment<6>(current_row_sol_id)); + fext = -f_sol; + break; + } + default: + assert(false && "must never happened"); + break; + } + + current_row_sol_id += contact_dim; + } + + return a; + } + +} // namespace pinocchio + +#endif // ifndef __pinocchio_algorithm_constraint_dynamics_hxx__ diff --git a/src/algorithm/contact-cholesky.hpp b/src/algorithm/contact-cholesky.hpp new file mode 100644 index 0000000000..efc0450736 --- /dev/null +++ b/src/algorithm/contact-cholesky.hpp @@ -0,0 +1,412 @@ +// +// Copyright (c) 2019-2020 INRIA +// + +#ifndef __pinocchio_algorithm_contact_cholesky_hpp__ +#define __pinocchio_algorithm_contact_cholesky_hpp__ + +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/math/matrix-block.hpp" +#include "pinocchio/algorithm/contact-info.hpp" + +namespace pinocchio +{ + + namespace cholesky + { + // Forward declaration of algo + namespace details + { + template + struct UvAlgo; + + template + struct UtvAlgo; + + template + struct UivAlgo; + + template + struct UtivAlgo; + + template + VectorLike & inverseAlgo(const ContactCholeskyDecompositionTpl & chol, + const Eigen::DenseIndex col, + const Eigen::MatrixBase & vec); + } + + /// + /// \brief Contact Cholesky decomposition structure. This structure allows + /// to compute in a efficient and parsimonious way the Cholesky decomposition + /// of the KKT matrix related to the contact dynamics. + /// Such a decomposition is usefull when computing both the forward dynamics in contact + /// or the related analytical derivatives. + /// + /// + /// \tparam _Scalar Scalar type. + /// \tparam _Options Alignment Options of the Eigen objects contained in the data structure. + /// + template + struct ContactCholeskyDecompositionTpl + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef pinocchio::Index Index; + typedef _Scalar Scalar; + enum { + LINEAR = 0, + ANGULAR = 3, + Options = _Options + }; + + typedef Eigen::Matrix Vector; + typedef Eigen::Matrix Matrix; + typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix) RowMatrix; + typedef RigidConstraintModelTpl RigidConstraintModel; + typedef RigidConstraintDataTpl RigidConstraintData; + typedef Eigen::Matrix IndexVector; + typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(IndexVector) VectorOfIndexVector; + typedef Eigen::Matrix BooleanVector; + + ///@{ + /// \brief Data information related to the Sparsity structure of the Cholesky decompostion + struct Slice + { + Slice(const Eigen::DenseIndex & first_index, + const Eigen::DenseIndex & size) + : first_index(first_index), size(size) + {} + + Eigen::DenseIndex first_index; + Eigen::DenseIndex size; + }; + + typedef std::vector SliceVector; + typedef std::vector VectorOfSliceVector; + ///@} + + /// + /// \brief Default constructor + /// + ContactCholeskyDecompositionTpl() {} + + /// + /// \brief Constructor from a model. + /// + /// \param[in] model Model of the kinematic tree. + /// + template class JointCollectionTpl> + ContactCholeskyDecompositionTpl(const ModelTpl & model) + { + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models; + allocate(model,empty_contact_models); + } + + /// + /// \brief Constructor from a model and a collection of RigidConstraintModel objects. + /// + /// \param[in] model Model of the kinematic tree + /// \param[in] contact_models Vector of RigidConstraintModel objects containing the contact information + /// + template class JointCollectionTpl, class Allocator> + ContactCholeskyDecompositionTpl(const ModelTpl & model, + const std::vector,Allocator> & contact_models) + { + allocate(model,contact_models); + } + + /// + /// \brief Memory allocation of the vectors D, Dinv, and the upper triangular matrix U. + /// + /// \param[in] model Model of the kinematic tree + /// \param[in] contact_models Vector of RigidConstraintModel objects containing the contact information + /// + template class JointCollectionTpl, class Allocator> + void allocate(const ModelTpl & model, + const std::vector,Allocator> & contact_models); + + /// + /// \brief Returns the Inverse of the Operational Space Inertia Matrix resulting from the decomposition. + /// + Matrix getInverseOperationalSpaceInertiaMatrix() const + { + Matrix res(constraintDim(), constraintDim()); + getInverseOperationalSpaceInertiaMatrix(res); + return res; + } + + template + void getInverseOperationalSpaceInertiaMatrix(const Eigen::MatrixBase & res) const + { + typedef typename SizeDepType::template BlockReturn::ConstType ConstBlockXpr; +// typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr; + const ConstBlockXpr U1 + = U.topLeftCorner(constraintDim(),constraintDim()); + + PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED(); + MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,res); + OSIMinv_tmp.noalias() = D.head(constraintDim()).asDiagonal() * U1.adjoint(); + res_.noalias() = -U1 * OSIMinv_tmp; + PINOCCHIO_EIGEN_MALLOC_ALLOWED(); + } + + /// + /// \brief Returns the Operational Space Inertia Matrix resulting from the decomposition. + /// + Matrix getOperationalSpaceInertiaMatrix() const + { + Matrix res(constraintDim(), constraintDim()); + getOperationalSpaceInertiaMatrix(res); + return res; + } + + template + void getOperationalSpaceInertiaMatrix(const Eigen::MatrixBase & res_) const + { + MatrixType& res = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,res_); + typedef typename SizeDepType::template BlockReturn::ConstType ConstBlockXpr; +// typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr; + const Eigen::TriangularView U1 + = U.topLeftCorner(constraintDim(),constraintDim()).template triangularView(); + + PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED(); + U1inv.setIdentity(); U1.solveInPlace(U1inv); // TODO: implement Sparse Inverse + OSIMinv_tmp.noalias() = -U1inv.adjoint() * Dinv.head(constraintDim()).asDiagonal(); + res.noalias() = OSIMinv_tmp * U1inv; + PINOCCHIO_EIGEN_MALLOC_ALLOWED(); + } + + template + void getInverseMassMatrix(const Eigen::MatrixBase & res_) const + { + MatrixType& res = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,res_); + typedef typename SizeDepType::template BlockReturn::ConstType ConstBlockXpr; +// typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr; + const Eigen::TriangularView U4 + = U.bottomRightCorner(nv,nv).template triangularView(); + + PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED(); + U4inv.setIdentity(); U4.solveInPlace(U4inv); // TODO: implement Sparse Inverse + Minv_tmp.noalias() = U4inv.adjoint() * Dinv.tail(nv).asDiagonal(); + res.noalias() = Minv_tmp * U4inv; + PINOCCHIO_EIGEN_MALLOC_ALLOWED(); + } + + /// + /// \brief Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix + /// related to the system mass matrix and the Jacobians of the contact patches contained in + /// the vector of RigidConstraintModel named contact_models. + /// + /// \param[in] model Model of the dynamical system + /// \param[in] data Data related to model containing the computed mass matrix and the Jacobian of the kinematic tree + /// \param[in] contact_models Vector containing the contact models (which frame is in contact and the type of contact: ponctual, 6D rigid, etc.) + /// \param[out] contact_datas Vector containing the contact data related to the contact_models. + /// \param[in] mu Regularization factor allowing to enforce the definite propertie of the KKT matrix. + /// + /// \remarks The mass matrix and the Jacobians of the dynamical system should have been computed first. This can be achieved by simply calling pinocchio::crba. + /// + template class JointCollectionTpl, class ContactModelAllocator, class ContactDataAllocator> + void compute(const ModelTpl & model, + DataTpl & data, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_datas, + const S1 mu = S1(0.)); + + /// \brief Size of the decomposition + Eigen::DenseIndex size() const { return D.size(); } + + /// \brief Returns the total dimension of the constraints contained in the Cholesky factorization + Eigen::DenseIndex constraintDim() const + { + return size() - nv; + } + + /// \brief Returns the number of contacts associated to this decomposition. + Eigen::DenseIndex numContacts() const { return num_contacts; } + + /// + /// \brief Computes the solution of \f$ A x = b \f$ where *this is the Cholesky decomposition of A. + ///  "in-place" version of ContactCholeskyDecompositionTpl::solve(b) where the result is written in b. + /// This functions takes as input the vector b, and returns the solution \f$ x = A^-1 b \f$. + /// + /// \param[inout] mat The right-and-side term which also contains the solution of the linear system. + /// + /// \sa ContactCholeskyDecompositionTpl::solve + template + void solveInPlace(const Eigen::MatrixBase & mat) const; + + /// + /// \brief Computes the solution of \f$ A x = b \f$ where *this is the Cholesky decomposition of A. + /// This functions takes as input the vector b, and returns the solution \f$ x = A^-1 b \f$. + /// + /// \param[inout] mat The right-and-side term. + /// + /// \sa ContactCholeskyDecompositionTpl::solveInPlace + template + Matrix solve(const Eigen::MatrixBase & mat) const; + + /// + /// \brief Retrieves the Cholesky decomposition of the Mass Matrix contained in *this. + /// + /// \param[in] model Model of the dynamical system. + /// + template class JointCollectionTpl> + ContactCholeskyDecompositionTpl + getMassMatrixChoeslkyDecomposition(const ModelTpl & model) const; + + ///@{ + /// \brief Vectorwize operations + template + void Uv(const Eigen::MatrixBase & mat) const; + + template + void Utv(const Eigen::MatrixBase & mat) const; + + template + void Uiv(const Eigen::MatrixBase & mat) const; + + template + void Utiv(const Eigen::MatrixBase & mat) const; + ///@} + + /// \brief Returns the matrix resulting from the decomposition + Matrix matrix() const; + + /// \brief Fill the input matrix with the matrix resulting from the decomposition + template + void matrix(const Eigen::MatrixBase & res) const; + + /// \brief Returns the inverse matrix resulting from the decomposition + Matrix inverse() const; + + /// \brief Fill the input matrix with the inverse matrix resulting from the decomposition + template + void inverse(const Eigen::MatrixBase & res) const; + + // data + Vector D, Dinv; + RowMatrix U; + + ///@{ + /// \brief Friend algorithms + template + friend struct details::UvAlgo; + + template + friend struct details::UtvAlgo; + + template + friend struct details::UivAlgo; + + template + friend struct details::UtivAlgo; + + template + friend VectorLike & details::inverseAlgo(const ContactCholeskyDecompositionTpl & chol, + const Eigen::DenseIndex col, + const Eigen::MatrixBase & vec); + ///@} + + template + bool operator==(const ContactCholeskyDecompositionTpl & other) const + { + bool is_same = true; + + if(nv != other.nv || num_contacts != other.num_contacts) + return false; + + if( D.size() != other.D.size() + || Dinv.size() != other.Dinv.size() + || U.rows() != other.U.rows() + || U.cols() != other.U.cols()) + return false; + + is_same &= (D == other.D); + is_same &= (Dinv == other.Dinv); + is_same &= (U == other.U); + + is_same &= (parents_fromRow == other.parents_fromRow); + is_same &= (nv_subtree_fromRow == other.nv_subtree_fromRow); + is_same &= (last_child == other.last_child); + is_same &= (joint1_indexes == other.joint1_indexes); + is_same &= (joint2_indexes == other.joint2_indexes); + is_same &= (colwise_sparsity_patterns == other.colwise_sparsity_patterns); + is_same &= (colwise_loop_sparsity_patterns == other.colwise_loop_sparsity_patterns); +// is_same &= (rowise_sparsity_pattern == other.rowise_sparsity_pattern); + + return is_same; + } + + template + bool operator!=(const ContactCholeskyDecompositionTpl & other) const + { + return !(*this == other); + } + + const IndexVector & getConstraintSparsityPattern(const size_t constraint_id) const + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(constraint_id < colwise_sparsity_patterns.size(), + "The index of the constraint is invalid."); + return colwise_sparsity_patterns[constraint_id]; + } + + const IndexVector & getLoopSparsityPattern(const size_t constraint_id) const + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(constraint_id < colwise_loop_sparsity_patterns.size(), + "The index of the constraint is invalid."); + return colwise_loop_sparsity_patterns[constraint_id]; + } + + const BooleanVector & getJoint1SparsityPattern(const size_t constraint_id) const + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(constraint_id < joint1_indexes.size(), + "The index of the constraint is invalid."); + return joint1_indexes[constraint_id]; + } + + const BooleanVector & getJoint2SparsityPattern(const size_t constraint_id) const + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(constraint_id < joint2_indexes.size(), + "The index of the constraint is invalid."); + return joint2_indexes[constraint_id]; + } + + + + protected: + + IndexVector parents_fromRow; + IndexVector nv_subtree_fromRow; + + /// \brief Last child of the given joint index + IndexVector last_child; + + std::vector joint1_indexes, joint2_indexes; + VectorOfIndexVector colwise_sparsity_patterns; + VectorOfIndexVector colwise_loop_sparsity_patterns; + Vector DUt; // temporary containing the results of D * U^t + + /// \brief Dimension of the tangent of the configuration space of the model + Eigen::DenseIndex nv; + + /// \brief Number of contacts. + Eigen::DenseIndex num_contacts; + + VectorOfSliceVector rowise_sparsity_pattern; + + /// \brief Inverse of the top left block of U + mutable Matrix U1inv; + /// \brief Inverse of the bottom right block of U + mutable Matrix U4inv; + + private: + + mutable RowMatrix OSIMinv_tmp, Minv_tmp; + + }; + + } // namespace cholesky + +} + +#endif // ifndef __pinocchio_algorithm_contact_cholesky_hpp__ diff --git a/src/algorithm/contact-cholesky.hxx b/src/algorithm/contact-cholesky.hxx new file mode 100644 index 0000000000..70724e5d38 --- /dev/null +++ b/src/algorithm/contact-cholesky.hxx @@ -0,0 +1,919 @@ +// +// Copyright (c) 2019-2021 INRIA +// + +#ifndef __pinocchio_algorithm_contact_cholesky_hxx__ +#define __pinocchio_algorithm_contact_cholesky_hxx__ + +#include "pinocchio/algorithm/contact-cholesky.hpp" +#include "pinocchio/algorithm/check.hpp" + +#include + +namespace pinocchio +{ + + namespace cholesky + { + + template + template class JointCollectionTpl, class Allocator> + void + ContactCholeskyDecompositionTpl:: + allocate(const ModelTpl & model, + const std::vector,Allocator> & contact_models) + { + typedef ModelTpl Model; + typedef RigidConstraintModelTpl RigidConstraintModel; + typedef std::vector RigidConstraintModelVector; + + nv = model.nv; + num_contacts = (Eigen::DenseIndex)contact_models.size(); + + Eigen::DenseIndex num_total_constraints = 0; + for(typename RigidConstraintModelVector::const_iterator it = contact_models.begin(); + it != contact_models.end(); + ++it) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(it->size() > 0, + "The dimension of the constraint must be positive"); + num_total_constraints += it->size(); + } + + U1inv.resize(num_total_constraints,num_total_constraints); + OSIMinv_tmp.resize(num_total_constraints,num_total_constraints); + U4inv.resize(nv,nv); + Minv_tmp.resize(nv,nv); + + const Eigen::DenseIndex total_dim = nv + num_total_constraints; + + // Compute first parents_fromRow for all the joints. + // This code is very similar to the code of Data::computeParents_fromRow, + // but shifted with a value corresponding to the number of constraints. + parents_fromRow.resize(total_dim); + parents_fromRow.fill(-1); + + nv_subtree_fromRow.resize(total_dim); +// nv_subtree_fromRow.fill(0); + + last_child.resize(model.njoints); + last_child.fill(-1); + for(long joint_id = model.njoints-1; joint_id >= 0; --joint_id) + { + const JointIndex & parent = model.parents[(size_t)joint_id]; + if(last_child[joint_id] == -1) + last_child[joint_id] = joint_id; + last_child[(Eigen::DenseIndex)parent] = std::max(last_child[joint_id], + last_child[(Eigen::DenseIndex)parent]); + } + + for(JointIndex joint_id = 1;joint_id < (JointIndex)(model.njoints);joint_id++) + { + const JointIndex & parent_id = model.parents[joint_id]; + + const typename Model::JointModel & joint = model.joints[joint_id]; + const typename Model::JointModel & parent_joint = model.joints[parent_id]; + const int nvj = joint.nv(); + const int idx_vj = joint.idx_v(); + + if(parent_id>0) + parents_fromRow[idx_vj + num_total_constraints] + = parent_joint.idx_v() + parent_joint.nv() - 1 + num_total_constraints; + else + parents_fromRow[idx_vj+num_total_constraints] = -1; + + nv_subtree_fromRow[idx_vj+num_total_constraints] + = model.joints[(size_t)last_child[(Eigen::DenseIndex)joint_id]].idx_v() + + model.joints[(size_t)last_child[(Eigen::DenseIndex)joint_id]].nv() + - idx_vj; + + for(int row=1;row(num_contacts), + BooleanVector::Constant(total_dim,default_sparsity_value)); + joint2_indexes.resize(static_cast(num_contacts), + BooleanVector::Constant(total_dim,default_sparsity_value)); + colwise_sparsity_patterns.resize(static_cast(num_contacts), + IndexVector()); + colwise_loop_sparsity_patterns.resize(static_cast(num_contacts), + IndexVector()); + for(size_t ee_id = 0; ee_id < joint1_indexes.size(); ++ee_id) + { + BooleanVector & joint1_indexes_ee = joint1_indexes[ee_id]; + joint1_indexes_ee.resize(total_dim); joint1_indexes_ee.fill(default_sparsity_value); + BooleanVector & joint2_indexes_ee = joint2_indexes[ee_id]; + joint2_indexes_ee.resize(total_dim); joint2_indexes_ee.fill(default_sparsity_value); + IndexVector & colwise_sparsity_patterns_ee = colwise_sparsity_patterns[ee_id]; + IndexVector & colwise_loop_sparsity_patterns_ee = colwise_loop_sparsity_patterns[ee_id]; + + const RigidConstraintModel & cmodel = contact_models[ee_id]; + + const JointIndex joint1_id = cmodel.joint1_id; + JointIndex current1_id = 0; + if(joint1_id > 0) + current1_id = joint1_id; + + const JointIndex joint2_id = cmodel.joint2_id; + JointIndex current2_id = 0; + if(joint2_id > 0) + current2_id = joint2_id; + + while(current1_id != current2_id) + { + if(current1_id > current2_id) + { + const typename Model::JointModel & joint1 = model.joints[current1_id]; + Eigen::DenseIndex current1_row_id = joint1.idx_v() + num_total_constraints; + for(int k = 0; k < joint1.nv(); ++k,++current1_row_id) + { + joint1_indexes_ee[current1_row_id] = true; + } + current1_id = model.parents[current1_id]; + } + else + { + const typename Model::JointModel & joint2 = model.joints[current2_id]; + Eigen::DenseIndex current2_row_id = joint2.idx_v() + num_total_constraints; + for(int k = 0; k < joint2.nv(); ++k,++current2_row_id) + { + joint2_indexes_ee[current2_row_id] = true; + } + current2_id = model.parents[current2_id]; + } + } + assert(current1_id == current2_id && "current1_id should be equal to current2_id"); + // current1_id and current2_id now contains the common ancestor to the two joints. + if(cmodel.type == CONTACT_3D && cmodel.reference_frame != WORLD) + { + JointIndex current_id = current1_id; + while(current_id > 0) + { + const typename Model::JointModel & joint = model.joints[current_id]; + Eigen::DenseIndex current_row_id = joint.idx_v() + num_total_constraints; + for(int k = 0; k < joint.nv(); ++k,++current_row_id) + { + joint1_indexes_ee[current_row_id] = true; + joint2_indexes_ee[current_row_id] = true; + } + current_id = model.parents[current_id]; + } + } + + Eigen::DenseIndex size = 0; + colwise_sparsity_patterns_ee.resize(total_dim); + for(Eigen::DenseIndex col_id = 0; col_id < total_dim; ++col_id) + { + if(joint1_indexes_ee[col_id] || joint2_indexes_ee[col_id]) + colwise_sparsity_patterns_ee[size++] = col_id; + } + colwise_sparsity_patterns_ee.conservativeResize(size); + + size = 0; + colwise_loop_sparsity_patterns_ee.resize(total_dim); + for(Eigen::DenseIndex col_id = 0; col_id < total_dim; ++col_id) + { + if(joint1_indexes_ee[col_id] != joint2_indexes_ee[col_id]) + colwise_loop_sparsity_patterns_ee[size++] = col_id; + } + colwise_loop_sparsity_patterns_ee.conservativeResize(size); + + } + + // Fill the sparsity pattern for each Row of the Cholesky decomposition (matrix U) + static const Slice default_slice_value(1,1); + static const SliceVector default_slice_vector(1,default_slice_value); + + rowise_sparsity_pattern.clear(); + rowise_sparsity_pattern.resize((size_t)num_total_constraints,default_slice_vector); + row_id = 0; size_t ee_id = 0; + for(typename RigidConstraintModelVector::const_iterator it = contact_models.begin(); + it != contact_models.end(); + ++it, ++ee_id) + { + const RigidConstraintModel & cmodel = *it; + const BooleanVector & joint1_indexes_ee = joint1_indexes[ee_id]; + const Eigen::DenseIndex contact_dim = cmodel.size(); + + for(Eigen::DenseIndex k = 0; k < contact_dim; ++k) + { + SliceVector & slice_vector = rowise_sparsity_pattern[(size_t)row_id]; + slice_vector.clear(); + slice_vector.push_back(Slice(row_id,num_total_constraints-row_id)); + + bool previous_index_was_true = true; + for(Eigen::DenseIndex joint1_indexes_ee_id = num_total_constraints; + joint1_indexes_ee_id < total_dim; + ++joint1_indexes_ee_id) + { + if(joint1_indexes_ee[joint1_indexes_ee_id]) + { + if(previous_index_was_true) // no discontinuity + slice_vector.back().size++; + else // discontinuity; need to create a new slice + { + const Slice new_slice(joint1_indexes_ee_id,1); + slice_vector.push_back(new_slice); + } + } + + previous_index_was_true = joint1_indexes_ee[joint1_indexes_ee_id]; + } + + row_id++; + } + } + + // Allocate memory + D.resize(total_dim); Dinv.resize(total_dim); + U.resize(total_dim,total_dim); + U.setIdentity(); + DUt.resize(total_dim); + } + + template + template class JointCollectionTpl, class ContactModelAllocator, class ContactDataAllocator> + void ContactCholeskyDecompositionTpl:: + compute(const ModelTpl & model, + DataTpl & data, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_datas, + const S1 mu) + { + typedef RigidConstraintModelTpl RigidConstraintModel; + typedef RigidConstraintDataTpl RigidConstraintData; + typedef MotionTpl Motion; + typedef SE3Tpl SE3; + assert(model.check(data) && "data is not consistent with model."); + PINOCCHIO_CHECK_INPUT_ARGUMENT((Eigen::DenseIndex)contact_models.size() == num_contacts, + "The number of contacts inside contact_models and the one during allocation do not match."); + PINOCCHIO_CHECK_INPUT_ARGUMENT((Eigen::DenseIndex)contact_datas.size() == num_contacts, + "The number of contacts inside contact_datas and the one during allocation do not match."); + PINOCCHIO_UNUSED_VARIABLE(model); + + const Eigen::DenseIndex total_dim = size(); + const Eigen::DenseIndex total_constraints_dim = total_dim - nv; + + typedef ModelTpl Model; + typedef DataTpl Data; + const typename Data::MatrixXs & M = data.M; + + const size_t num_ee = contact_models.size(); + + // Update frame placements if needed + for(size_t ee_id = 0; ee_id < num_ee; ++ee_id) + { + const RigidConstraintModel & cmodel = contact_models[ee_id]; + RigidConstraintData & cdata = contact_datas[ee_id]; + + const typename Model::JointIndex joint1_id = cmodel.joint1_id; + if(joint1_id > 0) + cdata.oMc1 = data.oMi[joint1_id] * cmodel.joint1_placement; + else + cdata.oMc1 = cmodel.joint1_placement; + + const typename Model::JointIndex joint2_id = cmodel.joint2_id; + if(joint2_id > 0) + cdata.oMc2 = data.oMi[joint2_id] * cmodel.joint2_placement; + else + cdata.oMc2 = cmodel.joint2_placement; + + // Compute relative placement + cdata.c1Mc2 = cdata.oMc1.actInv(cdata.oMc2); + } + + // Core +// Motion Jcol_motion; + for(Eigen::DenseIndex j=nv-1;j>=0;--j) + { + // Classic Cholesky decomposition related to the mass matrix + const Eigen::DenseIndex jj = total_constraints_dim + j; // shifted index + const Eigen::DenseIndex NVT = nv_subtree_fromRow[jj]-1; + typename Vector::SegmentReturnType DUt_partial = DUt.head(NVT); + + if(NVT) + DUt_partial.noalias() = U.row(jj).segment(jj+1,NVT).transpose() + .cwiseProduct(D.segment(jj+1,NVT)); + + D[jj] = M(j,j) - U.row(jj).segment(jj+1,NVT).dot(DUt_partial); + assert(check_expression_if_real(D[jj] != Scalar(0)) && "The diagonal element is equal to zero."); + Dinv[jj] = Scalar(1)/D[jj]; + + for(Eigen::DenseIndex _ii = parents_fromRow[jj]; _ii >= total_constraints_dim; _ii = parents_fromRow[_ii]) + { + const Eigen::DenseIndex _i = _ii - total_constraints_dim; + U(_ii,jj) = (M(_i,j) - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + + // Constraint handling + if(num_ee == 0) + continue; + + Eigen::DenseIndex current_row = total_constraints_dim - 1; + + for(size_t k = 0; k < num_ee; ++k) + { + const size_t ee_id = num_ee - k - 1; // start from the last end effector + + const BooleanVector & joint1_indexes_ee = joint1_indexes[ee_id]; + const BooleanVector & joint2_indexes_ee = joint2_indexes[ee_id]; + const RigidConstraintModel & cmodel = contact_models[ee_id]; + const RigidConstraintData & cdata = contact_datas[ee_id]; + + const Eigen::DenseIndex constraint_dim = cmodel.size(); + const SE3 & oMc1 = cdata.oMc1; + const SE3 & oMc2 = cdata.oMc2; + const SE3 & c1Mc2 = cdata.c1Mc2; + + if(joint1_indexes_ee[jj] || joint2_indexes_ee[jj]) + { + const int sign = + joint1_indexes_ee[jj] != joint2_indexes_ee[jj] + ? joint1_indexes_ee[jj] ? +1:-1 + : 0; // specific case for CONTACT_3D + + typedef typename Data::Matrix6x::ColXpr ColXpr; + const ColXpr Jcol = data.J.col(j); + const MotionRef Jcol_motion(Jcol); + + switch(cmodel.type) + { + case CONTACT_3D: + { + switch(cmodel.reference_frame) + { + case WORLD: + { + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jcol_motion.linear()[contact_dim::value-_i-1] * sign + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + case LOCAL: + { + if(sign == 0) + { + const Motion Jcol_local1(oMc1.actInv(Jcol_motion)); // TODO: simplify computations + const Motion Jcol_local2(oMc2.actInv(Jcol_motion)); // TODO: simplify computations + const typename Motion::Vector3 Jdiff_linear = Jcol_local1.linear() - c1Mc2.rotation()*Jcol_local2.linear(); + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jdiff_linear[contact_dim::value-_i-1] + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + else if(sign == 1) + { + const Motion Jcol_local(oMc1.actInv(Jcol_motion)); + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jcol_local.linear()[contact_dim::value-_i-1] * sign + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + else // sign == -1 + { + Motion Jcol_local(oMc2.actInv(Jcol_motion)); + Jcol_local.linear() = c1Mc2.rotation()*Jcol_local.linear(); + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jcol_local.linear()[contact_dim::value-_i-1] * sign + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + } + case LOCAL_WORLD_ALIGNED: + { + if(sign == 0) + { + const typename Motion::Vector3 Jdiff_linear = (oMc2.translation() - oMc1.translation()).cross(Jcol_motion.angular()); + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jdiff_linear[contact_dim::value-_i-1] + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + else + { + typename Motion::Vector3 Jcol_local_world_aligned_linear(Jcol_motion.linear()); + if(sign == 1) + Jcol_local_world_aligned_linear + -= oMc1.translation().cross(Jcol_motion.angular()); + else + Jcol_local_world_aligned_linear + -= oMc2.translation().cross(Jcol_motion.angular()); + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jcol_local_world_aligned_linear[contact_dim::value-_i-1] * sign + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + } + } + break; + } + + case CONTACT_6D: + { + assert(check_expression_if_real(sign != 0) && "sign should be equal to +1 or -1."); + switch(cmodel.reference_frame) + { + case WORLD: + { + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jcol_motion.toVector()[contact_dim::value-_i-1] * sign + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + case LOCAL: + { + const Motion Jcol_local(oMc1.actInv(Jcol_motion)); + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jcol_local.toVector()[contact_dim::value-_i-1] * sign + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + case LOCAL_WORLD_ALIGNED: + { + Motion Jcol_local_world_aligned(Jcol_motion); + Jcol_local_world_aligned.linear() + -= oMc1.translation().cross(Jcol_local_world_aligned.angular()); + for(Eigen::DenseIndex _i = 0; _i < contact_dim::value; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = (Jcol_local_world_aligned.toVector()[contact_dim::value-_i-1] * sign + - U.row(_ii).segment(jj+1,NVT).dot(DUt_partial)) * Dinv[jj]; + } + break; + } + } + break; + } + + default: + assert(false && "must never happened"); + break; + } + + } + else if((joint1_indexes_ee[jj] == false) && (joint2_indexes_ee[jj] == false)) + { + for(Eigen::DenseIndex _i = 0; _i < constraint_dim; _i++) + { + const Eigen::DenseIndex _ii = current_row - _i; + U(_ii,jj) = -U.row(_ii).segment(jj+1,NVT).dot(DUt_partial) * Dinv[jj]; + } + } + current_row -= constraint_dim; + } + + } + + // Upper left triangular part of U + for(Eigen::DenseIndex j = total_constraints_dim-1; j>=0; --j) + { + const Eigen::DenseIndex slice_dim = total_dim - j - 1; + typename Vector::SegmentReturnType DUt_partial = DUt.head(slice_dim); + DUt_partial.noalias() = U.row(j).segment(j+1,slice_dim).transpose().cwiseProduct(D.segment(j+1,slice_dim)); + + D[j] = -mu - U.row(j).segment(j+1,slice_dim).dot(DUt_partial); + assert(check_expression_if_real(D[j] != Scalar(0)) && "The diagonal element is equal to zero."); + Dinv[j] = Scalar(1)/D[j]; + + for(Eigen::DenseIndex _i = j-1; _i >= 0; _i--) + { + U(_i,j) = -U.row(_i).segment(j+1,slice_dim).dot(DUt_partial) * Dinv[j]; + } + } + } + + template + template + void ContactCholeskyDecompositionTpl:: + solveInPlace(const Eigen::MatrixBase & mat) const + { + MatrixLike & mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat); + + Uiv(mat_); + mat_.array().colwise() *= Dinv.array(); + Utiv(mat_); + } + + template + template + typename ContactCholeskyDecompositionTpl::Matrix + ContactCholeskyDecompositionTpl:: + solve(const Eigen::MatrixBase & mat) const + { + Matrix res(mat); + solveInPlace(res); + return res; + } + + template + template class JointCollectionTpl> + ContactCholeskyDecompositionTpl + ContactCholeskyDecompositionTpl:: + getMassMatrixChoeslkyDecomposition(const ModelTpl & model) const + { + typedef ContactCholeskyDecompositionTpl ReturnType; + ReturnType res(model); + + res.D = D.tail(nv); + res.Dinv = Dinv.tail(nv); + res.U = U.bottomRightCorner(nv,nv); + + return res; + } + + namespace details + { + template + struct UvAlgo + { + template + static void run(const ContactCholeskyDecompositionTpl & chol, + const Eigen::MatrixBase & mat) + { + MatrixLike & mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(mat.rows() == chol.size(), + "The input matrix is of wrong size"); + + for(Eigen::DenseIndex col_id = 0; col_id < mat_.cols(); ++col_id) + UvAlgo::run(chol,mat_.col(col_id)); + } + }; + + template + struct UvAlgo + { + template + static void run(const ContactCholeskyDecompositionTpl & chol, + const Eigen::MatrixBase & vec) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike) + VectorLike & vec_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike,vec); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(vec.size() == chol.size(), + "The input vector is of wrong size"); + const Eigen::DenseIndex num_total_constraints = chol.size() - chol.nv; + + // TODO: exploit the Sparsity pattern of the first rows of U + for(Eigen::DenseIndex k = 0; k < num_total_constraints; ++k) + { + const Eigen::DenseIndex slice_dim = chol.size() - k - 1; + vec_[k] += chol.U.row(k).tail(slice_dim).dot(vec_.tail(slice_dim)); + } + + for(Eigen::DenseIndex k = num_total_constraints; k <= chol.size()-2; ++k) + vec_[k] += chol.U.row(k).segment(k+1,chol.nv_subtree_fromRow[k]-1).dot(vec_.segment(k+1,chol.nv_subtree_fromRow[k]-1)); + } + }; + } // namespace details + + template + template + void ContactCholeskyDecompositionTpl:: + Uv(const Eigen::MatrixBase & mat) const + { + details::UvAlgo::run(*this, + PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat)); + } + + namespace details + { + template + struct UtvAlgo + { + template + static void run(const ContactCholeskyDecompositionTpl & chol, + const Eigen::MatrixBase & mat) + { + MatrixLike & mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(mat.rows() == chol.size(), + "The input matrix is of wrong size"); + + for(Eigen::DenseIndex col_id = 0; col_id < mat_.cols(); ++col_id) + UtvAlgo::run(chol,mat_.col(col_id)); + } + }; + + template + struct UtvAlgo + { + template + static void run(const ContactCholeskyDecompositionTpl & chol, + const Eigen::MatrixBase & vec) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike) + VectorLike & vec_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike,vec); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(vec.size() == chol.size(), + "The input vector is of wrong size"); + const Eigen::DenseIndex num_total_constraints = chol.constraintDim(); + + for(Eigen::DenseIndex k = chol.size()-2; k >= num_total_constraints; --k) + vec_.segment(k+1,chol.nv_subtree_fromRow[k]-1) + += chol.U.row(k).segment(k+1,chol.nv_subtree_fromRow[k]-1).transpose()*vec_[k]; + + // TODO: exploit the Sparsity pattern of the first rows of U + for(Eigen::DenseIndex k = num_total_constraints-1; k >=0; --k) + { + const Eigen::DenseIndex slice_dim = chol.size() - k - 1; + vec_.tail(slice_dim) += chol.U.row(k).tail(slice_dim).transpose()*vec_[k]; + } + } + }; + } // namespace details + + template + template + void ContactCholeskyDecompositionTpl:: + Utv(const Eigen::MatrixBase & mat) const + { + details::UtvAlgo::run(*this, + PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat)); + } + + namespace details + { + template + struct UivAlgo + { + template + static void run(const ContactCholeskyDecompositionTpl & chol, + const Eigen::MatrixBase & mat) + { + MatrixLike & mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(mat.rows() == chol.size(), + "The input matrix is of wrong size"); + + for(Eigen::DenseIndex col_id = 0; col_id < mat_.cols(); ++col_id) + UivAlgo::run(chol,mat_.col(col_id)); + } + }; + + template + struct UivAlgo + { + template + static void run(const ContactCholeskyDecompositionTpl & chol, + const Eigen::MatrixBase & vec) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike) + VectorLike & vec_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike,vec); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(vec.size() == chol.size(), + "The input vector is of wrong size"); + + const Eigen::DenseIndex num_total_constraints = chol.size() - chol.nv; + for(Eigen::DenseIndex k = chol.size()-2; k >= num_total_constraints; --k) + vec_[k] -= chol.U.row(k).segment(k+1,chol.nv_subtree_fromRow[k]-1).dot(vec_.segment(k+1,chol.nv_subtree_fromRow[k]-1)); + + // TODO: exploit the Sparsity pattern of the first rows of U + for(Eigen::DenseIndex k = num_total_constraints-1; k >=0; --k) + { + const Eigen::DenseIndex slice_dim = chol.size() - k - 1; + vec_[k] -= chol.U.row(k).tail(slice_dim).dot(vec_.tail(slice_dim)); + } + } + }; + } // namespace details + + template + template + void ContactCholeskyDecompositionTpl:: + Uiv(const Eigen::MatrixBase & mat) const + { + details::UivAlgo::run(*this, + PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat)); + } + + namespace details + { + template + struct UtivAlgo + { + template + static void run(const ContactCholeskyDecompositionTpl & chol, + const Eigen::MatrixBase & mat) + { + MatrixLike & mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(mat.rows() == chol.size(), + "The input matrix is of wrong size"); + + for(Eigen::DenseIndex col_id = 0; col_id < mat_.cols(); ++col_id) + UtivAlgo::run(chol,mat_.col(col_id)); + } + }; + + template + struct UtivAlgo + { + template + static void run(const ContactCholeskyDecompositionTpl & chol, + const Eigen::MatrixBase & vec) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike) + VectorLike & vec_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike,vec); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(vec.size() == chol.size(), + "The input vector is of wrong size"); + const Eigen::DenseIndex num_total_constraints = chol.constraintDim(); + + // TODO: exploit the Sparsity pattern of the first rows of U + for(Eigen::DenseIndex k = 0; k < num_total_constraints; ++k) + { + const Eigen::DenseIndex slice_dim = chol.size() - k - 1; + vec_.tail(slice_dim) -= chol.U.row(k).tail(slice_dim).transpose() * vec_[k]; + } + + for(Eigen::DenseIndex k = num_total_constraints; k <= chol.size()-2; ++k) + vec_.segment(k+1,chol.nv_subtree_fromRow[k]-1) + -= chol.U.row(k).segment(k+1,chol.nv_subtree_fromRow[k]-1).transpose() * vec_[k]; + } + }; + } // namespace details + + template + template + void ContactCholeskyDecompositionTpl:: + Utiv(const Eigen::MatrixBase & mat) const + { + details::UtivAlgo::run(*this, + PINOCCHIO_EIGEN_CONST_CAST(MatrixLike,mat)); + } + + template + typename ContactCholeskyDecompositionTpl::Matrix + ContactCholeskyDecompositionTpl::matrix() const + { + Matrix res(size(),size()); + matrix(res); + return res; + } + + template + template + void ContactCholeskyDecompositionTpl:: + matrix(const Eigen::MatrixBase & res) const + { + MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,res); + res_.noalias() = U * D.asDiagonal() * U.transpose(); + } + + template + typename ContactCholeskyDecompositionTpl::Matrix + ContactCholeskyDecompositionTpl::inverse() const + { + Matrix res(size(),size()); + inverse(res); + return res; + } + + namespace details + { + + template + EIGEN_DONT_INLINE + VectorLike & inverseAlgo(const ContactCholeskyDecompositionTpl & chol, + const Eigen::DenseIndex col, + const Eigen::MatrixBase & vec) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike); + + typedef ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; + typedef typename ContactCholeskyDecomposition::RowMatrix RowMatrix; + + const Eigen::DenseIndex & chol_dim = chol.size(); + PINOCCHIO_CHECK_INPUT_ARGUMENT(col < chol_dim && col >= 0); + PINOCCHIO_CHECK_INPUT_ARGUMENT(vec.size() == chol_dim); + + const typename ContactCholeskyDecomposition::IndexVector & nvt = chol.nv_subtree_fromRow; + VectorLike & vec_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike,vec); + + const Eigen::DenseIndex last_col = std::min(col-1,chol_dim-2); // You can start from nv-2 (no child in nv-1) + vec_[col] = Scalar(1); + vec_.tail(chol_dim - col - 1).setZero(); + + // TODO: exploit the sparsity pattern of the first rows of U + for(Eigen::DenseIndex k = last_col; k >= 0; --k) + { + const Eigen::DenseIndex nvt_max = std::min(col-k,nvt[k]-1); + const typename RowMatrix::ConstRowXpr U_row = chol.U.row(k); + vec_[k] = -U_row.segment(k+1,nvt_max).dot(vec_.segment(k+1,nvt_max)); +// if(k >= chol_constraint_dim) +// { +// vec_[k] = -U_row.segment(k+1,nvt_max).dot(vec_.segment(k+1,nvt_max)); +// } +// else +// { +// typedef typename ContactCholeskyDecomposition::SliceVector SliceVector; +// typedef typename ContactCholeskyDecomposition::Slice Slice; +// const SliceVector & slice_vector = chol.rowise_sparsity_pattern[(size_t)k]; +// +// const Slice & slice_0 = slice_vector[0]; +// assert(slice_0.first_index == k); +// Eigen::DenseIndex last_index1 = slice_0.first_index + slice_0.size; +// const Eigen::DenseIndex last_index2 = k + nvt_max; +// Eigen::DenseIndex slice_dim = std::min(last_index1,last_index2) - k; +// vec_[k] = -U_row.segment(slice_0.first_index+1,slice_dim-1).dot(vec_.segment(slice_0.first_index+1,slice_dim-1)); +// +// typename SliceVector::const_iterator slice_it = slice_vector.begin()++; +// for(;slice_it != slice_vector.end(); ++slice_it) +// { +// const Slice & slice = *slice_it; +// last_index1 = slice.first_index + slice.size; +// slice_dim = std::min(last_index1,last_index2+1) - slice.first_index; +// if(slice_dim <= 0) break; +// +// vec_[k] -= U_row.segment(slice.first_index,slice_dim).dot(vec_.segment(slice.first_index,slice_dim)); +// } +// } + } + + vec_.head(col+1).array() *= chol.Dinv.head(col+1).array(); + + for(Eigen::DenseIndex k = 0; k < col+1; ++k) // You can stop one step before nv. + { + const Eigen::DenseIndex nvt_max = nvt[k]-1; + vec_.segment(k+1,nvt_max) -= chol.U.row(k).segment(k+1,nvt_max).transpose() * vec_[k]; + } + + return vec_; + } + } // namespace details + + template + template + void ContactCholeskyDecompositionTpl:: + inverse(const Eigen::MatrixBase & res) const + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(res.rows() == size()); + PINOCCHIO_CHECK_INPUT_ARGUMENT(res.cols() == size()); + + MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType,res); + + for(Eigen::DenseIndex col_id = 0; col_id < size(); ++col_id) + details::inverseAlgo(*this,col_id,res_.col(col_id)); + + res_.template triangularView() + = res_.transpose().template triangularView(); + } + + } // namespace cholesky +} + +#endif // ifndef __pinocchio_algorithm_contact_cholesky_hxx__ diff --git a/src/algorithm/contact-dynamics.hpp b/src/algorithm/contact-dynamics.hpp index fb95692a4b..fdbddebe38 100644 --- a/src/algorithm/contact-dynamics.hpp +++ b/src/algorithm/contact-dynamics.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2016-2020 CNRS INRIA // -#ifndef __pinocchio_contact_dynamics_hpp__ -#define __pinocchio_contact_dynamics_hpp__ +#ifndef __pinocchio_algorithm_contact_dynamics_hpp__ +#define __pinocchio_algorithm_contact_dynamics_hpp__ #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" @@ -13,6 +13,8 @@ namespace pinocchio /// /// \brief Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is called. + /// \deprecated This function has been deprecated and will be removed in future releases of Pinocchio. + /// Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and constraintDynamics(model, data, q, v, tau, contact_models, contact_datas[,prox_settings]) instead. /// \note It computes the following problem:
///
\f$ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ % /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$

@@ -42,6 +44,7 @@ namespace pinocchio /// template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename ConstraintMatrixType, typename DriftVectorType> + PINOCCHIO_DEPRECATED inline const typename DataTpl::TangentVectorType & forwardDynamics(const ModelTpl & model, DataTpl & data, @@ -54,6 +57,9 @@ namespace pinocchio /// /// \brief Compute the forward dynamics with contact constraints, assuming pinocchio::computeAllTerms has been called. + /// \deprecated This function has been deprecated and will be removed in future releases of Pinocchio. + /// Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and constraintDynamics(model, data, q, v, tau, contact_models, contact_datas[,prox_settings]) instead. + /// \note It computes the following problem:
///
\f$ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ % /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \f$

@@ -82,6 +88,7 @@ namespace pinocchio /// template class JointCollectionTpl, typename TangentVectorType, typename ConstraintMatrixType, typename DriftVectorType> + PINOCCHIO_DEPRECATED inline const typename DataTpl::TangentVectorType & forwardDynamics(const ModelTpl & model, DataTpl & data, @@ -165,6 +172,7 @@ namespace pinocchio /// /// \brief Computes the inverse of the KKT matrix for dynamics with contact constraints. + /// \deprecated forwardDynamics/impuseDynamics is deprecated, and this function signature needs forwardDynamics to be called before. Please use the inverse() function in ContactCholeskyDecomposition class instead. /// It computes the following matrix:
///
\f$ \left[\begin{matrix}\mathbf{M}^{-1}-\mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & \mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1} \\ \widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & -\widehat{\mathbf{M}}^{-1}\end{matrix}\right] \f$

/// @@ -180,6 +188,7 @@ namespace pinocchio /// template class JointCollectionTpl, typename ConstraintMatrixType, typename KKTMatrixType> + PINOCCHIO_DEPRECATED inline void getKKTContactDynamicMatrixInverse(const ModelTpl & model, const DataTpl & data, const Eigen::MatrixBase & J, @@ -187,6 +196,8 @@ namespace pinocchio /// /// \brief Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called. + /// \deprecated This function has been deprecated and will be removed in future releases of Pinocchio. + /// Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and impulseDynamics(model, data, q, v_before, contact_models, contact_datas[,r_coeff, mu]) instead. /// \note It computes the following problem:
///
\f$ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ % /// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$

@@ -209,6 +220,7 @@ namespace pinocchio /// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector. /// template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType> + PINOCCHIO_DEPRECATED inline const typename DataTpl::TangentVectorType & impulseDynamics(const ModelTpl & model, DataTpl & data, @@ -220,6 +232,8 @@ namespace pinocchio /// /// \brief Compute the impulse dynamics with contact constraints, assuming pinocchio::crba has been called. + /// \deprecated This function has been deprecated and will be removed in future releases of Pinocchio. + /// Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and impulseDynamics(model, data, q, v_before, contact_models, contact_datas[,r_coeff, mu]) instead. /// \note It computes the following problem:
///
\f$ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ % /// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$

@@ -241,6 +255,7 @@ namespace pinocchio /// \return A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector. /// template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType> + PINOCCHIO_DEPRECATED inline const typename DataTpl::TangentVectorType & impulseDynamics(const ModelTpl & model, DataTpl & data, @@ -252,8 +267,8 @@ namespace pinocchio /// /// \brief Compute the impulse dynamics with contact constraints. /// - /// \deprecated This function signature has been deprecated and will be removed in future releases of Pinocchio. - /// Please change for the new signature of impulseDynamics(model,data[,q],v_before,J[,r_coeff[,inv_damping]]). + /// \deprecated This function has been deprecated and will be removed in future releases of Pinocchio. + /// Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and impulseDynamics(model, data, q, v_before, contact_models, contact_datas[,r_coeff, mu]) instead. /// /// \note It computes the following problem:
///
\f$ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ % @@ -297,4 +312,4 @@ namespace pinocchio #include "pinocchio/algorithm/contact-dynamics.hxx" -#endif // ifndef __pinocchio_contact_dynamics_hpp__ +#endif // ifndef __pinocchio_algorithm_contact_dynamics_hpp__ diff --git a/src/algorithm/contact-dynamics.hxx b/src/algorithm/contact-dynamics.hxx index ae88fad123..b447da152b 100644 --- a/src/algorithm/contact-dynamics.hxx +++ b/src/algorithm/contact-dynamics.hxx @@ -2,19 +2,80 @@ // Copyright (c) 2016-2020 CNRS INRIA // -#ifndef __pinocchio_contact_dynamics_hxx__ -#define __pinocchio_contact_dynamics_hxx__ +#ifndef __pinocchio_algorithm_constrained_dynamics_hxx__ +#define __pinocchio_algorithm_constrained_dynamics_hxx__ #include "pinocchio/algorithm/compute-all-terms.hpp" #include "pinocchio/algorithm/cholesky.hpp" #include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/check.hpp" +#include "pinocchio/math/matrix.hpp" #include namespace pinocchio { + namespace internal + { + template::value> + struct PerformCholeskySolveInPlace + { + template + static void run(const Eigen::MatrixBase & mat, + Eigen::LLT & llt, + const Eigen::MatrixBase & res, + const bool compute) + { + if(compute) llt.compute(mat); + llt.solveInPlace(res.const_cast_derived()); + } + + template + static void run(const Eigen::MatrixBase & /*mat*/, + const Eigen::LLT & llt, + const Eigen::MatrixBase & res) + { + llt.solveInPlace(res.const_cast_derived()); + } + }; + + template + struct PerformCholeskySolveInPlace + { + template + static void run(const Eigen::MatrixBase & mat, + const Eigen::LLT & /*llt*/, + const Eigen::MatrixBase & res) + { + typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixIn) mat_inv(mat.rows(),mat.cols()); + inverse(mat,mat_inv); + res.const_cast_derived() = mat_inv * res; + } + }; + + template::value> + struct PerformCholeskyCompute + { + template + static void run(const Eigen::MatrixBase & mat, + Eigen::LLT & llt) + { + llt.compute(mat); + } + }; + + template + struct PerformCholeskyCompute + { + template + static void run(const Eigen::MatrixBase &, + Eigen::LLT &) + {} + }; + + } + template class JointCollectionTpl, typename TangentVectorType, typename ConstraintMatrixType, typename DriftVectorType> inline const typename DataTpl::TangentVectorType & @@ -51,12 +112,13 @@ namespace pinocchio data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt; data.JMinvJt.diagonal().array() += inv_damping; - data.llt_JMinvJt.compute(data.JMinvJt); // Compute the Lagrange Multipliers - lambda_c.noalias() = -J*data.torque_residual; - lambda_c -= gamma; - data.llt_JMinvJt.solveInPlace(lambda_c); + lambda_c.noalias() = -J*data.torque_residual - gamma; +// data.llt_JMinvJt.compute(data.JMinvJt); +// data.llt_JMinvJt.solveInPlace(lambda_c); + internal::PerformCholeskyCompute::run(data.JMinvJt,data.llt_JMinvJt); + internal::PerformCholeskySolveInPlace::run(data.JMinvJt,data.llt_JMinvJt,lambda_c); // Compute the joint acceleration a.noalias() = J.transpose() * lambda_c; @@ -96,10 +158,11 @@ namespace pinocchio const Scalar & inv_damping) { assert(model.check(data)); - PINOCCHIO_CHECK_INPUT_ARGUMENT(inv_damping >= 0., "mu must be positive."); + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real(inv_damping >= 0.), + "mu must be positive."); // Compute the mass matrix. - crbaMinimal(model,data,q); + crba(model,data,q); // Compute the UDUt decomposition of data.M. cholesky::decompose(model, data); @@ -114,7 +177,8 @@ namespace pinocchio data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt; data.JMinvJt.diagonal().array() += inv_damping; - data.llt_JMinvJt.compute(data.JMinvJt); +// data.llt_JMinvJt.compute(data.JMinvJt); + internal::PerformCholeskyCompute::run(data.JMinvJt,data.llt_JMinvJt); getKKTContactDynamicMatrixInverse(model,data,J,KKTMatrix_inv.const_cast_derived()); } @@ -141,7 +205,9 @@ namespace pinocchio BlockType bottomLeft = KKTMatrix_inv_.bottomLeftCorner(nc, model.nv); BlockType bottomRight = KKTMatrix_inv_.bottomRightCorner(nc, nc); - bottomRight = -Data::MatrixXs::Identity(nc,nc); data.llt_JMinvJt.solveInPlace(bottomRight); + bottomRight = -Data::MatrixXs::Identity(nc,nc); +// data.llt_JMinvJt.solveInPlace(bottomRight); + internal::PerformCholeskySolveInPlace::run(data.JMinvJt,data.llt_JMinvJt,bottomRight); topLeft.setIdentity(); cholesky::solve(model, data, topLeft); bottomLeft.noalias() = J*topLeft; @@ -192,16 +258,18 @@ namespace pinocchio data.sDUiJt = J.transpose(); // Compute U^-1 * J.T cholesky::Uiv(model, data, data.sDUiJt); - for(int k=0;k::run(data.JMinvJt,data.llt_JMinvJt); + internal::PerformCholeskySolveInPlace::run(data.JMinvJt,data.llt_JMinvJt,impulse_c); // Compute the joint velocity after impacts dq_after.noalias() = J.transpose() * impulse_c; @@ -212,4 +280,4 @@ namespace pinocchio } } // namespace pinocchio -#endif // ifndef __pinocchio_contact_dynamics_hxx__ +#endif // ifndef __pinocchio_algorithm_constrained_dynamics_hxx__ diff --git a/src/algorithm/contact-info.hpp b/src/algorithm/contact-info.hpp new file mode 100644 index 0000000000..dcf0fe29d3 --- /dev/null +++ b/src/algorithm/contact-info.hpp @@ -0,0 +1,426 @@ +// +// Copyright (c) 2019-2020 INRIA CNRS +// + +#ifndef __pinocchio_algorithm_contact_info_hpp__ +#define __pinocchio_algorithm_contact_info_hpp__ + +#include "pinocchio/multibody/fwd.hpp" +#include "pinocchio/algorithm/fwd.hpp" +#include "pinocchio/spatial/se3.hpp" +#include "pinocchio/spatial/motion.hpp" +#include "pinocchio/spatial/force.hpp" + +#include +#include + +namespace pinocchio +{ + /// \brief Type of contact + enum ContactType + { + CONTACT_3D = 0, /// \brief Point contact model + CONTACT_6D, /// \brief Frame contact model + CONTACT_UNDEFINED /// \brief The default contact is undefined + }; + + template + struct contact_dim + { + enum { value = 0 }; + }; + + template<> + struct contact_dim + { + enum { value = 3 }; + }; + + template<> + struct contact_dim + { + enum { value = 6 }; + }; + + template struct BaumgarteCorrectorParametersTpl; + + template + struct traits< BaumgarteCorrectorParametersTpl<_Scalar> > + { + typedef _Scalar Scalar; + }; + + template + struct BaumgarteCorrectorParametersTpl + : NumericalBase< BaumgarteCorrectorParametersTpl<_Scalar> > + { + typedef _Scalar Scalar; + + BaumgarteCorrectorParametersTpl() + : Kp(Scalar(0)), Kd(Scalar(0)) + {}; + + bool operator ==(const BaumgarteCorrectorParametersTpl & other) const + { return Kp == other.Kp && Kd == other.Kd; } + + bool operator !=(const BaumgarteCorrectorParametersTpl & other) const + { return !(*this == other); } + + // parameters + /// \brief Proportional corrector value. + Scalar Kp; + + /// \brief Damping corrector value. + Scalar Kd; + }; + + template struct RigidConstraintModelTpl; + template struct RigidConstraintDataTpl; + + template + struct traits< RigidConstraintModelTpl<_Scalar,_Options> > + { + typedef _Scalar Scalar; + }; + + template + struct traits< RigidConstraintDataTpl<_Scalar,_Options> > + { + typedef _Scalar Scalar; + }; + + /// + /// \brief Contact model structure containg all the info describing the rigid contact model + /// + template + struct RigidConstraintModelTpl + : NumericalBase< RigidConstraintModelTpl<_Scalar,_Options> > + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef _Scalar Scalar; + enum { Options = _Options }; + + typedef RigidConstraintModelTpl ContactModel; + typedef RigidConstraintDataTpl ContactData; + + typedef SE3Tpl SE3; + typedef MotionTpl Motion; + typedef ForceTpl Force; + typedef pinocchio::JointIndex JointIndex; + typedef BaumgarteCorrectorParametersTpl BaumgarteCorrectorParameters; + + /// \brief Name of the contact + std::string name; + + /// \brief Type of the contact. + ContactType type; + + /// \brief Index of the first joint in the model tree + JointIndex joint1_id; + + /// \brief Index of the second joint in the model tree + JointIndex joint2_id; + + /// \brief Relative placement with respect to the frame of joint1. + SE3 joint1_placement; + + /// \brief Relative placement with respect to the frame of joint2. + SE3 joint2_placement; + + /// \brief Reference frame where the constraint is expressed (LOCAL_WORLD_ALIGNED or LOCAL) + ReferenceFrame reference_frame; + + /// \brief Desired contact placement + SE3 desired_contact_placement; + + /// \brief Desired contact spatial velocity + Motion desired_contact_velocity; + + /// \brief Desired contact spatial acceleration + Motion desired_contact_acceleration; + + /// \brief Corrector parameters + BaumgarteCorrectorParameters corrector; + + /// \brief Default constructor. + RigidConstraintModelTpl() + : type(CONTACT_UNDEFINED) + , joint1_id(std::numeric_limits::max()) + , joint2_id(std::numeric_limits::max()) + , joint1_placement(SE3::Identity()) + , joint2_placement(SE3::Identity()) + , reference_frame(LOCAL) + , desired_contact_placement(SE3::Identity()) + , desired_contact_velocity(Motion::Zero()) + , desired_contact_acceleration(Motion::Zero()) + {} + + /// + /// \brief Contructor with from a given type, joint indexes and placements. + /// + /// \param[in] type Type of the contact. + /// \param[in] joint1_id Index of the joint 1 in the model tree. + /// \param[in] joint2_id Index of the joint 2 in the model tree. + /// \param[in] joint1_placement Placement of the constraint w.r.t the frame of joint1. + /// \param[in] joint2_placement Placement of the constraint w.r.t the frame of joint2. + /// \param[in] reference_frame Reference frame in which the constraints quantities are expressed. + /// + RigidConstraintModelTpl(const ContactType type, + const JointIndex joint1_id, + const SE3 & joint1_placement, + const JointIndex joint2_id, + const SE3 & joint2_placement, + const ReferenceFrame & reference_frame = LOCAL) + : type(type) + , joint1_id(joint1_id) + , joint2_id(joint2_id) + , joint1_placement(joint1_placement) + , joint2_placement(joint2_placement) + , reference_frame(reference_frame) + , desired_contact_placement(SE3::Identity()) + , desired_contact_velocity(Motion::Zero()) + , desired_contact_acceleration(Motion::Zero()) + {} + + /// + /// \brief Contructor with from a given type, joint1_id and placement. + /// + /// \param[in] type Type of the contact. + /// \param[in] joint1_id Index of the joint 1 in the model tree. + /// \param[in] joint1_placement Placement of the constraint w.r.t the frame of joint1. + /// \param[in] reference_frame Reference frame in which the constraints quantities are expressed. + /// + RigidConstraintModelTpl(const ContactType type, + const JointIndex joint1_id, + const SE3 & joint1_placement, + const ReferenceFrame & reference_frame = LOCAL) + : type(type) + , joint1_id(joint1_id) + , joint2_id(0) + , joint1_placement(joint1_placement) + , joint2_placement(SE3::Identity()) + , reference_frame(reference_frame) + , desired_contact_placement(SE3::Identity()) + , desired_contact_velocity(Motion::Zero()) + , desired_contact_acceleration(Motion::Zero()) + {} + + /// + /// \brief Contructor with from a given type and the joint ids. + /// + /// \param[in] type Type of the contact. + /// \param[in] joint1_id Index of the joint 1 in the model tree. + /// \param[in] joint2_id Index of the joint 2 in the model tree. + /// + RigidConstraintModelTpl(const ContactType type, + const JointIndex joint1_id, + const JointIndex joint2_id, + const ReferenceFrame & reference_frame = LOCAL) + : type(type) + , joint1_id(joint1_id) + , joint2_id(joint2_id) + , joint1_placement(SE3::Identity()) + , joint2_placement(SE3::Identity()) + , reference_frame(reference_frame) + , desired_contact_placement(SE3::Identity()) + , desired_contact_velocity(Motion::Zero()) + , desired_contact_acceleration(Motion::Zero()) + {} + + /// + /// \brief Contructor with from a given type and . + /// + /// \param[in] type Type of the contact. + /// \param[in] joint1_id Index of the joint 1 in the model tree. + /// + /// \remarks The second joint id (joint2_id) is set to be 0 (corresponding to the index of the universe). + /// + RigidConstraintModelTpl(const ContactType type, + const JointIndex joint1_id, + const ReferenceFrame & reference_frame = LOCAL) + : type(type) + , joint1_id(joint1_id) + , joint2_id(0) // set to be the Universe + , joint1_placement(SE3::Identity()) + , joint2_placement(SE3::Identity()) + , reference_frame(reference_frame) + , desired_contact_placement(SE3::Identity()) + , desired_contact_velocity(Motion::Zero()) + , desired_contact_acceleration(Motion::Zero()) + {} + + /// + /// \brief Comparison operator + /// + /// \param[in] other Other RigidConstraintModelTpl to compare with. + /// + /// \returns true if the two *this is equal to other (type, joint1_id and placement attributs must be the same). + /// + template + bool operator==(const RigidConstraintModelTpl & other) const + { + return + name == other.name + && type == other.type + && joint1_id == other.joint1_id + && joint2_id == other.joint2_id + && joint1_placement == other.joint1_placement + && joint2_placement == other.joint2_placement + && reference_frame == other.reference_frame + && corrector == other.corrector; + } + + /// + /// \brief Oposite of the comparison operator. + /// + /// \param[in] other Other RigidConstraintModelTpl to compare with. + /// + /// \returns false if the two *this is not equal to other (at least type, joint1_id or placement attributs is different). + /// + template + bool operator!=(const RigidConstraintModelTpl & other) const + { + return !(*this == other); + } + + int size() const + { + switch(type) + { + case CONTACT_3D: + return contact_dim::value; + case CONTACT_6D: + return contact_dim::value; + default: + return contact_dim::value; + } + return -1; + } + + /// \returns An expression of *this with the Scalar type casted to NewScalar. + template + RigidConstraintModelTpl cast() const + { + typedef RigidConstraintModelTpl ReturnType; + ReturnType res; + res.type = type; + res.joint1_id = joint1_id; + res.joint2_id = joint2_id; + res.joint1_placement = joint1_placement.template cast(); + res.joint2_placement = joint2_placement.template cast(); + res.reference_frame = reference_frame; + res.desired_contact_placement = desired_contact_placement.template cast(); + res.desired_contact_velocity = desired_contact_velocity.template cast(); + res.desired_contact_acceleration = desired_contact_acceleration.template cast(); + return res; + } + + }; + + /// + /// \brief Contact model structure containg all the info describing the rigid contact model + /// + template + struct RigidConstraintDataTpl + : NumericalBase< RigidConstraintDataTpl<_Scalar,_Options> > + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef _Scalar Scalar; + enum { Options = _Options }; + + typedef RigidConstraintModelTpl ContactModel; + typedef RigidConstraintDataTpl ContactData; + + typedef SE3Tpl SE3; + typedef MotionTpl Motion; + typedef ForceTpl Force; + + // data + + /// \brief Resulting contact forces + Force contact_force; + + /// \brief Placement of the constraint frame 1 with respect to the WORLD frame + SE3 oMc1; + + /// \brief Placement of the constraint frame 2 with respect to the WORLD frame + SE3 oMc2; + + /// \brief Relative displacement between the two frames + SE3 c1Mc2; + + /// \brief Current contact placement error + Motion contact_placement_error; + + /// \brief Current contact spatial velocity of the constraint 1 + Motion contact1_velocity; + + /// \brief Current contact spatial velocity of the constraint 2 + Motion contact2_velocity; + + /// \brief Current contact velocity error + Motion contact_velocity_error; + + /// \brief Current contact spatial acceleration + Motion contact_acceleration; + + /// \brief Contact spatial acceleration desired + Motion contact_acceleration_desired; + + /// \brief Current contact spatial error (due to the integration step). + Motion contact_acceleration_error; + + /// \brief Current contact drift acceleration (acceleration only due to the Coriolis and centrifugal effects) for the constraint frame 1. + Motion contact1_acceleration_drift; + + /// \brief Current contact drift acceleration (acceleration only due to the Coriolis and centrifugal effects) for the constraint frame 2. + Motion contact2_acceleration_drift; + + /// \brief Contact deviation from the reference acceleration (a.k.a the error) + Motion contact_acceleration_deviation; + + RigidConstraintDataTpl(const ContactModel & /*contact_model*/) + : contact_force(Force::Zero()) + , contact_placement_error(Motion::Zero()) + , contact1_velocity(Motion::Zero()) + , contact2_velocity(Motion::Zero()) + , contact_velocity_error(Motion::Zero()) + , contact_acceleration(Motion::Zero()) + , contact_acceleration_desired(Motion::Zero()) + , contact_acceleration_error(Motion::Zero()) + , contact1_acceleration_drift(Motion::Zero()) + , contact2_acceleration_drift(Motion::Zero()) + , contact_acceleration_deviation(Motion::Zero()) + {} + + bool operator==(const RigidConstraintDataTpl & other) const + { + return + contact_force == other.contact_force + && oMc1 == other.oMc1 + && oMc2 == other.oMc2 + && c1Mc2 == other.c1Mc2 + && contact_placement_error == other.contact_placement_error + && contact1_velocity == other.contact1_velocity + && contact2_velocity == other.contact2_velocity + && contact_velocity_error == other.contact_velocity_error + && contact_acceleration == other.contact_acceleration + && contact_acceleration_desired == other.contact_acceleration_desired + && contact_acceleration_error == other.contact_acceleration_error + && contact1_acceleration_drift == other.contact1_acceleration_drift + && contact2_acceleration_drift == other.contact2_acceleration_drift + && contact_acceleration_deviation == other.contact_acceleration_deviation + ; + } + + bool operator!=(const RigidConstraintDataTpl & other) const + { + return !(*this == other); + } + }; + +} + +#endif // ifndef __pinocchio_algorithm_contact_info_hpp__ diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp index b85f01725a..352ca458a8 100644 --- a/src/algorithm/crba.hpp +++ b/src/algorithm/crba.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2018 CNRS +// Copyright (c) 2015-2020 CNRS INRIA // #ifndef __pinocchio_crba_hpp__ @@ -11,29 +11,32 @@ namespace pinocchio { - /// - /// \brief Computes the upper triangular part of the joint space inertia matrix M by - /// using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008). - /// The result is accessible through data.M. - /// - /// \note You can easly get data.M symetric by copying the stricly upper trinangular part - /// in the stricly lower tringular part with - /// data.M.triangularView() = data.M.transpose().triangularView(); - /// - /// \tparam JointCollection Collection of Joint types. - /// \tparam ConfigVectorType Type of the joint configuration vector. - /// - /// \param[in] model The model structure of the rigid body system. - /// \param[in] data The data structure of the rigid body system. - /// \param[in] q The joint configuration vector (dim model.nq). - /// - /// \return The joint space inertia matrix with only the upper triangular part computed. - /// - template class JointCollectionTpl, typename ConfigVectorType> - inline const typename DataTpl::MatrixXs & - crba(const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q); + namespace deprecated + { + /// + /// \brief Computes the upper triangular part of the joint space inertia matrix M by + /// using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008). + /// The result is accessible through data.M. + /// + /// \note You can easly get data.M symetric by copying the stricly upper trinangular part + /// in the stricly lower tringular part with + /// data.M.triangularView() = data.M.transpose().triangularView(); + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] q The joint configuration vector (dim model.nq). + /// + /// \return The joint space inertia matrix with only the upper triangular part computed. + /// + template class JointCollectionTpl, typename ConfigVectorType> + inline const typename DataTpl::MatrixXs & + crba(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q); + } /// /// \brief Computes the upper triangular part of the joint space inertia matrix M by @@ -44,10 +47,13 @@ namespace pinocchio /// in the stricly lower tringular part with /// data.M.triangularView() = data.M.transpose().triangularView(); /// + /// \note This algorithm also takes into account the rotor inertia effects, by adding on the diagonal of the Joint Space Inertia Matrix their contributions. + /// This is done only for single DOF joint (e.g. Revolute, Prismatic, etc.). + /// /// \tparam JointCollection Collection of Joint types. /// \tparam ConfigVectorType Type of the joint configuration vector. /// - /// \note A direct outcome of this algorithm is the computation of the centroidal momemntum matrix (data.Ag) + /// \note A direct outcome of this algorithm is the computation of the centroidal momemntum matrix (data.Ag), a forward geometry /// and the joint jacobian matrix (data.J). /// /// \param[in] model The model structure of the rigid body system. @@ -58,9 +64,9 @@ namespace pinocchio /// template class JointCollectionTpl, typename ConfigVectorType> inline const typename DataTpl::MatrixXs & - crbaMinimal(const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q); + crba(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q); PINOCCHIO_DEFINE_ALGO_CHECKER(CRBA); diff --git a/src/algorithm/crba.hxx b/src/algorithm/crba.hxx index 2c8e6dbccb..cd422c04f4 100644 --- a/src/algorithm/crba.hxx +++ b/src/algorithm/crba.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2019 CNRS +// Copyright (c) 2015-2020 CNRS INRIA // #ifndef __pinocchio_crba_hxx__ @@ -14,6 +14,7 @@ namespace pinocchio { + template class JointCollectionTpl, typename ConfigVectorType> struct CrbaForwardStep : public fusion::JointUnaryVisitorBase< CrbaForwardStep > @@ -79,7 +80,7 @@ namespace pinocchio /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]) = jdata.S().transpose()*data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]); - + const JointIndex & parent = model.parents[i]; if(parent>0) { @@ -128,7 +129,7 @@ namespace pinocchio jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); - data.Ycrb[i] = model.inertias[i]; + data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); } }; @@ -145,7 +146,6 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, - JointDataBase & jdata, const Model & model, Data & data) { @@ -153,63 +153,66 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; const JointIndex & i = jmodel.id(); - /* F[1:6,i] = Y*S */ - jdata.U() = data.Ycrb[i] * jdata.S(); - ColsBlock jF = data.Ag.template middleCols(jmodel.idx_v()); - // = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv()); + // Centroidal momentum map + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock J_cols = jmodel.jointCols(data.J); + motionSet::inertiaAction(data.oYcrb[i],J_cols,Ag_cols); - forceSet::se3Action(data.oMi[i],jdata.U(),jF); - - /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ + // Joint Space Inertia Matrix data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() - = jmodel.jointCols(data.J).transpose()*data.Ag.middleCols(jmodel.idx_v(),data.nvSubtree[i]); + = J_cols.transpose()*data.Ag.middleCols(jmodel.idx_v(),data.nvSubtree[i]); const JointIndex & parent = model.parents[i]; - /* Yli += liXi Yi */ - data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); + data.oYcrb[parent] += data.oYcrb[i]; } }; - - template class JointCollectionTpl, typename ConfigVectorType> - inline const typename DataTpl::MatrixXs & - crba(const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q) + namespace deprecated { - assert(model.check(data) && "data is not consistent with model."); - PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); - - typedef typename ModelTpl::JointIndex JointIndex; - - typedef CrbaForwardStep Pass1; - for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) - { - Pass1::run(model.joints[i],data.joints[i], - typename Pass1::ArgsType(model,data,q.derived())); - } - - typedef CrbaBackwardStep Pass2; - for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) + template class JointCollectionTpl, typename ConfigVectorType> + inline const typename DataTpl::MatrixXs & + crba(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q) { - Pass2::run(model.joints[i],data.joints[i], - typename Pass2::ArgsType(model,data)); + assert(model.check(data) && "data is not consistent with model."); + PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); + + typedef typename ModelTpl::JointIndex JointIndex; + + typedef CrbaForwardStep Pass1; + for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) + { + Pass1::run(model.joints[i],data.joints[i], + typename Pass1::ArgsType(model,data,q.derived())); + } + + typedef CrbaBackwardStep Pass2; + for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) + { + Pass2::run(model.joints[i],data.joints[i], + typename Pass2::ArgsType(model,data)); + } + + // Add the armature contribution + data.M.diagonal() += model.armature; + + return data.M; } - - return data.M; } template class JointCollectionTpl, typename ConfigVectorType> inline const typename DataTpl::MatrixXs & - crbaMinimal(const ModelTpl & model, - DataTpl & data, - const Eigen::MatrixBase & q) + crba(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q) { assert(model.check(data) && "data is not consistent with model."); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); typedef typename ModelTpl::JointIndex JointIndex; + data.oYcrb[0].setZero(); typedef CrbaForwardStepMinimal Pass1; for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) { @@ -220,17 +223,20 @@ namespace pinocchio typedef CrbaBackwardStepMinimal Pass2; for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) { - Pass2::run(model.joints[i],data.joints[i], + Pass2::run(model.joints[i], typename Pass2::ArgsType(model,data)); } + // Add the armature contribution + data.M.diagonal() += model.armature; + // Retrieve the Centroidal Momemtum map typedef DataTpl Data; typedef typename Data::Force Force; typedef Eigen::Block Block3x; - data.mass[0] = data.Ycrb[0].mass(); - data.com[0] = data.Ycrb[0].lever(); + data.mass[0] = data.oYcrb[0].mass(); + data.com[0] = data.oYcrb[0].lever(); const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); diff --git a/src/algorithm/dynamics.hpp b/src/algorithm/dynamics.hpp index 196a220f97..17275fddf8 100644 --- a/src/algorithm/dynamics.hpp +++ b/src/algorithm/dynamics.hpp @@ -1,14 +1,14 @@ // -// Copyright (c) 2018 INRIA +// Copyright (c) 2019 INRIA // -#ifndef __pinocchio_dynamics_hpp__ -#define __pinocchio_dynamics_hpp__ +#ifndef __pinocchio_algorithm_dynamics_hpp__ +#define __pinocchio_algorithm_dynamics_hpp__ #include "pinocchio/macros.hpp" -PINOCCHIO_PRAGMA_DEPRECATED_HEADER(pinocchio/algorithm/dynamics.hpp,pinocchio/algorithm/contact-dynamics.hpp) +PINOCCHIO_PRAGMA_DEPRECATED_HEADER(pinocchio/algorithm/dynamics.hpp,pinocchio/algorithm/constrained-dynamics.hpp) -#include "pinocchio/algorithm/contact-dynamics.hpp" +#include "pinocchio/algorithm/constrained-dynamics.hpp" -#endif // ifndef __pinocchio_dynamics_hpp__ +#endif // ifndef __pinocchio_algorithm_dynamics_hpp__ diff --git a/src/algorithm/energy.hpp b/src/algorithm/energy.hpp index 41c0b62f0d..2740c56431 100644 --- a/src/algorithm/energy.hpp +++ b/src/algorithm/energy.hpp @@ -163,6 +163,48 @@ namespace pinocchio { else return computePotentialEnergy(model,data); } + + /// + /// \brief Computes the mechanical energy of the system stored in data.mechanical_energy. + /// The result is accessible through data.kinetic_energy. + /// + /// \tparam JointCollection Collection of Joint types. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// + /// \return The total mechanal energy of the system in [J]. + /// + template class JointCollectionTpl> + inline Scalar + computeMechanicalEnergy(const ModelTpl & model, + DataTpl & data); + + /// + /// \brief Computes the mechanical energy of the system stored in data.mechanical_energy. + /// The result is accessible through data.kinetic_energy. + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType Type of the joint velocity vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] q The joint configuration vector (dim model.nq). + /// \param[in] v The joint velocity vector (dim model.nv). + /// + /// \return The total mechanal energy of the system in [J]. + /// The fonctions also computes the data.kinetic_energy and data.potential_energy. + /// + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> + inline Scalar computeMechanicalEnergy(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v) + { + forwardKinematics(model,data,q,v); + return computeMechanicalEnergy(model,data); + } } #include "pinocchio/algorithm/energy.hxx" diff --git a/src/algorithm/energy.hxx b/src/algorithm/energy.hxx index 9c3b30ce5e..bd6e9a7729 100644 --- a/src/algorithm/energy.hxx +++ b/src/algorithm/energy.hxx @@ -5,12 +5,34 @@ #ifndef __pinocchio_algorithm_energy_hxx__ #define __pinocchio_algorithm_energy_hxx__ -#include "pinocchio/multibody/model.hpp" -#include "pinocchio/multibody/data.hpp" #include "pinocchio/algorithm/check.hpp" namespace pinocchio { + + template class JointCollectionTpl> + struct KineticEnergyAlgoForwardStep + : public fusion::JointUnaryVisitorBase< KineticEnergyAlgoForwardStep > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data) + { + const JointIndex & i = jmodel.id(); + data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); + data.kinetic_energy += (jmodel.jointVelocitySelector(model.armature).array() * jdata.joint_v().array().square()).sum(); + } + + }; template class JointCollectionTpl> inline Scalar @@ -23,10 +45,12 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; data.kinetic_energy = Scalar(0); - + typedef KineticEnergyAlgoForwardStep Pass; for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) - data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); - + { + Pass::run(model.joints[i],data.joints[i], + typename Pass::ArgsType(model,data)); + } data.kinetic_energy *= .5; return data.kinetic_energy; @@ -56,6 +80,37 @@ namespace pinocchio return data.potential_energy; } + + template class JointCollectionTpl> + inline Scalar + computeMechanicalEnergy(const ModelTpl & model, + DataTpl & data) + { + assert(model.check(data) && "data is not consistent with model."); + + typedef ModelTpl Model; + typedef DataTpl Data; + typedef typename Model::Motion Motion; + typedef typename Model::JointIndex JointIndex; + + data.kinetic_energy = Scalar(0); + data.potential_energy = Scalar(0); + + typename Data::Vector3 com_global; // tmp variable + const typename Motion::ConstLinearType & g = model.gravity.linear(); + typedef KineticEnergyAlgoForwardStep Pass; + for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i) + { + Pass::run(model.joints[i],data.joints[i], + typename Pass::ArgsType(model,data)); + com_global.noalias() = data.oMi[i].translation() + data.oMi[i].rotation() * model.inertias[i].lever(); + data.potential_energy -= model.inertias[i].mass() * com_global.dot(g); + } + data.kinetic_energy *= .5; + + data.mechanical_energy = data.kinetic_energy + data.potential_energy; + + return data.mechanical_energy; + } } #endif // __pinocchio_algorithm_energy_hxx__ - diff --git a/src/algorithm/frames-derivatives.hpp b/src/algorithm/frames-derivatives.hpp index 5b865addd7..256926809c 100644 --- a/src/algorithm/frames-derivatives.hpp +++ b/src/algorithm/frames-derivatives.hpp @@ -10,8 +10,36 @@ namespace pinocchio { + /** - * @brief Computes the partial derivatives of the frame velocity quantity with respect to q and v. + * @brief Computes the partial derivatives of the spatial velocity of a frame given by its relative placement, with respect to q and v. + * You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. + * + * @tparam JointCollection Collection of Joint types. + * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector. + * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] joint_id Index of the supporting joint + * @param[in] placement Placement of the Frame w.r.t. the joint frame. + * @param[in] rf Reference frame in which the velocity is expressed. + * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. + * @param[out] v_partial_dv Partial derivative of the frame spatial velociy w.r.t. \f$ v \f$. + * + */ + template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> + void + getFrameVelocityDerivatives(const ModelTpl & model, + const DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_partial_dq, + const Eigen::MatrixBase & v_partial_dv); + + /** + * @brief Computes the partial derivatives of the frame spatial velocity with respect to q and v. * You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. * * @tparam JointCollection Collection of Joint types. @@ -22,8 +50,8 @@ namespace pinocchio * @param[in] data Data associated to model * @param[in] frame_id Id of the operational Frame * @param[in] rf Reference frame in which the velocity is expressed. - * @param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$. - * @param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ \dot{q} \f$. + * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. + * @param[out] v_partial_dv Partial derivative of the frame spatial velociy w.r.t. \f$ v \f$. * */ template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> @@ -33,7 +61,54 @@ namespace pinocchio const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase & v_partial_dq, - const Eigen::MatrixBase & v_partial_dv); + const Eigen::MatrixBase & v_partial_dv) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid."); + typedef ModelTpl Model; + typedef DataTpl Data; + typedef typename Model::Frame Frame; + + const Frame & frame = model.frames[frame_id]; + typename Data::SE3 & oMframe = data.oMf[frame_id]; + oMframe = data.oMi[frame.parent] * frame.placement; // for backward compatibility + getFrameVelocityDerivatives(model,data,frame.parent,frame.placement,rf, + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dv)); + } + + /** + * @brief Computes the partial derivatives of the spatial acceleration of a frame given by its relative placement, with respect to q, v and a. + * You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. + * It is important to notice that a direct outcome (for free) of this algo is v_partial_dq and v_partial_dv which is equal to a_partial_da. + * + * @tparam JointCollection Collection of Joint types. + * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector. + * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector. + * @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector. + * @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] joint_id Index of the supporting joint + * @param[in] placement Placement of the Frame w.r.t. the joint frame. + * @param[in] rf Reference frame in which the velocity is expressed. + * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ q \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ v \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ \dot{v} \f$. + * + */ + template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4> + void + getFrameAccelerationDerivatives(const ModelTpl & model, + DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_partial_dq, + const Eigen::MatrixBase & a_partial_dq, + const Eigen::MatrixBase & a_partial_dv, + const Eigen::MatrixBase & a_partial_da); /** * @brief Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. @@ -50,10 +125,10 @@ namespace pinocchio * @param[in] data Data associated to model * @param[in] frame_id Id of the operational Frame * @param[in] rf Reference frame in which the velocity is expressed. - * @param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$. - * @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q \f$. - * @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{q} \f$. - * @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \ddot{q} \f$. + * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ q \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ v \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ \dot{v} \f$. * */ template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4> @@ -65,7 +140,69 @@ namespace pinocchio const Eigen::MatrixBase & v_partial_dq, const Eigen::MatrixBase & a_partial_dq, const Eigen::MatrixBase & a_partial_dv, - const Eigen::MatrixBase & a_partial_da); + const Eigen::MatrixBase & a_partial_da) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid."); + typedef ModelTpl Model; + typedef DataTpl Data; + typedef typename Model::Frame Frame; + + const Frame & frame = model.frames[frame_id]; + typename Data::SE3 & oMframe = data.oMf[frame_id]; + oMframe = data.oMi[frame.parent] * frame.placement; // for backward compatibility + getFrameAccelerationDerivatives(model,data,frame.parent,frame.placement,rf, + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,a_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dv), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_da)); + } + + /** + * @brief Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. + * You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. + * It is important to notice that a direct outcome (for free) of this algo is v_partial_dq and v_partial_dv which is equal to a_partial_da. + * + * @tparam JointCollection Collection of Joint types. + * @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector. + * @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector. + * @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector. + * @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector. + * @tparam Matrix6xOut5 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] joint_id Index of the supporting joint + * @param[in] placement Placement of the Frame w.r.t. the joint frame. + * @param[in] rf Reference frame in which the velocity is expressed. + * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. + * @param[out] v_partial_dv Partial derivative of the frame spatial velociy w.r.t. \f$ v \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ q \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ v \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ \dot{v} \f$. + * + */ + template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5> + void + getFrameAccelerationDerivatives(const ModelTpl & model, + DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_partial_dq, + const Eigen::MatrixBase & v_partial_dv, + const Eigen::MatrixBase & a_partial_dq, + const Eigen::MatrixBase & a_partial_dv, + const Eigen::MatrixBase & a_partial_da) + { + getFrameAccelerationDerivatives(model,data,joint_id,placement,rf, + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_dv), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5,a_partial_da)); + + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv) = a_partial_da; + } + /** * @brief Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. @@ -83,11 +220,11 @@ namespace pinocchio * @param[in] data Data associated to model * @param[in] frame_id Id of the operational Frame * @param[in] rf Reference frame in which the velocity is expressed. - * @param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$. - * @param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ \dot{q} \f$. - * @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q \f$. - * @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{q} \f$. - * @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \ddot{q} \f$. + * @param[out] v_partial_dq Partial derivative of the frame spatial velocity w.r.t. \f$ q \f$. + * @param[out] v_partial_dv Partial derivative of the frame spatial velociy w.r.t. \f$ v \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ q \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ v \f$. + * @param[out] a_partial_dq Partial derivative of the frame spatial acceleration w.r.t. \f$ \dot{v} \f$. * */ template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5> @@ -100,7 +237,23 @@ namespace pinocchio const Eigen::MatrixBase & v_partial_dv, const Eigen::MatrixBase & a_partial_dq, const Eigen::MatrixBase & a_partial_dv, - const Eigen::MatrixBase & a_partial_da); + const Eigen::MatrixBase & a_partial_da) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT((int)frame_id < model.nframes, "The frame_id is not valid."); + typedef ModelTpl Model; + typedef DataTpl Data; + typedef typename Model::Frame Frame; + + const Frame & frame = model.frames[frame_id]; + typename Data::SE3 & oMframe = data.oMf[frame_id]; + oMframe = data.oMi[frame.parent] * frame.placement; // for backward compatibility + getFrameAccelerationDerivatives(model,data,frame.parent,frame.placement,rf, + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_dv), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5,a_partial_da)); + } } #include "pinocchio/algorithm/frames-derivatives.hxx" diff --git a/src/algorithm/frames-derivatives.hxx b/src/algorithm/frames-derivatives.hxx index 5e884899bb..eb7800d503 100644 --- a/src/algorithm/frames-derivatives.hxx +++ b/src/algorithm/frames-derivatives.hxx @@ -14,46 +14,34 @@ namespace pinocchio template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> void getFrameVelocityDerivatives(const ModelTpl & model, - DataTpl & data, - const FrameIndex frame_id, + const DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, const ReferenceFrame rf, const Eigen::MatrixBase & v_partial_dq, const Eigen::MatrixBase & v_partial_dv) { - typedef ModelTpl Model; typedef DataTpl Data; typedef typename Data::Matrix6x Matrix6x; + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Matrix6x); EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Matrix6x); - PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); - PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv); - assert(model.check(data) && "data is not consistent with model."); - - PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames"); - typedef typename Model::Frame Frame; - typedef typename Data::Motion Motion; - const Frame & frame = model.frames[frame_id]; - const JointIndex joint_id = frame.parent; - Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq); Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv); getJointVelocityDerivatives(model,data,joint_id,rf, v_partial_dq_,v_partial_dv_); - // Update frame placement - typename Data::SE3 & oMframe = data.oMf[frame_id]; - oMframe = data.oMi[joint_id] * frame.placement; - typedef typename SizeDepType<1>::template ColsReturn::Type ColsBlockOut1; typedef MotionRef MotionOut1; typedef typename SizeDepType<1>::template ColsReturn::Type ColsBlockOut2; typedef MotionRef MotionOut2; Motion v_tmp; - const typename Data::SE3::Vector3 trans = data.oMi[joint_id].rotation() * frame.placement.translation(); + const typename SE3::Vector3 trans = data.oMi[joint_id].rotation() * placement.translation(); const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1; switch (rf) { @@ -75,9 +63,9 @@ namespace pinocchio for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id]) { v_tmp = v_partial_dq_.col(col_id); - MotionOut1(v_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp); + MotionOut1(v_partial_dq_.col(col_id)) = placement.actInv(v_tmp); v_tmp = v_partial_dv_.col(col_id); - MotionOut2(v_partial_dv_.col(col_id)) = frame.placement.actInv(v_tmp); + MotionOut2(v_partial_dv_.col(col_id)) = placement.actInv(v_tmp); } break; @@ -91,17 +79,19 @@ namespace pinocchio void getFrameAccelerationDerivatives(const ModelTpl & model, DataTpl & data, - const FrameIndex frame_id, + const JointIndex joint_id, + const SE3Tpl & placement, const ReferenceFrame rf, const Eigen::MatrixBase & v_partial_dq, const Eigen::MatrixBase & a_partial_dq, const Eigen::MatrixBase & a_partial_dv, const Eigen::MatrixBase & a_partial_da) { - typedef ModelTpl Model; typedef DataTpl Data; typedef typename Data::Matrix6x Matrix6x; + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Matrix6x); EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Matrix6x); @@ -114,12 +104,6 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv); assert(model.check(data) && "data is not consistent with model."); - PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames"); - typedef typename Model::Frame Frame; - typedef typename Data::Motion Motion; - const Frame & frame = model.frames[frame_id]; - const JointIndex joint_id = frame.parent; - Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq); Matrix6xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,a_partial_dq); Matrix6xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dv); @@ -128,10 +112,6 @@ namespace pinocchio getJointAccelerationDerivatives(model,data,joint_id,rf, v_partial_dq_,a_partial_dq_,a_partial_dv_,a_partial_da_); - // Update frame placement - typename Data::SE3 & oMframe = data.oMf[frame_id]; - oMframe = data.oMi[joint_id] * frame.placement; - typedef typename SizeDepType<1>::template ColsReturn::Type ColsBlockOut1; typedef MotionRef MotionOut1; typedef typename SizeDepType<1>::template ColsReturn::Type ColsBlockOut2; @@ -141,8 +121,6 @@ namespace pinocchio typedef typename SizeDepType<1>::template ColsReturn::Type ColsBlockOut4; typedef MotionRef MotionOut4; - Motion v_tmp; - const typename Data::SE3::Vector3 trans = data.oMi[joint_id].rotation() * frame.placement.translation(); const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1; switch (rf) { @@ -151,6 +129,8 @@ namespace pinocchio break; case LOCAL_WORLD_ALIGNED: + { + const typename SE3::Vector3 trans = data.oMi[joint_id].rotation() * placement.translation(); for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id]) { MotionOut1 m1(v_partial_dq_.col(col_id)); @@ -163,46 +143,27 @@ namespace pinocchio m4.linear() -= trans.cross(m4.angular()); } break; - + } case LOCAL: + { + Motion v_tmp; for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id]) { v_tmp = v_partial_dq_.col(col_id); - MotionOut1(v_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp); + MotionOut1(v_partial_dq_.col(col_id)) = placement.actInv(v_tmp); v_tmp = a_partial_dq_.col(col_id); - MotionOut2(a_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp); + MotionOut2(a_partial_dq_.col(col_id)) = placement.actInv(v_tmp); v_tmp = a_partial_dv_.col(col_id); - MotionOut3(a_partial_dv_.col(col_id)) = frame.placement.actInv(v_tmp); + MotionOut3(a_partial_dv_.col(col_id)) = placement.actInv(v_tmp); v_tmp = a_partial_da_.col(col_id); - MotionOut4(a_partial_da_.col(col_id)) = frame.placement.actInv(v_tmp); + MotionOut4(a_partial_da_.col(col_id)) = placement.actInv(v_tmp); } break; - + } default: break; } } - - template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5> - void getFrameAccelerationDerivatives(const ModelTpl & model, - DataTpl & data, - const FrameIndex frame_id, - const ReferenceFrame rf, - const Eigen::MatrixBase & v_partial_dq, - const Eigen::MatrixBase & v_partial_dv, - const Eigen::MatrixBase & a_partial_dq, - const Eigen::MatrixBase & a_partial_dv, - const Eigen::MatrixBase & a_partial_da) - { - getFrameAccelerationDerivatives(model,data, - frame_id,rf, - PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), - PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dq), - PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_dv), - PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5,a_partial_da)); - - PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv) = a_partial_da; - } } #endif // ifndef __pinocchio_algorithm_frames_derivatives_hxx__ diff --git a/src/algorithm/frames.hpp b/src/algorithm/frames.hpp index 56b6e3f531..0d0d2cceb5 100644 --- a/src/algorithm/frames.hpp +++ b/src/algorithm/frames.hpp @@ -80,6 +80,31 @@ namespace pinocchio } + /** + * @brief Returns the spatial velocity of the Frame expressed in the desired reference frame. + * You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] joint_id Id of the parent joint + * @param[in] placement frame placement with respect to the parent joint + * @param[in] rf Reference frame in which the velocity is expressed. + * + * @return The spatial velocity of the Frame expressed in the desired reference frame. + * + * @warning Fist or second order forwardKinematics should have been called first + */ + + template class JointCollectionTpl> + inline MotionTpl + getFrameVelocity(const ModelTpl & model, + const DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf = LOCAL); + + + /** * @brief Returns the spatial velocity of the Frame expressed in the desired reference frame. * You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure. @@ -93,12 +118,44 @@ namespace pinocchio * * @warning Fist or second order forwardKinematics should have been called first */ + template class JointCollectionTpl> inline MotionTpl getFrameVelocity(const ModelTpl & model, const DataTpl & data, const FrameIndex frame_id, - const ReferenceFrame rf = LOCAL); + const ReferenceFrame rf = LOCAL) + { + typedef ModelTpl Model; + const typename Model::Frame & frame = model.frames[frame_id]; + + return getFrameVelocity(model, data, frame.parent, frame.placement, rf); + } + + + /** + * @brief Returns the spatial acceleration of the Frame expressed in the desired reference frame. + * You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] joint_id Id of the parent joint + * @param[in] placement frame placement with respect to the parent joint + * @param[in] rf Reference frame in which the acceleration is expressed. + * + * @return The spatial acceleration of the Frame expressed in the desired reference frame. + * + * @warning Second order forwardKinematics should have been called first + */ + + template class JointCollectionTpl> + inline MotionTpl + getFrameAcceleration(const ModelTpl & model, + const DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf = LOCAL); + /** * @brief Returns the spatial acceleration of the Frame expressed in the desired reference frame. @@ -113,12 +170,44 @@ namespace pinocchio * * @warning Second order forwardKinematics should have been called first */ + template class JointCollectionTpl> inline MotionTpl getFrameAcceleration(const ModelTpl & model, const DataTpl & data, const FrameIndex frame_id, - const ReferenceFrame rf = LOCAL); + const ReferenceFrame rf = LOCAL) + { + typedef ModelTpl Model; + const typename Model::Frame & frame = model.frames[frame_id]; + return getFrameAcceleration(model, data, frame.parent, frame.placement, rf); + + } + + /** + * @brief Returns the "classical" acceleration of the Frame expressed in the desired reference frame. + * This is different from the "spatial" acceleration in that centrifugal effects are accounted for. + * You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] joint_id Id of the parent joint + * @param[in] placement frame placement with respect to the parent joint + * @param[in] rf Reference frame in which the acceleration is expressed. + * + * @return The classical acceleration of the Frame expressed in the desired reference frame. + * + * @warning Second order forwardKinematics should have been called first + */ + + template class JointCollectionTpl> + inline MotionTpl + getFrameClassicalAcceleration(const ModelTpl & model, + const DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf = LOCAL); + /** * @brief Returns the "classical" acceleration of the Frame expressed in the desired reference frame. @@ -134,30 +223,105 @@ namespace pinocchio * * @warning Second order forwardKinematics should have been called first */ + + template class JointCollectionTpl> inline MotionTpl getFrameClassicalAcceleration(const ModelTpl & model, const DataTpl & data, const FrameIndex frame_id, - const ReferenceFrame rf = LOCAL); + const ReferenceFrame rf = LOCAL) + { + typedef ModelTpl Model; + const typename Model::Frame & frame = model.frames[frame_id]; + return getFrameClassicalAcceleration(model, data, frame.parent, frame.placement, rf); + } + /** - * @brief Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or in the WORLD coordinate system, - * depending on the value of rf. - * You must first call pinocchio::computeJointJacobians followed by pinocchio::framesForwardKinematics to update placement values in data structure. + * @brief Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the WORLD coordinate system, depending on the value of reference_frame. + * You must first call pinocchio::computeJointJacobians. * - * @remark Similarly to pinocchio::getJointJacobian with LOCAL or WORLD parameters, if rf == LOCAL, this function returns the Jacobian of the frame expressed - * in the local coordinates of the frame, or if rl == WORLD, it returns the Jacobian expressed of the point coincident with the origin + * @remarks Similarly to pinocchio::getJointJacobian: + * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in the local coordinate system of the frame + * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame centered on the frame origin + * and expressed in a coordinate system aligned with the WORLD. + * - if rf == WORLD, this function returns the Jacobian of the frame expressed at the point coincident with the origin * and expressed in a coordinate system aligned with the WORLD. * * @tparam JointCollection Collection of Joint types. * @tparam Matrix6xLike Type of the matrix containing the joint Jacobian. * - * @param[in] model The kinematic model - * @param[in] data Data associated to model - * @param[in] frame_id Id of the operational Frame - * @param[in] rf Reference frame in which the Jacobian is expressed. - * @param[out] J The Jacobian of the Frame expressed in the coordinates Frame. + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] joint_id Index of the joint. + * @param[in] reference_frame Reference frame in which the Jacobian is expressed. + * @param[out] J The Jacobian of the Frame expressed in the reference_frame coordinate system. + * + * @warning The function pinocchio::computeJointJacobians should have been called first. + */ + template class JointCollectionTpl, typename Matrix6xLike> + void getFrameJacobian(const ModelTpl & model, + DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame reference_frame, + const Eigen::MatrixBase & J); + + /** + * @brief Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. + * You must first call pinocchio::computeJointJacobians. + * + * @remarks Similarly to pinocchio::getJointJacobian: + * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in the local coordinate system of the frame + * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame centered on the frame origin + * and expressed in a coordinate system aligned with the WORLD. + * - if rf == WORLD, this function returns the Jacobian of the frame expressed at the point coincident with the origin + * and expressed in a coordinate system aligned with the WORLD. + * + * @tparam JointCollection Collection of Joint types. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] joint_id Index of the joint. + * @param[in] reference_frame Reference frame in which the Jacobian is expressed. + * + * @warning The function pinocchio::computeJointJacobians should have been called first. + */ + template class JointCollectionTpl> + Eigen::Matrix + getFrameJacobian(const ModelTpl & model, + DataTpl & data, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame reference_frame) + { + typedef Eigen::Matrix Matrix6x; + Matrix6x res(Matrix6x::Zero(6,model.nv)); + + getFrameJacobian(model,data,joint_id,placement,reference_frame,res); + return res; + } + + /** + * @brief Returns the jacobian of the frame expressed either expressed in the local frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. + * You must first call pinocchio::computeJointJacobians. + * + * @remarks Similarly to pinocchio::getJointJacobian: + * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in the local coordinate system of the frame + * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame centered on the frame origin + * and expressed in a coordinate system aligned with the WORLD. + * - if rf == WORLD, this function returns the Jacobian of the frame expressed at the point coincident with the origin + * and expressed in a coordinate system aligned with the WORLD. + * + * @tparam JointCollection Collection of Joint types. + * @tparam Matrix6xLike Type of the matrix containing the joint Jacobian. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] frame_id Index of the frame + * @param[in] reference_frame Reference frame in which the Jacobian is expressed. + * @param[out] J The Jacobian of the Frame expressed in the coordinates Frame. * * @warning The function pinocchio::computeJointJacobians should have been called first. */ @@ -165,8 +329,54 @@ namespace pinocchio inline void getFrameJacobian(const ModelTpl & model, DataTpl & data, const FrameIndex frame_id, - const ReferenceFrame rf, - const Eigen::MatrixBase & J); + const ReferenceFrame reference_frame, + const Eigen::MatrixBase & J) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds."); + + typedef ModelTpl Model; + typedef typename Model::Frame Frame; + + const Frame & frame = model.frames[frame_id]; + data.oMf[frame_id] = data.oMi[frame.parent] * frame.placement; + + getFrameJacobian(model,data,frame.parent,frame.placement,reference_frame, + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)); + } + + /** + * @brief Returns the jacobian of the frame expressed either expressed in the local frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. + * You must first call pinocchio::computeJointJacobians. + * + * @remarks Similarly to pinocchio::getJointJacobian: + * - if rf == LOCAL, this function returns the Jacobian of the frame expressed in the local coordinate system of the frame + * - if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame centered on the frame origin + * and expressed in a coordinate system aligned with the WORLD. + * - if rf == WORLD, this function returns the Jacobian of the frame expressed at the point coincident with the origin + * and expressed in a coordinate system aligned with the WORLD. + * + * @tparam JointCollection Collection of Joint types. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] frame_id Index of the frame + * @param[in] reference_frame Reference frame in which the Jacobian is expressed. + * + * @warning The function pinocchio::computeJointJacobians should have been called first. + */ + template class JointCollectionTpl> + Eigen::Matrix + getFrameJacobian(const ModelTpl & model, + DataTpl & data, + const FrameIndex frame_id, + const ReferenceFrame reference_frame) + { + typedef Eigen::Matrix Matrix6x; + Matrix6x res(Matrix6x::Zero(6,model.nv)); + + getFrameJacobian(model,data,frame_id,reference_frame,res); + return res; + } /// /// \brief Computes the Jacobian of a specific Frame expressed in the desired reference_frame given as argument. @@ -242,7 +452,7 @@ namespace pinocchio } /// - /// \brief Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the WORLD frame (rf = WORLD) or in the LOCAL frame (rf = LOCAL) or in the LOCAL_WORLD_ALIGNED frame (rf = LOCAL_WORLD_ALIGNED). + /// \brief Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the WORLD frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the LOCAL frame (rf = LOCAL). /// /// \note This jacobian is extracted from data.dJ. You have to run pinocchio::computeJointJacobiansTimeVariation before calling it. /// diff --git a/src/algorithm/frames.hxx b/src/algorithm/frames.hxx index 2967fd197f..6c282c0219 100644 --- a/src/algorithm/frames.hxx +++ b/src/algorithm/frames.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2020 CNRS INRIA +// Copyright (c) 2015-2021 CNRS INRIA // #ifndef __pinocchio_algorithm_frames_hxx__ @@ -9,7 +9,7 @@ #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/check.hpp" -namespace pinocchio +namespace pinocchio { template class JointCollectionTpl> @@ -24,7 +24,7 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; // The following for loop starts by index 1 because the first frame is fixed - // and corresponds to the universe.s + // and corresponds to the universe. for(FrameIndex i=1; i < (FrameIndex) model.nframes; ++i) { const Frame & frame = model.frames[i]; @@ -65,7 +65,8 @@ namespace pinocchio inline MotionTpl getFrameVelocity(const ModelTpl & model, const DataTpl & data, - const FrameIndex frame_id, + const JointIndex joint_id, + const SE3Tpl & placement, const ReferenceFrame rf) { assert(model.check(data) && "data is not consistent with model."); @@ -73,17 +74,16 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::Motion Motion; - const typename Model::Frame & frame = model.frames[frame_id]; - const typename Model::SE3 & oMi = data.oMi[frame.parent]; - const typename Model::Motion & v = data.v[frame.parent]; + const typename Model::SE3 & oMi = data.oMi[joint_id]; + const typename Model::Motion & v = data.v[joint_id]; switch(rf) { case LOCAL: - return frame.placement.actInv(v); + return placement.actInv(v); case WORLD: return oMi.act(v); case LOCAL_WORLD_ALIGNED: - return Motion(oMi.rotation() * (v.linear() + v.angular().cross(frame.placement.translation())), + return Motion(oMi.rotation() * (v.linear() + v.angular().cross(placement.translation())), oMi.rotation() * v.angular()); default: throw std::invalid_argument("Bad reference frame."); @@ -94,7 +94,8 @@ namespace pinocchio inline MotionTpl getFrameAcceleration(const ModelTpl & model, const DataTpl & data, - const FrameIndex frame_id, + const JointIndex joint_id, + const SE3Tpl & placement, const ReferenceFrame rf) { assert(model.check(data) && "data is not consistent with model."); @@ -102,17 +103,16 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::Motion Motion; - const typename Model::Frame & frame = model.frames[frame_id]; - const typename Model::SE3 & oMi = data.oMi[frame.parent]; - const typename Model::Motion & a = data.a[frame.parent]; + const typename Model::SE3 & oMi = data.oMi[joint_id]; + const typename Model::Motion & a = data.a[joint_id]; switch(rf) { - case LOCAL: - return frame.placement.actInv(a); + case LOCAL: + return placement.actInv(a); case WORLD: return oMi.act(a); case LOCAL_WORLD_ALIGNED: - return Motion(oMi.rotation() * (a.linear() + a.angular().cross(frame.placement.translation())), + return Motion(oMi.rotation() * (a.linear() + a.angular().cross(placement.translation())), oMi.rotation() * a.angular()); default: throw std::invalid_argument("Bad reference frame."); @@ -123,45 +123,40 @@ namespace pinocchio inline MotionTpl getFrameClassicalAcceleration(const ModelTpl & model, const DataTpl & data, - const FrameIndex frame_id, + const JointIndex joint_id, + const SE3Tpl & placement, const ReferenceFrame rf) { assert(model.check(data) && "data is not consistent with model."); typedef MotionTpl Motion; - Motion vel = getFrameVelocity(model, data, frame_id, rf); - Motion acc = getFrameAcceleration(model, data, frame_id, rf); + Motion vel = getFrameVelocity(model, data, joint_id, placement, rf); + Motion acc = getFrameAcceleration(model, data, joint_id, placement, rf); acc.linear() += vel.angular().cross(vel.linear()); return acc; } - + template class JointCollectionTpl, typename Matrix6xLike> inline void getFrameJacobian(const ModelTpl & model, DataTpl & data, - const FrameIndex frame_id, - const ReferenceFrame rf, + const JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame reference_frame, const Eigen::MatrixBase & J) { PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id < (JointIndex)model.njoints, "The index of the Joint is outside the bounds."); assert(model.check(data) && "data is not consistent with model."); - typedef ModelTpl Model; - typedef typename Model::Frame Frame; typedef DataTpl Data; - typedef typename Model::JointIndex JointIndex; - - const Frame & frame = model.frames[frame_id]; - const JointIndex & joint_id = frame.parent; - - Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J); - typename Data::SE3 & oMframe = data.oMf[frame_id]; - oMframe = data.oMi[joint_id] * frame.placement; - details::translateJointJacobian(model,data,joint_id,rf,oMframe,data.J,J_); + const typename Data::SE3 oMframe = data.oMi[joint_id] * placement; + details::translateJointJacobian(model,data,joint_id,reference_frame,oMframe,data.J, + PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)); } template class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike> @@ -174,6 +169,7 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv, "The numbers of columns in the Jacobian matrix does not math the number of Dofs in the model."); typedef ModelTpl Model; typedef DataTpl Data; @@ -246,6 +242,7 @@ namespace pinocchio const Eigen::MatrixBase & dJ) { assert(model.check(data) && "data is not consistent with model."); + PINOCCHIO_CHECK_ARGUMENT_SIZE(dJ.cols(), model.nv, "The numbers of columns in the Jacobian matrix does not math the number of Dofs in the model."); typedef ModelTpl Model; typedef DataTpl Data; diff --git a/src/algorithm/fwd.hpp b/src/algorithm/fwd.hpp new file mode 100644 index 0000000000..6a65a55b6e --- /dev/null +++ b/src/algorithm/fwd.hpp @@ -0,0 +1,29 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_algorithm_fwd_hpp__ +#define __pinocchio_algorithm_fwd_hpp__ + +#include "pinocchio/fwd.hpp" + +namespace pinocchio +{ + template struct ProximalSettingsTpl; + typedef ProximalSettingsTpl ProximalSettings; + + namespace cholesky + { + template struct ContactCholeskyDecompositionTpl; + typedef ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; + } + + template struct RigidConstraintModelTpl; + template struct RigidConstraintDataTpl; + + typedef RigidConstraintModelTpl RigidConstraintModel; + typedef RigidConstraintDataTpl RigidConstraintData; +} + + +#endif // ifndef __pinocchio_algorithm_fwd_hpp__ diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx index b6d6b3b0f2..44d202c188 100644 --- a/src/algorithm/geometry.hxx +++ b/src/algorithm/geometry.hxx @@ -40,6 +40,12 @@ namespace pinocchio const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint; if (joint_id>0) geom_data.oMg[i] = (data.oMi[joint_id] * geom_model.geometryObjects[i].placement); else geom_data.oMg[i] = geom_model.geometryObjects[i].placement; +#ifdef PINOCCHIO_WITH_HPP_FCL +PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH +PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + geom_data.collisionObjects[i].setTransform( toFclTransform3f(geom_data.oMg[i]) ); +PINOCCHIO_COMPILER_DIAGNOSTIC_POP +#endif // PINOCCHIO_WITH_HPP_FCL } } #ifdef PINOCCHIO_WITH_HPP_FCL @@ -122,7 +128,10 @@ namespace pinocchio const Eigen::MatrixBase & q, const bool stopAtFirstCollision) { + assert(model.check(data) && "data is not consistent with model."); + updateGeometryPlacements(model, data, geom_model, geom_data, q); + return computeCollisions(geom_model,geom_data, stopAtFirstCollision); } @@ -186,7 +195,6 @@ namespace pinocchio } } } - return min_index; } diff --git a/src/algorithm/impulse-dynamics-derivatives.hpp b/src/algorithm/impulse-dynamics-derivatives.hpp new file mode 100644 index 0000000000..2e14440d59 --- /dev/null +++ b/src/algorithm/impulse-dynamics-derivatives.hpp @@ -0,0 +1,41 @@ +// +// Copyright (c) 2020 CNRS INRIA +// + +#ifndef __pinocchio_algorithm_impulse_dynamics_derivatives_hpp__ +#define __pinocchio_algorithm_impulse_dynamics_derivatives_hpp__ + +#include "pinocchio/algorithm/contact-info.hpp" + +namespace pinocchio +{ + + template class JointCollectionTpl, class ContactModelAllocator, class ContactDataAllocator, typename MatrixType1, typename MatrixType2, typename MatrixType3, typename MatrixType4> + inline void computeImpulseDynamicsDerivatives(const ModelTpl & model, + DataTpl & data, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_data, + const Scalar r_coeff, + const Scalar mu, + const Eigen::MatrixBase & dvimpulse_partial_dq, + const Eigen::MatrixBase & dvimpulse_partial_dv, + const Eigen::MatrixBase & impulse_partial_dq, + const Eigen::MatrixBase & impulse_partial_dv); + + template class JointCollectionTpl, class ContactModelAllocator, class ContactDataAllocator> + inline void computeImpulseDynamicsDerivatives(const ModelTpl & model, + DataTpl & data, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_data, + const Scalar r_coeff = Scalar(0.), + const Scalar mu = Scalar(0.)) + { + computeImpulseDynamicsDerivatives(model, data, contact_models, contact_data, r_coeff, mu, + data.ddq_dq, data.ddq_dv, data.dlambda_dq, data.dlambda_dv); + }; + +} // namespace pinocchio + +#include "pinocchio/algorithm/impulse-dynamics-derivatives.hxx" + +#endif // ifndef __pinocchio_algorithm_impulse_dynamics_derivatives_hpp__ diff --git a/src/algorithm/impulse-dynamics-derivatives.hxx b/src/algorithm/impulse-dynamics-derivatives.hxx new file mode 100644 index 0000000000..5e5627de50 --- /dev/null +++ b/src/algorithm/impulse-dynamics-derivatives.hxx @@ -0,0 +1,422 @@ +// +// Copyright (c) 2020 CNRS INRIA +// + +#ifndef __pinocchio_algorithm_impulse_dynamics_derivatives_hxx__ +#define __pinocchio_algorithm_impulse_dynamics_derivatives_hxx__ + +#include "pinocchio/algorithm/check.hpp" +#include "pinocchio/algorithm/rnea-derivatives.hpp" +#include "pinocchio/algorithm/kinematics-derivatives.hpp" +#include "pinocchio/algorithm/contact-cholesky.hpp" +#include "pinocchio/algorithm/constrained-dynamics-derivatives.hxx" + +namespace pinocchio +{ + + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2> + struct JointImpulseVelocityDerivativesBackwardStep3D + : public fusion::JointUnaryVisitorBase< JointImpulseVelocityDerivativesBackwardStep3D > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + const Model & model, + Data & data, + const typename Model::JointIndex & joint_id, + const ReferenceFrame & rf, + const Scalar & r_coeff, + const Eigen::MatrixBase & v_partial_dq, + const Eigen::MatrixBase & v_partial_dv) + { + typedef typename Model::JointIndex JointIndex; + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + + const SE3 & oMlast = data.oMi[joint_id]; + + typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + ColsBlock Jcols = jmodel.jointCols(data.J); + + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; + Matrix3xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1,v_partial_dq); + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut2; + Matrix3xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2,v_partial_dv); + + // dvec/dv + const int nv = jmodel.nv(); + Eigen::Matrix v_spatial_partial_dv_cols(6,nv); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + + motionSet::se3ActionInverse(oMlast,Jcols,v_spatial_partial_dv_cols); + v_partial_dv_cols = v_spatial_partial_dv_cols.template middleRows<3>(Motion::LINEAR); + + // dvec/dq + ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + +#define FOR_NV() for(Eigen::DenseIndex j = 0; j < nv; ++j) +#define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR) +#define GET_ANGULAR(vec6) vec6.template segment<3>(Motion::ANGULAR) + + const Scalar factor = Scalar(1) + r_coeff; + + if(parent > 0) + { + const Motion vtmp = oMlast.actInv(factor*data.ov[parent] + data.oa[parent]); + FOR_NV() + v_partial_dq_cols.col(j).noalias() + = vtmp.angular().cross(GET_LINEAR(v_spatial_partial_dv_cols.col(j))) + + vtmp.linear().cross(GET_ANGULAR(v_spatial_partial_dv_cols.col(j))); + } + else + v_partial_dq_cols.setZero(); + + if(rf == LOCAL_WORLD_ALIGNED) + { + const Motion vtmp = oMlast.actInv(factor*data.ov[joint_id] + data.oa[joint_id]); + FOR_NV() + v_partial_dq_cols.col(j) = oMlast.rotation() * (v_partial_dq_cols.col(j) + + GET_ANGULAR(v_spatial_partial_dv_cols.col(j)).cross(vtmp.linear())); + FOR_NV() + v_partial_dv_cols.col(j) = oMlast.rotation() * v_partial_dv_cols.col(j); + } + +#undef FOR_NV +#undef GET_LINEAR +#undef GET_ANGULAR + } + }; + + template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> + struct JointImpulseVelocityDerivativesBackwardStep6D + : public fusion::JointUnaryVisitorBase< JointImpulseVelocityDerivativesBackwardStep6D > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + const Model & model, + Data & data, + const typename Model::JointIndex & joint_id, + const ReferenceFrame & rf, + const Scalar & r_coeff, + const Eigen::MatrixBase & v_partial_dq, + const Eigen::MatrixBase & v_partial_dv) + { + typedef typename Model::JointIndex JointIndex; + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + Motion vtmp; // Temporary variables + + const SE3 & oMlast = data.oMi[joint_id]; + const Motion & v_last = data.ov[joint_id]; + const Motion & dv_last = data.oa[joint_id]; + + typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + ColsBlock Jcols = jmodel.jointCols(data.J); + + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; + Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq); + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut2; + Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv); + + // dvec/dv + ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + + switch(rf) + { + case LOCAL_WORLD_ALIGNED: + details::translateJointJacobian(oMlast,Jcols,v_partial_dv_cols); + break; + case LOCAL: + motionSet::se3ActionInverse(oMlast,Jcols,v_partial_dv_cols); + break; + default: + assert(false && "This must never happened"); + } + + // dvec/dq + ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + const Scalar factor = Scalar(1) + r_coeff; + switch(rf) + { + case LOCAL_WORLD_ALIGNED: + if(parent > 0) + { + vtmp = factor*(data.ov[parent] - v_last); + vtmp += data.oa[parent] - dv_last; + } + else + { + vtmp = -(factor*v_last + dv_last); + } + vtmp.linear() += vtmp.angular().cross(oMlast.translation()); + motionSet::motionAction(vtmp,v_partial_dv_cols,v_partial_dq_cols); + break; + case LOCAL: + if(parent > 0) + { + vtmp = oMlast.actInv(factor*data.ov[parent] + data.oa[parent]); + motionSet::motionAction(vtmp,v_partial_dv_cols,v_partial_dq_cols); + } + break; + default: + assert(false && "This must never happened"); + } + } + }; + + template class JointCollectionTpl, class ContactModelAllocator, class ContactDataAllocator, + typename MatrixType1, typename MatrixType2, typename MatrixType3, typename MatrixType4> + inline void computeImpulseDynamicsDerivatives(const ModelTpl & model, + DataTpl & data, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_data, + const Scalar r_coeff, + const Scalar mu, + const Eigen::MatrixBase & dvimpulse_partial_dq, + const Eigen::MatrixBase & dvimpulse_partial_dv, + const Eigen::MatrixBase & impulse_partial_dq, + const Eigen::MatrixBase & impulse_partial_dv) + { + const Eigen::DenseIndex nc = data.contact_chol.constraintDim(); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(contact_data.size() == contact_models.size(), + "contact_data and contact_models do not have the same size"); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(dvimpulse_partial_dq.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(dvimpulse_partial_dq.rows() == model.nv); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(dvimpulse_partial_dv.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(dvimpulse_partial_dv.rows() == model.nv); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(impulse_partial_dq.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(impulse_partial_dq.rows() == nc); + PINOCCHIO_CHECK_INPUT_ARGUMENT(impulse_partial_dv.cols() == model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(impulse_partial_dv.rows() == nc); + + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real((r_coeff >= Scalar(0)) && (r_coeff <= Scalar(1))) && "mu must be positive."); + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real(mu >= (Scalar(0))) && "mu must be positive."); +// PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real(model.gravity.angular().isZero()), +// "The gravity must be a pure force vector, no angular part"); + + assert(model.check(data) && "data is not consistent with model."); + data.dtau_dq.setZero(); + + typedef ModelTpl Model; + typedef DataTpl Data; + typedef typename Data::SE3 SE3; + typedef typename Data::Force Force; + + typedef RigidConstraintModelTpl RigidConstraintModel; + typedef RigidConstraintDataTpl RigidConstraintData; + + typedef typename ModelTpl::JointIndex JointIndex; + + typedef ComputeConstraintDynamicsDerivativesForwardStep Pass1; + for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) + { + Pass1::run(model.joints[i],data.joints[i], + typename Pass1::ArgsType(model,data)); + } + + // Add the contribution of the impulse. TODO: this should be done at the contact model level. + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_data[k]; + const SE3 & oMc = cdata.oMc1; + const JointIndex joint1_id = cmodel.joint1_id; + Force & of = data.of[joint1_id]; + + switch(cmodel.reference_frame) + { + case LOCAL: + { + of -= oMc.act(cdata.contact_force); + break; + } + case LOCAL_WORLD_ALIGNED: + { + of -= cdata.contact_force; + of.angular().noalias() -= oMc.translation().cross(cdata.contact_force.linear()); + break; + } + default: + assert(false && "Should never happened"); + break; + } + } + + typedef ComputeContactDynamicDerivativesBackwardStep Pass2; + for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) + { + Pass2::run(model.joints[i], + typename Pass2::ArgsType(model,data)); + } + + Eigen::DenseIndex current_row_sol_id = 0; + typedef typename SizeDepType<3>::template RowsReturn::Type Rows3Block; + typedef typename SizeDepType<6>::template RowsReturn::Type Rows6Block; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + + const typename Model::JointIndex joint1_id = cmodel.joint1_id; + + switch(cmodel.type) + { + case CONTACT_6D: + { + typedef JointImpulseVelocityDerivativesBackwardStep6D Pass3; + Rows6Block contact_dvc_dq = SizeDepType<6>::middleRows(data.dvc_dq, + current_row_sol_id); + Rows6Block contact_dvc_dv = SizeDepType<6>::middleRows(data.dac_da, + current_row_sol_id); + for(JointIndex i = joint1_id; i > 0; i = model.parents[i]) + { + Pass3::run(model.joints[i], + typename Pass3::ArgsType(model,data, + joint1_id, + cmodel.reference_frame, + r_coeff, + PINOCCHIO_EIGEN_CONST_CAST(Rows6Block,contact_dvc_dq), + PINOCCHIO_EIGEN_CONST_CAST(Rows6Block,contact_dvc_dv))); + } + break; + } + case CONTACT_3D: + { + typedef JointImpulseVelocityDerivativesBackwardStep3D Pass3; + Rows3Block contact_dvc_dq = SizeDepType<3>::middleRows(data.dvc_dq, + current_row_sol_id); + Rows3Block contact_dvc_dv = SizeDepType<3>::middleRows(data.dac_da, + current_row_sol_id); + for(JointIndex i = joint1_id; i > 0; i = model.parents[i]) + { + Pass3::run(model.joints[i], + typename Pass3::ArgsType(model,data, + joint1_id, + cmodel.reference_frame, + r_coeff, + contact_dvc_dq, + contact_dvc_dv)); + } + break; + } + default: + assert(false && "must never happen"); + break; + } + current_row_sol_id += cmodel.size(); + } + + data.contact_chol.getOperationalSpaceInertiaMatrix(data.osim); + data.contact_chol.getInverseMassMatrix(data.Minv); + + //Temporary: dlambda_dtau stores J*Minv + typename Data::MatrixXs & JMinv = data.dlambda_dtau; + typename Data::MatrixXs & Jc = data.dac_da; + + JMinv.noalias() = Jc * data.Minv; + data.dvc_dq.noalias() -= JMinv * data.dtau_dq; + + MatrixType3 & dic_dq = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,impulse_partial_dq); + dic_dq.noalias() = -data.osim * data.dvc_dq; //OUTPUT + + //TODO: Implem sparse. + data.dtau_dq.noalias() -= Jc.transpose()*impulse_partial_dq; + + PINOCCHIO_EIGEN_CONST_CAST(MatrixType4,impulse_partial_dv).noalias() = -(Scalar(1) + r_coeff)*data.osim*Jc;; //OUTPUT + + PINOCCHIO_EIGEN_CONST_CAST(MatrixType1,dvimpulse_partial_dq).noalias() = -data.Minv*data.dtau_dq; //OUTPUT + PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,dvimpulse_partial_dv).noalias() = JMinv.transpose()*impulse_partial_dv; //OUTPUT + + //update in world frame a(df/dt) = d(af)/dt + av X af + + current_row_sol_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_data[k]; + const typename Model::JointIndex joint1_id = cmodel.joint1_id; + const int colRef = nv(model.joints[joint1_id])+idx_v(model.joints[joint1_id])-1; + switch(cmodel.reference_frame) + { + case LOCAL: + break; + case LOCAL_WORLD_ALIGNED: + { + const Force & of = cdata.contact_force; + switch(cmodel.type) + { + case CONTACT_6D: + { + Rows6Block contact_dvc_dv = SizeDepType<6>::middleRows(data.dac_da,current_row_sol_id); + Rows6Block contact_dic_dq = SizeDepType<6>::middleRows(dic_dq, current_row_sol_id); + for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j]) + { + typedef typename Rows6Block::ColXpr ColType; + typedef typename Rows6Block::ColXpr ColTypeOut; + MotionRef min(contact_dvc_dv.col(j)); + ForceRef fout(contact_dic_dq.col(j)); + fout.linear().noalias() += min.angular().cross(of.linear()); + fout.angular().noalias() += min.angular().cross(of.angular()); + } + break; + } + case CONTACT_3D: + { + Rows3Block contact_dic_dq = SizeDepType<3>::middleRows(dic_dq, current_row_sol_id); + for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j]) + { + typedef typename Data::Matrix6x::ColXpr ColType; + MotionRef min(data.J.col(j)); + contact_dic_dq.col(j).noalias() += min.angular().cross(of.linear()); + } + break; + } + default: + assert(false && "must never happen"); + break; + } + break; + } + default: + assert(false && "must never happen"); + break; + } + current_row_sol_id += cmodel.size(); + } + + } +} // namespace pinocchio + +#endif // ifndef __pinocchio_algorithm_impulse_dynamics_derivatives_hxx__ diff --git a/src/algorithm/impulse-dynamics.hpp b/src/algorithm/impulse-dynamics.hpp new file mode 100644 index 0000000000..17b8972a5f --- /dev/null +++ b/src/algorithm/impulse-dynamics.hpp @@ -0,0 +1,57 @@ +// +// Copyright (c) 2020 CNRS INRIA +// + +#ifndef __pinocchio_algorithm_impulse_dynamics_hpp__ +#define __pinocchio_algorithm_impulse_dynamics_hpp__ + +#include "pinocchio/algorithm/constrained-dynamics.hpp" +#include "pinocchio/algorithm/contact-info.hpp" + +#include "pinocchio/algorithm/proximal.hpp" + +namespace pinocchio +{ + + /// + /// \brief Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called. + /// \note It computes the following problem:
+ ///
\f$ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ + /// \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \f$

+ /// where \f$ \dot{q}^{-} \f$ is the generalized velocity before impact, + /// \f$ M \f$ is the joint space mass matrix, \f$ J \f$ the constraint Jacobian and \f$ \epsilon \f$ is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact). + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam ConfigVectorType Type of the joint configuration vector. + /// \tparam TangentVectorType1 Type of the joint velocity vector. + /// \tparam Allocator Allocator class for the std::vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] q The joint configuration (size model.nq). + /// \param[in] v_before The joint velocity (size model.nv). + /// \param[in] contact_models Vector of contact information related to the problem. + /// \param[in] contact_datas Vector of contact datas related to the contact models. + /// \param[in] r_coeff coefficient of restitution: must be in [0.,1.] + /// \param[in] mu Damping factor for cholesky decomposition. Set to zero if constraints are full rank. + /// + /// \note A hint: a typical value for mu is 1e-12 when two contact constraints are redundant. + /// + /// \return A reference to the joint velocities stored in data.dq_after. The Lagrange Multipliers linked to the contact forces are available throw data.impulse_c vector. + /// + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, class ContactModelAllocator, class ContactDataAllocator> + inline const typename DataTpl::TangentVectorType & + impulseDynamics(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v_before, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_datas, + const Scalar r_coeff = Scalar(0.), + const Scalar mu = Scalar(0.)); + +} // namespace pinocchio + +#include "pinocchio/algorithm/impulse-dynamics.hxx" + +#endif // ifndef __pinocchio_algorithm_impulse_dynamics_hpp__ diff --git a/src/algorithm/impulse-dynamics.hxx b/src/algorithm/impulse-dynamics.hxx new file mode 100644 index 0000000000..313c0d9380 --- /dev/null +++ b/src/algorithm/impulse-dynamics.hxx @@ -0,0 +1,179 @@ +// +// Copyright (c) 2020 CNRS INRIA +// + +#ifndef __pinocchio_algorithm_impulse_dynamics_hxx__ +#define __pinocchio_algorithm_impulse_dynamics_hxx__ + +#include "pinocchio/algorithm/check.hpp" +#include "pinocchio/algorithm/constrained-dynamics.hxx" +#include + +namespace pinocchio +{ + + template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, class ContactModelAllocator, class ContactDataAllocator> + inline const typename DataTpl::TangentVectorType & + impulseDynamics(const ModelTpl & model, + DataTpl & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v_before, + const std::vector,ContactModelAllocator> & contact_models, + std::vector,ContactDataAllocator> & contact_datas, + const Scalar r_coeff, + const Scalar mu) + { + assert(model.check(data) && "data is not consistent with model."); + PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, + "The joint configuration vector is not of right size"); + PINOCCHIO_CHECK_INPUT_ARGUMENT(v_before.size() == model.nv, + "The joint velocity vector is not of right size"); + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real(mu >= Scalar(0)), + "mu has to be positive"); + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real((r_coeff >= Scalar(0)) &&(r_coeff <= Scalar(1))), + "r_coeff has to be in [0,1]"); + PINOCCHIO_CHECK_INPUT_ARGUMENT(contact_models.size() == contact_datas.size(), + "The contact models and data do not have the same vector size."); + + typedef DataTpl Data; + + typedef RigidConstraintModelTpl RigidConstraintModel; + typedef RigidConstraintDataTpl RigidConstraintData; + + typename Data::TangentVectorType & dq_after = data.dq_after; + typename Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + typename Data::VectorXs & primal_dual_contact_solution = data.primal_dual_contact_solution; + + data.oYcrb[0].setZero(); + typedef ContactAndImpulseDynamicsForwardStep Pass1; + for(JointIndex i=1;i<(JointIndex) model.njoints;++i) + { + Pass1::run(model.joints[i],data.joints[i], + typename Pass1::ArgsType(model,data,q.derived(),v_before.derived())); + } + + typedef ContactAndImpulseDynamicsBackwardStep Pass2; + for(JointIndex i=(JointIndex)(model.njoints-1);i>0;--i) + { + Pass2::run(model.joints[i], + typename Pass2::ArgsType(model,data)); + } + + data.M.diagonal() += model.armature; + contact_chol.compute(model,data,contact_models,contact_datas,mu); + + //Centroidal computations + typedef typename Data::Force Force; + typedef Eigen::Block Block3x; + data.com[0] = data.oYcrb[0].lever(); + + data.Ag = data.dFda; + const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); + Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); + for(long i = 0; i(current_row_sol_id); + impulse.angular().setZero(); + break; + } + case CONTACT_6D: + { + typedef typename Data::VectorXs::template FixedSegmentReturnType<6>::Type Segment6d; + const ForceRef i_sol(primal_dual_contact_solution.template segment<6>(current_row_sol_id)); + impulse = -i_sol; + break; + } + default: + assert(false && "must never happened"); + break; + } + + current_row_sol_id += contact_dim; + } + + return dq_after; + } + +} // namespace pinocchio + +#endif // ifndef __pinocchio_algorithm_impulse_dynamics_hxx__ diff --git a/src/algorithm/jacobian.hpp b/src/algorithm/jacobian.hpp index d3b58560a7..5fd0f888fd 100644 --- a/src/algorithm/jacobian.hpp +++ b/src/algorithm/jacobian.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // -#ifndef __pinocchio_jacobian_hpp__ -#define __pinocchio_jacobian_hpp__ +#ifndef __pinocchio_algorithm_jacobian_hpp__ +#define __pinocchio_algorithm_jacobian_hpp__ #include "pinocchio/multibody/fwd.hpp" #include "pinocchio/multibody/model.hpp" @@ -51,24 +51,48 @@ namespace pinocchio DataTpl & data); /// - /// \brief Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or in the local frame (rf = LOCAL) of the joint. + /// \brief Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame, in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the local frame (rf = LOCAL) of the joint. /// \note This jacobian is extracted from data.J. You have to run pinocchio::computeJointJacobians before calling it. /// /// \tparam JointCollection Collection of Joint types. /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. /// - /// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system. /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. - /// \param[in] jointId The id of the joint. + /// \param[in] joint_id The id of the joint. + /// \param[in] reference_frame Reference frame in which the result is expressed. /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.fill(0.). /// template class JointCollectionTpl, typename Matrix6Like> inline void getJointJacobian(const ModelTpl & model, const DataTpl & data, - const typename ModelTpl::JointIndex jointId, - const ReferenceFrame rf, + const JointIndex joint_id, + const ReferenceFrame reference_frame, const Eigen::MatrixBase & J); + /// + /// \brief Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame, in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the local frame (rf = LOCAL) of the joint. + /// \note This jacobian is extracted from data.J. You have to run pinocchio::computeJointJacobians before calling it. + /// + /// \tparam JointCollection Collection of Joint types. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] joint_id The index of the joint. + /// \param[in] reference_frame Reference frame in which the result is expressed. + /// + template class JointCollectionTpl> + Eigen::Matrix + getJointJacobian(const ModelTpl & model, + const DataTpl & data, + const JointIndex joint_id, + const ReferenceFrame reference_frame) + { + typedef Eigen::Matrix ReturnType; + ReturnType res(ReturnType::Zero(6,model.nv)); + + getJointJacobian(model,data,joint_id,reference_frame,res); + return res; + } /// /// \brief Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store the result in the input argument J. @@ -80,7 +104,7 @@ namespace pinocchio /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] q The joint configuration vector (dim model.nq). - /// \param[in] jointId The id of the joint refering to model.joints[jointId]. + /// \param[in] joint_id The id of the joint refering to model.joints[jointId]. /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero(). /// /// \return The Jacobian of the specific joint frame expressed in the local frame of the joint (matrix 6 x model.nv). @@ -94,7 +118,7 @@ namespace pinocchio inline void computeJointJacobian(const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & q, - const JointIndex jointId, + const JointIndex joint_id, const Eigen::MatrixBase & J); /// @@ -136,23 +160,23 @@ namespace pinocchio const Eigen::MatrixBase & v); /// - /// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD) or in the local frame (rf = LOCAL) of the joint. + /// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the local frame (rf = LOCAL) of the joint. /// \note This jacobian is extracted from data.dJ. You have to run pinocchio::computeJointJacobiansTimeVariation before calling it. /// /// \tparam JointCollection Collection of Joint types. /// \tparam Matrix6xLike Type of the matrix containing the joint Jacobian. /// - /// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system. - /// \param[in] model The model structure of the rigid body system. - /// \param[in] data The data structure of the rigid body system. - /// \param[in] jointId The id of the joint. + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] joint_id The id of the joint. + /// \param[in] reference_frame Reference frame in which the result is expressed. /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.). /// template class JointCollectionTpl, typename Matrix6Like> inline void getJointJacobianTimeVariation(const ModelTpl & model, const DataTpl & data, - const JointIndex jointId, - const ReferenceFrame rf, + const JointIndex joint_id, + const ReferenceFrame reference_frame, const Eigen::MatrixBase & dJ); } // namespace pinocchio @@ -163,4 +187,4 @@ namespace pinocchio #include "pinocchio/algorithm/jacobian.hxx" -#endif // ifndef __pinocchio_jacobian_hpp__ +#endif // ifndef __pinocchio_algorithm_jacobian_hpp__ diff --git a/src/algorithm/jacobian.hxx b/src/algorithm/jacobian.hxx index 1d05d62b81..2f8d2abe19 100644 --- a/src/algorithm/jacobian.hxx +++ b/src/algorithm/jacobian.hxx @@ -2,8 +2,8 @@ // Copyright (c) 2015-2020 CNRS INRIA // -#ifndef __pinocchio_jacobian_hxx__ -#define __pinocchio_jacobian_hxx__ +#ifndef __pinocchio_algorithm_jacobian_hxx__ +#define __pinocchio_algorithm_jacobian_hxx__ #include "pinocchio/multibody/visitor.hpp" #include "pinocchio/algorithm/check.hpp" @@ -236,13 +236,13 @@ namespace pinocchio template class JointCollectionTpl, typename Matrix6xLike> inline void getJointJacobian(const ModelTpl & model, const DataTpl & data, - const typename ModelTpl::JointIndex jointId, - const ReferenceFrame rf, + const JointIndex joint_id, + const ReferenceFrame reference_frame, const Eigen::MatrixBase & J) { assert(model.check(data) && "data is not consistent with model."); - details::translateJointJacobian(model,data,jointId,rf, + details::translateJointJacobian(model,data,joint_id,reference_frame, data.J,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J)); } @@ -402,4 +402,4 @@ namespace pinocchio /// @endcond -#endif // ifndef __pinocchio_jacobian_hxx__ +#endif // ifndef __pinocchio_algorithm_jacobian_hxx__ diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp index 202bc41ad4..f8e0006ea9 100644 --- a/src/algorithm/joint-configuration.hpp +++ b/src/algorithm/joint-configuration.hpp @@ -350,7 +350,6 @@ namespace pinocchio * * @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, * to the tangent space at \f$ q \f$. - * It performs the product with the Jacobian of integrate by exploiting at best the sparsity of the underlying operations. * In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between * \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector. * A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$. @@ -379,7 +378,6 @@ namespace pinocchio * * @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, * to the tangent space at \f$ q \f$. - * It performs the product with the Jacobian of integrate by exploiting at best the sparsity of the underlying operations. * In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between * \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector. * A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$. diff --git a/src/algorithm/kinematics-derivatives.hpp b/src/algorithm/kinematics-derivatives.hpp index 149ad9ec9f..e36ca44b69 100644 --- a/src/algorithm/kinematics-derivatives.hpp +++ b/src/algorithm/kinematics-derivatives.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2017-2019 CNRS INRIA +// Copyright (c) 2017-2020 CNRS INRIA // #ifndef __pinocchio_algorithm_kinematics_derivatives_hpp__ @@ -50,7 +50,7 @@ namespace pinocchio /// \param[in] data The data structure of the rigid body system. /// \param[in] rf Reference frame in which the Jacobian is expressed. /// \param[out] v_partial_dq Partial derivative of the joint velociy w.r.t. \f$ q \f$. - /// \param[out] v_partial_dv Partial derivative of the joint velociy w.r.t. \f$ \dot{q} \f$. + /// \param[out] v_partial_dv Partial derivative of the joint velociy w.r.t. \f$ v \f$. /// template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> inline void getJointVelocityDerivatives(const ModelTpl & model, @@ -78,8 +78,8 @@ namespace pinocchio /// \param[in] rf Reference frame in which the Jacobian is expressed. /// \param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$. /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q \f$. - /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{q} \f$. - /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \ddot{q} \f$. + /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ v \f$. + /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{v} \f$. /// template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4> inline void getJointAccelerationDerivatives(const ModelTpl & model, @@ -109,10 +109,10 @@ namespace pinocchio /// \param[in] jointId Index of the joint in model. /// \param[in] rf Reference frame in which the Jacobian is expressed. /// \param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$. - /// \param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ \dot{q} \f$. + /// \param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ v \f$. /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q \f$. - /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{q} \f$. - /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \ddot{q} \f$. + /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ v \f$. + /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{v} \f$. /// template class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5> inline void getJointAccelerationDerivatives(const ModelTpl & model, @@ -125,6 +125,99 @@ namespace pinocchio const Eigen::MatrixBase & a_partial_dv, const Eigen::MatrixBase & a_partial_da); + /// + /// \brief Computes the partial derivatives of the velocity of a point given by its placement information w.r.t. the joint frame. + /// You must first call computForwardKinematicsDerivatives before calling this function. + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector. + /// \tparam Matrix3xOut2 Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint velocity vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] joint_id Index of the joint in model. + /// \param[in] placement Relative placement of the point w.r.t. the joint frame. + /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or LOCAL_WORLD_ALIGNED). + /// \param[out] v_point_partial_dq Partial derivative of the point velocity w.r.t. \f$ q \f$. + /// \param[out] v_point_partial_dv Partial derivative of the point velociy w.r.t. \f$ v \f$. + /// + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2, typename Matrix3xOut3, typename Matrix3xOut4> + inline void getPointVelocityDerivatives(const ModelTpl & model, + const DataTpl & data, + const Model::JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_point_partial_dq, + const Eigen::MatrixBase & v_point_partial_dv); + + /// + /// \brief Computes the partial derivatives of the classic acceleration of a point given by its placement information w.r.t. the joint frame. + /// You must first call computForwardKinematicsDerivatives before calling this function. + /// It is important to notice that a direct outcome (for free) of this algo is v_point_partial_dq. + /// v_point_partial_dv is not computed it is equal to a_point_partial_da. + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector. + /// \tparam Matrix3xOut2 Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint configuration vector. + /// \tparam Matrix3xOut3 Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint velocity vector. + /// \tparam Matrix3xOut4 Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint acceleration vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] joint_id Index of the joint in model. + /// \param[in] placement Relative placement of the point w.r.t. the joint frame. + /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or LOCAL_WORLD_ALIGNED). + /// \param[out] v_point_partial_dq Partial derivative of the point velocity w.r.t. \f$ q \f$. + /// \param[out] a_point_partial_dq Partial derivative of the point classic acceleration w.r.t. \f$ q \f$. + /// \param[out] a_point_partial_dv Partial derivative of the point classic acceleration w.r.t. \f$ v \f$. + /// \param[out] a_point_partial_da Partial derivative of the point classic acceleration w.r.t. \f$ \dot{v} \f$. + /// + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2, typename Matrix3xOut3, typename Matrix3xOut4> + inline void getPointClassicAccelerationDerivatives(const ModelTpl & model, + const DataTpl & data, + const Model::JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_point_partial_dq, + const Eigen::MatrixBase & a_point_partial_dq, + const Eigen::MatrixBase & a_point_partial_dv, + const Eigen::MatrixBase & a_point_partial_da); + + /// + /// \brief Computes the partial derivaties of the classic acceleration of a point given by its placement information w.r.t. to the joint frame. + /// You must first call computForwardKinematicsDerivatives before calling this function. + /// It is important to notice that a direct outcome (for free) of this algo is v_point_partial_dq and v_point_partial_dv.. + /// + /// \tparam JointCollection Collection of Joint types. + /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector. + /// \tparam Matrix3xOut2 Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint velocity vector. + /// \tparam Matrix3xOut3 Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint configuration vector. + /// \tparam Matrix3xOut4 Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint velocity vector. + /// \tparam Matrix3xOut5 Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint acceleration vector. + /// + /// \param[in] model The model structure of the rigid body system. + /// \param[in] data The data structure of the rigid body system. + /// \param[in] joint_id Index of the joint in model. + /// \param[in] placement Relative placement of the point w.r.t. the joint frame. + /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or LOCAL_WORLD_ALIGNED). + /// \param[out] v_point_partial_dq Partial derivative of the point velocity w.r.t. \f$ q \f$. + /// \param[out] v_point_partial_dv Partial derivative of the point velociy w.r.t. \f$ v \f$. + /// \param[out] a_point_partial_dq Partial derivative of the point classic acceleration w.r.t. \f$ q \f$. + /// \param[out] a_point_partial_dv Partial derivative of the point classic acceleration w.r.t. \f$ v \f$. + /// \param[out] a_point_partial_da Partial derivative of the point classic acceleration w.r.t. \f$ \dot{v} \f$. + /// + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2, typename Matrix3xOut3, typename Matrix3xOut4, typename Matrix3xOut5> + inline void getPointClassicAccelerationDerivatives(const ModelTpl & model, + const DataTpl & data, + const Model::JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_point_partial_dq, + const Eigen::MatrixBase & v_point_partial_dv, + const Eigen::MatrixBase & a_point_partial_dq, + const Eigen::MatrixBase & a_point_partial_dv, + const Eigen::MatrixBase & a_point_partial_da); + /// /// \brief Computes all the terms required to compute the second order derivatives of the placement information, also know as the /// kinematic Hessian. This function assumes that the joint Jacobians (a.k.a data.J) has been computed first. See \ref computeJointJacobians diff --git a/src/algorithm/kinematics-derivatives.hxx b/src/algorithm/kinematics-derivatives.hxx index 95ec12d6e6..e00dd75915 100644 --- a/src/algorithm/kinematics-derivatives.hxx +++ b/src/algorithm/kinematics-derivatives.hxx @@ -5,6 +5,7 @@ #ifndef __pinocchio_algorithm_kinematics_derivatives_hxx__ #define __pinocchio_algorithm_kinematics_derivatives_hxx__ +#include "pinocchio/spatial/classic-acceleration.hpp" #include "pinocchio/multibody/visitor.hpp" #include "pinocchio/algorithm/check.hpp" #include "pinocchio/algorithm/jacobian.hpp" @@ -207,11 +208,13 @@ namespace pinocchio EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Data::Matrix6x); EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Data::Matrix6x); + typedef ModelTpl Model; + PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT((int) jointId < model.njoints, "The joint id is invalid."); assert(model.check(data) && "data is not consistent with model."); - typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; typedef JointVelocityDerivativesBackwardStep Pass1; @@ -375,9 +378,10 @@ namespace pinocchio { atmp = oMlast.actInv(data.oa[parent]); motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols); + + motionSet::motionAction(vtmp,v_partial_dq_cols,a_partial_dq_cols); } - motionSet::motionAction(vtmp,v_partial_dq_cols,a_partial_dq_cols); break; } @@ -396,15 +400,19 @@ namespace pinocchio const Eigen::MatrixBase & a_partial_dv, const Eigen::MatrixBase & a_partial_da) { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Data::Matrix6x); EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Data::Matrix6x); EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3,Data::Matrix6x); EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4,Data::Matrix6x); + typedef ModelTpl Model; + PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dq.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dv.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT((int) jointId < model.njoints, "The joint id is invalid."); assert(model.check(data) && "data is not consistent with model."); typedef ModelTpl Model; @@ -446,6 +454,366 @@ namespace pinocchio PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv) = a_partial_da; } + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2> + struct PointVelocityDerivativesBackwardStep + : public fusion::JointUnaryVisitorBase< PointVelocityDerivativesBackwardStep > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + const Model & model, + const Data & data, + const typename Data::SE3 & oMpoint, + const typename Data::Motion & spatial_point_velocity, + const ReferenceFrame & rf, + const Eigen::MatrixBase & v_partial_dq, + const Eigen::MatrixBase & v_partial_dv) + { + typedef typename Model::JointIndex JointIndex; + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + Motion vtmp; // Temporary variable + + const SE3 & oMlast = oMpoint; + + typedef typename SizeDepType::template ColsReturn::ConstType ColsBlock; + ColsBlock Jcols = jmodel.jointCols(data.J); + + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; + Matrix3xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1,v_partial_dq); + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut2; + Matrix3xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2,v_partial_dv); + + ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + + const int nv = jmodel.nv(); + Eigen::Matrix v_spatial_partial_dv_cols(6,nv); + +#define FOR_NV() for(Eigen::DenseIndex j = 0; j < nv; ++j) +#define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR) +#define GET_ANGULAR(vec6) vec6.template segment<3>(Motion::ANGULAR) + + // dvec/dv + motionSet::se3ActionInverse(oMlast,Jcols,v_spatial_partial_dv_cols); + v_partial_dv_cols = v_spatial_partial_dv_cols.template middleRows<3>(Motion::LINEAR); + + // dvec/dq + if(parent > 0) + { + vtmp = oMlast.actInv(data.ov[parent]); + FOR_NV() + v_partial_dq_cols.col(j).noalias() + = vtmp.angular().cross(GET_LINEAR(v_spatial_partial_dv_cols.col(j))) + + vtmp.linear().cross(GET_ANGULAR(v_spatial_partial_dv_cols.col(j))); + } + else + v_partial_dq_cols.setZero(); + + if(rf == LOCAL_WORLD_ALIGNED) + { + FOR_NV() + v_partial_dq_cols.col(j) = oMlast.rotation() * (v_partial_dq_cols.col(j) + + GET_ANGULAR(v_spatial_partial_dv_cols.col(j)).cross(spatial_point_velocity.linear())); + FOR_NV() + v_partial_dv_cols.col(j) = oMlast.rotation() * v_partial_dv_cols.col(j); + } + +#undef FOR_NV +#undef GET_LINEAR +#undef GET_ANGULAR + } + + }; + + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2> + inline void getPointVelocityDerivatives(const ModelTpl & model, + const DataTpl & data, + const Model::JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_point_partial_dq, + const Eigen::MatrixBase & v_point_partial_dv) + { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut1,Data::Matrix3x); + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2,Data::Matrix3x); + + typedef ModelTpl Model; + typedef typename Model::JointIndex JointIndex; + typedef DataTpl Data; + + PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dq.cols(), model.nv); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dv.cols(), model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT((int) joint_id < model.njoints, "The joint id is invalid."); + PINOCCHIO_CHECK_INPUT_ARGUMENT(rf == LOCAL || rf == LOCAL_WORLD_ALIGNED, + "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED"); + assert(model.check(data) && "data is not consistent with model."); + + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; + + const SE3 oMpoint = data.oMi[joint_id] * placement; + const Motion spatial_velocity = oMpoint.actInv(data.ov[joint_id]); + + typedef PointVelocityDerivativesBackwardStep Pass1; + for(JointIndex i = joint_id; i > 0; i = model.parents[i]) + { + Pass1::run(model.joints[i], + typename Pass1::ArgsType(model, data, + oMpoint, spatial_velocity, + rf, + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1,v_point_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2,v_point_partial_dv))); + + } + } + + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2, typename Matrix3xOut3, typename Matrix3xOut4> + struct PointClassicAccelerationDerivativesBackwardStep + : public fusion::JointUnaryVisitorBase< PointClassicAccelerationDerivativesBackwardStep > + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template + static void algo(const JointModelBase & jmodel, + const Model & model, + const Data & data, + const typename Data::SE3 & oMpoint, + const typename Data::Motion & spatial_point_velocity, + const typename Data::Motion::Vector3 & point_classic_acceleration, + const ReferenceFrame & rf, + const Eigen::MatrixBase & v_partial_dq, + const Eigen::MatrixBase & a_partial_dq, + const Eigen::MatrixBase & a_partial_dv, + const Eigen::MatrixBase & a_partial_da) + { + typedef typename Model::JointIndex JointIndex; + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; + + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; + Motion vtmp; // Temporary variable + Motion atmp; // Temporary variable + + const SE3 & oMlast = oMpoint; + const Motion & v_last = spatial_point_velocity; + + typedef typename SizeDepType::template ColsReturn::ConstType ColsBlock; + ColsBlock dJcols = jmodel.jointCols(data.dJ); + ColsBlock Jcols = jmodel.jointCols(data.J); + + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; + Matrix3xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1,v_partial_dq); + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut2; + Matrix3xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2,a_partial_dq); + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut3; + Matrix3xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3,a_partial_dv); + typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut4; + Matrix3xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4,a_partial_da); + + ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_); + ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_); + ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_); + + const int nv = jmodel.nv(); + Eigen::Matrix a_spatial_partial_da_cols(6,nv); + Eigen::Matrix v_spatial_partial_dq_cols(6,nv); + +#define FOR_NV() for(Eigen::DenseIndex j = 0; j < nv; ++j) +#define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR) +#define GET_ANGULAR(vec6) vec6.template segment<3>(Motion::ANGULAR) + + // dacc/da + motionSet::se3ActionInverse(oMlast,Jcols,a_spatial_partial_da_cols); + a_partial_da_cols = a_spatial_partial_da_cols.template middleRows<3>(Motion::LINEAR); + + // dacc/dv + // also computes dvec/dq + if(parent > 0) + { + vtmp = oMlast.actInv(data.ov[parent]); + motionSet::motionAction(vtmp,a_spatial_partial_da_cols,v_spatial_partial_dq_cols); + v_partial_dq_cols = v_spatial_partial_dq_cols.template middleRows<3>(Motion::LINEAR); + } + else + v_partial_dq_cols.setZero(); + + if(parent > 0) + vtmp -= v_last; + else + vtmp = -v_last; + +// motionSet::motionAction(vtmp,a_partial_da_cols,a_partial_dv_cols); + FOR_NV() + a_partial_dv_cols.col(j).noalias() + = vtmp.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j))) + + vtmp.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j))); +// motionSet::se3ActionInverse(oMlast,dJcols,a_partial_dv_cols); + FOR_NV() + a_partial_dv_cols.col(j) + += oMlast.rotation().transpose() * (GET_LINEAR(dJcols.col(j)) + + GET_ANGULAR(dJcols.col(j)).cross(oMlast.translation())); + // wxv + FOR_NV() + a_partial_dv_cols.col(j) + += v_last.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j))) + - v_last.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j))); + + // dacc/dq + if(parent > 0) + { + atmp = oMlast.actInv(data.oa[parent]); +// motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols); + FOR_NV() + a_partial_dq_cols.col(j).noalias() + = atmp.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j))) + + atmp.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j))); + +// motionSet::motionAction(vtmp,v_partial_dq_cols,a_partial_dq_cols); + FOR_NV() + a_partial_dq_cols.col(j) + += vtmp.angular().cross(GET_LINEAR(v_spatial_partial_dq_cols.col(j))) + + vtmp.linear().cross(GET_ANGULAR(v_spatial_partial_dq_cols.col(j))); + + // wxv + FOR_NV() + a_partial_dq_cols.col(j) + += v_last.angular().cross(GET_LINEAR(v_spatial_partial_dq_cols.col(j))) + - v_last.linear().cross(GET_ANGULAR(v_spatial_partial_dq_cols.col(j))); + + } + else + a_partial_dq_cols.setZero(); + + if(rf == LOCAL_WORLD_ALIGNED) + { + + FOR_NV() + v_partial_dq_cols.col(j) = oMlast.rotation() * (v_partial_dq_cols.col(j) + + GET_ANGULAR(a_spatial_partial_da_cols.col(j)).cross(spatial_point_velocity.linear())); + FOR_NV() + a_partial_dq_cols.col(j) = oMlast.rotation() * ( a_partial_dq_cols.col(j) + + GET_ANGULAR(a_spatial_partial_da_cols.col(j)).cross(point_classic_acceleration)); + FOR_NV() + a_partial_dv_cols.col(j) = oMlast.rotation() * a_partial_dv_cols.col(j); + FOR_NV() + a_partial_da_cols.col(j) = oMlast.rotation() * a_partial_da_cols.col(j); + } + +#undef FOR_NV +#undef GET_LINEAR +#undef GET_ANGULAR + } + + }; + + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2, typename Matrix3xOut3, typename Matrix3xOut4> + inline void getPointClassicAccelerationDerivatives(const ModelTpl & model, + const DataTpl & data, + const Model::JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_point_partial_dq, + const Eigen::MatrixBase & a_point_partial_dq, + const Eigen::MatrixBase & a_point_partial_dv, + const Eigen::MatrixBase & a_point_partial_da) + { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut1,Data::Matrix3x); + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2,Data::Matrix3x); + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut3,Data::Matrix3x); + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut4,Data::Matrix3x); + + typedef ModelTpl Model; + typedef DataTpl Data; + typedef typename Data::SE3 SE3; + typedef typename Data::Motion Motion; + typedef typename SE3::Vector3 Vector3; + typedef typename Model::JointIndex JointIndex; + + PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dq.cols(), model.nv); + PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_dq.cols(), model.nv); + PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_dv.cols(), model.nv); + PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_da.cols(), model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT((int) joint_id < model.njoints, "The joint id is invalid."); + PINOCCHIO_CHECK_INPUT_ARGUMENT(rf == LOCAL || rf == LOCAL_WORLD_ALIGNED, + "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED"); + assert(model.check(data) && "data is not consistent with model."); + + + const SE3 oMpoint = data.oMi[joint_id] * placement; + const Motion spatial_velocity = oMpoint.actInv(data.ov[joint_id]); + const Motion spatial_acceleration = oMpoint.actInv(data.oa[joint_id]); + const Vector3 point_acc = classicAcceleration(spatial_velocity, spatial_acceleration); + + + typedef PointClassicAccelerationDerivativesBackwardStep Pass1; + for(JointIndex i = joint_id; i > 0; i = model.parents[i]) + { + Pass1::run(model.joints[i], + typename Pass1::ArgsType(model, data, + oMpoint, spatial_velocity, point_acc, + rf, + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1,v_point_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2,a_point_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3,a_point_partial_dv), + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4,a_point_partial_da))); + + } + + } + + template class JointCollectionTpl, typename Matrix3xOut1, typename Matrix3xOut2, typename Matrix3xOut3, typename Matrix3xOut4, typename Matrix3xOut5> + inline void getPointClassicAccelerationDerivatives(const ModelTpl & model, + const DataTpl & data, + const Model::JointIndex joint_id, + const SE3Tpl & placement, + const ReferenceFrame rf, + const Eigen::MatrixBase & v_point_partial_dq, + const Eigen::MatrixBase & v_point_partial_dv, + const Eigen::MatrixBase & a_point_partial_dq, + const Eigen::MatrixBase & a_point_partial_dv, + const Eigen::MatrixBase & a_point_partial_da) + { + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2,Data::Matrix3x); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dv.cols(), model.nv); + getPointClassicAccelerationDerivatives(model,data,joint_id,placement,rf, + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1,v_point_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3,a_point_partial_dq), + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4,a_point_partial_dv), + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut5,a_point_partial_da)); + + PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2,v_point_partial_dv) = a_point_partial_da; + } + template class JointCollectionTpl> inline void computeJointKinematicHessians(const ModelTpl & model, diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp index d4201ac206..7c4b995138 100644 --- a/src/algorithm/kinematics.hpp +++ b/src/algorithm/kinematics.hpp @@ -20,7 +20,7 @@ namespace pinocchio /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// - /// \remark This algorithm may be useful to call to update global joint placement + /// \remarks This algorithm may be useful to call to update global joint placement /// after calling pinocchio::rnea, pinocchio::aba, etc for example. /// template class JointCollectionTpl> diff --git a/src/algorithm/model.hxx b/src/algorithm/model.hxx index f6954388b3..c5cd02e987 100644 --- a/src/algorithm/model.hxx +++ b/src/algorithm/model.hxx @@ -317,6 +317,7 @@ namespace pinocchio reduced_model.joints .reserve((size_t)njoints); reduced_model.jointPlacements .reserve((size_t)njoints); reduced_model.parents .reserve((size_t)njoints); + reduced_model.inertias .reserve((size_t)njoints); reduced_model.names[0] = input_model.names[0]; reduced_model.joints[0] = input_model.joints[0]; diff --git a/src/algorithm/proximal.hpp b/src/algorithm/proximal.hpp new file mode 100644 index 0000000000..68079ba7e9 --- /dev/null +++ b/src/algorithm/proximal.hpp @@ -0,0 +1,77 @@ +// +// Copyright (c) 2019-2021 INRIA +// + +#ifndef __pinocchio_algorithm_proximal_hpp__ +#define __pinocchio_algorithm_proximal_hpp__ + +#include + +namespace pinocchio +{ + + /// + /// \brief Structure containing all the settings paramters for the proximal algorithms. + /// + /// \tparam _Scalar Scalar type of the for the regularization and the accuracy parameter. + /// + /// It contains the accuracy, the maximal number of iterations and the regularization factor common to all proximal algorithms. + /// + template + struct ProximalSettingsTpl + { + typedef _Scalar Scalar; + + /// \brief Default constructor. + ProximalSettingsTpl() + : accuracy(Eigen::NumTraits::dummy_precision()) + , mu(Eigen::NumTraits::dummy_precision()) + , max_iter(1) + , residual(-1.) + , iter(0) + {} + + /// + /// \brief Constructor with all the setting parameters. + /// + ProximalSettingsTpl(Scalar accuracy, + Scalar mu, + int max_iter) + : accuracy(accuracy) + , mu(mu) + , max_iter(max_iter) + , residual(-1.) + , iter(0) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real(accuracy >= 0.) && "accuracy must be positive"); + PINOCCHIO_CHECK_INPUT_ARGUMENT(check_expression_if_real(mu >= 0.) && "mu must be positive"); + assert(max_iter >= 1 && "max_iter must be greater or equal to 1"); + } + + // data + + /// \brief Minimal proximal accuracy required for primal and dual feasibility + Scalar accuracy; + + /// \brief Regularization parameter of the Proximal algorithms. + Scalar mu; + + /// \brief Maximal number of iterations. + int max_iter; + + // data that can be modified by the algorithm + + /// \brief Final residual when the algorithm has converged or reached the maximal number of allowed iterations. + Scalar residual; + + /// \brief Final number of iteration of the algorithm when it has converged or reached the maximal number of allowed iterations. + int iter; + + }; + + typedef ProximalSettingsTpl ProximalSettings; + +} + +#endif // ifndef __pinocchio_algorithm_proximal_hpp__ + diff --git a/src/algorithm/rnea-derivatives.hpp b/src/algorithm/rnea-derivatives.hpp index adf4f217db..80cbafe834 100644 --- a/src/algorithm/rnea-derivatives.hpp +++ b/src/algorithm/rnea-derivatives.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2017-2019 CNRS INRIA // -#ifndef __pinocchio_rnea_derivatives_hpp__ -#define __pinocchio_rnea_derivatives_hpp__ +#ifndef __pinocchio_algorithm_rnea_derivatives_hpp__ +#define __pinocchio_algorithm_rnea_derivatives_hpp__ #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" @@ -214,4 +214,4 @@ namespace pinocchio #include "pinocchio/algorithm/rnea-derivatives.hxx" -#endif // ifndef __pinocchio_rnea_derivatives_hpp__ +#endif // ifndef __pinocchio_algorithm_rnea_derivatives_hpp__ diff --git a/src/algorithm/rnea-derivatives.hxx b/src/algorithm/rnea-derivatives.hxx index fe4d35f53b..0848fc7ca0 100644 --- a/src/algorithm/rnea-derivatives.hxx +++ b/src/algorithm/rnea-derivatives.hxx @@ -1,9 +1,9 @@ // -// Copyright (c) 2017-2020 CNRS INRIA +// Copyright (c) 2017-2021 CNRS INRIA // -#ifndef __pinocchio_rnea_derivatives_hxx__ -#define __pinocchio_rnea_derivatives_hxx__ +#ifndef __pinocchio_algorithm_rnea_derivatives_hxx__ +#define __pinocchio_algorithm_rnea_derivatives_hxx__ #include "pinocchio/multibody/visitor.hpp" #include "pinocchio/algorithm/check.hpp" @@ -33,8 +33,8 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; const Motion & minus_gravity = data.oa_gf[0]; jmodel.calc(jdata.derived(),q.derived()); @@ -79,17 +79,19 @@ namespace pinocchio const Eigen::MatrixBase & gravity_partial_dq) { typedef typename Model::JointIndex JointIndex; + typedef Eigen::Matrix MatrixNV6; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; - typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) YS(jmodel.nv(),6); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; ColsBlock J_cols = jmodel.jointCols(data.J); ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols); @@ -99,9 +101,9 @@ namespace pinocchio motionSet::act(J_cols,data.of[i],dFdq_cols); - lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv())); + motionSet::inertiaAction(data.oYcrb[i],J_cols,Ag_cols); for(int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(typename Model::Index)j]) - gravity_partial_dq_.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j); + gravity_partial_dq_.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = Ag_cols.transpose() * data.dAdq.col(j); jmodel.jointVelocitySelector(g).noalias() = J_cols.transpose()*data.of[i].toVector(); if(parent>0) @@ -110,15 +112,6 @@ namespace pinocchio data.of[parent] += data.of[i]; } } - - template - static void lhsInertiaMult(const typename Data::Inertia & Y, - const Eigen::MatrixBase & J, - const Eigen::MatrixBase & F) - { - Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F); - motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose()); - } }; template class JointCollectionTpl, typename ConfigVectorType, typename ReturnMatrixType> @@ -216,8 +209,8 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; Motion & ov = data.ov[i]; Motion & oa = data.oa[i]; Motion & oa_gf = data.oa_gf[i]; @@ -313,22 +306,23 @@ namespace pinocchio const Eigen::MatrixBase & rnea_partial_da) { typedef typename Model::JointIndex JointIndex; + typedef typename Data::Matrix6x Matrix6x; - const JointIndex & i = jmodel.id(); - const JointIndex & parent = model.parents[i]; - typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; - typename Data::RowMatrix6 & M6tmpR2 = data.M6tmpR2; + const JointIndex i = jmodel.id(); + const JointIndex parent = model.parents[i]; - typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + typedef typename SizeDepType::template ColsReturn::Type ColsBlock; + Matrix6x & dYtJ = data.Fcrb[0]; ColsBlock J_cols = jmodel.jointCols(data.J); ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); - + ColsBlock dFda_cols = jmodel.jointCols(data.dFda); // Also equals to Ag_cols + ColsBlock dYtJ_cols = jmodel.jointCols(dYtJ); + MatrixType1 & rnea_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType1,rnea_partial_dq); MatrixType2 & rnea_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,rnea_partial_dv); MatrixType3 & rnea_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,rnea_partial_da); @@ -336,17 +330,16 @@ namespace pinocchio // tau jmodel.jointVelocitySelector(data.tau).noalias() = J_cols.transpose()*data.of[i].toVector(); + const Eigen::DenseIndex nv_subtree = data.nvSubtree[i]; + const Eigen::DenseIndex nv = jmodel.nv(); + const Eigen::DenseIndex idx_v = jmodel.idx_v(); + const Eigen::DenseIndex idx_v_plus = idx_v + nv; + const Eigen::DenseIndex nv_subtree_plus = nv_subtree - nv; + // dtau/da similar to data.M motionSet::inertiaAction(data.oYcrb[i],J_cols,dFda_cols); - rnea_partial_da_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() - = J_cols.transpose()*data.dFda.middleCols(jmodel.idx_v(),data.nvSubtree[i]); - - // dtau/dv - dFdv_cols.noalias() = data.doYcrb[i] * J_cols; - motionSet::inertiaAction(data.oYcrb[i],dAdv_cols,dFdv_cols); - - rnea_partial_dv_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() - = J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); + rnea_partial_da_.block(idx_v,idx_v,nv,nv_subtree).noalias() + = J_cols.transpose()*data.dFda.middleCols(idx_v,nv_subtree); // dtau/dq if(parent>0) @@ -356,29 +349,27 @@ namespace pinocchio } else motionSet::inertiaAction(data.oYcrb[i],dAdq_cols,dFdq_cols); - - rnea_partial_dq_.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() - = J_cols.transpose()*data.dFdq.middleCols(jmodel.idx_v(),data.nvSubtree[i]); + + dYtJ_cols.transpose().noalias() = J_cols.transpose() * data.doYcrb[i]; + rnea_partial_dq_.block(idx_v_plus,idx_v,nv_subtree_plus,nv).noalias() = + data.dFda.middleCols(idx_v_plus,nv_subtree_plus).transpose() * dAdq_cols + + dYtJ.middleCols(idx_v_plus,nv_subtree_plus).transpose() * dVdq_cols; + + rnea_partial_dq_.block(idx_v,idx_v,nv,nv_subtree).noalias() + = J_cols.transpose()*data.dFdq.middleCols(idx_v,nv_subtree); motionSet::act(J_cols,data.of[i],dFdq_cols); - if(parent > 0) - { - lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv())); - M6tmpR2.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.doYcrb[i]; - for(int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(typename Model::Index)j]) - { - rnea_partial_dq_.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() - = M6tmpR.topRows(jmodel.nv()) * data.dAdq.col(j) - + M6tmpR2.topRows(jmodel.nv()) * data.dVdq.col(j); - } - for(int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(typename Model::Index)j]) - { - rnea_partial_dv_.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() - = M6tmpR.topRows(jmodel.nv()) * data.dAdv.col(j) - + M6tmpR2.topRows(jmodel.nv()) * data.J.col(j); - } - } + // dtau/dv + dFdv_cols.noalias() = data.doYcrb[i] * J_cols; + motionSet::inertiaAction(data.oYcrb[i],dAdv_cols,dFdv_cols); + + rnea_partial_dv_.block(idx_v_plus,idx_v,nv_subtree_plus,nv).noalias() = + data.dFda.middleCols(idx_v_plus,nv_subtree_plus).transpose() * dAdv_cols + + dYtJ.middleCols(idx_v_plus,nv_subtree_plus).transpose() * J_cols; + + rnea_partial_dv_.block(idx_v,idx_v,nv,nv_subtree).noalias() + = J_cols.transpose()*data.dFdv.middleCols(idx_v,nv_subtree); if(parent>0) { @@ -386,25 +377,6 @@ namespace pinocchio data.doYcrb[parent] += data.doYcrb[i]; data.of[parent] += data.of[i]; } - - // Restore the status of dAdq_cols (remove gravity) - PINOCCHIO_CHECK_INPUT_ARGUMENT(isZero(model.gravity.angular()), - "The gravity must be a pure force vector, no angular part"); - for(Eigen::DenseIndex k =0; k < jmodel.nv(); ++k) - { - MotionRef m_in(J_cols.col(k)); - MotionRef m_out(dAdq_cols.col(k)); - m_out.linear() += model.gravity.linear().cross(m_in.angular()); - } - } - - template - static void lhsInertiaMult(const typename Data::Inertia & Y, - const Eigen::MatrixBase & J, - const Eigen::MatrixBase & F) - { - Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F); - motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose()); } }; @@ -429,9 +401,12 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.rows(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.rows(), model.nv); + PINOCCHIO_CHECK_INPUT_ARGUMENT(isZero(model.gravity.angular()), + "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); typedef ModelTpl Model; + typedef DataTpl Data; typedef typename Model::JointIndex JointIndex; data.oa_gf[0] = -model.gravity; @@ -452,6 +427,18 @@ namespace pinocchio PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,rnea_partial_dv), PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,rnea_partial_da))); } + + // Restore the status of dAdq_cols (remove gravity) + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + MotionRef m_in(data.J.col(k)); + MotionRef m_out(data.dAdq.col(k)); + m_out.linear() += model.gravity.linear().cross(m_in.angular()); + } + + // Add armature contribution + data.tau.array() += model.armature.array() * a.array(); // TODO: check if there is memory allocation + PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,rnea_partial_da).diagonal() += model.armature; } template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, @@ -480,6 +467,7 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); typedef ModelTpl Model; + typedef DataTpl Data; typedef typename Model::JointIndex JointIndex; data.oa_gf[0] = -model.gravity; @@ -501,9 +489,21 @@ namespace pinocchio PINOCCHIO_EIGEN_CONST_CAST(MatrixType2,rnea_partial_dv), PINOCCHIO_EIGEN_CONST_CAST(MatrixType3,rnea_partial_da))); } + + // Restore the status of dAdq_cols (remove gravity) + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + MotionRef m_in(data.J.col(k)); + MotionRef m_out(data.dAdq.col(k)); + m_out.linear() += model.gravity.linear().cross(m_in.angular()); + } + + // Add armature contribution + data.tau.array() += model.armature.array() * a.array(); // TODO: check if there is memory allocation + data.M.diagonal() += model.armature; } } // namespace pinocchio -#endif // ifndef __pinocchio_rnea_derivatives_hxx__ +#endif // ifndef __pinocchio_algorithm_rnea_derivatives_hxx__ diff --git a/src/algorithm/rnea.hxx b/src/algorithm/rnea.hxx index 924a089c11..1861266d32 100644 --- a/src/algorithm/rnea.hxx +++ b/src/algorithm/rnea.hxx @@ -43,7 +43,6 @@ namespace pinocchio jmodel.calc(jdata.derived(),q.derived(),v.derived()); data.liMi[i] = model.jointPlacements[i]*jdata.M(); -// data.liMi[i].setIdentity(); data.v[i] = jdata.v(); if(parent>0) @@ -88,6 +87,7 @@ namespace pinocchio const JointIndex parent = model.parents[i]; jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose()*data.f[i]; + if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]); } }; @@ -115,17 +115,18 @@ namespace pinocchio typename Pass1::ArgsType arg1(model,data,q.derived(),v.derived(),a.derived()); for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) { - Pass1::run(model.joints[i],data.joints[i], - arg1); + Pass1::run(model.joints[i],data.joints[i],arg1); } typedef RneaBackwardStep Pass2; typename Pass2::ArgsType arg2(model,data); for(JointIndex i=(JointIndex)model.njoints-1; i>0; --i) { - Pass2::run(model.joints[i],data.joints[i], - arg2); + Pass2::run(model.joints[i],data.joints[i],arg2); } + + // Add rotorinertia contribution + data.tau.array() += model.armature.array() * a.array(); // Check if there is memory allocation return data.tau; } @@ -165,7 +166,9 @@ namespace pinocchio Pass2::run(model.joints[i],data.joints[i], typename Pass2::ArgsType(model,data)); } - + + // Add armature contribution + data.tau.array() += model.armature.array() * a.array(); // TODO: check if there is memory allocation return data.tau; } @@ -441,12 +444,12 @@ namespace pinocchio // computes S expressed at the world frame typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); - Jcols = data.oMi[i].act(jdata.S()); // collection of S expressed at the world frame + ColsBlock J_cols = jmodel.jointCols(data.J); + J_cols = data.oMi[i].act(jdata.S()); // collection of S expressed at the world frame // computes vxS expressed at the world frame - ColsBlock dJcols = jmodel.jointCols(data.dJ); - motionSet::motionAction(data.ov[i],Jcols,dJcols); + ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + motionSet::motionAction(data.ov[i],J_cols,dJ_cols); // computes vxI typedef typename Data::Inertia Inertia; @@ -472,28 +475,31 @@ namespace pinocchio Data & data) { typedef typename Model::JointIndex JointIndex; + typedef Eigen::Matrix MatrixNV6; const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; + + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(),6); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i],dJcols,jmodel.jointCols(data.dFdv)); - jmodel.jointCols(data.dFdv) += data.vxI[i] * Jcols; + motionSet::inertiaAction(data.oYcrb[i],dJ_cols,jmodel.jointCols(data.dFdv)); + jmodel.jointCols(data.dFdv).noalias() += data.vxI[i] * J_cols; data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() - = Jcols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); + = J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); - lhsInertiaMult(data.oYcrb[i],Jcols.transpose(),M6tmpR.topRows(jmodel.nv())); + motionSet::inertiaAction(data.oYcrb[i],J_cols,Ag_cols); for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dJ.col(j); + data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = Ag_cols.transpose() * data.dJ.col(j); - M6tmpR.topRows(jmodel.nv()).noalias() = Jcols.transpose() * data.vxI[i]; + Mat_tmp.noalias() = J_cols.transpose() * data.vxI[i]; for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += M6tmpR.topRows(jmodel.nv()) * data.J.col(j); + data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += Mat_tmp * data.J.col(j); if(parent>0) { @@ -502,15 +508,6 @@ namespace pinocchio } } - - template - static void lhsInertiaMult(const typename Data::Inertia & Y, - const Eigen::MatrixBase & J, - const Eigen::MatrixBase & F) - { - Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F); - motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose()); - } }; template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> @@ -561,31 +558,31 @@ namespace pinocchio Data & data) { typedef typename Model::JointIndex JointIndex; - typedef CoriolisMatrixBackwardStep EquivalentPass; + typedef Eigen::Matrix MatrixNV6; const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; + + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(),6); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; ColsBlock dJ_cols = jmodel.jointCols(data.dJ); ColsBlock J_cols = jmodel.jointCols(data.J); - typename Data::Matrix6x & dFdv = data.Fcrb[0]; - ColsBlock dFdv_cols = jmodel.jointCols(dFdv); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i],dJ_cols,dFdv_cols); - dFdv_cols.noalias() += data.vxI[i] * J_cols; + motionSet::inertiaAction(data.oYcrb[i],dJ_cols,jmodel.jointCols(data.dFdv)); + jmodel.jointCols(data.dFdv).noalias() += data.vxI[i] * J_cols; data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() - = J_cols.transpose() * dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); + = J_cols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); - EquivalentPass::lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv())); + motionSet::inertiaAction(data.oYcrb[i],J_cols,Ag_cols); for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dJ.col(j); + data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = Ag_cols.transpose() * data.dJ.col(j); - M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.vxI[i]; + Mat_tmp.noalias() = J_cols.transpose() * data.vxI[i]; for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() += M6tmpR.topRows(jmodel.nv()) * data.J.col(j); + data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j) += Mat_tmp * data.J.col(j); if(parent>0) { @@ -602,8 +599,8 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); - typedef ModelTpl Model; - typedef typename Model::JointIndex JointIndex; + typedef DataTpl Data; + typedef typename Data::Inertia Inertia; for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) { diff --git a/src/algorithm/utils/force.hpp b/src/algorithm/utils/force.hpp new file mode 100644 index 0000000000..2a063f07e4 --- /dev/null +++ b/src/algorithm/utils/force.hpp @@ -0,0 +1,101 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_algorithm_utils_force_hpp__ +#define __pinocchio_algorithm_utils_force_hpp__ + +#include "pinocchio/spatial/se3.hpp" +#include "pinocchio/spatial/force.hpp" +#include "pinocchio/multibody/fwd.hpp" + +namespace pinocchio +{ + /// + /// @copydoc changeReferenceFrame(const SE3Tpl &,const ForceDense &,const ReferenceFrame,const ReferenceFrame) + /// \param[out] f_out Resulting force quantity. + /// + template + void changeReferenceFrame(const SE3Tpl & placement, + const ForceDense & f_in, + const ReferenceFrame rf_in, + const ReferenceFrame rf_out, + ForceDense & f_out) + { + if(rf_in == rf_out) + { + f_out = f_in; + return; + } + + // case: LOCAL/WORLD and WORLD/LOCAL + if(rf_in == LOCAL && rf_out == WORLD) + { + f_out = placement.act(f_in); return; + } + if(rf_in == WORLD && rf_out == LOCAL) + { + f_out = placement.actInv(f_in); return; + } + + // case: LOCAL/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/LOCAL + if(rf_in == LOCAL && rf_out == LOCAL_WORLD_ALIGNED) + { + f_out.angular().noalias() = placement.rotation()*f_in.angular(); + f_out.linear().noalias() = placement.rotation()*f_in.linear(); + return; + } + if(rf_in == LOCAL_WORLD_ALIGNED && rf_out == LOCAL) + { + f_out.angular().noalias() = placement.rotation().transpose()*f_in.angular(); + f_out.linear().noalias() = placement.rotation().transpose()*f_in.linear(); + return; + } + + // case: WORLD/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/WORLD + if(rf_in == WORLD && rf_out == LOCAL_WORLD_ALIGNED) + { + f_out.linear() = f_in.linear(); + f_out.angular().noalias() = f_in.angular() + f_in.linear().cross(placement.translation()); + return; + } + if(rf_in == LOCAL_WORLD_ALIGNED && rf_out == WORLD) + { + f_out.linear() = f_in.linear(); + f_out.angular().noalias() = f_in.angular() - f_in.linear().cross(placement.translation()); + return; + } + + assert(false && "This must never happened."); + } + + /// + /// \brief Change the expression of a given Force vector from one reference frame to another reference frame. + /// + /// \example If ones has an initial f_in Force expressed locally (rf_in=LOCAL) in a Frame localized at a given placement value, + ///  ones may want to retrieve its value inside another reference frame e.g. the world (rf_out=WORLD). + /// + /// \param[in] placement Placement of the frame having force f_in + /// \param[in] f_in Input force quantity. + /// \param[in] rf_in Reference frame in which m_in is expressed + /// \param[in] rf_out Reference frame in which the result m_out is expressed + /// \param[out] f_out Resulting force quantity. + /// + template + typename ForceIn::ForcePlain + changeReferenceFrame(const SE3Tpl & placement, + const ForceDense & f_in, + const ReferenceFrame rf_in, + const ReferenceFrame rf_out) + { + typedef typename ForceIn::ForcePlain ReturnType; + ReturnType res; + + changeReferenceFrame(placement,f_in,rf_in,rf_out,res); + + return res; + } + +} // namespace pinocchio + +#endif // #ifndef __pinocchio_algorithm_utils_force_hpp__ diff --git a/src/algorithm/utils/motion.hpp b/src/algorithm/utils/motion.hpp new file mode 100644 index 0000000000..45b69fa7f6 --- /dev/null +++ b/src/algorithm/utils/motion.hpp @@ -0,0 +1,102 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_algorithm_utils_motion_hpp__ +#define __pinocchio_algorithm_utils_motion_hpp__ + +#include "pinocchio/spatial/se3.hpp" +#include "pinocchio/spatial/motion.hpp" +#include "pinocchio/multibody/fwd.hpp" + +namespace pinocchio +{ + + /// + /// @copydoc changeReferenceFrame(const SE3Tpl &,const MotionDense &,const ReferenceFrame,const ReferenceFrame) + /// \param[out] m_out Resulting motion quantity. + /// + template + void changeReferenceFrame(const SE3Tpl & placement, + const MotionDense & m_in, + const ReferenceFrame rf_in, + const ReferenceFrame rf_out, + MotionDense & m_out) + { + if(rf_in == rf_out) + { + m_out = m_in; + return; + } + + // case: LOCAL/WORLD and WORLD/LOCAL + if(rf_in == LOCAL && rf_out == WORLD) + { + m_out = placement.act(m_in); return; + } + if(rf_in == WORLD && rf_out == LOCAL) + { + m_out = placement.actInv(m_in); return; + } + + // case: LOCAL/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/LOCAL + if(rf_in == LOCAL && rf_out == LOCAL_WORLD_ALIGNED) + { + m_out.linear().noalias() = placement.rotation()*m_in.linear(); + m_out.angular().noalias() = placement.rotation()*m_in.angular(); + return; + } + if(rf_in == LOCAL_WORLD_ALIGNED && rf_out == LOCAL) + { + m_out.linear().noalias() = placement.rotation().transpose()*m_in.linear(); + m_out.angular().noalias() = placement.rotation().transpose()*m_in.angular(); + return; + } + + // case: WORLD/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/WORLD + if(rf_in == WORLD && rf_out == LOCAL_WORLD_ALIGNED) + { + m_out.angular() = m_in.angular(); + m_out.linear().noalias() = m_in.linear() + m_in.angular().cross(placement.translation()); + return; + } + if(rf_in == LOCAL_WORLD_ALIGNED && rf_out == WORLD) + { + m_out.angular() = m_in.angular(); + m_out.linear().noalias() = m_in.linear() - m_in.angular().cross(placement.translation()); + return; + } + + assert(false && "This must never happened."); + } + + /// + /// \brief Change the expression of a given Motion vector from one reference frame to another reference frame. + /// + /// \example If ones has an initial m_in Motion expressed locally (rf_in=LOCAL) in a Frame localized at a given placement value, + ///  ones may want to retrieve its value inside another reference frame e.g. the world (rf_out=WORLD). + /// + /// \param[in] placement Placement of the frame having velocity m_in + /// \param[in] m_in Input motion quantity. + /// \param[in] rf_in Reference frame in which m_in is expressed + /// \param[in] rf_out Reference frame in which the result m_out is expressed + /// \param[out] m_out Resulting motion quantity. + /// + template + typename MotionIn::MotionPlain + changeReferenceFrame(const SE3Tpl & placement, + const MotionDense & m_in, + const ReferenceFrame rf_in, + const ReferenceFrame rf_out) + { + typedef typename MotionIn::MotionPlain ReturnType; + ReturnType res; + + changeReferenceFrame(placement,m_in,rf_in,rf_out,res); + + return res; + } + +} // namespace pinocchio + +#endif // #ifndef __pinocchio_multibody_fwd_hpp__ diff --git a/src/autodiff/casadi-algo.hpp b/src/autodiff/casadi-algo.hpp new file mode 100644 index 0000000000..399c443539 --- /dev/null +++ b/src/autodiff/casadi-algo.hpp @@ -0,0 +1,786 @@ +// +// Copyright (c) 2021 INRIA +// + +#ifndef __pinocchio_autodiff_code_generator_algo_hpp__ +#define __pinocchio_autodiff_code_generator_algo_hpp__ + +#include + +#include "pinocchio/autodiff/casadi.hpp" +#include "pinocchio/algorithm/aba.hpp" +#include "pinocchio/algorithm/aba-derivatives.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/contact-dynamics-derivatives.hpp" +#include "pinocchio/algorithm/contact-dynamics.hpp" + +namespace pinocchio +{ + namespace casadi + { + + template + struct AutoDiffAlgoBase + { + typedef _Scalar Scalar; + typedef ::casadi::SX ADScalar; + typedef ::casadi::SXVector ADSVector; + typedef ::casadi::DM DMMatrix; + typedef ::casadi::DMVector DMVector; + enum { Options = 0 }; + + typedef pinocchio::ModelTpl Model; + typedef pinocchio::DataTpl Data; + typedef pinocchio::ModelTpl ADModel; + typedef pinocchio::DataTpl ADData; + + typedef typename Model::ConfigVectorType ConfigVectorType; + typedef typename Model::TangentVectorType TangentVectorType; + typedef typename ADModel::ConfigVectorType ADConfigVectorType; + typedef typename ADModel::TangentVectorType ADTangentVectorType; + + typedef typename Data::MatrixXs MatrixXs; + typedef typename Data::VectorXs VectorXs; + typedef typename Data::RowMatrixXs RowMatrixXs; + + typedef ::casadi::Function ADFun; + + AutoDiffAlgoBase(const Model& model, + const std::string& filename, + const std::string& libname, + const std::string& fun_name) + : ad_model(model.template cast()) + , ad_data(ad_model) + , filename(filename) + , libname(libname) + , fun_name(fun_name) + , cg_generated(filename) + , build_forward(true) + , build_jacobian(true) { } + + virtual ~AutoDiffAlgoBase() {} + + /// \brief build the mapping Y = f(X) + virtual void buildMap() = 0; + + void initLib() + { + buildMap(); + //Generated code; + cg_generated.add(ad_fun); + if(build_jacobian) + { + cg_generated.add(ad_fun_derivs); + } + cg_generated.generate(); + } + + bool existLib() const + { + std::ifstream file((libname + ".so").c_str()); + return file.good(); + } + + void compileLib() + { + std::string compile_command = "clang -fPIC -shared -Ofast -DNDEBUG " + +filename+".c -o "+libname+".so"; + + int flag = system(compile_command.c_str()); + if (flag!=0) + { + std::cerr<<"Compilation failed"< fun_output, fun_output_derivs; + }; + + + template + struct AutoDiffABA : public AutoDiffAlgoBase<_Scalar> + { + typedef AutoDiffAlgoBase<_Scalar> Base; + typedef typename Base::Scalar Scalar; + + typedef typename Base::TangentVectorType TangentVectorType; + typedef typename Base::RowMatrixXs RowMatrixXs; + typedef typename Base::VectorXs VectorXs; + typedef typename Base::MatrixXs MatrixXs; + + typedef typename Base::ADFun ADFun; + typedef typename Base::DMVector DMVector; + typedef typename Base::DMMatrix DMMatrix; + typedef typename Base::ADScalar ADScalar; + typedef typename Base::ADSVector ADSVector; + typedef typename Base::ADConfigVectorType ADConfigVectorType; + typedef typename Base::ADTangentVectorType ADTangentVectorType; + + + explicit AutoDiffABA(const Model& model, + const std::string& filename = "casadi_aba", + const std::string& libname = "libcasadi_cg_aba", + const std::string& fun_name = "eval_f") + : Base(model, filename, libname, fun_name) + , cs_q(ADScalar::sym("q", model.nq)) + , cs_v(ADScalar::sym("v", model.nv)) + , cs_tau(ADScalar::sym("tau", model.nv)) + , cs_v_int(ADScalar::sym("v_inc", model.nv)) + , q_ad(model.nq) + , q_int_ad(model.nq) + , v_ad(model.nv) + , v_int_ad(model.nv) + , tau_ad(model.nv) + , cs_ddq(model.nv,1) + , q_vec((size_t)model.nq) + , v_vec((size_t)model.nv) + , v_int_vec((size_t)model.nv) + , tau_vec((size_t)model.nv) + , ddq(model.nv) + , ddq_dq(model.nv,model.nv) + , ddq_dv(model.nv,model.nv) + , ddq_dtau(model.nv,model.nv) + { + q_ad = Eigen::Map(static_cast< std::vector >(cs_q).data(),model.nq,1); + v_int_ad = Eigen::Map(static_cast< std::vector >(cs_v_int).data(),model.nv,1); + v_ad = Eigen::Map(static_cast< std::vector >(cs_v).data(),model.nv,1); + tau_ad = Eigen::Map(static_cast< std::vector >(cs_tau).data(),model.nv,1); + + Eigen::Map(v_int_vec.data(),model.nv,1).setZero(); + + } + + virtual ~AutoDiffABA() {} + + virtual void buildMap() + { + //Integrate q + v_int = q_int + pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad); + //Run ABA with new q_int + pinocchio::aba(ad_model,ad_data,q_int_ad,v_ad,tau_ad); + //Copy Output + pinocchio::casadi::copy(ad_data.ddq,cs_ddq); + + cs_ddq_dq = jacobian(cs_ddq, cs_v_int); + cs_ddq_dv = jacobian(cs_ddq, cs_v); + cs_ddq_dtau = jacobian(cs_ddq, cs_tau); + + ad_fun = ADFun(fun_name, + ADSVector {cs_q, cs_v_int, cs_v, cs_tau}, + ADSVector {cs_ddq}); + + ad_fun_derivs = ADFun(fun_name+"_derivs", + ADSVector {cs_q,cs_v_int,cs_v,cs_tau}, + ADSVector {cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau}); + } + + template + void evalFunction(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau) + { + Eigen::Map(q_vec.data(),ad_model.nq,1) = q; + Eigen::Map(v_vec.data(),ad_model.nv,1) = v; + Eigen::Map(tau_vec.data(),ad_model.nv,1) = tau; + fun_output = fun(DMVector {q_vec, v_int_vec, v_vec, tau_vec}); + ddq = + Eigen::Map(static_cast< std::vector > + (fun_output[0]).data(),ad_model.nv,1); + } + + template + void evalJacobian(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau) + { + Eigen::Map(q_vec.data(),ad_model.nq,1) = q; + Eigen::Map(v_vec.data(),ad_model.nv,1) = v; + Eigen::Map(tau_vec.data(),ad_model.nv,1) = tau; + fun_output_derivs = fun_derivs (DMVector {q_vec,v_int_vec,v_vec,tau_vec}); + + ddq_dq = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[0]).data(),ad_model.nv,ad_model.nv); + ddq_dv = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[1]).data(),ad_model.nv,ad_model.nv); + ddq_dtau = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[2]).data(),ad_model.nv,ad_model.nv); + } + + TangentVectorType ddq; + RowMatrixXs ddq_dq, ddq_dv, ddq_dtau; + + protected: + using Base::ad_model; + using Base::ad_data; + using Base::filename; + using Base::libname; + using Base::fun_name; + using Base::cg_generated; + + using Base::ad_fun; + using Base::ad_fun_derivs; + using Base::fun; + using Base::fun_derivs; + using Base::fun_output; + using Base::fun_output_derivs; + + ADScalar cs_q, cs_v, cs_tau, cs_v_int, cs_ddq; + + //Derivatives + ADScalar cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau; + + ADConfigVectorType q_ad, q_int_ad; + ADTangentVectorType v_ad, v_int_ad, tau_ad; + + std::vector q_vec, v_vec, v_int_vec, tau_vec; + }; + + template + struct AutoDiffABADerivatives : public AutoDiffAlgoBase<_Scalar> + { + + typedef AutoDiffAlgoBase<_Scalar> Base; + typedef typename Base::Scalar Scalar; + + typedef typename Base::TangentVectorType TangentVectorType; + typedef typename Base::RowMatrixXs RowMatrixXs; + typedef typename Base::VectorXs VectorXs; + typedef typename Base::MatrixXs MatrixXs; + + typedef typename Base::ADFun ADFun; + typedef typename Base::DMVector DMVector; + typedef typename Base::DMMatrix DMMatrix; + typedef typename Base::ADScalar ADScalar; + typedef typename Base::ADSVector ADSVector; + typedef typename Base::ADConfigVectorType ADConfigVectorType; + typedef typename Base::ADTangentVectorType ADTangentVectorType; + + explicit AutoDiffABADerivatives(const Model& model, + const std::string& filename = "casadi_abaDerivs", + const std::string& libname = "libcasadi_cg_abaDerivs", + const std::string& fun_name = "eval_f") + : Base(model, filename, libname, fun_name) + , cs_q(ADScalar::sym("q", model.nq)) + , cs_v(ADScalar::sym("v", model.nv)) + , cs_tau(ADScalar::sym("tau", model.nv)) + , q_ad(model.nq) + , v_ad(model.nv) + , tau_ad(model.nv) + , cs_ddq(model.nv,1) + , q_vec((size_t)model.nq) + , v_vec((size_t)model.nv) + , tau_vec((size_t)model.nv) + , ddq(model.nv) + , ddq_dq(model.nv,model.nv) + , ddq_dv(model.nv,model.nv) + , ddq_dtau(model.nv,model.nv) + { + q_ad = Eigen::Map(static_cast< std::vector >(cs_q).data(),model.nq,1); + v_ad = Eigen::Map(static_cast< std::vector >(cs_v).data(),model.nv,1); + tau_ad = Eigen::Map(static_cast< std::vector >(cs_tau).data(),model.nv,1); + + Base::build_jacobian = false; + } + + virtual ~AutoDiffABADerivatives() {} + + virtual void buildMap() + { + //Run ABA with new q_int + pinocchio::computeABADerivatives(ad_model,ad_data,q_ad,v_ad,tau_ad); + //Copy Output + ad_data.Minv.template triangularView() + = ad_data.Minv.transpose().template triangularView(); + pinocchio::casadi::copy(ad_data.ddq,cs_ddq); + pinocchio::casadi::copy(ad_data.ddq_dq,cs_ddq_dq); + pinocchio::casadi::copy(ad_data.ddq_dv,cs_ddq_dv); + pinocchio::casadi::copy(ad_data.Minv,cs_ddq_dtau); + + ad_fun = ADFun(fun_name, + ADSVector {cs_q, cs_v, cs_tau}, + ADSVector {cs_ddq, cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau}); + } + + template + void evalFunction(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau) + { + Eigen::Map(q_vec.data(),ad_model.nq,1) = q; + Eigen::Map(v_vec.data(),ad_model.nv,1) = v; + Eigen::Map(tau_vec.data(),ad_model.nv,1) = tau; + fun_output = fun(DMVector {q_vec, v_vec, tau_vec}); + + ddq = + Eigen::Map(static_cast< std::vector > + (fun_output[0]).data(),ad_model.nv,1); + ddq_dq = + Eigen::Map(static_cast< std::vector > + (fun_output[1]).data(),ad_model.nv,ad_model.nv); + ddq_dv = + Eigen::Map(static_cast< std::vector > + (fun_output[2]).data(),ad_model.nv,ad_model.nv); + ddq_dtau = + Eigen::Map(static_cast< std::vector > + (fun_output[3]).data(),ad_model.nv,ad_model.nv); + } + + TangentVectorType ddq; + RowMatrixXs ddq_dq, ddq_dv, ddq_dtau; + + protected: + using Base::ad_model; + using Base::ad_data; + using Base::filename; + using Base::libname; + using Base::fun_name; + using Base::cg_generated; + + using Base::ad_fun; + using Base::ad_fun_derivs; + using Base::fun; + using Base::fun_derivs; + using Base::fun_output; + using Base::fun_output_derivs; + + ADScalar cs_q, cs_v, cs_tau, cs_ddq; + //Derivatives + ADScalar cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau; + + ADConfigVectorType q_ad; + ADTangentVectorType v_ad, tau_ad; + std::vector q_vec, v_vec, tau_vec; + }; + + + template + struct AutoDiffConstraintDynamics : public AutoDiffAlgoBase<_Scalar> + { + typedef AutoDiffAlgoBase<_Scalar> Base; + typedef typename Base::Scalar Scalar; + typedef typename Base::ADScalar ADScalar; + typedef typename Base::ADSVector ADSVector; + + typedef typename Base::TangentVectorType TangentVectorType; + typedef typename Base::RowMatrixXs RowMatrixXs; + typedef typename Base::VectorXs VectorXs; + typedef typename Base::MatrixXs MatrixXs; + + typedef typename Base::ADFun ADFun; + typedef typename Base::DMVector DMVector; + typedef typename Base::DMMatrix DMMatrix; + typedef typename Base::ADConfigVectorType ADConfigVectorType; + typedef typename Base::ADTangentVectorType ADTangentVectorType; + + + typedef typename pinocchio::RigidConstraintModelTpl ContactModel; + typedef Eigen::aligned_allocator ContactModelAllocator; + typedef typename std::vector ContactModelVector; + typedef typename pinocchio::RigidConstraintDataTpl ContactData; + typedef Eigen::aligned_allocator ContactDataAllocator; + typedef typename std::vector ContactDataVector; + + typedef typename pinocchio::RigidConstraintModelTpl ADContactModel; + typedef Eigen::aligned_allocator ADContactModelAllocator; + typedef typename std::vector ADContactModelVector; + typedef typename pinocchio::RigidConstraintDataTpl ADContactData; + typedef Eigen::aligned_allocator ADContactDataAllocator; + typedef typename std::vector ADContactDataVector; + + static Eigen::DenseIndex constraintDim(const ContactModelVector& contact_models) + { + Eigen::DenseIndex num_total_constraints = 0; + for(typename ContactModelVector::const_iterator it = contact_models.begin(); + it != contact_models.end(); + ++it) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(it->size() > 0, + "The dimension of the constraint must be positive"); + num_total_constraints += it->size(); + } + return num_total_constraints; + } + + explicit AutoDiffConstraintDynamics(const Model& model, + const ContactModelVector& contact_models, + const std::string& filename = "casadi_contactDyn", + const std::string& libname = "libcasadi_cg_contactDyn", + const std::string& fun_name = "eval_f") + : Base(model, filename, libname, fun_name) + , nc(constraintDim(contact_models)) + , cs_q(ADScalar::sym("q", model.nq)) + , cs_v(ADScalar::sym("v", model.nv)) + , cs_tau(ADScalar::sym("tau", model.nv)) + , cs_v_int(ADScalar::sym("v_inc", model.nv)) + , q_ad(model.nq) + , q_int_ad(model.nq) + , v_ad(model.nv) + , v_int_ad(model.nv) + , tau_ad(model.nv) + , cs_ddq(model.nv,1) + , cs_lambda_c(model.nv,1) + , q_vec((size_t)model.nq) + , v_vec((size_t)model.nv) + , v_int_vec((size_t)model.nv) + , tau_vec((size_t)model.nv) + , ddq(model.nv) + , ddq_dq(model.nv,model.nv) + , ddq_dv(model.nv,model.nv) + , ddq_dtau(model.nv,model.nv) + { + lambda_c.resize(nc); lambda_c.setZero(); + dlambda_dq.resize(nc,model.nv); dlambda_dq.setZero(); + dlambda_dv.resize(nc,model.nv); dlambda_dv.setZero(); + dlambda_dtau.resize(nc,model.nv); dlambda_dtau.setZero(); + q_ad = Eigen::Map(static_cast< std::vector >(cs_q).data(),model.nq,1); + v_int_ad = Eigen::Map(static_cast< std::vector >(cs_v_int).data(),model.nv,1); + v_ad = Eigen::Map(static_cast< std::vector >(cs_v).data(),model.nv,1); + tau_ad = Eigen::Map(static_cast< std::vector >(cs_tau).data(),model.nv,1); + + Eigen::Map(v_int_vec.data(),model.nv,1).setZero(); + + for(int k=0;k()); + ad_contact_datas.push_back(ADContactData(ad_contact_models[k])); + } + } + + virtual ~AutoDiffConstraintDynamics() {} + + + virtual void buildMap() + { + pinocchio::initConstraintDynamics(ad_model, ad_data, ad_contact_models); + //Integrate q + v_int = q_int + pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad); + //Run contact dynamics with new q_int + pinocchio::constraintDynamics(ad_model,ad_data,q_int_ad,v_ad,tau_ad, + ad_contact_models,ad_contact_datas); + //Copy Output + pinocchio::casadi::copy(ad_data.ddq,cs_ddq); + pinocchio::casadi::copy(ad_data.lambda_c,cs_lambda_c); + + cs_ddq_dq = jacobian(cs_ddq, cs_v_int); + cs_ddq_dv = jacobian(cs_ddq, cs_v); + cs_ddq_dtau = jacobian(cs_ddq, cs_tau); + + cs_lambda_dq = jacobian(cs_lambda_c, cs_v_int); + cs_lambda_dv = jacobian(cs_lambda_c, cs_v); + cs_lambda_dtau = jacobian(cs_lambda_c, cs_tau); + + ad_fun = ADFun(fun_name, + ADSVector {cs_q, cs_v_int, cs_v, cs_tau}, + ADSVector {cs_ddq, cs_lambda_c}); + ad_fun_derivs = ADFun(fun_name+"_derivs", + ADSVector {cs_q,cs_v_int,cs_v,cs_tau}, + ADSVector {cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau, + cs_lambda_dq, cs_lambda_dv, cs_lambda_dtau}); + } + + template + void evalFunction(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau) + { + Eigen::Map(q_vec.data(),ad_model.nq,1) = q; + Eigen::Map(v_vec.data(),ad_model.nv,1) = v; + Eigen::Map(tau_vec.data(),ad_model.nv,1) = tau; + fun_output = fun(DMVector {q_vec, v_int_vec, v_vec, tau_vec}); + ddq = + Eigen::Map(static_cast< std::vector > + (fun_output[0]).data(),ad_model.nv,1); + lambda_c = + Eigen::Map(static_cast< std::vector > + (fun_output[1]).data(),nc,1); + } + + template + void evalJacobian(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau) + { + Eigen::Map(q_vec.data(),ad_model.nq,1) = q; + Eigen::Map(v_vec.data(),ad_model.nv,1) = v; + Eigen::Map(tau_vec.data(),ad_model.nv,1) = tau; + fun_output_derivs = fun_derivs (DMVector {q_vec,v_int_vec,v_vec,tau_vec}); + ddq_dq = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[0]).data(),ad_model.nv,ad_model.nv); + ddq_dv = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[1]).data(),ad_model.nv,ad_model.nv); + ddq_dtau = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[2]).data(),ad_model.nv,ad_model.nv); + dlambda_dq = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[3]).data(),nc,ad_model.nv); + dlambda_dv = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[4]).data(),nc,ad_model.nv); + dlambda_dtau = + Eigen::Map(static_cast< std::vector > + (fun_output_derivs[5]).data(),nc,ad_model.nv); + + } + + TangentVectorType ddq; + VectorXs lambda_c; + RowMatrixXs ddq_dq, ddq_dv, ddq_dtau, + dlambda_dq, dlambda_dv, dlambda_dtau; + + protected: + using Base::ad_model; + using Base::ad_data; + using Base::filename; + using Base::libname; + using Base::fun_name; + using Base::cg_generated; + + using Base::ad_fun; + using Base::ad_fun_derivs; + using Base::fun; + using Base::fun_derivs; + using Base::fun_output; + using Base::fun_output_derivs; + + ADContactModelVector ad_contact_models; + ADContactDataVector ad_contact_datas; + + Eigen::DenseIndex nc; + ADScalar cs_q, cs_v, cs_tau, cs_v_int, cs_ddq, cs_lambda_c; + + //Derivatives + ADScalar cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau, + cs_lambda_dq, cs_lambda_dv, cs_lambda_dtau; + + ADConfigVectorType q_ad, q_int_ad; + ADTangentVectorType v_ad, v_int_ad, tau_ad; + + std::vector q_vec, v_vec, v_int_vec, tau_vec; + }; + + + template + struct AutoDiffConstraintDynamicsDerivatives : public AutoDiffAlgoBase<_Scalar> + { + typedef AutoDiffAlgoBase<_Scalar> Base; + typedef typename Base::Scalar Scalar; + typedef typename Base::ADScalar ADScalar; + typedef typename Base::ADSVector ADSVector; + + typedef typename Base::TangentVectorType TangentVectorType; + typedef typename Base::RowMatrixXs RowMatrixXs; + typedef typename Base::VectorXs VectorXs; + typedef typename Base::MatrixXs MatrixXs; + + typedef typename Base::ADFun ADFun; + typedef typename Base::DMVector DMVector; + typedef typename Base::DMMatrix DMMatrix; + typedef typename Base::ADConfigVectorType ADConfigVectorType; + typedef typename Base::ADTangentVectorType ADTangentVectorType; + + + typedef typename pinocchio::RigidConstraintModelTpl ContactModel; + typedef Eigen::aligned_allocator ContactModelAllocator; + typedef typename std::vector ContactModelVector; + typedef typename pinocchio::RigidConstraintDataTpl ContactData; + typedef Eigen::aligned_allocator ContactDataAllocator; + typedef typename std::vector ContactDataVector; + + typedef typename pinocchio::RigidConstraintModelTpl ADContactModel; + typedef Eigen::aligned_allocator ADContactModelAllocator; + typedef typename std::vector ADContactModelVector; + typedef typename pinocchio::RigidConstraintDataTpl ADContactData; + typedef Eigen::aligned_allocator ADContactDataAllocator; + typedef typename std::vector ADContactDataVector; + + static Eigen::DenseIndex constraintDim(const ContactModelVector& contact_models) + { + Eigen::DenseIndex num_total_constraints = 0; + for(typename ContactModelVector::const_iterator it = contact_models.begin(); + it != contact_models.end(); + ++it) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(it->size() > 0, + "The dimension of the constraint must be positive"); + num_total_constraints += it->size(); + } + return num_total_constraints; + } + + explicit AutoDiffConstraintDynamicsDerivatives(const Model& model, + const ContactModelVector& contact_models, + const std::string& filename = "casadi_contactDynDerivs", + const std::string& libname = "libcasadi_cg_contactDynDerivs", + const std::string& fun_name = "eval_f") + : Base(model, filename, libname, fun_name) + , nc(constraintDim(contact_models)) + , cs_q(ADScalar::sym("q", model.nq)) + , cs_v(ADScalar::sym("v", model.nv)) + , cs_tau(ADScalar::sym("tau", model.nv)) + , q_ad(model.nq) + , v_ad(model.nv) + , tau_ad(model.nv) + , cs_ddq(model.nv,1) + , cs_lambda_c(model.nv,1) + , q_vec((size_t)model.nq) + , v_vec((size_t)model.nv) + , tau_vec((size_t)model.nv) + , ddq(model.nv) + , ddq_dq(model.nv,model.nv) + , ddq_dv(model.nv,model.nv) + , ddq_dtau(model.nv,model.nv) + { + lambda_c.resize(nc); lambda_c.setZero(); + dlambda_dq.resize(nc,model.nv); dlambda_dq.setZero(); + dlambda_dv.resize(nc,model.nv); dlambda_dv.setZero(); + dlambda_dtau.resize(nc,model.nv); dlambda_dtau.setZero(); + + q_ad = Eigen::Map(static_cast< std::vector >(cs_q).data(),model.nq,1); + v_ad = Eigen::Map(static_cast< std::vector >(cs_v).data(),model.nv,1); + tau_ad = Eigen::Map(static_cast< std::vector >(cs_tau).data(),model.nv,1); + + + for(int k=0;k()); + ad_contact_datas.push_back(ADContactData(ad_contact_models[k])); + } + + Base::build_jacobian = false; + + } + + virtual ~AutoDiffConstraintDynamicsDerivatives() {} + + + virtual void buildMap() + { + pinocchio::initConstraintDynamics(ad_model, ad_data, ad_contact_models); + pinocchio::constraintDynamics(ad_model,ad_data,q_ad,v_ad,tau_ad, + ad_contact_models,ad_contact_datas); + pinocchio::computeConstraintDynamicsDerivatives(ad_model,ad_data, + ad_contact_models,ad_contact_datas); + //Copy Output + pinocchio::casadi::copy(ad_data.ddq,cs_ddq); + pinocchio::casadi::copy(ad_data.lambda_c,cs_lambda_c); + pinocchio::casadi::copy(ad_data.ddq_dq,cs_ddq_dq); + pinocchio::casadi::copy(ad_data.ddq_dv,cs_ddq_dv); + pinocchio::casadi::copy(ad_data.ddq_dtau,cs_ddq_dtau); + pinocchio::casadi::copy(ad_data.dlambda_dq,cs_lambda_dq); + pinocchio::casadi::copy(ad_data.dlambda_dv,cs_lambda_dv); + pinocchio::casadi::copy(ad_data.dlambda_dtau,cs_lambda_dtau); + + ad_fun = ADFun(fun_name, + ADSVector {cs_q, cs_v, cs_tau}, + ADSVector {cs_ddq, cs_lambda_c, + cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau, + cs_lambda_dq, cs_lambda_dv, cs_lambda_dtau}); + } + + template + void evalFunction(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau) + { + Eigen::Map(q_vec.data(),ad_model.nq,1) = q; + Eigen::Map(v_vec.data(),ad_model.nv,1) = v; + Eigen::Map(tau_vec.data(),ad_model.nv,1) = tau; + fun_output = fun(DMVector {q_vec, v_vec, tau_vec}); + ddq = + Eigen::Map(static_cast< std::vector > + (fun_output[0]).data(),ad_model.nv,1); + lambda_c = + Eigen::Map(static_cast< std::vector > + (fun_output[1]).data(),nc,1); + ddq_dq = + Eigen::Map(static_cast< std::vector > + (fun_output[2]).data(),ad_model.nv,ad_model.nv); + ddq_dv = + Eigen::Map(static_cast< std::vector > + (fun_output[3]).data(),ad_model.nv,ad_model.nv); + ddq_dtau = + Eigen::Map(static_cast< std::vector > + (fun_output[4]).data(),ad_model.nv,ad_model.nv); + dlambda_dq = + Eigen::Map(static_cast< std::vector > + (fun_output[5]).data(),nc,ad_model.nv); + dlambda_dv = + Eigen::Map(static_cast< std::vector > + (fun_output[6]).data(),nc,ad_model.nv); + dlambda_dtau = + Eigen::Map(static_cast< std::vector > + (fun_output[7]).data(),nc,ad_model.nv); + } + + TangentVectorType ddq, lambda_c; + RowMatrixXs ddq_dq, ddq_dv, ddq_dtau, + dlambda_dq, dlambda_dv, dlambda_dtau; + + protected: + using Base::ad_model; + using Base::ad_data; + using Base::filename; + using Base::libname; + using Base::fun_name; + using Base::cg_generated; + + using Base::ad_fun; + using Base::ad_fun_derivs; + using Base::fun; + using Base::fun_derivs; + using Base::fun_output; + using Base::fun_output_derivs; + ADContactModelVector ad_contact_models; + ADContactDataVector ad_contact_datas; + + Eigen::DenseIndex nc; + ADScalar cs_q, cs_v, cs_tau, cs_ddq, cs_lambda_c; + + //Derivatives + ADScalar cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau, + cs_lambda_dq, cs_lambda_dv, cs_lambda_dtau; + + ADConfigVectorType q_ad; + ADTangentVectorType v_ad, tau_ad; + + std::vector q_vec, v_vec, tau_vec; + }; + + } //namespace casadi + +} // namespace pinocchio + +#endif // ifndef __pinocchio_autodiff_code_generator_algo_hpp__ diff --git a/src/autodiff/casadi.hpp b/src/autodiff/casadi.hpp index 972aa425a0..77733f216e 100644 --- a/src/autodiff/casadi.hpp +++ b/src/autodiff/casadi.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2019-2020 INRIA, CNRS +// Copyright (c) 2019-2021 INRIA CNRS // #ifndef __pinocchio_autodiff_casadi_hpp__ @@ -44,6 +44,7 @@ namespace pinocchio } }; + } namespace Eigen @@ -52,12 +53,12 @@ namespace Eigen { // Specialization of Eigen::internal::cast_impl for Casadi input types template - struct cast_impl + struct cast_impl<::casadi::Matrix,Scalar> { #if EIGEN_VERSION_AT_LEAST(3,2,90) EIGEN_DEVICE_FUNC #endif - static inline Scalar run(const casadi::SX & x) + static inline Scalar run(const ::casadi::Matrix & x) { return static_cast(x); } @@ -181,7 +182,7 @@ namespace pinocchio } // namespace casadi } // namespace pinocchio -// Overloading of max operator +// Overloading of min/max operator namespace pinocchio { namespace math @@ -189,7 +190,51 @@ namespace pinocchio namespace internal { template - struct return_type_max< ::casadi::Matrix,::casadi::Matrix> + struct return_type_min< ::casadi::Matrix,::casadi::Matrix > + { + typedef ::casadi::Matrix type; + }; + + template + struct return_type_min< ::casadi::Matrix,T> + { + typedef ::casadi::Matrix type; + }; + + template + struct return_type_min > + { + typedef ::casadi::Matrix type; + }; + + template + struct call_min< ::casadi::Matrix,::casadi::Matrix > + { + static inline ::casadi::Matrix run(const ::casadi::Matrix & a, + const ::casadi::Matrix & b) + { return fmin(a,b); } + }; + + template + struct call_min< ::casadi::Matrix,S2> + { + typedef ::casadi::Matrix CasadiType; + static inline ::casadi::Matrix run(const ::casadi::Matrix & a, + const S2 & b) + { return fmin(a,static_cast(b)); } + }; + + template + struct call_min > + { + typedef ::casadi::Matrix CasadiType; + static inline ::casadi::Matrix run(const S1 & a, + const ::casadi::Matrix & b) + { return fmin(static_cast(a),b); } + }; + + template + struct return_type_max< ::casadi::Matrix,::casadi::Matrix > { typedef ::casadi::Matrix type; }; @@ -224,7 +269,7 @@ namespace pinocchio }; template - struct call_max> + struct call_max > { typedef ::casadi::Matrix CasadiType; static inline ::casadi::Matrix run(const S1 & a, @@ -237,6 +282,74 @@ namespace pinocchio } // namespace pinocchio +namespace Eigen +{ + + template + struct ScalarBinaryOpTraits< ::casadi::Matrix,::casadi::Matrix, internal::scalar_max_op< ::casadi::Matrix,::casadi::Matrix > > + { + typedef ::casadi::Matrix ReturnType; + }; + + template + struct ScalarBinaryOpTraits< ::casadi::Matrix,S2, internal::scalar_max_op<::casadi::Matrix,S2> > + { + typedef ::casadi::Matrix ReturnType; + }; + + template + struct ScalarBinaryOpTraits< S1, ::casadi::Matrix, internal::scalar_max_op< S1,::casadi::Matrix > > + { + typedef ::casadi::Matrix ReturnType; + }; + + namespace internal + { + + template + struct scalar_max_op< ::casadi::Matrix,::casadi::Matrix > : binary_op_base< ::casadi::Matrix,::casadi::Matrix > + { + typedef ::casadi::Matrix LhsScalar; + typedef ::casadi::Matrix RhsScalar; + typedef typename ScalarBinaryOpTraits::ReturnType result_type; + + EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const + { return ::pinocchio::math::internal::call_max::run(a,b); } +// template +// EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const +// { return internal::pmax(a,b); } +// template +// EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type predux(const Packet& a) const +// { return internal::predux_max(a); } + }; + + template + struct scalar_max_op< S1,::casadi::Matrix > : binary_op_base< S1,::casadi::Matrix > + { + typedef S1 LhsScalar; + typedef ::casadi::Matrix RhsScalar; + typedef typename ScalarBinaryOpTraits::ReturnType result_type; + + EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const + { return ::pinocchio::math::internal::call_max::run(a,b); } + }; + + template + struct scalar_max_op< ::casadi::Matrix,S2 > : binary_op_base< ::casadi::Matrix,S2 > + { + typedef ::casadi::Matrix LhsScalar; + typedef S2 RhsScalar; + typedef typename ScalarBinaryOpTraits::ReturnType result_type; + + EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op) + EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const + { return ::pinocchio::math::internal::call_max::run(a,b); } + }; + } +} + #include "pinocchio/autodiff/casadi/spatial/se3-tpl.hpp" #include "pinocchio/autodiff/casadi/utils/static-if.hpp" #include "pinocchio/autodiff/casadi/math/matrix.hpp" diff --git a/src/autodiff/casadi/math/quaternion.hpp b/src/autodiff/casadi/math/quaternion.hpp index bf3a60c97f..fa95fc5b04 100644 --- a/src/autodiff/casadi/math/quaternion.hpp +++ b/src/autodiff/casadi/math/quaternion.hpp @@ -64,4 +64,21 @@ namespace pinocchio } // namespace pinocchio +namespace Eigen +{ + namespace internal + { + template + struct quaternionbase_assign_impl,3,3,Options>,3,3 > + { + template + EIGEN_DEVICE_FUNC static inline void + run(QuaternionBase & q, const Eigen::Matrix<::casadi::Matrix,3,3,Options> & a_mat) + { + ::pinocchio::quaternion::internal::quaternionbase_assign_impl<::casadi::Matrix, false>::run(q,a_mat); + } + }; + } +} + #endif // ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__ diff --git a/src/autodiff/casadi/utils/static-if.hpp b/src/autodiff/casadi/utils/static-if.hpp index f0f53aa637..ec978d27c1 100644 --- a/src/autodiff/casadi/utils/static-if.hpp +++ b/src/autodiff/casadi/utils/static-if.hpp @@ -45,7 +45,34 @@ namespace pinocchio } } }; + + + template + struct comparison_eq_impl<::casadi::Matrix,::casadi::Matrix > + { + typedef ::casadi::Matrix CasadiType; + + static inline bool run(const CasadiType & lhs_value, + const CasadiType & rhs_value) + { + return (lhs_value == rhs_value).is_zero(); + } + }; + } // namespace internal } // namespace pinocchio +namespace std { + + template + struct equal_to<::casadi::Matrix > + { + bool operator() (const ::casadi::Matrix & lhs_value, + const ::casadi::Matrix & rhs_value) const + { + return (lhs_value == rhs_value).is_zero(); + } + }; +} + #endif // ifndef __pinocchio_autodiff_casadi_utils_static_if_hpp__ diff --git a/src/autodiff/cppad.hpp b/src/autodiff/cppad.hpp index cb0167c51f..d946ffd3a7 100644 --- a/src/autodiff/cppad.hpp +++ b/src/autodiff/cppad.hpp @@ -32,7 +32,7 @@ namespace boost typedef CppAD::AD ADScalar; template - static inline ADScalar get(const mpl::int_& n) + static inline ADScalar get(const mpl::int_ & n) { return ADScalar(constant_pi::get(n)); } diff --git a/src/autodiff/cppad/math/quaternion.hpp b/src/autodiff/cppad/math/quaternion.hpp index 9b33f4b1de..996e73c3d0 100644 --- a/src/autodiff/cppad/math/quaternion.hpp +++ b/src/autodiff/cppad/math/quaternion.hpp @@ -42,8 +42,8 @@ namespace pinocchio for(Eigen::DenseIndex k = 0; k < 4; ++k) { ADScalar t_is_negative_cond1 = CppAD::CondExpGt(mat.coeff(1,1), mat.coeff(0,0), - quat_t_negative_1.coeffs().coeff(k), - quat_t_negative_0.coeffs().coeff(k)); + quat_t_negative_1.coeffs().coeff(k), + quat_t_negative_0.coeffs().coeff(k)); ADScalar t_is_negative_cond2 = CppAD::CondExpGt(mat.coeff(2,2), mat.coeff(0,0), quat_t_negative_2.coeffs().coeff(k), diff --git a/src/codegen/code-generator-algo.hpp b/src/codegen/code-generator-algo.hpp index 72a2ac05a7..dd5c8f9d36 100644 --- a/src/codegen/code-generator-algo.hpp +++ b/src/codegen/code-generator-algo.hpp @@ -12,6 +12,8 @@ #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/aba.hpp" #include "pinocchio/algorithm/rnea-derivatives.hpp" +#include "pinocchio/algorithm/contact-dynamics-derivatives.hpp" +#include "pinocchio/algorithm/contact-dynamics.hpp" #include "pinocchio/algorithm/aba-derivatives.hpp" namespace pinocchio @@ -43,6 +45,8 @@ namespace pinocchio dtau_dv = MatrixXs::Zero(model.nv,model.nv); dtau_da = MatrixXs::Zero(model.nv,model.nv); } + + virtual ~CodeGenRNEA() {} void buildMap() { @@ -143,6 +147,8 @@ namespace pinocchio da_dtau = MatrixXs::Zero(model.nv,model.nv); } + virtual ~CodeGenABA() {} + void buildMap() { CppAD::Independent(ad_X); @@ -239,6 +245,8 @@ namespace pinocchio Base::build_jacobian = false; } + + virtual ~CodeGenCRBA() {} void buildMap() { @@ -328,6 +336,8 @@ namespace pinocchio Base::build_jacobian = false; } + + virtual ~CodeGenMinv() {} void buildMap() { @@ -416,7 +426,6 @@ namespace pinocchio ad_a = ADTangentVectorType(model.nv); ad_a.setZero(); x = VectorXs::Zero(Base::getInputDimension()); - partial_derivatives = VectorXs::Zero(Base::getOutputDimension()); ad_dtau_dq = ADMatrixXs::Zero(model.nv,model.nv); ad_dtau_dv = ADMatrixXs::Zero(model.nv,model.nv); @@ -428,6 +437,8 @@ namespace pinocchio Base::build_jacobian = false; } + + virtual ~CodeGenRNEADerivatives() {} void buildMap() { @@ -464,8 +475,8 @@ namespace pinocchio // fill x Eigen::DenseIndex it_x = 0; x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq; - x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv; - x.segment(it_x,ad_model.nq) = a; it_x += ad_model.nv; + x.segment(it_x,ad_model.nv) = v; it_x += ad_model.nv; + x.segment(it_x,ad_model.nv) = a; it_x += ad_model.nv; Base::evalFunction(x); @@ -490,7 +501,6 @@ namespace pinocchio using Base::y; VectorXs x; - VectorXs partial_derivatives; ADMatrixXs ad_dtau_dq, ad_dtau_dv, ad_dtau_da; MatrixXs dtau_dq, dtau_dv, dtau_da; @@ -525,7 +535,6 @@ namespace pinocchio ad_tau = ADTangentVectorType(model.nv); ad_tau.setZero(); x = VectorXs::Zero(Base::getInputDimension()); - partial_derivatives = VectorXs::Zero(Base::getOutputDimension()); ad_dddq_dq = ADMatrixXs::Zero(model.nv,model.nv); ad_dddq_dv = ADMatrixXs::Zero(model.nv,model.nv); @@ -537,6 +546,8 @@ namespace pinocchio Base::build_jacobian = false; } + + virtual ~CodeGenABADerivatives() {} void buildMap() { @@ -560,11 +571,11 @@ namespace pinocchio it_Y += ad_model.nv*ad_model.nv; Eigen::Map(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dtau; it_Y += ad_model.nv*ad_model.nv; - + ad_fun.Dependent(ad_X,ad_Y); ad_fun.optimize("no_compare_op"); } - + template void evalFunction(const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, @@ -573,8 +584,8 @@ namespace pinocchio // fill x Eigen::DenseIndex it_x = 0; x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq; - x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv; - x.segment(it_x,ad_model.nq) = tau; it_x += ad_model.nv; + x.segment(it_x,ad_model.nv) = v; it_x += ad_model.nv; + x.segment(it_x,ad_model.nv) = tau; it_x += ad_model.nv; Base::evalFunction(x); @@ -586,7 +597,6 @@ namespace pinocchio it_y += ad_model.nv*ad_model.nv; dddq_dtau = Eigen::Map(Base::y.data()+it_y,ad_model.nv,ad_model.nv); it_y += ad_model.nv*ad_model.nv; - } protected: @@ -599,7 +609,6 @@ namespace pinocchio using Base::y; VectorXs x; - VectorXs partial_derivatives; ADMatrixXs ad_dddq_dq, ad_dddq_dv, ad_dddq_dtau; MatrixXs dddq_dq, dddq_dv, dddq_dtau; @@ -607,6 +616,342 @@ namespace pinocchio ADTangentVectorType ad_v, ad_tau; }; + template + struct CodeGenConstraintDynamicsDerivatives : public CodeGenBase<_Scalar> + { + typedef CodeGenBase<_Scalar> Base; + typedef typename Base::Scalar Scalar; + typedef typename Base::ADScalar ADScalar; + + typedef typename Base::Model Model; + + typedef typename Base::ADConfigVectorType ADConfigVectorType; + typedef typename Base::ADTangentVectorType ADTangentVectorType; + typedef typename Base::MatrixXs MatrixXs; + typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs; + typedef typename Base::VectorXs VectorXs; + + typedef typename pinocchio::RigidConstraintModelTpl ContactModel; + typedef Eigen::aligned_allocator ContactModelAllocator; + typedef typename std::vector ContactModelVector; + typedef typename pinocchio::RigidConstraintDataTpl ContactData; + typedef Eigen::aligned_allocator ContactDataAllocator; + typedef typename std::vector ContactDataVector; + + typedef typename pinocchio::RigidConstraintModelTpl ADContactModel; + typedef Eigen::aligned_allocator ADContactModelAllocator; + typedef typename std::vector ADContactModelVector; + typedef typename pinocchio::RigidConstraintDataTpl ADContactData; + typedef Eigen::aligned_allocator ADContactDataAllocator; + typedef typename std::vector ADContactDataVector; + + typedef typename Base::ADData ADData; + typedef typename ADData::MatrixXs ADMatrixXs; + typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs; + + Eigen::DenseIndex constraintDim(const ContactModelVector& contact_models) const + { + Eigen::DenseIndex num_total_constraints = 0; + for(typename ContactModelVector::const_iterator it = contact_models.begin(); + it != contact_models.end(); + ++it) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(it->size() > 0, + "The dimension of the constraint must be positive"); + num_total_constraints += it->size(); + } + return num_total_constraints; + } + + + CodeGenConstraintDynamicsDerivatives(const Model & model, + const ContactModelVector& contact_models, + const std::string & function_name = "partial_constraintDynamics", + const std::string & library_name = "cg_partial_constraintDynamics_eval") + : Base(model,model.nq+2*model.nv, + 3*model.nv*model.nv + 3*constraintDim(contact_models)*model.nv, + function_name,library_name), + nc(constraintDim(contact_models)), + dddq_dq(model.nv, model.nv), + dddq_dv(model.nv, model.nv), + dddq_dtau(model.nv, model.nv) + { + dlambda_dq.resize(nc, model.nv); dlambda_dq.setZero(); + dlambda_dv.resize(nc, model.nv); dlambda_dv.setZero(); + dlambda_dtau.resize(nc, model.nv); dlambda_dtau.setZero(); + + ad_q = ADConfigVectorType(model.nq); ad_q = neutral(ad_model); + ad_v = ADTangentVectorType(model.nv); ad_v.setZero(); + ad_tau = ADTangentVectorType(model.nv); ad_tau.setZero(); + + x = VectorXs::Zero(Base::getInputDimension()); + + for(int k=0;k()); + } + + for(int k=0;k(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_data.ddq_dq; + it_Y += ad_model.nv*ad_model.nv; + Eigen::Map(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_data.ddq_dv; + it_Y += ad_model.nv*ad_model.nv; + Eigen::Map(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_data.ddq_dtau; + it_Y += ad_model.nv*ad_model.nv; + Eigen::Map(ad_Y.data()+it_Y,nc,ad_model.nv) = ad_data.dlambda_dq; + it_Y += nc*ad_model.nv; + Eigen::Map(ad_Y.data()+it_Y,nc,ad_model.nv) = ad_data.dlambda_dv; + it_Y += nc*ad_model.nv; + Eigen::Map(ad_Y.data()+it_Y,nc,ad_model.nv) = ad_data.dlambda_dtau; + it_Y += nc*ad_model.nv; + ad_fun.Dependent(ad_X,ad_Y); + ad_fun.optimize("no_compare_op"); + } + + template + void evalFunction(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau) + { + // fill x + Eigen::DenseIndex it_x = 0; + x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq; + x.segment(it_x,ad_model.nv) = v; it_x += ad_model.nv; + x.segment(it_x,ad_model.nv) = tau; it_x += ad_model.nv; + + Base::evalFunction(x); + + // fill partial derivatives + Eigen::DenseIndex it_y = 0; + dddq_dq = Eigen::Map(Base::y.data()+it_y,ad_model.nv,ad_model.nv); + it_y += ad_model.nv*ad_model.nv; + dddq_dv = Eigen::Map(Base::y.data()+it_y,ad_model.nv,ad_model.nv); + it_y += ad_model.nv*ad_model.nv; + dddq_dtau = Eigen::Map(Base::y.data()+it_y,ad_model.nv,ad_model.nv); + it_y += ad_model.nv*ad_model.nv; + dlambda_dq = Eigen::Map(Base::y.data()+it_y,nc,ad_model.nv); + it_y += nc*ad_model.nv; + dlambda_dv = Eigen::Map(Base::y.data()+it_y,nc,ad_model.nv); + it_y += nc*ad_model.nv; + dlambda_dtau = Eigen::Map(Base::y.data()+it_y,nc,ad_model.nv); + it_y += nc*ad_model.nv; + } + + protected: + + using Base::ad_model; + using Base::ad_data; + using Base::ad_fun; + using Base::ad_X; + using Base::ad_Y; + using Base::y; + + Eigen::DenseIndex nc; + ADContactModelVector ad_contact_models; + ADContactDataVector ad_contact_datas; + + VectorXs x; + MatrixXs dddq_dq, dddq_dv, dddq_dtau; + MatrixXs dlambda_dq, dlambda_dv, dlambda_dtau; + + + ADConfigVectorType ad_q; + ADTangentVectorType ad_v, ad_tau; + }; + + template + struct CodeGenConstraintDynamics : public CodeGenBase<_Scalar> + { + typedef CodeGenBase<_Scalar> Base; + typedef typename Base::Scalar Scalar; + typedef typename Base::ADScalar ADScalar; + + typedef typename Base::Model Model; + + typedef typename Base::ADConfigVectorType ADConfigVectorType; + typedef typename Base::ADTangentVectorType ADTangentVectorType; + typedef typename Base::MatrixXs MatrixXs; + typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs; + typedef typename Base::VectorXs VectorXs; + + typedef typename pinocchio::RigidConstraintModelTpl ContactModel; + typedef Eigen::aligned_allocator ContactModelAllocator; + typedef typename std::vector ContactModelVector; + typedef typename pinocchio::RigidConstraintDataTpl ContactData; + typedef Eigen::aligned_allocator ContactDataAllocator; + typedef typename std::vector ContactDataVector; + + typedef typename pinocchio::RigidConstraintModelTpl ADContactModel; + typedef Eigen::aligned_allocator ADContactModelAllocator; + typedef typename std::vector ADContactModelVector; + typedef typename pinocchio::RigidConstraintDataTpl ADContactData; + typedef Eigen::aligned_allocator ADContactDataAllocator; + typedef typename std::vector ADContactDataVector; + + typedef typename Base::ADData ADData; + typedef typename ADData::MatrixXs ADMatrixXs; + typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs; + + Eigen::DenseIndex constraintDim(const ContactModelVector& contact_models) const + { + Eigen::DenseIndex num_total_constraints = 0; + for(typename ContactModelVector::const_iterator it = contact_models.begin(); + it != contact_models.end(); + ++it) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT(it->size() > 0, + "The dimension of the constraint must be positive"); + num_total_constraints += it->size(); + } + return num_total_constraints; + } + + CodeGenConstraintDynamics(const Model & model, + const ContactModelVector& contact_models, + const std::string & function_name = "constraintDynamics", + const std::string & library_name = "cg_constraintDynamics_eval") + : Base(model,model.nq+2*model.nv, + model.nv + constraintDim(contact_models), + function_name,library_name), + nc(constraintDim(contact_models)), + da_dq(MatrixXs::Zero(model.nv,model.nq)), + da_dv(MatrixXs::Zero(model.nv,model.nv)), + da_dtau(MatrixXs::Zero(model.nv,model.nv)), + ddq(model.nv) + { + lambda_c.resize(nc); lambda_c.setZero(); + dlambda_dq.resize(nc, model.nq); dlambda_dq.setZero(); + dlambda_dv.resize(nc, model.nv); dlambda_dv.setZero(); + dlambda_dtau.resize(nc,model.nv); dlambda_dtau.setZero(); + + + ad_q = ADConfigVectorType(model.nq); ad_q = neutral(ad_model); + ad_v = ADTangentVectorType(model.nv); ad_v.setZero(); + ad_tau = ADTangentVectorType(model.nv); ad_tau.setZero(); + + x = VectorXs::Zero(Base::getInputDimension()); + + for(int k=0;k()); + } + + for(int k=0;k + void evalFunction(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau) + { + // fill x + Eigen::DenseIndex it_x = 0; + x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq; + x.segment(it_x,ad_model.nv) = v; it_x += ad_model.nv; + x.segment(it_x,ad_model.nv) = tau; it_x += ad_model.nv; + + Base::evalFunction(x); + + Eigen::DenseIndex it_y = 0; + ddq = Base::y.segment(it_y, ad_model.nv); + it_y += ad_model.nv; + lambda_c = Base::y.segment(it_y, nc); + } + + using Base::evalJacobian; + template + void evalJacobian(const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & a) + { + // fill x + Eigen::DenseIndex it = 0; + x.segment(it,ad_model.nq) = q; it += ad_model.nq; + x.segment(it,ad_model.nv) = v; it += ad_model.nv; + x.segment(it,ad_model.nv) = a; it += ad_model.nv; + + evalJacobian(x); + it = 0; + da_dq = Base::jac.middleCols(it,ad_model.nq).topRows(ad_model.nv); + dlambda_dq = Base::jac.middleCols(it,ad_model.nq).bottomRows(nc); it += ad_model.nq; + da_dv = Base::jac.middleCols(it,ad_model.nv).topRows(ad_model.nv); + dlambda_dv = Base::jac.middleCols(it,ad_model.nv).bottomRows(nc); it += ad_model.nv; + da_dtau = Base::jac.middleCols(it,ad_model.nv).topRows(ad_model.nv); + dlambda_dtau = Base::jac.middleCols(it,ad_model.nv).bottomRows(nc); it += ad_model.nv; + } + VectorXs lambda_c, ddq; + MatrixXs da_dq,da_dv,da_dtau; + MatrixXs dlambda_dq,dlambda_dv,dlambda_dtau; + + protected: + + using Base::ad_model; + using Base::ad_data; + using Base::ad_fun; + using Base::ad_X; + using Base::ad_Y; + using Base::y; + + ADContactModelVector ad_contact_models; + ADContactDataVector ad_contact_datas; + + Eigen::DenseIndex nc; + VectorXs x; + + ADConfigVectorType ad_q; + ADTangentVectorType ad_v, ad_tau; + }; + + + template struct CodeGenIntegrate : public CodeGenBase<_Scalar> { diff --git a/src/codegen/code-generator-base.hpp b/src/codegen/code-generator-base.hpp index a996afcc15..1de5711a5d 100644 --- a/src/codegen/code-generator-base.hpp +++ b/src/codegen/code-generator-base.hpp @@ -62,6 +62,8 @@ namespace pinocchio jac = RowMatrixXs(ad_Y.size(),ad_X.size()); } + + virtual ~CodeGenBase() {} /// \brief build the mapping Y = f(X) virtual void buildMap() = 0; diff --git a/src/codegen/cppadcg.hpp b/src/codegen/cppadcg.hpp index 25c6519d2e..3e4e61cb88 100644 --- a/src/codegen/cppadcg.hpp +++ b/src/codegen/cppadcg.hpp @@ -32,7 +32,7 @@ namespace boost typedef CppAD::cg::CG CGScalar; template - static inline CGScalar get(const mpl::int_& n) + static inline CGScalar get(const mpl::int_ & n) { return CGScalar(constant_pi::get(n)); } diff --git a/src/container/aligned-vector.hpp b/src/container/aligned-vector.hpp index a6fe4cb5e7..88eda3a6d3 100644 --- a/src/container/aligned-vector.hpp +++ b/src/container/aligned-vector.hpp @@ -12,6 +12,8 @@ ::pinocchio::container::aligned_vector // std::vector > +#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T) std::vector > + namespace pinocchio { namespace container diff --git a/src/eigen-macros.hpp b/src/eigen-macros.hpp index 27473c7795..035a47db80 100644 --- a/src/eigen-macros.hpp +++ b/src/eigen-macros.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2017-2018 CNRS +// Copyright (c) 2017-2020 CNRS INRIA // #ifndef __pinocchio_eigen_macros_hpp__ @@ -47,4 +47,16 @@ Eigen::internal::scalar_product_traits::S #define PINOCCHIO_WITH_EIGEN_TENSOR_MODULE #endif +/// \brief Check memory allocation for Eigen + +#ifdef PINOCCHIO_EIGEN_CHECK_MALLOC + #define PINOCCHIO_EIGEN_MALLOC(allowed) ::Eigen::internal::set_is_malloc_allowed(allowed) + #define PINOCCHIO_EIGEN_MALLOC_ALLOWED() PINOCCHIO_EIGEN_MALLOC(true) + #define PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED() PINOCCHIO_EIGEN_MALLOC(false) +#else + #define PINOCCHIO_EIGEN_MALLOC(allowed) + #define PINOCCHIO_EIGEN_MALLOC_ALLOWED() + #define PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED() +#endif + #endif // ifndef __pinocchio_eigen_macros_hpp__ diff --git a/src/fwd.hpp b/src/fwd.hpp index 08a996c1b6..0149507ff3 100644 --- a/src/fwd.hpp +++ b/src/fwd.hpp @@ -21,11 +21,31 @@ namespace pinocchio {} #include "pinocchio/utils/helpers.hpp" #include "pinocchio/utils/cast.hpp" +#include "pinocchio/utils/check.hpp" #include "pinocchio/container/boost-container-limits.hpp" -// Import Eigen and all the required modules +#define PINOCCHIO_SCALAR_TYPE_DEFAULT double +#ifndef PINOCCHIO_SCALAR_TYPE +#define PINOCCHIO_SCALAR_TYPE PINOCCHIO_SCALAR_TYPE_DEFAULT +#endif + +#ifdef PINOCCHIO_EIGEN_CHECK_MALLOC + #ifndef EIGEN_RUNTIME_NO_MALLOC + #define EIGEN_RUNTIME_NO_MALLOC_WAS_NOT_DEFINED + #define EIGEN_RUNTIME_NO_MALLOC + #endif +#endif + #include + +#ifdef PINOCCHIO_EIGEN_CHECK_MALLOC + #ifdef EIGEN_RUNTIME_NO_MALLOC_WAS_NOT_DEFINED + #undef EIGEN_RUNTIME_NO_MALLOC + #undef EIGEN_RUNTIME_NO_MALLOC_WAS_NOT_DEFINED + #endif +#endif + #include "pinocchio/eigen-macros.hpp" #ifdef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE #include @@ -47,6 +67,12 @@ namespace pinocchio { template struct traits {}; } + + template + struct NumericalBase + { + typedef typename traits::Scalar Scalar; + }; /// /// \brief Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type. diff --git a/src/macros.hpp b/src/macros.hpp index 802b4d5f78..a6f842f74c 100644 --- a/src/macros.hpp +++ b/src/macros.hpp @@ -93,6 +93,7 @@ namespace pinocchio // Handle explicitely the GCC boring warning: 'anonymous variadic macros were introduced in C++11' #include #include +#include #if defined(__GNUC__) #pragma GCC system_header diff --git a/src/math/matrix.hpp b/src/math/matrix.hpp index c48e1dcd7a..e0947259d6 100644 --- a/src/math/matrix.hpp +++ b/src/math/matrix.hpp @@ -7,8 +7,8 @@ #include "pinocchio/macros.hpp" #include "pinocchio/math/fwd.hpp" +#include "pinocchio/utils/static-if.hpp" -#include #include namespace pinocchio @@ -192,6 +192,44 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike); return internal::isNormalizedAlgo::run(vec,prec); } + + namespace internal + { + template::value> + struct normalizeAlgo + { + static void run(const Eigen::MatrixBase & vec) + { + return vec.const_cast_derived().normalize(); + } + }; + + template + struct normalizeAlgo + { + static void run(const Eigen::MatrixBase & vec) + { + using namespace internal; + typedef typename VectorLike::RealScalar RealScalar; + typedef typename VectorLike::Scalar Scalar; + const RealScalar z = vec.squaredNorm(); + const Scalar sqrt_z = if_then_else(GT,z,Scalar(0),math::sqrt(z),Scalar(1)); + vec.const_cast_derived() /= sqrt_z; + } + }; + } + + /// + /// \brief Normalize the input vector. + /// + /// \param[in] vec Input vector + /// + template + inline void normalize(const Eigen::MatrixBase & vec) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike); + internal::normalizeAlgo::run(vec.const_cast_derived()); + } namespace internal { diff --git a/src/math/multiprecision.hpp b/src/math/multiprecision.hpp index df21dbbf8e..ae9e6c518b 100644 --- a/src/math/multiprecision.hpp +++ b/src/math/multiprecision.hpp @@ -183,8 +183,12 @@ namespace Eigen struct conj_retval; template +#if EIGEN_VERSION_AT_LEAST(3,3,9) + struct conj_default_impl; +#else struct conj_impl; - +#endif + template struct conj_retval > { @@ -192,7 +196,11 @@ namespace Eigen }; template +#if EIGEN_VERSION_AT_LEAST(3,3,9) + struct conj_default_impl, true> +#else struct conj_impl, true> +#endif { #if EIGEN_VERSION_AT_LEAST(3,2,90) EIGEN_DEVICE_FUNC diff --git a/src/math/quaternion.hpp b/src/math/quaternion.hpp index 218bb1d9f0..7b023507d5 100644 --- a/src/math/quaternion.hpp +++ b/src/math/quaternion.hpp @@ -228,6 +228,52 @@ namespace pinocchio { return pinocchio::isNormalized(quat.coeffs(),prec); } + + /// + /// \brief Normalize the input quaternion. + /// + /// \param[in] quat Input quaternion + /// + template + inline void normalize(const Eigen::QuaternionBase & quat) + { + return pinocchio::normalize(quat.const_cast_derived().coeffs()); + } + + /// + /// \returns the spherical linear interpolation between the two quaternions + /// + /// \param[in] u Interpolation factor + /// \param[in] quat Input quaternion + /// + template + inline void slerp(const Scalar & u, + const Eigen::QuaternionBase & quat0, + const Eigen::QuaternionBase & quat1, + const Eigen::QuaternionBase & res) + { + const Scalar one = Scalar(1) - Eigen::NumTraits::epsilon(); + const Scalar d = quat0.dot(quat1); + const Scalar absD = fabs(d); + + const Scalar theta = acos(absD); + const Scalar sinTheta = sin(theta); + + using namespace pinocchio::internal; + + const Scalar scale0 = if_then_else(pinocchio::internal::GE,absD,one, + Scalar(1) - u, // then + sin( ( Scalar(1) - u ) * theta) / sinTheta // else + ); + + const Scalar scale1_factor = if_then_else(pinocchio::internal::LT,d,Scalar(0),Scalar(-1),Scalar(1)); + const Scalar scale1 = if_then_else(pinocchio::internal::GE,absD,one, + u, // then + sin( ( u * theta) ) / sinTheta // else + ) * scale1_factor; + + PINOCCHIO_EIGEN_CONST_CAST(QuaternionOut,res.derived()).coeffs() = scale0 * quat0.coeffs() + scale1 * quat1.coeffs(); + } } // namespace quaternion diff --git a/src/math/rotation.hpp b/src/math/rotation.hpp index 8099765acd..93c8b47b60 100644 --- a/src/math/rotation.hpp +++ b/src/math/rotation.hpp @@ -7,6 +7,7 @@ #include "pinocchio/fwd.hpp" #include "pinocchio/math/matrix.hpp" +#include "pinocchio/math/sincos.hpp" #include #include @@ -14,6 +15,7 @@ namespace pinocchio { + /// /// \brief Computes a rotation matrix from a vector and values of sin and cos /// orientations values. @@ -50,6 +52,21 @@ namespace pinocchio res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value; } + /// + /// \brief Computes a rotation matrix from a vector and the angular value + /// orientations values. + /// + /// \remarks This code is issue from Eigen::AxisAngle::toRotationMatrix + /// + template + void toRotationMatrix(const Eigen::MatrixBase & axis, + const Scalar & angle, + const Eigen::MatrixBase & res) + { + Scalar sa, ca; SINCOS(angle,&sa,&ca); + toRotationMatrix(axis,ca,sa,PINOCCHIO_EIGEN_CONST_CAST(Matrix3,res)); + } + /// /// \brief Orthogonormalization procedure for a rotation matrix (closed enough to SO(3)). /// @@ -64,7 +81,7 @@ namespace pinocchio typedef typename Matrix3::Scalar Scalar; enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options }; typedef Eigen::Quaternion Quaternion; - Quaternion quat(rot); quat.normalize(); + Quaternion quat(rot); normalize(quat.coeffs()); rot_ = quat.toRotationMatrix(); } diff --git a/src/multibody/constraint.hpp b/src/multibody/constraint.hpp deleted file mode 100644 index f14bec4042..0000000000 --- a/src/multibody/constraint.hpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// Copyright (c) 2015-2019 CNRS, INRIA -// - -#ifndef __pinocchio_multibody_constraint_hpp__ -#define __pinocchio_multibody_constraint_hpp__ - -#include "pinocchio/macros.hpp" - -namespace pinocchio -{ - - template struct ConstraintTpl; - - typedef ConstraintTpl<1,double,0> Constraint1d; - typedef ConstraintTpl<3,double,0> Constraint3d; - typedef ConstraintTpl<6,double,0> Constraint6d; - typedef ConstraintTpl ConstraintXd; - -} // namespace pinocchio - -#include "pinocchio/multibody/constraint-base.hpp" -#include "pinocchio/multibody/constraint-generic.hpp" - -#endif // ifndef __pinocchio_multibody_constraint_hpp__ - diff --git a/src/multibody/data.hpp b/src/multibody/data.hpp index 9fd22f5152..1401ca78b6 100644 --- a/src/multibody/data.hpp +++ b/src/multibody/data.hpp @@ -16,18 +16,25 @@ #include "pinocchio/multibody/fwd.hpp" #include "pinocchio/multibody/joint/joint-generic.hpp" #include "pinocchio/container/aligned-vector.hpp" +#include "pinocchio/algorithm/contact-cholesky.hpp" #include "pinocchio/serialization/serializable.hpp" -#include #include namespace pinocchio { + + template class JointCollectionTpl> + struct traits< DataTpl<_Scalar,_Options,JointCollectionTpl> > + { + typedef _Scalar Scalar; + }; template class JointCollectionTpl> struct DataTpl : serialization::Serializable< DataTpl<_Scalar,_Options,JointCollectionTpl> > + , NumericalBase< DataTpl<_Scalar,_Options,JointCollectionTpl> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -82,40 +89,54 @@ namespace pinocchio /// \brief The type of Tensor for Kinematics and Dynamics second order derivatives typedef Tensor Tensor3x; + typedef cholesky::ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; + /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, /// encapsulated in JointDataAccessor. JointDataVector joints; - /// \brief Vector of joint accelerations expressed at the centers of the joints frames. + /// \brief Vector of joint accelerations expressed in the local frame of the joint. PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a; /// \brief Vector of joint accelerations expressed at the origin of the world. PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa; + /// \brief Vector of joint accelerations expressed at the origin of the world. + /// These accelerations are used in the context of augmented Lagrangian algorithms. + PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_drift; + + /// \brief Vector of joint accelerations expressed at the origin of the world. + /// These accelerations are used in the context of augmented Lagrangian algorithms. + PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_augmented; + /// \brief Vector of joint accelerations due to the gravity field. PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf; - /// \brief Vector of joint accelerations expressed at the origin of the world including gravity contribution. + /// \brief Vector of joint accelerations expressed at the origin of the world including the gravity contribution. PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf; - /// \brief Vector of joint velocities expressed at the centers of the joints. + /// \brief Vector of joint velocities expressed in the local frame of the joint. PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v; - /// \brief Vector of joint velocities expressed at the origin. + /// \brief Vector of joint velocities expressed at the origin of the world. PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov; /// \brief Vector of body forces expressed in the local frame of the joint. For each body, the force represents the sum of /// all external forces acting on the body. PINOCCHIO_ALIGNED_STD_VECTOR(Force) f; - /// \brief Vector of body forces expressed in the world frame. For each body, the force represents the sum of + /// \brief Vector of body forces expressed at the origin of the world. For each body, the force represents the sum of /// all external forces acting on the body. PINOCCHIO_ALIGNED_STD_VECTOR(Force) of; + /// \brief Vector of body forces expressed in the world frame. For each body, the force represents the sum of + /// all external forces acting on the body. These forces are used in the context of augmented Lagrangian algorithms. + PINOCCHIO_ALIGNED_STD_VECTOR(Force) of_augmented; + /// \brief Vector of spatial momenta expressed in the local frame of the joint. PINOCCHIO_ALIGNED_STD_VECTOR(Force) h; - /// \brief Vector of spatial momenta expressed in the world frame. + /// \brief Vector of spatial momenta expressed at the origin of the world. PINOCCHIO_ALIGNED_STD_VECTOR(Force) oh; /// \brief Vector of absolute joint placements (wrt the world). @@ -156,7 +177,7 @@ namespace pinocchio /// \brief The Coriolis matrix (a square matrix of dim model.nv). MatrixXs C; - /// \brief Variation of the spatial momenta with respect to the joint configuration. + /// \brief Variation of the spatial momenta set with respect to the joint configuration. Matrix6x dHdq; /// \brief Variation of the forceset with respect to the joint configuration. @@ -182,7 +203,7 @@ namespace pinocchio /// \brief Left variation of the inertia matrix PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx; - + /// \brief Rigid Body Inertia supported by the joint expressed in the world frame PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias; @@ -192,21 +213,19 @@ namespace pinocchio /// \brief Time variation of Composite Rigid Body Inertia expressed in the world frame PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb; - /// \brief Temporary for derivative algorithms - Matrix6 Itmp; - - /// \brief Temporary for derivative algorithms - Matrix6 M6tmp; - RowMatrix6 M6tmpR; - RowMatrix6 M6tmpR2; - - /// \brief The joint accelerations computed from ABA + /// \brief The joint accelerations computed by ABA TangentVectorType ddq; // ABA internal data - /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA] + /// \brief Articulated Body Inertia matrix of the subtree expressed in the LOCAL coordinate frame of the joint PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6 + /// \brief Articulated Body Inertia matrix of the subtree expressed in the WORLD coordinate frame + PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oYaba; // TODO: change with dense symmetric matrix6 + + /// \brief Articulated Body Inertia matrix with contact apparent inertia, of a given the subtree and expressed in the WORLD coordinate frame + PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oYaba_contact; // TODO: change with dense symmetric matrix6 + /// \brief Intermediate quantity corresponding to apparent torque [ABA] TangentVectorType u; // Joint Inertia @@ -290,16 +309,33 @@ namespace pinocchio Matrix6x dAdv; /// \brief Partial derivative of the joint torque vector with respect to the joint configuration. - MatrixXs dtau_dq; + RowMatrixXs dtau_dq; /// \brief Partial derivative of the joint torque vector with respect to the joint velocity. - MatrixXs dtau_dv; + RowMatrixXs dtau_dv; /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration. - MatrixXs ddq_dq; + RowMatrixXs ddq_dq; /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity. - MatrixXs ddq_dv; + RowMatrixXs ddq_dv; + + /// \brief Partial derivative of the joint acceleration vector with respect to the joint torques. + RowMatrixXs ddq_dtau; + + /// \brief Stack of partial derivative of the contact frame acceleration with respect to the joint parameters. + MatrixXs dvc_dq; + MatrixXs dac_dq; + MatrixXs dac_dv; + MatrixXs dac_da; + + /// \brief Operational space inertia matrix; + MatrixXs osim; + + /// \brief Partial derivatives of the contact lambdas wrt joint params; + MatrixXs dlambda_dq; + MatrixXs dlambda_dv; + MatrixXs dlambda_dtau; /// \brief Vector of joint placements wrt to algorithm end effector. PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf; @@ -320,12 +356,15 @@ namespace pinocchio /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$. Matrix3x Jcom; - /// \brief Kinetic energy of the model. + /// \brief Kinetic energy of the system. Scalar kinetic_energy; - /// \brief Potential energy of the model. + /// \brief Potential energy of the system. Scalar potential_energy; + /// \brief Mechanical energy of the system. + Scalar mechanical_energy; + // Temporary variables used in forward dynamics /// \brief Inverse of the operational-space inertia matrix @@ -337,6 +376,12 @@ namespace pinocchio /// \brief Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics. VectorXs lambda_c; + /// \brief Proximal Lagrange Multipliers used in the computation of the Forward Dynamics computations. + VectorXs lambda_c_prox; + + /// \brief Difference between two consecutive iterations of the proxy algorithm. + VectorXs diff_lambda_c; + /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$. MatrixXs sDUiJt; @@ -361,6 +406,15 @@ namespace pinocchio /// \brief Tensor containing the kinematic Hessian of all the joints. Tensor3x kinematic_hessians; + /// \brief Cholesky decomposition of the KKT contact matrix + ContactCholeskyDecomposition contact_chol; + + /// \brief RHS vector when solving the contact dynamics KKT problem + VectorXs primal_dual_contact_solution; + + /// \brief Primal RHS in contact dynamic equations + VectorXs primal_rhs_contact; + /// /// \brief Default constructor of pinocchio::Data from a pinocchio::Model. /// diff --git a/src/multibody/data.hxx b/src/multibody/data.hxx index a8f559a080..b92d60c159 100644 --- a/src/multibody/data.hxx +++ b/src/multibody/data.hxx @@ -22,12 +22,15 @@ namespace pinocchio : joints(0) , a((std::size_t)model.njoints,Motion::Zero()) , oa((std::size_t)model.njoints,Motion::Zero()) + , oa_drift((std::size_t)model.njoints,Motion::Zero()) + , oa_augmented((std::size_t)model.njoints,Motion::Zero()) , a_gf((std::size_t)model.njoints,Motion::Zero()) , oa_gf((std::size_t)model.njoints,Motion::Zero()) , v((std::size_t)model.njoints,Motion::Zero()) , ov((std::size_t)model.njoints,Motion::Zero()) , f((std::size_t)model.njoints,Force::Zero()) , of((std::size_t)model.njoints,Force::Zero()) + , of_augmented((std::size_t)model.njoints,Force::Zero()) , h((std::size_t)model.njoints,Force::Zero()) , oh((std::size_t)model.njoints,Force::Zero()) , oMi((std::size_t)model.njoints,SE3::Identity()) @@ -55,6 +58,7 @@ namespace pinocchio , doYcrb((std::size_t)model.njoints,Inertia::Matrix6::Zero()) , ddq(VectorXs::Zero(model.nv)) , Yaba((std::size_t)model.njoints,Inertia::Matrix6::Zero()) + , oYaba((std::size_t)model.njoints,Inertia::Matrix6::Zero()) , u(VectorXs::Zero(model.nv)) , Ag(Matrix6x::Zero(6,model.nv)) , dAg(Matrix6x::Zero(6,model.nv)) @@ -78,21 +82,25 @@ namespace pinocchio , dVdq(Matrix6x::Zero(6,model.nv)) , dAdq(Matrix6x::Zero(6,model.nv)) , dAdv(Matrix6x::Zero(6,model.nv)) - , dtau_dq(MatrixXs::Zero(model.nv,model.nv)) - , dtau_dv(MatrixXs::Zero(model.nv,model.nv)) - , ddq_dq(MatrixXs::Zero(model.nv,model.nv)) - , ddq_dv(MatrixXs::Zero(model.nv,model.nv)) + , dtau_dq(RowMatrixXs::Zero(model.nv,model.nv)) + , dtau_dv(RowMatrixXs::Zero(model.nv,model.nv)) + , ddq_dq(RowMatrixXs::Zero(model.nv,model.nv)) + , ddq_dv(RowMatrixXs::Zero(model.nv,model.nv)) + , ddq_dtau(RowMatrixXs::Zero(model.nv,model.nv)) , iMf((std::size_t)model.njoints,SE3::Identity()) , com((std::size_t)model.njoints,Vector3::Zero()) , vcom((std::size_t)model.njoints,Vector3::Zero()) , acom((std::size_t)model.njoints,Vector3::Zero()) , mass((std::size_t)model.njoints,(Scalar)(-1)) , Jcom(Matrix3x::Zero(3,model.nv)) - , kinetic_energy((Scalar)-1) - , potential_energy((Scalar)-1) + , kinetic_energy(Scalar(0)) + , potential_energy(Scalar(0)) + , mechanical_energy(Scalar(0)) , JMinvJt() , llt_JMinvJt() , lambda_c() + , lambda_c_prox() + , diff_lambda_c() , sDUiJt(MatrixXs::Zero(model.nv,model.nv)) , torque_residual(VectorXs::Zero(model.nv)) , dq_after(VectorXs::Zero(model.nv)) @@ -118,9 +126,7 @@ namespace pinocchio { Fcrb[i].resize(6,model.nv); } computeLastChild(model); - - /* Init for Coriolis */ - + /* Init for Cholesky */ computeParents_fromRow(model); computeSupports_fromRow(model); @@ -145,8 +151,8 @@ namespace pinocchio lastChild[parent] = std::max(lastChild[(Index)i],lastChild[parent]); nvSubtree[(Index)i] - = idx_v(model.joints[(Index)lastChild[(Index)i]]) + nv(model.joints[(Index)lastChild[(Index)i]]) - - idx_v(model.joints[(Index)i]); + = model.joints[(Index)lastChild[(Index)i]].idx_v() + model.joints[(Index)lastChild[(Index)i]].nv() + - model.joints[(Index)i].idx_v(); } } @@ -159,14 +165,13 @@ namespace pinocchio for(Index joint=1;joint<(Index)(model.njoints);joint++) { const Index & parent = model.parents[joint]; - const int nvj = nv (model.joints[joint]); - const int idx_vj = idx_v(model.joints[joint]); + const int nvj = model.joints[joint].nv(); + const int idx_vj = model.joints[joint].idx_v(); assert(idx_vj >= 0 && idx_vj < model.nv); - - if(parent>0) parents_fromRow[(size_t)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1; - else parents_fromRow[(size_t)idx_vj] = -1; - nvSubtree_fromRow[(size_t)idx_vj] = nvSubtree[joint]; + if(parent>0) parents_fromRow[(Index)idx_vj] = model.joints[parent].idx_v()+model.joints[parent].nv()-1; + else parents_fromRow[(Index)idx_vj] = -1; + nvSubtree_fromRow[(Index)idx_vj] = nvSubtree[joint]; start_idx_v_fromRow[(size_t)idx_vj] = idx_vj; end_idx_v_fromRow[(size_t)idx_vj] = idx_vj+nvj-1; @@ -218,12 +223,15 @@ namespace pinocchio data1.joints == data2.joints && data1.a == data2.a && data1.oa == data2.oa + && data1.oa_drift == data2.oa_drift + && data1.oa_augmented == data2.oa_augmented && data1.a_gf == data2.a_gf && data1.oa_gf == data2.oa_gf && data1.v == data2.v && data1.ov == data2.ov && data1.f == data2.f && data1.of == data2.of + && data1.of_augmented == data2.of_augmented && data1.h == data2.h && data1.oh == data2.oh && data1.oMi == data2.oMi @@ -251,6 +259,8 @@ namespace pinocchio && data1.doYcrb == data2.doYcrb && data1.ddq == data2.ddq && data1.Yaba == data2.Yaba + && data1.oYaba == data2.oYaba + && data1.oYaba_contact == data2.oYaba_contact && data1.u == data2.u && data1.Ag == data2.Ag && data1.dAg == data2.dAg @@ -277,6 +287,14 @@ namespace pinocchio && data1.dtau_dv == data2.dtau_dv && data1.ddq_dq == data2.ddq_dq && data1.ddq_dv == data2.ddq_dv + && data1.dvc_dq == data2.dvc_dq + && data1.dac_dq == data2.dac_dq + && data1.dac_dv == data2.dac_dv + && data1.dac_da == data2.dac_da + && data1.osim == data2.osim + && data1.dlambda_dq == data2.dlambda_dq + && data1.dlambda_dv == data2.dlambda_dv + && data1.dlambda_dtau == data2.dlambda_dtau && data1.iMf == data2.iMf && data1.com == data2.com && data1.vcom == data2.vcom @@ -285,14 +303,20 @@ namespace pinocchio && data1.Jcom == data2.Jcom && data1.kinetic_energy == data2.kinetic_energy && data1.potential_energy == data2.potential_energy + && data1.mechanical_energy == data2.mechanical_energy && data1.JMinvJt == data2.JMinvJt && data1.lambda_c == data2.lambda_c + && data1.lambda_c_prox == data2.lambda_c_prox + && data1.diff_lambda_c == data2.diff_lambda_c + && data1.sDUiJt == data2.sDUiJt && data1.torque_residual == data2.torque_residual && data1.dq_after == data2.dq_after && data1.impulse_c == data2.impulse_c && data1.staticRegressor == data2.staticRegressor && data1.bodyRegressor == data2.bodyRegressor && data1.jointTorqueRegressor == data2.jointTorqueRegressor +// && data1.contact_chol == data2.contact_chol + && data1.primal_dual_contact_solution == data2.primal_dual_contact_solution ; // operator== for Eigen::Tensor provides an Expression which might be not evaluated as a boolean diff --git a/src/multibody/fcl.hxx b/src/multibody/fcl.hxx index 00c2d8e16e..25a1779988 100644 --- a/src/multibody/fcl.hxx +++ b/src/multibody/fcl.hxx @@ -15,6 +15,13 @@ namespace pinocchio : Base((std::numeric_limits::max)(),(std::numeric_limits::max)()) {} + /// + /// \brief Default constructor of a collision pair from two collision object indexes. + /// \remarks The two indexes must be different, otherwise the constructor throws. + /// + /// \param[in] co1 Index of the first collision object. + /// \param[in] co2 Index of the second collision object. + /// inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2) : Base(co1,co2) { diff --git a/src/multibody/force-set.hpp b/src/multibody/force-set.hpp index 3b2c3a0850..866d4919b8 100644 --- a/src/multibody/force-set.hpp +++ b/src/multibody/force-set.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2015 CNRS // -#ifndef __pinocchio_force_set_hpp__ -#define __pinocchio_force_set_hpp__ +#ifndef __pinocchio_spatial_force_set_hpp__ +#define __pinocchio_spatial_force_set_hpp__ #include "pinocchio/spatial/fwd.hpp" #include @@ -161,5 +161,5 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_force_set_hpp__ +#endif // ifndef __pinocchio_spatial_force_set_hpp__ diff --git a/src/multibody/frame.hpp b/src/multibody/frame.hpp index a939b6a717..5d003ea91a 100644 --- a/src/multibody/frame.hpp +++ b/src/multibody/frame.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2016-2021 CNRS INRIA // -#ifndef __pinocchio_frame_hpp__ -#define __pinocchio_frame_hpp__ +#ifndef __pinocchio_multibody_frame_hpp__ +#define __pinocchio_multibody_frame_hpp__ #include "pinocchio/spatial/se3.hpp" #include "pinocchio/spatial/inertia.hpp" @@ -24,12 +24,18 @@ namespace pinocchio BODY = 0x1 << 3, // body frame type SENSOR = 0x1 << 4 // sensor frame type }; + + template + struct traits< FrameTpl<_Scalar,_Options> > + { + typedef _Scalar Scalar; + }; /// /// \brief A Plucker coordinate frame attached to a parent joint inside a kinematic tree /// template - struct FrameTpl + struct FrameTpl : NumericalBase< FrameTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef pinocchio::JointIndex JointIndex; @@ -73,6 +79,17 @@ namespace pinocchio , inertia(inertia) {} + /// + /// \brief Copy constructor by casting + /// + /// \param[in] other Frame to copy + /// + template + explicit FrameTpl(const FrameTpl & other) + { + *this = other.template cast(); + } + /// /// \brief Equality comparison operator. /// @@ -159,4 +176,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_frame_hpp__ +#endif // ifndef __pinocchio_multibody_frame_hpp__ diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp index 1acf1e0b7c..2c5ade0364 100644 --- a/src/multibody/geometry.hpp +++ b/src/multibody/geometry.hpp @@ -10,7 +10,7 @@ #include "pinocchio/container/aligned-vector.hpp" #include "pinocchio/serialization/serializable.hpp" - +#include #include #include #include @@ -18,8 +18,18 @@ namespace pinocchio { + + struct GeometryModel; + struct GeometryData; + + template<> + struct traits + { + typedef double Scalar; + }; struct GeometryModel + : NumericalBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -169,14 +179,22 @@ namespace pinocchio /// \brief Vector of GeometryObjects used for collision computations GeometryObjectVector geometryObjects; - + /// /// \brief Vector of collision pairs. + /// CollisionPairVector collisionPairs; }; // struct GeometryModel + template<> + struct traits + { + typedef double Scalar; + }; + struct GeometryData : serialization::Serializable + , NumericalBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -208,6 +226,19 @@ namespace pinocchio std::vector activeCollisionPairs; #ifdef PINOCCHIO_WITH_HPP_FCL + /// + /// \brief Collision objects (ie a fcl placed geometry). + /// + /// The object contains a pointer on the collision geometries contained in geomModel.geometryObjects. + /// \sa GeometryModel::geometryObjects and GeometryObjects + /// + PINOCCHIO_DEPRECATED std::vector collisionObjects; + + /// + /// \brief Defines what information should be computed by distance computation. + /// + /// \deprecated use \ref distanceRequests instead + PINOCCHIO_DEPRECATED fcl::DistanceRequest distanceRequest; /// /// \brief Defines what information should be computed by distance computation. @@ -219,6 +250,12 @@ namespace pinocchio /// std::vector distanceResults; + /// + /// \brief Defines what information should be computed by collision test. + /// + /// \deprecated use \ref collisionRequests instead + PINOCCHIO_DEPRECATED fcl::CollisionRequest collisionRequest; + /// /// \brief Defines what information should be computed by collision test. /// There is one request per pair of geometries. diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx index 9a54f39001..167e29bf4d 100644 --- a/src/multibody/geometry.hxx +++ b/src/multibody/geometry.hxx @@ -21,13 +21,17 @@ namespace pinocchio { - +// Avoid deprecated warning of collisionObjects +PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH +PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS inline GeometryData::GeometryData(const GeometryModel & geom_model) : oMg(geom_model.ngeoms) , activeCollisionPairs(geom_model.collisionPairs.size(), true) #ifdef PINOCCHIO_WITH_HPP_FCL + , distanceRequest(true) , distanceRequests(geom_model.collisionPairs.size(), hpp::fcl::DistanceRequest(true)) , distanceResults(geom_model.collisionPairs.size()) + , collisionRequest(::hpp::fcl::NO_REQUEST,1) , collisionRequests(geom_model.collisionPairs.size(), hpp::fcl::CollisionRequest(::hpp::fcl::NO_REQUEST,1)) , collisionResults(geom_model.collisionPairs.size()) , radius() @@ -37,6 +41,11 @@ namespace pinocchio , outerObjects() { #ifdef PINOCCHIO_WITH_HPP_FCL + collisionObjects.reserve(geom_model.geometryObjects.size()); + BOOST_FOREACH(const GeometryObject & geom_object, geom_model.geometryObjects) + { + collisionObjects.push_back(fcl::CollisionObject(geom_object.geometry)); + } BOOST_FOREACH(hpp::fcl::CollisionRequest & creq, collisionRequests) { creq.enable_cached_gjk_guess = true; @@ -68,8 +77,11 @@ namespace pinocchio : oMg (other.oMg) , activeCollisionPairs (other.activeCollisionPairs) #ifdef PINOCCHIO_WITH_HPP_FCL + , collisionObjects (other.collisionObjects) + , distanceRequest (other.distanceRequest) , distanceRequests (other.distanceRequests) , distanceResults (other.distanceResults) + , collisionRequest (other.collisionRequest) , collisionRequests (other.collisionRequests) , collisionResults (other.collisionResults) , radius (other.radius) @@ -82,6 +94,7 @@ namespace pinocchio {} inline GeometryData::~GeometryData() {} +PINOCCHIO_COMPILER_DIAGNOSTIC_POP template class JointCollectionTpl> GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object, @@ -164,8 +177,8 @@ namespace pinocchio } #else os << "WARNING** Without fcl library, no collision checking or distance computations are possible. Only geometry placements can be computed." << std::endl; -#endif os << "Number of geometry objects = " << geomData.oMg.size() << std::endl; +#endif return os; } diff --git a/src/multibody/joint-motion-subspace-base.hpp b/src/multibody/joint-motion-subspace-base.hpp new file mode 100644 index 0000000000..f1693bf43e --- /dev/null +++ b/src/multibody/joint-motion-subspace-base.hpp @@ -0,0 +1,163 @@ +// +// Copyright (c) 2015-2020 CNRS INRIA +// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. +// + +#ifndef __pinocchio_multibody_constraint_base_hpp__ +#define __pinocchio_multibody_constraint_base_hpp__ + +#include "pinocchio/macros.hpp" +#include "pinocchio/spatial/fwd.hpp" +#include "pinocchio/spatial/motion.hpp" +#include "pinocchio/spatial/act-on-set.hpp" + +#include + +// S : v \in M^6 -> v_J \in lie(Q) ~= R^nv +// S^T : f_J \in lie(Q)^* ~= R^nv -> f \in F^6 + +#define PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,TYPENAME) \ + typedef TYPENAME traits::Scalar Scalar; \ + typedef TYPENAME traits::JointMotion JointMotion; \ + typedef TYPENAME traits::JointForce JointForce; \ + typedef TYPENAME traits::DenseBase DenseBase; \ + typedef TYPENAME traits::MatrixReturnType MatrixReturnType; \ + typedef TYPENAME traits::ConstMatrixReturnType ConstMatrixReturnType; \ + enum { LINEAR = traits::LINEAR, ANGULAR = traits::ANGULAR }; \ + enum { Options = traits::Options }; + +#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED) PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,typename) +#define PINOCCHIO_CONSTRAINT_TYPEDEF(DERIVED) PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,PINOCCHIO_EMPTY_ARG) + +namespace pinocchio +{ + + /// \brief Return type of the Constraint::Transpose * Force operation + template + struct ConstraintForceOp + { + typedef ReturnTypeNotDefined ReturnType; + }; + + /// \brief Return type of the Constraint::Transpose * ForceSet operation + template + struct ConstraintForceSetOp + { + typedef ReturnTypeNotDefined ReturnType; + }; + + template + class JointMotionSubspaceBase + : public NumericalBase + { + protected: + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(Derived) + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Derived & derived() { return *static_cast(this); } + const Derived & derived() const { return *static_cast(this); } + + template + JointMotion operator*(const Eigen::MatrixBase & vj) const + { return derived().__mult__(vj); } + + MatrixReturnType matrix() { return derived().matrix_impl(); } + ConstMatrixReturnType matrix() const { return derived().matrix_impl(); } + + int nv() const { return derived().nv_impl(); } + + static int rows() { return 6; } + int cols() const { return nv(); } + + template + bool isApprox(const JointMotionSubspaceBase & other, + const Scalar & prec = Eigen::NumTraits::dummy_precision()) const + { return matrix().isApprox(other.matrix(),prec); } + + void disp(std::ostream & os) const { derived().disp_impl(os); } + friend std::ostream & operator << (std::ostream & os,const JointMotionSubspaceBase & X) + { + X.disp(os); + return os; + } + + typename SE3GroupAction::ReturnType + se3Action(const SE3Tpl & m) const + { + return derived().se3Action(m); + } + + typename SE3GroupAction::ReturnType + se3ActionInverse(const SE3Tpl & m) const + { + return derived().se3ActionInverse(m); + } + + template + typename MotionAlgebraAction::ReturnType + motionAction(const MotionDense & v) const + { + return derived().motionAction(v); + } + + bool operator==(const JointMotionSubspaceBase & other) const + { + return derived().isEqual(other.derived()); + } + + }; // class JointMotionSubspaceBase + + /// \brief Operation Y * S used in the CRBA algorithm for instance + template + typename MultiplicationOp,ConstraintDerived>::ReturnType + operator*(const InertiaTpl & Y, + const JointMotionSubspaceBase & constraint) + { + return impl::LhsMultiplicationOp,ConstraintDerived>::run(Y, + constraint.derived()); + } + + /// \brief Operation Y_matrix * S used in the ABA algorithm for instance + template + typename MultiplicationOp,ConstraintDerived>::ReturnType + operator*(const Eigen::MatrixBase & Y, + const JointMotionSubspaceBase & constraint) + { + return impl::LhsMultiplicationOp,ConstraintDerived>::run(Y.derived(), + constraint.derived()); + } + + namespace details + { + template + struct StDiagonalMatrixSOperation + { + typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; + typedef typename traits::ReducedSquaredMatrix ReducedSquaredMatrix; + + static ReturnType run(const JointMotionSubspaceBase & /*constraint*/) + { + return ReducedSquaredMatrix::Identity(Constraint::NV,Constraint::NV); + } + }; + } + + template + struct JointMotionSubspaceTransposeBase + { + typedef typename traits::StDiagonalMatrixSOperationReturnType StDiagonalMatrixSOperationReturnType; + }; + + template + typename JointMotionSubspaceTransposeBase::StDiagonalMatrixSOperationReturnType + operator*(const JointMotionSubspaceTransposeBase & /*S_transpose*/, + const JointMotionSubspaceBase & S) + { + return details::StDiagonalMatrixSOperation::run(S.derived()); + } + +} // namespace pinocchio + +#endif // ifndef __pinocchio_multibody_constraint_base_hpp__ diff --git a/src/multibody/joint-motion-subspace-generic.hpp b/src/multibody/joint-motion-subspace-generic.hpp new file mode 100644 index 0000000000..b77dc2122d --- /dev/null +++ b/src/multibody/joint-motion-subspace-generic.hpp @@ -0,0 +1,186 @@ +// +// Copyright (c) 2015-2020 CNRS INRIA +// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. +// + +#ifndef __pinocchio_multibody_constraint_generic_hpp__ +#define __pinocchio_multibody_constraint_generic_hpp__ + +namespace pinocchio +{ + + template + struct traits< JointMotionSubspaceTpl<_Dim, _Scalar, _Options> > + { + typedef _Scalar Scalar; + enum + { + LINEAR = 0, + ANGULAR = 3, + Options = _Options, + Dim = _Dim + }; + + typedef MotionTpl JointMotion; + typedef Eigen::Matrix JointForce; + typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + + typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType; + typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType; + + typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; + }; // traits JointMotionSubspaceTpl + + template + struct SE3GroupAction< JointMotionSubspaceTpl > + { typedef Eigen::Matrix ReturnType; }; + + template + struct MotionAlgebraAction< JointMotionSubspaceTpl, MotionDerived > + { typedef Eigen::Matrix ReturnType; }; + + template + struct JointMotionSubspaceTpl + : public JointMotionSubspaceBase< JointMotionSubspaceTpl<_Dim,_Scalar,_Options> > + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef JointMotionSubspaceBase Base; + + friend class JointMotionSubspaceBase; + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceTpl) + + enum { NV = _Dim }; + + using Base::nv; + + template + explicit JointMotionSubspaceTpl(const Eigen::MatrixBase & _S) : S(_S) + { + // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace path + // TODO + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D); + } + + JointMotionSubspaceTpl() : S() + { + EIGEN_STATIC_ASSERT(_Dim!=Eigen::Dynamic, + YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR) + } + + // It is only valid for dynamics size + explicit JointMotionSubspaceTpl(const int dim) : S(6,dim) + { + EIGEN_STATIC_ASSERT(_Dim==Eigen::Dynamic, + YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR) + } + + static JointMotionSubspaceTpl Zero(const int dim) + { + return JointMotionSubspaceTpl(dim); + } + + template + JointMotion __mult__(const Eigen::MatrixBase & vj) const + { + return JointMotion(S*vj); + } + + struct Transpose : JointMotionSubspaceTransposeBase + { + const JointMotionSubspaceTpl & ref; + Transpose( const JointMotionSubspaceTpl & ref ) : ref(ref) {} + + template + JointForce operator*(const ForceDense & f) const + { return (ref.S.transpose()*f.toVector()).eval(); } + + template + typename Eigen::Matrix + operator*(const Eigen::MatrixBase & F) + { + return (ref.S.transpose()*F).eval(); + } + + }; + + Transpose transpose() const { return Transpose(*this); } + + MatrixReturnType matrix_impl() { return S; } + ConstMatrixReturnType matrix_impl() const { return S; } + + int nv_impl() const { return (int)S.cols(); } + + template + friend typename JointMotionSubspaceTpl<_Dim,_Scalar,_Options>::DenseBase + operator*(const InertiaTpl & Y, const JointMotionSubspaceTpl & S) + { + typedef typename JointMotionSubspaceTpl::DenseBase ReturnType; + ReturnType res(6,S.nv()); + motionSet::inertiaAction(Y,S.S,res); + return res; + } + + template + friend Eigen::Matrix<_Scalar,6,_Dim> + operator*(const Eigen::Matrix & Ymatrix, const JointMotionSubspaceTpl & S) + { + typedef Eigen::Matrix<_Scalar,6,_Dim> ReturnType; + return ReturnType(Ymatrix*S.matrix()); + + } + + DenseBase se3Action(const SE3Tpl & m) const + { + DenseBase res(6,nv()); + motionSet::se3Action(m,S,res); + return res; + } + + DenseBase se3ActionInverse(const SE3Tpl & m) const + { + DenseBase res(6,nv()); + motionSet::se3ActionInverse(m,S,res); + return res; + } + + template + DenseBase motionAction(const MotionDense & v) const + { + DenseBase res(6,nv()); + motionSet::motionAction(v,S,res); + return res; + } + + void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;} + + bool isEqual(const JointMotionSubspaceTpl & other) const + { + return S == other.S; + } + + protected: + DenseBase S; + }; // class JointMotionSubspaceTpl + + namespace details + { + template + struct StDiagonalMatrixSOperation< JointMotionSubspaceTpl > + { + typedef JointMotionSubspaceTpl Constraint; + typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; + + static ReturnType run(const JointMotionSubspaceBase & constraint) + { + return constraint.matrix().transpose() * constraint.matrix(); + } + }; + } + + +} // namespace pinocchio + +#endif // ifndef __pinocchio_multibody_constraint_generic_hpp__ + diff --git a/src/multibody/joint-motion-subspace.hpp b/src/multibody/joint-motion-subspace.hpp new file mode 100644 index 0000000000..c04de7deb8 --- /dev/null +++ b/src/multibody/joint-motion-subspace.hpp @@ -0,0 +1,25 @@ +// +// Copyright (c) 2015-2020 CNRS INRIA +// + +#ifndef __pinocchio_multibody_joint_motion_subspace_hpp__ +#define __pinocchio_multibody_joint_motion_subspace_hpp__ + +#include "pinocchio/macros.hpp" + +namespace pinocchio +{ + + template struct JointMotionSubspaceTpl; + + typedef JointMotionSubspaceTpl<1,double,0> JointMotionSubspace1d; + typedef JointMotionSubspaceTpl<3,double,0> JointMotionSubspace3d; + typedef JointMotionSubspaceTpl<6,double,0> JointMotionSubspace6d; + typedef JointMotionSubspaceTpl JointMotionSubspaceXd; + +} // namespace pinocchio + +#include "pinocchio/multibody/joint-motion-subspace-base.hpp" +#include "pinocchio/multibody/joint-motion-subspace-generic.hpp" + +#endif // ifndef __pinocchio_multibody_joint_motion_subspace_hpp__ diff --git a/src/multibody/joint/joint-basic-visitors.hpp b/src/multibody/joint/joint-basic-visitors.hpp index 2d8db59f99..643e756989 100644 --- a/src/multibody/joint/joint-basic-visitors.hpp +++ b/src/multibody/joint/joint-basic-visitors.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2016,2018 CNRS +// Copyright (c) 2016-2020 CNRS INRIA // -#ifndef __pinocchio_joint_basic_visitors_hpp__ -#define __pinocchio_joint_basic_visitors_hpp__ +#ifndef __pinocchio_multibody_joint_basic_visitors_hpp__ +#define __pinocchio_multibody_joint_basic_visitors_hpp__ #include "pinocchio/multibody/joint/fwd.hpp" @@ -66,14 +66,16 @@ namespace pinocchio * @tparam JointCollection Collection of Joint types. * @tparam Matrix6Type A matrix 6x6 like Eigen container. * - * @param[in] jmodel The corresponding JointModelVariant to the JointDataVariant we want to update - * @param[inout] jdata The JointDataVariant we want to update - * @param[inout] I Inertia matrix of the subtree following the jmodel in the kinematic chain as dense matrix + * @param[in] jmodel The corresponding JointModelVariant to the JointDataVariant we want to update + * @param[in,out] jdata The JointDataVariant we want to update + * @param[in] armature Armature related to the current joint. + * @param[in,out] I Inertia matrix of the subtree following the jmodel in the kinematic chain as dense matrix * * @param[in] update_I If I should be updated or not */ - template class JointCollectionTpl, typename Matrix6Type> + template class JointCollectionTpl, typename VectorLike, typename Matrix6Type> inline void calc_aba(const JointModelTpl & jmodel, JointDataTpl & jdata, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I); @@ -204,6 +206,29 @@ namespace pinocchio // Visitors on JointDatas // + + /** + * @brief Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector + * + * @param[in] jdata The joint data to visit. + * + * @return The current value of the joint configuration vector + */ + template class JointCollectionTpl> + inline typename JointDataTpl::ConfigVector_t + joint_q(const JointDataTpl & jdata); + + + /** + * @brief Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector + * + * @param[in] jdata The joint data to visit. + * + * @return The current value of the joint velocity vector + */ + template class JointCollectionTpl> + inline typename JointDataTpl::TangentVector_t + joint_v(const JointDataTpl & jdata); /** * @brief Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint @@ -214,8 +239,8 @@ namespace pinocchio * @return The constraint dense corresponding to the joint derived constraint */ template class JointCollectionTpl> - inline ConstraintTpl - constraint_xd(const JointDataTpl & jdata); + inline JointMotionSubspaceTpl + joint_motin_subspace_xd(const JointDataTpl & jdata); /** * @brief Visit a JointDataTpl through JointTransformVisitor to get the joint internal transform (transform @@ -300,6 +325,18 @@ namespace pinocchio template class JointCollectionTpl, typename JointDataDerived> bool isEqual(const JointDataTpl & jmodel_generic, const JointDataBase & jmodel); + + /** + * @brief Visit a JointDataTpl through JointStUInertiaVisitor to get St*I*S matrix of the inertia matrix + * decomposition. + * + * @param[in] jdata The joint data to visit. + * + * @return The St*I*S matrix + */ + template class JointCollectionTpl> + inline Eigen::Matrix + stu_inertia(const JointDataTpl & jdata); } // namespace pinocchio @@ -309,4 +346,4 @@ namespace pinocchio // #include "pinocchio/multibody/joint/joint-basic-visitors.hxx" -#endif // ifndef __pinocchio_joint_basic_visitors_hpp__ +#endif // ifndef __pinocchio_multibody_joint_basic_visitors_hpp__ diff --git a/src/multibody/joint/joint-basic-visitors.hxx b/src/multibody/joint/joint-basic-visitors.hxx index 4ac12b2c18..918275c4af 100644 --- a/src/multibody/joint/joint-basic-visitors.hxx +++ b/src/multibody/joint/joint-basic-visitors.hxx @@ -1,9 +1,9 @@ // -// Copyright (c) 2016-2019 CNRS INRIA +// Copyright (c) 2016-2020 CNRS INRIA // -#ifndef __pinocchio_joint_basic_visitors_hxx__ -#define __pinocchio_joint_basic_visitors_hxx__ +#ifndef __pinocchio_multibody_joint_basic_visitors_hxx__ +#define __pinocchio_multibody_joint_basic_visitors_hxx__ #include "pinocchio/multibody/joint/joint-basic-visitors.hpp" #include "pinocchio/multibody/visitor.hpp" @@ -106,37 +106,42 @@ namespace pinocchio * @brief JointCalcAbaVisitor fusion visitor */ - template + template struct JointCalcAbaVisitor - : fusion::JointUnaryVisitorBase< JointCalcAbaVisitor > + : fusion::JointUnaryVisitorBase< JointCalcAbaVisitor > { - typedef boost::fusion::vector ArgsType; + typedef boost::fusion::vector ArgsType; template static void algo(const pinocchio::JointModelBase & jmodel, pinocchio::JointDataBase & jdata, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, - const bool & update_I + bool update_I ) { - Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I); - jmodel.calc_aba(jdata.derived(),I_,update_I); + jmodel.calc_aba(jdata.derived(), + armature.derived(), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), + update_I); } }; - template class JointCollectionTpl, typename Matrix6Type> + template class JointCollectionTpl, typename VectorLike, typename Matrix6Type> inline void calc_aba(const JointModelTpl & jmodel, JointDataTpl & jdata, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) { - typedef JointCalcAbaVisitor Algo; - - Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I); - Algo::run(jmodel, jdata, typename Algo::ArgsType(I_, update_I) ); + typedef JointCalcAbaVisitor Algo; + Algo::run(jmodel, jdata, typename Algo::ArgsType(PINOCCHIO_EIGEN_CONST_CAST(VectorLike,armature), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), + update_I) ); } /** @@ -311,7 +316,7 @@ namespace pinocchio static bool algo(const JointModelBase & jmodel_lhs, const JointModelDerived & jmodel_rhs) { - return jmodel_lhs.derived() == jmodel_rhs; + return internal::comparison_eq(jmodel_lhs.derived(), jmodel_rhs); } }; @@ -350,15 +355,65 @@ namespace pinocchio // // Visitors on JointDatas // + + template class JointCollectionTpl> + struct JointQVisitor + : boost::static_visitor< typename JointDataTpl::ConfigVector_t > + { + typedef typename JointDataTpl::ConfigVector_t ReturnType; + + template + ReturnType operator()(const JointDataBase & jdata) const + { + return jdata.joint_q(); + } + + static ReturnType run(const JointDataTpl & jdata) + { + return boost::apply_visitor(JointQVisitor(), jdata); + } + }; + + template class JointCollectionTpl> + inline typename JointDataTpl::ConfigVector_t + joint_q(const JointDataTpl & jdata) + { + return JointQVisitor::run(jdata); + } + + template class JointCollectionTpl> + struct JointVVisitor + : boost::static_visitor< typename JointDataTpl::ConfigVector_t > + { + typedef typename JointDataTpl::TangentVector_t ReturnType; + + template + ReturnType operator()(const JointDataBase & jdata) const + { + return jdata.joint_v(); + } + + static ReturnType run(const JointDataTpl & jdata) + { + return boost::apply_visitor(JointVVisitor(), jdata); + } + }; + + template class JointCollectionTpl> + inline typename JointDataTpl::TangentVector_t + joint_v(const JointDataTpl & jdata) + { + return JointVVisitor::run(jdata); + } /** * @brief JointConstraintVisitor visitor */ template class JointCollectionTpl> struct JointConstraintVisitor - : boost::static_visitor< ConstraintTpl > + : boost::static_visitor< JointMotionSubspaceTpl > { - typedef ConstraintTpl ReturnType; + typedef JointMotionSubspaceTpl ReturnType; template ReturnType operator()(const JointDataBase & jdata) const @@ -373,8 +428,8 @@ namespace pinocchio }; template class JointCollectionTpl> - inline ConstraintTpl - constraint_xd(const JointDataTpl & jdata) + inline JointMotionSubspaceTpl + joint_motin_subspace_xd(const JointDataTpl & jdata) { return JointConstraintVisitor::run(jdata); } @@ -548,6 +603,35 @@ namespace pinocchio return JointUDInvInertiaVisitor::run(jdata); } + /** + * @brief JointStUInertiaVisitor visitor + */ + template class JointCollectionTpl> + struct JointStUInertiaVisitor + : boost::static_visitor< Eigen::Matrix > + { + typedef Eigen::Matrix ReturnType; + + template + ReturnType operator()(const JointDataBase & jdata) const + { + return ReturnType(jdata.StU()); + } + + static ReturnType run(const JointDataTpl & jdata) + { + return boost::apply_visitor(JointStUInertiaVisitor(),jdata); + + } + }; + + template class JointCollectionTpl> + inline Eigen::Matrix + stu_inertia(const JointDataTpl & jdata) + { + return JointStUInertiaVisitor::run(jdata); + } + /** * @brief JointDataShortnameVisitor visitor */ @@ -596,4 +680,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_joint_basic_visitors_hxx__ +#endif // ifndef __pinocchio_multibody_joint_basic_visitors_hxx__ diff --git a/src/multibody/joint/joint-collection.hpp b/src/multibody/joint/joint-collection.hpp index 4e74a7843b..bfb66df14b 100644 --- a/src/multibody/joint/joint-collection.hpp +++ b/src/multibody/joint/joint-collection.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2018-2019 CNRS INRIA // -#ifndef __pinocchio_joint_collection_hpp__ -#define __pinocchio_joint_collection_hpp__ +#ifndef __pinocchio_multibody_joint_collection_hpp__ +#define __pinocchio_multibody_joint_collection_hpp__ #include "pinocchio/multibody/joint/fwd.hpp" #include "pinocchio/multibody/joint/joints.hpp" @@ -150,4 +150,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_joint_collection_hpp__ +#endif // ifndef __pinocchio_multibody_joint_collection_hpp__ diff --git a/src/multibody/joint/joint-composite.hpp b/src/multibody/joint/joint-composite.hpp index 1b071ae36c..83e680f12f 100644 --- a/src/multibody/joint/joint-composite.hpp +++ b/src/multibody/joint/joint-composite.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2016-2021 CNRS INRIA // -#ifndef __pinocchio_joint_composite_hpp__ -#define __pinocchio_joint_composite_hpp__ +#ifndef __pinocchio_multibody_joint_composite_hpp__ +#define __pinocchio_multibody_joint_composite_hpp__ #include "pinocchio/multibody/joint/fwd.hpp" #include "pinocchio/multibody/joint/joint-collection.hpp" @@ -33,7 +33,7 @@ namespace pinocchio typedef JointCollectionTpl JointCollection; typedef JointDataCompositeTpl JointDataDerived; typedef JointModelCompositeTpl JointModelDerived; - typedef ConstraintTpl Constraint_t; + typedef JointMotionSubspaceTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionTpl Motion_t; typedef MotionTpl Bias_t; @@ -42,20 +42,26 @@ namespace pinocchio typedef Eigen::Matrix U_t; typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template class JointCollectionTpl> - struct traits< JointModelCompositeTpl > - { typedef JointCompositeTpl JointDerived; }; + template class JointCollectionTpl> + struct traits< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> > + { + typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + typedef _Scalar Scalar; + }; - template class JointCollectionTpl> - struct traits< JointDataCompositeTpl > - { typedef JointCompositeTpl JointDerived; }; + template class JointCollectionTpl> + struct traits< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> > + { + typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + typedef _Scalar Scalar; + }; template class JointCollectionTpl> struct JointDataCompositeTpl @@ -79,6 +85,8 @@ namespace pinocchio : joints() , iMlast(0) , pjMi(0) + , joint_q(ConfigVector_t::Zero(0)) + , joint_v(TangentVector_t::Zero(0)) , S(0) , M(Transformation_t::Identity()) , v(Motion_t::Zero()) @@ -88,8 +96,10 @@ namespace pinocchio {} - JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv) + JointDataCompositeTpl(const JointDataVector & joint_data, const int nq, const int nv) : joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size()) + , joint_q(ConfigVector_t::Zero(nq)) + , joint_v(TangentVector_t::Zero(nv)) , S(Constraint_t::Zero(nv)) , M(Transformation_t::Identity()) , v(Motion_t::Zero()) @@ -108,6 +118,9 @@ namespace pinocchio /// \brief Transforms from previous joint to joint i PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi; + + ConfigVector_t joint_q; + TangentVector_t joint_v; Constraint_t S; Transformation_t M; @@ -118,7 +131,6 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; - D_t StU; static std::string classname() { return std::string("JointDataComposite"); } @@ -156,14 +168,13 @@ namespace pinocchio PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); typedef JointCollectionTpl JointCollection; - typedef JointModelTpl JointModel; - typedef JointModel JointModelVariant; + typedef JointModelTpl JointModelVariant; typedef SE3Tpl SE3; typedef MotionTpl Motion; typedef InertiaTpl Inertia; - typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector; + typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector; using Base::id; using Base::idx_q; @@ -274,17 +285,16 @@ namespace pinocchio const Eigen::MatrixBase & qs, const Eigen::MatrixBase & vs) const; - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { data.U.noalias() = I * data.S.matrix(); data.StU.noalias() = data.S.matrix().transpose() * data.U; + data.StU.diagonal() += armature; - // compute inverse -// data.Dinv.setIdentity(); -// data.StU.llt().solveInPlace(data.Dinv); internal::PerformStYSInversion::run(data.StU,data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; @@ -328,15 +338,15 @@ namespace pinocchio { std::cout << "JointModelCompositeTpl::isEqual" << std::endl; return Base::isEqual(other) - && nq() == other.nq() - && nv() == other.nv() - && m_idx_q == other.m_idx_q - && m_idx_v == other.m_idx_v - && m_nqs == other.m_nqs - && m_nvs == other.m_nvs - && joints == other.joints - && jointPlacements == other.jointPlacements - && njoints == other.njoints; + && internal::comparison_eq(nq(), other.nq()) + && internal::comparison_eq(nv(), other.nv()) + && internal::comparison_eq(m_idx_q, other.m_idx_q) + && internal::comparison_eq(m_idx_v, other.m_idx_v) + && internal::comparison_eq(m_nqs, other.m_nqs) + && internal::comparison_eq(m_nvs, other.m_nvs) + && internal::comparison_eq(joints, other.joints) + && internal::comparison_eq(jointPlacements, other.jointPlacements) + && internal::comparison_eq(njoints, other.njoints); } /// \returns An expression of *this with the Scalar type casted to NewScalar. @@ -505,4 +515,4 @@ namespace boost /* --- Details -------------------------------------------------------------- */ #include "pinocchio/multibody/joint/joint-composite.hxx" -#endif // ifndef __pinocchio_joint_composite_hpp__ +#endif // ifndef __pinocchio_multibody_joint_composite_hpp__ diff --git a/src/multibody/joint/joint-composite.hxx b/src/multibody/joint/joint-composite.hxx index 9494821f5f..d7e66af3b7 100644 --- a/src/multibody/joint/joint-composite.hxx +++ b/src/multibody/joint/joint-composite.hxx @@ -1,9 +1,9 @@ // -// Copyright (c) 2016-2019 CNRS INRIA +// Copyright (c) 2016-2020 CNRS INRIA // -#ifndef __pinocchio_joint_composite_hxx__ -#define __pinocchio_joint_composite_hxx__ +#ifndef __pinocchio_multibody_joint_composite_hxx__ +#define __pinocchio_multibody_joint_composite_hxx__ #include "pinocchio/multibody/visitor.hpp" @@ -63,6 +63,7 @@ namespace pinocchio typedef JointCompositeCalcZeroOrderStep Algo; + data.joint_q = qs.segment(idx_q(),nq()); for (int i=(int)(joints.size()-1); i >= 0; --i) { Algo::run(joints[(size_t)i], @@ -138,6 +139,8 @@ namespace pinocchio typedef JointCompositeCalcFirstOrderStep Algo; + jdata.joint_q = qs.segment(idx_q(),nq()); + jdata.joint_v = vs.segment(idx_v(),nv()); for (int i=(int)(joints.size()-1); i >= 0; --i) { Algo::run(joints[(size_t)i], @@ -150,4 +153,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_joint_composite_hxx__ +#endif // ifndef __pinocchio_multibody_joint_composite_hxx__ diff --git a/src/multibody/joint/joint-data-base.hpp b/src/multibody/joint/joint-data-base.hpp index 6df639d24e..c46e256876 100644 --- a/src/multibody/joint/joint-data-base.hpp +++ b/src/multibody/joint/joint-data-base.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // @@ -11,6 +11,10 @@ #define PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,TYPENAME) \ PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,TYPENAME); \ + typedef TYPENAME traits::ConfigVectorTypeConstRef ConfigVectorTypeConstRef; \ + typedef TYPENAME traits::ConfigVectorTypeRef ConfigVectorTypeRef; \ + typedef TYPENAME traits::TangentVectorTypeConstRef TangentVectorTypeConstRef; \ + typedef TYPENAME traits::TangentVectorTypeRef TangentVectorTypeRef; \ typedef TYPENAME traits::ConstraintTypeConstRef ConstraintTypeConstRef; \ typedef TYPENAME traits::ConstraintTypeRef ConstraintTypeRef; \ typedef TYPENAME traits::TansformTypeConstRef TansformTypeConstRef; \ @@ -44,6 +48,10 @@ #endif #define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \ + ConfigVectorTypeConstRef joint_q_accessor() const { return joint_q; } \ + ConfigVectorTypeRef joint_q_accessor() { return joint_q; } \ + TangentVectorTypeConstRef joint_v_accessor() const { return joint_v; } \ + TangentVectorTypeRef joint_v_accessor() { return joint_v; } \ ConstraintTypeConstRef S_accessor() const { return S; } \ ConstraintTypeRef S_accessor() { return S; } \ TansformTypeConstRef M_accessor() const { return M; } \ @@ -57,9 +65,15 @@ DTypeConstRef Dinv_accessor() const { return Dinv; } \ DTypeRef Dinv_accessor() { return Dinv; } \ UDTypeConstRef UDinv_accessor() const { return UDinv; } \ - UDTypeRef UDinv_accessor() { return UDinv; } + UDTypeRef UDinv_accessor() { return UDinv; } \ + DTypeConstRef StU_accessor() const { return StU; } \ + DTypeRef StU_accessor() { return StU; } \ #define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \ + typedef const ConfigVector_t & ConfigVectorTypeConstRef; \ + typedef ConfigVector_t & ConfigVectorTypeRef; \ + typedef const TangentVector_t & TangentVectorTypeConstRef; \ + typedef TangentVector_t & TangentVectorTypeRef; \ typedef const Constraint_t & ConstraintTypeConstRef; \ typedef Constraint_t & ConstraintTypeRef; \ typedef const Transformation_t & TansformTypeConstRef; \ @@ -80,6 +94,7 @@ namespace pinocchio template struct JointDataBase + : NumericalBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -88,6 +103,12 @@ namespace pinocchio Derived & derived() { return *static_cast(this); } const Derived & derived() const { return *static_cast(this); } + + ConfigVectorTypeConstRef joint_q() const { return derived().joint_q_accessor(); } + ConfigVectorTypeRef joint_q() { return derived().joint_q_accessor(); } + + TangentVectorTypeConstRef joint_v() const { return derived().joint_v_accessor(); } + TangentVectorTypeRef joint_v() { return derived().joint_v_accessor(); } ConstraintTypeConstRef S() const { return derived().S_accessor(); } ConstraintTypeRef S() { return derived().S_accessor(); } @@ -104,6 +125,8 @@ namespace pinocchio DTypeRef Dinv() { return derived().Dinv_accessor(); } UDTypeConstRef UDinv() const { return derived().UDinv_accessor(); } UDTypeRef UDinv() { return derived().UDinv_accessor(); } + DTypeConstRef StU() const { return derived().StU_accessor(); } + DTypeRef StU() { return derived().StU_accessor(); } std::string shortname() const { return derived().shortname(); } static std::string classname() { return Derived::classname(); } @@ -127,15 +150,18 @@ namespace pinocchio } /// \brief Default operator== implementation - bool isEqual(const JointDataBase & other) const + bool isEqual(const JointDataBase & other) const { - return S() == other.S() - && M() == other.M() - && v() == other.v() - && c() == other.c() - && U() == other.U() - && Dinv() == other.Dinv() - && UDinv() == other.UDinv() + return + internal::comparison_eq(joint_q(), other.joint_q()) + && internal::comparison_eq(joint_v(), other.joint_v()) + && internal::comparison_eq(S(), other.S()) + && internal::comparison_eq(M(), other.M()) + && internal::comparison_eq(v(), other.v()) + && internal::comparison_eq(c(), other.c()) + && internal::comparison_eq(U(), other.U()) + && internal::comparison_eq(Dinv(), other.Dinv()) + && internal::comparison_eq(UDinv(), other.UDinv()) ; } @@ -155,7 +181,7 @@ namespace pinocchio /// \brief Default operator!= implementation bool isNotEqual(const JointDataBase & other) const { - return !(derived() == other.derived()); + return !(internal::comparison_eq(derived(), other.derived())); } protected: diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp index 4d92de2c44..cd112ec2b5 100644 --- a/src/multibody/joint/joint-free-flyer.hpp +++ b/src/multibody/joint/joint-free-flyer.hpp @@ -1,26 +1,26 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_free_flyer_hpp__ -#define __pinocchio_joint_free_flyer_hpp__ +#ifndef __pinocchio_multibody_joint_free_flyer_hpp__ +#define __pinocchio_multibody_joint_free_flyer_hpp__ #include "pinocchio/macros.hpp" #include "pinocchio/spatial/inertia.hpp" #include "pinocchio/spatial/explog.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/math/fwd.hpp" #include "pinocchio/math/quaternion.hpp" namespace pinocchio { - template struct ConstraintIdentityTpl; + template struct JointMotionSubspaceIdentityTpl; template - struct traits< ConstraintIdentityTpl<_Scalar,_Options> > + struct traits< JointMotionSubspaceIdentityTpl<_Scalar,_Options> > { typedef _Scalar Scalar; enum { Options = _Options }; @@ -32,17 +32,19 @@ namespace pinocchio typedef MotionTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType; typedef typename Matrix6::IdentityReturnType MatrixReturnType; + typedef typename Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType; }; // traits ConstraintRevolute - template - struct ConstraintIdentityTpl - : ConstraintBase< ConstraintIdentityTpl<_Scalar,_Options> > + struct JointMotionSubspaceIdentityTpl + : JointMotionSubspaceBase< JointMotionSubspaceIdentityTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintIdentityTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceIdentityTpl) enum { NV = 6 }; @@ -70,7 +72,7 @@ namespace pinocchio int nv_impl() const { return NV; } - struct TransposeConst + struct TransposeConst : JointMotionSubspaceTransposeBase { template typename ForceDense::ToVectorConstReturnType @@ -94,17 +96,17 @@ namespace pinocchio motionAction(const MotionBase & v) const { return v.toActionMatrix(); } - bool isEqual(const ConstraintIdentityTpl &) const { return true; } + bool isEqual(const JointMotionSubspaceIdentityTpl &) const { return true; } - }; // struct ConstraintIdentityTpl + }; // struct JointMotionSubspaceIdentityTpl template MotionRef - operator*(const ConstraintIdentityTpl &, + operator*(const JointMotionSubspaceIdentityTpl &, const Eigen::MatrixBase & v) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6); -// typedef typename ConstraintIdentityTpl::Motion Motion; +// typedef typename JointMotionSubspaceIdentityTpl::Motion Motion; typedef MotionRef Motion; return Motion(v.derived()); } @@ -112,7 +114,7 @@ namespace pinocchio /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ template inline typename InertiaTpl::Matrix6 - operator*(const InertiaTpl & Y, const ConstraintIdentityTpl &) + operator*(const InertiaTpl & Y, const JointMotionSubspaceIdentityTpl &) { return Y.matrix(); } @@ -120,17 +122,17 @@ namespace pinocchio /* [ABA] Y*S operator*/ template inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) - operator*(const Eigen::MatrixBase & Y, const ConstraintIdentityTpl &) + operator*(const Eigen::MatrixBase & Y, const JointMotionSubspaceIdentityTpl &) { return Y.derived(); } template - struct SE3GroupAction< ConstraintIdentityTpl > + struct SE3GroupAction< JointMotionSubspaceIdentityTpl > { typedef typename SE3Tpl::ActionMatrixType ReturnType; }; template - struct MotionAlgebraAction< ConstraintIdentityTpl,MotionDerived > + struct MotionAlgebraAction< JointMotionSubspaceIdentityTpl,MotionDerived > { typedef typename SE3Tpl::ActionMatrixType ReturnType; }; template struct JointFreeFlyerTpl; @@ -146,7 +148,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataFreeFlyerTpl JointDataDerived; typedef JointModelFreeFlyerTpl JointModelDerived; - typedef ConstraintIdentityTpl Constraint_t; + typedef JointMotionSubspaceIdentityTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -155,20 +157,26 @@ namespace pinocchio typedef Eigen::Matrix U_t; typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataFreeFlyerTpl > - { typedef JointFreeFlyerTpl JointDerived; }; + template + struct traits< JointDataFreeFlyerTpl<_Scalar,_Options> > + { + typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits< JointModelFreeFlyerTpl > - { typedef JointFreeFlyerTpl JointDerived; }; + template + struct traits< JointModelFreeFlyerTpl<_Scalar,_Options> > + { + typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; template struct JointDataFreeFlyerTpl : public JointDataBase< JointDataFreeFlyerTpl<_Scalar,_Options> > @@ -178,6 +186,9 @@ namespace pinocchio PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + ConfigVector_t joint_q; + TangentVector_t joint_v; + Constraint_t S; Transformation_t M; Motion_t v; @@ -187,14 +198,20 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; JointDataFreeFlyerTpl() - : M(Transformation_t::Identity()) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) , v(Motion_t::Zero()) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Identity()) - {} + , StU(D_t::Zero()) + { + joint_q[6] = Scalar(1); + } static std::string classname() { return std::string("JointDataFreeFlyer"); } std::string shortname() const { return classname(); } @@ -243,31 +260,18 @@ namespace pinocchio data.M.rotation(quat.matrix()); } - template - EIGEN_DONT_INLINE - void calc(JointDataDerived & data, - const typename Eigen::PlainObjectBase & qs) const - { - typedef typename Eigen::Quaternion Quaternion; - typedef Eigen::Map ConstQuaternionMap; - - typename ConfigVector::template ConstFixedSegmentReturnType::Type q = qs.template segment(idx_q()); - ConstQuaternionMap quat(q.template tail<4>().data()); - - calc(data,q.template head<3>(),quat); - } - template EIGEN_DONT_INLINE void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { typedef typename Eigen::Quaternion Quaternion; + typedef Eigen::Map ConstQuaternionMap; - typename ConfigVector::template ConstFixedSegmentReturnType::Type q = qs.template segment(idx_q()); - const Quaternion quat(q.template tail<4>()); + data.joint_q = qs.template segment(idx_q()); + ConstQuaternionMap quat(data.joint_q.template tail<4>().data()); - calc(data,q.template head<3>(),quat); + calc(data,data.joint_q.template head<3>(),quat); } template @@ -278,23 +282,25 @@ namespace pinocchio { calc(data,qs.derived()); - data.v = vs.template segment(idx_v()); + data.joint_v = vs.template segment(idx_v()); + data.v = data.joint_v; } - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { data.U = I; + data.StU = I; + data.StU.diagonal() += armature; - // compute inverse -// data.Dinv.setIdentity(); -// I.llt().solveInPlace(data.Dinv); - internal::PerformStYSInversion::run(I,data.Dinv); + internal::PerformStYSInversion::run(data.StU,data.Dinv); + data.UDinv.noalias() = I * data.Dinv; - if (update_I) - PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero(); + if(update_I) + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose(); } static std::string classname() { return std::string("JointModelFreeFlyer"); } @@ -335,4 +341,4 @@ namespace boost : public integral_constant {}; } -#endif // ifndef __pinocchio_joint_free_flyer_hpp__ +#endif // ifndef __pinocchio_multibody_joint_free_flyer_hpp__ diff --git a/src/multibody/joint/joint-generic.hpp b/src/multibody/joint/joint-generic.hpp index 395b28c1f1..7475352d70 100644 --- a/src/multibody/joint/joint-generic.hpp +++ b/src/multibody/joint/joint-generic.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2016-2021 CNRS INRIA // -#ifndef __pinocchio_joint_generic_hpp__ -#define __pinocchio_joint_generic_hpp__ +#ifndef __pinocchio_multibody_joint_generic_hpp__ +#define __pinocchio_multibody_joint_generic_hpp__ #include "pinocchio/multibody/joint/joint-collection.hpp" #include "pinocchio/multibody/joint/joint-composite.hpp" @@ -33,7 +33,7 @@ namespace pinocchio typedef JointDataTpl JointDataDerived; typedef JointModelTpl JointModelDerived; - typedef ConstraintTpl Constraint_t; + typedef JointMotionSubspaceTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionTpl Motion_t; typedef MotionTpl Bias_t; @@ -60,15 +60,26 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + typedef ConfigVector_t ConfigVectorTypeConstRef; + typedef ConfigVector_t ConfigVectorTypeRef; + typedef TangentVector_t TangentVectorTypeConstRef; + typedef TangentVector_t TangentVectorTypeRef; }; - template class JointCollectionTpl> - struct traits< JointDataTpl > - { typedef JointTpl JointDerived; }; + template class JointCollectionTpl> + struct traits< JointDataTpl<_Scalar,_Options,JointCollectionTpl> > + { + typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + typedef typename traits::Scalar Scalar; + }; - template class JointCollectionTpl> - struct traits< JointModelTpl > - { typedef JointTpl JointDerived; }; + template class JointCollectionTpl> + struct traits< JointModelTpl<_Scalar,_Options,JointCollectionTpl> > + { + typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + typedef typename traits::Scalar Scalar; + }; template class JointCollectionTpl> struct JointDataTpl @@ -91,7 +102,9 @@ namespace pinocchio JointDataVariant & toVariant() { return *static_cast(this); } const JointDataVariant & toVariant() const { return *static_cast(this); } - Constraint_t S() const { return constraint_xd(*this); } + ConfigVector_t joint_q() const { return pinocchio::joint_q(*this); } + TangentVector_t joint_v() const { return pinocchio::joint_v(*this); } + Constraint_t S() const { return joint_motin_subspace_xd(*this); } Transformation_t M() const { return joint_transform(*this); } Motion_t v() const { return motion(*this); } Bias_t c() const { return bias(*this); } @@ -100,6 +113,7 @@ namespace pinocchio U_t U() const { return u_inertia(*this); } D_t Dinv() const { return dinv_inertia(*this); } UD_t UDinv() const { return udinv_inertia(*this); } + D_t StU() const { return stu_inertia(*this); } JointDataTpl() : JointDataVariant() @@ -117,6 +131,8 @@ namespace pinocchio } // Define all the standard accessors + ConfigVector_t joint_q_accessor() const { return joint_q(); } + TangentVector_t joint_v_accessor() const { return joint_v(); } Constraint_t S_accessor() const { return S(); } Transformation_t M_accessor() const { return M(); } Motion_t v_accessor() const { return v(); } @@ -124,6 +140,7 @@ namespace pinocchio U_t U_accessor() const { return U(); } D_t Dinv_accessor() const { return Dinv(); } UD_t UDinv_accessor() const { return UDinv(); } + D_t StU_accessor() const { return StU(); } static std::string classname() { return "JointData"; } std::string shortname() const { return ::pinocchio::shortname(*this); } @@ -239,9 +256,17 @@ namespace pinocchio const Eigen::MatrixBase & v) const { calc_first_order(*this,data,q,v); } - template - void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & I, const bool update_I) const - { ::pinocchio::calc_aba(*this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); } + template + void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, + const Eigen::MatrixBase & I, + const bool update_I) const + { + ::pinocchio::calc_aba(*this,data, + armature.derived(), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I), + update_I); + } std::string shortname() const { return ::pinocchio::shortname(*this); } static std::string classname() { return "JointModel"; } @@ -302,4 +327,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_joint_generic_hpp__ +#endif // ifndef __pinocchio_multibody_joint_generic_hpp__ diff --git a/src/multibody/joint/joint-mimic.hpp b/src/multibody/joint/joint-mimic.hpp index d402ed14e2..e96fc27bc2 100644 --- a/src/multibody/joint/joint-mimic.hpp +++ b/src/multibody/joint/joint-mimic.hpp @@ -11,10 +11,10 @@ namespace pinocchio { - template struct ScaledConstraint; + template struct ScaledJointMotionSubspace; template - struct traits< ScaledConstraint > + struct traits< ScaledJointMotionSubspace > { typedef typename traits::Scalar Scalar; enum { Options = traits::Options }; @@ -22,23 +22,27 @@ namespace pinocchio LINEAR = traits::LINEAR, ANGULAR = traits::ANGULAR }; + typedef typename traits::JointMotion JointMotion; typedef typename traits::JointForce JointForce; typedef typename traits::DenseBase DenseBase; + typedef typename traits::ReducedSquaredMatrix ReducedSquaredMatrix; + typedef typename traits::MatrixReturnType MatrixReturnType; typedef typename traits::ConstMatrixReturnType ConstMatrixReturnType; - }; // traits ScaledConstraint + typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; + }; // traits ScaledJointMotionSubspace template - struct SE3GroupAction< ScaledConstraint > + struct SE3GroupAction< ScaledJointMotionSubspace > { typedef typename SE3GroupAction::ReturnType ReturnType; }; template - struct MotionAlgebraAction< ScaledConstraint, MotionDerived > + struct MotionAlgebraAction< ScaledJointMotionSubspace, MotionDerived > { typedef typename MotionAlgebraAction::ReturnType ReturnType; }; template - struct ConstraintForceOp< ScaledConstraint, ForceDerived> + struct ConstraintForceOp< ScaledJointMotionSubspace, ForceDerived> { typedef typename Constraint::Scalar Scalar; typedef typename ConstraintForceOp::ReturnType OriginalReturnType; @@ -48,7 +52,7 @@ namespace pinocchio }; template - struct ConstraintForceSetOp< ScaledConstraint, ForceSet> + struct ConstraintForceSetOp< ScaledJointMotionSubspace, ForceSet> { typedef typename Constraint::Scalar Scalar; typedef typename ConstraintForceSetOp::ReturnType OriginalReturnType; @@ -57,36 +61,36 @@ namespace pinocchio }; template - struct ScaledConstraint - : ConstraintBase< ScaledConstraint > + struct ScaledJointMotionSubspace + : JointMotionSubspaceBase< ScaledJointMotionSubspace > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledConstraint) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspace) enum { NV = Constraint::NV }; - typedef ConstraintBase Base; + typedef JointMotionSubspaceBase Base; using Base::nv; typedef typename SE3GroupAction::ReturnType SE3ActionReturnType; - ScaledConstraint() {} + ScaledJointMotionSubspace() {} - explicit ScaledConstraint(const Scalar & scaling_factor) + explicit ScaledJointMotionSubspace(const Scalar & scaling_factor) : m_scaling_factor(scaling_factor) {} - ScaledConstraint(const Constraint & constraint, + ScaledJointMotionSubspace(const Constraint & constraint, const Scalar & scaling_factor) : m_constraint(constraint) , m_scaling_factor(scaling_factor) {} - ScaledConstraint(const ScaledConstraint & other) + ScaledJointMotionSubspace(const ScaledJointMotionSubspace & other) : m_constraint(other.m_constraint) , m_scaling_factor(other.m_scaling_factor) {} - ScaledConstraint & operator=(const ScaledConstraint & other) + ScaledJointMotionSubspace & operator=(const ScaledJointMotionSubspace & other) { m_constraint = other.m_constraint; m_scaling_factor = other.m_scaling_factor; @@ -119,26 +123,26 @@ namespace pinocchio int nv_impl() const { return m_constraint.nv(); } - struct TransposeConst + struct TransposeConst : JointMotionSubspaceTransposeBase { - const ScaledConstraint & ref; - TransposeConst(const ScaledConstraint & ref) : ref(ref) {} + const ScaledJointMotionSubspace & ref; + TransposeConst(const ScaledJointMotionSubspace & ref) : ref(ref) {} template - typename ConstraintForceOp::ReturnType + typename ConstraintForceOp::ReturnType operator*(const ForceDense & f) const { - // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level; - typedef typename ConstraintForceOp::ReturnType ReturnType; + // TODO: I don't know why, but we should a dense a return type, otherwise it fails at the evaluation level; + typedef typename ConstraintForceOp::ReturnType ReturnType; return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); } /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) template - typename ConstraintForceSetOp::ReturnType + typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F) const { - typedef typename ConstraintForceSetOp::ReturnType ReturnType; + typedef typename ConstraintForceSetOp::ReturnType ReturnType; return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F)); } @@ -153,10 +157,10 @@ namespace pinocchio } template - typename MotionAlgebraAction::ReturnType + typename MotionAlgebraAction::ReturnType motionAction(const MotionDense & m) const { - typedef typename MotionAlgebraAction::ReturnType ReturnType; + typedef typename MotionAlgebraAction::ReturnType ReturnType; ReturnType res = m_scaling_factor * m_constraint.motionAction(m); return res; } @@ -167,23 +171,39 @@ namespace pinocchio inline const Constraint & constraint() const { return m_constraint; } inline Constraint & constraint() { return m_constraint; } - bool isEqual(const ScaledConstraint & other) const + bool isEqual(const ScaledJointMotionSubspace & other) const { - return m_constraint == other.m_constraint - && m_scaling_factor == other.m_scaling_factor; + return internal::comparison_eq(m_constraint, other.m_constraint) && + internal::comparison_eq(m_scaling_factor, other.m_scaling_factor); } protected: Constraint m_constraint; Scalar m_scaling_factor; - }; // struct ScaledConstraint + }; // struct ScaledJointMotionSubspace + + namespace details + { + template + struct StDiagonalMatrixSOperation< ScaledJointMotionSubspace > + { + typedef ScaledJointMotionSubspace Constraint; + typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; + + static ReturnType run(const JointMotionSubspaceBase & constraint) + { + const Constraint & constraint_ = constraint.derived(); + return (constraint_.constraint().transpose() * constraint_.constraint()) * (constraint_.scaling() * constraint_.scaling()); + } + }; + } template - struct MultiplicationOp, ScaledConstraint<_Constraint> > + struct MultiplicationOp, ScaledJointMotionSubspace<_Constraint> > { typedef InertiaTpl Inertia; - typedef ScaledConstraint<_Constraint> Constraint; + typedef ScaledJointMotionSubspace<_Constraint> Constraint; typedef typename Constraint::Scalar Scalar; typedef typename MultiplicationOp::ReturnType OriginalReturnType; @@ -195,10 +215,10 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ScaledConstraint<_Constraint> > + struct LhsMultiplicationOp, ScaledJointMotionSubspace<_Constraint> > { typedef InertiaTpl Inertia; - typedef ScaledConstraint<_Constraint> Constraint; + typedef ScaledJointMotionSubspace<_Constraint> Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, @@ -210,7 +230,7 @@ namespace pinocchio } // namespace impl template - struct MultiplicationOp, ScaledConstraint<_Constraint> > + struct MultiplicationOp, ScaledJointMotionSubspace<_Constraint> > { typedef typename MultiplicationOp::ReturnType OriginalReturnType; typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType; @@ -220,9 +240,9 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ScaledConstraint<_Constraint> > + struct LhsMultiplicationOp, ScaledJointMotionSubspace<_Constraint> > { - typedef ScaledConstraint<_Constraint> Constraint; + typedef ScaledJointMotionSubspace<_Constraint> Constraint; typedef typename MultiplicationOp,Constraint>::ReturnType ReturnType; static inline ReturnType run(const Eigen::MatrixBase & Y, @@ -254,7 +274,7 @@ namespace pinocchio typedef JointDataMimic JointDataDerived; typedef JointModelMimic JointModelDerived; - typedef ScaledConstraint::Constraint_t> Constraint_t; + typedef ScaledJointMotionSubspace::Constraint_t> Constraint_t; typedef typename traits::Transformation_t Transformation_t; typedef typename traits::Motion_t Motion_t; typedef typename traits::Bias_t Bias_t; @@ -264,19 +284,25 @@ namespace pinocchio typedef typename traits::D_t D_t; typedef typename traits::UD_t UD_t; - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE - typedef typename traits::ConfigVector_t ConfigVector_t; typedef typename traits::TangentVector_t TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; template struct traits< JointDataMimic > - { typedef JointMimic::JointDerived> JointDerived; }; + { + typedef JointMimic::JointDerived> JointDerived; + typedef typename traits::Scalar Scalar; + }; template struct traits< JointModelMimic > - { typedef JointMimic::JointDerived> JointDerived; }; + { + typedef JointMimic::JointDerived> JointDerived; + typedef typename traits::Scalar Scalar; + }; template struct JointDataMimic @@ -291,14 +317,11 @@ namespace pinocchio JointDataMimic() : m_scaling((Scalar)0) - , m_q_transform(ConfigVector_t::Zero()) - , m_v_transform(TangentVector_t::Zero()) + , joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) , S((Scalar)0) {} - JointDataMimic(const JointDataMimic & other) - { *this = other; } - JointDataMimic(const JointDataBase & jdata, const Scalar & scaling) : m_jdata_ref(jdata.derived()) @@ -310,8 +333,8 @@ namespace pinocchio { m_jdata_ref = other.m_jdata_ref; m_scaling = other.m_scaling; - m_q_transform = other.m_q_transform; - m_v_transform = other.m_v_transform; + joint_q = other.joint_q; + joint_v = other.joint_v; S = Constraint_t(m_jdata_ref.S,other.m_scaling); return *this; } @@ -320,10 +343,10 @@ namespace pinocchio bool isEqual(const JointDataMimic & other) const { return Base::isEqual(other) - && m_jdata_ref == other.m_jdata_ref - && m_scaling == other.m_scaling - && m_q_transform == other.m_q_transform - && m_v_transform == other.m_v_transform + && internal::comparison_eq(m_jdata_ref, other.m_jdata_ref) + && internal::comparison_eq(m_scaling, other.m_scaling) + && internal::comparison_eq(joint_q, other.joint_q) + && internal::comparison_eq(joint_v, other.joint_v) ; } @@ -338,6 +361,12 @@ namespace pinocchio } // Accessors + ConfigVectorTypeConstRef joint_q_accessor() const { return joint_q; } + ConfigVectorTypeRef joint_q_accessor() { return joint_q; } + + TangentVectorTypeConstRef joint_v_accessor() const { return joint_v; } + TangentVectorTypeRef joint_v_accessor() { return joint_v; } + ConstraintTypeConstRef S_accessor() const { return S; } ConstraintTypeRef S_accessor() { return S; } @@ -359,6 +388,9 @@ namespace pinocchio UDTypeConstRef UDinv_accessor() const { return m_jdata_ref.UDinv; } UDTypeRef UDinv_accessor() { return m_jdata_ref.UDinv; } + DTypeConstRef StU_accessor() const { return m_jdata_ref.StU; } + DTypeRef StU_accessor() { return m_jdata_ref.StU; } + template friend struct JointModelMimic; @@ -368,11 +400,11 @@ namespace pinocchio const Scalar & scaling() const { return m_scaling; } Scalar & scaling() { return m_scaling; } - ConfigVector_t & jointConfiguration() { return m_q_transform; } - const ConfigVector_t & jointConfiguration() const { return m_q_transform; } + ConfigVector_t & jointConfiguration() { return joint_q; } + const ConfigVector_t & jointConfiguration() const { return joint_q; } - TangentVector_t & jointVelocity() { return m_v_transform; } - const TangentVector_t & jointVelocity() const { return m_v_transform; } + TangentVector_t & jointVelocity() { return joint_v; } + const TangentVector_t & jointVelocity() const { return joint_v; } protected: @@ -380,9 +412,9 @@ namespace pinocchio Scalar m_scaling; /// \brief Transform configuration vector - ConfigVector_t m_q_transform; + ConfigVector_t joint_q; /// \brief Transform velocity vector. - TangentVector_t m_v_transform; + TangentVector_t joint_v; public: @@ -456,8 +488,8 @@ namespace pinocchio typedef typename ConfigVectorAffineTransform::Type AffineTransform; AffineTransform::run(qs.head(m_jmodel_ref.nq()), - m_scaling,m_offset,jdata.m_q_transform); - m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.m_q_transform); + m_scaling,m_offset,jdata.joint_q); + m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.joint_q); } template @@ -469,20 +501,22 @@ namespace pinocchio typedef typename ConfigVectorAffineTransform::Type AffineTransform; AffineTransform::run(qs.head(m_jmodel_ref.nq()), - m_scaling,m_offset,jdata.m_q_transform); - jdata.m_v_transform = m_scaling * vs.head(m_jmodel_ref.nv()); + m_scaling,m_offset,jdata.joint_q); + jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); m_jmodel_ref.calc(jdata.m_jdata_ref, - jdata.m_q_transform, - jdata.m_v_transform); + jdata.joint_q, + jdata.joint_v); } - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { // TODO: fixme m_jmodel_ref.calc_aba(data.m_jdata_ref, + armature, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I), update_I); } @@ -504,8 +538,8 @@ namespace pinocchio typedef typename CastType::type ReturnType; ReturnType res(m_jmodel_ref.template cast(), - (NewScalar)m_scaling, - (NewScalar)m_offset); + pinocchio::cast(m_scaling), + pinocchio::cast(m_offset)); res.setIndexes(id(),idx_q(),idx_v()); return res; } diff --git a/src/multibody/joint/joint-model-base.hpp b/src/multibody/joint/joint-model-base.hpp index f917c88d49..6d5442f66a 100644 --- a/src/multibody/joint/joint-model-base.hpp +++ b/src/multibody/joint/joint-model-base.hpp @@ -65,6 +65,7 @@ namespace pinocchio template struct JointModelBase + : NumericalBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -91,12 +92,16 @@ namespace pinocchio derived().calc(data,qs.derived(),vs.derived()); } - template + template void calc_aba(JointDataDerived & data, - const Eigen::MatrixBase & I, + const Eigen::MatrixBase & armature, + const Eigen::MatrixBase & I, const bool update_I = false) const { - derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I); + derived().calc_aba(data, + armature.derived(), + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I), + update_I); } int nv() const { return derived().nv_impl(); } @@ -152,7 +157,7 @@ namespace pinocchio template bool operator!=(const JointModelBase & other) const - { return !(derived() == other.derived()); } + { return !(internal::comparison_eq(derived(), other.derived())); } template bool isEqual(const JointModelBase &) const @@ -166,9 +171,9 @@ namespace pinocchio template bool hasSameIndexes(const JointModelBase & other) const { - return other.id() == id() - && other.idx_q() == idx_q() - && other.idx_v() == idx_v(); + return internal::comparison_eq(other.id(), id()) + && internal::comparison_eq(other.idx_q(), idx_q()) + && internal::comparison_eq(other.idx_v(), idx_v()); } /* Acces to dedicated segment in robot config space. */ diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp index d18d7f2ecf..d7070303c8 100644 --- a/src/multibody/joint/joint-planar.hpp +++ b/src/multibody/joint/joint-planar.hpp @@ -1,15 +1,15 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_planar_hpp__ -#define __pinocchio_joint_planar_hpp__ +#ifndef __pinocchio_multibody_joint_planar_hpp__ +#define __pinocchio_multibody_joint_planar_hpp__ #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" #include "pinocchio/spatial/cartesian-axis.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/spatial/motion.hpp" #include "pinocchio/spatial/inertia.hpp" @@ -61,7 +61,7 @@ namespace pinocchio EIGEN_MAKE_ALIGNED_OPERATOR_NEW MOTION_TYPEDEF_TPL(MotionPlanarTpl); - typedef CartesianAxis<2> AxisZ; + typedef CartesianAxis<2> ZAxis; MotionPlanarTpl() {} @@ -121,7 +121,7 @@ namespace pinocchio // Linear // TODO: use v.angular() as temporary variable Vector3 v3_tmp; - AxisZ::alphaCross(wz(),m.translation(),v3_tmp); + ZAxis::alphaCross(wz(),m.translation(),v3_tmp); v3_tmp[0] += vx(); v3_tmp[1] += vy(); v.linear().noalias() = m.rotation().transpose() * v3_tmp; @@ -141,7 +141,7 @@ namespace pinocchio void motionAction(const MotionDense & v, MotionDense & mout) const { // Linear - AxisZ::alphaCross(-wz(),v.linear(),mout.linear()); + ZAxis::alphaCross(-wz(),v.linear(),mout.linear()); typename M1::ConstAngularType w_in = v.angular(); typename M2::LinearType v_out = mout.linear(); @@ -151,7 +151,7 @@ namespace pinocchio v_out[2] += -w_in[1] * vx() + w_in[0] * vy() ; // Angular - AxisZ::alphaCross(-wz(),v.angular(),mout.angular()); + ZAxis::alphaCross(-wz(),v.angular(),mout.angular()); } template @@ -176,7 +176,7 @@ namespace pinocchio bool isEqual_impl(const MotionPlanarTpl & other) const { - return m_data == other.m_data; + return internal::comparison_eq(m_data, other.m_data); } protected: @@ -198,10 +198,10 @@ namespace pinocchio return result; } - template struct ConstraintPlanarTpl; + template struct JointMotionSubspacePlanarTpl; template - struct traits< ConstraintPlanarTpl<_Scalar,_Options> > + struct traits< JointMotionSubspacePlanarTpl<_Scalar,_Options> > { typedef _Scalar Scalar; enum { Options = _Options }; @@ -212,20 +212,24 @@ namespace pinocchio typedef MotionPlanarTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; - }; // struct traits ConstraintPlanarTpl + + typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; + }; // struct traits JointMotionSubspacePlanarTpl template - struct ConstraintPlanarTpl - : ConstraintBase< ConstraintPlanarTpl<_Scalar,_Options> > + struct JointMotionSubspacePlanarTpl + : JointMotionSubspaceBase< JointMotionSubspacePlanarTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPlanarTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspacePlanarTpl) enum { NV = 3 }; - ConstraintPlanarTpl() {}; + JointMotionSubspacePlanarTpl() {}; template JointMotion __mult__(const Eigen::MatrixBase & vj) const @@ -236,10 +240,10 @@ namespace pinocchio int nv_impl() const { return NV; } - struct ConstraintTranspose + struct ConstraintTranspose : JointMotionSubspaceTransposeBase { - const ConstraintPlanarTpl & ref; - ConstraintTranspose(const ConstraintPlanarTpl & ref) : ref(ref) {} + const JointMotionSubspacePlanarTpl & ref; + ConstraintTranspose(const JointMotionSubspacePlanarTpl & ref) : ref(ref) {} template typename ForceDense::Vector3 operator* (const ForceDense & phi) @@ -328,9 +332,9 @@ namespace pinocchio return res; } - bool isEqual(const ConstraintPlanarTpl &) const { return true; } + bool isEqual(const JointMotionSubspacePlanarTpl &) const { return true; } - }; // struct ConstraintPlanarTpl + }; // struct JointMotionSubspacePlanarTpl template inline typename MotionDerived::MotionPlain @@ -342,7 +346,7 @@ namespace pinocchio /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ template inline typename Eigen::Matrix - operator*(const InertiaTpl & Y, const ConstraintPlanarTpl &) + operator*(const InertiaTpl & Y, const JointMotionSubspacePlanarTpl &) { typedef InertiaTpl Inertia; typedef typename Inertia::Scalar Scalar; @@ -374,7 +378,7 @@ namespace pinocchio template inline Eigen::Matrix operator*(const Eigen::MatrixBase & Y, - const ConstraintPlanarTpl &) + const JointMotionSubspacePlanarTpl &) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6); typedef Eigen::Matrix Matrix63; @@ -387,11 +391,11 @@ namespace pinocchio } template - struct SE3GroupAction< ConstraintPlanarTpl > + struct SE3GroupAction< JointMotionSubspacePlanarTpl > { typedef Eigen::Matrix ReturnType; }; template - struct MotionAlgebraAction< ConstraintPlanarTpl,MotionDerived > + struct MotionAlgebraAction< JointMotionSubspacePlanarTpl,MotionDerived > { typedef Eigen::Matrix ReturnType; }; template struct JointPlanarTpl; @@ -407,7 +411,7 @@ namespace pinocchio typedef _Scalar Scalar; typedef JointDataPlanarTpl JointDataDerived; typedef JointModelPlanarTpl JointModelDerived; - typedef ConstraintPlanarTpl Constraint_t; + typedef JointMotionSubspacePlanarTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionPlanarTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -416,17 +420,25 @@ namespace pinocchio typedef Eigen::Matrix U_t; typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataPlanarTpl > { typedef JointPlanarTpl JointDerived; }; - template - struct traits< JointModelPlanarTpl > { typedef JointPlanarTpl JointDerived; }; + template + struct traits< JointDataPlanarTpl<_Scalar,_Options> > + { + typedef JointPlanarTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; + template + struct traits< JointModelPlanarTpl<_Scalar,_Options> > + { + typedef JointPlanarTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; template struct JointDataPlanarTpl : public JointDataBase< JointDataPlanarTpl<_Scalar,_Options> > @@ -436,6 +448,9 @@ namespace pinocchio PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + ConfigVector_t joint_q; + TangentVector_t joint_v; + Constraint_t S; Transformation_t M; Motion_t v; @@ -445,15 +460,17 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; - D_t StU; - JointDataPlanarTpl () - : M(Transformation_t::Identity()) + JointDataPlanarTpl() + : joint_q(Scalar(0),Scalar(0),Scalar(1),Scalar(0)) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) , v(Motion_t::Vector3::Zero()) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() { return std::string("JointDataPlanar"); } @@ -493,15 +510,14 @@ namespace pinocchio void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVector::Scalar Scalar; - typename ConfigVector::template ConstFixedSegmentReturnType::Type & q = qs.template segment(idx_q()); + data.joint_q = qs.template segment(idx_q()); const Scalar - &c_theta = q(2), - &s_theta = q(3); + & c_theta = data.joint_q(2), + & s_theta = data.joint_q(3); data.M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta; - data.M.translation().template head<2>() = q.template head<2>(); + data.M.translation().template head<2>() = data.joint_q.template head<2>(); } @@ -512,15 +528,18 @@ namespace pinocchio { calc(data,qs.derived()); - typename TangentVector::template ConstFixedSegmentReturnType::Type & q_dot = vs.template segment(idx_v ()); + data.joint_v = vs.template segment(idx_v()); +#define q_dot data.joint_v data.v.vx() = q_dot(0); data.v.vy() = q_dot(1); data.v.wz() = q_dot(2); +#undef q_dot } - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { @@ -530,9 +549,7 @@ namespace pinocchio data.StU.template leftCols<2>() = data.U.template topRows<2>().transpose(); data.StU.template rightCols<1>() = data.U.template bottomRows<1>(); - // compute inverse -// data.Dinv.setIdentity(); -// data.StU.llt().solveInPlace(data.Dinv); + data.StU.diagonal() += armature; internal::PerformStYSInversion::run(data.StU,data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; @@ -579,4 +596,4 @@ namespace boost : public integral_constant {}; } -#endif // ifndef __pinocchio_joint_planar_hpp__ +#endif // ifndef __pinocchio_multibody_joint_planar_hpp__ diff --git a/src/multibody/joint/joint-prismatic-unaligned.hpp b/src/multibody/joint/joint-prismatic-unaligned.hpp index e8c719c6ce..ba51bfdffc 100644 --- a/src/multibody/joint/joint-prismatic-unaligned.hpp +++ b/src/multibody/joint/joint-prismatic-unaligned.hpp @@ -1,15 +1,15 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_prismatic_unaligned_hpp__ -#define __pinocchio_joint_prismatic_unaligned_hpp__ +#ifndef __pinocchio_multibody_joint_prismatic_unaligned_hpp__ +#define __pinocchio_multibody_joint_prismatic_unaligned_hpp__ #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" #include "pinocchio/multibody/joint/joint-translation.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/spatial/inertia.hpp" #include "pinocchio/math/matrix.hpp" @@ -148,7 +148,8 @@ namespace pinocchio bool isEqual_impl(const MotionPrismaticUnalignedTpl & other) const { - return m_axis == other.m_axis && m_v == other.m_v; + return internal::comparison_eq(m_axis, other.m_axis) && + internal::comparison_eq(m_v, other.m_v); } const Scalar & linearRate() const { return m_v; } @@ -179,10 +180,10 @@ namespace pinocchio return m2.motionAction(m1); } - template struct ConstraintPrismaticUnalignedTpl; + template struct JointMotionSubspacePrismaticUnalignedTpl; template - struct traits< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> > + struct traits< JointMotionSubspacePrismaticUnalignedTpl<_Scalar,_Options> > { typedef _Scalar Scalar; enum { Options = _Options }; @@ -190,54 +191,58 @@ namespace pinocchio LINEAR = 0, ANGULAR = 3 }; + typedef MotionPrismaticUnalignedTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + typedef Eigen::Matrix Vector3; + typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; - typedef Eigen::Matrix Vector3; - }; // traits ConstraintPrismaticUnalignedTpl + typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; + }; // traits JointMotionSubspacePrismaticUnalignedTpl template - struct SE3GroupAction< ConstraintPrismaticUnalignedTpl > + struct SE3GroupAction< JointMotionSubspacePrismaticUnalignedTpl > { typedef Eigen::Matrix ReturnType; }; template - struct MotionAlgebraAction< ConstraintPrismaticUnalignedTpl,MotionDerived > + struct MotionAlgebraAction< JointMotionSubspacePrismaticUnalignedTpl,MotionDerived > { typedef Eigen::Matrix ReturnType; }; template - struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl, ForceDerived> + struct ConstraintForceOp< JointMotionSubspacePrismaticUnalignedTpl, ForceDerived> { - typedef typename traits< ConstraintPrismaticUnalignedTpl >::Vector3 Vector3; + typedef typename traits< JointMotionSubspacePrismaticUnalignedTpl >::Vector3 Vector3; typedef Eigen::Matrix::ConstAngularType),1,1,Options> ReturnType; }; template - struct ConstraintForceSetOp< ConstraintPrismaticUnalignedTpl, ForceSet> + struct ConstraintForceSetOp< JointMotionSubspacePrismaticUnalignedTpl, ForceSet> { - typedef typename traits< ConstraintPrismaticUnalignedTpl >::Vector3 Vector3; + typedef typename traits< JointMotionSubspacePrismaticUnalignedTpl >::Vector3 Vector3; typedef typename MatrixMatrixProduct, typename Eigen::MatrixBase::template NRowsBlockXpr<3>::Type >::type ReturnType; }; template - struct ConstraintPrismaticUnalignedTpl - : ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> > + struct JointMotionSubspacePrismaticUnalignedTpl + : JointMotionSubspaceBase< JointMotionSubspacePrismaticUnalignedTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticUnalignedTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspacePrismaticUnalignedTpl) enum { NV = 1 }; - typedef typename traits::Vector3 Vector3; + typedef typename traits::Vector3 Vector3; - ConstraintPrismaticUnalignedTpl() {} + JointMotionSubspacePrismaticUnalignedTpl() {} template - ConstraintPrismaticUnalignedTpl(const Eigen::MatrixBase & axis) + JointMotionSubspacePrismaticUnalignedTpl(const Eigen::MatrixBase & axis) : m_axis(axis) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); } @@ -249,10 +254,10 @@ namespace pinocchio } template - typename SE3GroupAction::ReturnType + typename SE3GroupAction::ReturnType se3Action(const SE3Tpl & m) const { - typename SE3GroupAction::ReturnType res; + typename SE3GroupAction::ReturnType res; MotionRef v(res); v.linear().noalias() = m.rotation()*m_axis; v.angular().setZero(); @@ -260,10 +265,10 @@ namespace pinocchio } template - typename SE3GroupAction::ReturnType + typename SE3GroupAction::ReturnType se3ActionInverse(const SE3Tpl & m) const { - typename SE3GroupAction::ReturnType res; + typename SE3GroupAction::ReturnType res; MotionRef v(res); v.linear().noalias() = m.rotation().transpose()*m_axis; v.angular().setZero(); @@ -272,16 +277,16 @@ namespace pinocchio int nv_impl() const { return NV; } - struct TransposeConst + struct TransposeConst : JointMotionSubspaceTransposeBase { - const ConstraintPrismaticUnalignedTpl & ref; - TransposeConst(const ConstraintPrismaticUnalignedTpl & ref) : ref(ref) {} + const JointMotionSubspacePrismaticUnalignedTpl & ref; + TransposeConst(const JointMotionSubspacePrismaticUnalignedTpl & ref) : ref(ref) {} template - typename ConstraintForceOp::ReturnType + typename ConstraintForceOp::ReturnType operator* (const ForceDense & f) const { - typedef typename ConstraintForceOp::ReturnType ReturnType; + typedef typename ConstraintForceOp::ReturnType ReturnType; ReturnType res; res[0] = ref.axis().dot(f.linear()); return res; @@ -289,7 +294,7 @@ namespace pinocchio /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ template - typename ConstraintForceSetOp::ReturnType + typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F) { EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE) @@ -328,19 +333,19 @@ namespace pinocchio const Vector3 & axis() const { return m_axis; } Vector3 & axis() { return m_axis; } - bool isEqual(const ConstraintPrismaticUnalignedTpl & other) const + bool isEqual(const JointMotionSubspacePrismaticUnalignedTpl & other) const { - return m_axis == other.m_axis; + return internal::comparison_eq(m_axis, other.m_axis); } protected: Vector3 m_axis; - }; // struct ConstraintPrismaticUnalignedTpl + }; // struct JointMotionSubspacePrismaticUnalignedTpl template - struct MultiplicationOp, ConstraintPrismaticUnalignedTpl > + struct MultiplicationOp, JointMotionSubspacePrismaticUnalignedTpl > { typedef Eigen::Matrix ReturnType; }; @@ -349,10 +354,10 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ConstraintPrismaticUnalignedTpl > + struct LhsMultiplicationOp, JointMotionSubspacePrismaticUnalignedTpl > { typedef InertiaTpl Inertia; - typedef ConstraintPrismaticUnalignedTpl Constraint; + typedef JointMotionSubspacePrismaticUnalignedTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, @@ -372,12 +377,12 @@ namespace pinocchio } // namespace impl template - struct MultiplicationOp, ConstraintPrismaticUnalignedTpl > + struct MultiplicationOp, JointMotionSubspacePrismaticUnalignedTpl > { typedef typename SizeDepType<3>::ColsReturn::ConstType M6LikeCols; typedef typename Eigen::internal::remove_const::type M6LikeColsNonConst; - typedef ConstraintPrismaticUnalignedTpl Constraint; + typedef JointMotionSubspacePrismaticUnalignedTpl Constraint; typedef typename Constraint::Vector3 Vector3; typedef const typename MatrixMatrixProduct::type ReturnType; }; @@ -386,9 +391,9 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ConstraintPrismaticUnalignedTpl > + struct LhsMultiplicationOp, JointMotionSubspacePrismaticUnalignedTpl > { - typedef ConstraintPrismaticUnalignedTpl Constraint; + typedef JointMotionSubspacePrismaticUnalignedTpl Constraint; typedef typename MultiplicationOp,Constraint>::ReturnType ReturnType; static inline ReturnType run(const Eigen::MatrixBase & Y, const Constraint & cru) @@ -412,7 +417,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataPrismaticUnalignedTpl JointDataDerived; typedef JointModelPrismaticUnalignedTpl JointModelDerived; - typedef ConstraintPrismaticUnalignedTpl Constraint_t; + typedef JointMotionSubspacePrismaticUnalignedTpl Constraint_t; typedef TransformTranslationTpl Transformation_t; typedef MotionPrismaticUnalignedTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -422,15 +427,18 @@ namespace pinocchio typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE - typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataPrismaticUnalignedTpl > - { typedef JointPrismaticUnalignedTpl JointDerived; }; + template + struct traits< JointDataPrismaticUnalignedTpl<_Scalar,_Options> > + { + typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; template struct JointDataPrismaticUnalignedTpl @@ -441,6 +449,9 @@ namespace pinocchio PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + ConfigVector_t joint_q; + TangentVector_t joint_v; + Transformation_t M; Constraint_t S; Motion_t v; @@ -450,22 +461,31 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; JointDataPrismaticUnalignedTpl() - : M(Transformation_t::Vector3::Zero()) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Vector3::Zero()) , S(Constraint_t::Vector3::Zero()) , v(Constraint_t::Vector3::Zero(),(Scalar)0) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} template JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase & axis) - : M() + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Vector3::Zero()) , S(axis) - , v(axis,(Scalar)NAN) - , U(), Dinv(), UDinv() + , v(axis,(Scalar)0) + , U(U_t::Zero()) + , Dinv(D_t::Zero()) + , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() { return std::string("JointDataPrismaticUnaligned"); } @@ -473,9 +493,12 @@ namespace pinocchio }; // struct JointDataPrismaticUnalignedTpl - template - struct traits< JointModelPrismaticUnalignedTpl > - { typedef JointPrismaticUnalignedTpl JointDerived; }; + template + struct traits< JointModelPrismaticUnalignedTpl<_Scalar,_Options> > + { + typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPrismaticUnalignedTpl); template @@ -500,7 +523,7 @@ namespace pinocchio const Scalar & z) : axis(x,y,z) { - axis.normalize(); + normalize(axis); assert(isUnitary(axis) && "Translation axis is not unitary"); } @@ -517,17 +540,15 @@ namespace pinocchio using Base::isEqual; bool isEqual(const JointModelPrismaticUnalignedTpl & other) const { - return Base::isEqual(other) && axis == other.axis; + return Base::isEqual(other) && internal::comparison_eq(axis, other.axis); } template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVector::Scalar Scalar; - const Scalar & q = qs[idx_q()]; - - data.M.translation().noalias() = axis * q; + data.joint_q[0] = qs[idx_q()]; + data.M.translation().noalias() = axis * data.joint_q[0]; } template @@ -537,16 +558,18 @@ namespace pinocchio { calc(data,qs.derived()); - typedef typename TangentVector::Scalar S2; - const S2 & v = vs[idx_v()]; - data.v.linearRate() = v; + data.joint_v[0] = vs[idx_v()]; + data.v.linearRate() = data.joint_v[0]; } - template - void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & I, const bool update_I) const + template + void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, + const Eigen::MatrixBase & I, + const bool update_I) const { data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis; - data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR)); + data.Dinv[0] = Scalar(1)/(axis.dot(data.U.template segment <3> (Inertia::LINEAR)) + armature[0]); data.UDinv.noalias() = data.U * data.Dinv; if (update_I) @@ -598,4 +621,4 @@ namespace boost } -#endif // ifndef __pinocchio_joint_prismatic_unaligned_hpp__ +#endif // ifndef __pinocchio_multibody_joint_prismatic_unaligned_hpp__ diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp index b2cdd12eb1..46ef324f08 100644 --- a/src/multibody/joint/joint-prismatic.hpp +++ b/src/multibody/joint/joint-prismatic.hpp @@ -1,14 +1,14 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_prismatic_hpp__ -#define __pinocchio_joint_prismatic_hpp__ +#ifndef __pinocchio_multibody_joint_prismatic_hpp__ +#define __pinocchio_multibody_joint_prismatic_hpp__ #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/spatial/inertia.hpp" #include "pinocchio/spatial/spatial-axis.hpp" #include "pinocchio/utils/axis-label.hpp" @@ -86,8 +86,9 @@ namespace pinocchio template void setTo(MotionDense & other) const { - for(Eigen::DenseIndex k = 0; k < 3; ++k) - other.linear()[k] = k == axis ? m_v : (Scalar)0; + for(Eigen::DenseIndex k = 0; k < 3; ++k) { + other.linear()[k] = k == axis ? m_v : Scalar(0); + } other.angular().setZero(); } @@ -141,14 +142,16 @@ namespace pinocchio motionAction(v,res); return res; } + Scalar & linearRate() { return m_v; } const Scalar & linearRate() const { return m_v; } - + bool isEqual_impl(const MotionPrismaticTpl & other) const { - return m_v == other.m_v; - } + return internal::comparison_eq(m_v, other.m_v); + } + protected: @@ -246,18 +249,19 @@ namespace pinocchio bool isEqual(const TransformPrismaticTpl & other) const { - return m_displacement == other.m_displacement; + return internal::comparison_eq(m_displacement, other.m_displacement); } + protected: Scalar m_displacement; }; - template struct ConstraintPrismaticTpl; + template struct JointMotionSubspacePrismaticTpl; template - struct traits< ConstraintPrismaticTpl<_Scalar,_Options,axis> > + struct traits< JointMotionSubspacePrismaticTpl<_Scalar,_Options,axis> > { typedef _Scalar Scalar; enum { Options = _Options }; @@ -268,37 +272,41 @@ namespace pinocchio typedef MotionPrismaticTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; + + typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; }; // traits ConstraintRevolute template - struct SE3GroupAction< ConstraintPrismaticTpl > + struct SE3GroupAction< JointMotionSubspacePrismaticTpl > { typedef Eigen::Matrix ReturnType; }; template - struct MotionAlgebraAction< ConstraintPrismaticTpl, MotionDerived > + struct MotionAlgebraAction< JointMotionSubspacePrismaticTpl, MotionDerived > { typedef Eigen::Matrix ReturnType; }; template - struct ConstraintForceOp< ConstraintPrismaticTpl, ForceDerived> + struct ConstraintForceOp< JointMotionSubspacePrismaticTpl, ForceDerived> { typedef typename ForceDense::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType; }; template - struct ConstraintForceSetOp< ConstraintPrismaticTpl, ForceSet> + struct ConstraintForceSetOp< JointMotionSubspacePrismaticTpl, ForceSet> { typedef typename Eigen::MatrixBase::ConstRowXpr ReturnType; }; template - struct ConstraintPrismaticTpl - : ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> > + struct JointMotionSubspacePrismaticTpl + : JointMotionSubspaceBase < JointMotionSubspacePrismaticTpl <_Scalar,_Options,axis> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspacePrismaticTpl) enum { NV = 1 }; typedef SpatialAxis Axis; - ConstraintPrismaticTpl() {}; + JointMotionSubspacePrismaticTpl() {}; template JointMotion __mult__(const Eigen::MatrixBase & v) const @@ -309,10 +317,10 @@ namespace pinocchio } template - typename SE3GroupAction::ReturnType + typename SE3GroupAction::ReturnType se3Action(const SE3Tpl & m) const { - typename SE3GroupAction::ReturnType res; + typename SE3GroupAction::ReturnType res; MotionRef v(res); v.linear() = m.rotation().col(axis); v.angular().setZero(); @@ -320,10 +328,10 @@ namespace pinocchio } template - typename SE3GroupAction::ReturnType + typename SE3GroupAction::ReturnType se3ActionInverse(const SE3Tpl & m) const { - typename SE3GroupAction::ReturnType res; + typename SE3GroupAction::ReturnType res; MotionRef v(res); v.linear() = m.rotation().transpose().col(axis); v.angular().setZero(); @@ -332,19 +340,19 @@ namespace pinocchio int nv_impl() const { return NV; } - struct TransposeConst + struct TransposeConst : JointMotionSubspaceTransposeBase { - const ConstraintPrismaticTpl & ref; - TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {} + const JointMotionSubspacePrismaticTpl & ref; + TransposeConst(const JointMotionSubspacePrismaticTpl & ref) : ref(ref) {} template - typename ConstraintForceOp::ReturnType + typename ConstraintForceOp::ReturnType operator* (const ForceDense & f) const { return f.linear().template segment<1>(axis); } /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ template - typename ConstraintForceSetOp::ReturnType + typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F ) { assert(F.rows()==6); @@ -369,21 +377,21 @@ namespace pinocchio } template - typename MotionAlgebraAction::ReturnType + typename MotionAlgebraAction::ReturnType motionAction(const MotionDense & m) const { - typename MotionAlgebraAction::ReturnType res; + typename MotionAlgebraAction::ReturnType res; MotionRef v(res); v = m.cross(Axis()); return res; } - bool isEqual(const ConstraintPrismaticTpl &) const { return true; } + bool isEqual(const JointMotionSubspacePrismaticTpl &) const { return true; } - }; // struct ConstraintPrismaticTpl + }; // struct JointMotionSubspacePrismaticTpl template - struct MultiplicationOp, ConstraintPrismaticTpl > + struct MultiplicationOp, JointMotionSubspacePrismaticTpl > { typedef Eigen::Matrix ReturnType; }; @@ -392,10 +400,10 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ConstraintPrismaticTpl > + struct LhsMultiplicationOp, JointMotionSubspacePrismaticTpl > { typedef InertiaTpl Inertia; - typedef ConstraintPrismaticTpl Constraint; + typedef JointMotionSubspacePrismaticTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) @@ -414,10 +422,10 @@ namespace pinocchio }; template - struct LhsMultiplicationOp, ConstraintPrismaticTpl > + struct LhsMultiplicationOp, JointMotionSubspacePrismaticTpl > { typedef InertiaTpl Inertia; - typedef ConstraintPrismaticTpl Constraint; + typedef JointMotionSubspacePrismaticTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) @@ -437,10 +445,10 @@ namespace pinocchio }; template - struct LhsMultiplicationOp, ConstraintPrismaticTpl > + struct LhsMultiplicationOp, JointMotionSubspacePrismaticTpl > { typedef InertiaTpl Inertia; - typedef ConstraintPrismaticTpl Constraint; + typedef JointMotionSubspacePrismaticTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) @@ -461,7 +469,7 @@ namespace pinocchio } // namespace impl template - struct MultiplicationOp, ConstraintPrismaticTpl > + struct MultiplicationOp, JointMotionSubspacePrismaticTpl > { typedef typename M6Like::ConstColXpr ReturnType; }; @@ -470,9 +478,9 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ConstraintPrismaticTpl > + struct LhsMultiplicationOp, JointMotionSubspacePrismaticTpl > { - typedef ConstraintPrismaticTpl Constraint; + typedef JointMotionSubspacePrismaticTpl Constraint; typedef typename MultiplicationOp,Constraint>::ReturnType ReturnType; static inline ReturnType run(const Eigen::MatrixBase & Y, const Constraint & /*constraint*/) @@ -506,7 +514,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataPrismaticTpl JointDataDerived; typedef JointModelPrismaticTpl JointModelDerived; - typedef ConstraintPrismaticTpl Constraint_t; + typedef JointMotionSubspacePrismaticTpl Constraint_t; typedef TransformPrismaticTpl Transformation_t; typedef MotionPrismaticTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -516,27 +524,37 @@ namespace pinocchio typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE - typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataPrismaticTpl > - { typedef JointPrismaticTpl JointDerived; }; + template + struct traits< JointDataPrismaticTpl<_Scalar,_Options,axis> > + { + typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits< JointModelPrismaticTpl > - { typedef JointPrismaticTpl JointDerived; }; + template + struct traits< JointModelPrismaticTpl<_Scalar,_Options,axis> > + { + typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived; + typedef _Scalar Scalar; + }; template - struct JointDataPrismaticTpl : public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> > + struct JointDataPrismaticTpl +: public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + + ConfigVector_t joint_q; + TangentVector_t joint_v; Constraint_t S; Transformation_t M; @@ -547,13 +565,17 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; JointDataPrismaticTpl() - : M((Scalar)0) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M((Scalar)0) , v((Scalar)0) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() @@ -590,9 +612,8 @@ namespace pinocchio void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVector::Scalar Scalar; - const Scalar & q = qs[idx_q()]; - data.M.displacement() = q; + data.joint_q[0] = qs[idx_q()]; + data.M.displacement() = data.joint_q[0]; } template @@ -602,16 +623,18 @@ namespace pinocchio { calc(data,qs.derived()); - typedef typename TangentVector::Scalar S2; - const S2 & v = vs[idx_v()]; - data.v.linearRate() = v; + data.joint_v[0] = vs[idx_v()]; + data.v.linearRate() = data.joint_v[0]; } - template - void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & I, const bool update_I) const + template + void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, + const Eigen::MatrixBase & I, + const bool update_I) const { data.U = I.col(Inertia::LINEAR + axis); - data.Dinv[0] = Scalar(1)/I(Inertia::LINEAR + axis, Inertia::LINEAR + axis); + data.Dinv[0] = Scalar(1)/(I(Inertia::LINEAR + axis, Inertia::LINEAR + axis) + armature[0]); data.UDinv.noalias() = data.U * data.Dinv[0]; if (update_I) @@ -671,4 +694,4 @@ namespace boost : public integral_constant {}; } -#endif // ifndef __pinocchio_joint_prismatic_hpp__ +#endif // ifndef __pinocchio_multibody_joint_prismatic_hpp__ diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp index c4b61fc1ec..f118ad232c 100644 --- a/src/multibody/joint/joint-revolute-unaligned.hpp +++ b/src/multibody/joint/joint-revolute-unaligned.hpp @@ -1,16 +1,18 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_revolute_unaligned_hpp__ -#define __pinocchio_joint_revolute_unaligned_hpp__ +#ifndef __pinocchio_multibody_joint_revolute_unaligned_hpp__ +#define __pinocchio_multibody_joint_revolute_unaligned_hpp__ #include "pinocchio/fwd.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/spatial/inertia.hpp" + #include "pinocchio/math/matrix.hpp" +#include "pinocchio/math/rotation.hpp" namespace pinocchio { @@ -157,7 +159,8 @@ namespace pinocchio bool isEqual_impl(const MotionRevoluteUnalignedTpl & other) const { - return m_axis == other.m_axis && m_w == other.m_w; + return internal::comparison_eq(m_axis, other.m_axis) && + internal::comparison_eq(m_w, other.m_w); } const Scalar & angularRate() const { return m_w; } @@ -190,10 +193,10 @@ namespace pinocchio return m2.motionAction(m1); } - template struct ConstraintRevoluteUnalignedTpl; + template struct JointMotionSubspaceRevoluteUnalignedTpl; template - struct traits< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> > + struct traits< JointMotionSubspaceRevoluteUnalignedTpl<_Scalar,_Options> > { typedef _Scalar Scalar; enum { Options = _Options }; @@ -201,54 +204,59 @@ namespace pinocchio LINEAR = 0, ANGULAR = 3 }; + typedef MotionRevoluteUnalignedTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; typedef Eigen::Matrix Vector3; - }; // traits ConstraintRevoluteUnalignedTpl + + typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; + }; // traits JointMotionSubspaceRevoluteUnalignedTpl template - struct SE3GroupAction< ConstraintRevoluteUnalignedTpl > + struct SE3GroupAction< JointMotionSubspaceRevoluteUnalignedTpl > { typedef Eigen::Matrix ReturnType; }; template - struct MotionAlgebraAction< ConstraintRevoluteUnalignedTpl, MotionDerived > + struct MotionAlgebraAction< JointMotionSubspaceRevoluteUnalignedTpl, MotionDerived > { typedef Eigen::Matrix ReturnType; }; template - struct ConstraintForceOp< ConstraintRevoluteUnalignedTpl, ForceDerived> + struct ConstraintForceOp< JointMotionSubspaceRevoluteUnalignedTpl, ForceDerived> { - typedef typename traits< ConstraintRevoluteUnalignedTpl >::Vector3 Vector3; + typedef typename traits< JointMotionSubspaceRevoluteUnalignedTpl >::Vector3 Vector3; typedef Eigen::Matrix::ConstAngularType),1,1,Options> ReturnType; }; template - struct ConstraintForceSetOp< ConstraintRevoluteUnalignedTpl, ForceSet> + struct ConstraintForceSetOp< JointMotionSubspaceRevoluteUnalignedTpl, ForceSet> { - typedef typename traits< ConstraintRevoluteUnalignedTpl >::Vector3 Vector3; + typedef typename traits< JointMotionSubspaceRevoluteUnalignedTpl >::Vector3 Vector3; typedef typename MatrixMatrixProduct, typename Eigen::MatrixBase::template NRowsBlockXpr<3>::Type >::type ReturnType; }; template - struct ConstraintRevoluteUnalignedTpl - : ConstraintBase< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> > + struct JointMotionSubspaceRevoluteUnalignedTpl + : JointMotionSubspaceBase< JointMotionSubspaceRevoluteUnalignedTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteUnalignedTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceRevoluteUnalignedTpl) enum { NV = 1 }; - typedef typename traits::Vector3 Vector3; + typedef typename traits::Vector3 Vector3; - ConstraintRevoluteUnalignedTpl() {} + JointMotionSubspaceRevoluteUnalignedTpl() {} template - ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase & axis) + JointMotionSubspaceRevoluteUnalignedTpl(const Eigen::MatrixBase & axis) : m_axis(axis) {} @@ -260,10 +268,10 @@ namespace pinocchio } template - typename SE3GroupAction::ReturnType + typename SE3GroupAction::ReturnType se3Action(const SE3Tpl & m) const { - typedef typename SE3GroupAction::ReturnType ReturnType; + typedef typename SE3GroupAction::ReturnType ReturnType; /* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */ ReturnType res; @@ -273,10 +281,10 @@ namespace pinocchio } template - typename SE3GroupAction::ReturnType + typename SE3GroupAction::ReturnType se3ActionInverse(const SE3Tpl & m) const { - typedef typename SE3GroupAction::ReturnType ReturnType; + typedef typename SE3GroupAction::ReturnType ReturnType; ReturnType res; res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis; @@ -286,16 +294,16 @@ namespace pinocchio int nv_impl() const { return NV; } - struct TransposeConst + struct TransposeConst : JointMotionSubspaceTransposeBase { - const ConstraintRevoluteUnalignedTpl & ref; - TransposeConst(const ConstraintRevoluteUnalignedTpl & ref) : ref(ref) {} + const JointMotionSubspaceRevoluteUnalignedTpl & ref; + TransposeConst(const JointMotionSubspaceRevoluteUnalignedTpl & ref) : ref(ref) {} template - typename ConstraintForceOp::ReturnType + typename ConstraintForceOp::ReturnType operator*(const ForceDense & f) const { - typedef typename ConstraintForceOp::ReturnType ReturnType; + typedef typename ConstraintForceOp::ReturnType ReturnType; ReturnType res; res[0] = ref.axis().dot(f.angular()); return res; @@ -303,7 +311,7 @@ namespace pinocchio /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ template - typename ConstraintForceSetOp::ReturnType + typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F) { EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE) @@ -330,7 +338,7 @@ namespace pinocchio } template - typename MotionAlgebraAction::ReturnType + typename MotionAlgebraAction::ReturnType motionAction(const MotionDense & m) const { const typename MotionDerived::ConstLinearType v = m.linear(); @@ -346,19 +354,19 @@ namespace pinocchio const Vector3 & axis() const { return m_axis; } Vector3 & axis() { return m_axis; } - bool isEqual(const ConstraintRevoluteUnalignedTpl & other) const + bool isEqual(const JointMotionSubspaceRevoluteUnalignedTpl & other) const { - return m_axis == other.m_axis; + return internal::comparison_eq(m_axis, other.m_axis); } protected: Vector3 m_axis; - }; // struct ConstraintRevoluteUnalignedTpl + }; // struct JointMotionSubspaceRevoluteUnalignedTpl template - struct MultiplicationOp, ConstraintRevoluteUnalignedTpl > + struct MultiplicationOp, JointMotionSubspaceRevoluteUnalignedTpl > { typedef Eigen::Matrix ReturnType; }; @@ -367,10 +375,10 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ConstraintRevoluteUnalignedTpl > + struct LhsMultiplicationOp, JointMotionSubspaceRevoluteUnalignedTpl > { typedef InertiaTpl Inertia; - typedef ConstraintRevoluteUnalignedTpl Constraint; + typedef JointMotionSubspaceRevoluteUnalignedTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & cru) @@ -392,12 +400,12 @@ namespace pinocchio } // namespace impl template - struct MultiplicationOp, ConstraintRevoluteUnalignedTpl > + struct MultiplicationOp, JointMotionSubspaceRevoluteUnalignedTpl > { typedef typename SizeDepType<3>::ColsReturn::ConstType M6LikeCols; typedef typename Eigen::internal::remove_const::type M6LikeColsNonConst; - typedef ConstraintRevoluteUnalignedTpl Constraint; + typedef JointMotionSubspaceRevoluteUnalignedTpl Constraint; typedef typename Constraint::Vector3 Vector3; typedef const typename MatrixMatrixProduct::type ReturnType; }; @@ -406,9 +414,9 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ConstraintRevoluteUnalignedTpl > + struct LhsMultiplicationOp, JointMotionSubspaceRevoluteUnalignedTpl > { - typedef ConstraintRevoluteUnalignedTpl Constraint; + typedef JointMotionSubspaceRevoluteUnalignedTpl Constraint; typedef typename MultiplicationOp,Constraint>::ReturnType ReturnType; static inline ReturnType run(const Eigen::MatrixBase & Y, @@ -433,7 +441,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataRevoluteUnalignedTpl JointDataDerived; typedef JointModelRevoluteUnalignedTpl JointModelDerived; - typedef ConstraintRevoluteUnalignedTpl Constraint_t; + typedef JointMotionSubspaceRevoluteUnalignedTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionRevoluteUnalignedTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -443,20 +451,26 @@ namespace pinocchio typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE - typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE + }; - template - struct traits< JointDataRevoluteUnalignedTpl > - { typedef JointRevoluteUnalignedTpl JointDerived; }; + template + struct traits< JointDataRevoluteUnalignedTpl<_Scalar,_Options> > + { + typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits > - { typedef JointRevoluteUnalignedTpl JointDerived; }; + template + struct traits< JointModelRevoluteUnalignedTpl<_Scalar,_Options> > + { + typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; template struct JointDataRevoluteUnalignedTpl @@ -466,6 +480,9 @@ namespace pinocchio typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + + ConfigVector_t joint_q; + TangentVector_t joint_v; Transformation_t M; Constraint_t S; @@ -476,24 +493,31 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; JointDataRevoluteUnalignedTpl() - : M(Transformation_t::Identity()) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) , S(Constraint_t::Vector3::Zero()) , v(Constraint_t::Vector3::Zero(),(Scalar)0) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} template JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase & axis) - : M(Transformation_t::Identity()) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) , S(axis) , v(axis,(Scalar)NAN) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() { return std::string("JointDataRevoluteUnaligned"); } @@ -524,7 +548,7 @@ namespace pinocchio const Scalar & z) : axis(x,y,z) { - axis.normalize(); + normalize(axis); assert(isUnitary(axis) && "Rotation axis is not unitary"); } @@ -541,19 +565,17 @@ namespace pinocchio using Base::isEqual; bool isEqual(const JointModelRevoluteUnalignedTpl & other) const { - return Base::isEqual(other) && axis == other.axis; + return Base::isEqual(other) && + internal::comparison_eq(axis, other.axis); } template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVector::Scalar OtherScalar; - typedef Eigen::AngleAxis AngleAxis; - - const OtherScalar & q = qs[idx_q()]; + data.joint_q[0] = qs[idx_q()]; - data.M.rotation(AngleAxis(q,axis).toRotationMatrix()); + toRotationMatrix(axis,data.joint_q[0],data.M.rotation()); } template @@ -566,11 +588,14 @@ namespace pinocchio data.v.angularRate() = static_cast(vs[idx_v()]); } - template - void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & I, const bool update_I) const + template + void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, + const Eigen::MatrixBase & I, + const bool update_I) const { data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis; - data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR)); + data.Dinv[0] = Scalar(1)/(axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + armature[0]); data.UDinv.noalias() = data.U * data.Dinv; if (update_I) @@ -622,4 +647,4 @@ namespace boost } -#endif // ifndef __pinocchio_joint_revolute_unaligned_hpp__ +#endif // ifndef __pinocchio_multibody_joint_revolute_unaligned_hpp__ diff --git a/src/multibody/joint/joint-revolute-unbounded-unaligned.hpp b/src/multibody/joint/joint-revolute-unbounded-unaligned.hpp index 22d8c749bc..d2ac90a0fc 100644 --- a/src/multibody/joint/joint-revolute-unbounded-unaligned.hpp +++ b/src/multibody/joint/joint-revolute-unbounded-unaligned.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2019 INRIA +// Copyright (c) 2019-2020 INRIA // -#ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__ -#define __pinocchio_joint_revolute_unbounded_unaligned_hpp__ +#ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__ +#define __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__ #include "pinocchio/fwd.hpp" #include "pinocchio/spatial/inertia.hpp" @@ -32,7 +32,7 @@ namespace pinocchio typedef JointDataRevoluteUnboundedUnalignedTpl JointDataDerived; typedef JointModelRevoluteUnboundedUnalignedTpl JointModelDerived; - typedef ConstraintRevoluteUnalignedTpl Constraint_t; + typedef JointMotionSubspaceRevoluteUnalignedTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionRevoluteUnalignedTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -46,13 +46,19 @@ namespace pinocchio PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataRevoluteUnboundedUnalignedTpl > - { typedef JointRevoluteUnboundedUnalignedTpl JointDerived; }; + template + struct traits< JointDataRevoluteUnboundedUnalignedTpl<_Scalar,_Options> > + { + typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits > - { typedef JointRevoluteUnboundedUnalignedTpl JointDerived; }; + template + struct traits< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> > + { + typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; template struct JointDataRevoluteUnboundedUnalignedTpl @@ -63,6 +69,9 @@ namespace pinocchio PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + ConfigVector_t joint_q; + TangentVector_t joint_v; + Transformation_t M; Constraint_t S; Motion_t v; @@ -72,24 +81,31 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; JointDataRevoluteUnboundedUnalignedTpl() - : M(Transformation_t::Identity()) + : joint_q(Scalar(1),Scalar(0)) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) , S(Constraint_t::Vector3::Zero()) , v(Constraint_t::Vector3::Zero(),(Scalar)0) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} template JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase & axis) - : M(Transformation_t::Identity()) + : joint_q(Scalar(1),Scalar(0)) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) , S(axis) - , v(axis,(Scalar)NAN) + , v(axis,(Scalar)0) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() { return std::string("JointDataRevoluteUnboundedUnalignedTpl"); } @@ -121,7 +137,7 @@ namespace pinocchio const Scalar & z) : axis(x,y,z) { - axis.normalize(); + normalize(axis); assert(isUnitary(axis) && "Rotation axis is not unitary"); } @@ -138,19 +154,17 @@ namespace pinocchio using Base::isEqual; bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const { - return Base::isEqual(other) && axis == other.axis; + return Base::isEqual(other) && internal::comparison_eq(axis, other.axis); } template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVector::Scalar OtherScalar; - typename ConfigVector::template ConstFixedSegmentReturnType::Type - & q = qs.template segment(idx_q()); + data.joint_q = qs.template segment(idx_q()); - const OtherScalar & ca = q(0); - const OtherScalar & sa = q(1); + const Scalar & ca = data.joint_q(0); + const Scalar & sa = data.joint_q(1); toRotationMatrix(axis,ca,sa,data.M.rotation()); } @@ -161,16 +175,18 @@ namespace pinocchio const typename Eigen::MatrixBase & vs) const { calc(data,qs.derived()); - data.v.angularRate() = static_cast(vs[idx_v()]); + data.joint_v[0] = vs[idx_v()]; + data.v.angularRate() = data.joint_v[0]; } - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis; - data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR)); + data.Dinv[0] = Scalar(1)/(axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + armature[0]); data.UDinv.noalias() = data.U * data.Dinv; if (update_I) @@ -192,7 +208,7 @@ namespace pinocchio // data /// - /// \brief 3d main axis of the joint. + /// \brief axis of rotation of the joint. /// Vector3 axis; }; // struct JointModelRevoluteUnboundedUnalignedTpl @@ -221,4 +237,4 @@ namespace boost } -#endif // ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__ +#endif // ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__ diff --git a/src/multibody/joint/joint-revolute-unbounded.hpp b/src/multibody/joint/joint-revolute-unbounded.hpp index 102d5783ca..d031e42b02 100644 --- a/src/multibody/joint/joint-revolute-unbounded.hpp +++ b/src/multibody/joint/joint-revolute-unbounded.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2016-2019 CNRS INRIA +// Copyright (c) 2016-2020 CNRS INRIA // -#ifndef __pinocchio_joint_revolute_unbounded_hpp__ -#define __pinocchio_joint_revolute_unbounded_hpp__ +#ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__ +#define __pinocchio_multibody_joint_revolute_unbounded_hpp__ #include "pinocchio/math/fwd.hpp" #include "pinocchio/math/sincos.hpp" @@ -27,7 +27,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataRevoluteUnboundedTpl JointDataDerived; typedef JointModelRevoluteUnboundedTpl JointModelDerived; - typedef ConstraintRevoluteTpl Constraint_t; + typedef JointMotionSubspaceRevoluteTpl Constraint_t; typedef TransformRevoluteTpl Transformation_t; typedef MotionRevoluteTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -36,28 +36,38 @@ namespace pinocchio typedef Eigen::Matrix U_t; typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataRevoluteUnboundedTpl > - { typedef JointRevoluteUnboundedTpl JointDerived; }; + template + struct traits< JointDataRevoluteUnboundedTpl<_Scalar,_Options,axis> > + { + typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits< JointModelRevoluteUnboundedTpl > - { typedef JointRevoluteUnboundedTpl JointDerived; }; + template + struct traits< JointModelRevoluteUnboundedTpl<_Scalar,_Options,axis> > + { + typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived; + typedef _Scalar Scalar; + }; template - struct JointDataRevoluteUnboundedTpl : public JointDataBase< JointDataRevoluteUnboundedTpl<_Scalar,_Options,axis> > + struct JointDataRevoluteUnboundedTpl + : public JointDataBase< JointDataRevoluteUnboundedTpl<_Scalar,_Options,axis> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + + ConfigVector_t joint_q; + TangentVector_t joint_v; Constraint_t S; Transformation_t M; @@ -68,13 +78,17 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; JointDataRevoluteUnboundedTpl() - : M((Scalar)0,(Scalar)1) + : joint_q(Scalar(1),Scalar(0)) + , joint_v(TangentVector_t::Zero()) + , M((Scalar)0,(Scalar)1) , v((Scalar)0) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() @@ -112,12 +126,10 @@ namespace pinocchio void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVector::Scalar OtherScalar; - typename ConfigVector::template ConstFixedSegmentReturnType::Type - & q = qs.template segment (idx_q()); + data.joint_q = qs.template segment(idx_q()); - const OtherScalar & ca = q(0); - const OtherScalar & sa = q(1); + const Scalar & ca = data.joint_q[0]; + const Scalar & sa = data.joint_q[1]; data.M.setValues(sa,ca); } @@ -128,15 +140,18 @@ namespace pinocchio const typename Eigen::MatrixBase & vs) const { calc(data,qs.derived()); - - data.v.angularRate() = static_cast(vs[idx_v()]); + data.joint_v[0] = vs[idx_v()]; + data.v.angularRate() = data.joint_v[0]; } - template - void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & I, const bool update_I) const + template + void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, + const Eigen::MatrixBase & I, + const bool update_I) const { data.U = I.col(Inertia::ANGULAR + axis); - data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis); + data.Dinv[0] = (Scalar)(1)/(I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis) + armature[0]); data.UDinv.noalias() = data.U * data.Dinv[0]; if (update_I) @@ -224,4 +239,4 @@ namespace boost : public integral_constant {}; } -#endif // ifndef __pinocchio_joint_revolute_unbounded_hpp__ +#endif // ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__ diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp index 2e7ffc7c94..048bcaa46d 100644 --- a/src/multibody/joint/joint-revolute.hpp +++ b/src/multibody/joint/joint-revolute.hpp @@ -1,14 +1,14 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_revolute_hpp__ -#define __pinocchio_joint_revolute_hpp__ +#ifndef __pinocchio_multibody_joint_revolute_hpp__ +#define __pinocchio_multibody_joint_revolute_hpp__ #include "pinocchio/math/sincos.hpp" #include "pinocchio/spatial/inertia.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" #include "pinocchio/spatial/spatial-axis.hpp" #include "pinocchio/utils/axis-label.hpp" @@ -151,8 +151,12 @@ namespace pinocchio void setValues(const OtherScalar & sin, const OtherScalar & cos) { m_sin = sin; m_cos = cos; } - LinearType translation() const { return LinearType::PlainObject::Zero(3); }; - AngularType rotation() const { + LinearType translation() const + { + return LinearType::PlainObject::Zero(3); + } + AngularType rotation() const + { AngularType m(AngularType::Identity(3)); _setRotation (m); return m; @@ -160,7 +164,8 @@ namespace pinocchio bool isEqual(const TransformRevoluteTpl & other) const { - return m_cos == other.m_cos && m_sin == other.m_sin; + return internal::comparison_eq(m_cos, other.m_cos) && + internal::comparison_eq(m_sin, other.m_sin); } protected: @@ -231,8 +236,9 @@ namespace pinocchio void setTo(MotionDense & m) const { m.linear().setZero(); - for(Eigen::DenseIndex k = 0; k < 3; ++k) - m.angular()[k] = k == axis ? m_w : (Scalar)0; + for(Eigen::DenseIndex k = 0; k < 3; ++k){ + m.angular()[k] = k == axis ? m_w : Scalar(0); + } } template @@ -301,7 +307,7 @@ namespace pinocchio bool isEqual_impl(const MotionRevoluteTpl & other) const { - return m_w == other.m_w; + return internal::comparison_eq(m_w, other.m_w); } protected: @@ -327,26 +333,26 @@ namespace pinocchio return m2.motionAction(m1); } - template struct ConstraintRevoluteTpl; + template struct JointMotionSubspaceRevoluteTpl; template - struct SE3GroupAction< ConstraintRevoluteTpl > + struct SE3GroupAction< JointMotionSubspaceRevoluteTpl > { typedef Eigen::Matrix ReturnType; }; template - struct MotionAlgebraAction< ConstraintRevoluteTpl, MotionDerived > + struct MotionAlgebraAction< JointMotionSubspaceRevoluteTpl, MotionDerived > { typedef Eigen::Matrix ReturnType; }; template - struct ConstraintForceOp< ConstraintRevoluteTpl, ForceDerived> + struct ConstraintForceOp< JointMotionSubspaceRevoluteTpl, ForceDerived> { typedef typename ForceDense::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType; }; template - struct ConstraintForceSetOp< ConstraintRevoluteTpl, ForceSet> + struct ConstraintForceSetOp< JointMotionSubspaceRevoluteTpl, ForceSet> { typedef typename Eigen::MatrixBase::ConstRowXpr ReturnType; }; template - struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> > + struct traits< JointMotionSubspaceRevoluteTpl<_Scalar,_Options,axis> > { typedef _Scalar Scalar; enum { Options = _Options }; @@ -354,35 +360,40 @@ namespace pinocchio LINEAR = 0, ANGULAR = 3 }; + typedef MotionRevoluteTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; - }; // traits ConstraintRevoluteTpl + + typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; + }; // traits JointMotionSubspaceRevoluteTpl template - struct ConstraintRevoluteTpl - : ConstraintBase< ConstraintRevoluteTpl<_Scalar,_Options,axis> > + struct JointMotionSubspaceRevoluteTpl + : JointMotionSubspaceBase< JointMotionSubspaceRevoluteTpl<_Scalar,_Options,axis> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceRevoluteTpl) enum { NV = 1 }; typedef SpatialAxis Axis; - ConstraintRevoluteTpl() {} + JointMotionSubspaceRevoluteTpl() {} template JointMotion __mult__(const Eigen::MatrixBase & v) const { return JointMotion(v[0]); } template - typename SE3GroupAction::ReturnType + typename SE3GroupAction::ReturnType se3Action(const SE3Tpl & m) const { - typedef typename SE3GroupAction::ReturnType ReturnType; + typedef typename SE3GroupAction::ReturnType ReturnType; ReturnType res; res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis)); res.template segment<3>(ANGULAR) = m.rotation().col(axis); @@ -390,10 +401,10 @@ namespace pinocchio } template - typename SE3GroupAction::ReturnType + typename SE3GroupAction::ReturnType se3ActionInverse(const SE3Tpl & m) const { - typedef typename SE3GroupAction::ReturnType ReturnType; + typedef typename SE3GroupAction::ReturnType ReturnType; typedef typename Axis::CartesianAxis3 CartesianAxis3; ReturnType res; res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation()); @@ -403,19 +414,19 @@ namespace pinocchio int nv_impl() const { return NV; } - struct TransposeConst + struct TransposeConst : JointMotionSubspaceTransposeBase { - const ConstraintRevoluteTpl & ref; - TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {} + const JointMotionSubspaceRevoluteTpl & ref; + TransposeConst(const JointMotionSubspaceRevoluteTpl & ref) : ref(ref) {} template - typename ConstraintForceOp::ReturnType + typename ConstraintForceOp::ReturnType operator*(const ForceDense & f) const { return f.angular().template segment<1>(axis); } /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) template - typename ConstraintForceSetOp::ReturnType + typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F) const { assert(F.rows()==6); @@ -440,19 +451,19 @@ namespace pinocchio } template - typename MotionAlgebraAction::ReturnType + typename MotionAlgebraAction::ReturnType motionAction(const MotionDense & m) const { - typedef typename MotionAlgebraAction::ReturnType ReturnType; + typedef typename MotionAlgebraAction::ReturnType ReturnType; ReturnType res; MotionRef v(res); v = m.cross(Axis()); return res; } - bool isEqual(const ConstraintRevoluteTpl &) const { return true; } + bool isEqual(const JointMotionSubspaceRevoluteTpl &) const { return true; } - }; // struct ConstraintRevoluteTpl + }; // struct JointMotionSubspaceRevoluteTpl template struct JointRevoluteTpl @@ -467,7 +478,7 @@ namespace pinocchio }; template - struct MultiplicationOp, ConstraintRevoluteTpl > + struct MultiplicationOp, JointMotionSubspaceRevoluteTpl > { typedef Eigen::Matrix ReturnType; }; @@ -476,10 +487,10 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ConstraintRevoluteTpl > + struct LhsMultiplicationOp, JointMotionSubspaceRevoluteTpl > { typedef InertiaTpl Inertia; - typedef ConstraintRevoluteTpl Constraint; + typedef JointMotionSubspaceRevoluteTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) @@ -507,10 +518,10 @@ namespace pinocchio }; template - struct LhsMultiplicationOp, ConstraintRevoluteTpl > + struct LhsMultiplicationOp, JointMotionSubspaceRevoluteTpl > { typedef InertiaTpl Inertia; - typedef ConstraintRevoluteTpl Constraint; + typedef JointMotionSubspaceRevoluteTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) @@ -538,10 +549,10 @@ namespace pinocchio }; template - struct LhsMultiplicationOp, ConstraintRevoluteTpl > + struct LhsMultiplicationOp, JointMotionSubspaceRevoluteTpl > { typedef InertiaTpl Inertia; - typedef ConstraintRevoluteTpl Constraint; + typedef JointMotionSubspaceRevoluteTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & /*constraint*/) @@ -570,7 +581,7 @@ namespace pinocchio } // namespace impl template - struct MultiplicationOp, ConstraintRevoluteTpl > + struct MultiplicationOp, JointMotionSubspaceRevoluteTpl > { typedef typename M6Like::ConstColXpr ReturnType; }; @@ -579,9 +590,9 @@ namespace pinocchio namespace impl { template - struct LhsMultiplicationOp, ConstraintRevoluteTpl > + struct LhsMultiplicationOp, JointMotionSubspaceRevoluteTpl > { - typedef ConstraintRevoluteTpl Constraint; + typedef JointMotionSubspaceRevoluteTpl Constraint; typedef typename MultiplicationOp,Constraint>::ReturnType ReturnType; static inline ReturnType run(const Eigen::MatrixBase & Y, const Constraint & /*constraint*/) @@ -603,7 +614,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataRevoluteTpl JointDataDerived; typedef JointModelRevoluteTpl JointModelDerived; - typedef ConstraintRevoluteTpl Constraint_t; + typedef JointMotionSubspaceRevoluteTpl Constraint_t; typedef TransformRevoluteTpl Transformation_t; typedef MotionRevoluteTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -613,27 +624,37 @@ namespace pinocchio typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE - typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataRevoluteTpl > - { typedef JointRevoluteTpl JointDerived; }; + template + struct traits< JointDataRevoluteTpl<_Scalar,_Options,axis> > + { + typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits< JointModelRevoluteTpl > - { typedef JointRevoluteTpl JointDerived; }; + template + struct traits< JointModelRevoluteTpl<_Scalar,_Options,axis> > + { + typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived; + typedef _Scalar Scalar; + }; template - struct JointDataRevoluteTpl : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> > + struct JointDataRevoluteTpl + : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + + ConfigVector_t joint_q; + TangentVector_t joint_v; Constraint_t S; Transformation_t M; @@ -644,13 +665,17 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; JointDataRevoluteTpl() - : M((Scalar)0,(Scalar)1) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M((Scalar)0,(Scalar)1) , v((Scalar)0) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() @@ -690,10 +715,8 @@ namespace pinocchio void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVector::Scalar OtherScalar; - - const OtherScalar & q = qs[idx_q()]; - OtherScalar ca,sa; SINCOS(q,&sa,&ca); + data.joint_q[0] = qs[idx_q()]; + Scalar ca,sa; SINCOS(data.joint_q[0],&sa,&ca); data.M.setValues(sa,ca); } @@ -705,16 +728,18 @@ namespace pinocchio { calc(data,qs.derived()); - data.v.angularRate() = static_cast(vs[idx_v()]); + data.joint_v[0] = vs[idx_v()]; + data.v.angularRate() = data.joint_v[0]; } - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { data.U = I.col(Inertia::ANGULAR + axis); - data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis); + data.Dinv[0] = Scalar(1)/(I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis) + armature[0]); data.UDinv.noalias() = data.U * data.Dinv[0]; if (update_I) @@ -774,4 +799,4 @@ namespace boost : public integral_constant {}; } -#endif // ifndef __pinocchio_joint_revolute_hpp__ +#endif // ifndef __pinocchio_multibody_joint_revolute_hpp__ diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp index ad09435dd5..d7ff40f110 100644 --- a/src/multibody/joint/joint-spherical-ZYX.hpp +++ b/src/multibody/joint/joint-spherical-ZYX.hpp @@ -1,15 +1,15 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_spherical_ZYX_hpp__ -#define __pinocchio_joint_spherical_ZYX_hpp__ +#ifndef __pinocchio_multibody_joint_spherical_ZYX_hpp__ +#define __pinocchio_multibody_joint_spherical_ZYX_hpp__ #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" #include "pinocchio/multibody/joint/joint-spherical.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/math/sincos.hpp" #include "pinocchio/math/matrix.hpp" #include "pinocchio/spatial/inertia.hpp" @@ -17,10 +17,10 @@ namespace pinocchio { - template struct ConstraintSphericalZYXTpl; + template struct JointMotionSubspaceSphericalZYXTpl; template - struct traits< ConstraintSphericalZYXTpl<_Scalar,_Options> > + struct traits< JointMotionSubspaceSphericalZYXTpl<_Scalar,_Options> > { typedef _Scalar Scalar; enum { Options = _Options }; @@ -33,26 +33,29 @@ namespace pinocchio typedef MotionSphericalTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; + + typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; }; // struct traits struct ConstraintRotationalSubspace template - struct ConstraintSphericalZYXTpl - : public ConstraintBase< ConstraintSphericalZYXTpl<_Scalar,_Options> > + struct JointMotionSubspaceSphericalZYXTpl + : public JointMotionSubspaceBase< JointMotionSubspaceSphericalZYXTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalZYXTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceSphericalZYXTpl) enum { NV = 3 }; typedef Eigen::Matrix Matrix3; - ConstraintSphericalZYXTpl() {} + JointMotionSubspaceSphericalZYXTpl() {} template - ConstraintSphericalZYXTpl(const Eigen::MatrixBase & subspace) + JointMotionSubspaceSphericalZYXTpl(const Eigen::MatrixBase & subspace) : m_S(subspace) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3); } @@ -68,10 +71,10 @@ namespace pinocchio int nv_impl() const { return NV; } - struct ConstraintTranspose + struct ConstraintTranspose : JointMotionSubspaceTransposeBase { - const ConstraintSphericalZYXTpl & ref; - ConstraintTranspose(const ConstraintSphericalZYXTpl & ref) : ref(ref) {} + const JointMotionSubspaceSphericalZYXTpl & ref; + ConstraintTranspose(const JointMotionSubspaceSphericalZYXTpl & ref) : ref(ref) {} template const typename MatrixMatrixProduct< @@ -167,24 +170,39 @@ namespace pinocchio Matrix3 & angularSubspace() { return m_S; } const Matrix3 & angularSubspace() const { return m_S; } - bool isEqual(const ConstraintSphericalZYXTpl & other) const + bool isEqual(const JointMotionSubspaceSphericalZYXTpl & other) const { - return m_S == other.m_S; + return internal::comparison_eq(m_S, other.m_S); } protected: Matrix3 m_S; - }; // struct ConstraintSphericalZYXTpl + }; // struct JointMotionSubspaceSphericalZYXTpl + + namespace details + { + template + struct StDiagonalMatrixSOperation< JointMotionSubspaceSphericalZYXTpl > + { + typedef JointMotionSubspaceSphericalZYXTpl Constraint; + typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; + + static ReturnType run(const JointMotionSubspaceBase & constraint) + { + return constraint.matrix().transpose() * constraint.matrix(); + } + }; + } /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ template Eigen::Matrix operator*(const InertiaTpl & Y, - const ConstraintSphericalZYXTpl & S) + const JointMotionSubspaceSphericalZYXTpl & S) { typedef typename InertiaTpl::Symmetric3 Symmetric3; - typedef ConstraintSphericalZYXTpl Constraint; + typedef JointMotionSubspaceSphericalZYXTpl Constraint; Eigen::Matrix M; alphaSkew (-Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::LINEAR)); M.template middleRows<3>(Constraint::ANGULAR) = (Y.inertia () - @@ -198,17 +216,17 @@ namespace pinocchio template const typename MatrixMatrixProduct< typename Eigen::internal::remove_const::ColsReturn::ConstType>::type, - typename ConstraintSphericalZYXTpl::Matrix3 + typename JointMotionSubspaceSphericalZYXTpl::Matrix3 >::type operator*(const Eigen::MatrixBase & Y, - const ConstraintSphericalZYXTpl & S) + const JointMotionSubspaceSphericalZYXTpl & S) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like,6,6); return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.angularSubspace(); } template - struct SE3GroupAction< ConstraintSphericalZYXTpl > + struct SE3GroupAction< JointMotionSubspaceSphericalZYXTpl > { // typedef const typename Eigen::ProductReturnType< // Eigen::Matrix , @@ -218,7 +236,7 @@ namespace pinocchio }; template - struct MotionAlgebraAction< ConstraintSphericalZYXTpl, MotionDerived > + struct MotionAlgebraAction< JointMotionSubspaceSphericalZYXTpl, MotionDerived > { typedef Eigen::Matrix ReturnType; }; @@ -236,7 +254,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataSphericalZYXTpl JointDataDerived; typedef JointModelSphericalZYXTpl JointModelDerived; - typedef ConstraintSphericalZYXTpl Constraint_t; + typedef JointMotionSubspaceSphericalZYXTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionSphericalTpl Motion_t; typedef MotionSphericalTpl Bias_t; @@ -245,29 +263,39 @@ namespace pinocchio typedef Eigen::Matrix U_t; typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataSphericalZYXTpl > - { typedef JointSphericalZYXTpl JointDerived; }; + template + struct traits< JointDataSphericalZYXTpl<_Scalar,_Options> > + { + typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits< JointModelSphericalZYXTpl > - { typedef JointSphericalZYXTpl JointDerived; }; + template + struct traits< JointModelSphericalZYXTpl<_Scalar,_Options> > + { + typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; template - struct JointDataSphericalZYXTpl : public JointDataBase< JointDataSphericalZYXTpl<_Scalar,_Options> > + struct JointDataSphericalZYXTpl + : public JointDataBase< JointDataSphericalZYXTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + ConfigVector_t joint_q; + TangentVector_t joint_v; + Constraint_t S; Transformation_t M; Motion_t v; @@ -280,13 +308,16 @@ namespace pinocchio D_t StU; JointDataSphericalZYXTpl () - : S(Constraint_t::Matrix3::Zero()) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , S(Constraint_t::Matrix3::Zero()) , M(Transformation_t::Identity()) , v(Motion_t::Vector3::Zero()) , c(Bias_t::Vector3::Zero()) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() { return std::string("JointDataSphericalZYX"); } @@ -315,15 +346,13 @@ namespace pinocchio void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - typename ConfigVector::template ConstFixedSegmentReturnType::Type & q = qs.template segment(idx_q()); - - typedef typename ConfigVector::Scalar S2; + data.joint_q = qs.template segment(idx_q()); - S2 c0,s0; SINCOS(q(0), &s0, &c0); - S2 c1,s1; SINCOS(q(1), &s1, &c1); - S2 c2,s2; SINCOS(q(2), &s2, &c2); + Scalar c0,s0; SINCOS(data.joint_q(0), &s0, &c0); + Scalar c1,s1; SINCOS(data.joint_q(1), &s1, &c1); + Scalar c2,s2; SINCOS(data.joint_q(2), &s2, &c2); - data.M.rotation () << c0 * c1, + data.M.rotation() << c0 * c1, c0 * s1 * s2 - s0 * c2, c0 * s1 * c2 + s0 * s2, s0 * c1, @@ -344,15 +373,13 @@ namespace pinocchio const typename Eigen::MatrixBase & qs, const typename Eigen::MatrixBase & vs) const { - typename ConfigVector::template ConstFixedSegmentReturnType::Type & q = qs.template segment(idx_q()); - - typedef typename ConfigVector::Scalar S2; + data.joint_q = qs.template segment(idx_q()); - S2 c0,s0; SINCOS(q(0), &s0, &c0); - S2 c1,s1; SINCOS(q(1), &s1, &c1); - S2 c2,s2; SINCOS(q(2), &s2, &c2); + Scalar c0,s0; SINCOS(data.joint_q(0), &s0, &c0); + Scalar c1,s1; SINCOS(data.joint_q(1), &s1, &c1); + Scalar c2,s2; SINCOS(data.joint_q(2), &s2, &c2); - data.M.rotation () << c0 * c1, + data.M.rotation() << c0 * c1, c0 * s1 * s2 - s0 * c2, c0 * s1 * c2 + s0 * s2, s0 * c1, @@ -367,27 +394,26 @@ namespace pinocchio c1 * s2, c2, Scalar(0), c1 * c2, -s2, Scalar(0); - typename TangentVector::template ConstFixedSegmentReturnType::Type - & q_dot = vs.template segment(idx_v()); - - data.v().noalias() = data.S.angularSubspace() * q_dot; - + data.joint_v = vs.template segment(idx_v()); + data.v().noalias() = data.S.angularSubspace() * data.joint_v; + +#define q_dot data.joint_v data.c()(0) = -c1 * q_dot(0) * q_dot(1); data.c()(1) = -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 * q_dot(1) * q_dot(2); data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2); +#undef q_dot } - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace(); data.StU.noalias() = data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR); + data.StU.diagonal() += armature; - // compute inverse -// data.Dinv.setIdentity(); -// data.StU.llt().solveInPlace(data.Dinv); internal::PerformStYSInversion::run(data.StU,data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; @@ -434,4 +460,4 @@ namespace boost : public integral_constant {}; } -#endif // ifndef __pinocchio_joint_spherical_ZYX_hpp__ +#endif // ifndef __pinocchio_multibody_joint_spherical_ZYX_hpp__ diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp index 62a6038711..2f1d2ce2b2 100644 --- a/src/multibody/joint/joint-spherical.hpp +++ b/src/multibody/joint/joint-spherical.hpp @@ -1,14 +1,14 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_spherical_hpp__ -#define __pinocchio_joint_spherical_hpp__ +#ifndef __pinocchio_multibody_joint_spherical_hpp__ +#define __pinocchio_multibody_joint_spherical_hpp__ #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/math/sincos.hpp" #include "pinocchio/spatial/inertia.hpp" #include "pinocchio/spatial/skew.hpp" @@ -97,13 +97,13 @@ namespace pinocchio bool isEqual_impl(const MotionSphericalTpl & other) const { - return m_w == other.m_w; + return internal::comparison_eq(m_w, other.m_w); } template bool isEqual_impl(const MotionDense & other) const { - return other.angular() == m_w && other.linear().isZero(0); + return internal::comparison_eq(other.angular(), m_w) && other.linear().isZero(0); } template @@ -178,10 +178,10 @@ namespace pinocchio return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.angular()); } - template struct ConstraintSphericalTpl; + template struct JointMotionSubspaceSphericalTpl; template - struct traits< ConstraintSphericalTpl<_Scalar,_Options> > + struct traits< JointMotionSubspaceSphericalTpl<_Scalar,_Options> > { typedef _Scalar Scalar; enum { Options = _Options }; @@ -192,19 +192,23 @@ namespace pinocchio typedef MotionSphericalTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; + typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; - }; // struct traits struct ConstraintSphericalTpl + + typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; + }; // struct traits struct JointMotionSubspaceSphericalTpl template - struct ConstraintSphericalTpl - : public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> > + struct JointMotionSubspaceSphericalTpl + : public JointMotionSubspaceBase< JointMotionSubspaceSphericalTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceSphericalTpl) - ConstraintSphericalTpl() {} + JointMotionSubspaceSphericalTpl() {} enum { NV = 3 }; @@ -217,7 +221,7 @@ namespace pinocchio return JointMotion(w); } - struct TransposeConst + struct TransposeConst : JointMotionSubspaceTransposeBase { template typename ForceDense::ConstAngularType @@ -258,9 +262,9 @@ namespace pinocchio Eigen::Matrix se3ActionInverse(const SE3Tpl & m) const { Eigen::Matrix X_subspace; - AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0)); - AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1)); - AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2)); + XAxis::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0)); + YAxis::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1)); + ZAxis::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2)); X_subspace.template middleRows<3>(LINEAR).noalias() = m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR); @@ -282,9 +286,9 @@ namespace pinocchio return res; } - bool isEqual(const ConstraintSphericalTpl &) const { return true; } + bool isEqual(const JointMotionSubspaceSphericalTpl &) const { return true; } - }; // struct ConstraintSphericalTpl + }; // struct JointMotionSubspaceSphericalTpl template inline typename MotionDerived::MotionPlain @@ -298,7 +302,7 @@ namespace pinocchio template inline Eigen::Matrix operator*(const InertiaTpl & Y, - const ConstraintSphericalTpl &) + const JointMotionSubspaceSphericalTpl &) { typedef InertiaTpl Inertia; typedef typename Inertia::Symmetric3 Symmetric3; @@ -313,18 +317,18 @@ namespace pinocchio template inline typename SizeDepType<3>::ColsReturn::ConstType operator*(const Eigen::MatrixBase & Y, - const ConstraintSphericalTpl &) + const JointMotionSubspaceSphericalTpl &) { - typedef ConstraintSphericalTpl Constraint; + typedef JointMotionSubspaceSphericalTpl Constraint; return Y.derived().template middleCols<3>(Constraint::ANGULAR); } template - struct SE3GroupAction< ConstraintSphericalTpl > + struct SE3GroupAction< JointMotionSubspaceSphericalTpl > { typedef Eigen::Matrix ReturnType; }; template - struct MotionAlgebraAction< ConstraintSphericalTpl,MotionDerived > + struct MotionAlgebraAction< JointMotionSubspaceSphericalTpl,MotionDerived > { typedef Eigen::Matrix ReturnType; }; template struct JointSphericalTpl; @@ -340,7 +344,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataSphericalTpl JointDataDerived; typedef JointModelSphericalTpl JointModelDerived; - typedef ConstraintSphericalTpl Constraint_t; + typedef JointMotionSubspaceSphericalTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionSphericalTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -349,29 +353,39 @@ namespace pinocchio typedef Eigen::Matrix U_t; typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits< JointDataSphericalTpl > - { typedef JointSphericalTpl JointDerived; }; + template + struct traits< JointDataSphericalTpl<_Scalar,_Options> > + { + typedef JointSphericalTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits< JointModelSphericalTpl > - { typedef JointSphericalTpl JointDerived; }; + template + struct traits< JointModelSphericalTpl<_Scalar,_Options> > + { + typedef JointSphericalTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; template - struct JointDataSphericalTpl : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> > + struct JointDataSphericalTpl + : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointSphericalTpl<_Scalar,_Options> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + + ConfigVector_t joint_q; + TangentVector_t joint_v; Constraint_t S; Transformation_t M; @@ -382,13 +396,17 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; - JointDataSphericalTpl () - : M(Transformation_t::Identity()) + JointDataSphericalTpl() + : joint_q(Scalar(0),Scalar(0),Scalar(0),Scalar(1)) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) , v(Motion_t::Vector3::Zero()) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() { return std::string("JointDataSpherical"); } @@ -434,6 +452,7 @@ namespace pinocchio void calc(JointDataDerived & data, const typename Eigen::QuaternionBase & quat) const { + data.joint_q = quat.coeffs(); data.M.rotation(quat.matrix()); } @@ -466,33 +485,25 @@ namespace pinocchio const typename Eigen::MatrixBase & vs) const { calc(data,qs.derived()); - - data.v.angular() = vs.template segment(idx_v()); + data.joint_v = vs.template segment(idx_v()); + data.v.angular() = data.joint_v; } - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { data.U = I.template block<6,3>(0,Inertia::ANGULAR); + data.StU = data.U.template middleRows<3>(Inertia::ANGULAR); + data.StU.diagonal() += armature; - // compute inverse -// data.Dinv.setIdentity(); -// data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv); - internal::PerformStYSInversion::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv); - - data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor - data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv; - - if (update_I) - { - Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I); - I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR) - -= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR); - I_.template block<6,3>(0,Inertia::ANGULAR).setZero(); - I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero(); - } + internal::PerformStYSInversion::run(data.StU,data.Dinv); + data.UDinv.noalias() = data.U * data.Dinv; + + if(update_I) + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose(); } static std::string classname() { return std::string("JointModelSpherical"); } @@ -533,4 +544,4 @@ namespace boost : public integral_constant {}; } -#endif // ifndef __pinocchio_joint_spherical_hpp__ +#endif // ifndef __pinocchio_multibody_joint_spherical_hpp__ diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp index a65869f20c..93d857f597 100644 --- a/src/multibody/joint/joint-translation.hpp +++ b/src/multibody/joint/joint-translation.hpp @@ -1,14 +1,14 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_joint_translation_hpp__ -#define __pinocchio_joint_translation_hpp__ +#ifndef __pinocchio_multibody_joint_translation_hpp__ +#define __pinocchio_multibody_joint_translation_hpp__ #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" -#include "pinocchio/multibody/constraint.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" #include "pinocchio/spatial/inertia.hpp" #include "pinocchio/spatial/skew.hpp" @@ -82,7 +82,7 @@ namespace pinocchio bool isEqual_impl(const MotionTranslationTpl & other) const { - return m_v == other.m_v; + return internal::comparison_eq(m_v, other.m_v); } MotionTranslationTpl & operator=(const MotionTranslationTpl & other) @@ -244,7 +244,7 @@ namespace pinocchio bool isEqual(const TransformTranslationTpl & other) const { - return m_translation == other.m_translation; + return internal::comparison_eq(m_translation, other.m_translation); } protected: @@ -252,10 +252,10 @@ namespace pinocchio LinearType m_translation; }; - template struct ConstraintTranslationTpl; + template struct JointMotionSubspaceTranslationTpl; template - struct traits< ConstraintTranslationTpl<_Scalar,_Options> > + struct traits< JointMotionSubspaceTranslationTpl<_Scalar,_Options> > { typedef _Scalar Scalar; @@ -265,22 +265,25 @@ namespace pinocchio typedef MotionTranslationTpl JointMotion; typedef Eigen::Matrix JointForce; typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; typedef DenseBase MatrixReturnType; typedef const DenseBase ConstMatrixReturnType; - }; // traits ConstraintTranslationTpl + + typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; + }; // traits JointMotionSubspaceTranslationTpl template - struct ConstraintTranslationTpl - : ConstraintBase< ConstraintTranslationTpl<_Scalar,_Options> > + struct JointMotionSubspaceTranslationTpl + : JointMotionSubspaceBase< JointMotionSubspaceTranslationTpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTranslationTpl) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceTranslationTpl) enum { NV = 3 }; - ConstraintTranslationTpl() {} + JointMotionSubspaceTranslationTpl() {} // template // Motion operator*(const MotionTranslationTpl & vj) const @@ -295,10 +298,10 @@ namespace pinocchio int nv_impl() const { return NV; } - struct ConstraintTranspose + struct ConstraintTranspose : JointMotionSubspaceTransposeBase { - const ConstraintTranslationTpl & ref; - ConstraintTranspose(const ConstraintTranslationTpl & ref) : ref(ref) {} + const JointMotionSubspaceTranslationTpl & ref; + ConstraintTranspose(const JointMotionSubspaceTranslationTpl & ref) : ref(ref) {} template typename ForceDense::ConstLinearType @@ -360,9 +363,9 @@ namespace pinocchio return res; } - bool isEqual(const ConstraintTranslationTpl &) const { return true; } + bool isEqual(const JointMotionSubspaceTranslationTpl &) const { return true; } - }; // struct ConstraintTranslationTpl + }; // struct JointMotionSubspaceTranslationTpl template inline typename MotionDerived::MotionPlain @@ -376,9 +379,9 @@ namespace pinocchio template inline Eigen::Matrix operator*(const InertiaTpl & Y, - const ConstraintTranslationTpl &) + const JointMotionSubspaceTranslationTpl &) { - typedef ConstraintTranslationTpl Constraint; + typedef JointMotionSubspaceTranslationTpl Constraint; Eigen::Matrix M; alphaSkew(Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::ANGULAR)); M.template middleRows<3>(Constraint::LINEAR).setZero(); @@ -391,18 +394,18 @@ namespace pinocchio template inline const typename SizeDepType<3>::ColsReturn::ConstType operator*(const Eigen::MatrixBase & Y, - const ConstraintTranslationTpl &) + const JointMotionSubspaceTranslationTpl &) { - typedef ConstraintTranslationTpl Constraint; + typedef JointMotionSubspaceTranslationTpl Constraint; return Y.derived().template middleCols<3>(Constraint::LINEAR); } template - struct SE3GroupAction< ConstraintTranslationTpl > + struct SE3GroupAction< JointMotionSubspaceTranslationTpl > { typedef Eigen::Matrix ReturnType; }; template - struct MotionAlgebraAction< ConstraintTranslationTpl,MotionDerived > + struct MotionAlgebraAction< JointMotionSubspaceTranslationTpl,MotionDerived > { typedef Eigen::Matrix ReturnType; }; template struct JointTranslationTpl; @@ -418,7 +421,7 @@ namespace pinocchio enum { Options = _Options }; typedef JointDataTranslationTpl JointDataDerived; typedef JointModelTranslationTpl JointModelDerived; - typedef ConstraintTranslationTpl Constraint_t; + typedef JointMotionSubspaceTranslationTpl Constraint_t; typedef TransformTranslationTpl Transformation_t; typedef MotionTranslationTpl Motion_t; typedef MotionZeroTpl Bias_t; @@ -427,20 +430,26 @@ namespace pinocchio typedef Eigen::Matrix U_t; typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; // traits JointTranslationTpl - template - struct traits< JointDataTranslationTpl > - { typedef JointTranslationTpl JointDerived; }; + template + struct traits< JointDataTranslationTpl<_Scalar,_Options> > + { + typedef JointTranslationTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; - template - struct traits< JointModelTranslationTpl > - { typedef JointTranslationTpl JointDerived; }; + template + struct traits< JointModelTranslationTpl<_Scalar,_Options> > + { + typedef JointTranslationTpl<_Scalar,_Options> JointDerived; + typedef _Scalar Scalar; + }; template struct JointDataTranslationTpl @@ -451,6 +460,9 @@ namespace pinocchio typedef JointTranslationTpl<_Scalar,_Options> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + + ConfigVector_t joint_q; + TangentVector_t joint_v; Constraint_t S; Transformation_t M; @@ -461,13 +473,17 @@ namespace pinocchio U_t U; D_t Dinv; UD_t UDinv; + D_t StU; JointDataTranslationTpl() - : M(Transformation_t::Vector3::Zero()) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Vector3::Zero()) , v(Motion_t::Vector3::Zero()) , U(U_t::Zero()) , Dinv(D_t::Zero()) , UDinv(UD_t::Zero()) + , StU(D_t::Zero()) {} static std::string classname() { return std::string("JointDataTranslation"); } @@ -496,7 +512,8 @@ namespace pinocchio void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - data.M.translation() = this->jointConfigSelector(qs); + data.joint_q = this->jointConfigSelector(qs); + data.M.translation() = data.joint_q; } template @@ -506,32 +523,27 @@ namespace pinocchio { calc(data,qs.derived()); - data.v.linear() = this->jointVelocitySelector(vs); + data.joint_v = this->jointVelocitySelector(vs); + data.v.linear() = data.joint_v; } - template + template void calc_aba(JointDataDerived & data, + const Eigen::MatrixBase & armature, const Eigen::MatrixBase & I, const bool update_I) const { data.U = I.template middleCols<3>(Inertia::LINEAR); - // compute inverse -// data.Dinv.setIdentity(); -// data.U.template middleRows<3>(Inertia::LINEAR).llt().solveInPlace(data.Dinv); - internal::PerformStYSInversion::run(data.U.template middleRows<3>(Inertia::LINEAR),data.Dinv); + data.StU = data.U.template middleRows<3>(Inertia::LINEAR); + data.StU.diagonal() += armature; - data.UDinv.template middleRows<3>(Inertia::LINEAR).setIdentity(); // can be put in data constructor - data.UDinv.template middleRows<3>(Inertia::ANGULAR).noalias() = data.U.template middleRows<3>(Inertia::ANGULAR) * data.Dinv; + internal::PerformStYSInversion::run(data.StU,data.Dinv); - if (update_I) - { - Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I); - I_.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR) - -= data.UDinv.template middleRows<3>(Inertia::ANGULAR) * I_.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR); - I_.template middleCols<3>(Inertia::LINEAR).setZero(); - I_.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero(); - } + data.UDinv.noalias() = data.U * data.Dinv; + + if(update_I) + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose(); } static std::string classname() { return std::string("JointModelTranslation"); } @@ -572,4 +584,4 @@ namespace boost : public integral_constant {}; } -#endif // ifndef __pinocchio_joint_translation_hpp__ +#endif // ifndef __pinocchio_multibody_joint_translation_hpp__ diff --git a/src/multibody/joint/joints.hpp b/src/multibody/joint/joints.hpp index 823a6e3deb..d8dd6ddc47 100644 --- a/src/multibody/joint/joints.hpp +++ b/src/multibody/joint/joints.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2019 INRIA // -#ifndef __pinocchio_joint_joints_hpp__ -#define __pinocchio_joint_joints_hpp__ +#ifndef __pinocchio_multibody_joint_joints_hpp__ +#define __pinocchio_multibody_joint_joints_hpp__ #include "pinocchio/multibody/joint/joint-free-flyer.hpp" #include "pinocchio/multibody/joint/joint-planar.hpp" @@ -18,4 +18,4 @@ #include "pinocchio/multibody/joint/joint-translation.hpp" #include "pinocchio/multibody/joint/joint-mimic.hpp" -#endif // ifndef __pinocchio_joint_joints_hpp__ +#endif // ifndef __pinocchio_multibody_joint_joints_hpp__ diff --git a/src/multibody/liegroup/liegroup-base.hxx b/src/multibody/liegroup/liegroup-base.hxx index a1d554a5fa..e198828fcf 100644 --- a/src/multibody/liegroup/liegroup-base.hxx +++ b/src/multibody/liegroup/liegroup-base.hxx @@ -493,7 +493,7 @@ namespace pinocchio { template typename LieGroupBase::ConfigVector_t LieGroupBase::integrate(const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v) const + const Eigen::MatrixBase & v) const { ConfigVector_t qout(nq()); integrate(q.derived(), v.derived(), qout); @@ -611,13 +611,8 @@ namespace pinocchio { const Scalar & u, const Eigen::MatrixBase & qout) const { - if (u == 0) PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q0; - else if(u == 1) PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q1; - else - { - TangentVector_t vdiff(u * difference(q0, q1)); - integrate(q0.derived(), vdiff, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout)); - } + TangentVector_t vdiff(u * difference(q0, q1)); + integrate(q0.derived(), vdiff, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout)); } template diff --git a/src/multibody/liegroup/liegroup-variant-visitors.hpp b/src/multibody/liegroup/liegroup-variant-visitors.hpp index 76665d13b8..e619733f87 100644 --- a/src/multibody/liegroup/liegroup-variant-visitors.hpp +++ b/src/multibody/liegroup/liegroup-variant-visitors.hpp @@ -155,21 +155,23 @@ namespace pinocchio const Eigen::MatrixBase & J, const ArgumentPosition arg); - template + template void dDifference(const LieGroupGenericTpl & lg, const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1, const Eigen::MatrixBase & Jin, int self, - const Eigen::MatrixBase & Jout); + const Eigen::MatrixBase & Jout, + const ArgumentPosition arg); - template + template void dDifference(const LieGroupGenericTpl & lg, const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1, int self, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & Jout); + const Eigen::MatrixBase & Jout, + const ArgumentPosition arg); template void dIntegrateTransport(const LieGroupGenericTpl & lg, diff --git a/src/multibody/liegroup/special-euclidean.hpp b/src/multibody/liegroup/special-euclidean.hpp index 94552a906e..ddbe174213 100644 --- a/src/multibody/liegroup/special-euclidean.hpp +++ b/src/multibody/liegroup/special-euclidean.hpp @@ -427,7 +427,7 @@ namespace pinocchio template static void normalize_impl(const Eigen::MatrixBase & qout) { - PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize(); + normalize(qout.const_cast_derived().template tail<2>()); } template @@ -786,10 +786,9 @@ namespace pinocchio } template - static void normalize_impl (const Eigen::MatrixBase& qout) + static void normalize_impl(const Eigen::MatrixBase & qout) { - Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout); - qout_.template tail<4>().normalize(); + normalize(qout.const_cast_derived().template tail<4>()); } template diff --git a/src/multibody/liegroup/special-orthogonal.hpp b/src/multibody/liegroup/special-orthogonal.hpp index b0adeaf97a..a3eb4f0667 100644 --- a/src/multibody/liegroup/special-orthogonal.hpp +++ b/src/multibody/liegroup/special-orthogonal.hpp @@ -89,7 +89,7 @@ namespace pinocchio // acos(x) = PI/2 - x and asin(x) = x (the precision of x is not lost in PI/2). // else if (tr > Scalar(2) - 1e-2) theta = asin ((R(1,0) - R(0,1)) / Scalar(2)); // else theta = (pos ? acos (tr/Scalar(2)) : -acos(tr/Scalar(2))); - assert (theta == theta); // theta != NaN + assert(check_expression_if_real(theta == theta) && "theta is NaN"); // theta != NaN // assert ((cos (theta) * R(0,0) + sin (theta) * R(1,0) > 0) && // (cos (theta) * R(1,0) - sin (theta) * R(0,0) < 1e-6)); return theta; @@ -268,46 +268,48 @@ namespace pinocchio template static void interpolate_impl(const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1, - const Scalar& u, + const Scalar & u, const Eigen::MatrixBase& qout) { ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout); - assert ( std::abs(q0.norm() - 1) < 1e-8 && "initial configuration not normalized"); - assert ( std::abs(q1.norm() - 1) < 1e-8 && "final configuration not normalized"); - Scalar cosTheta = q0.dot(q1); - Scalar sinTheta = q0(0)*q1(1) - q0(1)*q1(0); - Scalar theta = atan2(sinTheta, cosTheta); - assert (fabs (sin (theta) - sinTheta) < 1e-8); + assert(check_expression_if_real(abs(q0.norm() - 1) < 1e-8) && "initial configuration not normalized"); + assert(check_expression_if_real(abs(q1.norm() - 1) < 1e-8) && "final configuration not normalized"); + const Scalar cosTheta = q0.dot(q1); + const Scalar sinTheta = q0(0)*q1(1) - q0(1)*q1(0); + const Scalar theta = atan2(sinTheta, cosTheta); + assert(check_expression_if_real(fabs(sin(theta) - sinTheta) < 1e-8)); - const Scalar PI_value = PI(); - - if (fabs (theta) > 1e-6 && fabs (theta) < PI_value - 1e-6) - { - out = (sin ((1-u)*theta)/sinTheta) * q0 - + (sin ( u *theta)/sinTheta) * q1; - } - else if (fabs (theta) < 1e-6) // theta = 0 - { - out = (1-u) * q0 + u * q1; - } - else // theta = +-PI - { - Scalar theta0 = atan2 (q0(1), q0(0)); - SINCOS(theta0,&out[1],&out[0]); - } + static const Scalar PI_value = PI(); + static const Scalar PI_value_lower = PI_value - static_cast(1e-6); + using namespace internal; + +// const Scalar theta0 = atan2(q0(1), q0(0)); + const Scalar abs_theta = fabs(theta); + out[0] = if_then_else(LT,abs_theta,static_cast(1e-6), + (Scalar(1)-u) * q0[0] + u * q1[0], // then + if_then_else(LT,abs_theta,PI_value_lower, // else + (sin((Scalar(1)-u)*theta)/sinTheta) * q0[0] + (sin( u *theta)/sinTheta) * q1[0], // then + q0(0) // cos(theta0) // else + )); + + out[1] = if_then_else(LT,abs_theta,static_cast(1e-6), + (Scalar(1)-u) * q0[1] + u * q1[1], // then + if_then_else(LT,abs_theta,PI_value_lower, // else + (sin((Scalar(1)-u)*theta)/sinTheta) * q0[1] + (sin( u *theta)/sinTheta) * q1[1], // then + q0(1) // sin(theta0) // else + )); } template static void normalize_impl (const Eigen::MatrixBase & qout) { - Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).derived(); - qout_.normalize(); + normalize(qout.const_cast_derived()); } template - static bool isNormalized_impl (const Eigen::MatrixBase & qin, - const Scalar& prec) + static bool isNormalized_impl(const Eigen::MatrixBase & qin, + const Scalar & prec) { const Scalar norm = qin.norm(); using std::abs; @@ -319,7 +321,7 @@ namespace pinocchio { Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout); - const Scalar PI_value = PI(); + static const Scalar PI_value = PI(); const Scalar angle = -PI_value + Scalar(2)* PI_value * ((Scalar)rand())/RAND_MAX; SINCOS(angle, &out(1), &out(0)); } @@ -561,10 +563,11 @@ namespace pinocchio ConstQuaternionMap_t quat1 (q1.derived().data()); assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); - QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data()); + QuaternionMap_t quat_res(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data()); - quat_map = quat0.slerp(u, quat1); - assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); + quaternion::slerp(u,quat0,quat1,quat_res); +// quat_res = quat0.slerp(u, quat1); + assert(quaternion::isNormalized(quat_res,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); } template @@ -579,8 +582,7 @@ namespace pinocchio template static void normalize_impl(const Eigen::MatrixBase & qout) { - Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout); - qout_.normalize(); + normalize(qout.const_cast_derived()); } template diff --git a/src/multibody/liegroup/vector-space.hpp b/src/multibody/liegroup/vector-space.hpp index dae17e35bc..ce923c7f9e 100644 --- a/src/multibody/liegroup/vector-space.hpp +++ b/src/multibody/liegroup/vector-space.hpp @@ -244,7 +244,7 @@ namespace pinocchio } template - void random_impl (const Eigen::MatrixBase& qout) const + void random_impl(const Eigen::MatrixBase& qout) const { PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).setRandom(); } @@ -258,12 +258,11 @@ namespace pinocchio ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).derived(); for (int i = 0; i < nq (); ++i) { - if(lower_pos_limit[i] == -std::numeric_limits::infinity() || - upper_pos_limit[i] == std::numeric_limits::infinity() ) + if(check_expression_if_real(lower_pos_limit[i] == -std::numeric_limits::infinity() || + upper_pos_limit[i] == std::numeric_limits::infinity()) ) { std::ostringstream error; error << "non bounded limit. Cannot uniformly sample joint at rank " << i; - // assert(false && "non bounded limit. Cannot uniformly sample joint revolute"); throw std::range_error(error.str()); } res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX; diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp index cbbe98658b..8f4f0fe3d4 100644 --- a/src/multibody/model.hpp +++ b/src/multibody/model.hpp @@ -20,16 +20,27 @@ #include "pinocchio/serialization/serializable.hpp" -#include #include #include namespace pinocchio { - + template class JointCollectionTpl> + struct CastType > + { + typedef ModelTpl type; + }; + + template class JointCollectionTpl> + struct traits< ModelTpl<_Scalar,_Options,JointCollectionTpl> > + { + typedef _Scalar Scalar; + }; + template class JointCollectionTpl> struct ModelTpl : serialization::Serializable< ModelTpl<_Scalar,_Options,JointCollectionTpl> > + , NumericalBase< ModelTpl<_Scalar,_Options,JointCollectionTpl> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -87,7 +98,7 @@ namespace pinocchio /// \brief Number of operational frames. int nframes; - /// \brief Spatial inertias of the body *i* expressed in the supporting joint frame *i*. + /// \brief Spatial inertias supported by the joint *i*. PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) inertias; /// \brief Placement (SE3) of the input of joint *i* regarding to the parent joint output *li*. @@ -110,6 +121,9 @@ namespace pinocchio /// \brief Joint parent of joint *i*, denoted *li* (li==parents[i]). std::vector parents; + + /// \brief Joint children of joint *i*, denoted *mu(i)* (i==parents[k] for k in mu(i)). + std::vector children; /// \brief Name of joint *i* std::vector names; @@ -117,6 +131,10 @@ namespace pinocchio /// \brief Map of reference configurations, indexed by user given names. ConfigVectorMap referenceConfigurations; + /// \brief Vector of armature values expressed at the joint level + /// This vector may contain the contribution of rotor inertia effects for instance. + VectorXs armature; + /// \brief Vector of rotor inertia parameters TangentVectorType rotorInertia; @@ -160,7 +178,7 @@ namespace pinocchio /// \brief Default 3D gravity vector (=(0,0,-9.81)). static const Vector3 gravity981; - /// \brief Model name; + /// \brief Model name. std::string name; /// \brief Default constructor. Builds an empty model with no joints. @@ -178,6 +196,7 @@ namespace pinocchio , idx_vs(1,0) , nvs(1,0) , parents(1, 0) + , children(1) , names(1) , supports(1,IndexVector(1,0)) , subtrees(1) @@ -191,164 +210,27 @@ namespace pinocchio } ~ModelTpl() {} // std::cout << "Destroy model" << std::endl; } - /// \returns An expression of *this with the Scalar type casted to NewScalar. - template - ModelTpl cast() const + /// + /// \brief Copy constructor by casting + /// + /// \param[in] other model to copy to *this + /// + template + explicit ModelTpl(const ModelTpl & other) { - typedef ModelTpl ReturnType; - ReturnType res; - res.nq = nq; res.nv = nv; - res.njoints = njoints; - res.nbodies = nbodies; - res.nframes = nframes; - res.parents = parents; - res.names = names; - res.subtrees = subtrees; - res.gravity = gravity.template cast(); - res.name = name; - - res.idx_qs = idx_qs; - res.nqs = nqs; - res.idx_vs = idx_vs; - res.nvs = nvs; - - // Eigen Vectors - res.rotorInertia = rotorInertia.template cast(); - res.rotorGearRatio = rotorGearRatio.template cast(); - res.friction = friction.template cast(); - res.damping = damping.template cast(); - res.effortLimit = effortLimit.template cast(); - res.velocityLimit = velocityLimit.template cast(); - res.lowerPositionLimit = lowerPositionLimit.template cast(); - res.upperPositionLimit = upperPositionLimit.template cast(); - - typename ConfigVectorMap::const_iterator it; - for (it = referenceConfigurations.begin(); - it != referenceConfigurations.end(); it++ ) - { - res.referenceConfigurations.insert(std::make_pair(it->first, it->second.template cast())); - } - - // reserve vectors - res.inertias.resize(inertias.size()); - res.jointPlacements.resize(jointPlacements.size()); - res.joints.resize(joints.size()); - res.frames.resize(frames.size()); - - /// copy into vectors - for(size_t k = 0; k < joints.size(); ++k) - { - res.inertias[k] = inertias[k].template cast(); - res.jointPlacements[k] = jointPlacements[k].template cast(); - res.joints[k] = joints[k].template cast(); - } - - for(size_t k = 0; k < frames.size(); ++k) - { - res.frames[k] = frames[k].template cast(); - } - - return res; + *this = other.template cast(); } + /// \returns A new copy of *this with the Scalar type casted to NewScalar. + template + typename CastType::type cast() const; + /// /// \brief Equality comparison operator. /// /// \returns true if *this is equal to other. /// - bool operator==(const ModelTpl & other) const - { - bool res = - other.nq == nq - && other.nv == nv - && other.njoints == njoints - && other.nbodies == nbodies - && other.nframes == nframes - && other.parents == parents - && other.names == names - && other.subtrees == subtrees - && other.gravity == gravity - && other.name == name; - - res &= - other.idx_qs == idx_qs - && other.nqs == nqs - && other.idx_vs == idx_vs - && other.nvs == nvs; - - if(other.referenceConfigurations.size() != referenceConfigurations.size()) - return false; - - typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin(); - typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin(); - for(long k = 0; k < (long)referenceConfigurations.size(); ++k) - { - std::advance(it,k); std::advance(it_other,k); - - if(it->second.size() != it_other->second.size()) - return false; - if(it->second != it_other->second) - return false; - } - - if(other.rotorInertia.size() != rotorInertia.size()) - return false; - res &= other.rotorInertia == rotorInertia; - if(!res) return res; - - if(other.friction.size() != friction.size()) - return false; - res &= other.friction == friction; - if(!res) return res; - - if(other.damping.size() != damping.size()) - return false; - res &= other.damping == damping; - if(!res) return res; - - if(other.rotorGearRatio.size() != rotorGearRatio.size()) - return false; - res &= other.rotorGearRatio == rotorGearRatio; - if(!res) return res; - - if(other.effortLimit.size() != effortLimit.size()) - return false; - res &= other.effortLimit == effortLimit; - if(!res) return res; - - if(other.velocityLimit.size() != velocityLimit.size()) - return false; - res &= other.velocityLimit == velocityLimit; - if(!res) return res; - - if(other.lowerPositionLimit.size() != lowerPositionLimit.size()) - return false; - res &= other.lowerPositionLimit == lowerPositionLimit; - if(!res) return res; - - if(other.upperPositionLimit.size() != upperPositionLimit.size()) - return false; - res &= other.upperPositionLimit == upperPositionLimit; - if(!res) return res; - - for(size_t k = 1; k < inertias.size(); ++k) - { - res &= other.inertias[k] == inertias[k]; - if(!res) return res; - } - - for(size_t k = 1; k < other.jointPlacements.size(); ++k) - { - res &= other.jointPlacements[k] == jointPlacements[k]; - if(!res) return res; - } - - res &= - other.joints == joints - && other.frames == frames; - - return res; - } + bool operator==(const ModelTpl & other) const; /// /// \returns true if *this is NOT equal to other. @@ -359,8 +241,9 @@ namespace pinocchio /// /// \brief Add a joint to the kinematic tree with infinite bounds. /// - /// \remark This method also adds a Frame of same name to the vector of frames. - /// \remark The inertia supported by the joint is set to Zero. + /// \remarks This method does not add a Frame of same name to the vector of frames. + /// Use Model::addJointFrame. + /// \remarks The inertia supported by the joint is set to Zero. /// /// \tparam JointModelDerived The type of the joint model. /// diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx index 498cb95581..462dfed95b 100644 --- a/src/multibody/model.hxx +++ b/src/multibody/model.hxx @@ -1,12 +1,11 @@ // -// Copyright (c) 2015-2020 CNRS INRIA +// Copyright (c) 2015-2021 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // #ifndef __pinocchio_multibody_model_hxx__ #define __pinocchio_multibody_model_hxx__ -#include "pinocchio/spatial/fwd.hpp" #include "pinocchio/utils/string-generator.hpp" #include "pinocchio/multibody/liegroup/liegroup-algo.hpp" @@ -77,12 +76,11 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(joint_friction.size(),joint_model.nv(),"The joint friction vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(joint_damping.size(),joint_model.nv(),"The joint damping vector is not of right size"); - JointIndex idx = (JointIndex)(njoints++); + JointIndex joint_id = (JointIndex)(njoints++); joints .push_back(JointModel(joint_model.derived())); -// JointModelDerived & jmodel = boost::get(joints.back()); JointModel & jmodel = joints.back(); - jmodel.setIndexes(idx,nq,nv); + jmodel.setIndexes(joint_id,nq,nv); const int joint_nq = jmodel.nq(); const int joint_idx_q = jmodel.idx_q(); @@ -92,10 +90,12 @@ namespace pinocchio assert(joint_idx_q >= 0); assert(joint_idx_v >= 0); - inertias .push_back(Inertia::Zero()); - parents .push_back(parent); - jointPlacements.push_back(joint_placement); - names .push_back(joint_name); + inertias .push_back(Inertia::Zero()); + parents .push_back(parent); + children .push_back(IndexVector()); + children[parent].push_back(joint_id); + jointPlacements .push_back(joint_placement); + names .push_back(joint_name); nq += joint_nq; nqs.push_back(joint_nq); idx_qs.push_back(joint_idx_q); nv += joint_nv; nvs.push_back(joint_nv); idx_vs.push_back(joint_idx_v); @@ -111,6 +111,8 @@ namespace pinocchio upperPositionLimit.conservativeResize(nq); jmodel.jointConfigSelector(upperPositionLimit) = max_config; + armature.conservativeResize(nv); + jmodel.jointVelocitySelector(armature).setZero(); rotorInertia.conservativeResize(nv); jmodel.jointVelocitySelector(rotorInertia).setZero(); rotorGearRatio.conservativeResize(nv); @@ -123,14 +125,14 @@ namespace pinocchio // Init and add joint index to its parent subtrees. subtrees.push_back(IndexVector(1)); - subtrees[idx][0] = idx; - addJointIndexToParentSubtrees(idx); + subtrees[joint_id][0] = joint_id; + addJointIndexToParentSubtrees(joint_id); // Init and add joint index to the supports supports.push_back(supports[parent]); - supports[idx].push_back(idx); + supports[joint_id].push_back(joint_id); - return idx; + return joint_id; } template class JointCollectionTpl> @@ -170,7 +172,7 @@ namespace pinocchio } template class JointCollectionTpl> - inline typename ModelTpl::FrameIndex + inline FrameIndex ModelTpl:: addJointFrame(const JointIndex & joint_index, int previous_frame_index) @@ -189,6 +191,171 @@ namespace pinocchio return addFrame(Frame(names[joint_index],joint_index,(FrameIndex)previous_frame_index,SE3::Identity(),JOINT)); } + template class JointCollectionTpl> + template + typename CastType >::type + ModelTpl::cast() const + { + typedef ModelTpl ReturnType; + + ReturnType res; + res.nq = nq; res.nv = nv; + res.njoints = njoints; + res.nbodies = nbodies; + res.nframes = nframes; + res.parents = parents; + res.children = children; + res.names = names; + res.subtrees = subtrees; + res.supports = supports; + res.gravity = gravity.template cast(); + res.name = name; + + res.idx_qs = idx_qs; + res.nqs = nqs; + res.idx_vs = idx_vs; + res.nvs = nvs; + + // Eigen Vectors + res.armature = armature.template cast(); + res.friction = friction.template cast(); + res.damping = damping.template cast(); + res.rotorInertia = rotorInertia.template cast(); + res.rotorGearRatio = rotorGearRatio.template cast(); + res.effortLimit = effortLimit.template cast(); + res.velocityLimit = velocityLimit.template cast(); + res.lowerPositionLimit = lowerPositionLimit.template cast(); + res.upperPositionLimit = upperPositionLimit.template cast(); + + typename ConfigVectorMap::const_iterator it; + for (it = referenceConfigurations.begin(); + it != referenceConfigurations.end(); it++) + { + res.referenceConfigurations.insert(std::make_pair(it->first, it->second.template cast())); + } + + // reserve vectors + res.inertias.resize(inertias.size()); + res.jointPlacements.resize(jointPlacements.size()); + res.joints.resize(joints.size()); + + // copy into vectors + for(size_t k = 0; k < joints.size(); ++k) + { + res.inertias[k] = inertias[k].template cast(); + res.jointPlacements[k] = jointPlacements[k].template cast(); + res.joints[k] = joints[k].template cast(); + } + + res.frames.resize(frames.size()); + for(size_t k = 0; k < frames.size(); ++k) + { + res.frames[k] = frames[k].template cast(); + } + + return res; + } + + template class JointCollectionTpl> + bool ModelTpl::operator==(const ModelTpl & other) const + { + bool res = + other.nq == nq + && other.nv == nv + && other.njoints == njoints + && other.nbodies == nbodies + && other.nframes == nframes + && other.parents == parents + && other.children == children + && other.names == names + && other.subtrees == subtrees + && other.gravity == gravity + && other.name == name; + + res &= + other.idx_qs == idx_qs + && other.nqs == nqs + && other.idx_vs == idx_vs + && other.nvs == nvs; + + if(other.referenceConfigurations.size() != referenceConfigurations.size()) + return false; + + typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin(); + typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin(); + for(long k = 0; k < (long)referenceConfigurations.size(); ++k) + { + std::advance(it,k); std::advance(it_other,k); + + if(it->second.size() != it_other->second.size()) + return false; + if(it->second != it_other->second) + return false; + } + if(other.armature.size() != armature.size()) + return false; + res &= other.armature == armature; + if(!res) return res; + + if(other.friction.size() != friction.size()) + return false; + res &= other.friction == friction; + if(!res) return res; + + if(other.damping.size() != damping.size()) + return false; + res &= other.damping == damping; + if(!res) return res; + + if(other.rotorInertia.size() != rotorInertia.size()) + return false; + res &= other.rotorInertia == rotorInertia; + if(!res) return res; + + if(other.rotorGearRatio.size() != rotorGearRatio.size()) + return false; + res &= other.rotorGearRatio == rotorGearRatio; + if(!res) return res; + + if(other.effortLimit.size() != effortLimit.size()) + return false; + res &= other.effortLimit == effortLimit; + if(!res) return res; + + if(other.velocityLimit.size() != velocityLimit.size()) + return false; + res &= other.velocityLimit == velocityLimit; + if(!res) return res; + + if(other.lowerPositionLimit.size() != lowerPositionLimit.size()) + return false; + res &= other.lowerPositionLimit == lowerPositionLimit; + if(!res) return res; + + if(other.upperPositionLimit.size() != upperPositionLimit.size()) + return false; + res &= other.upperPositionLimit == upperPositionLimit; + if(!res) return res; + + for(size_t k = 1; k < inertias.size(); ++k) + { + res &= other.inertias[k] == inertias[k]; + if(!res) return res; + } + + for(size_t k = 1; k < other.jointPlacements.size(); ++k) + { + res &= other.jointPlacements[k] == jointPlacements[k]; + if(!res) return res; + } + + res &= + other.joints == joints + && other.frames == frames; + + return res; + } + template class JointCollectionTpl> inline void ModelTpl:: appendBodyToJoint(const typename ModelTpl::JointIndex joint_index, @@ -213,7 +380,8 @@ namespace pinocchio // type is FIXED_JOINT previousFrame = (int)getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT)); } - assert((size_t)previousFrame < frames.size() && "Frame index out of bound"); + PINOCCHIO_CHECK_INPUT_ARGUMENT((size_t)previousFrame < frames.size(), + "Frame index out of bound"); return addFrame(Frame(body_name, parentJoint, (FrameIndex)previousFrame, body_placement, BODY)); } @@ -239,7 +407,8 @@ namespace pinocchio { typedef std::vector::iterator::difference_type it_diff_t; it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin(); - assert((res @@ -209,4 +209,4 @@ namespace pinocchio } // namespace fusion } // namespace pinocchio -#endif // ifndef __pinocchio_multibody_visitior_joint_binary_visitor_hpp__ +#endif // ifndef __pinocchio_multibody_visitor_joint_binary_visitor_hpp__ diff --git a/src/multibody/visitor/joint-unary-visitor.hpp b/src/multibody/visitor/joint-unary-visitor.hpp index ea66006953..6022720277 100644 --- a/src/multibody/visitor/joint-unary-visitor.hpp +++ b/src/multibody/visitor/joint-unary-visitor.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2015-2021 CNRS INRIA +// Copyright (c) 2015-2019 CNRS INRIA // -#ifndef __pinocchio_multibody_visitior_joint_unary_visitor_hpp__ -#define __pinocchio_multibody_visitior_joint_unary_visitor_hpp__ +#ifndef __pinocchio_multibody_visitor_joint_unary_visitor_hpp__ +#define __pinocchio_multibody_visitor_joint_unary_visitor_hpp__ #include @@ -232,4 +232,4 @@ namespace pinocchio } // namespace fusion } // namespace pinocchio -#endif // ifndef __pinocchio_multibody_visitior_joint_unary_visitor_hpp__ +#endif // ifndef __pinocchio_multibody_visitor_joint_unary_visitor_hpp__ diff --git a/src/parsers/sample-models.hxx b/src/parsers/sample-models.hxx index 7bd6e3ec1b..5292e4b724 100644 --- a/src/parsers/sample-models.hxx +++ b/src/parsers/sample-models.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // @@ -15,15 +15,18 @@ namespace pinocchio template class JointCollectionTpl, typename JointModel> - static void addJointAndBody(ModelTpl & model, - const JointModelBase & joint, - const std::string & parent_name, - const std::string & name, - const typename ModelTpl::SE3 & placement = ModelTpl::SE3::Random(), - bool setRandomLimits = true) + static JointIndex addJointAndBody(ModelTpl & model, + const JointModelBase & joint, + const std::string & parent_name, + const std::string & name, + const typename ModelTpl::SE3 & placement = ModelTpl::SE3::Random(), + bool setRandomLimits = true) { typedef typename JointModel::ConfigVector_t CV; typedef typename JointModel::TangentVector_t TV; + + CV qmin = CV::Constant(joint.nq(),-3.14), qmax = CV::Constant(joint.nq(),3.14); + TV vmax = TV::Constant(joint.nv(),10), taumax = TV::Constant(joint.nv(),10); JointIndex idx; @@ -37,18 +40,23 @@ namespace pinocchio ); else idx = model.addJoint(model.getJointId(parent_name),joint, - placement, name + "_joint"); + placement, name + "_joint", + taumax, + vmax, + qmin, + qmax); model.addJointFrame(idx); model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity()); model.addBodyFrame(name + "_body", idx); + return idx; } template class JointCollectionTpl> static void addManipulator(ModelTpl & model, - typename ModelTpl::JointIndex rootJoint = 0, + typename ModelTpl::JointIndex root_joint_idx = 0, const typename ModelTpl::SE3 & Mroot = ModelTpl::SE3::Identity(), const std::string & pre = "") @@ -59,55 +67,47 @@ namespace pinocchio typedef typename Model::Inertia Inertia; typedef JointCollectionTpl JC; - typedef typename JC::JointModelRX::ConfigVector_t CV; - typedef typename JC::JointModelRX::TangentVector_t TV; - - JointIndex idx = rootJoint; - - SE3 Marm(SE3::Matrix3::Identity(),SE3::Vector3::UnitZ()); - SE3 I4 = SE3::Identity(); - Inertia Ijoint(.1,Inertia::Vector3::Zero(),Inertia::Matrix3::Identity()*.01); - Inertia Iarm(1.,typename Inertia::Vector3(0,0,.5),Inertia::Matrix3::Identity()); - CV qmin = CV::Constant(-3.14), qmax = CV::Constant(3.14); - TV vmax = TV::Constant( 10), taumax = TV::Constant(10); - - idx = model.addJoint(idx,typename JC::JointModelRX(),Mroot, - pre+"shoulder1_joint",taumax,vmax,qmin,qmax); - model.appendBodyToJoint(idx,Ijoint); - model.addJointFrame(idx); - model.addBodyFrame(pre+"shoulder1_body",idx); - - idx = model.addJoint(idx,typename JC::JointModelRY(),I4, - pre+"shoulder2_joint",taumax,vmax,qmin,qmax); - model.appendBodyToJoint(idx,Ijoint); - model.addJointFrame(idx); - model.addBodyFrame(pre+"shoulder2_body",idx); - - idx = model.addJoint(idx,typename JC::JointModelRZ(),I4, - pre+"shoulder3_joint",taumax,vmax,qmin,qmax); - model.appendBodyToJoint(idx,Iarm); - model.addJointFrame(idx); - model.addBodyFrame(pre+"upperarm_body",idx); + static const SE3 Marm(SE3::Matrix3::Identity(),SE3::Vector3::UnitZ()); + static const SE3 Id4 = SE3::Identity(); + static const Inertia Ijoint(.1,Inertia::Vector3::Zero(),Inertia::Matrix3::Identity()*.01); + static const Inertia Iarm(1.,typename Inertia::Vector3(0,0,.5),Inertia::Matrix3::Identity()); + static const Scalar qmin = -3.14, qmax = 3.14; + static const Scalar vmax = 10., taumax = 10.; - idx = model.addJoint(idx,typename JC::JointModelRY(),Marm, - pre+"elbow_joint",taumax,vmax,qmin,qmax); - model.appendBodyToJoint(idx,Iarm); - model.addJointFrame(idx); - model.addBodyFrame(pre+"lowerarm_body",idx); - model.addBodyFrame(pre+"elbow_body",idx); + JointIndex joint_id; - idx = model.addJoint(idx,typename JC::JointModelRX(),Marm, - pre+"wrist1_joint",taumax,vmax,qmin,qmax); - model.appendBodyToJoint(idx,Ijoint); - model.addJointFrame(idx); - model.addBodyFrame(pre+"wrist1_body",idx); + const std::string & root_joint_name = model.names[root_joint_idx]; + joint_id = addJointAndBody(model,typename JC::JointModelRX(),root_joint_name,pre+"shoulder1",Mroot); + model.inertias[joint_id] = Ijoint; + const JointIndex root_joint_id = joint_id; - idx = model.addJoint(idx,typename JC::JointModelRY(),I4, - pre+"wrist2_joint",taumax,vmax,qmin,qmax); - model.appendBodyToJoint(idx,Iarm); - model.addJointFrame(idx); - model.addBodyFrame(pre+"effector_body",idx); + joint_id = addJointAndBody(model,typename JC::JointModelRY(),model.names[joint_id],pre+"shoulder2",Id4); + model.inertias[joint_id] = Ijoint; + + joint_id = addJointAndBody(model,typename JC::JointModelRZ(),model.names[joint_id],pre+"shoulder3",Id4); + model.inertias[joint_id] = Iarm; + model.addBodyFrame(pre+"upperarm_body",joint_id); + + joint_id = addJointAndBody(model,typename JC::JointModelRY(),model.names[joint_id],pre+"elbow",Id4); + model.inertias[joint_id] = Iarm; + model.addBodyFrame(pre+"lowerarm_body",joint_id); + model.addBodyFrame(pre+"elbow_body",joint_id); + + joint_id = addJointAndBody(model,typename JC::JointModelRX(),model.names[joint_id],pre+"wrist1",Marm); + model.inertias[joint_id] = Ijoint; + + joint_id = addJointAndBody(model,typename JC::JointModelRY(),model.names[joint_id],pre+"wrist2",Id4); + model.inertias[joint_id] = Iarm; + model.addBodyFrame(pre+"effector_body",joint_id); + const JointModel & base_joint = model.joints[root_joint_id]; + const int idx_q = base_joint.idx_q(); + const int idx_v = base_joint.idx_v(); + + model.lowerPositionLimit.template segment<6>(idx_q).fill(qmin); + model.upperPositionLimit.template segment<6>(idx_q).fill(qmax); + model.velocityLimit.template segment<6>(idx_v).fill(vmax); + model.effortLimit.template segment<6>(idx_v).fill(taumax); } #ifdef PINOCCHIO_WITH_HPP_FCL diff --git a/src/parsers/sdf.hpp b/src/parsers/sdf.hpp new file mode 100644 index 0000000000..32874a6567 --- /dev/null +++ b/src/parsers/sdf.hpp @@ -0,0 +1,98 @@ +// +// Copyright (c) 2020 CNRS +// + +#ifndef __pinocchio_parsers_sdf_hpp__ +#define __pinocchio_parsers_sdf_hpp__ + +#include "pinocchio/parsers/urdf.hpp" +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/multibody/geometry.hpp" +#include "pinocchio/algorithm/contact-info.hpp" + +namespace pinocchio +{ + namespace sdf + { + +#ifdef PINOCCHIO_WITH_HPP_FCL +#ifdef PINOCCHIO_WITH_SDF + /** + * @brief Build The GeometryModel from a URDF file. Search for meshes + * in the directories specified by the user first and then in + * the environment variable ROS_PACKAGE_PATH + * + * @param[in] model The model of the robot, built with + * urdf::buildModel + * @param[in] filename The URDF complete (absolute) file path + * @param[in] packageDirs A vector containing the different directories + * where to search for models and meshes, typically + * obtained from calling pinocchio::rosPaths() + * + * @param[in] type The type of objects that must be loaded (must be VISUAL or COLLISION) + * @param[in] meshLoader object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader. + * @param[out] geomModel Reference where to put the parsed information. + * + * @return Returns the reference on geom model for convenience. + * + * \warning If hpp-fcl has not been found during compilation, COLLISION objects can not be loaded + * + */ + template class JointCollectionTpl> + GeometryModel & buildGeom(ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const std::string & filename, + const GeometryType type, + GeometryModel & geomModel, + const std::vector & packageDirs = std::vector (), + ::hpp::fcl::MeshLoaderPtr meshLoader = ::hpp::fcl::MeshLoaderPtr()); + + + /// + /// \brief Build the model from a URDF file with a particular joint as root of the model tree inside + /// the model given as reference argument. + /// + /// \param[in] filename The URDF complete file path. + /// \param[in] rootJoint The joint at the root of the model tree. + /// \param[in] verbose Print parsing info. + /// \param[out] model Reference model where to put the parsed information. + /// \return Return the reference on argument model for convenience. + /// + template class JointCollectionTpl> + ModelTpl & + buildModel(const std::string & filename, + const typename ModelTpl::JointModel & rootJoint, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const bool verbose = false); + + + /// + /// \brief Build the model from a URDF file with a fixed joint as root of the model tree. + /// + /// \param[in] filename The URDF complete file path. + /// \param[in] verbose Print parsing info. + /// \param[out] model Reference model where to put the parsed information. + /// \return Return the reference on argument model for convenience. + /// + template class JointCollectionTpl> + ModelTpl & + buildModel(const std::string & filename, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const bool verbose = false); + +#endif +#endif + + } // namespace sdf +} // namespace pinocchio + +#ifdef PINOCCHIO_WITH_SDF +#ifdef PINOCCHIO_WITH_HPP_FCL +#include "pinocchio/parsers/sdf/model.hxx" +#include "pinocchio/parsers/sdf/geometry.hxx" +#endif +#endif + +#endif // ifndef __pinocchio_parsers_sdf_hpp__ diff --git a/src/parsers/sdf/geometry.cpp b/src/parsers/sdf/geometry.cpp new file mode 100644 index 0000000000..a2d6dc770f --- /dev/null +++ b/src/parsers/sdf/geometry.cpp @@ -0,0 +1,93 @@ +// +// Copyright (c) 2021 CNRS +// + +#include "pinocchio/parsers/sdf.hpp" +#include "pinocchio/parsers/utils.hpp" + +#include +#include +#include +#ifdef PINOCCHIO_WITH_HPP_FCL +#include +#include +#endif + +namespace pinocchio +{ + namespace sdf + { + namespace details + { + +#ifdef PINOCCHIO_WITH_HPP_FCL + + void recursiveParseGraphForGeom(const SdfGraph& graph, + ::hpp::fcl::MeshLoaderPtr& meshLoader, + const ::sdf::ElementPtr link, + GeometryModel & geomModel, + const std::vector & package_dirs, + const GeometryType type) + { + const std::string& linkName = link->Get("name"); + switch(type) + { + case COLLISION: + addLinkGeometryToGeomModel< ::pinocchio::COLLISION >(graph, meshLoader, link, + geomModel, package_dirs); + break; + case VISUAL: + addLinkGeometryToGeomModel< ::pinocchio::VISUAL >(graph, meshLoader, link, + geomModel, package_dirs); + break; + default: + break; + } + const std::vector& childrenOfLink = + graph.childrenOfLinks.find(linkName)->second; + + for(std::vector::const_iterator childOfChild = + std::begin(childrenOfLink); + childOfChild != std::end(childrenOfLink); ++childOfChild) + { + const ::sdf::ElementPtr childJointElement = + graph.mapOfJoints.find(*childOfChild)->second; + + if (childJointElement->template Get("type") != "ball") { + const std::string childLinkName = + childJointElement->GetElement("child")->template Get(); + const ::sdf::ElementPtr childLinkElement = + graph.mapOfLinks.find(childLinkName)->second; + recursiveParseGraphForGeom(graph, meshLoader, childLinkElement, + geomModel, package_dirs,type); + } + } + } + + void parseTreeForGeom(const SdfGraph& graph, + GeometryModel & geomModel, + const GeometryType type, + const std::vector & package_dirs, + ::hpp::fcl::MeshLoaderPtr meshLoader) + { + std::vector hint_directories(package_dirs); + std::vector ros_pkg_paths = rosPaths(); + hint_directories.insert(hint_directories.end(), ros_pkg_paths.begin(), + ros_pkg_paths.end()); + + if (!meshLoader) meshLoader = fcl::MeshLoaderPtr(new fcl::MeshLoader); + const ::sdf::ElementPtr jointElement = graph.mapOfJoints.find("static")->second; + + const std::string& childName = + jointElement->GetElement("child")->Get(); + const ::sdf::ElementPtr childElement = graph.mapOfLinks.find(childName)->second; + recursiveParseGraphForGeom(graph, meshLoader, childElement, + geomModel, hint_directories, type); + + } + +#endif + + } // namespace details + } // namespace sdf +} // namespace pinocchio diff --git a/src/parsers/sdf/geometry.hxx b/src/parsers/sdf/geometry.hxx new file mode 100644 index 0000000000..905e7812bb --- /dev/null +++ b/src/parsers/sdf/geometry.hxx @@ -0,0 +1,353 @@ +// +// Copyright (c) 2020 CNRS +// + +#ifndef __pinocchio_multibody_parsers_sdf_geometry_hxx__ +#define __pinocchio_multibody_parsers_sdf_geometry_hxx__ + +#include "pinocchio/parsers/sdf.hpp" +#include "pinocchio/parsers/utils.hpp" + +#include +#include +#include + +#ifdef PINOCCHIO_WITH_HPP_FCL +#include +#include +#endif // PINOCCHIO_WITH_HPP_FCL + +namespace pinocchio +{ + namespace sdf + { + namespace details + { + + + /** + * @brief Get the first geometry attached to a link + * + * @param[in] link The URDF link + * + * @return Either the first collision or visual + */ + template + inline bool hasLinkElement(const ::sdf::ElementPtr link); + + template <> + inline bool hasLinkElement<::pinocchio::COLLISION>(const ::sdf::ElementPtr link) + { + return link->HasElement("collision"); + } + + template <> + inline bool hasLinkElement<::pinocchio::VISUAL>(const ::sdf::ElementPtr link) + { + return link->HasElement("visual"); + } + + /** + * @brief Get the array of geometries attached to a link + * + * @param[in] link The URDF link + * + * @return the array of either collisions or visuals + */ + template + inline const std::vector< ::sdf::ElementPtr> + getLinkGeometryArray(const ::sdf::ElementPtr link); + + template <> + inline const std::vector< ::sdf::ElementPtr> + getLinkGeometryArray<::pinocchio::COLLISION>(const ::sdf::ElementPtr link) + { + std::vector< ::sdf::ElementPtr> geometry_array; + ::sdf::ElementPtr geomElement = link->GetElement("collision"); + while (geomElement) + { + //Inserting data in std::map + geometry_array.push_back(geomElement); + geomElement = link->GetNextElement("collision"); + } + return geometry_array; + } + + template <> + inline const std::vector< ::sdf::ElementPtr> + getLinkGeometryArray<::pinocchio::VISUAL>(const ::sdf::ElementPtr link) + { + std::vector< ::sdf::ElementPtr> geometry_array; + ::sdf::ElementPtr geomElement = link->GetElement("visual"); + while (geomElement) + { + //Inserting data in std::map + geometry_array.push_back(geomElement); + geomElement = link->GetNextElement("visual"); + } + return geometry_array; + } + + + template + static void retrieveMeshScale(const fcl::Vec3f & meshScale, + const Eigen::MatrixBase & scale) + { + Vector3 & scale_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,scale); + scale_ << + meshScale[0], + meshScale[1], + meshScale[2]; + } + + + /** + * @brief Get a fcl::CollisionObject from an urdf geometry, searching + * for it in specified package directories + * + * @param[in] urdf_geometry A shared pointer on the input urdf Geometry + * @param[in] package_dirs A vector containing the different directories where to search for packages + * @param[out] meshPath The Absolute path of the mesh currently read + * @param[out] meshScale Scale of transformation currently applied to the mesh + * + * @return A shared pointer on the geometry converted as a fcl::CollisionGeometry + */ + boost::shared_ptr + inline retrieveCollisionGeometry(const SdfGraph& graph, + fcl::MeshLoaderPtr& meshLoader, + const std::string& linkName, + const std::string& geomName, + const ::sdf::ElementPtr sdf_geometry, + const std::vector & package_dirs, + std::string & meshPath, + Eigen::Vector3d & meshScale) + { + boost::shared_ptr geometry; + + // Handle the case where collision geometry is a mesh + if (sdf_geometry->HasElement("mesh")) + { + const ::sdf::ElementPtr sdf_mesh = sdf_geometry->GetElement("mesh"); + std::string collisionFilename = sdf_mesh->Get("uri"); + + meshPath = retrieveResourcePath(collisionFilename, package_dirs); + if (meshPath == "") { + std::stringstream ss; + ss << "Mesh " << collisionFilename << " could not be found."; + throw std::invalid_argument (ss.str()); + } + + const ignition::math::Vector3d ign_scale = + sdf_mesh->Get("scale"); + + fcl::Vec3f scale = fcl::Vec3f(ign_scale.X(), + ign_scale.Y(), + ign_scale.Z() + ); + + retrieveMeshScale(scale, meshScale); + + // Create FCL mesh by parsing Collada file. + hpp::fcl::BVHModelPtr_t bvh = meshLoader->load (meshPath, scale); + geometry = bvh; + } + + // Handle the case where collision geometry is a cylinder + // Use FCL capsules for cylinders + else if (sdf_geometry->HasElement("cylinder")) + { + meshScale << 1,1,1; + const ::sdf::ElementPtr collisionGeometry = + sdf_geometry->GetElement("cylinder"); + + double radius = collisionGeometry->Get("radius"); + double length = collisionGeometry->Get("length"); + + // Create fcl capsule geometry. + meshPath = "CYLINDER"; + geometry = boost::shared_ptr < fcl::CollisionGeometry >( + new fcl::Cylinder (radius, length)); + } + // Handle the case where collision geometry is a box. + else if (sdf_geometry->HasElement("box")) + { + meshPath = "BOX"; + meshScale << 1,1,1; + + const ::sdf::ElementPtr collisionGeometry = + sdf_geometry->GetElement("box"); + double x = collisionGeometry->Get("x"); + double y = collisionGeometry->Get("y"); + double z = collisionGeometry->Get("z"); + geometry = boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Box (x, y, z)); + } + // Handle the case where collision geometry is a sphere. + else if (sdf_geometry->HasElement("sphere")) + { + meshPath = "SPHERE"; + meshScale << 1,1,1; + const ::sdf::ElementPtr collisionGeometry = + sdf_geometry->GetElement("sphere"); + + double radius = collisionGeometry->Get("radius"); + geometry = + boost::shared_ptr < fcl::CollisionGeometry > (new fcl::Sphere (radius)); + } + else throw std::invalid_argument("Unknown geometry type :"); + + if (!geometry) + { + throw std::invalid_argument("The polyhedron retrived is empty"); + } + return geometry; + } + + template + static void addLinkGeometryToGeomModel(const SdfGraph & graph, + ::hpp::fcl::MeshLoaderPtr & meshLoader, + ::sdf::ElementPtr link, + GeometryModel & geomModel, + const std::vector & package_dirs) + { + + typedef std::vector< ::sdf::ElementPtr> GeometryArray; + typedef GeometryModel::SE3 SE3; + + bool has_element = false; + if(hasLinkElement) + { + std::string meshPath = ""; + + Eigen::Vector3d meshScale(Eigen::Vector3d::Ones()); + const std::string link_name = link->Get("name"); + + const GeometryArray geometries_array = + getLinkGeometryArray(link); + + FrameIndex frame_id = graph.urdfVisitor.getBodyId(link_name); + Frame frame = graph.urdfVisitor.getBodyFrame (link_name); + + SE3 body_placement = frame.placement; + + std::size_t objectId = 0; + for (typename GeometryArray::const_iterator i = geometries_array.begin(); + i != geometries_array.end(); ++i) + { + meshPath.clear(); +#ifdef PINOCCHIO_WITH_HPP_FCL + const std::string geom_name = (*i)->template Get("name"); + + const ::sdf::ElementPtr sdf_geometry = (*i)->GetElement("geometry"); + + const GeometryObject::CollisionGeometryPtr geometry = + retrieveCollisionGeometry(graph, meshLoader, link_name, geom_name, + sdf_geometry, package_dirs, meshPath, meshScale); +#else + const ::sdf::ElementPtr sdf_mesh = sdf_geometry->GetElement("mesh"); + + if (sdf_mesh) + { + std::string collisionFilename = sdf_mesh->Get("url"); + meshPath = retrieveResourcePath(collisionFilename, package_dirs); + + const ignition::math::Vector3d ign_scale = + sdf_mesh->Get("scale"); + + const fcl::Vec3f scale = fcl::Vec3f(ign_scale.X(), ign_scale.Y(), + ign_scale.Z()); + retrieveMeshScale(scale, meshScale); + } + + const boost::shared_ptr + geometry(new fcl::CollisionGeometry()); +#endif // PINOCCHIO_WITH_HPP_FCL + + const ignition::math::Pose3d& pose = + (*i)->template Get("pose"); + + Eigen::Vector4d meshColor(Eigen::Vector4d::Zero()); + std::string meshTexturePath; + bool overrideMaterial = false; + const ::sdf::ElementPtr sdf_material = (*i)->GetElement("material"); + if (sdf_material) + { + const ignition::math::Vector4d ign_meshColor = + sdf_material->Get("ambient"); + + meshColor << ign_meshColor.X(), + ign_meshColor.Y(), + ign_meshColor.Z(), + ign_meshColor.W(); + overrideMaterial = true; + } + + const SE3 lMg = convertFromPose3d(pose); + const SE3 geomPlacement = body_placement * lMg; + std::ostringstream geometry_object_suffix; + geometry_object_suffix << "_" << objectId; + const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str()); + GeometryObject geometry_object(geometry_object_name, + frame_id, frame.parent, + geometry, + geomPlacement, meshPath, meshScale, + overrideMaterial, meshColor, meshTexturePath); + geomModel.addGeometryObject(geometry_object); + ++objectId; + } + } + } + + + /** + * @brief Recursive procedure for reading the URDF tree, looking for geometries + * This function fill the geometric model whith geometry objects retrieved from the URDF tree + * + * @param[in] tree The URDF kinematic tree + * @param[in] meshLoader The FCL mesh loader to avoid duplications of already loaded geometries + * @param[in] link The current URDF link + * @param model The model to which is the GeometryModel associated + * @param geomModel The GeometryModel where the Collision Objects must be added + * @param[in] package_dirs A vector containing the different directories where to search for packages + * @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION) + * + */ + PINOCCHIO_DLLAPI void recursiveParseGraphForGeom(const SdfGraph& graph, + ::hpp::fcl::MeshLoaderPtr& meshLoader, + const ::sdf::ElementPtr link, + GeometryModel & geomModel, + const std::vector & package_dirs, + const GeometryType type); + + PINOCCHIO_DLLAPI void parseTreeForGeom(const SdfGraph& graph, + GeometryModel & geomModel, + const GeometryType type, + const std::vector & package_dirs, + ::hpp::fcl::MeshLoaderPtr meshLoader); + } // namespace details + + template class JointCollectionTpl> + GeometryModel& buildGeom(ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const std::string & filename, + const GeometryType type, + GeometryModel & geomModel, + const std::vector & package_dirs, + ::hpp::fcl::MeshLoaderPtr meshLoader) + + { + ::pinocchio::urdf::details::UrdfVisitor visitor (model); + ::pinocchio::sdf::details::SdfGraph graph (visitor, contact_models); + //if (verbose) visitor.log = &std::cout; + + //Create maps from the SDF Graph + graph.parseGraph(filename); + + details::parseTreeForGeom (graph, geomModel, type, + package_dirs, meshLoader); + return geomModel; + } + + } // namespace sdf +} // namespace pinocchio + +#endif // ifndef __pinocchio_multibody_parsers_sdf_geometry_hxx__ diff --git a/src/parsers/sdf/model.cpp b/src/parsers/sdf/model.cpp new file mode 100644 index 0000000000..475dcd29f0 --- /dev/null +++ b/src/parsers/sdf/model.cpp @@ -0,0 +1,48 @@ +// +// Copyright (c) 2021 CNRS +// + +#include "pinocchio/math/matrix.hpp" +#include "pinocchio/parsers/sdf.hpp" +#include "pinocchio/parsers/urdf.hpp" +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/algorithm/contact-info.hpp" + + +#include +#include +#include +#include +#include +#include + +namespace pinocchio +{ + namespace sdf + { + namespace details + { +#ifdef PINOCCHIO_WITH_HPP_FCL + void parseRootTree(SdfGraph& graph) + { + //First joint connecting universe + const ::sdf::ElementPtr jointElement = graph.mapOfJoints.find("static")->second; + const std::string childName = + jointElement->GetElement("child")->Get();; + const ::sdf::ElementPtr childElement = graph.mapOfLinks.find(childName)->second; + const ::sdf::ElementPtr inertialElem = childElement->GetElement("inertial"); + const Inertia Y = ::pinocchio::sdf::details::convertInertiaFromSdf(inertialElem); + + graph.urdfVisitor.addRootJoint(convertInertiaFromSdf(inertialElem), childName); + const std::vector& childrenOfLink = + graph.childrenOfLinks.find(childName)->second; + for(std::vector::const_iterator childOfChild = std::begin(childrenOfLink); + childOfChild != std::end(childrenOfLink); ++childOfChild) + { + graph.recursiveFillModel(graph.mapOfJoints.find(*childOfChild)->second); + } + } +#endif + } + } +} diff --git a/src/parsers/sdf/model.hxx b/src/parsers/sdf/model.hxx new file mode 100644 index 0000000000..0f7c3120ac --- /dev/null +++ b/src/parsers/sdf/model.hxx @@ -0,0 +1,500 @@ +// +// Copyright (c) 2020 CNRS +// + +#ifndef __pinocchio_multibody_parsers_sdf_model_hxx__ +#define __pinocchio_multibody_parsers_sdf_model_hxx__ + +#include "pinocchio/math/matrix.hpp" +#include "pinocchio/parsers/sdf.hpp" +#include "pinocchio/parsers/urdf.hpp" +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/algorithm/contact-info.hpp" + + +#include +#include +#include +#include +#include +#include + +namespace pinocchio +{ + namespace sdf + { + namespace details + { + + static SE3 convertFromPose3d(const ignition::math::Pose3d& posePlacement) + { + const ignition::math::Quaterniond& q = posePlacement.Rot(); + const ignition::math::Vector3d& p = posePlacement.Pos(); + return SE3(SE3::Quaternion(q.W(),q.X(),q.Y(),q.Z()).matrix(), + SE3::Vector3(p.X(),p.Y(),p.Z())); + } + + /// + /// \brief Convert SDF Inertial quantity to Spatial Inertia. + /// + /// \param[in] Y The input URDF Inertia. + /// + /// \return The converted Spatial Inertia pinocchio::Inertia. + /// + static Inertia convertInertiaFromSdf(const ::sdf::ElementPtr inertial) + { + + const ignition::math::Pose3d& pose = + inertial->template Get("pose"); + const double& mass = inertial->template Get("mass"); + + const ::sdf::ElementPtr inertiaElem = inertial->GetElement("inertia"); + const double ixx = inertiaElem->template Get("ixx"); + const double ixy = inertiaElem->template Get("ixy"); + const double ixz = inertiaElem->template Get("ixz"); + const double iyy = inertiaElem->template Get("iyy"); + const double iyz = inertiaElem->template Get("iyz"); + const double izz = inertiaElem->template Get("izz"); + + const Inertia::Vector3 com(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z()); + const Inertia::Matrix3 & R = + Eigen::Quaterniond(pose.Rot().W(),pose.Rot().X(), + pose.Rot().Y(),pose.Rot().Z()).matrix(); + + Inertia::Matrix3 I; + I << ixx,ixy,ixz, + ixy,iyy,iyz, + ixz,iyz,izz; + + return Inertia(mass,com,R*I*R.transpose()); + } + + + struct SdfGraph + { + public: + typedef std::map ElementMap_t; + typedef std::map > StringVectorMap_t; + + ElementMap_t mapOfLinks, mapOfJoints; + StringVectorMap_t childrenOfLinks; + std::string modelName; + std::vector childToBeAdded; + + typedef ::pinocchio::urdf::details::UrdfVisitorBase UrdfVisitorBase; + UrdfVisitorBase& urdfVisitor; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models; + + SdfGraph(::pinocchio::urdf::details::UrdfVisitorBase& urdfVisitor, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models) + : urdfVisitor(urdfVisitor), + contact_models(contact_models) + {} + + void parseGraph(const std::string & filename) + { + // load and check sdf file + ::sdf::SDFPtr sdfElement(new ::sdf::SDF()); + ::sdf::init(sdfElement); + if (!::sdf::readFile(filename, sdfElement)) + { + throw std::invalid_argument("The file " + filename + " does not " + "contain a valid SDF model 1."); + } + + // start parsing model + const ::sdf::ElementPtr rootElement = sdfElement->Root(); + + if (!rootElement->HasElement("model")) + { + throw std::invalid_argument("The file " + filename + " does not " + "contain a valid SDF model 2."); + } + + const ::sdf::ElementPtr modelElement = rootElement->GetElement("model"); + + modelName = modelElement->template Get("name"); + + // parse model links + ::sdf::ElementPtr linkElement = modelElement->GetElement("link"); + while (linkElement) + { + const std::string linkName = linkElement->Get("name"); + //Inserting data in std::map + mapOfLinks.insert(std::make_pair(linkName, linkElement)); + childrenOfLinks.insert(std::make_pair(linkName, std::vector())); + linkElement = linkElement->GetNextElement("link"); + } + + childrenOfLinks.insert(std::make_pair("world", std::vector())); + + // parse model joints + ::sdf::ElementPtr jointElement = modelElement->GetElement("joint"); + while (jointElement) + { + const std::string jointName = jointElement->template Get("name"); + std::string parentLinkName = + jointElement->GetElement("parent")->template Get(); + //Inserting data in std::map + mapOfJoints.insert(std::make_pair(jointName, jointElement)); + //Create data of children of links + childrenOfLinks.find(parentLinkName)->second.push_back(jointName); + jointElement = jointElement->GetNextElement("joint"); + } + } + + static bool existConstraint(const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const std::string& jointName) + { + for(PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)::const_iterator + cm = std::begin(contact_models); + cm != std::end(contact_models); ++cm) + { + if(cm->name == "jointName") + return true; + } + return false; + } + + static bool existChildName(const std::vector& listOfNames, + const std::string& childName) + { + for(std::vector::const_iterator cm = std::begin(listOfNames); + cm != std::end(listOfNames); ++cm) + { + if((*cm) == childName) + return true; + } + return false; + } + + + static int getConstraintId(const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const std::string& jointName) + { + std::size_t i = 0; + for(PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)::const_iterator + cm = std::begin(contact_models); + cm != std::end(contact_models); ++cm) + { + if(cm->name == "jointName") + return i; + i++; + } + return -1; + } + + + int getConstraintIdFromChild(const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const std::string& childName) + { + std::size_t i = 0; + for(PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)::const_iterator + cm = std::begin(contact_models); + cm != std::end(contact_models); ++cm) + { + const std::string childFromMap = + mapOfJoints.find(cm->name)->second->GetElement("child")->Get(); + if(childFromMap == childName) + return i; + i++; + } + return -1; + } + + /// + /// \brief Recursive procedure for reading the SDF tree. + /// The function returns an exception as soon as a necessary Inertia or Joint information are missing. + /// + /// \param[in] link The current SDF link. + /// \param[in] model The model where the link must be added. + /// + void recursiveFillModel(const ::sdf::ElementPtr jointElement) + { + typedef UrdfVisitorBase::Scalar Scalar; + typedef UrdfVisitorBase::SE3 SE3; + typedef UrdfVisitorBase::Vector Vector; + typedef UrdfVisitorBase::Vector3 Vector3; + const std::string& jointName = jointElement->template Get("name"); + + std::ostringstream joint_info; + const std::string parentName = jointElement->GetElement("parent")->Get(); + const std::string childName = + jointElement->GetElement("child")->Get(); + const ::sdf::ElementPtr childElement = mapOfLinks.find(childName)->second; + const ::sdf::ElementPtr parentElement = mapOfLinks.find(parentName)->second; + const ignition::math::Pose3d parentPlacement = + parentElement->template Get("pose"); + const ignition::math::Pose3d childPlacement = + childElement->template Get("pose"); + + const JointIndex parentJointId = urdfVisitor.getParentId(parentName); + const std::string& parentJointName = urdfVisitor.getJointName(parentJointId); + + + SE3 cMj(SE3::Identity()), pMjp(SE3::Identity()); + + if (jointElement->HasElement("pose")) + { + const ignition::math::Pose3d cMj_ig = + jointElement->template Get("pose"); + cMj = ::pinocchio::sdf::details::convertFromPose3d(cMj_ig); + } + + if (parentJointName != "root_joint") { + const ::sdf::ElementPtr parentJointElement = mapOfJoints.find(parentJointName)->second; + + if (parentJointElement->HasElement("pose")) + { + + const ignition::math::Pose3d parentcMj_ig = + parentJointElement->template Get("pose"); + pMjp = ::pinocchio::sdf::details::convertFromPose3d(parentcMj_ig); + } + } + + const SE3 oMp = ::pinocchio::sdf::details::convertFromPose3d(parentPlacement); + const SE3 oMc = ::pinocchio::sdf::details::convertFromPose3d(childPlacement); + const SE3 jointPlacement = pMjp.inverse() * oMp.inverse() * oMc * cMj; + + joint_info << "Joint " << jointName << " connects parent " << parentName + << " link to child " << childName << " link" << " with joint type " + << jointElement->template Get("type")<template Get("type") == "ball") { + JointIndex existingParentJointId; + if (urdfVisitor.existFrame(childName, BODY)) + { + if (! existConstraint(contact_models, jointName)) + { + std::cout<second; + std::cout<<"connected by joint " + <HasElement("pose")) + { + const ignition::math::Pose3d prevcMj_ig = + prevJointElement->template Get("pose"); + cMj1 = ::pinocchio::sdf::details::convertFromPose3d(prevcMj_ig); + } + + ::pinocchio::RigidConstraintModel rcm (::pinocchio::CONTACT_3D, + parentJointId, + jointPlacement, + existingParentJointId, + cMj1.inverse() * cMj); + rcm.name = jointName; + contact_models.push_back(rcm); + + } + else + { + const int i = getConstraintId(contact_models, jointName); + if(i != -1) { + contact_models[i].joint2_id = parentJointId; + contact_models[i].joint2_placement = jointPlacement; + } + else + { + throw std::invalid_argument("Unknown error with sdf parsing"); + } + } + } + else + { + std::cout<GetElement("inertial"); + const Inertia Y = ::pinocchio::sdf::details::convertInertiaFromSdf(inertialElem); + + FrameIndex parentFrameId = urdfVisitor.getBodyId(parentName); + Vector max_effort(1), max_velocity(1), min_config(1), max_config(1); + Vector spring_stiffness(1), spring_reference(1); + Vector friction(Vector::Constant(1,0.)), damping(Vector::Constant(1,0.)); + ignition::math::Vector3d axis_ignition; + Vector3 axis; + + const Scalar infty = std::numeric_limits::infinity(); + + if (jointElement->HasElement("axis")) + { + const ::sdf::ElementPtr axisElem = jointElement->GetElement("axis"); + + axis_ignition = + axisElem->Get("xyz"); + axis << axis_ignition.X(), axis_ignition.Y(), axis_ignition.Z(); + + if (axisElem->HasElement("limit")) + { + const ::sdf::ElementPtr limitElem = axisElem->GetElement("limit"); + if (limitElem->HasElement("upper")) + { + max_config[0] = limitElem->Get("upper"); + } + if (limitElem->HasElement("lower")) + { + min_config[0] = limitElem->Get("lower"); + } + if (limitElem->HasElement("effort")) + { + max_effort[0] = limitElem->Get("effort"); + } + if (limitElem->HasElement("velocity")) + { + max_velocity[0] = limitElem->Get("velocity"); + } + } + if (axisElem->HasElement("dynamics")) + { + const ::sdf::ElementPtr dynamicsElem = axisElem->GetElement("dynamics"); + if (dynamicsElem->HasElement("spring_reference")) + { + spring_reference[0] = dynamicsElem->Get("spring_reference"); + } + if (dynamicsElem->HasElement("spring_stiffness")) + { + spring_stiffness[0] = dynamicsElem->Get("spring_stiffness"); + } + if (dynamicsElem->HasElement("damping")) + { + damping[0] = dynamicsElem->Get("damping"); + } + } + } + + if (jointElement->template Get("type") == "universal") + { + } + else if (jointElement->template Get("type") == "revolute") + { + joint_info << "joint REVOLUTE with axis"<< axis.transpose(); + urdfVisitor.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis, + parentFrameId, jointPlacement, jointName, + Y, cMj.inverse(), childName, + max_effort, max_velocity, min_config, max_config, + friction,damping); + } + else if (jointElement->template Get("type") == "gearbox") + { + joint_info << "joint GEARBOX with axis"; + urdfVisitor.addFixedJointAndBody(parentFrameId, jointPlacement, jointName, + Y, childName); + } + else if (jointElement->template Get("type") == "ball") + { + max_effort = Vector::Constant(3, infty); + max_velocity = Vector::Constant(3, infty); + min_config = Vector::Constant(4,-infty); + max_config = Vector::Constant(4, infty); + min_config.setConstant(-1.01); + max_config.setConstant( 1.01); + friction = Vector::Constant(3, 0.); + damping = Vector::Constant(3, 0.); + + joint_info << "joint BALL"; + //std::cerr<<"TODO: Fix BALL JOINT"<template Get("type")<& childrenOfLink = + childrenOfLinks.find(childName)->second; + + for(std::vector::const_iterator childOfChild = std::begin(childrenOfLink); + childOfChild != std::end(childrenOfLink); ++childOfChild) + { + const ::sdf::ElementPtr childOfChildElement = + mapOfJoints.find(*childOfChild)->second; + recursiveFillModel(childOfChildElement); + } + } + } + }; + + void PINOCCHIO_DLLAPI parseRootTree(SdfGraph& graph); + } + + template class JointCollectionTpl> + ModelTpl & + buildModel(const std::string & filename, + const typename ModelTpl::JointModel & root_joint, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const bool verbose) + { + ::pinocchio::urdf::details::UrdfVisitorWithRootJoint visitor (model, root_joint); + + ::pinocchio::sdf::details::SdfGraph graph (visitor, contact_models); + + if (verbose) visitor.log = &std::cout; + + //Create maps from the SDF Graph + graph.parseGraph(filename); + //Use the SDF graph to create the model + details::parseRootTree(graph); + return model; + } + + template class JointCollectionTpl> + ModelTpl & + buildModel(const std::string & filename, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models, + const bool verbose) + { + ::pinocchio::urdf::details::UrdfVisitor visitor (model); + ::pinocchio::sdf::details::SdfGraph graph (visitor, contact_models); + + if (verbose) visitor.log = &std::cout; + + //Create maps from the SDF Graph + graph.parseGraph(filename); + //Use the SDF graph to create the model + details::parseRootTree(graph); + return model; + } + } +} + +#endif // ifndef __pinocchio_parsers_sdf_hpp__ diff --git a/src/parsers/srdf.hxx b/src/parsers/srdf.hxx index d38d97dce8..3b34eece39 100644 --- a/src/parsers/srdf.hxx +++ b/src/parsers/srdf.hxx @@ -169,13 +169,13 @@ namespace pinocchio { if (joint.first == "joint") { - std::string joint_name = joint.second.get(".name"); - const Scalar rotor_mass = (Scalar)joint.second.get(".mass"); - const Scalar rotor_gr = (Scalar)joint.second.get(".gear_ratio"); + const std::string joint_name = joint.second.get(".name"); + const Scalar rotor_inertia = (Scalar)joint.second.get(".mass"); + const Scalar rotor_gear_ratio = (Scalar)joint.second.get(".gear_ratio"); if (verbose) { std::cout << "(" << joint_name << " , " << - rotor_mass << " , " << rotor_gr << ")" << std::endl; + rotor_inertia << " , " << rotor_gear_ratio << ")" << std::endl; } // Search in model the joint and its config id typename Model::JointIndex joint_id = model.getJointId(joint_name); @@ -184,8 +184,10 @@ namespace pinocchio { const JointModel & joint = model.joints[joint_id]; PINOCCHIO_CHECK_INPUT_ARGUMENT(joint.nv()==1); - model.rotorInertia(joint.idx_v()) = rotor_mass; - model.rotorGearRatio(joint.idx_v()) = rotor_gr; // joint with 1 dof + + model.armature[joint.idx_v()] += rotor_inertia * rotor_gear_ratio * rotor_gear_ratio; + model.rotorInertia(joint.idx_v()) = rotor_inertia; + model.rotorGearRatio(joint.idx_v()) = rotor_gear_ratio; // joint with 1 dof } else { @@ -217,21 +219,22 @@ namespace pinocchio const ConfigVectorType& fromXML, ConfigVectorType& config) { - _algo (joint.derived(), joint_name, fromXML, config); + algo_impl(joint.derived(), joint_name, fromXML, config); } - private: + private: template - static void _algo (const JointModelRevoluteUnboundedTpl & joint, - const std::string& joint_name, - const ConfigVectorType& fromXML, - ConfigVectorType& config) + static void algo_impl(const JointModelRevoluteUnboundedTpl & joint, + const std::string& joint_name, + const ConfigVectorType& fromXML, + ConfigVectorType& config) { typedef JointModelRevoluteUnboundedTpl JointModelRUB; PINOCCHIO_STATIC_ASSERT(JointModelRUB::NQ == 2, JOINT_MODEL_REVOLUTE_SHOULD_HAVE_2_PARAMETERS); if (fromXML.size() != 1) std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl; - else { + else + { SINCOS(fromXML[0], &config[joint.idx_q()+1], &config[joint.idx_q()+0]); @@ -239,12 +242,12 @@ namespace pinocchio } template - static void _algo (const JointModel & joint, - const std::string& joint_name, - const ConfigVectorType& fromXML, - ConfigVectorType& config) + static void algo_impl(const JointModel & joint, + const std::string& joint_name, + const ConfigVectorType & fromXML, + ConfigVectorType & config) { - if (joint.nq() != fromXML.size()) + if(joint.nq() != fromXML.size()) std::cerr << "Could not read joint config (" << joint_name << " , " << fromXML.transpose() << ")" << std::endl; else config.segment(joint.idx_q(),joint.nq()) = fromXML; @@ -320,8 +323,8 @@ namespace pinocchio joint_config = Eigen::Map(config_vec.data(), (Eigen::DenseIndex)config_vec.size()); typedef LoadReferenceConfigurationStep LoadReferenceConfigurationStep_t; - LoadReferenceConfigurationStep_t::run (joint, - typename LoadReferenceConfigurationStep_t::ArgsType (joint_name, joint_config, ref_config)); + LoadReferenceConfigurationStep_t::run(joint, + typename LoadReferenceConfigurationStep_t::ArgsType(joint_name, joint_config, ref_config)); if (verbose) { std::cout << "(" << joint_name << " , " << joint_config.transpose() << ")" << std::endl; diff --git a/src/parsers/urdf/geometry.hxx b/src/parsers/urdf/geometry.hxx index 3c3d7e52b9..a740ce490b 100644 --- a/src/parsers/urdf/geometry.hxx +++ b/src/parsers/urdf/geometry.hxx @@ -7,7 +7,6 @@ #include "pinocchio/parsers/urdf.hpp" -#include #include #include #include diff --git a/src/parsers/urdf/model.hxx b/src/parsers/urdf/model.hxx index 6982ff2a49..100495053e 100644 --- a/src/parsers/urdf/model.hxx +++ b/src/parsers/urdf/model.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2021 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // @@ -13,6 +13,7 @@ #include #include #include +#include namespace pinocchio { @@ -26,9 +27,10 @@ namespace pinocchio class UrdfVisitorBaseTpl { public: enum JointType { - REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR + REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, SPHERICAL }; + typedef enum ::pinocchio::FrameType FrameType; typedef _Scalar Scalar; typedef SE3Tpl SE3; typedef InertiaTpl Inertia; @@ -58,6 +60,25 @@ namespace pinocchio const VectorConstRef& damping ) = 0; + virtual void addJointAndBody( + JointType type, + const Vector3& axis, + const FrameIndex & parentFrameId, + const SE3 & placement, + const std::string & joint_name, + const Inertia& Y, + const SE3& frame_placement, + const std::string & body_name, + const VectorConstRef& max_effort, + const VectorConstRef& max_velocity, + const VectorConstRef& min_config, + const VectorConstRef& max_config, + const VectorConstRef& friction, + const VectorConstRef& damping + ) = 0; + + + virtual void addFixedJointAndBody( const FrameIndex & parentFrameId, const SE3 & joint_placement, @@ -74,6 +95,21 @@ namespace pinocchio virtual FrameIndex getBodyId ( const std::string& frame_name) const = 0; + virtual JointIndex getJointId ( + const std::string& joint_name) const = 0; + + virtual const std::string& getJointName ( + const JointIndex jointId) const = 0; + + virtual Frame getBodyFrame (const std::string& frame_name) const = 0; + + + virtual JointIndex getParentId ( + const std::string& frame_name) const = 0; + + virtual bool existFrame ( + const std::string& frame_name, const FrameType type) const = 0; + UrdfVisitorBaseTpl () : log (NULL) {} template @@ -133,6 +169,29 @@ namespace pinocchio const VectorConstRef& max_config, const VectorConstRef& friction, const VectorConstRef& damping) + { + addJointAndBody(type,axis,parentFrameId,placement, + joint_name, Y, SE3::Identity(), body_name, + max_effort, max_velocity, min_config, max_config, + friction, damping); + } + + + void addJointAndBody( + JointType type, + const Vector3& axis, + const FrameIndex & parentFrameId, + const SE3 & placement, + const std::string & joint_name, + const Inertia& Y, + const SE3& frame_placement, + const std::string & body_name, + const VectorConstRef& max_effort, + const VectorConstRef& max_velocity, + const VectorConstRef& min_config, + const VectorConstRef& max_config, + const VectorConstRef& friction, + const VectorConstRef& damping) { JointIndex joint_id; const Frame & frame = model.frames[parentFrameId]; @@ -186,12 +245,21 @@ namespace pinocchio friction, damping ); break; + case Base::SPHERICAL: + joint_id = model.addJoint(frame.parent, + typename JointCollection::JointModelSpherical(), + frame.placement * placement, + joint_name, + max_effort,max_velocity,min_config,max_config, + friction, damping + ); + break; default: PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); }; FrameIndex jointFrameId = model.addJointFrame(joint_id, (int)parentFrameId); - appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name); + appendBodyToJoint(jointFrameId, Y, frame_placement, body_name); } void addFixedJointAndBody( @@ -201,14 +269,13 @@ namespace pinocchio const Inertia & Y, const std::string & body_name) { - const Frame & parent_frame = model.frames[parent_frame_id]; - const JointIndex parent_frame_parent = parent_frame.parent; + const Frame & frame = model.frames[parent_frame_id]; - const SE3 placement = parent_frame.placement * joint_placement; - FrameIndex fid = model.addFrame(Frame(joint_name, parent_frame.parent, parent_frame_id, - placement, FIXED_JOINT, Y)); + FrameIndex fid = model.addFrame(Frame(joint_name, frame.parent, parent_frame_id, + frame.placement * joint_placement, FIXED_JOINT) + ); - model.addBodyFrame(body_name, parent_frame_parent, placement, (int)fid); + appendBodyToJoint((FrameIndex)fid, Y, SE3::Identity(), body_name); } void appendBodyToJoint( @@ -237,6 +304,7 @@ namespace pinocchio FrameIndex getBodyId (const std::string& frame_name) const { + if (model.existFrame(frame_name, BODY)) { FrameIndex fid = model.getFrameId (frame_name, BODY); assert(model.frames[fid].type == BODY); @@ -246,31 +314,54 @@ namespace pinocchio + frame_name); } - private: - /// - /// \brief The four possible cartesian types of an 3D axis. - /// - enum CartesianAxis { AXIS_X=0, AXIS_Y=1, AXIS_Z=2, AXIS_UNALIGNED }; + FrameIndex getJointId (const std::string& joint_name) const + { - /// - /// \brief Extract the cartesian property of a particular 3D axis. - /// - /// \param[in] axis The input URDF axis. - /// - /// \return The property of the particular axis pinocchio::urdf::CartesianAxis. - /// - static inline CartesianAxis extractCartesianAxis (const Vector3 & axis) + if (model.existJointName(joint_name)) { + JointIndex jid = model.getJointId (joint_name); + return jid; + } else + throw std::invalid_argument("Model does not have any joint named " + + joint_name); + } + + const std::string& getJointName (const JointIndex jointId) const { - if( axis == Vector3(1., 0., 0.)) - return AXIS_X; - else if( axis == Vector3(0., 1., 0.)) - return AXIS_Y; - else if( axis == Vector3(0., 0., 1.)) - return AXIS_Z; - else - return AXIS_UNALIGNED; + return model.names[jointId]; } + + Frame getBodyFrame (const std::string& frame_name) const + { + if (model.existFrame(frame_name, BODY)) { + FrameIndex fid = model.getFrameId (frame_name, BODY); + assert(model.frames[fid].type == BODY); + return model.frames[fid]; + } else + throw std::invalid_argument("Model does not have any body named " + + frame_name); + } + + + + JointIndex getParentId (const std::string& frame_name) const + { + + if (model.existFrame(frame_name, BODY)) { + FrameIndex fid = model.getFrameId (frame_name, BODY); + assert(model.frames[fid].type == BODY); + return model.frames[fid].parent; + } else + throw std::invalid_argument("Model does not have any body named " + + frame_name); + } + + bool existFrame ( + const std::string& frame_name, const FrameType type) const + { + return model.existFrame(frame_name, BODY); + } + template JointIndex addJoint( @@ -320,6 +411,32 @@ namespace pinocchio break; } } + + private: + /// + /// \brief The four possible cartesian types of an 3D axis. + /// + enum CartesianAxis { AXIS_X=0, AXIS_Y=1, AXIS_Z=2, AXIS_UNALIGNED }; + + /// + /// \brief Extract the cartesian property of a particular 3D axis. + /// + /// \param[in] axis The input URDF axis. + /// + /// \return The property of the particular axis pinocchio::urdf::CartesianAxis. + /// + static inline CartesianAxis extractCartesianAxis (const Vector3 & axis) + { + if( axis == Vector3(1., 0., 0.)) + return AXIS_X; + else if( axis == Vector3(0., 1., 0.)) + return AXIS_Y; + else if( axis == Vector3(0., 0., 1.)) + return AXIS_Z; + else + return AXIS_UNALIGNED; + } + }; template class JointCollectionTpl> @@ -361,13 +478,13 @@ namespace pinocchio typedef UrdfVisitorBaseTpl UrdfVisitorBase; void PINOCCHIO_DLLAPI parseRootTree(const ::urdf::ModelInterface * urdfTree, - UrdfVisitorBase & model); + UrdfVisitorBase & model); void PINOCCHIO_DLLAPI parseRootTree(const std::string & filename, - UrdfVisitorBase & model); + UrdfVisitorBase & model); void PINOCCHIO_DLLAPI parseRootTreeFromXML(const std::string & xmlString, - UrdfVisitorBase & model); + UrdfVisitorBase & model); } template class JointCollectionTpl> diff --git a/src/parsers/utils.hpp b/src/parsers/utils.hpp index c88f32ff5b..a1c79269d3 100644 --- a/src/parsers/utils.hpp +++ b/src/parsers/utils.hpp @@ -5,7 +5,6 @@ #ifndef __pinocchio_parsers_utils_hpp__ #define __pinocchio_parsers_utils_hpp__ -#include #include #include // #include @@ -73,7 +72,7 @@ namespace pinocchio std::string scheme = string.substr(0, pos_separator); std::string path = string.substr(pos_separator+3, std::string::npos); - if(scheme == "package") + if(scheme == "package" || scheme == "model") { // if exists p1/string, path = p1/string, // else if exists p2/string, path = p2/string diff --git a/src/serialization/data.hpp b/src/serialization/data.hpp index 84e3229bfd..81ea593f2e 100644 --- a/src/serialization/data.hpp +++ b/src/serialization/data.hpp @@ -8,9 +8,9 @@ #include #include -#include "pinocchio/serialization/fwd.hpp" #include "pinocchio/serialization/aligned-vector.hpp" #include "pinocchio/serialization/spatial.hpp" +#include "pinocchio/serialization/eigen.hpp" #include "pinocchio/serialization/joints.hpp" #include "pinocchio/serialization/frame.hpp" @@ -29,12 +29,15 @@ namespace boost PINOCCHIO_MAKE_DATA_NVP(ar,data,joints); PINOCCHIO_MAKE_DATA_NVP(ar,data,a); PINOCCHIO_MAKE_DATA_NVP(ar,data,oa); + PINOCCHIO_MAKE_DATA_NVP(ar,data,oa_drift); + PINOCCHIO_MAKE_DATA_NVP(ar,data,oa_augmented); PINOCCHIO_MAKE_DATA_NVP(ar,data,a_gf); PINOCCHIO_MAKE_DATA_NVP(ar,data,oa_gf); PINOCCHIO_MAKE_DATA_NVP(ar,data,v); PINOCCHIO_MAKE_DATA_NVP(ar,data,ov); PINOCCHIO_MAKE_DATA_NVP(ar,data,f); PINOCCHIO_MAKE_DATA_NVP(ar,data,of); + PINOCCHIO_MAKE_DATA_NVP(ar,data,of_augmented); PINOCCHIO_MAKE_DATA_NVP(ar,data,h); PINOCCHIO_MAKE_DATA_NVP(ar,data,oh); PINOCCHIO_MAKE_DATA_NVP(ar,data,oMi); @@ -62,6 +65,8 @@ namespace boost PINOCCHIO_MAKE_DATA_NVP(ar,data,doYcrb); PINOCCHIO_MAKE_DATA_NVP(ar,data,ddq); PINOCCHIO_MAKE_DATA_NVP(ar,data,Yaba); + PINOCCHIO_MAKE_DATA_NVP(ar,data,oYaba); + PINOCCHIO_MAKE_DATA_NVP(ar,data,oYaba_contact); PINOCCHIO_MAKE_DATA_NVP(ar,data,u); PINOCCHIO_MAKE_DATA_NVP(ar,data,Ag); PINOCCHIO_MAKE_DATA_NVP(ar,data,dAg); @@ -88,6 +93,14 @@ namespace boost PINOCCHIO_MAKE_DATA_NVP(ar,data,dtau_dv); PINOCCHIO_MAKE_DATA_NVP(ar,data,ddq_dq); PINOCCHIO_MAKE_DATA_NVP(ar,data,ddq_dv); + PINOCCHIO_MAKE_DATA_NVP(ar,data,dvc_dq); + PINOCCHIO_MAKE_DATA_NVP(ar,data,dac_dq); + PINOCCHIO_MAKE_DATA_NVP(ar,data,dac_dv); + PINOCCHIO_MAKE_DATA_NVP(ar,data,dac_da); + PINOCCHIO_MAKE_DATA_NVP(ar,data,osim); + PINOCCHIO_MAKE_DATA_NVP(ar,data,dlambda_dq); + PINOCCHIO_MAKE_DATA_NVP(ar,data,dlambda_dv); + PINOCCHIO_MAKE_DATA_NVP(ar,data,dlambda_dtau); PINOCCHIO_MAKE_DATA_NVP(ar,data,iMf); PINOCCHIO_MAKE_DATA_NVP(ar,data,com); PINOCCHIO_MAKE_DATA_NVP(ar,data,vcom); @@ -96,15 +109,21 @@ namespace boost PINOCCHIO_MAKE_DATA_NVP(ar,data,Jcom); PINOCCHIO_MAKE_DATA_NVP(ar,data,kinetic_energy); PINOCCHIO_MAKE_DATA_NVP(ar,data,potential_energy); + PINOCCHIO_MAKE_DATA_NVP(ar,data,mechanical_energy); PINOCCHIO_MAKE_DATA_NVP(ar,data,JMinvJt); PINOCCHIO_MAKE_DATA_NVP(ar,data,lambda_c); PINOCCHIO_MAKE_DATA_NVP(ar,data,torque_residual); PINOCCHIO_MAKE_DATA_NVP(ar,data,dq_after); PINOCCHIO_MAKE_DATA_NVP(ar,data,impulse_c); + PINOCCHIO_MAKE_DATA_NVP(ar,data,lambda_c_prox); + PINOCCHIO_MAKE_DATA_NVP(ar,data,diff_lambda_c); + PINOCCHIO_MAKE_DATA_NVP(ar,data,sDUiJt); PINOCCHIO_MAKE_DATA_NVP(ar,data,staticRegressor); PINOCCHIO_MAKE_DATA_NVP(ar,data,bodyRegressor); PINOCCHIO_MAKE_DATA_NVP(ar,data,jointTorqueRegressor); + PINOCCHIO_MAKE_DATA_NVP(ar,data,jointTorqueRegressor); PINOCCHIO_MAKE_DATA_NVP(ar,data,kinematic_hessians); + PINOCCHIO_MAKE_DATA_NVP(ar,data,primal_dual_contact_solution); } } // namespace serialization diff --git a/src/serialization/joints-data.hpp b/src/serialization/joints-data.hpp index 7dc6ac199a..c537b91fa9 100644 --- a/src/serialization/joints-data.hpp +++ b/src/serialization/joints-data.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2019 INRIA +// Copyright (c) 2019-2020 INRIA // #ifndef __pinocchio_serialization_joints_data_hpp__ @@ -47,6 +47,9 @@ namespace boost pinocchio::JointDataBase & joint_data, const unsigned int /*version*/) { + ar & make_nvp("joint_q",joint_data.joint_q()); + ar & make_nvp("joint_v",joint_data.joint_v()); + ar & make_nvp("S",joint_data.S()); ar & make_nvp("M",joint_data.M()); ar & make_nvp("v",joint_data.v()); @@ -55,6 +58,7 @@ namespace boost ar & make_nvp("U",joint_data.U()); ar & make_nvp("Dinv",joint_data.Dinv()); ar & make_nvp("UDinv",joint_data.UDinv()); + ar & make_nvp("StU",joint_data.StU()); } } diff --git a/src/serialization/joints-motion-subspace.hpp b/src/serialization/joints-motion-subspace.hpp new file mode 100644 index 0000000000..3932e09bfb --- /dev/null +++ b/src/serialization/joints-motion-subspace.hpp @@ -0,0 +1,98 @@ +// +// Copyright (c) 2019-2020 INRIA +// + +#ifndef __pinocchio_serialization_joints_motion_subspace_hpp__ +#define __pinocchio_serialization_joints_motion_subspace_hpp__ + +#include "pinocchio/serialization/fwd.hpp" + +#include +#include + +namespace boost +{ + namespace serialization + { + + template + void serialize(Archive & /*ar*/, + pinocchio::JointMotionSubspaceRevoluteTpl & /*S*/, + const unsigned int /*version*/) + {} + + template + void serialize(Archive & /*ar*/, + pinocchio::JointMotionSubspacePrismaticTpl & /*S*/, + const unsigned int /*version*/) + {} + + template + void serialize(Archive & /*ar*/, + pinocchio::JointMotionSubspaceSphericalTpl & /*S*/, + const unsigned int /*version*/) + {} + + template + void serialize(Archive & /*ar*/, + pinocchio::JointMotionSubspaceTranslationTpl & /*S*/, + const unsigned int /*version*/) + {} + + template + void serialize(Archive & /*ar*/, + pinocchio::JointMotionSubspaceIdentityTpl & /*S*/, + const unsigned int /*version*/) + {} + + template + void serialize(Archive & ar, + pinocchio::JointMotionSubspaceRevoluteUnalignedTpl & S, + const unsigned int /*version*/) + { + ar & make_nvp("axis",S.axis()); + } + + template + void serialize(Archive & ar, + pinocchio::JointMotionSubspacePrismaticUnalignedTpl & S, + const unsigned int /*version*/) + { + ar & make_nvp("axis",S.axis()); + } + + template + void serialize(Archive & ar, + pinocchio::JointMotionSubspaceTpl & S, + const unsigned int /*version*/) + { + ar & make_nvp("matrix",S.matrix()); + } + + template + void serialize(Archive & ar, + pinocchio::ScaledJointMotionSubspace & S, + const unsigned int /*version*/) + { + ar & make_nvp("scaling",S.scaling()); + ar & make_nvp("constraint",S.constraint()); + } + + template + void serialize(Archive & /*ar*/, + pinocchio::JointMotionSubspacePlanarTpl & /*S*/, + const unsigned int /*version*/) + {} + + template + void serialize(Archive & ar, + pinocchio::JointMotionSubspaceSphericalZYXTpl & S, + const unsigned int /*version*/) + { + ar & make_nvp("angularSubspace",S.angularSubspace()); + } + + } +} + +#endif // ifndef __pinocchio_serialization_joints_motion_subspace_hpp__ diff --git a/src/serialization/joints.hpp b/src/serialization/joints.hpp index b6687bdfaf..3c460ee4fe 100644 --- a/src/serialization/joints.hpp +++ b/src/serialization/joints.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2019 INRIA +// Copyright (c) 2019-2020 INRIA // #ifndef __pinocchio_serialization_joints_hpp__ @@ -11,6 +11,7 @@ #include "pinocchio/multibody/joint/joint-collection.hpp" #include "pinocchio/serialization/fwd.hpp" +#include "pinocchio/serialization/eigen.hpp" #include "pinocchio/serialization/vector.hpp" #include "pinocchio/serialization/aligned-vector.hpp" @@ -19,7 +20,7 @@ #include "pinocchio/serialization/joints-transform.hpp" #include "pinocchio/serialization/joints-motion.hpp" -#include "pinocchio/serialization/joints-constraint.hpp" +#include "pinocchio/serialization/joints-motion-subspace.hpp" #include "pinocchio/serialization/joints-model.hpp" #include "pinocchio/serialization/joints-data.hpp" diff --git a/src/serialization/model.hpp b/src/serialization/model.hpp index 45f1841d40..2b164317c0 100644 --- a/src/serialization/model.hpp +++ b/src/serialization/model.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2019 INRIA +// Copyright (c) 2019-2020 INRIA // -#ifndef __pinocchio_multibody_model_serialization_hpp__ -#define __pinocchio_multibody_model_serialization_hpp__ +#ifndef __pinocchio_serialization_model_hpp__ +#define __pinocchio_serialization_model_hpp__ #include #include @@ -13,6 +13,7 @@ #include "pinocchio/serialization/fwd.hpp" #include "pinocchio/serialization/aligned-vector.hpp" #include "pinocchio/serialization/spatial.hpp" +#include "pinocchio/serialization/eigen.hpp" #include "pinocchio/serialization/joints.hpp" #include "pinocchio/serialization/frame.hpp" @@ -35,6 +36,7 @@ namespace boost ar & make_nvp("nbodies",model.nbodies); ar & make_nvp("nframes",model.nframes); ar & make_nvp("parents",model.parents); + ar & make_nvp("children",model.children); ar & make_nvp("names",model.names); ar & make_nvp("supports",model.supports); ar & make_nvp("subtrees",model.subtrees); @@ -42,6 +44,7 @@ namespace boost ar & make_nvp("name",model.name); ar & make_nvp("referenceConfigurations",model.referenceConfigurations); + ar & make_nvp("armature",model.armature); ar & make_nvp("rotorInertia",model.rotorInertia); ar & make_nvp("rotorGearRatio",model.rotorGearRatio); ar & make_nvp("friction",model.friction); @@ -61,4 +64,4 @@ namespace boost } // namespace serialization } // namespace boost -#endif // ifndef __pinocchio_multibody_model_serialization_hpp__ +#endif // ifndef __pinocchio_serialization_model_hpp__ diff --git a/src/spatial/cartesian-axis.hpp b/src/spatial/cartesian-axis.hpp index f023f4a3e4..97900dd734 100644 --- a/src/spatial/cartesian-axis.hpp +++ b/src/spatial/cartesian-axis.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2017-2018 CNRS +// Copyright (c) 2017-2020 CNRS INRIA // #ifndef __pinocchio_cartesian_axis_hpp__ @@ -14,6 +14,8 @@ namespace pinocchio struct CartesianAxis { enum { axis = _axis, dim = 3 }; + + typedef Eigen::Matrix Vector3; template inline static void cross(const Eigen::MatrixBase & vin, @@ -69,6 +71,19 @@ namespace pinocchio v3_[i] = i == axis ? Scalar(1) : Scalar(0); } + template + static const Eigen::Matrix & vector() + { + typedef Eigen::Matrix Vector3; + static const Vector3 vec = Vector3::Unit(axis); + return vec; + } + + static const Vector3 & vector() + { + return vector(); + } + }; // struct CartesianAxis template<> @@ -140,9 +155,14 @@ namespace pinocchio vout_[0] = -s*vin[1]; vout_[1] = s*vin[0]; vout_[2] = 0.; } - typedef CartesianAxis<0> AxisX; - typedef CartesianAxis<1> AxisY; - typedef CartesianAxis<2> AxisZ; + typedef CartesianAxis<0> XAxis; + typedef XAxis AxisX; + + typedef CartesianAxis<1> YAxis; + typedef YAxis AxisY; + + typedef CartesianAxis<2> ZAxis; + typedef ZAxis AxisZ; } diff --git a/src/spatial/classic-acceleration.hpp b/src/spatial/classic-acceleration.hpp new file mode 100644 index 0000000000..9798fd9829 --- /dev/null +++ b/src/spatial/classic-acceleration.hpp @@ -0,0 +1,120 @@ +// +// Copyright (c) 2019 INRIA +// + +#ifndef __pinocchio_spatial_classic_acceleration_hpp__ +#define __pinocchio_spatial_classic_acceleration_hpp__ + +#include "pinocchio/spatial/se3.hpp" +#include "pinocchio/spatial/motion.hpp" + +namespace pinocchio +{ + /// + /// \brief Computes the classic acceleration from a given spatial velocity and spatial acceleration. + /// + /// \tparam Motion1 type of the input spatial velocity. + /// \tparam Motion2 type of the input spatial acceleration. + /// \tparam Vector3Like type of the return type (a type similar to a 3D vector). + /// + /// \param[in] spatial_velocity input spatial velocity. + /// \param[in] spatial_acceleration input spatial acceleration. + /// \param[out] res computed classic acceleration. + /// + /// \remarks To be valid, the spatial velocity and the spatial acceleration have to be expressed at the same Frame. + /// + template + inline void classicAcceleration(const MotionDense & spatial_velocity, + const MotionDense & spatial_acceleration, + const Eigen::MatrixBase & res) + { + PINOCCHIO_EIGEN_CONST_CAST(Vector3Like,res).noalias() + = spatial_acceleration.linear() + spatial_velocity.angular().cross(spatial_velocity.linear()); + } + + /// + /// \brief Computes the classic acceleration from a given spatial velocity and spatial acceleration. + /// + /// \tparam Motion1 type of the input spatial velocity. + /// \tparam Motion2 type of the input spatial acceleration. + /// + /// \param[in] spatial_velocity input spatial velocity. + /// \param[in] spatial_acceleration input spatial acceleration. + /// + /// \remarks To be valid, the spatial velocity and the spatial acceleration have to be expressed at the same Frame. + /// + template + inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) + classicAcceleration(const MotionDense & spatial_velocity, + const MotionDense & spatial_acceleration) + { + typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) ReturnType; + ReturnType res; + classicAcceleration(spatial_velocity,spatial_acceleration,res); + return res; + } + + /// + /// \brief Computes the classic acceleration of a given frame B + /// knowing the spatial velocity and spatial acceleration of a frame A + /// and the relative placement between these two frames. + /// + /// \tparam Motion1 type of the input spatial velocity. + /// \tparam Motion2 type of the input spatial acceleration. + /// \tparam SE3Scalar Scalar type of the SE3 object. + /// \tparam SE3Options Options of the SE3 object. + /// \tparam Vector3Like type of the return type (a type similar to a 3D vector). + /// + /// \param[in] spatial_velocity input spatial velocity. + /// \param[in] spatial_acceleration input spatial acceleration. + /// \param[in] placement relative placement betwen the frame A and the frame B. + /// \param[out] res computed classic acceleration. + /// + template + inline void classicAcceleration(const MotionDense & spatial_velocity, + const MotionDense & spatial_acceleration, + const SE3Tpl & placement, + const Eigen::MatrixBase & res) + { + typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion1::LinearType) Vector3; + + const Vector3 linear_velocity_frame_B + = spatial_velocity.linear() + + spatial_velocity.angular().cross(placement.translation()); + + const Vector3 linear_acceleration_frame_B // expressed in the coordinate frame A + = spatial_acceleration.linear() + + spatial_velocity.angular().cross(linear_velocity_frame_B); + + PINOCCHIO_EIGEN_CONST_CAST(Vector3Like,res).noalias() + = placement.rotation().transpose() * (linear_acceleration_frame_B - placement.translation().cross(spatial_acceleration.angular())); + } + + /// + /// \brief Computes the classic acceleration of a given frame B + /// knowing the spatial velocity and spatial acceleration of a frame A + /// and the relative placement between these two frames. + /// + /// \tparam Motion1 type of the input spatial velocity. + /// \tparam Motion2 type of the input spatial acceleration. + /// \tparam SE3Scalar Scalar type of the SE3 object. + /// \tparam SE3Options Options of the SE3 object. + /// + /// \param[in] spatial_velocity input spatial velocity. + /// \param[in] spatial_acceleration input spatial acceleration. + /// \param[in] placement relative placement betwen the frame A and the frame B. + /// + template + inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) + classicAcceleration(const MotionDense & spatial_velocity, + const MotionDense & spatial_acceleration, + const SE3Tpl & placement) + { + typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename Motion2::Vector3) ReturnType; + ReturnType res; + classicAcceleration(spatial_velocity,spatial_acceleration,placement,res); + return res; + } +} + +#endif // ifndef __pinocchio_spatial_classic_acceleration_hpp__ diff --git a/src/spatial/explog.hpp b/src/spatial/explog.hpp index d0367c4b3e..a63f3cf1e3 100644 --- a/src/spatial/explog.hpp +++ b/src/spatial/explog.hpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2020 CNRS INRIA +// Copyright (c) 2015-2021 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // @@ -38,8 +38,9 @@ namespace pinocchio typedef typename Vector3Like::Scalar Scalar; typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain; typedef Eigen::Matrix Matrix3; + const static Scalar eps = Eigen::NumTraits::epsilon(); - const Scalar t2 = v.squaredNorm(); + const Scalar t2 = v.squaredNorm() + eps * eps; const Scalar t = math::sqrt(t2); Scalar ct,st; SINCOS(t,&st,&ct); @@ -104,7 +105,7 @@ namespace pinocchio /// \f[ /// \frac{\sin{||r||}}{||r||} I_3 /// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x - /// + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T + /// + \frac{1}{||r||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T /// \f] /// template @@ -134,7 +135,7 @@ namespace pinocchio n2_inv * (1 - a)); switch(op) - { + { case SETTO: Jout.diagonal().setConstant(a); Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1); @@ -159,7 +160,7 @@ namespace pinocchio default: assert(false && "Wrong Op requesed value"); break; - } + } } /// @@ -167,7 +168,7 @@ namespace pinocchio /// \f[ /// \frac{\sin{||r||}}{||r||} I_3 /// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x - /// + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T + /// + \frac{1}{||r||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T /// \f] /// template @@ -306,9 +307,10 @@ namespace pinocchio const typename MotionDerived::ConstAngularType & w = nu.angular(); const typename MotionDerived::ConstLinearType & v = nu.linear(); + const static Scalar eps = Eigen::NumTraits::epsilon(); Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term; - const Scalar t2 = w.squaredNorm(); + const Scalar t2 = w.squaredNorm() + eps*eps; const Scalar t = math::sqrt(t2); Scalar ct,st; SINCOS(t,&st,&ct); const Scalar inv_t2 = Scalar(1)/t2; @@ -436,7 +438,7 @@ namespace pinocchio -Scalar(2)*t2inv*t2inv + (Scalar(1) + st*tinv) * t2inv * inv_2_2ct); switch(op) - { + { case SETTO: { Jexp3(w, Jout.template bottomRightCorner<3,3>()); @@ -502,6 +504,21 @@ namespace pinocchio Jexp6(nu, Jexp); } + /// \brief Derivative of exp6 + /// Computed as the inverse of Jlog6 + template + Eigen::Matrix + Jexp6(const MotionDense & nu) + { + typedef typename MotionDerived::Scalar Scalar; + enum { Options = MotionDerived::Options }; + typedef Eigen::Matrix ReturnType; + + ReturnType res; + Jexp6(nu,res); + return res; + } + /** \brief Derivative of log6 * \f[ * \left(\begin{array}{cc} @@ -540,6 +557,21 @@ namespace pinocchio { Jlog6_impl::run(M,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog)); } + + /// + /// \copydoc Jlog6(const SE3Tpl &, const Eigen::MatrixBase &) + /// + /// \param[in] M The rigid transformation. + /// + template + Eigen::Matrix Jlog6(const SE3Tpl & M) + { + typedef Eigen::Matrix ReturnType; + + ReturnType res; + Jlog6(M,res); + return res; + } template template diff --git a/src/spatial/force-base.hpp b/src/spatial/force-base.hpp index 7d558cdaeb..1cc75846d0 100644 --- a/src/spatial/force-base.hpp +++ b/src/spatial/force-base.hpp @@ -1,10 +1,10 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_force_base_hpp__ -#define __pinocchio_force_base_hpp__ +#ifndef __pinocchio_spatial_force_base_hpp__ +#define __pinocchio_spatial_force_base_hpp__ namespace pinocchio { @@ -19,7 +19,7 @@ namespace pinocchio * @tparam Derived { description } */ template< class Derived> - class ForceBase + class ForceBase : NumericalBase { public: FORCE_TYPEDEF_TPL(Derived); @@ -27,6 +27,8 @@ namespace pinocchio Derived & derived() { return *static_cast(this); } const Derived& derived() const { return *static_cast(this); } + Derived & const_cast_derived() const { return *const_cast(&derived()); } + /** * @brief Return the angular part of the force vector * @@ -47,7 +49,6 @@ namespace pinocchio /// \copydoc ForceBase::linear LinearType linear() { return derived().linear_impl(); } - /** * @brief Set the angular part of the force vector * @@ -207,4 +208,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_force_base_hpp__ +#endif // ifndef __pinocchio_spatial_force_base_hpp__ diff --git a/src/spatial/force-dense.hpp b/src/spatial/force-dense.hpp index 9d55b0698e..5f469d4e7a 100644 --- a/src/spatial/force-dense.hpp +++ b/src/spatial/force-dense.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2017-2019 CNRS INRIA +// Copyright (c) 2017-2020 CNRS INRIA // -#ifndef __pinocchio_force_dense_hpp__ -#define __pinocchio_force_dense_hpp__ +#ifndef __pinocchio_spatial_force_dense_hpp__ +#define __pinocchio_spatial_force_dense_hpp__ namespace pinocchio { @@ -48,17 +48,22 @@ namespace pinocchio // Arithmetic operators template - Derived & setFrom(const ForceDense & other) + Derived & operator=(const ForceDense & other) { - linear() = other.linear(); - angular() = other.angular(); - return derived(); + return derived().set(other.derived()); + } + + Derived & operator=(const ForceDense & other) + { + return derived().set(other.derived()); } template - Derived & operator=(const ForceDense & other) + Derived & set(const ForceDense & other) { - return derived().setFrom(other.derived()); + linear() = other.linear(); + angular() = other.angular(); + return derived(); } template @@ -185,6 +190,10 @@ namespace pinocchio /// \returns a ForceRef on this. ForceRefType ref() { return derived().ref(); } + + protected: + + ForceDense() {}; }; // class ForceDense @@ -196,4 +205,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_force_dense_hpp__ +#endif // ifndef __pinocchio_spatial_force_dense_hpp__ diff --git a/src/spatial/force-ref.hpp b/src/spatial/force-ref.hpp index 7cf19bedb8..16144b8a74 100644 --- a/src/spatial/force-ref.hpp +++ b/src/spatial/force-ref.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2017-2019 CNRS INRIA // -#ifndef __pinocchio_force_ref_hpp__ -#define __pinocchio_force_ref_hpp__ +#ifndef __pinocchio_spatial_force_ref_hpp__ +#define __pinocchio_spatial_force_ref_hpp__ namespace pinocchio { @@ -163,4 +163,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_force_ref_hpp__ +#endif // ifndef __pinocchio_spatial_force_ref_hpp__ diff --git a/src/spatial/force-tpl.hpp b/src/spatial/force-tpl.hpp index c29686c7d9..264ee367db 100644 --- a/src/spatial/force-tpl.hpp +++ b/src/spatial/force-tpl.hpp @@ -3,8 +3,8 @@ // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_force_tpl_hpp__ -#define __pinocchio_force_tpl_hpp__ +#ifndef __pinocchio_spatial_force_tpl_hpp__ +#define __pinocchio_spatial_force_tpl_hpp__ namespace pinocchio { @@ -51,8 +51,8 @@ namespace pinocchio template ForceTpl(const Eigen::MatrixBase & v, const Eigen::MatrixBase & w) { - assert(v.size() == 3); - assert(w.size() == 3); + EIGEN_STATIC_ASSERT_VECTOR_ONLY(V1); + EIGEN_STATIC_ASSERT_VECTOR_ONLY(V2); linear() = v; angular() = w; } @@ -67,6 +67,16 @@ namespace pinocchio ForceTpl(const ForceTpl & clone) // Copy constructor : m_data(clone.toVector()) {} + + template + explicit ForceTpl(const ForceTpl & other) + { + *this = other.template cast(); + } + + template + explicit ForceTpl(const ForceBase & clone) + { *this = clone; } ForceTpl& operator=(const ForceTpl & clone) // Copy assignment operator { @@ -128,4 +138,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_force_tpl_hpp__ +#endif // ifndef __pinocchio_spatial_force_tpl_hpp__ diff --git a/src/spatial/force.hpp b/src/spatial/force.hpp index a8d7bfbdef..4f0894f7da 100644 --- a/src/spatial/force.hpp +++ b/src/spatial/force.hpp @@ -3,8 +3,8 @@ // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_force_hpp__ -#define __pinocchio_force_hpp__ +#ifndef __pinocchio_spatial_force_hpp__ +#define __pinocchio_spatial_force_hpp__ #include "pinocchio/spatial/fwd.hpp" #include "pinocchio/spatial/se3.hpp" @@ -38,5 +38,5 @@ FORCE_TYPEDEF_GENERIC(Derived,PINOCCHIO_MACRO_EMPTY_ARG) #include "pinocchio/spatial/force-tpl.hpp" #include "pinocchio/spatial/force-ref.hpp" -#endif // ifndef __pinocchio_force_hpp__ +#endif // ifndef __pinocchio_spatial_force_hpp__ diff --git a/src/spatial/fwd.hpp b/src/spatial/fwd.hpp index 171bef0bb0..892285c3e0 100644 --- a/src/spatial/fwd.hpp +++ b/src/spatial/fwd.hpp @@ -34,7 +34,7 @@ namespace pinocchio * \addtogroup pinocchio_spatial * @{ */ - + template struct SE3Tpl; template class MotionBase; @@ -49,7 +49,7 @@ namespace pinocchio template class ForceRef; template class ForceTpl; - template class InertiaTpl; + template struct InertiaTpl; template class Symmetric3Tpl; diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp index cf7e766e15..549be4ed98 100644 --- a/src/spatial/inertia.hpp +++ b/src/spatial/inertia.hpp @@ -17,42 +17,43 @@ namespace pinocchio { - template< class Derived> - class InertiaBase + template + struct InertiaBase : NumericalBase { - protected: - - typedef Derived Derived_t; - SPATIAL_TYPEDEF_TEMPLATE(Derived_t); + SPATIAL_TYPEDEF_TEMPLATE(Derived); - public: - Derived_t & derived() { return *static_cast(this); } - const Derived_t & derived() const { return *static_cast(this); } + Derived & derived() { return *static_cast(this); } + const Derived & derived() const { return *static_cast(this); } + + Derived & const_cast_derived() const { return *const_cast(&derived()); } - Scalar mass() const { return static_cast(this)->mass(); } - Scalar & mass() { return static_cast(this)->mass(); } - const Vector3 & lever() const { return static_cast(this)->lever(); } - Vector3 & lever() { return static_cast(this)->lever(); } - const Symmetric3 & inertia() const { return static_cast(this)->inertia(); } - Symmetric3 & inertia() { return static_cast(this)->inertia(); } + Scalar mass() const { return static_cast(this)->mass(); } + Scalar & mass() { return static_cast(this)->mass(); } + const Vector3 & lever() const { return static_cast(this)->lever(); } + Vector3 & lever() { return static_cast(this)->lever(); } + const Symmetric3 & inertia() const { return static_cast(this)->inertia(); } + Symmetric3 & inertia() { return static_cast(this)->inertia(); } Matrix6 matrix() const { return derived().matrix_impl(); } operator Matrix6 () const { return matrix(); } - Derived_t& operator= (const Derived_t& clone){return derived().__equl__(clone);} - bool operator==(const Derived_t & other) const {return derived().isEqual(other);} - bool operator!=(const Derived_t & other) const { return !(*this == other); } + Derived& operator= (const Derived& clone){return derived().__equl__(clone);} + bool operator==(const Derived & other) const {return derived().isEqual(other);} + bool operator!=(const Derived & other) const { return !(*this == other); } - Derived_t& operator+= (const Derived_t & Yb) { return derived().__pequ__(Yb); } - Derived_t operator+(const Derived_t & Yb) const { return derived().__plus__(Yb); } + Derived& operator+= (const Derived & Yb) { return derived().__pequ__(Yb); } + Derived operator+(const Derived & Yb) const { return derived().__plus__(Yb); } template ForceTpl::Scalar,traits::Options> operator*(const MotionDense & v) const { return derived().__mult__(v); } - Scalar vtiv(const Motion & v) const { return derived().vtiv_impl(v); } - Matrix6 variation(const Motion & v) const { return derived().variation_impl(v); } + template + Scalar vtiv(const MotionDense & v) const { return derived().vtiv_impl(v); } + + template + Matrix6 variation(const MotionDense & v) const { return derived().variation_impl(v); } /// \brief Time variation operator. /// It computes the time derivative of an inertia I corresponding to the formula \f$ \dot{I} = v \times^{*} I \f$. @@ -61,14 +62,15 @@ namespace pinocchio /// \param[in] I The spatial inertia in motion. /// \param[out] Iout The time derivative of the inertia I. /// - template - static void vxi(const Motion & v, const Derived & I, const Eigen::MatrixBase & Iout) + template + static void vxi(const MotionDense & v, const Derived & I, const Eigen::MatrixBase & Iout) { EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); Derived::vxi_impl(v,I,Iout); } - Matrix6 vxi(const Motion & v) const + template + Matrix6 vxi(const MotionDense & v) const { Matrix6 Iout; vxi(v,derived(),Iout); @@ -82,14 +84,15 @@ namespace pinocchio /// \param[in] I The spatial inertia in motion. /// \param[out] Iout The time derivative of the inertia I. /// - template - static void ivx(const Motion & v, const Derived & I, const Eigen::MatrixBase & Iout) + template + static void ivx(const MotionDense & v, const Derived & I, const Eigen::MatrixBase & Iout) { EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); Derived::ivx_impl(v,I,Iout); } - Matrix6 ivx(const Motion & v) const + template + Matrix6 ivx(const MotionDense & v) const { Matrix6 Iout; ivx(v,derived(),Iout); @@ -107,13 +110,17 @@ namespace pinocchio { return derived().isZero_impl(prec); } /// aI = aXb.act(bI) - Derived_t se3Action(const SE3 & M) const { return derived().se3Action_impl(M); } + template + Derived se3Action(const SE3Tpl & M) const + { return derived().se3Action_impl(M); } /// bI = aXb.actInv(aI) - Derived_t se3ActionInverse(const SE3 & M) const { return derived().se3ActionInverse_impl(M); } + template + Derived se3ActionInverse(const SE3Tpl & M) const + { return derived().se3ActionInverse_impl(M); } - void disp(std::ostream & os) const { static_cast(this)->disp_impl(os); } - friend std::ostream & operator << (std::ostream & os,const InertiaBase & X) + void disp(std::ostream & os) const { static_cast(this)->disp_impl(os); } + friend std::ostream & operator << (std::ostream & os,const InertiaBase & X) { X.disp(os); return os; @@ -149,30 +156,26 @@ namespace pinocchio }; // traits InertiaTpl template - class InertiaTpl : public InertiaBase< InertiaTpl< _Scalar, _Options > > + struct InertiaTpl : public InertiaBase< InertiaTpl< _Scalar, _Options > > { - public: - friend class InertiaBase< InertiaTpl< _Scalar, _Options > >; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl); enum { Options = _Options }; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare; + typedef typename Eigen::Matrix Vector10; - typedef typename Eigen::Matrix<_Scalar, 10, 1, _Options> Vector10; - - public: // Constructors - InertiaTpl() - {} + InertiaTpl() {} InertiaTpl(const Scalar mass, const Vector3 & com, const Matrix3 & rotational_inertia) : m_mass(mass), m_com(com), m_inertia(rotational_inertia) {} - InertiaTpl(const Matrix6 & I6) + explicit InertiaTpl(const Matrix6 & I6) { - assert((I6 - I6.transpose()).isMuchSmallerThan(I6)); + assert(check_expression_if_real(isZero(I6 - I6.transpose()))); mass() = I6(LINEAR, LINEAR); const Matrix3 & mc_cross = I6.template block <3,3>(ANGULAR,LINEAR); lever() = unSkew(mc_cross); @@ -185,15 +188,17 @@ namespace pinocchio inertia() = S3; } - InertiaTpl(Scalar mass, const Vector3 & com, const Symmetric3 & rotational_inertia) + InertiaTpl(const Scalar & mass, const Vector3 & com, const Symmetric3 & rotational_inertia) : m_mass(mass), m_com(com), m_inertia(rotational_inertia) {} InertiaTpl(const InertiaTpl & clone) // Copy constructor - : m_mass(clone.mass()), m_com(clone.lever()), m_inertia(clone.inertia()) + : m_mass(clone.mass()) + , m_com(clone.lever()) + , m_inertia(clone.inertia()) {} - InertiaTpl& operator=(const InertiaTpl & clone) // Copy assignment operator + InertiaTpl & operator=(const InertiaTpl & clone) // Copy assignment operator { m_mass = clone.mass(); m_com = clone.lever(); @@ -201,12 +206,11 @@ namespace pinocchio return *this; } - template - InertiaTpl(const InertiaTpl & clone) - : m_mass(clone.mass()) - , m_com(clone.lever()) - , m_inertia(clone.inertia().matrix()) - {} + template + explicit InertiaTpl(const InertiaTpl & clone) + { + *this = clone.template cast(); + } // Initializers static InertiaTpl Zero() @@ -225,7 +229,7 @@ namespace pinocchio Symmetric3::Identity()); } - void setIdentity () + void setIdentity() { mass() = Scalar(1); lever().setZero(); inertia().setIdentity(); } @@ -443,7 +447,8 @@ namespace pinocchio // f.angular().noalias() = c.cross(f.linear()) + I*v.angular(); } - Scalar vtiv_impl(const Motion & v) const + template + Scalar vtiv_impl(const MotionDense & v) const { const Vector3 cxw (lever().cross(v.angular())); Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2)*v.linear().dot(cxw)); @@ -454,7 +459,8 @@ namespace pinocchio return res; } - Matrix6 variation(const Motion & v) const + template + Matrix6 variation(const MotionDense & v) const { Matrix6 res; const Motion mv(v*mass()); @@ -475,8 +481,8 @@ namespace pinocchio return res; } - template - static void vxi_impl(const Motion & v, + template + static void vxi_impl(const MotionDense & v, const InertiaTpl & I, const Eigen::MatrixBase & Iout) { @@ -510,8 +516,8 @@ namespace pinocchio } - template - static void ivx_impl(const Motion & v, + template + static void ivx_impl(const MotionDense & v, const InertiaTpl & I, const Eigen::MatrixBase & Iout) { @@ -549,7 +555,8 @@ namespace pinocchio Symmetric3 & inertia() { return m_inertia; } /// aI = aXb.act(bI) - InertiaTpl se3Action_impl(const SE3 & M) const + template + InertiaTpl se3Action_impl(const SE3Tpl & M) const { /* The multiplication RIR' has a particular form that could be used, however it * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/ @@ -560,14 +567,16 @@ namespace pinocchio } ///bI = aXb.actInv(aI) - InertiaTpl se3ActionInverse_impl(const SE3 & M) const + template + InertiaTpl se3ActionInverse_impl(const SE3Tpl & M) const { return InertiaTpl(mass(), M.rotation().transpose()*(lever()-M.translation()), inertia().rotate(M.rotation().transpose()) ); } - Force vxiv( const Motion& v ) const + template + Force vxiv( const MotionDense & v) const { const Vector3 & mcxw = mass()*lever().cross(v.angular()); const Vector3 & mv_mcxw = mass()*v.linear()-mcxw; @@ -587,7 +596,7 @@ namespace pinocchio template InertiaTpl cast() const { - return InertiaTpl(static_cast(mass()), + return InertiaTpl(pinocchio::cast(mass()), lever().template cast(), inertia().template cast()); } @@ -610,4 +619,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_inertia_hpp__ +#endif // ifndef __pinocchio_spatial_inertia_hpp__ diff --git a/src/spatial/log.hxx b/src/spatial/log.hxx index 90ec6f8826..678ad42562 100644 --- a/src/spatial/log.hxx +++ b/src/spatial/log.hxx @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2020 CNRS INRIA +// Copyright (c) 2015-2021 CNRS INRIA // #ifndef __pinocchio_spatial_log_hxx__ @@ -7,6 +7,33 @@ namespace pinocchio { + + namespace internal + { + template + void compute_theta_axis(const typename Matrix3::Scalar & val, + const Eigen::MatrixBase & R, + typename Matrix3::Scalar & angle, + const Eigen::MatrixBase & _axis) + { + typedef typename Matrix3::Scalar Scalar; + + static const long i1 = (i0+1) % 3; + static const long i2 = (i0+2) % 3; + Vector3 & axis = _axis.const_cast_derived(); + + const Scalar s = math::sqrt(val) * if_then_else(GE,R.coeff(i2,i1),R.coeff(i1,i2),Scalar(1.),Scalar(-1.)); + axis[i0] = s/Scalar(2); + axis[i1] = Scalar(1)/(2*s) * (R.coeff(i1,i0) + R.coeff(i0,i1)); + axis[i2] = Scalar(1)/(2*s) * (R.coeff(i2,i0) + R.coeff(i0,i2)); + const Scalar w = Scalar(1)/(2*s)*(R.coeff(i2,i1) - R.coeff(i1,i2)); + + const Scalar axis_norm = axis.norm(); + angle = 2*math::atan2(axis_norm,w); + axis /= axis_norm; + } + } + /// \brief Generic evaluation of log3 function template struct log3_impl @@ -14,57 +41,70 @@ namespace pinocchio template static void run(const Eigen::MatrixBase & R, typename Matrix3Like::Scalar & theta, - const Eigen::MatrixBase & res) + const Eigen::MatrixBase & angle_axis) { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3); - PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, res, 3, 1); - // TODO: add static_assert + PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, angle_axis, 3, 1); + using namespace internal; typedef typename Matrix3Like::Scalar Scalar; typedef Eigen::Matrix Vector3; - static const Scalar PI_value = PI(); + const static Scalar PI_value = PI(); + Vector3Out & angle_axis_ = angle_axis.const_cast_derived(); - Scalar tr = R.trace(); - if(tr >= Scalar(3)) - { - tr = Scalar(3); // clip value - theta = Scalar(0); // acos((3-1)/2) - } - else if(tr <= Scalar(-1)) + const Scalar tr = R.trace(); + const Scalar cos_value = (tr - Scalar(1))/Scalar(2); + const Scalar theta_nominal = if_then_else(LE,tr,Scalar(3), + if_then_else(GE,tr,Scalar(-1), + math::acos(cos_value), // then + PI_value // else + ), + Scalar(0) // else + ); + assert(check_expression_if_real(theta_nominal == theta_nominal) && "theta contains some NaN"); // theta != NaN + + Vector3 antisymmetric_R; unSkew(R,antisymmetric_R); + const Scalar norm_antisymmetric_R_squared = antisymmetric_R.squaredNorm(); + + // Singular cases when theta == PI + Vector3 angle_axis_singular; Scalar theta_singular; { - tr = Scalar(-1); // clip value - theta = PI_value; // acos((-1-1)/2) + Vector3 val_singular; + val_singular.array() = 2*R.diagonal().array() - tr + Scalar(1); + Vector3 axis_0, axis_1, axis_2; + Scalar theta_0, theta_1, theta_2; + + internal::compute_theta_axis<0>(val_singular[0], R, theta_0, axis_0); + internal::compute_theta_axis<1>(val_singular[1], R, theta_1, axis_1); + internal::compute_theta_axis<2>(val_singular[2], R, theta_2, axis_2); + + theta_singular = if_then_else(GE, val_singular[0], val_singular[1], + if_then_else(GE, val_singular[0], val_singular[2], + theta_0, theta_2), + if_then_else(GE, val_singular[1], val_singular[2], + theta_1, theta_2)); + + for(int k = 0; k < 3; ++k) + angle_axis_singular[k] = if_then_else(GE, val_singular[0], val_singular[1], + if_then_else(GE, val_singular[0], val_singular[2], + axis_0[k], axis_2[k]), + if_then_else(GE, val_singular[1], val_singular[2], + axis_1[k], axis_2[k])); } - else - theta = math::acos((tr - Scalar(1))/Scalar(2)); - assert(theta == theta && "theta contains some NaN"); // theta != NaN - Vector3Out & res_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Out,res); + const Scalar t = if_then_else(GE,theta_nominal,TaylorSeriesExpansion::template precision<2>(), + theta_nominal / sin(theta_nominal), // then + Scalar(1.) + norm_antisymmetric_R_squared/Scalar(6) + norm_antisymmetric_R_squared*norm_antisymmetric_R_squared*Scalar(3)/Scalar(40) // else + ); - // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small. - if(theta >= PI_value - 1e-2) - { - // 1e-2: A low value is not required since the computation is - // using explicit formula. However, the precision of this method - // is the square root of the precision with the antisymmetric - // method (Nominal case). - const Scalar cphi = -(tr - Scalar(1))/Scalar(2); - const Scalar beta = theta*theta / (Scalar(1) + cphi); - const Vector3 tmp((R.diagonal().array() + cphi) * beta); - res_(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0)); - res_(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0)); - res_(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0)); - } - else - { - const Scalar t = ((theta > TaylorSeriesExpansion::template precision<3>()) - ? theta / sin(theta) - : Scalar(1)) / Scalar(2); - res_(0) = t * (R (2, 1) - R (1, 2)); - res_(1) = t * (R (0, 2) - R (2, 0)); - res_(2) = t * (R (1, 0) - R (0, 1)); - } + theta = if_then_else(GE,cos_value,Scalar(-1.) + TaylorSeriesExpansion::template precision<2>(), + theta_nominal, theta_singular); + + for(int k = 0; k < 3; ++k) + angle_axis_[k] = if_then_else(GE,cos_value,Scalar(-1.) + TaylorSeriesExpansion::template precision<2>(), + t*antisymmetric_R[k], theta_singular*angle_axis_singular[k]); + } }; @@ -79,26 +119,22 @@ namespace pinocchio { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, log, 3, 1); PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jlog, 3, 3); - // TODO: add static_assert - Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog); + using namespace internal; + Scalar ct,st; SINCOS(theta,&st,&ct); + const Scalar st_1mct = st/(Scalar(1)-ct); - Scalar alpha, diag_value; - if(theta < TaylorSeriesExpansion::template precision<3>()) - { - alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720); - diag_value = Scalar(0.5) * (2 - theta*theta / Scalar(6)); - } - else - { - // Jlog = alpha I - Scalar ct,st; SINCOS(theta,&st,&ct); - const Scalar st_1mct = st/(Scalar(1)-ct); - - alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta); - diag_value = Scalar(0.5) * (theta * st_1mct); - } + const Scalar alpha = if_then_else(LT,theta,TaylorSeriesExpansion::template precision<3>(), + Scalar(1)/Scalar(12) + theta*theta / Scalar(720), // then + Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta) // else + ); + + const Scalar diag_value = if_then_else(LT,theta,TaylorSeriesExpansion::template precision<3>(), + Scalar(0.5) * (2 - theta*theta / Scalar(6)), // then + Scalar(0.5) * (theta * st_1mct) // else + ); + Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog); Jlog_.noalias() = alpha * log * log.transpose(); Jlog_.diagonal().array() += diag_value; @@ -117,26 +153,29 @@ namespace pinocchio { typedef SE3Tpl SE3; typedef typename SE3::Vector3 Vector3; - + typename SE3::ConstAngularRef R = M.rotation(); typename SE3::ConstLinearRef p = M.translation(); - Scalar t; - Vector3 w(log3(R,t)); // t in [0,π] - const Scalar t2 = t*t; - Scalar alpha, beta; + using namespace internal; - if (t < TaylorSeriesExpansion::template precision<3>()) - { - alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720); - beta = Scalar(1)/Scalar(12) + t2/Scalar(720); - } - else - { - Scalar st,ct; SINCOS(t,&st,&ct); - alpha = t*st/(Scalar(2)*(Scalar(1)-ct)); - beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct)); - } + Vector3 antisymmetric_R; unSkew(R,antisymmetric_R); + const Scalar norm_antisymmetric_R_squared = antisymmetric_R.squaredNorm(); + + Scalar theta; + const Vector3 w(log3(R,theta)); // t in [0,π] + const Scalar & t2 = norm_antisymmetric_R_squared; + + Scalar st,ct; SINCOS(theta,&st,&ct); + const Scalar alpha = if_then_else(LT,theta,TaylorSeriesExpansion::template precision<3>(), + Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720), // then + theta*st/(Scalar(2)*(Scalar(1)-ct)) // else + ); + + const Scalar beta = if_then_else(LT,theta,TaylorSeriesExpansion::template precision<3>(), + Scalar(1)/Scalar(12) + t2/Scalar(720), // then + Scalar(1)/(theta*theta) - st/(Scalar(2)*theta*(Scalar(1)-ct)) // else + ); mout.linear().noalias() = alpha * p - Scalar(0.5) * w.cross(p) + (beta * w.dot(p)) * w; mout.angular() = w; @@ -151,7 +190,6 @@ namespace pinocchio const Eigen::MatrixBase & Jlog) { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jlog, 6, 6); - // TODO: add static_assert typedef SE3Tpl SE3; typedef typename SE3::Vector3 Vector3; @@ -159,6 +197,8 @@ namespace pinocchio typename SE3::ConstAngularRef R = M.rotation(); typename SE3::ConstLinearRef p = M.translation(); + + using namespace internal; Scalar t; Vector3 w(log3(R,t)); @@ -176,27 +216,24 @@ namespace pinocchio D = A; const Scalar t2 = t*t; - Scalar beta, beta_dot_over_theta; - if(t < TaylorSeriesExpansion::template precision<3>()) - { - beta = Scalar(1)/Scalar(12) + t2/Scalar(720); - beta_dot_over_theta = Scalar(1)/Scalar(360); - } - else - { - const Scalar tinv = Scalar(1)/t, - t2inv = tinv*tinv; - Scalar st,ct; SINCOS (t, &st, &ct); - const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct)); - - beta = t2inv - st*tinv*inv_2_2ct; - beta_dot_over_theta = -Scalar(2)*t2inv*t2inv + - (Scalar(1) + st*tinv) * t2inv * inv_2_2ct; - } + const Scalar tinv = Scalar(1)/t, + t2inv = tinv*tinv; + + Scalar st,ct; SINCOS (t, &st, &ct); + const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct)); - Scalar wTp = w.dot(p); + const Scalar beta = if_then_else(LT,t,TaylorSeriesExpansion::template precision<3>(), + Scalar(1)/Scalar(12) + t2/Scalar(720), // then + t2inv - st*tinv*inv_2_2ct // else + ); + + const Scalar beta_dot_over_theta = if_then_else(LT,t,TaylorSeriesExpansion::template precision<3>(), + Scalar(1)/Scalar(360), // then + -Scalar(2)*t2inv*t2inv + (Scalar(1) + st*tinv) * t2inv * inv_2_2ct // else + ); - Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p); + const Scalar wTp = w.dot(p); + const Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p); // C can be treated as a temporary variable C.noalias() = v3_tmp * w.transpose(); C.noalias() += beta * w * p.transpose(); diff --git a/src/spatial/motion-base.hpp b/src/spatial/motion-base.hpp index 3935c4c5a0..b42b2811e4 100644 --- a/src/spatial/motion-base.hpp +++ b/src/spatial/motion-base.hpp @@ -1,16 +1,16 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_motion_base_hpp__ -#define __pinocchio_motion_base_hpp__ +#ifndef __pinocchio_spatial_motion_base_hpp__ +#define __pinocchio_spatial_motion_base_hpp__ namespace pinocchio { template - class MotionBase + class MotionBase : NumericalBase { public: MOTION_TYPEDEF_TPL(Derived); @@ -18,6 +18,8 @@ namespace pinocchio Derived & derived() { return *static_cast(this); } const Derived & derived() const { return *static_cast(this); } + Derived & const_cast_derived() const { return *const_cast(&derived()); } + ConstAngularType angular() const { return derived().angular_impl(); } ConstLinearType linear() const { return derived().linear_impl(); } AngularType angular() { return derived().angular_impl(); } @@ -112,4 +114,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_motion_base_hpp__ +#endif // ifndef __pinocchio_spatial_motion_base_hpp__ diff --git a/src/spatial/motion-dense.hpp b/src/spatial/motion-dense.hpp index 941f503e9f..fb795a0177 100644 --- a/src/spatial/motion-dense.hpp +++ b/src/spatial/motion-dense.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2017-2019 CNRS INRIA +// Copyright (c) 2017-2020 CNRS INRIA // -#ifndef __pinocchio_motion_dense_hpp__ -#define __pinocchio_motion_dense_hpp__ +#ifndef __pinocchio_spatial_motion_dense_hpp__ +#define __pinocchio_spatial_motion_dense_hpp__ #include "pinocchio/spatial/skew.hpp" @@ -70,6 +70,17 @@ namespace pinocchio // Arithmetic operators template Derived & operator=(const MotionDense & other) + { + return derived().set(other.derived()); + } + + Derived & operator=(const MotionDense & other) + { + return derived().set(other.derived()); + } + + template + Derived & set(const MotionDense & other) { linear() = other.linear(); angular() = other.angular(); @@ -217,6 +228,10 @@ namespace pinocchio /// \returns a MotionRef on this. MotionRefType ref() { return derived().ref(); } + protected: + + MotionDense() {}; + }; // class MotionDense /// Basic operations specialization @@ -237,4 +252,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_motion_dense_hpp__ +#endif // ifndef __pinocchio_spatial_motion_dense_hpp__ diff --git a/src/spatial/motion-ref.hpp b/src/spatial/motion-ref.hpp index 0377fb8532..e9421eb225 100644 --- a/src/spatial/motion-ref.hpp +++ b/src/spatial/motion-ref.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2017-2019 CNRS INRIA // -#ifndef __pinocchio_motion_ref_hpp__ -#define __pinocchio_motion_ref_hpp__ +#ifndef __pinocchio_spatial_motion_ref_hpp__ +#define __pinocchio_spatial_motion_ref_hpp__ namespace pinocchio { @@ -257,4 +257,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_motion_ref_hpp__ +#endif // ifndef __pinocchio_spatial_motion_ref_hpp__ diff --git a/src/spatial/motion-tpl.hpp b/src/spatial/motion-tpl.hpp index 6978636d45..3c7dc3c499 100644 --- a/src/spatial/motion-tpl.hpp +++ b/src/spatial/motion-tpl.hpp @@ -3,8 +3,8 @@ // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_motion_tpl_hpp__ -#define __pinocchio_motion_tpl_hpp__ +#ifndef __pinocchio_spatial_motion_tpl_hpp__ +#define __pinocchio_spatial_motion_tpl_hpp__ namespace pinocchio { @@ -54,13 +54,14 @@ namespace pinocchio using Base::__mult__; // Constructors - MotionTpl() : m_data() {} + MotionTpl() {} template - MotionTpl(const Eigen::MatrixBase & v, const Eigen::MatrixBase & w) + MotionTpl(const Eigen::MatrixBase & v, + const Eigen::MatrixBase & w) { - assert(v.size() == 3); - assert(w.size() == 3); + EIGEN_STATIC_ASSERT_VECTOR_ONLY(V1); + EIGEN_STATIC_ASSERT_VECTOR_ONLY(V2); linear() = v; angular() = w; } @@ -69,7 +70,17 @@ namespace pinocchio : m_data(v) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); - assert(v.size() == 6); + } + + MotionTpl(const MotionTpl & other) + { + *this = other; + } + + template + explicit MotionTpl(const MotionTpl & other) + { + *this = other.template cast(); } template @@ -169,4 +180,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_motion_tpl_hpp__ +#endif // ifndef __pinocchio_spatial_motion_tpl_hpp__ diff --git a/src/spatial/motion-zero.hpp b/src/spatial/motion-zero.hpp index 7524283b57..ce67ea8f38 100644 --- a/src/spatial/motion-zero.hpp +++ b/src/spatial/motion-zero.hpp @@ -1,10 +1,10 @@ // -// Copyright (c) 2015-2019 CNRS, INRIA +// Copyright (c) 2015-2019 CNRS INRIA // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_motion_zero_hpp__ -#define __pinocchio_motion_zero_hpp__ +#ifndef __pinocchio_spatial_motion_zero_hpp__ +#define __pinocchio_spatial_motion_zero_hpp__ namespace pinocchio { @@ -142,4 +142,4 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } // namespace pinocchio -#endif // ifndef __pinocchio_motion_zero_hpp__ +#endif // ifndef __pinocchio_spatial_motion_zero_hpp__ diff --git a/src/spatial/motion.hpp b/src/spatial/motion.hpp index ecc39cdfb6..5ca61f6caf 100644 --- a/src/spatial/motion.hpp +++ b/src/spatial/motion.hpp @@ -3,8 +3,8 @@ // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_motion_hpp__ -#define __pinocchio_motion_hpp__ +#ifndef __pinocchio_spatial_motion_hpp__ +#define __pinocchio_spatial_motion_hpp__ #include "pinocchio/spatial/fwd.hpp" #include "pinocchio/macros.hpp" @@ -51,4 +51,4 @@ namespace pinocchio #include "pinocchio/spatial/motion-ref.hpp" #include "pinocchio/spatial/motion-zero.hpp" -#endif // ifndef __pinocchio_motion_hpp__ +#endif // ifndef __pinocchio_spatial_motion_hpp__ diff --git a/src/spatial/se3-base.hpp b/src/spatial/se3-base.hpp index 8ac55ac0c6..3faed30008 100644 --- a/src/spatial/se3-base.hpp +++ b/src/spatial/se3-base.hpp @@ -3,8 +3,8 @@ // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_se3_base_hpp__ -#define __pinocchio_se3_base_hpp__ +#ifndef __pinocchio_spatial_se3_base_hpp__ +#define __pinocchio_spatial_se3_base_hpp__ namespace pinocchio { @@ -27,13 +27,15 @@ namespace pinocchio * \ingroup pinocchio_spatial */ template - struct SE3Base + struct SE3Base : NumericalBase { PINOCCHIO_SE3_TYPEDEF_TPL(Derived); Derived & derived() { return *static_cast(this); } const Derived& derived() const { return *static_cast(this); } + Derived & const_cast_derived() const { return *const_cast(&derived()); } + ConstAngularRef rotation() const { return derived().rotation_impl(); } ConstLinearRef translation() const { return derived().translation_impl(); } AngularRef rotation() { return derived().rotation_impl(); } @@ -152,4 +154,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_se3_base_hpp__ +#endif // ifndef __pinocchio_spatial_se3_base_hpp__ diff --git a/src/spatial/se3-tpl.hpp b/src/spatial/se3-tpl.hpp index c1c03c1e15..708fea754f 100644 --- a/src/spatial/se3-tpl.hpp +++ b/src/spatial/se3-tpl.hpp @@ -3,8 +3,8 @@ // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_se3_tpl_hpp__ -#define __pinocchio_se3_tpl_hpp__ +#ifndef __pinocchio_spatial_se3_tpl_hpp__ +#define __pinocchio_spatial_se3_tpl_hpp__ #include "pinocchio/spatial/fwd.hpp" #include "pinocchio/spatial/se3-base.hpp" @@ -44,9 +44,11 @@ namespace pinocchio }; // traits SE3Tpl template - struct SE3Tpl : public SE3Base< SE3Tpl<_Scalar,_Options> > + struct SE3Tpl + : public SE3Base< SE3Tpl<_Scalar,_Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PINOCCHIO_SE3_TYPEDEF_TPL(SE3Tpl); typedef SE3Base< SE3Tpl<_Scalar,_Options> > Base; typedef Eigen::Quaternion Quaternion; @@ -78,6 +80,17 @@ namespace pinocchio EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3) } + SE3Tpl(const SE3Tpl & other) + { + *this = other; + } + + template + explicit SE3Tpl(const SE3Tpl & other) + { + *this = other.template cast(); + } + template explicit SE3Tpl(const Eigen::MatrixBase & m) : rot(m.template block<3,3>(LINEAR,LINEAR)) @@ -86,7 +99,7 @@ namespace pinocchio EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4); } - SE3Tpl(int) + explicit SE3Tpl(int) : rot(AngularType::Identity()) , trans(LinearType::Zero()) {} @@ -357,5 +370,5 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_se3_tpl_hpp__ +#endif // ifndef __pinocchio_spatial_se3_tpl_hpp__ diff --git a/src/spatial/se3.hpp b/src/spatial/se3.hpp index 1caeafa9d4..9741600735 100644 --- a/src/spatial/se3.hpp +++ b/src/spatial/se3.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2015-2018 CNRS +// Copyright (c) 2015-2020 CNRS // -#ifndef __pinocchio_se3_hpp__ -#define __pinocchio_se3_hpp__ +#ifndef __pinocchio_spatial_se3_hpp__ +#define __pinocchio_spatial_se3_hpp__ #include "pinocchio/spatial/fwd.hpp" #include "pinocchio/macros.hpp" @@ -43,4 +43,4 @@ namespace pinocchio #include "pinocchio/spatial/se3-base.hpp" #include "pinocchio/spatial/se3-tpl.hpp" -#endif // ifndef __pinocchio_se3_hpp__ +#endif // ifndef __pinocchio_spatial_se3_hpp__ diff --git a/src/spatial/skew.hpp b/src/spatial/skew.hpp index 3e1c40ce60..7feb781fc7 100644 --- a/src/spatial/skew.hpp +++ b/src/spatial/skew.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2015-2018 CNRS INRIA +// Copyright (c) 2015-2021 CNRS INRIA // -#ifndef __pinocchio_skew_hpp__ -#define __pinocchio_skew_hpp__ +#ifndef __pinocchio_spatial_skew_hpp__ +#define __pinocchio_spatial_skew_hpp__ #include "pinocchio/macros.hpp" @@ -82,12 +82,11 @@ namespace pinocchio inline void unSkew(const Eigen::MatrixBase & M, const Eigen::MatrixBase & v) { + typedef typename Vector3::RealScalar Scalar; EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3); EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3); - assert((M + M.transpose()).isMuchSmallerThan(M)); - + Vector3 & v_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,v); - typedef typename Vector3::RealScalar Scalar; v_[0] = Scalar(0.5) * (M(2,1) - M(1,2)); v_[1] = Scalar(0.5) * (M(0,2) - M(2,0)); @@ -104,10 +103,10 @@ namespace pinocchio /// \return The vector entries of the skew-symmetric matrix. /// template - inline Eigen::Matrix + inline Eigen::Matrix unSkew(const Eigen::MatrixBase & M) { - Eigen::Matrix v; + Eigen::Matrix v; unSkew(M,v); return v; } @@ -243,4 +242,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_skew_hpp__ +#endif // ifndef __pinocchio_spatial_skew_hpp__ diff --git a/src/spatial/symmetric3.hpp b/src/spatial/symmetric3.hpp index cbc8e6eabe..2f582d0640 100644 --- a/src/spatial/symmetric3.hpp +++ b/src/spatial/symmetric3.hpp @@ -1,9 +1,9 @@ // -// Copyright (c) 2014-2019 CNRS INRIA +// Copyright (c) 2014-2020 CNRS INRIA // -#ifndef __pinocchio_symmetric3_hpp__ -#define __pinocchio_symmetric3_hpp__ +#ifndef __pinocchio_spatial_symmetric3__ +#define __pinocchio_spatial_symmetric3__ #include "pinocchio/macros.hpp" #include "pinocchio/math/matrix.hpp" @@ -26,12 +26,12 @@ namespace pinocchio EIGEN_MAKE_ALIGNED_OPERATOR_NEW public: - Symmetric3Tpl(): m_data() {} + Symmetric3Tpl() {} template explicit Symmetric3Tpl(const Eigen::Matrix & I) { - assert( (I-I.transpose()).isMuchSmallerThan(I) ); + assert(check_expression_if_real(pinocchio::isZero((I-I.transpose())))); m_data(0) = I(0,0); m_data(1) = I(1,0); m_data(2) = I(1,1); m_data(3) = I(2,0); m_data(4) = I(2,1); m_data(5) = I(2,2); @@ -39,6 +39,17 @@ namespace pinocchio explicit Symmetric3Tpl(const Vector6 & I) : m_data(I) {} + Symmetric3Tpl(const Symmetric3Tpl & other) + { + *this = other; + } + + template + explicit Symmetric3Tpl(const Symmetric3Tpl & other) + { + *this = other.template cast(); + } + Symmetric3Tpl(const Scalar & a0, const Scalar & a1, const Scalar & a2, const Scalar & a3, const Scalar & a4, const Scalar & a5) { m_data << a0,a1,a2,a3,a4,a5; } @@ -62,6 +73,15 @@ namespace pinocchio static Symmetric3Tpl Identity() { return Symmetric3Tpl(1, 0, 1, 0, 0, 1); } void setIdentity() { m_data << 1, 0, 1, 0, 0, 1; } + + template + void setDiagonal(const Eigen::MatrixBase & diag) + { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); + m_data[0] = diag[0]; + m_data[2] = diag[1]; + m_data[5] = diag[2]; + } /* Required by Inertia::operator== */ bool operator==(const Symmetric3Tpl & other) const @@ -379,17 +399,17 @@ namespace pinocchio return ((i!=2)&&(j!=2)) ? m_data[i+j] : m_data[i+j+1]; } - Symmetric3Tpl operator-(const Matrix3 &S) const + Symmetric3Tpl operator-(const Matrix3 & S) const { - assert( (S-S.transpose()).isMuchSmallerThan(S) ); + assert(check_expression_if_real(pinocchio::isZero(S-S.transpose()))); return Symmetric3Tpl( m_data(0)-S(0,0), m_data(1)-S(1,0), m_data(2)-S(1,1), m_data(3)-S(2,0), m_data(4)-S(2,1), m_data(5)-S(2,2) ); } - Symmetric3Tpl operator+(const Matrix3 &S) const + Symmetric3Tpl operator+(const Matrix3 & S) const { - assert( (S-S.transpose()).isMuchSmallerThan(S) ); + assert(check_expression_if_real(pinocchio::isZero(S-S.transpose()))); return Symmetric3Tpl( m_data(0)+S(0,0), m_data(1)+S(1,0), m_data(2)+S(1,1), m_data(3)+S(2,0), m_data(4)+S(2,1), m_data(5)+S(2,2) ); @@ -415,7 +435,7 @@ namespace pinocchio Symmetric3Tpl rotate(const Eigen::MatrixBase & R) const { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3); - assert(isUnitary(R.transpose()*R) && "R is not a Unitary matrix"); + assert(check_expression_if_real(isUnitary(R.transpose()*R)) && "R is not a Unitary matrix"); Symmetric3Tpl Sres; @@ -471,5 +491,5 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_symmetric3_hpp__ +#endif // ifndef __pinocchio_spatial_symmetric3__ diff --git a/src/utils/check.hpp b/src/utils/check.hpp new file mode 100644 index 0000000000..5bdd97c5e5 --- /dev/null +++ b/src/utils/check.hpp @@ -0,0 +1,40 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_utils_check_hpp__ +#define __pinocchio_utils_check_hpp__ + +#include + +namespace pinocchio +{ + + template::value> + struct check_expression_if_real_valued + { + static bool run(const void *) { return default_value; } + }; + + template + struct check_expression_if_real_valued + { + static bool run(const void * expression_ptr) + { return *static_cast(expression_ptr); } + }; + + template + bool check_expression_if_real(const Any & expression) + { + return check_expression_if_real_valued::run(static_cast(&expression)); + } + + template + bool check_expression_if_real(const Any & expression) + { + return check_expression_if_real_valued::run(static_cast(&expression)); + } + +} + +#endif // ifndef __pinocchio_utils_check_hpp__ diff --git a/src/utils/file-io.hpp b/src/utils/file-io.hpp new file mode 100644 index 0000000000..83e9d22b22 --- /dev/null +++ b/src/utils/file-io.hpp @@ -0,0 +1,84 @@ +// +// Copyright (c) 2020 LAAS-CNRS +// + +// modified from https://gist.github.com/rudolfovich/f250900f1a833e715260a66c87369d15 + +#ifndef __pinocchio_utils_file_io_hpp__ +#define __pinocchio_utils_file_io_hpp__ + +#include +#include +#include + +namespace pinocchio +{ + class CsvStream { + std::ofstream fs_; + bool is_first_; + const std::string separator_; + const std::string escape_seq_; + const std::string special_chars_; + + public: + CsvStream(const std::string filename, const std::string separator = ",") + : fs_(), is_first_(true), separator_(separator), escape_seq_("\""), special_chars_("\"") { + fs_.exceptions(std::ios::failbit | std::ios::badbit); + fs_.open(filename.c_str()); + } + + ~CsvStream() { + flush(); + fs_.close(); + } + + void flush() { fs_.flush(); } + + inline static CsvStream& endl(CsvStream& file) { + file.endrow(); + return file; + } + + void endrow() { + fs_ << std::endl; + is_first_ = true; + } + + CsvStream& operator<<(CsvStream& (*val)(CsvStream&)) { return val(*this); } + + CsvStream& operator<<(const char* val) { return write(escape(val)); } + + CsvStream& operator<<(const std::string& val) { return write(escape(val)); } + + template + CsvStream& operator<<(const T& val) { + return write(val); + } + + private: + template + CsvStream& write(const T& val) { + if (!is_first_) { + fs_ << separator_; + } else { + is_first_ = false; + } + fs_ << val; + return *this; + } + + std::string escape(const std::string& val) { + std::ostringstream result; + result << '"'; + std::string::size_type to, from = 0u, len = val.length(); + while (from < len && std::string::npos != (to = val.find_first_of(special_chars_, from))) { + result << val.substr(from, to - from) << escape_seq_ << val[to]; + from = to + 1; + } + result << val.substr(from) << '"'; + return result.str(); + } + }; +} + +#endif diff --git a/src/utils/static-if.hpp b/src/utils/static-if.hpp index 37b879df96..334f2ee71a 100644 --- a/src/utils/static-if.hpp +++ b/src/utils/static-if.hpp @@ -16,6 +16,9 @@ namespace pinocchio template struct if_then_else_impl; + + template + struct comparison_eq_impl; /// \brief Template specialization for similar return types; template @@ -37,20 +40,20 @@ namespace pinocchio { switch(op) { - case LT: - if(lhs_value < rhs_value) + case EQ: + if(lhs_value == rhs_value) return then_value; else return else_value; break; - case LE: - if(lhs_value <= rhs_value) + case LT: + if(lhs_value < rhs_value) return then_value; else return else_value; break; - case EQ: - if(lhs_value == rhs_value) + case LE: + if(lhs_value <= rhs_value) return then_value; else return else_value; @@ -87,9 +90,37 @@ namespace pinocchio then_value, else_value); } - - - + + //Generic + template + struct comparison_eq_impl + { + static inline bool run(const LhsType & lhs_value, + const RhsType & rhs_value) + { + return lhs_value == rhs_value; + } + }; + + template + struct comparison_eq_impl + { + static inline bool run(const condition_type & lhs_value, + const condition_type & rhs_value) + { + return lhs_value == rhs_value; + } + }; + + template + inline bool + comparison_eq(const LhsType & lhs_value, + const RhsType & rhs_value) + { + typedef comparison_eq_impl algo; + return algo::run(lhs_value, + rhs_value); + } } // namespace internal } // namespace pinocchio diff --git a/src/utils/timer2.hpp b/src/utils/timer2.hpp new file mode 100644 index 0000000000..7059ba64df --- /dev/null +++ b/src/utils/timer2.hpp @@ -0,0 +1,39 @@ +// +// Copyright (c) 2021 LAAS-CNRS +// + +#ifndef __pinocchio_utils_timer2_hpp__ +#define __pinocchio_utils_timer2_hpp__ + +#include + +namespace pinocchio { + +class Timer { + public: + Timer() { clock_gettime(CLOCK_MONOTONIC, &start_); } + + inline void reset() { clock_gettime(CLOCK_MONOTONIC, &start_); } + + inline double get_duration() { + clock_gettime(CLOCK_MONOTONIC, &finish_); + duration_ = static_cast(finish_.tv_sec - start_.tv_sec) * 1000000; + duration_ += static_cast(finish_.tv_nsec - start_.tv_nsec) / 1000; + return duration_ / 1000.; + } + + inline double get_us_duration() { + clock_gettime(CLOCK_MONOTONIC, &finish_); + duration_ = static_cast(finish_.tv_sec - start_.tv_sec) * 1000000; + duration_ += static_cast(finish_.tv_nsec - start_.tv_nsec) / 1000; + return duration_; + } + + private: + struct timespec start_; + struct timespec finish_; + double duration_; +}; +} // namespace pinocchio + +#endif diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 60386f554e..405286e4ab 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -8,36 +8,47 @@ # --- MACROS ------------------------------------------------------------------ MACRO(ADD_TEST_CFLAGS target flag) - SET_PROPERTY(TARGET "test-cpp-${target}" APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}") + GET_CPP_TEST_NAME(${target} ${CMAKE_CURRENT_SOURCE_DIR} test_name) + SET_PROPERTY(TARGET ${test_name} APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}") + UNSET(test_name) ENDMACRO(ADD_TEST_CFLAGS) -MACRO(ADD_PINOCCHIO_UNIT_TEST NAME) +FUNCTION(GET_CPP_TEST_NAME name src_dir full_test_name) + STRING(REPLACE "${PINOCCHIO_UNIT_TEST_DIR}" "" prefix_name "${src_dir}") + STRING(REGEX REPLACE "[/]" "-" prefix_name "${prefix_name}-") + + SET(${full_test_name} "test-cpp${prefix_name}${name}" PARENT_SCOPE) +ENDFUNCTION(GET_CPP_TEST_NAME) + +SET(PINOCCHIO_UNIT_TEST_DIR ${CMAKE_CURRENT_SOURCE_DIR}) + +MACRO(ADD_PINOCCHIO_UNIT_TEST name) SET(options HEADER_ONLY) SET(oneValueArgs) SET(multiValueArgs PACKAGES) CMAKE_PARSE_ARGUMENTS(unit_test "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN} ) - SET(PKGS ${unit_test_PACKAGES}) - SET(TEST_NAME "test-cpp-${NAME}") - ADD_UNIT_TEST(${TEST_NAME} ${NAME}.cpp) + GET_CPP_TEST_NAME(${name} ${CMAKE_CURRENT_SOURCE_DIR} TEST_NAME) + ADD_UNIT_TEST(${TEST_NAME} ${name}.cpp) SET_TARGET_PROPERTIES(${TEST_NAME} PROPERTIES LINKER_LANGUAGE CXX) TARGET_INCLUDE_DIRECTORIES(${TEST_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - ADD_TEST_CFLAGS(${NAME} "-DBOOST_TEST_DYN_LINK") - SET(MODULE_NAME "${NAME}Test") + ADD_TEST_CFLAGS(${name} "-DBOOST_TEST_DYN_LINK") + SET(MODULE_NAME "${name}Test") STRING(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME}) - ADD_TEST_CFLAGS(${NAME} "-DBOOST_TEST_MODULE=${MODULE_NAME}") - ADD_TEST_CFLAGS(${NAME} "-DPINOCCHIO_MODEL_DIR=\\\\\"${PINOCCHIO_MODEL_DIR}\\\\\"") + ADD_TEST_CFLAGS(${name} "-DBOOST_TEST_MODULE=${MODULE_NAME}") + ADD_TEST_CFLAGS(${name} "-DPINOCCHIO_MODEL_DIR=\\\\\"${PINOCCHIO_MODEL_DIR}\\\\\"") IF(WIN32) - ADD_TEST_CFLAGS(${NAME} "-DNOMINMAX -D_USE_MATH_DEFINES") + ADD_TEST_CFLAGS(${name} "-DNOMINMAX -D_USE_MATH_DEFINES") ENDIF(WIN32) IF(NOT unit_test_HEADER_ONLY) TARGET_LINK_LIBRARIES(${TEST_NAME} PUBLIC ${PROJECT_NAME}) ENDIF(NOT unit_test_HEADER_ONLY) TARGET_LINK_LIBRARIES(${TEST_NAME} PRIVATE ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY}) + UNSET(TEST_NAME) ENDMACRO(ADD_PINOCCHIO_UNIT_TEST) MACRO(ADD_OPTIONAL_COMPILE_DEPENDENCY PKG_CONFIG_STRING) @@ -80,7 +91,10 @@ ADD_PINOCCHIO_UNIT_TEST(centroidal) ADD_PINOCCHIO_UNIT_TEST(com) ADD_PINOCCHIO_UNIT_TEST(joint-jacobian) ADD_PINOCCHIO_UNIT_TEST(cholesky) +ADD_PINOCCHIO_UNIT_TEST(constrained-dynamics) ADD_PINOCCHIO_UNIT_TEST(contact-dynamics) +ADD_PINOCCHIO_UNIT_TEST(closed-loop-dynamics) +ADD_PINOCCHIO_UNIT_TEST(impulse-dynamics) ADD_PINOCCHIO_UNIT_TEST(sample-models) ADD_PINOCCHIO_UNIT_TEST(kinematics) @@ -99,7 +113,7 @@ IF(BUILD_PYTHON_INTERFACE AND NOT BUILD_PYTHON_INTERFACE_WITH_PYPY) ADD_PINOCCHIO_UNIT_TEST(python_parser PACKAGES eigenpy) TARGET_INCLUDE_DIRECTORIES(test-cpp-python_parser SYSTEM PUBLIC ${PYTHON_INCLUDE_DIRS}) - TARGET_LINK_LIBRARIES(test-cpp-python_parser PUBLIC ${PYWRAP}) + TARGET_LINK_LIBRARIES(test-cpp-python_parser PUBLIC ${PYWRAP}_default) TARGET_LINK_LIBRARIES(test-cpp-python_parser PUBLIC ${PYTHON_LIBRARIES}) SET(ENV_VARIABLES "PYTHONPATH=${CMAKE_BINARY_DIR}/bindings/python:$ENV{PYTHONPATH}") IF(WIN32) @@ -127,9 +141,10 @@ ADD_PINOCCHIO_UNIT_TEST(joint-generic) ADD_PINOCCHIO_UNIT_TEST(joint-composite) ADD_PINOCCHIO_UNIT_TEST(joint-mimic) +# Main corpus ADD_PINOCCHIO_UNIT_TEST(model) ADD_PINOCCHIO_UNIT_TEST(data) -ADD_PINOCCHIO_UNIT_TEST(constraint) +ADD_PINOCCHIO_UNIT_TEST(joint-motion-subspace) ADD_PINOCCHIO_UNIT_TEST(compute-all-terms) ADD_PINOCCHIO_UNIT_TEST(energy) ADD_PINOCCHIO_UNIT_TEST(frames) @@ -146,6 +161,10 @@ ADD_PINOCCHIO_UNIT_TEST(cartesian-product-liegroups) ADD_PINOCCHIO_UNIT_TEST(regressor) ADD_PINOCCHIO_UNIT_TEST(version) ADD_PINOCCHIO_UNIT_TEST(copy) +ADD_PINOCCHIO_UNIT_TEST(contact-cholesky) +ADD_PINOCCHIO_UNIT_TEST(classic-acceleration) + +# Serialization MAKE_DIRECTORY("${CMAKE_CURRENT_BINARY_DIR}/serialization-data") ADD_PINOCCHIO_UNIT_TEST(serialization) ADD_TEST_CFLAGS(serialization "-DTEST_SERIALIZATION_FOLDER=\\\\\"${CMAKE_CURRENT_BINARY_DIR}/serialization-data\\\\\"") @@ -157,7 +176,11 @@ ADD_PINOCCHIO_UNIT_TEST(rnea-derivatives) ADD_PINOCCHIO_UNIT_TEST(aba-derivatives) ADD_PINOCCHIO_UNIT_TEST(centroidal-derivatives) ADD_PINOCCHIO_UNIT_TEST(center-of-mass-derivatives) +ADD_PINOCCHIO_UNIT_TEST(constrained-dynamics-derivatives) ADD_PINOCCHIO_UNIT_TEST(contact-dynamics-derivatives) +ADD_PINOCCHIO_UNIT_TEST(impulse-dynamics-derivatives) + +ADD_SUBDIRECTORY(algorithm) # Multiprecision arithmetic IF(BUILD_ADVANCED_TESTING) diff --git a/unittest/aba-derivatives.cpp b/unittest/aba-derivatives.cpp index 0cb3ad9c6b..59142c509e 100644 --- a/unittest/aba-derivatives.cpp +++ b/unittest/aba-derivatives.cpp @@ -2,8 +2,6 @@ // Copyright (c) 2018-2020 CNRS INRIA // -#include "pinocchio/multibody/model.hpp" -#include "pinocchio/multibody/data.hpp" #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/kinematics.hpp" @@ -31,7 +29,6 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives) buildModels::humanoidRandom(model); Data data(model), data_ref(model); - model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); VectorXd q = randomConfiguration(model); @@ -39,29 +36,57 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives) VectorXd tau(VectorXd::Random(model.nv)); VectorXd a(aba(model,data_ref,q,v,tau)); + VectorXd tau_from_a(rnea(model,data_ref,q,v,a)); + BOOST_CHECK(tau_from_a.isApprox(tau)); + MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau); computeRNEADerivatives(model,data_ref,q,v,a); + BOOST_CHECK(data.J.isApprox(data_ref.J)); for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) { BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); - BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); - BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); + BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); + BOOST_CHECK(data.oh[k].isApprox(data_ref.oh[k])); + BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa[k] - model.gravity)); BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k])); BOOST_CHECK(data.of[k].isApprox(data_ref.of[k])); BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k])); BOOST_CHECK(data.doYcrb[k].isApprox(data_ref.doYcrb[k])); } - computeJointJacobians(model,data_ref,q); - BOOST_CHECK(data.J.isApprox(data_ref.J)); + aba(model,data_ref,q,v,tau); + for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) + { + BOOST_CHECK(data.oYaba[k].isApprox(data_ref.oMi[k].toDualActionMatrix()*data_ref.Yaba[k]*data_ref.oMi[k].inverse().toActionMatrix())); +// BOOST_CHECK(data.of[k].isApprox(data.oMi[k].act(data_ref.f[k]))); + } + BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); aba(model,data_ref,q,v,tau); + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.u.isApprox(data_ref.u)); + for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) + { + BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); + BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); + //BOOST_CHECK(data.oYaba[k].isApprox(data_ref.oYaba[k])); +// BOOST_CHECK(data.of[k].isApprox(data_ref.of[k])); + BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k])); + //BOOST_CHECK(data.joints[k].U().isApprox(data_ref.joints[k].U())); + //BOOST_CHECK(data.joints[k].StU().isApprox(data_ref.joints[k].StU())); + BOOST_CHECK(data.joints[k].Dinv().isApprox(data_ref.joints[k].Dinv())); + //BOOST_CHECK(data.joints[k].UDinv().isApprox(data_ref.joints[k].UDinv())); + } BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); + computeJointJacobians(model,data_ref,q); + BOOST_CHECK(data.J.isApprox(data_ref.J)); + + computeMinverse(model,data_ref,q); data_ref.Minv.triangularView() = data_ref.Minv.transpose().triangularView(); @@ -310,7 +335,6 @@ BOOST_AUTO_TEST_CASE(test_aba_derivatives_vs_kinematics_derivatives) VectorXd tau = rnea(model,data_ref,q,v,a); - /// Check againt computeGeneralizedGravityDerivatives MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); MatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); diff --git a/unittest/aba.cpp b/unittest/aba.cpp index a37589c8ec..9f2b0ef596 100644 --- a/unittest/aba.cpp +++ b/unittest/aba.cpp @@ -2,11 +2,6 @@ // Copyright (c) 2016-2020 CNRS INRIA // -#include "pinocchio/spatial/fwd.hpp" -#include "pinocchio/spatial/se3.hpp" -#include "pinocchio/multibody/visitor.hpp" -#include "pinocchio/multibody/model.hpp" -#include "pinocchio/multibody/data.hpp" #include "pinocchio/algorithm/aba.hpp" #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/jacobian.hpp" @@ -27,6 +22,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) template void test_joint_methods(const pinocchio::JointModelBase & jmodel) { + std::cout << "shortname: " << jmodel.shortname() << std::endl; typedef typename pinocchio::JointModelBase::JointDataDerived JointData; typedef typename JointModel::ConfigVector_t ConfigVector_t; typedef typename pinocchio::LieGroup::type LieGroupType; @@ -39,18 +35,22 @@ void test_joint_methods(const pinocchio::JointModelBase & jmodel) ConfigVector_t q = LieGroupType().randomConfiguration(ql,qu); pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Random().matrix()); pinocchio::Inertia::Matrix6 I_check = I; + const Eigen::VectorXd armature = Eigen::VectorXd::Random(jmodel.nv()) + Eigen::VectorXd::Ones(jmodel.nv()); jmodel.calc(jdata,q); - jmodel.calc_aba(jdata,I,true); + jmodel.calc_aba(jdata,armature,I,true); + std::cout << "armature: " << armature.transpose() << std::endl; Eigen::MatrixXd S = jdata.S.matrix(); Eigen::MatrixXd U_check = I_check*S; - Eigen::MatrixXd D_check = S.transpose()*U_check; - Eigen::MatrixXd Dinv_check = D_check.inverse(); + Eigen::MatrixXd StU_check = S.transpose()*U_check; StU_check.diagonal() += armature; + Eigen::MatrixXd Dinv_check = StU_check.inverse(); Eigen::MatrixXd UDinv_check = U_check*Dinv_check; - Eigen::MatrixXd update_check = U_check*Dinv_check*U_check.transpose(); + Eigen::MatrixXd update_check = UDinv_check*U_check.transpose(); I_check -= update_check; + std::cout << "I_check:\n" << I_check << std::endl; + std::cout << "I:\n" << I << std::endl; BOOST_CHECK(jdata.U.isApprox(U_check)); BOOST_CHECK(jdata.Dinv.isApprox(Dinv_check)); BOOST_CHECK(jdata.UDinv.isApprox(UDinv_check)); @@ -60,7 +60,7 @@ void test_joint_methods(const pinocchio::JointModelBase & jmodel) // where the correct result is exacly zero and isApprox would fail. // Only for this single case, we use the infinity norm of the difference if(jmodel.shortname() == "JointModelFreeFlyer") - BOOST_CHECK((I-I_check).lpNorm() < Eigen::NumTraits::dummy_precision()); + BOOST_CHECK((I-I_check).isZero()); else BOOST_CHECK(I.isApprox(I_check)); } @@ -106,7 +106,7 @@ struct TestJointMethods{ }; -BOOST_AUTO_TEST_CASE( test_joint_basic ) +BOOST_AUTO_TEST_CASE(test_joint_basic) { using namespace pinocchio; @@ -123,7 +123,7 @@ BOOST_AUTO_TEST_CASE( test_joint_basic ) boost::mpl::for_each(TestJointMethods()); } -BOOST_AUTO_TEST_CASE ( test_aba_simple ) +BOOST_AUTO_TEST_CASE(test_aba_simple) { using namespace Eigen; using namespace pinocchio; @@ -133,13 +133,16 @@ BOOST_AUTO_TEST_CASE ( test_aba_simple ) pinocchio::Data data(model); pinocchio::Data data_ref(model); - VectorXd q = VectorXd::Ones(model.nq); - q.segment<4>(3).normalize(); + model.lowerPositionLimit.head<7>().fill(-1.); + model.upperPositionLimit.head<7>().fill( 1.); + VectorXd q = randomConfiguration(model); + VectorXd v = VectorXd::Ones(model.nv); VectorXd tau = VectorXd::Zero(model.nv); VectorXd a = VectorXd::Ones(model.nv); tau = rnea(model, data_ref, q, v, a); + forwardKinematics(model, data_ref, q); aba(model, data, q, v, tau); for(size_t k = 1; k < (size_t)model.njoints; ++k) @@ -149,10 +152,22 @@ BOOST_AUTO_TEST_CASE ( test_aba_simple ) } BOOST_CHECK(data.ddq.isApprox(a, 1e-12)); + + // Test multiple calls + { + Data datas(model); + VectorXd a1 = aba(model,datas,q,v,tau); + VectorXd a2 = aba(model,datas,q,v,tau); + VectorXd a3 = aba(model,datas,q,v,tau); + + BOOST_CHECK(a1.isApprox(a2)); + BOOST_CHECK(a1.isApprox(a3)); + BOOST_CHECK(a2.isApprox(a3)); + } } -BOOST_AUTO_TEST_CASE ( test_aba_with_fext ) +BOOST_AUTO_TEST_CASE(test_aba_with_fext) { using namespace Eigen; using namespace pinocchio; @@ -161,8 +176,10 @@ BOOST_AUTO_TEST_CASE ( test_aba_with_fext ) pinocchio::Data data(model); - VectorXd q = VectorXd::Random(model.nq); - q.segment<4>(3).normalize(); + model.lowerPositionLimit.head<7>().fill(-1.); + model.upperPositionLimit.head<7>().fill( 1.); + VectorXd q = randomConfiguration(model); + VectorXd v = VectorXd::Random(model.nv); VectorXd a = VectorXd::Random(model.nv); @@ -174,7 +191,6 @@ BOOST_AUTO_TEST_CASE ( test_aba_with_fext ) data.M.triangularView() = data.M.transpose().triangularView(); - VectorXd tau = data.M * a + data.nle; Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv); for(Model::Index i=1;i<(Model::Index)model.njoints;++i) { @@ -185,9 +201,10 @@ BOOST_AUTO_TEST_CASE ( test_aba_with_fext ) aba(model, data, q, v, tau, fext); BOOST_CHECK(data.ddq.isApprox(a, 1e-12)); + } -BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea ) +BOOST_AUTO_TEST_CASE(test_aba_vs_rnea) { using namespace Eigen; using namespace pinocchio; @@ -197,7 +214,10 @@ BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea ) pinocchio::Data data(model); pinocchio::Data data_ref(model); - VectorXd q = VectorXd::Ones(model.nq); + model.lowerPositionLimit.head<7>().fill(-1.); + model.upperPositionLimit.head<7>().fill( 1.); + VectorXd q = randomConfiguration(model); + VectorXd v = VectorXd::Ones(model.nv); VectorXd tau = VectorXd::Zero(model.nv); VectorXd a = VectorXd::Ones(model.nv); @@ -212,13 +232,11 @@ BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea ) VectorXd tau_ref = rnea(model, data_ref, q, v, a); BOOST_CHECK(tau_ref.isApprox(tau, 1e-12)); - - BOOST_CHECK(data.ddq.isApprox(a, 1e-12)); } -BOOST_AUTO_TEST_CASE ( test_computeMinverse ) +BOOST_AUTO_TEST_CASE(test_computeMinverse) { using namespace Eigen; using namespace pinocchio; @@ -258,6 +276,46 @@ BOOST_AUTO_TEST_CASE ( test_computeMinverse ) } +BOOST_AUTO_TEST_CASE(test_computeMinverse_noupdate) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + buildModels::humanoidRandom(model); + model.gravity.setZero(); + + pinocchio::Data data(model); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + crba(model, data_ref, q); + data_ref.M.triangularView() + = data_ref.M.transpose().triangularView(); + MatrixXd Minv_ref(data_ref.M.inverse()); + + aba(model,data,q,v,tau); + computeMinverse(model, data, q); + BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>())); + + data.Minv.triangularView() + = data.Minv.transpose().triangularView(); + + BOOST_CHECK(data.Minv.isApprox(Minv_ref)); + + Data data_ref2(model); + computeMinverse(model,data_ref2,q); + data_ref2.Minv.triangularView() + = data_ref2.Minv.transpose().triangularView(); + BOOST_CHECK(data.Minv.isApprox(data_ref2.Minv)); + +} + BOOST_AUTO_TEST_CASE(test_multiple_calls) { using namespace Eigen; @@ -283,4 +341,22 @@ BOOST_AUTO_TEST_CASE(test_multiple_calls) BOOST_CHECK(data1.Minv.isApprox(data2.Minv)); } +BOOST_AUTO_TEST_CASE(test_roto_inertia_effects) +{ + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model); + model.armature = Eigen::VectorXd::Random(model.nv) + Eigen::VectorXd::Constant(model.nv,1.); + + pinocchio::Data data(model), data_ref(model); + + Eigen::VectorXd q = randomConfiguration(model); + crba(model,data_ref,q); + data_ref.M.triangularView() = data_ref.M.transpose().triangularView(); + + computeMinverse(model,data,q); + data.Minv.triangularView() = data.Minv.transpose().triangularView(); + + BOOST_CHECK((data.Minv*data_ref.M).isIdentity()); +} + BOOST_AUTO_TEST_SUITE_END () diff --git a/unittest/algorithm/CMakeLists.txt b/unittest/algorithm/CMakeLists.txt new file mode 100644 index 0000000000..674c9a177e --- /dev/null +++ b/unittest/algorithm/CMakeLists.txt @@ -0,0 +1 @@ +ADD_SUBDIRECTORY(utils) diff --git a/unittest/algorithm/utils/CMakeLists.txt b/unittest/algorithm/utils/CMakeLists.txt new file mode 100644 index 0000000000..ce0290012b --- /dev/null +++ b/unittest/algorithm/utils/CMakeLists.txt @@ -0,0 +1,2 @@ +ADD_PINOCCHIO_UNIT_TEST(motion) +ADD_PINOCCHIO_UNIT_TEST(force) diff --git a/unittest/algorithm/utils/force.cpp b/unittest/algorithm/utils/force.cpp new file mode 100644 index 0000000000..50fe61c9db --- /dev/null +++ b/unittest/algorithm/utils/force.cpp @@ -0,0 +1,59 @@ +// +// Copyright (c) 2020 INRIA +// + +#include + +#include +#include + +#include "pinocchio/algorithm/utils/force.hpp" + +using namespace pinocchio; + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +BOOST_AUTO_TEST_CASE(test_force) +{ + using namespace pinocchio; + + const SE3 placement = SE3::Random(); + const Force f_in = Force::Random(); + + // test case 1-2-3 + { + BOOST_CHECK(changeReferenceFrame(placement,f_in,LOCAL,LOCAL) == f_in); + BOOST_CHECK(changeReferenceFrame(placement,f_in,WORLD,WORLD) == f_in); + BOOST_CHECK(changeReferenceFrame(placement,f_in,LOCAL_WORLD_ALIGNED,LOCAL_WORLD_ALIGNED) == f_in); + } + + const ReferenceFrame all_choices[3] = {LOCAL, WORLD, LOCAL_WORLD_ALIGNED}; + for(int i = 0; i < 3; ++i) + for(int j = 0; j < 3; ++j) + BOOST_CHECK(changeReferenceFrame(SE3::Identity(),f_in,all_choices[i],all_choices[j]) == f_in); + + // LOCAL/WORLD and WORLD/LOCAL + { + Force f_sol_w = placement.act(f_in); + BOOST_CHECK(changeReferenceFrame(placement,f_in,LOCAL,WORLD) == f_sol_w); + BOOST_CHECK(changeReferenceFrame(placement,f_sol_w,WORLD,LOCAL).isApprox(f_in)); + } + + // LOCAL/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/LOCAL + { + const SE3 placement_LWA(placement.rotation(),SE3::Vector3::Zero()); + Force f_sol_lwa = placement_LWA.act(f_in); + BOOST_CHECK(changeReferenceFrame(placement,f_in,LOCAL,LOCAL_WORLD_ALIGNED) == f_sol_lwa); + BOOST_CHECK(changeReferenceFrame(placement,f_sol_lwa,LOCAL_WORLD_ALIGNED,LOCAL).isApprox(f_in)); + } + + // WORLD/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/WORLD + { + const SE3 placement_W(SE3::Matrix3::Identity(),placement.translation()); + Force f_sol_w = placement_W.act(f_in); + BOOST_CHECK(changeReferenceFrame(placement,f_in,LOCAL_WORLD_ALIGNED,WORLD) == f_sol_w); + BOOST_CHECK(changeReferenceFrame(placement,f_sol_w,WORLD,LOCAL_WORLD_ALIGNED).isApprox(f_in)); + } +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/algorithm/utils/motion.cpp b/unittest/algorithm/utils/motion.cpp new file mode 100644 index 0000000000..6f7754d3ed --- /dev/null +++ b/unittest/algorithm/utils/motion.cpp @@ -0,0 +1,60 @@ +// +// Copyright (c) 2020 INRIA +// + +#include + +#include +#include + +#include "pinocchio/algorithm/utils/motion.hpp" + +using namespace pinocchio; + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +BOOST_AUTO_TEST_CASE(test_motion) +{ + using namespace pinocchio; + + const SE3 placement = SE3::Random(); + const Motion m_in = Motion::Random(); + + // test case 1-2-3 + { + BOOST_CHECK(changeReferenceFrame(placement,m_in,LOCAL,LOCAL) == m_in); + BOOST_CHECK(changeReferenceFrame(placement,m_in,WORLD,WORLD) == m_in); + BOOST_CHECK(changeReferenceFrame(placement,m_in,LOCAL_WORLD_ALIGNED,LOCAL_WORLD_ALIGNED) == m_in); + } + + const ReferenceFrame all_choices[3] = {LOCAL, WORLD, LOCAL_WORLD_ALIGNED}; + for(int i = 0; i < 3; ++i) + for(int j = 0; j < 3; ++j) + BOOST_CHECK(changeReferenceFrame(SE3::Identity(),m_in,all_choices[i],all_choices[j]) == m_in); + + // LOCAL/WORLD and WORLD/LOCAL + { + Motion m_sol_w = placement.act(m_in); + BOOST_CHECK(changeReferenceFrame(placement,m_in,LOCAL,WORLD) == m_sol_w); + BOOST_CHECK(changeReferenceFrame(placement,m_sol_w,WORLD,LOCAL).isApprox(m_in)); + } + + // LOCAL/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/LOCAL + { + const SE3 placement_LWA(placement.rotation(),SE3::Vector3::Zero()); + Motion m_sol_lwa = placement_LWA.act(m_in); + BOOST_CHECK(changeReferenceFrame(placement,m_in,LOCAL,LOCAL_WORLD_ALIGNED) == m_sol_lwa); + BOOST_CHECK(changeReferenceFrame(placement,m_sol_lwa,LOCAL_WORLD_ALIGNED,LOCAL).isApprox(m_in)); + } + + // WORLD/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/WORLD + { + const SE3 placement_W(SE3::Matrix3::Identity(),placement.translation()); + Motion m_sol_w = placement_W.act(m_in); + BOOST_CHECK(changeReferenceFrame(placement,m_in,LOCAL_WORLD_ALIGNED,WORLD) == m_sol_w); + BOOST_CHECK(changeReferenceFrame(placement,m_sol_w,WORLD,LOCAL_WORLD_ALIGNED).isApprox(m_in)); + } +} + +BOOST_AUTO_TEST_SUITE_END() + diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index b8aac2ea16..1a60e5a8eb 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -3,9 +3,6 @@ // Copyright(c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#include -#include - #include "pinocchio/math/fwd.hpp" #include "pinocchio/multibody/joint/joints.hpp" #include "pinocchio/algorithm/rnea.hpp" @@ -14,6 +11,9 @@ #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/compute-all-terms.hpp" +#include +#include + using namespace pinocchio; template struct init; @@ -141,11 +141,11 @@ struct TestJointModelIsEqual : TestJointModel static void test(const JointModelBase & jmodel) { JointModel jmodel_copy = jmodel.derived(); - BOOST_CHECK(jmodel_copy == jmodel); + BOOST_CHECK(jmodel_copy == jmodel.derived()); JointModel jmodel_any; - BOOST_CHECK(jmodel_any != jmodel); - BOOST_CHECK(!jmodel_any.isEqual(jmodel)); + BOOST_CHECK(jmodel_any != jmodel.derived()); + BOOST_CHECK(!jmodel_any.isEqual(jmodel.derived())); } }; diff --git a/unittest/cartesian-product-liegroups.cpp b/unittest/cartesian-product-liegroups.cpp index 30b53bdea7..80c22bb396 100644 --- a/unittest/cartesian-product-liegroups.cpp +++ b/unittest/cartesian-product-liegroups.cpp @@ -10,6 +10,7 @@ #include "pinocchio/multibody/joint/joint-generic.hpp" +#include #include #include #include diff --git a/unittest/casadi-algo-derivatives.cpp b/unittest/casadi-algo-derivatives.cpp index d3d98e358a..0b85a202b9 100644 --- a/unittest/casadi-algo-derivatives.cpp +++ b/unittest/casadi-algo-derivatives.cpp @@ -3,6 +3,7 @@ // #include "pinocchio/autodiff/casadi.hpp" +#include "pinocchio/autodiff/casadi-algo.hpp" #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/rnea-derivatives.hpp" @@ -14,6 +15,7 @@ #include +#include #include #include @@ -60,7 +62,6 @@ BOOST_AUTO_TEST_CASE(test_integrate) casadi::SX cs_q_int(model.nq,1); pinocchio::casadi::copy(q_int_ad,cs_q_int); - std::cout << "cs_q_int:" << cs_q_int << std::endl; casadi::Function eval_integrate("eval_integrate", casadi::SXVector {cs_q,cs_v_int}, casadi::SXVector {cs_q_int}); @@ -76,7 +77,6 @@ BOOST_AUTO_TEST_CASE(test_integrate) ConfigVector q_plus(model.nq); pinocchio::integrate(model,q,TangentVector::Zero(model.nv),q_plus); - std::cout << "q_int_vec: " << q_int_vec.transpose() << std::endl; BOOST_CHECK(q_plus.isApprox(q_int_vec)); } @@ -153,7 +153,6 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives) // check return value casadi::DM tau_res = eval_rnea(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0]; - std::cout << "tau_res = " << tau_res << std::endl; Data::TangentVectorType tau_vec = Eigen::Map(static_cast< std::vector >(tau_res).data(),model.nv,1); BOOST_CHECK(data.tau.isApprox(tau_vec)); @@ -384,4 +383,190 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives) BOOST_CHECK(ddq_dtau_ref.isApprox(ddq_dtau_res_direct_map)); } +BOOST_AUTO_TEST_CASE(test_aba_casadi_algo) +{ + typedef double Scalar; + typedef pinocchio::ModelTpl Model; + typedef pinocchio::DataTpl Data; + typedef typename Model::ConfigVectorType ConfigVector; + typedef typename Model::TangentVectorType TangentVector; + + Model model; + pinocchio::buildModels::humanoidRandom(model); + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + pinocchio::Data data(model); + + ConfigVector q(model.nq); + q = pinocchio::randomConfiguration(model); + TangentVector v(TangentVector::Random(model.nv)); + TangentVector tau(TangentVector::Random(model.nv)); + + pinocchio::aba(model,data,q,v,tau); + pinocchio::computeABADerivatives(model,data,q,v,tau); + data.Minv.triangularView() + = data.Minv.transpose().triangularView(); + + pinocchio::casadi::AutoDiffABA ad_casadi(model); + ad_casadi.initLib(); + ad_casadi.loadLib(); + + ad_casadi.evalFunction(q,v,tau); + ad_casadi.evalJacobian(q,v,tau); + BOOST_CHECK(ad_casadi.ddq.isApprox(data.ddq)); + BOOST_CHECK(ad_casadi.ddq_dq.isApprox(data.ddq_dq)); + BOOST_CHECK(ad_casadi.ddq_dv.isApprox(data.ddq_dv)); + BOOST_CHECK(ad_casadi.ddq_dtau.isApprox(data.Minv)); +} + +BOOST_AUTO_TEST_CASE(test_aba_derivatives_casadi_algo) +{ + typedef double Scalar; + typedef pinocchio::ModelTpl Model; + typedef pinocchio::DataTpl Data; + typedef typename Model::ConfigVectorType ConfigVector; + typedef typename Model::TangentVectorType TangentVector; + + Model model; + pinocchio::buildModels::humanoidRandom(model); + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + pinocchio::Data data(model); + + ConfigVector q(model.nq); + q = pinocchio::randomConfiguration(model); + TangentVector v(TangentVector::Random(model.nv)); + TangentVector tau(TangentVector::Random(model.nv)); + + pinocchio::aba(model,data,q,v,tau); + pinocchio::computeABADerivatives(model,data,q,v,tau); + data.Minv.triangularView() + = data.Minv.transpose().triangularView(); + + pinocchio::casadi::AutoDiffABADerivatives ad_casadi(model); + ad_casadi.initLib(); + ad_casadi.loadLib(); + + ad_casadi.evalFunction(q,v,tau); + + BOOST_CHECK(ad_casadi.ddq.isApprox(data.ddq)); + BOOST_CHECK(ad_casadi.ddq_dq.isApprox(data.ddq_dq)); + BOOST_CHECK(ad_casadi.ddq_dv.isApprox(data.ddq_dv)); + BOOST_CHECK(ad_casadi.ddq_dtau.isApprox(data.Minv)); +} + +BOOST_AUTO_TEST_CASE(test_constraintDynamics_casadi_algo) +{ + typedef double Scalar; + typedef pinocchio::ModelTpl Model; + typedef pinocchio::DataTpl Data; + typedef typename Model::ConfigVectorType ConfigVector; + typedef typename Model::TangentVectorType TangentVector; + + const Scalar prec = Eigen::NumTraits::dummy_precision(); + + Model model; + pinocchio::buildModels::humanoidRandom(model); + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + pinocchio::Data data(model); + + const std::string RF = "rleg6_joint"; const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) contact_data; + + pinocchio::RigidConstraintModel ci_RF(pinocchio::CONTACT_3D,RF_id,pinocchio::LOCAL_WORLD_ALIGNED); + ci_RF.joint1_placement.setRandom(); + contact_models.push_back(ci_RF); contact_data.push_back(pinocchio::RigidConstraintData(ci_RF)); + + pinocchio::RigidConstraintModel ci_LF(pinocchio::CONTACT_6D,LF_id,pinocchio::LOCAL_WORLD_ALIGNED); + ci_LF.joint1_placement.setRandom(); + contact_models.push_back(ci_LF); contact_data.push_back(pinocchio::RigidConstraintData(ci_LF)); + const double mu0 = 0.; + ConfigVector q(model.nq); + q = pinocchio::randomConfiguration(model); + TangentVector v(TangentVector::Random(model.nv)); + TangentVector tau(TangentVector::Random(model.nv)); + + pinocchio::initConstraintDynamics(model,data,contact_models); + pinocchio::constraintDynamics(model,data,q,v,tau,contact_models,contact_data); + pinocchio::computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + pinocchio::casadi::AutoDiffConstraintDynamics ad_casadi(model, contact_models); + ad_casadi.initLib(); + ad_casadi.loadLib(); + + ad_casadi.evalFunction(q,v,tau); + BOOST_CHECK(ad_casadi.ddq.isApprox(data.ddq,1e1*prec)); + BOOST_CHECK(ad_casadi.lambda_c.isApprox(data.lambda_c,1e1*prec)); + ad_casadi.evalJacobian(q,v,tau); + + BOOST_CHECK(ad_casadi.ddq_dq.isApprox(data.ddq_dq,1e1*prec)); + BOOST_CHECK(ad_casadi.ddq_dv.isApprox(data.ddq_dv,1e1*prec)); + BOOST_CHECK(ad_casadi.ddq_dtau.isApprox(data.ddq_dtau,1e1*prec)); + BOOST_CHECK(ad_casadi.dlambda_dq.isApprox(data.dlambda_dq,1e1*prec)); + BOOST_CHECK(ad_casadi.dlambda_dv.isApprox(data.dlambda_dv,1e1*prec)); + BOOST_CHECK(ad_casadi.dlambda_dtau.isApprox(data.dlambda_dtau,1e1*prec)); + +} + +BOOST_AUTO_TEST_CASE(test_constraintDynamicsDerivatives_casadi_algo) +{ + typedef double Scalar; + typedef pinocchio::ModelTpl Model; + typedef pinocchio::DataTpl Data; + typedef typename Model::ConfigVectorType ConfigVector; + typedef typename Model::TangentVectorType TangentVector; + + const Scalar prec = Eigen::NumTraits::dummy_precision(); + + Model model; + pinocchio::buildModels::humanoidRandom(model); + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + pinocchio::Data data(model); + + const std::string RF = "rleg6_joint"; const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) contact_data; + + pinocchio::RigidConstraintModel ci_RF(pinocchio::CONTACT_3D,RF_id,pinocchio::LOCAL_WORLD_ALIGNED); + ci_RF.joint1_placement.setRandom(); + contact_models.push_back(ci_RF); contact_data.push_back(pinocchio::RigidConstraintData(ci_RF)); + + pinocchio::RigidConstraintModel ci_LF(pinocchio::CONTACT_6D,LF_id,pinocchio::LOCAL_WORLD_ALIGNED); + ci_LF.joint1_placement.setRandom(); + contact_models.push_back(ci_LF); contact_data.push_back(pinocchio::RigidConstraintData(ci_LF)); + const double mu0 = 0.; + ConfigVector q(model.nq); + q = pinocchio::randomConfiguration(model); + TangentVector v(TangentVector::Random(model.nv)); + TangentVector tau(TangentVector::Random(model.nv)); + + pinocchio::initConstraintDynamics(model,data,contact_models); + pinocchio::constraintDynamics(model,data,q,v,tau,contact_models,contact_data); + pinocchio::computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + pinocchio::casadi::AutoDiffConstraintDynamicsDerivatives ad_casadi(model, + contact_models); + + ad_casadi.initLib(); + ad_casadi.loadLib(); + + ad_casadi.evalFunction(q,v,tau); + BOOST_CHECK(ad_casadi.ddq.isApprox(data.ddq,1e1*prec)); + BOOST_CHECK(ad_casadi.lambda_c.isApprox(data.lambda_c,1e1*prec)); + BOOST_CHECK(ad_casadi.ddq_dq.isApprox(data.ddq_dq,1e1*prec)); + BOOST_CHECK(ad_casadi.ddq_dv.isApprox(data.ddq_dv,1e1*prec)); + BOOST_CHECK(ad_casadi.ddq_dtau.isApprox(data.ddq_dtau,1e1*prec)); + BOOST_CHECK(ad_casadi.dlambda_dq.isApprox(data.dlambda_dq,1e1*prec)); + BOOST_CHECK(ad_casadi.dlambda_dv.isApprox(data.dlambda_dv,1e1*prec)); + BOOST_CHECK(ad_casadi.dlambda_dtau.isApprox(data.dlambda_dtau,1e1*prec)); + +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/casadi-basic.cpp b/unittest/casadi-basic.cpp index 2cdcf50752..10bcb2b6df 100644 --- a/unittest/casadi-basic.cpp +++ b/unittest/casadi-basic.cpp @@ -1,16 +1,27 @@ // -// Copyright (c) 2019 INRIA +// Copyright (c) 2019-2020 INRIA // #include #include // to avoid C99 warnings +#include #include #include BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) +BOOST_AUTO_TEST_CASE(test_basic) +{ + casadi::SX a(1,1); + casadi::SX b; + + casadi::MX c; + + b = a(0,0); +} + BOOST_AUTO_TEST_CASE(test_eigen) { Eigen::Matrix A, B; diff --git a/unittest/casadi-joints.cpp b/unittest/casadi-joints.cpp index c4bac73881..fc530ccbe7 100644 --- a/unittest/casadi-joints.cpp +++ b/unittest/casadi-joints.cpp @@ -8,6 +8,7 @@ #include "pinocchio/multibody/liegroup/liegroup.hpp" #include "pinocchio/multibody/liegroup/liegroup-algo.hpp" +#include #include #include @@ -23,7 +24,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) typedef pinocchio::MotionTpl MotionAD; typedef pinocchio::SE3Tpl SE3; typedef pinocchio::MotionTpl Motion; - typedef pinocchio::ConstraintTpl ConstraintXd; + typedef pinocchio::JointMotionSubspaceTpl JointMotionSubspaceXd; typedef Eigen::Matrix VectorXAD; typedef Eigen::Matrix Vector6AD; @@ -101,7 +102,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) jmodel.calc(jdata,q,v); Motion m(jdata.v); - ConstraintXd Sref(jdata.S.matrix()); + JointMotionSubspaceXd Sref(jdata.S.matrix()); jmodel_ad.calc(jdata_ad,q_ad,v_ad); Vector6AD Y; @@ -127,7 +128,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) casadi::DMVector res_S = eval_S(casadi::DMVector {q_vec,v_vec}); std::cout << "res_S:" << res_S << std::endl; - ConstraintXd::DenseBase Sref_mat = Sref.matrix(); + JointMotionSubspaceXd::DenseBase Sref_mat = Sref.matrix(); for(Eigen::DenseIndex i = 0; i < 6; ++i) { @@ -303,7 +304,7 @@ struct TestADOnJoints typedef pinocchio::MotionTpl MotionAD; typedef pinocchio::SE3Tpl SE3; typedef pinocchio::MotionTpl Motion; - typedef pinocchio::ConstraintTpl ConstraintXd; + typedef pinocchio::JointMotionSubspaceTpl JointMotionSubspaceXd; typedef Eigen::Matrix VectorXAD; typedef Eigen::Matrix Vector6AD; @@ -387,7 +388,7 @@ struct TestADOnJoints jmodel.calc(jdata,q,v); Motion m(jdata_base.v()); - ConstraintXd Sref(jdata_base.S().matrix()); + JointMotionSubspaceXd Sref(jdata_base.S().matrix()); jmodel_ad.calc(jdata_ad,q_ad,v_ad); Vector6AD Y; @@ -413,7 +414,7 @@ struct TestADOnJoints casadi::DMVector res_S = eval_S(casadi::DMVector {q_vec,v_vec}); std::cout << "res_S:" << res_S << std::endl; - ConstraintXd::DenseBase Sref_mat = Sref.matrix(); + JointMotionSubspaceXd::DenseBase Sref_mat = Sref.matrix(); for(Eigen::DenseIndex i = 0; i < 6; ++i) { diff --git a/unittest/casadi-spatial.cpp b/unittest/casadi-spatial.cpp index b2727e0bf3..a5c35f7d0c 100644 --- a/unittest/casadi-spatial.cpp +++ b/unittest/casadi-spatial.cpp @@ -10,6 +10,7 @@ #include // to avoid C99 warnings +#include #include #include @@ -23,6 +24,15 @@ BOOST_AUTO_TEST_CASE(test_se3) SE3 M3 = M2 * M1; SE3 M1inv = M1.inverse(); + + ::casadi::SX trans; + pinocchio::casadi::copy(M1.translation(),trans); + + const Eigen::DenseIndex col = 0; + for(Eigen::DenseIndex k = 0; k < 3; ++k) + { + BOOST_CHECK(casadi::SX::is_equal(trans(k,col),M1.translation()[k])); + } } BOOST_AUTO_TEST_CASE(test_motion) diff --git a/unittest/centroidal-derivatives.cpp b/unittest/centroidal-derivatives.cpp index 52f9492a7d..a7ead0ea41 100644 --- a/unittest/centroidal-derivatives.cpp +++ b/unittest/centroidal-derivatives.cpp @@ -1,19 +1,17 @@ // -// Copyright (c) 2018-2019 INRIA +// Copyright (c) 2018-2020 INRIA // -#include "pinocchio/multibody/model.hpp" -#include "pinocchio/multibody/data.hpp" -#include "pinocchio/multibody/joint/joint-spherical.hpp" #include "pinocchio/algorithm/crba.hpp" +#include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/centroidal.hpp" #include "pinocchio/algorithm/centroidal-derivatives.hpp" #include "pinocchio/algorithm/rnea-derivatives.hpp" +#include "pinocchio/algorithm/aba-derivatives.hpp" #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/center-of-mass.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/parsers/sample-models.hpp" -#include "pinocchio/utils/timer.hpp" #include @@ -102,6 +100,7 @@ BOOST_AUTO_TEST_CASE(test_centroidal_derivatives) BOOST_CHECK(data.hg.isApprox(data_ref.hg)); BOOST_CHECK(data.dhg.isApprox(data_ref.dhg)); + BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); pinocchio::Data data_fd(model); @@ -177,9 +176,12 @@ BOOST_AUTO_TEST_CASE(test_centroidal_derivatives) BOOST_CHECK(data.J.isApprox(data_ref.J)); BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); + + pinocchio::computeCentroidalMap(model,data_ref,q); + BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); } -BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives) +BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives_fromRNEA) { pinocchio::Model model; pinocchio::buildModels::humanoidRandom(model); @@ -230,4 +232,64 @@ BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives) } +BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives_fromABA) +{ + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model); + pinocchio::Data data(model), data_ref(model), data_rnea(model); + + model.lowerPositionLimit.head<7>().fill(-1.); + model.upperPositionLimit.head<7>().fill( 1.); + + Eigen::VectorXd q = pinocchio::randomConfiguration(model); + Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); + Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv); + + pinocchio::Data::Matrix6x + dh_dq(6,model.nv), dhdot_dq(6,model.nv), dhdot_dv(6,model.nv), dhdot_da(6,model.nv); + pinocchio::Data::Matrix6x + dh_dq_ref(6,model.nv), dhdot_dq_ref(6,model.nv), dhdot_dv_ref(6,model.nv), dhdot_da_ref(6,model.nv); + + pinocchio::computeCentroidalDynamicsDerivatives(model,data_ref,q,v,a, + dh_dq_ref, dhdot_dq_ref,dhdot_dv_ref,dhdot_da_ref); + + Eigen::VectorXd tau = rnea(model,data_rnea,q,v,a); + pinocchio::computeABADerivatives(model,data,q,v,tau); + pinocchio::computeRNEADerivatives(model,data_rnea,q,v,a); + pinocchio::getCentroidalDynamicsDerivatives(model,data, + dh_dq, dhdot_dq,dhdot_dv,dhdot_da); + + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.dVdq.isApprox(data_rnea.dVdq)); + BOOST_CHECK(data.dAdq.isApprox(data_rnea.dAdq)); + BOOST_CHECK(data.dAdv.isApprox(data_rnea.dAdv)); + BOOST_CHECK(data.dFdq.isApprox(data_rnea.dFdq)); + BOOST_CHECK(data.dFdv.isApprox(data_rnea.dFdv)); + BOOST_CHECK(data.dFda.isApprox(data_rnea.dFda)); + + for(pinocchio::Model::JointIndex k = 1; k < (pinocchio::Model::JointIndex)model.njoints; ++k) + { + BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k])); + BOOST_CHECK(data.oh[k].isApprox(data_ref.oh[k])); + const pinocchio::Force & force_ref = data_ref.of[k]; + const pinocchio::Force gravity_contribution = data.oYcrb[k] * (-model.gravity); + const pinocchio::Force force = data.of[k] - gravity_contribution; + BOOST_CHECK(force.isApprox(force_ref)); + } + + BOOST_CHECK(data.com[0].isApprox(data_ref.com[0])); + + BOOST_CHECK(data.hg.isApprox(data_ref.hg)); + BOOST_CHECK(data.dhg.isApprox(data_ref.dhg)); + + BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdq)); + BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv)); + BOOST_CHECK(data.dFda.isApprox(data_ref.dFda)); + BOOST_CHECK(dh_dq.isApprox(dh_dq_ref)); + BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_ref)); + BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_ref)); + BOOST_CHECK(dhdot_da.isApprox(dhdot_da_ref)); + +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/centroidal.cpp b/unittest/centroidal.cpp index 026975a998..1eddc6f963 100644 --- a/unittest/centroidal.cpp +++ b/unittest/centroidal.cpp @@ -64,11 +64,19 @@ BOOST_AUTO_TEST_CASE(test_ccrba) Eigen::VectorXd q = randomConfiguration(model); Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv); - crba(model,data_ref,q); + pinocchio::deprecated::crba(model,data_ref,q); data_ref.M.triangularView() = data_ref.M.transpose().triangularView(); + data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]); - pinocchio::SE3 cMo (pinocchio::SE3::Matrix3::Identity(), -getComFromCrba(model, data_ref)); + pinocchio::Data data_ref_other(model); + pinocchio::crba(model,data_ref_other,q); + data_ref_other.M.triangularView() + = data_ref_other.M.transpose().triangularView(); + BOOST_CHECK(data_ref_other.M.isApprox(data_ref.M)); + + pinocchio::SE3 cMo(pinocchio::SE3::Matrix3::Identity(), -data_ref.Ycrb[0].lever()); + BOOST_CHECK(data_ref.Ycrb[0].isApprox(data_ref_other.oYcrb[0])); ccrba(model, data, q, v); BOOST_CHECK(data.com[0].isApprox(-cMo.translation(),1e-12)); @@ -124,11 +132,12 @@ BOOST_AUTO_TEST_CASE(test_dccrb) const Eigen::VectorXd g = rnea(model,data_ref,q,0*v,0*a); rnea(model,data_ref,q,v,a); - crba(model,data_ref,q); + pinocchio::deprecated::crba(model,data_ref,q); data_ref.M.triangularView() = data_ref.M.transpose().triangularView(); SE3 cMo(SE3::Identity()); - cMo.translation() = -getComFromCrba(model, data_ref); + data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]); + cMo.translation() = -data_ref.Ycrb[0].lever(); SE3 oM1 (data_ref.liMi[1]); SE3 cM1 (cMo * oM1); @@ -143,6 +152,7 @@ BOOST_AUTO_TEST_CASE(test_dccrb) BOOST_CHECK(data.hg.isApprox(data_ref.hg)); centerOfMass(model,data_ref,q,v,a); + BOOST_CHECK(data_ref.com[0].isApprox(data_ref.Ycrb[0].lever())); BOOST_CHECK(data_ref.vcom[0].isApprox(data.hg.linear()/data_ref.M(0,0))); BOOST_CHECK(data_ref.vcom[0].isApprox(data.vcom[0])); BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear()/data_ref.M(0,0))); @@ -163,8 +173,8 @@ BOOST_AUTO_TEST_CASE(test_dccrb) q_plus = integrate(model,q,alpha*v); forwardKinematics(model,data_ref,q); - crba(model,data_ref,q); - crba(model,data_ref_plus,q_plus); + pinocchio::deprecated::crba(model,data_ref,q); + pinocchio::deprecated::crba(model,data_ref_plus,q_plus); forwardKinematics(model,data_ref_plus,q_plus); dccrba(model,data,q,v); @@ -182,7 +192,7 @@ BOOST_AUTO_TEST_CASE(test_dccrb) SE3 oMc_ref(SE3::Identity()); oMc_ref.translation() = data_ref.com[0]; const Data::Matrix6x Ag_ref = oMc_ref.toDualActionMatrix() * data_ref.Ag; - crba(model,data_ref,q); + pinocchio::deprecated::crba(model,data_ref,q); const Data::Matrix6x Ag_ref_from_M = data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>(); const double alpha = 1e-8; @@ -192,7 +202,7 @@ BOOST_AUTO_TEST_CASE(test_dccrb) SE3 oMc_ref_plus(SE3::Identity()); oMc_ref_plus.translation() = data_ref.com[0]; const Data::Matrix6x Ag_plus_ref = oMc_ref_plus.toDualActionMatrix() * data_ref.Ag; - crba(model,data_ref,q_plus); + pinocchio::deprecated::crba(model,data_ref,q_plus); const Data::Matrix6x Ag_plus_ref_from_M = data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>(); const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref)/alpha; const Data::Matrix6x dAg_ref_from_M = (Ag_plus_ref_from_M - Ag_ref_from_M)/alpha; diff --git a/unittest/cholesky.cpp b/unittest/cholesky.cpp index dd3a6c4a74..71adc665f7 100644 --- a/unittest/cholesky.cpp +++ b/unittest/cholesky.cpp @@ -247,15 +247,11 @@ BOOST_AUTO_TEST_CASE ( test_timings ) cholesky::Uiv(model,data,Ui_v_unit_ref); Ui_v_unit_ref.array() *= data.Dinv.array(); cholesky::Utiv(model,data,Ui_v_unit_ref); - - BOOST_CHECK(Ui_v_unit.isApprox(Ui_v_unit_ref)); + BOOST_CHECK(Ui_v_unit.head(k+1).isApprox(Ui_v_unit_ref.head(k+1))); Ui_v_unit_ref = v_unit; cholesky::solve(model,data,Ui_v_unit_ref); - BOOST_CHECK(Ui_v_unit.isApprox(Ui_v_unit_ref)); - -// std::cout << "Ui_v_unit : " << Ui_v_unit.transpose() << std::endl; -// std::cout << "Ui_v_unit_ref : " << Ui_v_unit_ref.transpose() << std::endl << std::endl; + BOOST_CHECK(Ui_v_unit.head(k+1).isApprox(Ui_v_unit_ref.head(k+1))); } MatrixXd Minv(model.nv,model.nv); diff --git a/unittest/classic-acceleration.cpp b/unittest/classic-acceleration.cpp new file mode 100644 index 0000000000..fce7978813 --- /dev/null +++ b/unittest/classic-acceleration.cpp @@ -0,0 +1,147 @@ +// +// Copyright(c) 2019 INRIA +// + +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/multibody/data.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/parsers/sample-models.hpp" + +#include "pinocchio/spatial/classic-acceleration.hpp" + +#include + +#include +#include + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +BOOST_AUTO_TEST_CASE(test_classic_acceleration) +{ + using namespace Eigen; + using namespace pinocchio; + + static const int num_tests = 1e2; + + Model model; + buildModels::humanoidRandom(model); + + model.upperPositionLimit.head<3>().fill(100); + model.upperPositionLimit.segment<4>(3).setOnes(); + model.lowerPositionLimit.head<7>() = - model.upperPositionLimit.head<7>(); + + VectorXd q(model.nq); + q = randomConfiguration(model); + VectorXd v(VectorXd::Random(model.nv)); + VectorXd a(VectorXd::Zero(model.nv)); + + Data data_ref(model), data(model); + + const std::string RF_joint_name = "rleg6_joint"; + const Model::JointIndex RF_joint_id = model.getJointId(RF_joint_name); + + for(int k = 0; k < num_tests; ++k) + { + q = randomConfiguration(model); + v = VectorXd::Random(model.nv); + + forwardKinematics(model,data,q,v,a); + const SE3 RF_world_transf = SE3(data.oMi[RF_joint_id].rotation(),SE3::Vector3::Zero()); + + const Motion RF_v_global = RF_world_transf.act(data.v[RF_joint_id]); + const Motion RF_a_global = RF_world_transf.act(data.a[RF_joint_id]); + const Motion::Vector3 classic_acc = classicAcceleration(RF_v_global, RF_a_global); + + + Motion::Vector3 classic_acc_other_signature; + classicAcceleration(RF_v_global, RF_a_global, classic_acc_other_signature); + BOOST_CHECK(classic_acc_other_signature.isApprox(classic_acc)); + + // Computes with finite differences + const double eps = 1e-4; + const double eps2 = eps * eps; + forwardKinematics(model,data_ref,q); + const SE3::Vector3 pos = data_ref.oMi[RF_joint_id].translation(); + + VectorXd v_plus = v + eps * a; + VectorXd q_plus = integrate(model,q,v*eps + a*eps2/2.); + forwardKinematics(model,data_ref,q_plus); + const SE3::Vector3 pos_plus = data_ref.oMi[RF_joint_id].translation(); + + VectorXd v_minus = v - eps * a; + VectorXd q_minus = integrate(model,q,-v*eps - a*eps2/2.); + forwardKinematics(model,data_ref,q_minus); + const SE3::Vector3 pos_minus = data_ref.oMi[RF_joint_id].translation(); + + const SE3::Vector3 classic_acc_ref = (pos_plus + pos_minus - 2.*pos) / eps2; + + BOOST_CHECK(classic_acc.isApprox(classic_acc_ref,math::sqrt(eps))); + } +} + +BOOST_AUTO_TEST_CASE(test_classic_acceleration_with_placement) +{ + using namespace Eigen; + using namespace pinocchio; + + static const int num_tests = 1e2; + + Model model; + buildModels::humanoidRandom(model); + + model.upperPositionLimit.head<3>().fill(1); + model.upperPositionLimit.segment<4>(3).setOnes(); + model.lowerPositionLimit.head<7>() = - model.upperPositionLimit.head<7>(); + + VectorXd q(model.nq); + q = randomConfiguration(model); + VectorXd v(VectorXd::Random(model.nv)); + VectorXd a(VectorXd::Zero(model.nv)); + + Data data_ref(model), data(model); + + const std::string RF_joint_name = "rleg6_joint"; + const Model::JointIndex RF_joint_id = model.getJointId(RF_joint_name); + + for(int k = 0; k < num_tests; ++k) + { + q = randomConfiguration(model); + v = VectorXd::Random(model.nv); + + forwardKinematics(model,data,q,v,a); + + const SE3 RF_world_transf = SE3(data.oMi[RF_joint_id].rotation(), + SE3::Vector3::Zero()); + + const Motion RF_v_global = RF_world_transf.act(data.v[RF_joint_id]); + const Motion RF_a_global = RF_world_transf.act(data.a[RF_joint_id]); + const Motion::Vector3 RF_classic_acc_ref = classicAcceleration(RF_v_global, RF_a_global); + + const SE3 identity_placement = SE3::Identity(); + Motion::Vector3 RF_classic_acc = classicAcceleration(RF_v_global, RF_a_global, identity_placement); + + BOOST_CHECK(RF_classic_acc.isApprox(RF_classic_acc_ref)); + + const SE3 random_placement = SE3::Random(); + + const Motion & v_A = data.v[RF_joint_id]; + const Motion & a_A = data.a[RF_joint_id]; + + const Motion v_B = random_placement.actInv(data.v[RF_joint_id]); + const Motion a_B = random_placement.actInv(data.a[RF_joint_id]); + + Motion::Vector3 classic_acc_B_ref = classicAcceleration(v_B, a_B); + Motion::Vector3 classic_acc_B = classicAcceleration(v_A, a_A, random_placement); + + BOOST_CHECK(classic_acc_B.isApprox(classic_acc_B_ref)); + + // Check other signature + Motion::Vector3 classic_acc_B_other_signature; + classicAcceleration(v_A, a_A, random_placement, classic_acc_B_other_signature); + + BOOST_CHECK(classic_acc_B_other_signature.isApprox(classic_acc_B)); + } +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/closed-loop-dynamics.cpp b/unittest/closed-loop-dynamics.cpp new file mode 100644 index 0000000000..2c57ccd861 --- /dev/null +++ b/unittest/closed-loop-dynamics.cpp @@ -0,0 +1,173 @@ +// +// Copyright (c) 2020 INRIA +// + +#include "pinocchio/algorithm/aba.hpp" +#include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/centroidal.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/contact-info.hpp" +#include "pinocchio/algorithm/constrained-dynamics.hpp" +#include "pinocchio/algorithm/contact-dynamics.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/parsers/sample-models.hpp" +#include "pinocchio/utils/timer.hpp" +#include "pinocchio/spatial/classic-acceleration.hpp" + +#include + +#include +#include + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +using namespace pinocchio; +using namespace Eigen; + +BOOST_AUTO_TEST_CASE(closed_loop_constraint_6D_LOCAL) +{ + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + + const VectorXd q = randomConfiguration(model); + const VectorXd v = VectorXd::Random(model.nv); + const VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + + RigidConstraintModel ci_RF_LF(CONTACT_6D,model.getJointId(RF),model.getJointId(LF),LOCAL); + ci_RF_LF.joint1_placement.setRandom(); + ci_RF_LF.joint2_placement.setRandom(); + contact_models.push_back(ci_RF_LF); + contact_datas.push_back(RigidConstraintData(ci_RF_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + Data::Matrix6x J_RF_local(6,model.nv), J_LF_local(6,model.nv); + J_RF_local.setZero(); J_LF_local.setZero(); + getJointJacobian(model,data_ref,model.getJointId(RF),LOCAL,J_RF_local); + getJointJacobian(model,data_ref,model.getJointId(LF),LOCAL,J_LF_local); + + const SE3 c1Mc2_ref = data_ref.oMi[ci_RF_LF.joint1_id].actInv(data_ref.oMi[ci_RF_LF.joint2_id]); + + J_ref = J_RF_local - c1Mc2_ref.toActionMatrix()*J_LF_local; + + Eigen::VectorXd rhs_ref(constraint_dim); + + rhs_ref.segment<6>(0) = (data_ref.a[ci_RF_LF.joint1_id] - c1Mc2_ref.act(data_ref.a[ci_RF_LF.joint2_id])).toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + forwardDynamics(model,data_ref,q,v,tau,J_ref,rhs_ref,mu0); + forwardKinematics(model,data_ref,q,v,data_ref.ddq); + + BOOST_CHECK((J_ref*data_ref.ddq+rhs_ref).isZero()); + + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); + + BOOST_CHECK((J_ref*data.ddq+rhs_ref).isZero()); + + BOOST_CHECK(data_ref.ddq.isApprox(data.ddq)); +} + +BOOST_AUTO_TEST_CASE(closed_loop_constraint_6D_LOCAL_WORLD_ALIGNED) +{ + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + + const VectorXd q = randomConfiguration(model); + const VectorXd v = VectorXd::Random(model.nv); + const VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + + RigidConstraintModel ci_RF_LF(CONTACT_6D,model.getJointId(RF),model.getJointId(LF),LOCAL_WORLD_ALIGNED); + ci_RF_LF.joint1_placement.setRandom(); + ci_RF_LF.joint2_placement.setRandom(); + contact_models.push_back(ci_RF_LF); + contact_datas.push_back(RigidConstraintData(ci_RF_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + Data::Matrix6x J_RF_local(6,model.nv), J_LF_local(6,model.nv); + J_RF_local.setZero(); J_LF_local.setZero(); + getJointJacobian(model,data_ref,model.getJointId(RF),LOCAL,J_RF_local); + getJointJacobian(model,data_ref,model.getJointId(LF),LOCAL,J_LF_local); + + const SE3 c1Mc2_ref = data_ref.oMi[ci_RF_LF.joint1_id].actInv(data_ref.oMi[ci_RF_LF.joint2_id]); + + + const SE3 rotate(data.oMi[ci_RF_LF.joint1_id].rotation(),SE3::Vector3::Identity()); + J_ref = rotate.toActionMatrix()*(J_RF_local - c1Mc2_ref.toActionMatrix()*J_LF_local); + + Eigen::VectorXd rhs_ref(constraint_dim); + + rhs_ref.segment<6>(0) = rotate.act(data_ref.a[ci_RF_LF.joint1_id] - c1Mc2_ref.act(data_ref.a[ci_RF_LF.joint2_id])).toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + forwardDynamics(model,data_ref,q,v,tau,J_ref,rhs_ref,mu0); + forwardKinematics(model,data_ref,q,v,data_ref.ddq); + + BOOST_CHECK((J_ref*data_ref.ddq+rhs_ref).isZero()); + + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); + + BOOST_CHECK((J_ref*data.ddq+rhs_ref).isZero()); + + BOOST_CHECK(data_ref.ddq.isApprox(data.ddq)); +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/com.cpp b/unittest/com.cpp index 4254833c19..3a9d284f12 100644 --- a/unittest/com.cpp +++ b/unittest/com.cpp @@ -213,10 +213,10 @@ BOOST_AUTO_TEST_CASE(test_subtree_com_jacobian) jacobianCenterOfMass(model,data_ref,q,true); centerOfMass(model, data, q, v); - Data::Matrix3x Jcom(3,model.nv); Jcom.setZero(); - jacobianSubtreeCenterOfMass(model, data, 0, Jcom); + Data::Matrix3x Jcom1(3,model.nv); Jcom1.setZero(); + jacobianSubtreeCenterOfMass(model, data, 0, Jcom1); - BOOST_CHECK(Jcom.isApprox(data_ref.Jcom)); + BOOST_CHECK(Jcom1.isApprox(data_ref.Jcom)); centerOfMass(model, data_ref, q, v, true); computeJointJacobians(model, data_ref, q); @@ -226,6 +226,7 @@ BOOST_AUTO_TEST_CASE(test_subtree_com_jacobian) jacobianCenterOfMass(model,data_extracted,q); // Get subtree jacobian and check that it is consistent with the com velocity + Data::Matrix3x Jcom(3,model.nv); Jcom.setZero(); for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; joint_id++) { SE3::Vector3 subtreeComVelocityInWorld_ref = data_ref.oMi[joint_id].rotation() * data_ref.vcom[joint_id]; @@ -268,6 +269,19 @@ BOOST_AUTO_TEST_CASE(test_subtree_com_jacobian) BOOST_CHECK(data.com[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.com[joint_id]))); BOOST_CHECK(subtreeComVelocityInWorld.isApprox(subtreeComVelocityInWorld_ref)); } + + // Other signatures + Data data_other_signature(model); + Data::Matrix3x Jcom2(3,model.nv); Jcom2.setZero(); + jacobianSubtreeCenterOfMass(model, data_other_signature, q, 0, Jcom2); + + BOOST_CHECK(Jcom2.isApprox(Jcom1)); + + jacobianCenterOfMass(model,data,q); + Data::Matrix3x Jcom3(3,model.nv); Jcom3.setZero(); + getJacobianSubtreeCenterOfMass(model, data, 0, Jcom3); + + BOOST_CHECK(Jcom3.isApprox(Jcom1)); } BOOST_AUTO_TEST_SUITE_END () diff --git a/unittest/constrained-dynamics-derivatives.cpp b/unittest/constrained-dynamics-derivatives.cpp new file mode 100644 index 0000000000..75fc5b381c --- /dev/null +++ b/unittest/constrained-dynamics-derivatives.cpp @@ -0,0 +1,328 @@ +// +// Copyright (c) 2019 INRIA +// + +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/rnea-derivatives.hpp" +#include "pinocchio/algorithm/kinematics-derivatives.hpp" +#include "pinocchio/algorithm/contact-dynamics.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/parsers/sample-models.hpp" + +#include + +#include +#include + +BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) + +using namespace Eigen; +using namespace pinocchio; + +BOOST_AUTO_TEST_CASE ( test_FD_with_contact_cst_gamma ) +{ + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_check(model); + + VectorXd q = VectorXd::Ones(model.nq); + normalize(model,q); + + computeJointJacobians(model, data, q); + + VectorXd v = VectorXd::Ones(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); +// const std::string LF = "lleg6_joint"; +// const Model::JointIndex LF_id = model.getJointId(LF); + + Data::Matrix6x J_RF(6,model.nv); J_RF.setZero(); + getJointJacobian(model, data, RF_id, LOCAL, J_RF); + Motion::Vector6 gamma_RF; gamma_RF.setZero(); + forwardKinematics(model,data,q,v,VectorXd::Zero(model.nv)); + gamma_RF += data.a[RF_id].toVector(); // Jdot * qdot + + forwardDynamics(model, data, q, v, tau, J_RF, gamma_RF); + VectorXd ddq_ref = data.ddq; + Force::Vector6 contact_force_ref = data.lambda_c; + + container::aligned_vector fext((size_t)model.njoints,Force::Zero()); + fext[RF_id] = ForceRef(contact_force_ref); + + // check call to RNEA + rnea(model,data_check,q,v,ddq_ref,fext); + + BOOST_CHECK(data_check.tau.isApprox(tau)); + forwardKinematics(model,data_check,q,VectorXd::Zero(model.nv),ddq_ref); + BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF)); + + Data data_fd(model); + VectorXd q_plus(model.nq); + VectorXd v_eps(model.nv); v_eps.setZero(); + VectorXd v_plus(v); + VectorXd tau_plus(tau); + const double eps = 1e-8; + + // check: dddq_dtau and dlambda_dtau + MatrixXd dddq_dtau(model.nv,model.nv); + Data::Matrix6x dlambda_dtau(6,model.nv); + + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += eps; + forwardDynamics(model, data_fd, q, v, tau_plus, J_RF, gamma_RF); + + const Data::TangentVectorType & ddq_plus = data_fd.ddq; + Force::Vector6 contact_force_plus = data_fd.lambda_c; + + dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps; + dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps; + + tau_plus[k] -= eps; + } + + MatrixXd A(model.nv+6,model.nv+6); + data.M.transpose().triangularView() = data.M.triangularView(); + A.topLeftCorner(model.nv,model.nv) = data.M; + A.bottomLeftCorner(6, model.nv) = J_RF; + A.topRightCorner(model.nv, 6) = J_RF.transpose(); + A.bottomRightCorner(6,6).setZero(); + + MatrixXd Ainv = A.inverse(); + BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps))); + BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps))); + + // check: dddq_dv and dlambda_dv + MatrixXd dddq_dv(model.nv,model.nv); + Data::Matrix6x dlambda_dv(6,model.nv); + + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += eps; + forwardDynamics(model, data_fd, q, v_plus, tau, J_RF, gamma_RF); + + const Data::TangentVectorType & ddq_plus = data_fd.ddq; + Force::Vector6 contact_force_plus = data_fd.lambda_c; + + dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps; + dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps; + + v_plus[k] -= eps; + } + + computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv)); + MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv; + MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv; + + BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps))); + BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps))); + + MatrixXd dddq_dq(model.nv,model.nv); + Data::Matrix6x dlambda_dq(6,model.nv); + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] = eps; + q_plus = integrate(model,q,v_eps); + computeJointJacobians(model, data_fd, q_plus); + getJointJacobian(model, data_fd, RF_id, LOCAL, J_RF); + forwardDynamics(model, data_fd, q_plus, v, tau, J_RF, gamma_RF); + + const Data::TangentVectorType & ddq_plus = data_fd.ddq; + Force::Vector6 contact_force_plus = data_fd.lambda_c; + + dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps; + dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps; + + v_eps[k] = 0.; + } + + computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext); + Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv); + v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); + Data data_kin(model); + computeForwardKinematicsDerivatives(model,data_kin,q,VectorXd::Zero(model.nv),ddq_ref); + getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); + + MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq; + dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq; + + MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq; + dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq; + + BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps))); + BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps))); + +} + +template +VectorXd constraintDynamics(const Model & model, Data & data, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v, + const Eigen::MatrixBase & tau, + const Model::JointIndex id) +{ + computeJointJacobians(model, data, q); + Data::Matrix6x J(6,model.nv); J.setZero(); + + getJointJacobian(model, data, id, LOCAL, J); + Motion::Vector6 gamma; + forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv)); + gamma = data.a[id].toVector(); + + forwardDynamics(model, data, q, v, tau, J, gamma); + VectorXd res(VectorXd::Zero(model.nv+6)); + + res.head(model.nv) = data.ddq; + res.tail(6) = data.lambda_c; + + return res; +} + +BOOST_AUTO_TEST_CASE ( test_FD_with_contact_varying_gamma ) +{ + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_check(model); + + VectorXd q = VectorXd::Ones(model.nq); + normalize(model,q); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + + Data::Matrix6x J_RF(6,model.nv); J_RF.setZero(); + computeJointJacobians(model, data, q); + getJointJacobian(model, data, RF_id, LOCAL, J_RF); + Motion::Vector6 gamma_RF; gamma_RF.setZero(); + + VectorXd x_ref = constraintDynamics(model,data,q,v,tau,RF_id); + VectorXd ddq_ref = x_ref.head(model.nv); + Force::Vector6 contact_force_ref = x_ref.tail(6); + + container::aligned_vector fext((size_t)model.njoints,Force::Zero()); + fext[RF_id] = ForceRef(contact_force_ref); + + // check call to RNEA + rnea(model,data_check,q,v,ddq_ref,fext); + + BOOST_CHECK(data_check.tau.isApprox(tau)); + forwardKinematics(model,data_check,q,v,ddq_ref); + BOOST_CHECK(data_check.a[RF_id].toVector().isZero()); + + Data data_fd(model); + VectorXd q_plus(model.nq); + VectorXd v_eps(model.nv); v_eps.setZero(); + VectorXd v_plus(v); + VectorXd tau_plus(tau); + VectorXd x_plus(model.nv + 6); + const double eps = 1e-8; + + // check: dddq_dtau and dlambda_dtau + MatrixXd dddq_dtau(model.nv,model.nv); + Data::Matrix6x dlambda_dtau(6,model.nv); + + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += eps; + x_plus = constraintDynamics(model,data,q,v,tau_plus,RF_id); + + const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); + Force::Vector6 contact_force_plus = x_plus.tail(6); + + dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps; + dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps; + + tau_plus[k] -= eps; + } + + MatrixXd A(model.nv+6,model.nv+6); + data.M.transpose().triangularView() = data.M.triangularView(); + A.topLeftCorner(model.nv,model.nv) = data.M; + A.bottomLeftCorner(6, model.nv) = J_RF; + A.topRightCorner(model.nv, 6) = J_RF.transpose(); + A.bottomRightCorner(6,6).setZero(); + + MatrixXd Ainv = A.inverse(); + BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps))); + BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps))); + + // check: dddq_dv and dlambda_dv + MatrixXd dddq_dv(model.nv,model.nv); + Data::Matrix6x dlambda_dv(6,model.nv); + + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += eps; + x_plus = constraintDynamics(model,data,q,v_plus,tau,RF_id); + + const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); + Force::Vector6 contact_force_plus = x_plus.tail(6); + + dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps; + dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps; + + v_plus[k] -= eps; + } + + + computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv)); + Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv); + v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); + Data data_kin(model); + computeForwardKinematicsDerivatives(model,data_kin,q,v,VectorXd::Zero(model.nv)); + getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); + + MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv; + dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv; + MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv; + dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv; + + BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps))); + BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps))); + + + MatrixXd dddq_dq(model.nv,model.nv); + Data::Matrix6x dlambda_dq(6,model.nv); + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] = eps; + q_plus = integrate(model,q,v_eps); + + x_plus = constraintDynamics(model,data,q_plus,v,tau,RF_id); + + const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); + Force::Vector6 contact_force_plus = x_plus.tail(6); + + dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps; + dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps; + + v_eps[k] = 0.; + } + + computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext); + v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); + computeForwardKinematicsDerivatives(model,data_kin,q,v,ddq_ref); + getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); + + MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq; + dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq; + + BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps))); + + MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq; + dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq; + + BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps))); + +} + +BOOST_AUTO_TEST_SUITE_END () + diff --git a/unittest/constrained-dynamics.cpp b/unittest/constrained-dynamics.cpp new file mode 100644 index 0000000000..87c9360b01 --- /dev/null +++ b/unittest/constrained-dynamics.cpp @@ -0,0 +1,1413 @@ +// +// Copyright (c) 2019-2020 INRIA +// + +#include "pinocchio/algorithm/aba.hpp" +#include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/centroidal.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/contact-info.hpp" +#include "pinocchio/algorithm/compute-all-terms.hpp" +#include "pinocchio/algorithm/constrained-dynamics.hpp" +#include "pinocchio/algorithm/contact-dynamics.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/parsers/sample-models.hpp" +#include "pinocchio/utils/timer.hpp" +#include "pinocchio/spatial/classic-acceleration.hpp" + +#include + +#include +#include + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +BOOST_AUTO_TEST_CASE(contact_models) +{ + using namespace pinocchio; + + // Check default constructor + RigidConstraintModel cmodel1; + BOOST_CHECK(cmodel1.type == CONTACT_UNDEFINED); + BOOST_CHECK(cmodel1.size() == 0); + + // Check complete constructor + const SE3 M(SE3::Random()); + RigidConstraintModel cmodel2(CONTACT_3D,0,M); + BOOST_CHECK(cmodel2.type == CONTACT_3D); + BOOST_CHECK(cmodel2.joint1_id == 0); + BOOST_CHECK(cmodel2.joint1_placement.isApprox(M)); + BOOST_CHECK(cmodel2.size() == 3); + + // Check contructor with two arguments + RigidConstraintModel cmodel2prime(CONTACT_3D,0); + BOOST_CHECK(cmodel2prime.type == CONTACT_3D); + BOOST_CHECK(cmodel2prime.joint1_id == 0); + BOOST_CHECK(cmodel2prime.joint1_placement.isIdentity()); + BOOST_CHECK(cmodel2prime.size() == 3); + + // Check default copy constructor + RigidConstraintModel cmodel3(cmodel2); + BOOST_CHECK(cmodel3 == cmodel2); + + // Check complete constructor 6D + RigidConstraintModel cmodel4(CONTACT_6D,0); + BOOST_CHECK(cmodel4.type == CONTACT_6D); + BOOST_CHECK(cmodel4.joint1_id == 0); + BOOST_CHECK(cmodel4.joint1_placement.isIdentity()); + BOOST_CHECK(cmodel4.size() == 6); +} + +/// \brief Computes motions in the world frame +pinocchio::Motion computeAcceleration(const pinocchio::Model & model, + pinocchio::Data & data, + const pinocchio::JointIndex & joint_id, + pinocchio::ReferenceFrame reference_frame, + const pinocchio::ContactType type, + const pinocchio::SE3 & placement = pinocchio::SE3::Identity()) +{ + PINOCCHIO_UNUSED_VARIABLE(model); + using namespace pinocchio; + Motion res(Motion::Zero()); + + const Data::SE3 & oMi = data.oMi[joint_id]; + const Data::SE3 & iMc = placement; + const Data::SE3 oMc = oMi * iMc; + + const Motion ov = oMi.act(data.v[joint_id]); + const Motion oa = oMi.act(data.a[joint_id]); + + switch (reference_frame) + { + case WORLD: + if(type == CONTACT_3D) + classicAcceleration(ov,oa,res.linear()); + else + res.linear() = oa.linear(); + res.angular() = oa.angular(); + break; + case LOCAL_WORLD_ALIGNED: + if(type == CONTACT_3D) + res.linear() = oMc.rotation() * classicAcceleration(data.v[joint_id],data.a[joint_id],iMc); + else + res.linear() = oMc.rotation() * (iMc.actInv(data.a[joint_id])).linear(); + res.angular() = oMi.rotation() * data.a[joint_id].angular(); + break; + case LOCAL: + if(type == CONTACT_3D) + classicAcceleration(data.v[joint_id],data.a[joint_id],iMc,res.linear()); + else + res.linear() = (iMc.actInv(data.a[joint_id])).linear(); + res.angular() = iMc.rotation().transpose() * data.a[joint_id].angular(); + break; + default: + break; + } + + return res; +} + +BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_empty) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + // const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) empty_contact_datas; + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv,model.nv); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + + initConstraintDynamics(model,data,empty_contact_models); + constraintDynamics(model,data,q,v,tau,empty_contact_models,empty_contact_datas,prox_settings); + + data.M.triangularView() = + data.M.transpose().triangularView(); + + Data data_ag(model); + ccrba(model,data_ag,q,v); + + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.M.isApprox(data_ref.M)); + BOOST_CHECK(data.Ag.isApprox(data_ag.Ag)); + BOOST_CHECK(data.nle.isApprox(data_ref.nle)); + + for(Model::JointIndex k = 1; k < model.joints.size(); ++k) + { + BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); + BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k])); + BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k]))); + BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k]))); + } + + // Check that the decomposition is correct + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + aba(model,data_ref,q,v,tau); + BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); +} + +BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_double_init) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data1(model), data2(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + // const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + + // Contact models and data + const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D6D; + + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL); + contact_models_6D.push_back(ci_RF); + contact_datas_6D.push_back(RigidConstraintData(ci_RF)); + contact_models_6D6D.push_back(ci_RF); + contact_datas_6D6D.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),LOCAL); + contact_models_6D6D.push_back(ci_LF); + contact_datas_6D6D.push_back(RigidConstraintData(ci_LF)); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + + initConstraintDynamics(model,data1,contact_models_empty); + BOOST_CHECK(data1.contact_chol.size() == (model.nv + 0)); + constraintDynamics(model,data1,q,v,tau,contact_models_empty,contact_datas_empty,prox_settings); + BOOST_CHECK(!hasNaN(data1.ddq)); + + initConstraintDynamics(model,data1,contact_models_6D); + BOOST_CHECK(data1.contact_chol.size() == (model.nv + 1*6)); + constraintDynamics(model,data1,q,v,tau,contact_models_6D,contact_datas_6D,prox_settings); + BOOST_CHECK(!hasNaN(data1.ddq)); + + initConstraintDynamics(model,data1,contact_models_6D6D); + BOOST_CHECK(data1.contact_chol.size() == (model.nv + 2*6)); + constraintDynamics(model,data1,q,v,tau,contact_models_6D6D,contact_datas_6D6D,prox_settings); + BOOST_CHECK(!hasNaN(data1.ddq)); + + initConstraintDynamics(model,data2,contact_models_6D6D); + initConstraintDynamics(model,data2,contact_models_6D); + initConstraintDynamics(model,data2,contact_models_empty); +} + +BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_in_contact_6D_LOCAL) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + // const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL); + ci_RF.joint1_placement.setRandom(); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),LOCAL); + ci_LF.joint1_placement.setRandom(); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + Data::Matrix6x Jtmp = Data::Matrix6x::Zero(6,model.nv); + + getJointJacobian(model,data_ref, + ci_RF.joint1_id,ci_RF.reference_frame, + Jtmp); + J_ref.middleRows<6>(0) = ci_RF.joint1_placement.inverse().toActionMatrix() * Jtmp; + + Jtmp.setZero(); + getJointJacobian(model,data_ref, + ci_LF.joint1_id,ci_LF.reference_frame, + Jtmp); + J_ref.middleRows<6>(6) = ci_LF.joint1_placement.inverse().toActionMatrix() * Jtmp; + + Eigen::VectorXd rhs_ref(constraint_dim); + rhs_ref.segment<6>(0) = computeAcceleration(model,data_ref,ci_RF.joint1_id,ci_RF.reference_frame,ci_RF.type,ci_RF.joint1_placement).toVector(); + rhs_ref.segment<6>(6) = computeAcceleration(model,data_ref,ci_LF.joint1_id,ci_LF.reference_frame,ci_LF.type,ci_LF.joint1_placement).toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + forwardDynamics(model,data_ref,q,v,tau,J_ref,rhs_ref,mu0); + forwardKinematics(model,data_ref,q,v,data_ref.ddq); + + BOOST_CHECK((J_ref*data_ref.ddq+rhs_ref).isZero()); + + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); + BOOST_CHECK((J_ref*data.ddq+rhs_ref).isZero()); + + BOOST_CHECK((J_ref*data.ddq+rhs_ref).isZero()); + + // Check that the decomposition is correct + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.lambda_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.lambda_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } +} + +BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_in_contact_6D_3D) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + const std::string RA = "rarm6_joint"; + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_3D,model.getJointId(LF),LOCAL_WORLD_ALIGNED); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + RigidConstraintModel ci_RA(CONTACT_3D,model.getJointId(RA),LOCAL); + contact_models.push_back(ci_RA); + contact_datas.push_back(RigidConstraintData(ci_RA)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + getJointJacobian(model,data_ref,model.getJointId(RF),LOCAL,J_ref.middleRows<6>(0)); + Data::Matrix6x J_LF(6,model.nv); J_LF.setZero(); + getJointJacobian(model,data_ref,model.getJointId(LF),LOCAL_WORLD_ALIGNED,J_LF); + J_ref.middleRows<3>(6) = J_LF.middleRows<3>(Motion::LINEAR); + Data::Matrix6x J_RA(6,model.nv); J_RA.setZero(); + getJointJacobian(model,data_ref,model.getJointId(RA),LOCAL,J_RA); + J_ref.middleRows<3>(9) = J_RA.middleRows<3>(Motion::LINEAR); + + Eigen::VectorXd rhs_ref(constraint_dim); + + rhs_ref.segment<6>(0) = computeAcceleration(model,data_ref,model.getJointId(RF),ci_RF.reference_frame,ci_RF.type).toVector(); + rhs_ref.segment<3>(6) = computeAcceleration(model,data_ref,model.getJointId(LF),ci_LF.reference_frame,ci_LF.type).linear(); + rhs_ref.segment<3>(9) = computeAcceleration(model,data_ref,model.getJointId(RA),ci_RA.reference_frame,ci_RA.type).linear(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + forwardDynamics(model,data_ref,q,v,tau,J_ref,rhs_ref,mu0); + forwardKinematics(model,data_ref,q,v,data_ref.ddq); + + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); + + // Check that the decomposition is correct + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.lambda_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.lambda_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } +} + +BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_in_contact_6D_LOCAL_WORLD_ALIGNED) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + // const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL_WORLD_ALIGNED); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),LOCAL); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + updateFramePlacements(model,data_ref); + getJointJacobian(model,data_ref, + ci_RF.joint1_id,ci_RF.reference_frame, + J_ref.middleRows<6>(0)); + getJointJacobian(model,data_ref, + ci_LF.joint1_id,ci_LF.reference_frame, + J_ref.middleRows<6>(6)); + + Eigen::VectorXd rhs_ref(constraint_dim); + + rhs_ref.segment<6>(0) = computeAcceleration(model,data_ref,ci_RF.joint1_id,ci_RF.reference_frame,ci_RF.type).toVector(); + rhs_ref.segment<6>(6) = computeAcceleration(model,data_ref,ci_LF.joint1_id,ci_LF.reference_frame,ci_LF.type).toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + forwardDynamics(model,data_ref,q,v,tau,J_ref,rhs_ref,mu0); + forwardKinematics(model,data_ref,q,v,data_ref.ddq); + + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); + + // Check that the decomposition is correct + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); + BOOST_CHECK((J_ref*data.ddq+rhs_ref).isZero()); + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.lambda_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.lambda_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } +} + +BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_in_contact_specifying_joint2id) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + const std::string RA = "rarm6_joint"; + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + + RigidConstraintModel ci_RF(CONTACT_6D,0,model.getJointId(RF),LOCAL_WORLD_ALIGNED); + RigidConstraintModel ci_RF_bis(CONTACT_6D,model.getJointId(RF),LOCAL_WORLD_ALIGNED); + ci_RF.joint1_placement.setRandom(); + ci_RF.joint2_placement.setRandom(); + ci_RF_bis.joint1_placement = ci_RF.joint2_placement; + ci_RF_bis.joint2_placement = ci_RF.joint1_placement; + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + + RigidConstraintModel ci_LF(CONTACT_6D,0,model.getJointId(LF),LOCAL); + RigidConstraintModel ci_LF_bis(CONTACT_6D,model.getJointId(LF),LOCAL); + ci_LF.joint1_placement.setRandom(); + ci_LF.joint2_placement.setRandom(); + ci_LF_bis.joint1_placement = ci_LF.joint2_placement; + ci_LF_bis.joint2_placement = ci_LF.joint1_placement; + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + RigidConstraintModel ci_RA(CONTACT_6D,0,model.getJointId(RA),LOCAL); + RigidConstraintModel ci_RA_bis(CONTACT_6D,model.getJointId(RA),LOCAL); + ci_RA.joint1_placement.setRandom(); + ci_RA.joint2_placement.setRandom(); + ci_RA_bis.joint1_placement = ci_RA.joint2_placement; + ci_RA_bis.joint2_placement = ci_RA.joint1_placement; + contact_models.push_back(ci_RA); + contact_datas.push_back(RigidConstraintData(ci_RA)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + updateFramePlacements(model,data_ref); + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv), J_RA(6,model.nv); + J_RF.setZero(); J_LF.setZero(); J_RA.setZero(); + Data::Matrix6x J_RF_local(6,model.nv), J_LF_local(6,model.nv), J_RA_local(6,model.nv); + J_RF_local.setZero(); J_LF_local.setZero(); J_RA_local.setZero(); + getJointJacobian(model,data_ref, + ci_RF.joint2_id,WORLD, + J_RF); + getJointJacobian(model,data_ref, + ci_RF.joint2_id,LOCAL, + J_RF_local); + getJointJacobian(model,data_ref, + ci_LF.joint2_id,WORLD, + J_LF); + getJointJacobian(model,data_ref, + ci_LF.joint2_id,LOCAL, + J_LF_local); + getJointJacobian(model,data_ref, + ci_RA.joint2_id,WORLD, + J_RA); + getJointJacobian(model,data_ref, + ci_RA.joint2_id,LOCAL, + J_RA_local); + + { + const SE3 oMc(SE3::Matrix3::Identity(),(data_ref.oMi[ci_RF.joint1_id]*ci_RF.joint1_placement).translation()); + J_ref.middleRows<6>(0) = -oMc.toActionMatrixInverse() * J_RF; + } + + { + J_ref.middleRows<6>(6) = -(data_ref.oMi[ci_LF.joint1_id]*ci_LF.joint1_placement).toActionMatrixInverse() * J_LF; + } + + { + J_ref.middleRows<6>(12) = -(data_ref.oMi[ci_RA.joint1_id]*ci_RA.joint1_placement).toActionMatrixInverse() * J_RA; + } + + Eigen::VectorXd rhs_ref(constraint_dim); + + forwardKinematics(model, data_ref, q, v, 0*v); + const SE3 c1Mc2_1 = (data.oMi[ci_RF.joint1_id]*ci_RF.joint1_placement).actInv(data_ref.oMi[ci_RF.joint2_id]*ci_RF.joint2_placement); + SE3 c1Mc2_1_W((data_ref.oMi[ci_RF.joint2_id]).rotation(), + -(data_ref.oMi[ci_RF.joint1_id] * ci_RF.joint1_placement).translation() + data_ref.oMi[ci_RF.joint2_id].translation()); + Motion acc_1 = c1Mc2_1_W.act(data_ref.a[ci_RF.joint2_id]); + + const SE3 c1Mc2_2 = (data_ref.oMi[ci_LF.joint1_id]*ci_LF.joint1_placement).actInv(data_ref.oMi[ci_LF.joint2_id]*ci_LF.joint2_placement); + Motion acc_2 = c1Mc2_2.act(ci_LF.joint2_placement.actInv(data_ref.a[ci_LF.joint2_id])); + + const SE3 c1Mc2_3 = (data_ref.oMi[ci_RA.joint1_id]*ci_RA.joint1_placement).actInv(data_ref.oMi[ci_RA.joint2_id]*ci_RA.joint2_placement); + Motion acc_3 = c1Mc2_3.act(ci_RA.joint2_placement.actInv(data_ref.a[ci_RA.joint2_id])); + + rhs_ref.segment<6>(0) = -acc_1.toVector(); + rhs_ref.segment<6>(6) = -acc_2.toVector(); + rhs_ref.segment<6>(12) = -acc_3.toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.topLeftCorner(constraint_dim,constraint_dim).diagonal().fill(-mu0); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + forwardDynamics(model,data_ref,q,v,tau,J_ref,rhs_ref,mu0); + forwardKinematics(model,data_ref,q,v,0*data_ref.ddq); + + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); + + std::cout << "acc_1 ref:\n" << acc_1 << std::endl; + std::cout << "acc_1:\n" << contact_datas[0].contact2_acceleration_drift << std::endl; + BOOST_CHECK(acc_1.isApprox(contact_datas[0].contact2_acceleration_drift)); + + std::cout << "acc_2 ref:\n" << acc_2 << std::endl; + std::cout << "acc_2:\n" << contact_datas[1].contact2_acceleration_drift << std::endl; + BOOST_CHECK(acc_2.isApprox(contact_datas[1].contact2_acceleration_drift)); + + std::cout << "acc_3 ref:\n" << acc_3 << std::endl; + std::cout << "acc_3:\n" << contact_datas[2].contact2_acceleration_drift << std::endl; + BOOST_CHECK(acc_3.isApprox(contact_datas[2].contact2_acceleration_drift)); + + BOOST_CHECK(contact_datas[0].c1Mc2.isApprox(c1Mc2_1)); + + const SE3 c1Mc2_1_LWA(contact_datas[0].oMc2.rotation(), + contact_datas[0].oMc1.rotation()*c1Mc2_1.translation()); + BOOST_CHECK((c1Mc2_1_LWA.toActionMatrix()*(ci_RF.joint2_placement.toActionMatrixInverse()*J_RF_local)).isApprox(-J_ref.middleRows<6>(0))); + BOOST_CHECK(contact_datas[0].oMc1.isApprox(ci_RF.joint1_placement)); + + BOOST_CHECK(contact_datas[1].c1Mc2.isApprox(c1Mc2_2)); + BOOST_CHECK((contact_datas[1].oMc1.toActionMatrixInverse()*J_LF).isApprox(-J_ref.middleRows<6>(6))); + BOOST_CHECK((data_ref.oMi[ci_LF.joint2_id].toActionMatrix()*J_LF_local).isApprox(J_LF)); + BOOST_CHECK(contact_datas[1].oMc1.isApprox(ci_LF.joint1_placement)); + BOOST_CHECK(data.oa[ci_LF.joint2_id].isApprox(data_ref.oMi[ci_LF.joint2_id].act(data_ref.a[ci_LF.joint2_id]))); + + BOOST_CHECK(contact_datas[2].c1Mc2.isApprox(c1Mc2_3)); + BOOST_CHECK((c1Mc2_3.toActionMatrix()*(ci_RA.joint2_placement.toActionMatrixInverse()*J_RA_local)).isApprox(-J_ref.middleRows<6>(12))); + BOOST_CHECK(contact_datas[2].oMc1.isApprox(ci_RA.joint1_placement)); + BOOST_CHECK(data.oa[ci_RA.joint2_id].isApprox(data_ref.oMi[ci_RA.joint2_id].act(data_ref.a[ci_RA.joint2_id]))); + + // Check that the decomposition is correct + + forwardKinematics(model,data_ref,q,v,0*data_ref.ddq); + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + forwardKinematics(model,data,q,v,data.ddq); + BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); + BOOST_CHECK((J_ref*data.ddq+rhs_ref).isZero()); + + Motion acc_1_final = c1Mc2_1_W.act(data.a[ci_RF.joint2_id]); + BOOST_CHECK(acc_1_final.isZero()); + + std::cout << "acc_1_final:\n" << acc_1_final << std::endl; + + Motion acc_2_final = c1Mc2_2.act(data.a[ci_LF.joint2_id]); + BOOST_CHECK(acc_2_final.isZero()); + + std::cout << "acc_2_final:\n" << acc_2_final << std::endl; + + Motion acc_3_final = c1Mc2_3.act(data.a[ci_RA.joint2_id]); + BOOST_CHECK(acc_3_final.isZero()); + + std::cout << "acc_3_final:\n" << acc_3_final << std::endl; + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.lambda_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.lambda_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_bis; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_bis; + + contact_models_bis.push_back(ci_RF_bis); + contact_models_bis.push_back(ci_LF_bis); + contact_models_bis.push_back(ci_RA_bis); + + for(PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)::const_iterator it = contact_models_bis.begin(); + it != contact_models_bis.end(); ++it) + contact_datas_bis.push_back(RigidConstraintData(*it)); + + Data data_bis(model); + initConstraintDynamics(model,data_bis,contact_models_bis); + constraintDynamics(model,data_bis,q,v,tau,contact_models_bis,contact_datas_bis,prox_settings); + + BOOST_CHECK(data_bis.ddq.isApprox(data.ddq)); + std::cout << "ddq: " << data_bis.ddq.transpose() << std::endl; + std::cout << "ddq: " << data.ddq.transpose() << std::endl; + +// Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + const RigidConstraintModel & cmodel_bis = contact_models_bis[k]; + const RigidConstraintData & cdata_bis = contact_datas_bis[k]; + + BOOST_CHECK(cmodel_bis.reference_frame == cmodel.reference_frame); + BOOST_CHECK(cmodel_bis.joint1_id == cmodel.joint2_id); + BOOST_CHECK(cmodel_bis.joint2_id == cmodel.joint1_id); + BOOST_CHECK(cdata.oMc1.isApprox(cdata_bis.oMc2)); + BOOST_CHECK(cdata.oMc2.isApprox(cdata_bis.oMc1)); + BOOST_CHECK(cdata.c1Mc2.isApprox(cdata_bis.c1Mc2.inverse())); + + std::cout << "cdata.c1Mc2:\n" << cdata.c1Mc2 << std::endl; + Force contact_force, contact_force_bis; + switch(cmodel.reference_frame) + { + case LOCAL_WORLD_ALIGNED: + { + SE3 c1Mc2_LWA(SE3::Matrix3::Identity(), + cdata.oMc1.rotation()*cdata.c1Mc2.translation()); + contact_force_bis = cdata_bis.contact_force; + BOOST_CHECK(cdata_bis.contact1_acceleration_drift.isApprox(c1Mc2_LWA.actInv(cdata.contact2_acceleration_drift))); + + contact_force = c1Mc2_LWA.actInv(cdata.contact_force); + BOOST_CHECK(contact_force.isApprox(-contact_force_bis)); + break; + } + case LOCAL: + { + contact_force_bis = cdata_bis.contact_force; + BOOST_CHECK(cdata_bis.contact1_acceleration_drift.isApprox(cdata.c1Mc2.actInv(cdata.contact2_acceleration_drift))); + + contact_force = cdata.c1Mc2.actInv(cdata.contact_force); + BOOST_CHECK(contact_force.isApprox(-contact_force_bis)); + break; + } + case WORLD: + BOOST_CHECK(false); + break; + } + + std::cout << "contact_force: " << contact_force.toVector().transpose() << std::endl; + std::cout << "contact_force_bis: " << contact_force_bis.toVector().transpose() << std::endl; + } +} + +PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) +createData(const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) & contact_models) +{ + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) contact_datas; + for(size_t k = 0; k < contact_models.size(); ++k) + contact_datas.push_back(pinocchio::RigidConstraintData(contact_models[k])); + + return contact_datas; +} + +BOOST_AUTO_TEST_CASE(test_correction_CONTACT_6D) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; +// pinocchio::buildModels::humanoidRandom(model,true); + const JointIndex joint_id = model.addJoint(0,JointModelFreeFlyer(),SE3::Identity(),"root"); + const Inertia box_inertia = Inertia::FromBox(100.,1.,1.,1.); + model.appendBodyToJoint(joint_id,box_inertia); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "root"; + const JointIndex RF_id = model.getJointId(RF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + + RigidConstraintModel ci_RF(CONTACT_6D,RF_id,LOCAL); + ci_RF.joint1_placement.setIdentity(); + ci_RF.joint2_placement.setIdentity(); + ci_RF.corrector.Kp = 10.; + ci_RF.corrector.Kd = 2. * sqrt(ci_RF.corrector.Kp); + contact_models.push_back(ci_RF); + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas = createData(contact_models); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas); + + BOOST_CHECK(contact_datas[0].oMc1.isApprox(data.oMi[ci_RF.joint1_id] * ci_RF.joint1_placement)); + BOOST_CHECK(contact_datas[0].oMc2.isApprox(data.oMi[ci_RF.joint2_id] * ci_RF.joint2_placement)); + BOOST_CHECK(contact_datas[0].contact1_velocity.isApprox(contact_datas[0].oMc1.actInv(data.ov[ci_RF.joint1_id]))); + BOOST_CHECK(contact_datas[0].contact2_velocity.isZero()); + + const double dt = 1e-8; + const VectorXd q_plus = integrate(model,q,v*dt); + + Data data_plus(model); + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_plus = createData(contact_models); + initConstraintDynamics(model,data_plus,contact_models); + constraintDynamics(model,data_plus,q_plus,v,tau,contact_models,contact_datas_plus); + + const Motion contact_RF_velocity_error_fd = log6(contact_datas[0].c1Mc2.act(contact_datas_plus[0].c1Mc2.inverse()))/dt; + BOOST_CHECK(contact_RF_velocity_error_fd.isApprox(contact_datas[0].contact_velocity_error,sqrt(dt))); + std::cout << "contact_RF_velocity_error_fd:\n" << contact_RF_velocity_error_fd << std::endl; + std::cout << "contact_velocity_error:\n" << contact_datas[0].contact_velocity_error << std::endl; + + // Simulation loop + { + const int N = 200; + const double dt = 1e-3; + const double mu = 1e-12; + +// model.gravity.setZero(); + Data data_sim(model); + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_sim = createData(contact_models); + initConstraintDynamics(model,data_sim,contact_models); + + Eigen::VectorXd q0 (model.nq); + const SE3 M0 = SE3::Random(); + q0 << M0.translation(), SE3::Quaternion(M0.rotation()).coeffs(); + const Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv); + Eigen::VectorXd a = Eigen::VectorXd(model.nv); + Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv); + + Eigen::VectorXd q(q0), v(v0); + + tau = rnea(model,data_sim,q,v,0*a); + ProximalSettings prox_settings(1e-12,mu,1); + constraintDynamics(model,data_sim,q0,v0,tau,contact_models,contact_data_sim,prox_settings); + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_sim_prev(contact_data_sim); + + for(int it = 0; it <= N; it++) + { + a = constraintDynamics(model,data_sim,q,v,tau,contact_models,contact_data_sim,prox_settings); + v += a*dt; + q = integrate(model,q,v*dt); + + if(it > 1) + { + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintData & cdata = contact_data_sim[k]; + const RigidConstraintData & cdata_prev = contact_data_sim_prev[k]; + + BOOST_CHECK(cdata.contact_placement_error.toVector().norm() <= cdata_prev.contact_placement_error.toVector().norm()); + } + } + + contact_data_sim_prev = contact_data_sim; + } + } +} + +BOOST_AUTO_TEST_CASE(test_correction_CONTACT_3D) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; +// pinocchio::buildModels::humanoidRandom(model,true); + const JointIndex joint_id = model.addJoint(0,JointModelFreeFlyer(),SE3::Identity(),"root"); + const Inertia box_inertia = Inertia::FromBox(100.,1.,1.,1.); + model.appendBodyToJoint(joint_id,box_inertia); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const double mu = 1e-8; + const std::string RF = "root"; + const JointIndex RF_id = model.getJointId(RF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + + RigidConstraintModel ci_RF1(CONTACT_3D,RF_id,LOCAL); + ci_RF1.joint1_placement.translation() = SE3::Vector3(0.5,0.5,-0.5); + ci_RF1.joint2_placement.setRandom(); + ci_RF1.corrector.Kp = 10.; + ci_RF1.corrector.Kd = 2. * sqrt(ci_RF1.corrector.Kp); + contact_models.push_back(ci_RF1); + + RigidConstraintModel ci_RF2(CONTACT_3D,RF_id,LOCAL); + ci_RF2.joint1_placement.translation() = SE3::Vector3(-0.5,0.5,-0.5); + ci_RF2.joint2_placement.setRandom(); + ci_RF2.corrector.Kp = 10.; + ci_RF2.corrector.Kd = 2. * sqrt(ci_RF2.corrector.Kp); + contact_models.push_back(ci_RF2); + + RigidConstraintModel ci_RF3(CONTACT_3D,RF_id,LOCAL); + ci_RF3.joint1_placement.translation() = SE3::Vector3(-0.5,-0.5,-0.5); + ci_RF3.joint2_placement.setRandom(); + ci_RF3.corrector.Kp = 10.; + ci_RF3.corrector.Kd = 2. * sqrt(ci_RF3.corrector.Kp); + contact_models.push_back(ci_RF3); + + RigidConstraintModel ci_RF4(CONTACT_3D,RF_id,LOCAL); + ci_RF4.joint1_placement.translation() = SE3::Vector3(0.5,-0.5,-0.5); + ci_RF4.joint2_placement.setRandom(); + ci_RF4.corrector.Kp = 10.; + ci_RF4.corrector.Kd = 2. * sqrt(ci_RF4.corrector.Kp); + contact_models.push_back(ci_RF4); + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas = createData(contact_models); + ProximalSettings prox_settings(1e-12,mu,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); + + // Simulation loop + { + const int N = 200; + const double dt = 1e-3; + +// model.gravity.setZero(); + Data data_sim(model); + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_sim = createData(contact_models); + initConstraintDynamics(model,data_sim,contact_models); + + Eigen::VectorXd q0 (model.nq); + const SE3 M0 = SE3::Random(); + q0 << M0.translation(), SE3::Quaternion(M0.rotation()).coeffs(); + const Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv); + Eigen::VectorXd a = Eigen::VectorXd(model.nv); + Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv); + + Eigen::VectorXd q(q0), v(v0); + + constraintDynamics(model,data_sim,q0,v0,tau,contact_models,contact_data_sim,prox_settings); + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_sim_prev(contact_data_sim); + + for(int it = 0; it <= N; it++) + { + a = constraintDynamics(model,data_sim,q,v,tau,contact_models,contact_data_sim,prox_settings); + v += a*dt; + q = integrate(model,q,v*dt); + + if(it > 1) + { + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintData & cdata = contact_data_sim[k]; + const RigidConstraintData & cdata_prev = contact_data_sim_prev[k]; + + BOOST_CHECK(cdata.contact_placement_error.toVector().norm() <= cdata_prev.contact_placement_error.toVector().norm()); + } + } + + contact_data_sim_prev = contact_data_sim; + } + } +} + +BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_in_contact_specifying_joint2id_case3D) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + const std::string RA = "rarm6_joint"; + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + + RigidConstraintModel ci_RF(CONTACT_3D,0,model.getJointId(RF),LOCAL_WORLD_ALIGNED); + RigidConstraintModel ci_RF_bis(CONTACT_3D,model.getJointId(RF),LOCAL_WORLD_ALIGNED); + ci_RF.joint1_placement.setRandom(); + ci_RF.joint2_placement.setRandom(); + ci_RF_bis.joint1_placement = ci_RF.joint2_placement; + ci_RF_bis.joint2_placement = ci_RF.joint1_placement; + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + + RigidConstraintModel ci_LF(CONTACT_3D,0,model.getJointId(LF),LOCAL); + RigidConstraintModel ci_LF_bis(CONTACT_3D,model.getJointId(LF),LOCAL); + ci_LF.joint1_placement.setRandom(); + ci_LF.joint2_placement.setRandom(); + ci_LF_bis.joint1_placement = ci_LF.joint2_placement; + ci_LF_bis.joint2_placement = ci_LF.joint1_placement; + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + RigidConstraintModel ci_RA(CONTACT_6D,0,model.getJointId(RA),LOCAL); + RigidConstraintModel ci_RA_bis(CONTACT_6D,model.getJointId(RA),LOCAL); + ci_RA.joint1_placement.setRandom(); + ci_RA.joint2_placement.setRandom(); + ci_RA_bis.joint1_placement = ci_RA.joint2_placement; + ci_RA_bis.joint2_placement = ci_RA.joint1_placement; + contact_models.push_back(ci_RA); + contact_datas.push_back(RigidConstraintData(ci_RA)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv), J_RA(6,model.nv); + J_RF.setZero(); J_LF.setZero(); J_RA.setZero(); + Data::Matrix6x J_RF_local(6,model.nv), J_LF_local(6,model.nv), J_RA_local(6,model.nv); + J_RF_local.setZero(); J_LF_local.setZero(); J_RA_local.setZero(); + getJointJacobian(model,data_ref, + ci_RF.joint2_id,WORLD, + J_RF); + getJointJacobian(model,data_ref, + ci_RF.joint2_id,LOCAL, + J_RF_local); + J_RF_local = ci_RF.joint2_placement.toActionMatrixInverse() * J_RF_local; + getJointJacobian(model,data_ref, + ci_LF.joint2_id,WORLD, + J_LF); + getJointJacobian(model,data_ref, + ci_LF.joint2_id,LOCAL, + J_LF_local); + J_LF_local = ci_LF.joint2_placement.toActionMatrixInverse() * J_LF_local; + getJointJacobian(model,data_ref, + ci_RA.joint2_id,WORLD, + J_RA); + getJointJacobian(model,data_ref, + ci_RA.joint2_id,LOCAL, + J_RA_local); + + { + const SE3 oMc2 = data_ref.oMi[ci_RF.joint2_id]*ci_RF.joint2_placement; + J_ref.middleRows<3>(0) = -oMc2.rotation() * J_RF_local.middleRows<3>(Motion::LINEAR); + } + + { + const SE3 oMc1 = data_ref.oMi[ci_LF.joint1_id]*ci_LF.joint1_placement; + const SE3 oMc2 = data_ref.oMi[ci_LF.joint2_id]*ci_LF.joint2_placement; + const SE3 c1Mc2 = oMc1.actInv(oMc2); + J_ref.middleRows<3>(3) = - c1Mc2.rotation() * J_LF_local.middleRows<3>(SE3::LINEAR); + } + + { + J_ref.middleRows<6>(6) = -(data_ref.oMi[ci_RA.joint1_id]*ci_RA.joint1_placement).toActionMatrixInverse() * J_RA; + } + + Eigen::VectorXd rhs_ref(constraint_dim); + + forwardKinematics(model, data_ref, q, v, 0*v); + Motion::Vector3 acc_1; + { + const SE3 oMc2 = data_ref.oMi[ci_RF.joint2_id]*ci_RF.joint2_placement; + const Motion v2 = ci_RF.joint2_placement.actInv(data_ref.v[ci_RF.joint2_id]); + const Motion a2 = ci_RF.joint2_placement.actInv(data_ref.a[ci_RF.joint2_id]); + acc_1 = oMc2.rotation()*(a2.linear() + v2.angular().cross(v2.linear())); + } + + Motion::Vector3 acc_2; + { + const SE3 oMc1 = data_ref.oMi[ci_LF.joint1_id]*ci_LF.joint1_placement; + const SE3 oMc2 = data_ref.oMi[ci_LF.joint2_id]*ci_LF.joint2_placement; + const SE3 c1Mc2 = oMc1.actInv(oMc2); + const Motion v2 = ci_LF.joint2_placement.actInv(data_ref.v[ci_LF.joint2_id]); + const Motion a2 = ci_LF.joint2_placement.actInv(data_ref.a[ci_LF.joint2_id]); + acc_2 = c1Mc2.rotation()*(a2.linear() + v2.angular().cross(v2.linear())); + } + + const SE3 c1Mc2_3 = (data_ref.oMi[ci_RA.joint1_id]*ci_RA.joint1_placement).actInv(data_ref.oMi[ci_RA.joint2_id]*ci_RA.joint2_placement); + Motion acc_3 = c1Mc2_3.act(ci_RA.joint2_placement.actInv(data_ref.a[ci_RA.joint2_id])); + + rhs_ref.segment<3>(0) = -acc_1; + rhs_ref.segment<3>(3) = -acc_2; + rhs_ref.segment<6>(6) = -acc_3.toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.topLeftCorner(constraint_dim,constraint_dim).diagonal().fill(-mu0); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + forwardDynamics(model,data_ref,q,v,tau,J_ref,rhs_ref,mu0); + forwardKinematics(model,data_ref,q,v,0*data_ref.ddq); + + ProximalSettings prox_settings(1e-12,0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); + + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.topRightCorner(constraint_dim,model.nv).isApprox(J_ref)); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + std::cout << "KKT_matrix.topRightCorner(constraint_dim,model.nv):\n" << KKT_matrix.topRightCorner(constraint_dim,model.nv) << std::endl; + std::cout << "KKT_matrix_ref.topRightCorner(constraint_dim,model.nv):\n" << KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) << std::endl; + + // Check solutions + forwardKinematics(model,data,q,v,data.ddq); + BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); + BOOST_CHECK((J_ref*data.ddq+rhs_ref).isZero()); + + std::cout << "data_ref.ddq: " << data_ref.ddq.transpose() << std::endl; + std::cout << "data.ddq: " << data.ddq.transpose() << std::endl; + std::cout << "res: " << (J_ref*data.ddq+rhs_ref).transpose() << std::endl; + std::cout << "res_ref: " << (J_ref*data_ref.ddq+rhs_ref).transpose() << std::endl; + + const Motion vel_1_final = ci_RF.joint2_placement.actInv(data.v[ci_RF.joint2_id]); + const Motion::Vector3 acc_1_final = ci_RF.joint2_placement.actInv(data.a[ci_RF.joint2_id]).linear() + vel_1_final.angular().cross(vel_1_final.linear()); + BOOST_CHECK(acc_1_final.isZero()); + + std::cout << "acc_1_final:" << acc_1_final.transpose() << std::endl; + + const Motion vel_2_final = ci_LF.joint2_placement.actInv(data.v[ci_LF.joint2_id]); + const Motion::Vector3 acc_2_final = ci_LF.joint2_placement.actInv(data.a[ci_LF.joint2_id]).linear() + vel_2_final.angular().cross(vel_2_final.linear()); + BOOST_CHECK(acc_2_final.isZero()); + + std::cout << "acc_2_final:" << acc_2_final.transpose() << std::endl; + + Motion acc_3_final = c1Mc2_3.act(data.a[ci_RA.joint2_id]); + BOOST_CHECK(acc_3_final.isZero()); + + std::cout << "acc_3_final:\n" << acc_3_final << std::endl; + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.lambda_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.lambda_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_bis; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_bis; + + contact_models_bis.push_back(ci_RF_bis); + contact_models_bis.push_back(ci_LF_bis); + contact_models_bis.push_back(ci_RA_bis); + + for(PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)::const_iterator it = contact_models_bis.begin(); + it != contact_models_bis.end(); ++it) + contact_datas_bis.push_back(RigidConstraintData(*it)); + + Data data_bis(model); + initConstraintDynamics(model,data_bis,contact_models_bis); + constraintDynamics(model,data_bis,q,v,tau,contact_models_bis,contact_datas_bis,prox_settings); + + BOOST_CHECK(data_bis.ddq.isApprox(data.ddq)); + std::cout << "ddq: " << data_bis.ddq.transpose() << std::endl; + std::cout << "ddq: " << data.ddq.transpose() << std::endl; + + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + const RigidConstraintModel & cmodel_bis = contact_models_bis[k]; + const RigidConstraintData & cdata_bis = contact_datas_bis[k]; + + BOOST_CHECK(cmodel_bis.reference_frame == cmodel.reference_frame); + BOOST_CHECK(cmodel_bis.joint1_id == cmodel.joint2_id); + BOOST_CHECK(cmodel_bis.joint2_id == cmodel.joint1_id); + BOOST_CHECK(cdata.oMc1.isApprox(cdata_bis.oMc2)); + BOOST_CHECK(cdata.oMc2.isApprox(cdata_bis.oMc1)); + BOOST_CHECK(cdata.c1Mc2.isApprox(cdata_bis.c1Mc2.inverse())); + + std::cout << "cdata.c1Mc2:\n" << cdata.c1Mc2 << std::endl; + Force contact_force, contact_force_bis; + switch(cmodel.reference_frame) + { + case LOCAL_WORLD_ALIGNED: + { + SE3 c1Mc2_LWA(SE3::Matrix3::Identity(), + cdata.oMc1.rotation()*cdata.c1Mc2.translation()); + contact_force_bis = cdata_bis.contact_force; + + if(cmodel.type == CONTACT_3D) + contact_force = cdata.contact_force; + else + { + contact_force = c1Mc2_LWA.actInv(cdata.contact_force); + BOOST_CHECK(cdata_bis.contact1_acceleration_drift.isApprox(c1Mc2_LWA.actInv(cdata.contact2_acceleration_drift))); + } + BOOST_CHECK(contact_force.isApprox(-contact_force_bis)); + break; + } + case LOCAL: + { + contact_force_bis = cdata_bis.contact_force; + + if(cmodel.type == CONTACT_3D) + contact_force.linear() = cdata.c1Mc2.actInv(cdata.contact_force).linear(); + else + { + contact_force = cdata.c1Mc2.actInv(cdata.contact_force); + BOOST_CHECK(cdata_bis.contact1_acceleration_drift.isApprox(cdata.c1Mc2.actInv(cdata.contact2_acceleration_drift))); + } + BOOST_CHECK(contact_force.isApprox(-contact_force_bis)); + break; + } + case WORLD: + BOOST_CHECK(false); + break; + } + + std::cout << "contact_force: " << contact_force.toVector().transpose() << std::endl; + std::cout << "contact_force_bis: " << contact_force_bis.toVector().transpose() << std::endl; + } +} + +BOOST_AUTO_TEST_CASE(test_diagonal_inertia) +{ + using namespace pinocchio; + + const double mu = 1e2; + const Inertia diagonal6_inertia(mu,Inertia::Vector3::Zero(), + Symmetric3(mu,0,mu,0,0,mu)); + const Inertia::Matrix6 diagonal6_inertia_mat = diagonal6_inertia.matrix(); + BOOST_CHECK(diagonal6_inertia_mat.block(Inertia::LINEAR,Inertia::ANGULAR,3,3).isZero()); + BOOST_CHECK(diagonal6_inertia_mat.block(Inertia::ANGULAR,Inertia::LINEAR,3,3).isZero()); + + const SE3 M = SE3::Random(); +// const Inertia::Matrix3 RtRmu = mu * M.rotation().transpose()*M.rotation(); + const Inertia::Matrix3 RtRmu = mu * Inertia::Matrix3::Identity(); + Inertia I6_translate(mu,M.translation(),Symmetric3(RtRmu)); + + const Inertia I6_ref = M.act(diagonal6_inertia); + BOOST_CHECK(I6_translate.isApprox(I6_ref)); + + const Inertia diagonal3_inertia(mu,Inertia::Vector3::Zero(), + Symmetric3(0,0,0,0,0,0)); + const Inertia::Matrix6 diagonal3_inertia_mat = diagonal3_inertia.matrix(); + BOOST_CHECK(diagonal3_inertia_mat.block(Inertia::LINEAR,Inertia::ANGULAR,3,3).isZero()); + BOOST_CHECK(diagonal3_inertia_mat.block(Inertia::ANGULAR,Inertia::LINEAR,3,3).isZero()); + BOOST_CHECK(diagonal3_inertia_mat.block(Inertia::ANGULAR,Inertia::ANGULAR,3,3).isZero()); + + Inertia I3_translate(mu, + M.translation(), + Symmetric3(0,0,0,0,0,0)); + + const Inertia I3_ref = M.act(diagonal3_inertia); + BOOST_CHECK(I3_translate.isApprox(I3_ref)); +} + + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/contact-cholesky.cpp b/unittest/contact-cholesky.cpp new file mode 100644 index 0000000000..19ec2ef6b9 --- /dev/null +++ b/unittest/contact-cholesky.cpp @@ -0,0 +1,1332 @@ +// +// Copyright (c) 2019-2020 INRIA +// + +#include + +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/cholesky.hpp" +#include "pinocchio/algorithm/contact-info.hpp" +#include "pinocchio/algorithm/crba.hpp" +#include "pinocchio/algorithm/contact-cholesky.hxx" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/parsers/sample-models.hpp" + +#include +#include + +namespace pinocchio +{ + namespace cholesky + { + template + struct ContactCholeskyDecompositionAccessorTpl + : public ContactCholeskyDecompositionTpl + { + typedef ContactCholeskyDecompositionTpl Base; + typedef typename Base::IndexVector IndexVector; + typedef typename Base::BooleanVector BooleanVector; + + ContactCholeskyDecompositionAccessorTpl(const Base & other) + : Base(other) + {} + + const IndexVector & getParents_fromRow() const + { return this->parents_fromRow; } + const IndexVector & getLastChild() const + { return this->last_child; } + const IndexVector & getNvSubtree_fromRow() const + { return this->nv_subtree_fromRow; } + const std::vector & getJoint1_indexes() const + { return this->joint1_indexes; } + const std::vector & getJoint2_indexes() const + { return this->joint2_indexes; } + }; + + typedef ContactCholeskyDecompositionAccessorTpl ContactCholeskyDecompositionAccessor; + } +} + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +BOOST_AUTO_TEST_CASE(contact_operator_equal) +{ + + using namespace Eigen; + using namespace pinocchio; + using namespace cholesky; + + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model); + Data humanoid_data(humanoid_model); + + pinocchio::Model manipulator_model; + pinocchio::buildModels::manipulator(manipulator_model); + Data manipulator_data(manipulator_model); + + const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty; + + humanoid_model.lowerPositionLimit.head<3>().fill(-1.); + humanoid_model.upperPositionLimit.head<3>().fill(1.); + VectorXd humanoid_q = randomConfiguration(humanoid_model); + crba(humanoid_model,humanoid_data,humanoid_q); + + VectorXd manipulator_q = randomConfiguration(manipulator_model); + crba(manipulator_model,manipulator_data,manipulator_q); + + ContactCholeskyDecomposition humanoid_chol(humanoid_model), manipulator_chol(manipulator_model); + humanoid_chol.compute(humanoid_model,humanoid_data,contact_models_empty,contact_datas_empty); + manipulator_chol.compute(manipulator_model,manipulator_data,contact_models_empty,contact_datas_empty); + + BOOST_CHECK(humanoid_chol == humanoid_chol); + BOOST_CHECK(humanoid_chol != manipulator_chol); +} + +BOOST_AUTO_TEST_CASE(contact_cholesky_simple) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + crba(model,data_ref,q); + + pinocchio::cholesky::decompose(model,data_ref); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + ContactCholeskyDecomposition contact_chol_decomposition; + const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty; + contact_chol_decomposition.allocate(model,contact_models_empty); + + BOOST_CHECK(contact_chol_decomposition.D.size() == model.nv); + BOOST_CHECK(contact_chol_decomposition.Dinv.size() == model.nv); + BOOST_CHECK(contact_chol_decomposition.U.rows() == model.nv); + BOOST_CHECK(contact_chol_decomposition.U.cols() == model.nv); + BOOST_CHECK(contact_chol_decomposition.size() == model.nv); + BOOST_CHECK(contact_chol_decomposition.U.diagonal().isOnes()); + + Data data(model); crba(model,data,q); + data.M.triangularView() = + data.M.triangularView().transpose(); + + contact_chol_decomposition.compute(model,data,contact_models_empty,contact_datas_empty); + + data_ref.Minv = data_ref.M.inverse(); + Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); + contact_chol_decomposition.getInverseMassMatrix(Minv_test); + + BOOST_CHECK(data.M.isApprox(data_ref.M)); + BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); + + BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv))); + BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv))); + BOOST_CHECK(data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv))); + + ContactCholeskyDecompositionAccessor access(contact_chol_decomposition); + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + BOOST_CHECK(access.getParents_fromRow()[k] == data.parents_fromRow[(size_t)k]); + } + + for(Eigen::DenseIndex k = 0; k < model.njoints; ++k) + { + BOOST_CHECK(access.getLastChild()[k] == data.lastChild[(size_t)k]); + } + + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + BOOST_CHECK(access.getNvSubtree_fromRow()[k] == data.nvSubtree_fromRow[(size_t)k]); + } + + // Test basic operation + VectorXd v_in(VectorXd::Random(model.nv)); + MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(),20)); + + // Test Uv + VectorXd Uv_op_res(v_in), Uv_op_ref(v_in); + + contact_chol_decomposition.Uv(Uv_op_res); + pinocchio::cholesky::Uv(model,data_ref,Uv_op_ref); + + BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref)); + + MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in); + + contact_chol_decomposition.Uv(Uv_mat_op_res); + pinocchio::cholesky::Uv(model,data_ref,Uv_mat_op_ref); + + BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref)); + + // Test Utv + VectorXd Utv_op_res(v_in), Utv_op_ref(v_in); + + contact_chol_decomposition.Utv(Utv_op_res); + pinocchio::cholesky::Utv(model,data_ref,Utv_op_ref); + + BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref)); + + MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in); + + contact_chol_decomposition.Utv(Utv_mat_op_res); + pinocchio::cholesky::Utv(model,data_ref,Utv_mat_op_ref); + + BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref)); + + // Test Uiv + VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in); + + contact_chol_decomposition.Uiv(Uiv_op_res); + pinocchio::cholesky::Uiv(model,data_ref,Uiv_op_ref); + + BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref)); + + MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in); + + contact_chol_decomposition.Uiv(Uiv_mat_op_res); + pinocchio::cholesky::Uiv(model,data_ref,Uiv_mat_op_ref); + + BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref)); + + // Test Utiv + VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in); + + contact_chol_decomposition.Utiv(Utiv_op_res); + pinocchio::cholesky::Utiv(model,data_ref,Utiv_op_ref); + + BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref)); + + MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in); + + contact_chol_decomposition.Utiv(Utiv_mat_op_res); + pinocchio::cholesky::Utiv(model,data_ref,Utiv_mat_op_ref); + + BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref)); + + // SolveInPlace + VectorXd sol(v_in); + contact_chol_decomposition.solveInPlace(sol); + + VectorXd sol_ref(data.M.inverse()*v_in); + + BOOST_CHECK(sol.isApprox(sol_ref)); + + MatrixXd sol_mat(mat_in); + contact_chol_decomposition.solveInPlace(sol_mat); + + MatrixXd sol_mat_ref(data.M.inverse()*mat_in); + + BOOST_CHECK(sol_mat.isApprox(sol_mat_ref)); + + // solve + MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in); + BOOST_CHECK(sol_copy_mat.isApprox(sol_mat)); + + // inverse + MatrixXd M_inv(model.nv,model.nv); + contact_chol_decomposition.inverse(M_inv); + + MatrixXd M_inv_ref = data.M.inverse(); + BOOST_CHECK(M_inv.isApprox(M_inv_ref)); + + // test retrieve Mass Matrix Cholesky Decomposition + ContactCholeskyDecomposition mass_matrix_chol + = contact_chol_decomposition.getMassMatrixChoeslkyDecomposition(model); + + // test Operational Space Inertia Matrix + MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); + MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); + BOOST_CHECK(iosim.size() == 0); + BOOST_CHECK(osim.size() == 0); + + BOOST_CHECK(mass_matrix_chol == contact_chol_decomposition); + BOOST_CHECK(mass_matrix_chol.U.isApprox(data_ref.U)); + BOOST_CHECK(mass_matrix_chol.D.isApprox(data_ref.D)); + BOOST_CHECK(mass_matrix_chol.Dinv.isApprox(data_ref.Dinv)); +} + +BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_WORLD) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),WORLD); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),WORLD); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + // Compute Mass Matrix + crba(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + // Compute Cholesky decomposition + pinocchio::cholesky::decompose(model,data_ref); + + // Compute Jacobians + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv); + J_RF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF); + J_LF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF); + + const int constraint_dim = 12; + const int total_dim = model.nv + constraint_dim; + + Data::MatrixXs H(total_dim,total_dim); H.setZero(); + H.bottomRightCorner(model.nv, model.nv) = data_ref.M; + H.middleRows<6>(0).rightCols(model.nv) = J_RF; + H.middleRows<6>(6).rightCols(model.nv) = J_LF; + + H.triangularView() = + H.triangularView().transpose(); + + Data data(model); crba(model,data,q); + ContactCholeskyDecomposition contact_chol_decomposition; + contact_chol_decomposition.allocate(model, contact_models); + + ContactCholeskyDecompositionAccessor access(contact_chol_decomposition); + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + if(data.parents_fromRow[(size_t)k] == -1) + BOOST_CHECK(access.getParents_fromRow()[k+constraint_dim] == -1); + else + BOOST_CHECK(access.getParents_fromRow()[k+constraint_dim] == data.parents_fromRow[(size_t)k]+constraint_dim); + } + + contact_chol_decomposition.compute(model,data,contact_models,contact_datas); + + data.M.triangularView() = + data.M.triangularView().transpose(); + + BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv))); + BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv))); + BOOST_CHECK(data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv))); + + Data::MatrixXs M_recomposed = + contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv) + * contact_chol_decomposition.D.tail(model.nv).asDiagonal() + * contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv).transpose(); + BOOST_CHECK(M_recomposed.isApprox(data.M)); + + Data::MatrixXs H_recomposed = contact_chol_decomposition.U * contact_chol_decomposition.D.asDiagonal() * contact_chol_decomposition.U.transpose(); + + BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv,model.nv).isApprox(data.M)); + BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim,model.nv).isApprox(H.topRightCorner(constraint_dim,model.nv))); + BOOST_CHECK(H_recomposed.isApprox(H)); + + // test Operational Space Inertia Matrix + + MatrixXd JMinvJt = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse() * H.middleRows<12>(0).rightCols(model.nv).transpose(); + MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); + MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); + + BOOST_CHECK(iosim.isApprox(JMinvJt)); + BOOST_CHECK(osim.isApprox(JMinvJt.inverse())); + + ContactCholeskyDecomposition contact_chol_decomposition_mu; + contact_chol_decomposition_mu.allocate(model, contact_models); + contact_chol_decomposition_mu.compute(model,data,contact_models,contact_datas,0.); + + BOOST_CHECK(contact_chol_decomposition_mu.D.isApprox(contact_chol_decomposition.D)); + BOOST_CHECK(contact_chol_decomposition_mu.Dinv.isApprox(contact_chol_decomposition.Dinv)); + BOOST_CHECK(contact_chol_decomposition_mu.U.isApprox(contact_chol_decomposition.U)); + + const double mu = 0.1; + contact_chol_decomposition_mu.compute(model,data,contact_models,contact_datas,mu); + Data::MatrixXs H_mu(H); + H_mu.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu); + + Data::MatrixXs H_recomposed_mu + = contact_chol_decomposition_mu.U + * contact_chol_decomposition_mu.D.asDiagonal() + * contact_chol_decomposition_mu.U.transpose(); + + BOOST_CHECK(H_recomposed_mu.isApprox(H_mu)); + + // Test basic operation + VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size())); + MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(),20)); + + // Test Uv + VectorXd Uv_op_res(v_in), Uv_op_ref(v_in); + + contact_chol_decomposition.Uv(Uv_op_res); + Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in; + + BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref)); + + MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in); + + contact_chol_decomposition.Uv(Uv_mat_op_res); + Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in; + + BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref)); + + // Test Utv + VectorXd Utv_op_res(v_in), Utv_op_ref(v_in); + + contact_chol_decomposition.Utv(Utv_op_res); + Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in; + + BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref)); + + MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in); + + contact_chol_decomposition.Utv(Utv_mat_op_res); + Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in; + + BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref)); + + // Test Uiv + VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in); + + contact_chol_decomposition.Uiv(Uiv_op_res); + Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in; + + BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref)); + + MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in); + + contact_chol_decomposition.Uiv(Uiv_mat_op_res); + Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in; + + BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref)); + + // Test Utiv + VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in); + + contact_chol_decomposition.Utiv(Utiv_op_res); + Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in; + + BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref)); + + MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in); + + contact_chol_decomposition.Utiv(Utiv_mat_op_res); + Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in; + + BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref)); + + // SolveInPlace + VectorXd sol(v_in); + contact_chol_decomposition.solveInPlace(sol); + + VectorXd sol_ref(H.inverse()*v_in); + + BOOST_CHECK(sol.isApprox(sol_ref)); + + MatrixXd sol_mat(mat_in), sol_mat_ref(mat_in); + + contact_chol_decomposition.solveInPlace(sol_mat); + sol_mat_ref.noalias() = H.inverse() * mat_in; + + BOOST_CHECK(sol_mat.isApprox(sol_mat_ref)); + + // solve + MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in); + BOOST_CHECK(sol_copy_mat.isApprox(sol_mat)); + + // inverse + MatrixXd H_inv(contact_chol_decomposition.size(),contact_chol_decomposition.size()); + contact_chol_decomposition.inverse(H_inv); + + MatrixXd H_inv_ref = H.inverse(); + BOOST_CHECK(H_inv.isApprox(H_inv_ref)); + + // Check matrix + MatrixXd mat1; + contact_chol_decomposition.matrix(mat1); + BOOST_CHECK(mat1.isApprox(H)); + + MatrixXd mat2(constraint_dim+model.nv,constraint_dim+model.nv); + contact_chol_decomposition.matrix(mat2.middleCols(0,constraint_dim+model.nv)); + BOOST_CHECK(mat2.isApprox(H)); + + MatrixXd mat3 = contact_chol_decomposition.matrix(); + BOOST_CHECK(mat3.isApprox(H)); +} + +BOOST_AUTO_TEST_CASE(contact_cholesky_contact3D_6D_WORLD) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + const std::string RA = "rarm6_joint"; + const std::string LA = "larm6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),WORLD); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_3D,model.getJointId(LF),WORLD); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); +// RigidConstraintModel ci_RA(CONTACT_3D,model.getJointId(RA),LOCAL); +// contact_models.push_back(ci_RA); +// contact_datas.push_back(RigidConstraintData(ci_RA)); +// RigidConstraintModel ci_LA(CONTACT_3D,model.getJointId(LA),LOCAL_WORLD_ALIGNED); +// contact_models.push_back(ci_LA); +// contact_datas.push_back(RigidConstraintData(ci_LA)); + + // Compute Mass Matrix + crba(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + // Compute Cholesky decomposition + pinocchio::cholesky::decompose(model,data_ref); + + // Compute Jacobians + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv), J_RA(6,model.nv), J_LA(6,model.nv); + J_RF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF); + J_LF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF); + J_RA.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA); + J_LA.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL_WORLD_ALIGNED, J_LA); + + const int constraint_dim = 9; + const int total_dim = model.nv + constraint_dim; + + Data::MatrixXs H(total_dim,total_dim); H.setZero(); + H.bottomRightCorner(model.nv, model.nv) = data_ref.M; + H.middleRows<6>(0).rightCols(model.nv) = J_RF; + H.middleRows<3>(6).rightCols(model.nv) = J_LF.middleRows<3>(Motion::LINEAR); +// H.middleRows<3>(9).rightCols(model.nv) = J_RA.middleRows<3>(Motion::LINEAR); +// H.middleRows<3>(12).rightCols(model.nv) = J_LA.middleRows<3>(Motion::LINEAR); + + H.triangularView() = + H.triangularView().transpose(); + + Data data(model); crba(model,data,q); + ContactCholeskyDecomposition contact_chol_decomposition; + contact_chol_decomposition.allocate(model, contact_models); + + ContactCholeskyDecompositionAccessor access(contact_chol_decomposition); + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + if(data.parents_fromRow[(size_t)k] == -1) + BOOST_CHECK(access.getParents_fromRow()[k+constraint_dim] == -1); + else + BOOST_CHECK(access.getParents_fromRow()[k+constraint_dim] == data.parents_fromRow[(size_t)k]+constraint_dim); + } + + contact_chol_decomposition.compute(model,data,contact_models,contact_datas); + + data.M.triangularView() = + data.M.triangularView().transpose(); + + BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv))); + BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv))); + BOOST_CHECK(data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv))); + + Data::MatrixXs M_recomposed = + contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv) + * contact_chol_decomposition.D.tail(model.nv).asDiagonal() + * contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv).transpose(); + BOOST_CHECK(M_recomposed.isApprox(data.M)); + + Data::MatrixXs H_recomposed = contact_chol_decomposition.U * contact_chol_decomposition.D.asDiagonal() * contact_chol_decomposition.U.transpose(); + + BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv,model.nv).isApprox(data.M)); + BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim,model.nv).isApprox(H.topRightCorner(constraint_dim,model.nv))); + BOOST_CHECK(H_recomposed.isApprox(H)); + + // Test basic operation + VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size())); + MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(),20)); + + // Test Uv + VectorXd Uv_op_res(v_in), Uv_op_ref(v_in); + + contact_chol_decomposition.Uv(Uv_op_res); + Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in; + + BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref)); + + MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in); + + contact_chol_decomposition.Uv(Uv_mat_op_res); + Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in; + + BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref)); + + // Test Utv + VectorXd Utv_op_res(v_in), Utv_op_ref(v_in); + + contact_chol_decomposition.Utv(Utv_op_res); + Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in; + + BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref)); + + MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in); + + contact_chol_decomposition.Utv(Utv_mat_op_res); + Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in; + + BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref)); + + // Test Uiv + VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in); + + contact_chol_decomposition.Uiv(Uiv_op_res); + Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in; + + BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref)); + + MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in); + + contact_chol_decomposition.Uiv(Uiv_mat_op_res); + Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in; + + BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref)); + + // Test Utiv + VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in); + + contact_chol_decomposition.Utiv(Utiv_op_res); + Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in; + + BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref)); + + MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in); + + contact_chol_decomposition.Utiv(Utiv_mat_op_res); + Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in; + + BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref)); + + // SolveInPlace + VectorXd sol(v_in); + contact_chol_decomposition.solveInPlace(sol); + + VectorXd sol_ref(H.inverse()*v_in); + + BOOST_CHECK(sol.isApprox(sol_ref)); + + MatrixXd sol_mat(mat_in), sol_mat_ref(mat_in); + + contact_chol_decomposition.solveInPlace(sol_mat); + sol_mat_ref.noalias() = H.inverse() * mat_in; + + BOOST_CHECK(sol_mat.isApprox(sol_mat_ref)); + + // solve + MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in); + BOOST_CHECK(sol_copy_mat.isApprox(sol_mat)); + + // inverse + MatrixXd H_inv(contact_chol_decomposition.size(),contact_chol_decomposition.size()); + contact_chol_decomposition.inverse(H_inv); + + MatrixXd H_inv_ref = H.inverse(); + BOOST_CHECK(H_inv.isApprox(H_inv_ref)); + + // Check matrix + MatrixXd mat1; + contact_chol_decomposition.matrix(mat1); + BOOST_CHECK(mat1.isApprox(H)); + + MatrixXd mat2(constraint_dim+model.nv,constraint_dim+model.nv); + contact_chol_decomposition.matrix(mat2.middleCols(0,constraint_dim+model.nv)); + BOOST_CHECK(mat2.isApprox(H)); + + MatrixXd mat3 = contact_chol_decomposition.matrix(); + BOOST_CHECK(mat3.isApprox(H)); +} + +BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_LOCAL) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),WORLD); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + // Compute Mass Matrix + crba(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + // Compute Cholesky decomposition + pinocchio::cholesky::decompose(model,data_ref); + + // Compute Jacobians + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv); + J_RF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), ci_RF.reference_frame, J_RF); + J_LF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), ci_LF.reference_frame, J_LF); + + const int constraint_dim = 12; + const int total_dim = model.nv + constraint_dim; + + Data::MatrixXs H(total_dim,total_dim); H.setZero(); + H.bottomRightCorner(model.nv, model.nv) = data_ref.M; + H.middleRows<6>(0).rightCols(model.nv) = J_RF; + H.middleRows<6>(6).rightCols(model.nv) = J_LF; + + H.triangularView() = + H.triangularView().transpose(); + + Data data(model); crba(model,data,q); + ContactCholeskyDecomposition contact_chol_decomposition; + contact_chol_decomposition.allocate(model, contact_models); + contact_chol_decomposition.compute(model,data,contact_models,contact_datas); + + data.M.triangularView() = + data.M.triangularView().transpose(); + + Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); + + BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv,model.nv).isApprox(data.M)); + BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim,model.nv).isApprox(H.topRightCorner(constraint_dim,model.nv))); + BOOST_CHECK(H_recomposed.isApprox(H)); + + // inverse + MatrixXd H_inv(contact_chol_decomposition.size(),contact_chol_decomposition.size()); + contact_chol_decomposition.inverse(H_inv); + + MatrixXd H_inv_ref = H_recomposed.inverse(); + BOOST_CHECK(H_inv.isApprox(H_inv_ref)); +} + +BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_LOCAL_WORLD_ALIGNED) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL_WORLD_ALIGNED); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),WORLD); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + // Compute Mass Matrix + crba(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + // Compute Cholesky decomposition + pinocchio::cholesky::decompose(model,data_ref); + + // Compute Jacobians + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv); + J_RF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), ci_RF.reference_frame, J_RF); + J_LF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), ci_LF.reference_frame, J_LF); + + const int constraint_dim = 12; + const int total_dim = model.nv + constraint_dim; + + Data::MatrixXs H(total_dim,total_dim); H.setZero(); + H.bottomRightCorner(model.nv, model.nv) = data_ref.M; + H.middleRows<6>(0).rightCols(model.nv) = J_RF; + H.middleRows<6>(6).rightCols(model.nv) = J_LF; + + H.triangularView() = + H.triangularView().transpose(); + + Data data(model); crba(model,data,q); + ContactCholeskyDecomposition contact_chol_decomposition; + contact_chol_decomposition.allocate(model, contact_models); + contact_chol_decomposition.compute(model,data,contact_models,contact_datas); + + data.M.triangularView() = + data.M.triangularView().transpose(); + + Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); + + BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv,model.nv).isApprox(data.M)); + BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim,model.nv).isApprox(H.topRightCorner(constraint_dim,model.nv))); + BOOST_CHECK(H_recomposed.isApprox(H)); + + // inverse + MatrixXd H_inv(contact_chol_decomposition.size(),contact_chol_decomposition.size()); + contact_chol_decomposition.inverse(H_inv); + + MatrixXd H_inv_ref = H_recomposed.inverse(); + BOOST_CHECK(H_inv.isApprox(H_inv_ref)); +} + +BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_by_joint_2) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + const std::string RA = "rarm6_joint"; + const std::string LA = "larm6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,0,model.getJointId(RF),WORLD); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,0,model.getJointId(LF),WORLD); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + RigidConstraintModel ci_RA(CONTACT_6D,0,model.getJointId(RA),LOCAL_WORLD_ALIGNED); + contact_models.push_back(ci_RA); + contact_datas.push_back(RigidConstraintData(ci_RA)); + RigidConstraintModel ci_LA(CONTACT_6D,0,model.getJointId(LA),LOCAL); + contact_models.push_back(ci_LA); + contact_datas.push_back(RigidConstraintData(ci_LA)); + + // Compute Mass Matrix + crba(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + // Compute Cholesky decomposition + pinocchio::cholesky::decompose(model,data_ref); + + // Compute Jacobians + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv), J_RA(6,model.nv), J_LA(6,model.nv); + J_RF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF); + J_LF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF); + J_RA.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RA), WORLD, J_RA); + J_LA.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LA), WORLD, J_LA); + + const int constraint_dim = 24; + const int total_dim = model.nv + constraint_dim; + + const SE3 oMRA_wla = SE3(SE3::Matrix3::Identity(),ci_RA.joint1_placement.translation()); + + const double mu = 0.1; + Data::MatrixXs H(total_dim,total_dim); H.setZero(); + H.topLeftCorner(constraint_dim,constraint_dim).diagonal().fill(-mu); + H.bottomRightCorner(model.nv, model.nv) = data_ref.M; + H.middleRows<6>(0).rightCols(model.nv) = -J_RF; + H.middleRows<6>(6).rightCols(model.nv) = -J_LF; + H.middleRows<6>(12).rightCols(model.nv) = -oMRA_wla.toActionMatrixInverse()*J_RA; + H.middleRows<6>(18).rightCols(model.nv) = -ci_LA.joint1_placement.toActionMatrixInverse()*J_LA; + + H.triangularView() = + H.triangularView().transpose(); + + Data data(model); crba(model,data,q); + ContactCholeskyDecomposition contact_chol_decomposition; + contact_chol_decomposition.allocate(model, contact_models); + contact_chol_decomposition.compute(model,data,contact_models,contact_datas,mu); + + data.M.triangularView() = + data.M.triangularView().transpose(); + + Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); + + BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv,model.nv).isApprox(data.M)); + BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim,model.nv).isApprox(H.topRightCorner(constraint_dim,model.nv))); + BOOST_CHECK(H_recomposed.isApprox(H)); + + VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size())); + MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(),20)); + + // Test Uv + VectorXd Uv_op_res(v_in), Uv_op_ref(v_in); + + contact_chol_decomposition.Uv(Uv_op_res); + Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in; + + BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref)); + + MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in); + + contact_chol_decomposition.Uv(Uv_mat_op_res); + Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in; + + BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref)); + + // Test Utv + VectorXd Utv_op_res(v_in), Utv_op_ref(v_in); + + contact_chol_decomposition.Utv(Utv_op_res); + Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in; + + BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref)); + + MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in); + + contact_chol_decomposition.Utv(Utv_mat_op_res); + Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in; + + BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref)); + + // Test Uiv + VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in); + + contact_chol_decomposition.Uiv(Uiv_op_res); + Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in; + + BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref)); + + MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in); + + contact_chol_decomposition.Uiv(Uiv_mat_op_res); + Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in; + + BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref)); + + // Test Utiv + VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in); + + contact_chol_decomposition.Utiv(Utiv_op_res); + Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in; + + BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref)); + + MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in); + + contact_chol_decomposition.Utiv(Utiv_mat_op_res); + Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in; + + BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref)); + + // inverse + MatrixXd H_inv(contact_chol_decomposition.size(),contact_chol_decomposition.size()); + contact_chol_decomposition.inverse(H_inv); + + MatrixXd H_inv2 = contact_chol_decomposition.U.transpose().inverse() * contact_chol_decomposition.Dinv.asDiagonal() * contact_chol_decomposition.U.inverse(); + + MatrixXd H_inv3 = MatrixXd::Identity(contact_chol_decomposition.size(),contact_chol_decomposition.size()); + contact_chol_decomposition.solveInPlace(H_inv3); + + MatrixXd H_inv_ref = H_recomposed.inverse(); + BOOST_CHECK(H_inv.topLeftCorner(constraint_dim,constraint_dim).isApprox(H_inv_ref.topLeftCorner(constraint_dim,constraint_dim))); + BOOST_CHECK(H_inv.bottomRightCorner(model.nv,model.nv).isApprox(H_inv_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(H_inv.topRightCorner(constraint_dim,model.nv).isApprox(H_inv_ref.topRightCorner(constraint_dim,model.nv))); + + BOOST_CHECK(H_inv_ref.isApprox(H_inv)); + BOOST_CHECK(H_inv_ref.isApprox(H_inv2)); + BOOST_CHECK(H_inv_ref.isApprox(H_inv3)); + const VectorXd ei = VectorXd::Unit(contact_chol_decomposition.size(),constraint_dim); + VectorXd ei_inv = ei; + contact_chol_decomposition.solveInPlace(ei_inv); + VectorXd ei_inv2 = ei; + contact_chol_decomposition.Uiv(ei_inv2); + ei_inv2 = contact_chol_decomposition.Dinv.asDiagonal() * ei_inv2; + contact_chol_decomposition.Utiv(ei_inv2); + + BOOST_CHECK(ei_inv.isApprox(H_inv_ref*ei)); + BOOST_CHECK(ei_inv2.isApprox(H_inv_ref*ei)); + BOOST_CHECK(ei_inv.isApprox(H_inv*ei)); + BOOST_CHECK(ei_inv2.isApprox(H_inv*ei)); +} + +BOOST_AUTO_TEST_CASE(contact_cholesky_contact3D_6D_WORLD_by_joint_2) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + const std::string RA = "rarm6_joint"; + const std::string LA = "larm6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,0,model.getJointId(RF),WORLD); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_3D,0,model.getJointId(LF),WORLD); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + RigidConstraintModel ci_RA(CONTACT_3D,0,model.getJointId(RA),LOCAL); + contact_models.push_back(ci_RA); + contact_datas.push_back(RigidConstraintData(ci_RA)); + RigidConstraintModel ci_LA(CONTACT_3D,0,model.getJointId(LA),LOCAL_WORLD_ALIGNED); + contact_models.push_back(ci_LA); + contact_datas.push_back(RigidConstraintData(ci_LA)); + + // Compute Mass Matrix + crba(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + // Compute Cholesky decomposition + pinocchio::cholesky::decompose(model,data_ref); + + // Compute Jacobians + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv), J_RA(6,model.nv), J_LA(6,model.nv); + J_RF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF); + J_LF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF); + J_RA.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA); + J_LA.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL_WORLD_ALIGNED, J_LA); + + const int constraint_dim = 15; + const int total_dim = model.nv + constraint_dim; + + Data::MatrixXs H(total_dim,total_dim); H.setZero(); + H.bottomRightCorner(model.nv, model.nv) = data_ref.M; + H.middleRows<6>(0).rightCols(model.nv) = -J_RF; + H.middleRows<3>(6).rightCols(model.nv) = -J_LF.middleRows<3>(Motion::LINEAR); + H.middleRows<3>(9).rightCols(model.nv) = -data_ref.oMi[model.getJointId(RA)].rotation()*J_RA.middleRows<3>(Motion::LINEAR); + H.middleRows<3>(12).rightCols(model.nv) = -J_LA.middleRows<3>(Motion::LINEAR); + + H.triangularView() = + H.triangularView().transpose(); + + Data data(model); crba(model,data,q); + ContactCholeskyDecomposition contact_chol_decomposition; + contact_chol_decomposition.allocate(model, contact_models); + + contact_chol_decomposition.compute(model,data,contact_models,contact_datas); + + data.M.triangularView() = + data.M.triangularView().transpose(); + + BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv))); + BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv))); + BOOST_CHECK(data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv))); + + Data::MatrixXs M_recomposed = + contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv) + * contact_chol_decomposition.D.tail(model.nv).asDiagonal() + * contact_chol_decomposition.U.bottomRightCorner(model.nv,model.nv).transpose(); + BOOST_CHECK(M_recomposed.isApprox(data.M)); + + Data::MatrixXs H_recomposed = contact_chol_decomposition.U * contact_chol_decomposition.D.asDiagonal() * contact_chol_decomposition.U.transpose(); + + BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv,model.nv).isApprox(data.M)); + BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim,model.nv).isApprox(H.topRightCorner(constraint_dim,model.nv))); + BOOST_CHECK(H_recomposed.isApprox(H)); +} + +BOOST_AUTO_TEST_CASE(loop_contact_cholesky_contact6D) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + const std::string RA = "rarm6_joint"; + const std::string LA = "larm6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel loop_RF_LF_local(CONTACT_6D,model.getJointId(RF),model.getJointId(LF),LOCAL); + RigidConstraintModel loop_RF_LF_world(CONTACT_6D,model.getJointId(RF),model.getJointId(LF),WORLD); + RigidConstraintModel loop_RA_LA_lwa(CONTACT_6D,model.getJointId(RA),model.getJointId(LA),LOCAL_WORLD_ALIGNED); + + loop_RF_LF_local.joint1_placement.setRandom(); + loop_RF_LF_local.joint2_placement.setRandom(); + + loop_RA_LA_lwa.joint1_placement.setRandom(); + loop_RA_LA_lwa.joint2_placement.setRandom(); + + contact_models.push_back(loop_RF_LF_local); + contact_datas.push_back(RigidConstraintData(loop_RF_LF_local)); + + contact_models.push_back(loop_RF_LF_world); + contact_datas.push_back(RigidConstraintData(loop_RF_LF_world)); + + contact_models.push_back(loop_RA_LA_lwa); + contact_datas.push_back(RigidConstraintData(loop_RA_LA_lwa)); + + // Compute Mass Matrix + crba(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + // Compute Cholesky decomposition + const double mu = 0.1; + pinocchio::cholesky::decompose(model,data_ref); + + // Compute Jacobians + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv), J_RA_local(6,model.nv), J_LA_local(6,model.nv); + J_RF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF); + Data::Matrix6x J_RF_local = loop_RF_LF_local.joint1_placement.toActionMatrixInverse() * J_RF; + J_LF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF); + Data::Matrix6x J_LF_local = loop_RF_LF_local.joint2_placement.toActionMatrixInverse() * J_LF; + J_RA_local.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA_local); + J_RA_local = loop_RA_LA_lwa.joint1_placement.toActionMatrixInverse() * J_RA_local; + J_LA_local.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL, J_LA_local); + J_LA_local = loop_RA_LA_lwa.joint2_placement.toActionMatrixInverse() * J_LA_local; + + Data::Matrix6x J_RF_world(6,model.nv), J_LF_world(6,model.nv); + J_RF_world.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF_world); + J_LF_world.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF_world); + + forwardKinematics(model,data_ref,q); + const SE3 oM1_loop1 = data_ref.oMi[loop_RF_LF_local.joint1_id] * loop_RF_LF_local.joint1_placement; + const SE3 oM2_loop1 = data_ref.oMi[loop_RF_LF_local.joint2_id] * loop_RF_LF_local.joint2_placement; + const SE3 _1M2_loop1 = oM1_loop1.inverse() * oM2_loop1; + const SE3 oM1_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint1_id] * loop_RA_LA_lwa.joint1_placement; + const SE3 oM2_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint2_id] * loop_RA_LA_lwa.joint2_placement; + const SE3 _1M2_loop2 = oM1_loop2.inverse() * oM2_loop2; + + const int constraint_dim = 18; + const int total_dim = model.nv + constraint_dim; + + Data::MatrixXs H(total_dim,total_dim); H.setZero(); + H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu); + H.bottomRightCorner(model.nv, model.nv) = data_ref.M; + H.middleRows<6>(0).rightCols(model.nv) = J_RF_local - _1M2_loop1.toActionMatrix() * J_LF_local; + H.middleRows<6>(6).rightCols(model.nv) = J_RF_world - J_LF_world; + const SE3 oM1_loop2_lwa = SE3(oM1_loop2.rotation(),SE3::Vector3::Zero()); + H.middleRows<6>(12).rightCols(model.nv) = oM1_loop2_lwa.toActionMatrix()*J_RA_local - (oM1_loop2_lwa.toActionMatrix()*_1M2_loop2.toActionMatrix())*J_LA_local; + + H.triangularView() = + H.triangularView().transpose(); + + Data data(model); crba(model,data,q); + ContactCholeskyDecomposition contact_chol_decomposition; + contact_chol_decomposition.allocate(model, contact_models); + contact_chol_decomposition.compute(model,data,contact_models,contact_datas,mu); + + Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); + + data.M.triangularView() = + data.M.triangularView().transpose(); + BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv,model.nv).isApprox(data.M)); + BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim,model.nv).isApprox(H.topRightCorner(constraint_dim,model.nv))); + BOOST_CHECK(H_recomposed.isApprox(H)); + + // inverse + MatrixXd H_inv(contact_chol_decomposition.size(),contact_chol_decomposition.size()); + contact_chol_decomposition.inverse(H_inv); + + MatrixXd H_inv_ref = H_recomposed.inverse(); + + BOOST_CHECK(H_inv_ref.isApprox(H_inv)); +} + +BOOST_AUTO_TEST_CASE(loop_contact_cholesky_contact_3d) +{ + using namespace Eigen; + using namespace pinocchio; + using namespace pinocchio::cholesky; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + const std::string RA = "rarm6_joint"; + const std::string LA = "larm6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel loop_RF_LF_local(CONTACT_3D,model.getJointId(RF),model.getJointId(LF),LOCAL); + RigidConstraintModel loop_RF_LF_world(CONTACT_3D,model.getJointId(RF),model.getJointId(LF),WORLD); + RigidConstraintModel loop_RA_LA_lwa(CONTACT_3D,model.getJointId(RA),model.getJointId(LA),LOCAL_WORLD_ALIGNED); + + loop_RF_LF_local.joint1_placement.setRandom(); + loop_RF_LF_local.joint2_placement.setRandom(); + + loop_RF_LF_world.joint1_placement.setRandom(); + loop_RF_LF_world.joint2_placement.setRandom(); + + loop_RA_LA_lwa.joint1_placement.setRandom(); + loop_RA_LA_lwa.joint2_placement.setRandom(); + + contact_models.push_back(loop_RF_LF_local); + contact_datas.push_back(RigidConstraintData(loop_RF_LF_local)); + + contact_models.push_back(loop_RF_LF_world); + contact_datas.push_back(RigidConstraintData(loop_RF_LF_world)); + + contact_models.push_back(loop_RA_LA_lwa); + contact_datas.push_back(RigidConstraintData(loop_RA_LA_lwa)); + + // Compute Mass Matrix + crba(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.triangularView().transpose(); + + // Compute Cholesky decomposition + const double mu = 0.1; + pinocchio::cholesky::decompose(model,data_ref); + + // Compute Jacobians + Data::Matrix6x J_RF(6,model.nv), J_LF(6,model.nv), J_RA_local(6,model.nv), J_LA_local(6,model.nv); + J_RF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF); + const Data::Matrix6x J_RF_local = loop_RF_LF_local.joint1_placement.toActionMatrixInverse() * J_RF; + J_LF.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF); + const Data::Matrix6x J_LF_local = loop_RF_LF_local.joint2_placement.toActionMatrixInverse() * J_LF; + J_RA_local.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA_local); + J_RA_local = loop_RA_LA_lwa.joint1_placement.toActionMatrixInverse() * J_RA_local; + J_LA_local.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL, J_LA_local); + J_LA_local = loop_RA_LA_lwa.joint2_placement.toActionMatrixInverse() * J_LA_local; + + Data::Matrix6x J_RF_world(6,model.nv), J_LF_world(6,model.nv); + J_RF_world.setZero(); + getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF_world); + J_LF_world.setZero(); + getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF_world); + + forwardKinematics(model,data_ref,q); + const SE3 oM1_loop1 = data_ref.oMi[loop_RF_LF_local.joint1_id] * loop_RF_LF_local.joint1_placement; + const SE3 oM2_loop1 = data_ref.oMi[loop_RF_LF_local.joint2_id] * loop_RF_LF_local.joint2_placement; + const SE3 _1M2_loop1 = oM1_loop1.inverse() * oM2_loop1; + const SE3 oM1_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint1_id] * loop_RA_LA_lwa.joint1_placement; + const SE3 oM2_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint2_id] * loop_RA_LA_lwa.joint2_placement; + const SE3 _1M2_loop2 = oM1_loop2.inverse() * oM2_loop2; + + const int constraint_dim = 9; + const int total_dim = model.nv + constraint_dim; + + Data::MatrixXs H(total_dim,total_dim); H.setZero(); + H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu); + H.bottomRightCorner(model.nv, model.nv) = data_ref.M; + H.middleRows<3>(0).rightCols(model.nv) = J_RF_local.middleRows<3>(Motion::LINEAR) - _1M2_loop1.rotation() * J_LF_local.middleRows<3>(Motion::LINEAR); + H.middleRows<3>(3).rightCols(model.nv) = J_RF_world.middleRows<3>(Motion::LINEAR) - J_LF_world.middleRows<3>(Motion::LINEAR); + const SE3 oM1_loop2_lwa = SE3(oM1_loop2.rotation(),SE3::Vector3::Zero()); + const SE3 oM2_loop2_lwa = SE3(oM2_loop2.rotation(),SE3::Vector3::Zero()); + H.middleRows<3>(6).rightCols(model.nv) = (oM1_loop2_lwa.toActionMatrix()*J_RA_local - (oM2_loop2_lwa.toActionMatrix())*J_LA_local).middleRows<3>(Motion::LINEAR); + + H.triangularView() = + H.triangularView().transpose(); + + Data data(model); crba(model,data,q); + ContactCholeskyDecomposition contact_chol_decomposition; + contact_chol_decomposition.allocate(model, contact_models); + contact_chol_decomposition.compute(model,data,contact_models,contact_datas,mu); + BOOST_CHECK(contact_datas[0].c1Mc2.isApprox(_1M2_loop1)); + BOOST_CHECK(contact_datas[2].c1Mc2.isApprox(_1M2_loop2)); + + Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); + + data.M.triangularView() = + data.M.triangularView().transpose(); + BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv,model.nv).isApprox(data.M)); + BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim,model.nv).isApprox(H.topRightCorner(constraint_dim,model.nv))); + BOOST_CHECK(H_recomposed.isApprox(H)); + + std::cout << "H_recomposed.topRightCorner(constraint_dim,model.nv):\n" << H_recomposed.topRightCorner(constraint_dim,model.nv) << std::endl; + std::cout << "H.topRightCorner(constraint_dim,model.nv):\n" << H .topRightCorner(constraint_dim,model.nv) << std::endl; + + // inverse + MatrixXd H_inv(contact_chol_decomposition.size(),contact_chol_decomposition.size()); + contact_chol_decomposition.inverse(H_inv); + + MatrixXd H_inv_ref = H_recomposed.inverse(); + + BOOST_CHECK(H_inv_ref.isApprox(H_inv)); +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/contact-dynamics-derivatives.cpp b/unittest/contact-dynamics-derivatives.cpp index 03853b0240..d659a19e8e 100644 --- a/unittest/contact-dynamics-derivatives.cpp +++ b/unittest/contact-dynamics-derivatives.cpp @@ -1,328 +1,1759 @@ // -// Copyright (c) 2019-2020 INRIA +// Copyright (c) 2020 CNRS INRIA // +#include + #include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/rnea-derivatives.hpp" +#include "pinocchio/algorithm/aba-derivatives.hpp" #include "pinocchio/algorithm/kinematics-derivatives.hpp" -#include "pinocchio/algorithm/contact-dynamics.hpp" +#include "pinocchio/algorithm/frames-derivatives.hpp" +#include "pinocchio/algorithm/constrained-dynamics.hpp" +#include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/parsers/sample-models.hpp" - -#include +#include "pinocchio/spatial/classic-acceleration.hpp" #include #include -BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) using namespace Eigen; using namespace pinocchio; -BOOST_AUTO_TEST_CASE ( test_FD_with_contact_cst_gamma ) +BOOST_AUTO_TEST_CASE(test_sparse_constraint_dynamics_derivatives_no_contact) { - pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model,true); - pinocchio::Data data(model), data_check(model); - - VectorXd q = VectorXd::Ones(model.nq); - normalize(model,q); + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); - computeJointJacobians(model, data, q); + // Contact models and data + const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) empty_contact_data; + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); - VectorXd v = VectorXd::Ones(model.nv); + initConstraintDynamics(model,data,empty_contact_models); + constraintDynamics(model,data,q,v,tau,empty_contact_models,empty_contact_data,prox_settings); + data.M.triangularView() + = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model,data,empty_contact_models,empty_contact_data); + + // Reference values + computeABADerivatives(model, data_ref, q, v, tau); + forwardKinematics(model, data_ref, q, v, data_ref.ddq); + BOOST_CHECK(data_ref.ddq.isApprox(data.ddq)); + + for(size_t k = 1; k < (size_t)model.njoints; ++k) + { + BOOST_CHECK(data_ref.oMi[k].isApprox(data.oMi[k])); + BOOST_CHECK(data_ref.ov[k].isApprox(data.ov[k])); + BOOST_CHECK(data_ref.v[k].isApprox(data.v[k])); + BOOST_CHECK(data_ref.a[k].isApprox(data.a[k])); + BOOST_CHECK(data_ref.oa[k].isApprox(data.oa[k])); + } + + BOOST_CHECK(data_ref.ddq.isApprox(data.ddq)); + BOOST_CHECK(data_ref.dVdq.isApprox(data.dVdq)); + BOOST_CHECK(data_ref.J.isApprox(data.J)); + BOOST_CHECK(data_ref.dAdq.isApprox(data.dAdq)); + BOOST_CHECK(data_ref.dAdv.isApprox(data.dAdv)); + BOOST_CHECK(data_ref.dFdq.isApprox(data.dFdq)); + BOOST_CHECK(data_ref.dFdv.isApprox(data.dFdv)); + + BOOST_CHECK(data_ref.dtau_dq.isApprox(data.dtau_dq)); + BOOST_CHECK(data_ref.dtau_dv.isApprox(data.dtau_dv)); + + BOOST_CHECK(data_ref.ddq_dq.isApprox(data.ddq_dq)); + BOOST_CHECK(data_ref.ddq_dv.isApprox(data.ddq_dv)); + BOOST_CHECK(data_ref.Minv.isApprox(data.ddq_dtau)); +} + +BOOST_AUTO_TEST_CASE(test_sparse_constraint_dynamics_derivatives) +{ + using namespace Eigen; + using namespace pinocchio; + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_LF(CONTACT_6D,LF_id,LOCAL); + ci_LF.joint1_placement.setRandom(); + RigidConstraintModel ci_RF(CONTACT_3D,RF_id,LOCAL); + ci_RF.joint1_placement.setRandom(); + + contact_models.push_back(ci_LF); contact_data.push_back(RigidConstraintData(ci_LF)); + contact_models.push_back(ci_RF); contact_data.push_back(RigidConstraintData(ci_RF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + data.M.triangularView() + = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model,data,contact_models,contact_data); + + // Reference values + crba(model, data_ref, q); + data_ref.M.triangularView() + = data_ref.M.transpose().triangularView(); + container::aligned_vector fext((size_t)model.njoints,Force::Zero()); + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_data[k]; + fext[cmodel.joint1_id] = cmodel.joint1_placement.act(cdata.contact_force); + + BOOST_CHECK(cdata.oMc1.isApprox(data_ref.oMi[cmodel.joint1_id] * cmodel.joint1_placement)); + } + + computeABADerivatives(model, data_ref, q, v, tau, fext); + forwardKinematics(model,data_ref,q,v); + + BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); + BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq)); + BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv)); + BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq)); + BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv)); + + MatrixXd + vLF_partial_dq(MatrixXd::Zero(6, model.nv)), + aLF_partial_dq(MatrixXd::Zero(6, model.nv)), + aLF_partial_dv(MatrixXd::Zero(6, model.nv)), + aLF_partial_da(MatrixXd::Zero(6, model.nv)); + + MatrixXd + vRF_partial_dq(MatrixXd::Zero(6, model.nv)), + aRF_partial_dq(MatrixXd::Zero(6, model.nv)), + aRF_partial_dv(MatrixXd::Zero(6, model.nv)), + aRF_partial_da(MatrixXd::Zero(6, model.nv)); + + getFrameAccelerationDerivatives(model, data_ref, + LF_id, + ci_LF.joint1_placement, + LOCAL, + vLF_partial_dq, + aLF_partial_dq, + aLF_partial_dv, + aLF_partial_da); + getFrameAccelerationDerivatives(model, data_ref, + RF_id, + ci_RF.joint1_placement, + LOCAL, + vRF_partial_dq, + aRF_partial_dq, + aRF_partial_dv, + aRF_partial_da); + + MatrixXd Jc(constraint_dim, model.nv); + Jc << aLF_partial_da, + aRF_partial_da.topRows<3>(); + + MatrixXd K(model.nv+constraint_dim, model.nv+constraint_dim); + K << data_ref.M, Jc.transpose(), + Jc, MatrixXd::Zero(constraint_dim,constraint_dim); + const MatrixXd Kinv = K.inverse(); + + MatrixXd osim((Jc * data_ref.M.inverse() * Jc.transpose()).inverse()); + BOOST_CHECK(data.osim.isApprox(osim)); + + MatrixXd ac_partial_dq(constraint_dim, model.nv); + + BOOST_CHECK(data.ov[RF_id].isApprox(data_ref.oMi[RF_id].act(data_ref.v[RF_id]))); + aRF_partial_dq.topRows<3>() += + cross(ci_RF.joint1_placement.actInv(data_ref.v[RF_id]).angular(), + vRF_partial_dq.topRows<3>()) + - cross(ci_RF.joint1_placement.actInv(data_ref.v[RF_id]).linear(), + vRF_partial_dq.bottomRows<3>()); + + BOOST_CHECK(data.ov[LF_id].isApprox(data_ref.oMi[LF_id].act(data_ref.v[LF_id]))); + + ac_partial_dq << aLF_partial_dq, + aRF_partial_dq.topRows<3>(); + + MatrixXd dac_dq = ac_partial_dq - Jc * data_ref.Minv*data_ref.dtau_dq; + + BOOST_CHECK(data.dac_dq.isApprox(dac_dq,1e-8)); + BOOST_CHECK(Kinv.bottomLeftCorner(constraint_dim, model.nv).isApprox(osim*Jc*data_ref.Minv)); + + MatrixXd df_dq = Kinv.bottomLeftCorner(constraint_dim, model.nv)* data_ref.dtau_dq + + Kinv.bottomRightCorner(constraint_dim,constraint_dim)*ac_partial_dq; + + MatrixXd ddq_dq = data_ref.Minv * (-data_ref.dtau_dq + Jc.transpose() * df_dq); + + BOOST_CHECK(df_dq.isApprox(data.dlambda_dq)); + BOOST_CHECK(ddq_dq.isApprox(data.ddq_dq)); +} + +pinocchio::Motion computeAcceleration(const pinocchio::Model & model, + const pinocchio::Data & data, + const pinocchio::JointIndex & joint_id, + const pinocchio::ReferenceFrame reference_frame, + const pinocchio::ContactType contact_type, + const pinocchio::SE3 & placement = pinocchio::SE3::Identity()) +{ + PINOCCHIO_UNUSED_VARIABLE(model); + using namespace pinocchio; + Motion res(Motion::Zero()); + + const Data::SE3 & oMi = data.oMi[joint_id]; + const Data::SE3 oMc = oMi * placement; + + const Data::SE3 & iMc = placement; + const Motion ov = oMi.act(data.v[joint_id]); + const Motion oa = oMi.act(data.a[joint_id]); + + switch (reference_frame) + { + case WORLD: + if(contact_type == CONTACT_6D) + return oa; + classicAcceleration(ov,oa,res.linear()); + res.angular() = oa.angular(); + break; + case LOCAL_WORLD_ALIGNED: + if(contact_type == CONTACT_6D) + { + res.linear() = oMc.rotation() * iMc.actInv(data.a[joint_id]).linear(); + res.angular() = oMi.rotation() * data.a[joint_id].angular(); + } + else + { + res.linear() = oMc.rotation() * classicAcceleration(data.v[joint_id],data.a[joint_id],iMc); + res.angular() = oMi.rotation() * data.a[joint_id].angular(); + } + break; + case LOCAL: + if(contact_type == CONTACT_6D) + return oMc.actInv(oa); + classicAcceleration(data.v[joint_id],data.a[joint_id],iMc,res.linear()); + res.angular() = iMc.rotation().transpose() * data.a[joint_id].angular(); + break; + default: + break; + } + + return res; +} + +pinocchio::Motion getContactAcceleration(const Model & model, + const Data & data, + const RigidConstraintModel & cmodel) +{ + return computeAcceleration(model,data,cmodel.joint1_id,cmodel.reference_frame,cmodel.type,cmodel.joint1_placement); +} + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_fd) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); VectorXd tau = VectorXd::Random(model.nv); + + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_LF(CONTACT_6D,LF_id,LOCAL); + ci_LF.joint1_placement.setRandom(); + + contact_models.push_back(ci_LF); contact_data.push_back(RigidConstraintData(ci_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + +PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) +createData(const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) & contact_models) +{ + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) contact_datas; + for(size_t k = 0; k < contact_models.size(); ++k) + contact_datas.push_back(pinocchio::RigidConstraintData(contact_models[k])); + return contact_datas; +} + +BOOST_AUTO_TEST_CASE(test_correction_6D) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + const double mu = 0.; + ProximalSettings prox_settings(1e-12,mu,1); const std::string RF = "rleg6_joint"; const Model::JointIndex RF_id = model.getJointId(RF); -// const std::string LF = "lleg6_joint"; -// const Model::JointIndex LF_id = model.getJointId(LF); + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; - Data::Matrix6x J_RF(6,model.nv); J_RF.setZero(); - getJointJacobian(model, data, RF_id, LOCAL, J_RF); - Motion::Vector6 gamma_RF; gamma_RF.setZero(); - forwardKinematics(model,data,q,v,VectorXd::Zero(model.nv)); - gamma_RF += data.a[RF_id].toVector(); // Jdot * qdot + RigidConstraintModel ci_RF(CONTACT_6D,RF_id,LOCAL); + ci_RF.joint1_placement.setRandom(); + ci_RF.joint2_placement.setRandom(); + ci_RF.corrector.Kp = 10.; + ci_RF.corrector.Kd = 2. * sqrt(ci_RF.corrector.Kp); + contact_models.push_back(ci_RF); - forwardDynamics(model, data, q, v, tau, J_RF, gamma_RF); - VectorXd ddq_ref = data.ddq; - Force::Vector6 contact_force_ref = data.lambda_c; + RigidConstraintModel ci_LF(CONTACT_3D,LF_id,LOCAL); + ci_LF.joint1_placement.setRandom(); + ci_LF.joint2_placement.setRandom(); + ci_LF.corrector.Kp = 10.; + ci_LF.corrector.Kd = 2. * sqrt(ci_LF.corrector.Kp); + contact_models.push_back(ci_LF); - PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero()); - fext[RF_id] = ForceRef(contact_force_ref); + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); - // check call to RNEA - rnea(model,data_check,q,v,ddq_ref,fext); + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas = createData(contact_models); + initConstraintDynamics(model,data,contact_models); + const Eigen::VectorXd ddq0 = constraintDynamics(model,data,q,v,tau,contact_models,contact_datas,prox_settings); - BOOST_CHECK(data_check.tau.isApprox(tau)); - forwardKinematics(model,data_check,q,VectorXd::Zero(model.nv),ddq_ref); - BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF)); + Eigen::MatrixXd ddq_dq(model.nv,model.nv), ddq_dv(model.nv,model.nv), ddq_dtau(model.nv,model.nv); + Eigen::MatrixXd dlambda_dq(constraint_dim,model.nv), dlambda_dv(constraint_dim,model.nv), dlambda_dtau(constraint_dim,model.nv); - Data data_fd(model); - VectorXd q_plus(model.nq); - VectorXd v_eps(model.nv); v_eps.setZero(); - VectorXd v_plus(v); - VectorXd tau_plus(tau); - const double eps = 1e-8; + computeConstraintDynamicsDerivatives(model,data,contact_models,contact_datas, + ddq_dq,ddq_dv,ddq_dtau, + dlambda_dq,dlambda_dv,dlambda_dtau); + computeForwardKinematicsDerivatives(model,data,q,v,0*v); - // check: dddq_dtau and dlambda_dtau - MatrixXd dddq_dtau(model.nv,model.nv); - Data::Matrix6x dlambda_dtau(6,model.nv); + Data::Matrix6x dv_RF_dq_L(Data::Matrix6x::Zero(6,model.nv)); + Data::Matrix6x dv_RF_dv_L(Data::Matrix6x::Zero(6,model.nv)); + getFrameVelocityDerivatives(model,data,ci_RF.joint1_id,ci_RF.joint1_placement,ci_RF.reference_frame,dv_RF_dq_L,dv_RF_dv_L); - for(int k = 0; k < model.nv; ++k) + Data::Matrix6x dv_LF_dq_L(Data::Matrix6x::Zero(6,model.nv)); + Data::Matrix6x dv_LF_dv_L(Data::Matrix6x::Zero(6,model.nv)); + getFrameVelocityDerivatives(model,data,ci_LF.joint1_id,ci_LF.joint1_placement,ci_LF.reference_frame,dv_LF_dq_L,dv_LF_dv_L); + + const double eps = 1e-8; + Data::Matrix6x dacc_corrector_RF_dq(6,model.nv); dacc_corrector_RF_dq.setZero(); + Data::Matrix6x dacc_corrector_RF_dv(6,model.nv); dacc_corrector_RF_dv.setZero(); + Data::Matrix3x dacc_corrector_LF_dq(3,model.nv); dacc_corrector_LF_dq.setZero(); + Data::Matrix3x dacc_corrector_LF_dv(3,model.nv); dacc_corrector_LF_dv.setZero(); + { - tau_plus[k] += eps; - forwardDynamics(model, data_fd, q, v, tau_plus, J_RF, gamma_RF); - - const Data::TangentVectorType & ddq_plus = data_fd.ddq; - Force::Vector6 contact_force_plus = data_fd.lambda_c; - - dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps; - dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps; + const SE3::Matrix6 Jlog = Jlog6(contact_datas[0].c1Mc2.inverse()); + dacc_corrector_RF_dq = - ci_RF.corrector.Kp * Jlog * dv_RF_dv_L; + dacc_corrector_RF_dq += - ci_RF.corrector.Kd * dv_RF_dq_L; - tau_plus[k] -= eps; + dacc_corrector_RF_dv = - ci_RF.corrector.Kd * dv_RF_dv_L; + BOOST_CHECK(dv_RF_dv_L.isApprox(data.contact_chol.matrix().topRightCorner(6,model.nv))); } - - MatrixXd A(model.nv+6,model.nv+6); - data.M.transpose().triangularView() = data.M.triangularView(); - A.topLeftCorner(model.nv,model.nv) = data.M; - A.bottomLeftCorner(6, model.nv) = J_RF; - A.topRightCorner(model.nv, 6) = J_RF.transpose(); - A.bottomRightCorner(6,6).setZero(); - - MatrixXd Ainv = A.inverse(); - BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps))); - BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps))); - // check: dddq_dv and dlambda_dv - MatrixXd dddq_dv(model.nv,model.nv); - Data::Matrix6x dlambda_dv(6,model.nv); - - for(int k = 0; k < model.nv; ++k) { - v_plus[k] += eps; - forwardDynamics(model, data_fd, q, v_plus, tau, J_RF, gamma_RF); - - const Data::TangentVectorType & ddq_plus = data_fd.ddq; - Force::Vector6 contact_force_plus = data_fd.lambda_c; + dacc_corrector_LF_dq = - ci_LF.corrector.Kp * dv_LF_dv_L.topRows<3>(); + dacc_corrector_LF_dq += - ci_LF.corrector.Kd * dv_LF_dq_L.topRows<3>(); + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + dacc_corrector_LF_dq.col(k) += ci_LF.corrector.Kp * dv_LF_dv_L.col(k).tail<3>().cross(contact_datas[1].contact_placement_error.linear()); + } - dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps; - dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps; - - v_plus[k] -= eps; + dacc_corrector_LF_dv = - ci_LF.corrector.Kd * dv_LF_dv_L.topRows<3>(); + BOOST_CHECK(dv_LF_dv_L.topRows<3>().isApprox(data.contact_chol.matrix().topRightCorner(9,model.nv).bottomRows<3>())); } - computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv)); - MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv; - MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_fd = createData(contact_models); + initConstraintDynamics(model,data_fd,contact_models); - BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps))); - BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps))); + Data::Matrix6x dacc_corrector_RF_dq_fd(6,model.nv); + Data::Matrix3x dacc_corrector_LF_dq_fd(3,model.nv); - MatrixXd dddq_dq(model.nv,model.nv); - Data::Matrix6x dlambda_dq(6,model.nv); - - for(int k = 0; k < model.nv; ++k) + Eigen::MatrixXd ddq_dq_fd(model.nv,model.nv), ddq_dv_fd(model.nv,model.nv), ddq_dtau_fd(model.nv,model.nv); + Eigen::MatrixXd dlambda_dq_fd(constraint_dim,model.nv), dlambda_dv_fd(constraint_dim,model.nv), dlambda_dtau_fd(constraint_dim,model.nv); + + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) { + Eigen::VectorXd v_eps = Eigen::VectorXd::Zero(model.nv); v_eps[k] = eps; - q_plus = integrate(model,q,v_eps); - computeJointJacobians(model, data_fd, q_plus); - getJointJacobian(model, data_fd, RF_id, LOCAL, J_RF); - forwardDynamics(model, data_fd, q_plus, v, tau, J_RF, gamma_RF); - - const Data::TangentVectorType & ddq_plus = data_fd.ddq; - Force::Vector6 contact_force_plus = data_fd.lambda_c; + const Eigen::VectorXd q_plus = integrate(model,q,v_eps); - dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps; - dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps; + Eigen::VectorXd ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_datas_fd,prox_settings); + dacc_corrector_RF_dq_fd.col(k) = (contact_datas_fd[0].contact_acceleration_error - contact_datas[0].contact_acceleration_error).toVector() / eps; + dacc_corrector_LF_dq_fd.col(k) = (contact_datas_fd[1].contact_acceleration_error.linear() - contact_datas[1].contact_acceleration_error.linear()) / eps; - v_eps[k] = 0.; + ddq_dq_fd.col(k) = (ddq_plus - ddq0)/eps; } - computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext); - Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv); - v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); - Data data_kin(model); - computeForwardKinematicsDerivatives(model,data_kin,q,VectorXd::Zero(model.nv),ddq_ref); - getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); - - MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq; - dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq; - - MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq; - dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq; - - BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps))); - BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps))); + BOOST_CHECK(ddq_dq_fd.isApprox(ddq_dq,sqrt(eps))); + BOOST_CHECK(dacc_corrector_RF_dq.isApprox(dacc_corrector_RF_dq_fd,sqrt(eps))); + BOOST_CHECK(dacc_corrector_LF_dq.isApprox(dacc_corrector_LF_dq_fd,sqrt(eps))); + // std::cout << "dacc_corrector_RF_dq:\n" << dacc_corrector_RF_dq << std::endl; + // std::cout << "dacc_corrector_RF_dq_fd:\n" << dacc_corrector_RF_dq_fd << std::endl; + Data::Matrix6x dacc_corrector_RF_dv_fd(6,model.nv); + Data::Matrix3x dacc_corrector_LF_dv_fd(3,model.nv); + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + Eigen::VectorXd v_plus(v); + v_plus[k] += eps; + + Eigen::VectorXd ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_datas_fd,prox_settings); + dacc_corrector_RF_dv_fd.col(k) = (contact_datas_fd[0].contact_acceleration_error - contact_datas[0].contact_acceleration_error).toVector() / eps; + dacc_corrector_LF_dv_fd.col(k) = (contact_datas_fd[1].contact_acceleration_error.linear() - contact_datas[1].contact_acceleration_error.linear()) / eps; + + ddq_dv_fd.col(k) = (ddq_plus - ddq0)/eps; + } + BOOST_CHECK(ddq_dv_fd.isApprox(ddq_dv,sqrt(eps))); + BOOST_CHECK(dacc_corrector_RF_dv.isApprox(dacc_corrector_RF_dv_fd,sqrt(eps))); + BOOST_CHECK(dacc_corrector_LF_dv.isApprox(dacc_corrector_LF_dv_fd,sqrt(eps))); } -template -VectorXd contactDynamics(const Model & model, Data & data, - const Eigen::MatrixBase & q, - const Eigen::MatrixBase & v, - const Eigen::MatrixBase & tau, - const Model::JointIndex id) +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_fd) { - computeJointJacobians(model, data, q); - Data::Matrix6x J(6,model.nv); J.setZero(); - - getJointJacobian(model, data, id, LOCAL, J); - Motion::Vector6 gamma; - forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv)); - gamma = data.a[id].toVector(); - - forwardDynamics(model, data, q, v, tau, J, gamma); - VectorXd res(VectorXd::Zero(model.nv+6)); - - res.head(model.nv) = data.ddq; - res.tail(6) = data.lambda_c; - - return res; -} + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); -BOOST_AUTO_TEST_CASE ( test_FD_with_contact_varying_gamma ) -{ - pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model,true); - pinocchio::Data data(model), data_check(model); - - VectorXd q = VectorXd::Ones(model.nq); - normalize(model,q); - VectorXd v = VectorXd::Random(model.nv); VectorXd tau = VectorXd::Random(model.nv); - + const std::string RF = "rleg6_joint"; - const Model::JointIndex RF_id = model.getJointId(RF); - - Data::Matrix6x J_RF(6,model.nv); J_RF.setZero(); - computeJointJacobians(model, data, q); - getJointJacobian(model, data, RF_id, LOCAL, J_RF); - Motion::Vector6 gamma_RF; gamma_RF.setZero(); - - VectorXd x_ref = contactDynamics(model,data,q,v,tau,RF_id); - VectorXd ddq_ref = x_ref.head(model.nv); - Force::Vector6 contact_force_ref = x_ref.tail(6); - - PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext((size_t)model.njoints,Force::Zero()); - fext[RF_id] = ForceRef(contact_force_ref); - - // check call to RNEA - rnea(model,data_check,q,v,ddq_ref,fext); - - BOOST_CHECK(data_check.tau.isApprox(tau)); - forwardKinematics(model,data_check,q,v,ddq_ref); - BOOST_CHECK(data_check.a[RF_id].toVector().isZero()); - - Data data_fd(model); + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_RF(CONTACT_3D,RF_id,LOCAL); + ci_RF.joint1_placement.setRandom(); + contact_models.push_back(ci_RF); contact_data.push_back(RigidConstraintData(ci_RF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); VectorXd q_plus(model.nq); - VectorXd v_eps(model.nv); v_eps.setZero(); - VectorXd v_plus(v); - VectorXd tau_plus(tau); - VectorXd x_plus(model.nv + 6); - const double eps = 1e-8; + VectorXd ddq_plus(model.nv); - // check: dddq_dtau and dlambda_dtau - MatrixXd dddq_dtau(model.nv,model.nv); - Data::Matrix6x dlambda_dtau(6,model.nv); + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); for(int k = 0; k < model.nv; ++k) { - tau_plus[k] += eps; - x_plus = contactDynamics(model,data,q,v,tau_plus,RF_id); + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } - const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); - Force::Vector6 contact_force_plus = x_plus.tail(6); + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); - dddq_dtau.col(k) = (ddq_plus - ddq_ref)/eps; - dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref)/eps; + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } - tau_plus[k] -= eps; + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; } - MatrixXd A(model.nv+6,model.nv+6); - data.M.transpose().triangularView() = data.M.triangularView(); - A.topLeftCorner(model.nv,model.nv) = data.M; - A.bottomLeftCorner(6, model.nv) = J_RF; - A.topRightCorner(model.nv, 6) = J_RF.transpose(); - A.bottomRightCorner(6,6).setZero(); + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} - MatrixXd Ainv = A.inverse(); - BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau,std::sqrt(eps))); - BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau,std::sqrt(eps))); - // check: dddq_dv and dlambda_dv - MatrixXd dddq_dv(model.nv,model.nv); - Data::Matrix6x dlambda_dv(6,model.nv); +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_loop_closure_j2_fd) +{ + using namespace Eigen; + using namespace pinocchio; - for(int k = 0; k < model.nv; ++k) - { - v_plus[k] += eps; - x_plus = contactDynamics(model,data,q,v_plus,tau,RF_id); + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); - const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); - Force::Vector6 contact_force_plus = x_plus.tail(6); + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); - dddq_dv.col(k) = (ddq_plus - ddq_ref)/eps; - dlambda_dv.col(k) = (contact_force_plus - contact_force_ref)/eps; + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); - v_plus[k] -= eps; - } - - - computeRNEADerivatives(model,data_check,q,v,VectorXd::Zero(model.nv)); - Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv); - v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); - Data data_kin(model); - computeForwardKinematicsDerivatives(model,data_kin,q,v,VectorXd::Zero(model.nv)); - getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); - - MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv; - dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv; - MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv; - dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv; + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + // Add Loop Closure Constraint - BOOST_CHECK(dddq_dv_anal.isApprox(dddq_dv,std::sqrt(eps))); - BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv,std::sqrt(eps))); + const std::string RA = "rarm5_joint"; + const Model::JointIndex RA_id = model.getJointId(RA); + const std::string LA = "larm5_joint"; + const Model::JointIndex LA_id = model.getJointId(LA); + + RigidConstraintModel ci_closure (CONTACT_3D, 0, SE3::Identity(), + RA_id, SE3::Random(), LOCAL); + contact_models.push_back(ci_closure); + contact_data.push_back(RigidConstraintData(ci_closure)); + // End of Loopo Closure Constraint + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); - MatrixXd dddq_dq(model.nv,model.nv); - Data::Matrix6x dlambda_dq(6,model.nv); + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); for(int k = 0; k < model.nv; ++k) { - v_eps[k] = eps; + v_eps[k] += alpha; q_plus = integrate(model,q,v_eps); - - x_plus = contactDynamics(model,data,q_plus,v,tau,RF_id); - - const Data::TangentVectorType ddq_plus = x_plus.head(model.nv); - Force::Vector6 contact_force_plus = x_plus.tail(6); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } - dddq_dq.col(k) = (ddq_plus - ddq_ref)/eps; - dlambda_dq.col(k) = (contact_force_plus - contact_force_ref)/eps; + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); - v_eps[k] = 0.; + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; } - computeRNEADerivatives(model,data_check,q,v,ddq_ref,fext); - v_partial_dq.setZero(); a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero(); - computeForwardKinematicsDerivatives(model,data_kin,q,v,ddq_ref); - getJointAccelerationDerivatives(model,data_kin,RF_id,LOCAL,v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} - MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq; - dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq; - BOOST_CHECK(dddq_dq_anal.isApprox(dddq_dq,std::sqrt(eps))); - - MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq; - dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq; +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_loop_closure_j2_fd) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + // Add Loop Closure Constraint - BOOST_CHECK(dlambda_dq_anal.isApprox(dlambda_dq,std::sqrt(eps))); + const std::string RA = "rarm5_joint"; + const Model::JointIndex RA_id = model.getJointId(RA); + const std::string LA = "larm5_joint"; + const Model::JointIndex LA_id = model.getJointId(LA); + + RigidConstraintModel ci_closure (CONTACT_6D, 0, SE3::Identity(), + RA_id, SE3::Random(), LOCAL); -} + contact_models.push_back(ci_closure); + contact_data.push_back(RigidConstraintData(ci_closure)); + // End of Loopo Closure Constraint -BOOST_AUTO_TEST_SUITE_END () + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_loop_closure_j1j2_fd) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + // Add Loop Closure Constraint + + const std::string RA = "rarm5_joint"; + const Model::JointIndex RA_id = model.getJointId(RA); + const std::string LA = "larm5_joint"; + const Model::JointIndex LA_id = model.getJointId(LA); + + RigidConstraintModel ci_closure (CONTACT_6D, LA_id, SE3::Random(), + RA_id, SE3::Random(), LOCAL); + + contact_models.push_back(ci_closure); + contact_data.push_back(RigidConstraintData(ci_closure)); + // End of Loopo Closure Constraint + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_WORL_ALIGNED_6D_loop_closure_j1j2_fd) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + // Add Loop Closure Constraint + + const std::string RA = "rarm5_joint"; + const Model::JointIndex RA_id = model.getJointId(RA); + const std::string LA = "larm5_joint"; + const Model::JointIndex LA_id = model.getJointId(LA); + + RigidConstraintModel ci_closure (CONTACT_6D, LA_id, SE3::Random(), + RA_id, SE3::Random(), LOCAL_WORLD_ALIGNED); + + contact_models.push_back(ci_closure); + contact_data.push_back(RigidConstraintData(ci_closure)); + // End of Loopo Closure Constraint + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_loop_closure_j1j2_fd) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + // Add Loop Closure Constraint + + const std::string RA = "rarm5_joint"; + const Model::JointIndex RA_id = model.getJointId(RA); + const std::string LA = "larm5_joint"; + const Model::JointIndex LA_id = model.getJointId(LA); + + RigidConstraintModel ci_closure (CONTACT_3D, LA_id, SE3::Random(), + RA_id, SE3::Random(), LOCAL); + + contact_models.push_back(ci_closure); + contact_data.push_back(RigidConstraintData(ci_closure)); + // End of Loopo Closure Constraint + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_3D_loop_closure_j1j2_fd) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + // Add Loop Closure Constraint + + const std::string RA = "rarm5_joint"; + const Model::JointIndex RA_id = model.getJointId(RA); + const std::string LA = "larm5_joint"; + const Model::JointIndex LA_id = model.getJointId(LA); + + RigidConstraintModel ci_closure (CONTACT_3D, LA_id, SE3::Random(), + RA_id, SE3::Random(), LOCAL_WORLD_ALIGNED); + + contact_models.push_back(ci_closure); + contact_data.push_back(RigidConstraintData(ci_closure)); + // End of Loopo Closure Constraint + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_6D_fd) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + // const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_LF(CONTACT_6D,LF_id,LOCAL_WORLD_ALIGNED); + + ci_LF.joint1_placement.setRandom(); + contact_models.push_back(ci_LF); contact_data.push_back(RigidConstraintData(ci_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + +BOOST_AUTO_TEST_CASE ( test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_3D_fd ) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + // const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_RF(CONTACT_3D,RF_id,LOCAL_WORLD_ALIGNED); + ci_RF.joint1_placement.setRandom(); + contact_models.push_back(ci_RF); contact_data.push_back(RigidConstraintData(ci_RF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_mix_fd) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + const std::string RH = "rarm6_joint"; + const Model::JointIndex RH_id = model.getJointId(RH); + const std::string LH = "larm6_joint"; + const Model::JointIndex LH_id = model.getJointId(LH); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_LF(CONTACT_6D,LF_id,LOCAL_WORLD_ALIGNED); + ci_LF.joint1_placement.setRandom(); + contact_models.push_back(ci_LF); contact_data.push_back(RigidConstraintData(ci_LF)); + RigidConstraintModel ci_RF(CONTACT_6D,RF_id,LOCAL); + ci_RF.joint1_placement.setRandom(); + contact_models.push_back(ci_RF); contact_data.push_back(RigidConstraintData(ci_RF)); + + RigidConstraintModel ci_LH(CONTACT_3D,LH_id,LOCAL_WORLD_ALIGNED); + ci_LH.joint1_placement.setRandom(); + contact_models.push_back(ci_LH); contact_data.push_back(RigidConstraintData(ci_LH)); + RigidConstraintModel ci_RH(CONTACT_3D,RH_id,LOCAL); + ci_RH.joint1_placement.setRandom(); + contact_models.push_back(ci_RH); contact_data.push_back(RigidConstraintData(ci_RH)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data,contact_models); + constraintDynamics(model,data,q,v,tau,contact_models,contact_data,prox_settings); + const Data::TangentVectorType a = data.ddq; + data.M.triangularView() = data.M.transpose().triangularView(); + computeConstraintDynamicsDerivatives(model, data, contact_models, contact_data); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd ddq_partial_dq_fd(model.nv,model.nv); ddq_partial_dq_fd.setZero(); + MatrixXd ddq_partial_dv_fd(model.nv,model.nv); ddq_partial_dv_fd.setZero(); + MatrixXd ddq_partial_dtau_fd(model.nv,model.nv); ddq_partial_dtau_fd.setZero(); + + MatrixXd lambda_partial_dtau_fd(constraint_dim,model.nv); lambda_partial_dtau_fd.setZero(); + MatrixXd lambda_partial_dq_fd(constraint_dim,model.nv); lambda_partial_dq_fd.setZero(); + MatrixXd lambda_partial_dv_fd(constraint_dim,model.nv); lambda_partial_dv_fd.setZero(); + + const VectorXd ddq0 = constraintDynamics(model,data_fd,q,v,tau,contact_models,contact_data,prox_settings); + const VectorXd lambda0 = data_fd.lambda_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd ddq_plus(model.nv); + + VectorXd lambda_plus(constraint_dim); + + const double alpha = 1e-8; + forwardKinematics(model,data,q,v,a); + + const Eigen::MatrixXd Jc = data.dac_da; + const Eigen::MatrixXd Jc_ref = data.contact_chol.matrix().topRightCorner(constraint_dim,model.nv); + + BOOST_CHECK(Jc.isApprox(Jc_ref)); + + const Eigen::MatrixXd JMinv = Jc * data.Minv; + const Eigen::MatrixXd dac_dq = data.dac_dq + JMinv * data.dtau_dq; + + Eigen::MatrixXd dac_dq_fd(constraint_dim,model.nv); + + Eigen::VectorXd contact_acc0(constraint_dim); + Eigen::DenseIndex row_id = 0; + + forwardKinematics(model,data,q,v,data.ddq); + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const Eigen::DenseIndex size = cmodel.size(); + + const Motion contact_acc = getContactAcceleration(model,data,cmodel); + + if(cmodel.type == CONTACT_3D) + contact_acc0.segment<3>(row_id) = contact_acc.linear(); + else + contact_acc0.segment<6>(row_id) = contact_acc.toVector(); + + row_id += size; + } + + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + ddq_plus = constraintDynamics(model,data_fd,q_plus,v,tau,contact_models,contact_data,prox_settings); + + ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + + Eigen::VectorXd contact_acc_plus(constraint_dim); + Eigen::DenseIndex row_id = 0; + forwardKinematics(model,data_fd,q_plus,v,data.ddq); + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const Eigen::DenseIndex size = cmodel.size(); + + const Motion contact_acc = getContactAcceleration(model,data_fd,cmodel); + + if(cmodel.type == CONTACT_3D) + contact_acc_plus.segment<3>(row_id) = contact_acc.linear(); + else + contact_acc_plus.segment<6>(row_id) = contact_acc.toVector(); + + row_id += size; + } + + dac_dq_fd.col(k) = (contact_acc_plus - contact_acc0)/alpha; + + v_eps[k] = 0.; + } + + // std::cout << "dac_dq_fd:\n" << dac_dq_fd << std::endl; + // std::cout << "dac_dq:\n" << data.dac_da << std::endl; + // std::cout << "dac_dq:\n" << dac_dq_fd<< std::endl; + + BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + // std::cout << "lambda_partial_dq_fd:\n" << lambda_partial_dq_fd << std::endl; + // std::cout << "dlambda_dq:\n" << data.dlambda_dq << std::endl; + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v_plus,tau,contact_models,contact_data,prox_settings); + ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); + + VectorXd tau_plus(tau); + for(int k = 0; k < model.nv; ++k) + { + tau_plus[k] += alpha; + ddq_plus = constraintDynamics(model,data_fd,q,v,tau_plus,contact_models,contact_data,prox_settings); + ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0)/alpha; + lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0)/alpha; + tau_plus[k] -= alpha; + } + + BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau,sqrt(alpha))); + BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau,sqrt(alpha))); +} + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_dirty_data) +{ + // Verify that a dirty data doesn't affect the results of the contact dynamics derivs + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data_dirty(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_LF(CONTACT_6D,LF_id,LOCAL); + RigidConstraintModel ci_RF(CONTACT_3D,RF_id,LOCAL); + + contact_models.push_back(ci_LF); contact_data.push_back(RigidConstraintData(ci_LF)); + contact_models.push_back(ci_RF); contact_data.push_back(RigidConstraintData(ci_RF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + ProximalSettings prox_settings(1e-12,mu0,1); + initConstraintDynamics(model,data_dirty,contact_models); + constraintDynamics(model,data_dirty,q,v,tau,contact_models,contact_data,prox_settings); + computeConstraintDynamicsDerivatives(model, data_dirty, contact_models, contact_data); + + // Reuse the same data with new configurations + q = randomConfiguration(model); + v = VectorXd::Random(model.nv); + tau = VectorXd::Random(model.nv); + constraintDynamics(model,data_dirty,q,v,tau,contact_models,contact_data,prox_settings); + computeConstraintDynamicsDerivatives(model, data_dirty, contact_models, contact_data); + + //Test with fresh data + Data data_fresh(model); + initConstraintDynamics(model,data_fresh,contact_models); + constraintDynamics(model,data_fresh,q,v,tau,contact_models,contact_data,prox_settings); + computeConstraintDynamicsDerivatives(model, data_fresh, contact_models, contact_data); + const double alpha = 1e-12; + + BOOST_CHECK(data_dirty.ddq_dq.isApprox(data_fresh.ddq_dq,sqrt(alpha))); + BOOST_CHECK(data_dirty.ddq_dv.isApprox(data_fresh.ddq_dv,sqrt(alpha))); + BOOST_CHECK(data_dirty.ddq_dtau.isApprox(data_fresh.ddq_dtau,sqrt(alpha))); + BOOST_CHECK(data_dirty.dlambda_dq.isApprox(data_fresh.dlambda_dq,sqrt(alpha))); + BOOST_CHECK(data_dirty.dlambda_dv.isApprox(data_fresh.dlambda_dv,sqrt(alpha))); + BOOST_CHECK(data_dirty.dlambda_dtau.isApprox(data_fresh.dlambda_dtau,sqrt(alpha))); +} + +BOOST_AUTO_TEST_SUITE_END () diff --git a/unittest/contact-dynamics.cpp b/unittest/contact-dynamics.cpp index 0de653d5d0..601ddd923e 100644 --- a/unittest/contact-dynamics.cpp +++ b/unittest/contact-dynamics.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2016-2020 CNRS, INRIA +// Copyright (c) 2016-2020 CNRS INRIA // #include "pinocchio/spatial/se3.hpp" @@ -63,8 +63,8 @@ BOOST_AUTO_TEST_CASE ( test_FD ) Eigen::MatrixXd G_ref(J.transpose()); cholesky::Uiv(model, data, G_ref); for(int k=0;k MotionAD; typedef pinocchio::SE3Tpl SE3; typedef pinocchio::MotionTpl Motion; - typedef pinocchio::ConstraintTpl ConstraintXd; + typedef pinocchio::JointMotionSubspaceTpl JointMotionSubspaceXd; typedef Eigen::Matrix VectorXAD; typedef Eigen::Matrix MatrixX; @@ -75,7 +75,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) VectorXAD Y(6); MotionAD m_ad(jdata_ad.v); Motion m(jdata.v); - ConstraintXd Sref(jdata.S.matrix()); + JointMotionSubspaceXd Sref(jdata.S.matrix()); for(Eigen::DenseIndex k = 0; k < 3; ++k) { @@ -190,7 +190,7 @@ struct TestADOnJoints typedef pinocchio::MotionTpl MotionAD; typedef pinocchio::SE3Tpl SE3; typedef pinocchio::MotionTpl Motion; - typedef pinocchio::ConstraintTpl ConstraintXd; + typedef pinocchio::JointMotionSubspaceTpl JointMotionSubspaceXd; typedef Eigen::Matrix VectorXAD; typedef Eigen::Matrix MatrixX; @@ -241,7 +241,7 @@ struct TestADOnJoints VectorXAD Y(6); MotionAD m_ad(jdata_ad_base.v()); Motion m(jdata_base.v()); - ConstraintXd Sref(jdata_base.S().matrix()); + JointMotionSubspaceXd Sref(jdata_base.S().matrix()); for(Eigen::DenseIndex k = 0; k < 3; ++k) { diff --git a/unittest/cppadcg-contact-dynamics.cpp b/unittest/cppadcg-contact-dynamics.cpp new file mode 100644 index 0000000000..f0c4feeab1 --- /dev/null +++ b/unittest/cppadcg-contact-dynamics.cpp @@ -0,0 +1,69 @@ +// +// Copyright (c) 2021 INRIA +// + +#include "pinocchio/codegen/cppadcg.hpp" +#include "pinocchio/codegen/code-generator-algo.hpp" +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/algorithm/contact-dynamics.hpp" +#include "pinocchio/parsers/sample-models.hpp" + +#include +#include + +using namespace pinocchio; + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +BOOST_AUTO_TEST_CASE(test_constraint_dynamics_code_generation) +{ + typedef double Scalar; + typedef CppAD::cg::CG CGScalar; + typedef CppAD::AD ADScalar; + + typedef Eigen::Matrix ADVector; + + typedef pinocchio::ModelTpl Model; + typedef Model::Data Data; + + typedef pinocchio::ModelTpl ADModel; + typedef ADModel::Data ADData; + + Model model; + pinocchio::buildModels::humanoidRandom(model); + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + Data data(model); + + const std::string RF = "rleg6_joint"; + const std::string LF = "lleg6_joint"; + + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D3D; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D3D; + + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL); + contact_models_6D3D.push_back(ci_RF); + contact_datas_6D3D.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_3D,model.getJointId(LF),LOCAL); + contact_models_6D3D.push_back(ci_LF); + contact_datas_6D3D.push_back(RigidConstraintData(ci_LF)); + + Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq); + Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); + Eigen::VectorXd tau = Eigen::VectorXd::Random(model.nv); + std::vector results_q(2,Eigen::VectorXd::Zero(model.nq)); + std::vector results_v(2,Eigen::VectorXd::Zero(model.nv)); + + CodeGenConstraintDynamics cg_constraintDynamics(model, contact_models_6D3D); + cg_constraintDynamics.initLib(); + cg_constraintDynamics.loadLib(); + cg_constraintDynamics.evalFunction(q,v,tau); + + pinocchio::initConstraintDynamics(model, data, contact_models_6D3D); + pinocchio::constraintDynamics(model, data, q, v, tau, + contact_models_6D3D, contact_datas_6D3D, 0.); + BOOST_CHECK(data.ddq.isApprox(cg_constraintDynamics.ddq)); + BOOST_CHECK(data.lambda_c.isApprox(cg_constraintDynamics.lambda_c)); +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 9452a97bf5..caa75e566f 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // /* @@ -134,10 +134,10 @@ BOOST_AUTO_TEST_CASE(test_minimal_crba) Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit); Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv)); - crba(model,data_ref,q); + pinocchio::deprecated::crba(model,data_ref,q); data_ref.M.triangularView() = data_ref.M.transpose().triangularView(); - crbaMinimal(model,data,q); + crba(model,data,q); data.M.triangularView() = data.M.transpose().triangularView(); BOOST_CHECK(data.M.isApprox(data_ref.M)); @@ -149,5 +149,28 @@ BOOST_AUTO_TEST_CASE(test_minimal_crba) } +BOOST_AUTO_TEST_CASE(test_roto_inertia_effects) +{ + pinocchio::Model model, model_ref; + pinocchio::buildModels::humanoidRandom(model); + model_ref = model; + + BOOST_CHECK(model == model_ref); + + pinocchio::Data data(model), data_ref(model_ref); + + model.armature = Eigen::VectorXd::Random(model.nv) + Eigen::VectorXd::Constant(model.nv,1.); + + Eigen::VectorXd q = randomConfiguration(model); + crba(model_ref,data_ref,q); + data_ref.M.triangularView() = data_ref.M.transpose().triangularView(); + data_ref.M.diagonal() += model.armature; + + crba(model,data,q); + data.M.triangularView() = data.M.transpose().triangularView(); + + BOOST_CHECK(data.M.isApprox(data_ref.M)); +} + BOOST_AUTO_TEST_SUITE_END () diff --git a/unittest/energy.cpp b/unittest/energy.cpp index 680d20a539..d3dfac2c53 100644 --- a/unittest/energy.cpp +++ b/unittest/energy.cpp @@ -4,11 +4,13 @@ #include "pinocchio/algorithm/energy.hpp" #include "pinocchio/algorithm/crba.hpp" +#include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/center-of-mass.hpp" #include "pinocchio/parsers/sample-models.hpp" +#include #include #include #include @@ -20,15 +22,15 @@ BOOST_AUTO_TEST_CASE(test_kinetic_energy) using namespace Eigen; using namespace pinocchio; - pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); - pinocchio::Data data(model); + Model model; + buildModels::humanoidRandom(model); + Data data(model); const VectorXd qmax = VectorXd::Ones(model.nq); VectorXd q = randomConfiguration(model,-qmax,qmax); - VectorXd v = VectorXd::Ones(model.nv); + VectorXd v = VectorXd::Random(model.nv); - data.M.fill(0); crba(model,data,q); + crba(model,data,q); data.M.triangularView() = data.M.transpose().triangularView(); @@ -38,14 +40,40 @@ BOOST_AUTO_TEST_CASE(test_kinetic_energy) BOOST_CHECK_SMALL(kinetic_energy_ref - kinetic_energy, 1e-12); } +BOOST_AUTO_TEST_CASE(test_kinetic_energy_with_armature) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model); + Data data(model), data_ref(model); + + model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv); + + const VectorXd qmax = VectorXd::Ones(model.nq); + VectorXd q = randomConfiguration(model,-qmax,qmax); + VectorXd v = VectorXd::Random(model.nv); + + crba(model,data_ref,q); + data_ref.M.triangularView() + = data_ref.M.transpose().triangularView(); + + double kinetic_energy_ref = 0.5 * v.transpose() * data_ref.M * v; + double kinetic_energy = computeKineticEnergy(model, data, q, v); + + BOOST_CHECK_SMALL(kinetic_energy_ref - kinetic_energy, + Eigen::NumTraits::dummy_precision()); +} + BOOST_AUTO_TEST_CASE(test_potential_energy) { using namespace Eigen; using namespace pinocchio; - pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); - pinocchio::Data data(model), data_ref(model); + Model model; + buildModels::humanoidRandom(model); + Data data(model), data_ref(model); const VectorXd qmax = VectorXd::Ones(model.nq); VectorXd q = randomConfiguration(model,-qmax,qmax); @@ -55,7 +83,106 @@ BOOST_AUTO_TEST_CASE(test_potential_energy) double potential_energy_ref = -data_ref.mass[0] * (data_ref.com[0].dot(model.gravity.linear())); - BOOST_CHECK_SMALL(potential_energy_ref - potential_energy, 1e-12); + BOOST_CHECK_SMALL(potential_energy_ref - potential_energy, + Eigen::NumTraits::dummy_precision()); +} + +BOOST_AUTO_TEST_CASE(test_mechanical_energy) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model); + Data data(model), data_ref(model); + + const VectorXd qmax = VectorXd::Ones(model.nq); + VectorXd q = randomConfiguration(model,-qmax,qmax); + VectorXd v = VectorXd::Random(model.nv); + + computeKineticEnergy(model,data_ref,q,v); + computePotentialEnergy(model,data_ref,q); + + const double mechanical_energy_ref = data_ref.kinetic_energy + data_ref.potential_energy; + double mechanical_energy = computeMechanicalEnergy(model, data, q, v); + + BOOST_CHECK_SMALL(mechanical_energy_ref - mechanical_energy, + Eigen::NumTraits::dummy_precision()); +} + +template +Eigen::VectorXd evalMv(const pinocchio::Model & model, + const Eigen::MatrixBase & q, + const Eigen::MatrixBase & v) +{ + pinocchio::Data data(model); + crba(model,data,q); + data.M.triangularView() = data.M.transpose().triangularView(); + return data.M * v; +} + +BOOST_AUTO_TEST_CASE(test_against_rnea) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model); + model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv); + model.armature.head<6>().setZero(); // Because we do not take into account the influcent of the armature on the Coriolis terms +// model.gravity.setZero(); + Data data(model), data_ref(model); + + const VectorXd qmax = VectorXd::Ones(model.nq); + VectorXd q = randomConfiguration(model,-qmax,qmax); + VectorXd v = VectorXd::Random(model.nv); //v.setZero(); + VectorXd a = VectorXd::Random(model.nv); //a.setZero(); + + const VectorXd tau_ref = rnea(model,data_ref,q,v,a); + VectorXd tau_fd = VectorXd::Zero(model.nv); + + const double eps = 1e-8; + computeMechanicalEnergy(model,data,q,v); + const double mechanical_energy = data.kinetic_energy - data.potential_energy; + forwardKinematics(model,data,q); + const SE3 oM1 = data.oMi[1]; + Data data_plus(model); + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + VectorXd q_plus = integrate(model,q,VectorXd::Unit(model.nv,k)*eps); + computeMechanicalEnergy(model,data_plus,q_plus,v); + const double mechanical_energy_plus = data_plus.kinetic_energy - data_plus.potential_energy; + const SE3 oM1_plus = data_plus.oMi[1]; + const SE3 Mdiff = oM1.actInv(oM1_plus); + if(k < 6) + { + Force f = Force::Zero(); + f.toVector()[k] = mechanical_energy; + Force f_plus = Force::Zero(); + f_plus.toVector()[k] = mechanical_energy_plus; + tau_fd.head<6>() -= (Mdiff.act(f_plus) - f).toVector()/eps; + } + else + { + tau_fd[k] = -(mechanical_energy_plus - mechanical_energy)/eps; + } + } + + const VectorXd Mv = evalMv(model,q,v); + const VectorXd q_plus = integrate(model,q,v*eps); + const VectorXd v_plus = v + a*eps; + VectorXd Mv_plus = evalMv(model,q_plus,v_plus); + + forwardKinematics(model,data,q_plus); + const SE3 oM1_plus = data.oMi[1]; + const SE3 Mdiff = oM1.actInv(oM1_plus); + Mv_plus.head<6>() = Mdiff.act(Force(Mv_plus.head<6>())).toVector(); // The torque at the free flyer level is expressed in a translated frame. + + tau_fd += (Mv_plus - Mv)/eps; + std::cout << "tau_fd: " << tau_fd.transpose() << std::endl; + std::cout << "tau_ref: " << tau_ref.transpose() << std::endl; + + BOOST_CHECK(tau_fd.isApprox(tau_ref,sqrt(eps))); } BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/explog.cpp b/unittest/explog.cpp index 208637fdba..c6ec518105 100644 --- a/unittest/explog.cpp +++ b/unittest/explog.cpp @@ -1,10 +1,11 @@ // -// Copyright (c) 2016-2019 CNRS INRIA +// Copyright (c) 2016-2021 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // #include "pinocchio/spatial/explog.hpp" +#include #include #include @@ -170,8 +171,9 @@ BOOST_AUTO_TEST_CASE(Jlog3_fd) for (int i = 0; i < 3; ++i) { dR[i] = eps; - SE3::Matrix3 R_dR = R * exp3(dR); - Jfd.col(i) = (log3(R_dR) - log3(R)) / eps; + SE3::Matrix3 R_dR_plus = R * exp3(dR); + SE3::Matrix3 R_dR_minus = R * exp3(-dR); + Jfd.col(i) = (log3(R_dR_plus) - log3(R_dR_minus)) / (2*eps); dR[i] = 0; } BOOST_CHECK(Jfd.isApprox(Jlog, std::sqrt(eps))); @@ -356,32 +358,37 @@ BOOST_AUTO_TEST_CASE(Jlog6_fd) } BOOST_CHECK(Jfd.isApprox(Jlog, sqrt(step))); + + SE3::Matrix6 Jlog2 = Jlog6(M); + BOOST_CHECK(Jlog2.isApprox(Jlog)); } -BOOST_AUTO_TEST_CASE(Jlog6_of_product_fd) +BOOST_AUTO_TEST_CASE(Jexp6_fd) { - SE3 Ma(SE3::Random()); - SE3 Mb(SE3::Random()); + SE3 M(SE3::Random()); - SE3::Matrix6 Jlog, Ja, Jb, Jfda, Jfdb; - Jlog6 (Ma.inverse() * Mb, Jlog); - Ja = - Jlog * (Ma.inverse() * Mb).toActionMatrixInverse(); - Jb = Jlog; - Jfda.setZero(); - Jfdb.setZero(); + const Motion v = log6(M); - Motion dM; dM.setZero(); - double step = 1e-8; + SE3::Matrix6 Jexp_fd, Jexp; + + Jexp6(Motion::Zero(), Jexp); + BOOST_CHECK(Jexp.isIdentity()); + + Jexp6(v, Jexp); + + Motion::Vector6 dv; dv.setZero(); + const double eps = 1e-8; for (int i = 0; i < 6; ++i) { - dM.toVector()[i] = step; - Jfda.col(i) = (log6((Ma*exp6(dM)).inverse()*Mb).toVector() - log6(Ma.inverse()*Mb).toVector()) / step; - Jfdb.col(i) = (log6(Ma.inverse()*Mb*exp6(dM)).toVector() - log6(Ma.inverse()*Mb).toVector()) / step; - dM.toVector()[i] = 0; + dv[i] = eps; + SE3 M_next = exp6(v+Motion(dv)); + Jexp_fd.col(i) = log6(M.actInv(M_next)).toVector() / eps; + dv[i] = 0; } - - BOOST_CHECK(Jfda.isApprox(Ja, sqrt(step))); - BOOST_CHECK(Jfdb.isApprox(Jb, sqrt(step))); + BOOST_CHECK(Jexp_fd.isApprox(Jexp, std::sqrt(eps))); + + SE3::Matrix6 Jexp2 = Jexp6(v); + BOOST_CHECK(Jexp2.isApprox(Jexp)); } BOOST_AUTO_TEST_CASE(Jexplog6) diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp index d6aeb00c4a..7cff9a0f94 100644 --- a/unittest/finite-differences.cpp +++ b/unittest/finite-differences.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2016-2019 CNRS, INRIA +// Copyright (c) 2016-2019 CNRS INRIA // #include "pinocchio/multibody/model.hpp" @@ -9,6 +9,7 @@ #include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/jacobian.hpp" +#include #include #include diff --git a/unittest/frames.cpp b/unittest/frames.cpp index 5121745cef..49d091a6e7 100644 --- a/unittest/frames.cpp +++ b/unittest/frames.cpp @@ -54,6 +54,11 @@ BOOST_AUTO_TEST_CASE(cast) BOOST_CHECK(frame.cast() == frame); BOOST_CHECK(frame.cast().cast() == frame.cast()); + + typedef FrameTpl Frameld; + Frameld frame2(frame); + + BOOST_CHECK(frame2 == frame.cast()); } BOOST_AUTO_TEST_CASE ( test_kinematics ) @@ -344,8 +349,8 @@ BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian ) VectorXd q = randomConfiguration(model); VectorXd v = VectorXd::Ones(model.nv); - /// In local frame - Model::Index idx = model.getFrameId(frame_name); + // In LOCAL frame + const Model::Index idx = model.getFrameId(frame_name); const Frame & frame = model.frames[idx]; BOOST_CHECK(frame.placement.isApprox_impl(framePlacement)); Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0); @@ -355,6 +360,7 @@ BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian ) computeJointJacobians(model,data,q); updateFramePlacement(model, data, idx); getFrameJacobian(model, data, idx, LOCAL, Jff); + BOOST_CHECK(Jff.isApprox(getFrameJacobian(model,data,idx,LOCAL))); computeJointJacobians(model,data_ref,q); getJointJacobian(model, data_ref, parent_idx, LOCAL, Jjj); @@ -365,16 +371,31 @@ BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian ) Data::Matrix6x Jjj_from_frame(jXf * Jff); BOOST_CHECK(Jjj_from_frame.isApprox(Jjj)); - BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12)); + BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint))); - // In world frame + // In WORLD frame + Jjj.fill(0); Jff.fill(0); Jff2.fill(0); getFrameJacobian(model,data,idx,WORLD,Jff); - getJointJacobian(model, data_ref, parent_idx,WORLD, Jjj); + BOOST_CHECK(Jff.isApprox(getFrameJacobian(model,data,idx,WORLD))); + getJointJacobian(model,data_ref,parent_idx,WORLD,Jjj); BOOST_CHECK(Jff.isApprox(Jjj)); computeFrameJacobian(model,data,q,idx,WORLD,Jff2); BOOST_CHECK(Jff2.isApprox(Jjj)); + + // In WORLD frame + Jjj.fill(0); Jff.fill(0); Jff2.fill(0); + getFrameJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,Jff); + BOOST_CHECK(Jff.isApprox(getFrameJacobian(model,data,idx,LOCAL_WORLD_ALIGNED))); + + getJointJacobian(model,data_ref,parent_idx,WORLD,Jjj); + const SE3 oMf_translation(SE3::Matrix3::Identity(),data.oMf[idx].translation()); + Jjj = oMf_translation.toActionMatrixInverse() * Jjj; + BOOST_CHECK(Jff.isApprox(Jjj)); + + computeFrameJacobian(model,data,q,idx,LOCAL_WORLD_ALIGNED,Jff2); + BOOST_CHECK(Jff2.isApprox(Jff)); } BOOST_AUTO_TEST_CASE ( test_frame_jacobian ) diff --git a/unittest/impulse-dynamics-derivatives.cpp b/unittest/impulse-dynamics-derivatives.cpp new file mode 100644 index 0000000000..a2f157bd02 --- /dev/null +++ b/unittest/impulse-dynamics-derivatives.cpp @@ -0,0 +1,331 @@ +// +// Copyright (c) 2020 CNRS INRIA +// + +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/rnea-derivatives.hpp" +#include "pinocchio/algorithm/impulse-dynamics.hpp" +#include "pinocchio/algorithm/impulse-dynamics-derivatives.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/parsers/sample-models.hpp" + +#include + +#include +#include + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +using namespace Eigen; +using namespace pinocchio; + +BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_derivatives_no_contact) +{ + //result: (dMdq)(dqafter-v) = drnea(q,0,dqafter-v) + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + + // Contact models and data + const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) empty_contact_data; + + const double mu0 = 0.; + const double r_coeff = 0.5; + + initConstraintDynamics(model,data,empty_contact_models); + impulseDynamics(model,data,q,v,empty_contact_models,empty_contact_data,r_coeff, mu0); + + const Eigen::VectorXd dv = data.dq_after-v; + computeImpulseDynamicsDerivatives(model,data,empty_contact_models,empty_contact_data,r_coeff,mu0); + + Motion gravity_bk = model.gravity; + model.gravity.setZero(); + computeRNEADerivatives(model, data_ref, q, Eigen::VectorXd::Zero(model.nv), dv); + // Reference values + BOOST_CHECK(data_ref.dtau_dq.isApprox(data.dtau_dq)); +} + +BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_derivatives) +{ + using namespace Eigen; + using namespace pinocchio; + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + VectorXd v = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_LF(CONTACT_6D,LF_id,LOCAL); + RigidConstraintModel ci_RF(CONTACT_3D,RF_id,LOCAL); + + contact_models.push_back(ci_LF); contact_data.push_back(RigidConstraintData(ci_LF)); + contact_models.push_back(ci_RF); contact_data.push_back(RigidConstraintData(ci_RF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + const double r_coeff = 0.5; + + initConstraintDynamics(model,data,contact_models); + impulseDynamics(model,data,q,v,contact_models,contact_data,r_coeff,mu0); + computeImpulseDynamicsDerivatives(model,data,contact_models,contact_data,r_coeff,mu0); + + typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector; + + ForceVector iext((size_t)model.njoints); + for(ForceVector::iterator it = iext.begin(); it != iext.end(); ++it) + (*it).setZero(); + + iext[model.getJointId(LF)] = contact_data[0].contact_force; + iext[model.getJointId(RF)] = contact_data[1].contact_force; + + Eigen::VectorXd effective_v = (1+r_coeff)*v+data.ddq; + + computeForwardKinematicsDerivatives(model, data_ref, q, effective_v, + Eigen::VectorXd::Zero(model.nv)); + + for(size_t i=0; i < data.ov.size(); i++) + { + BOOST_CHECK(((1+r_coeff)*data.ov[i]+data.oa[i] - data_ref.ov[i]).isZero()); + } + + Eigen::MatrixXd Jc(9,model.nv), dv_dq(9,model.nv), Jc_tmp(6,model.nv), dv_dq_tmp(6,model.nv); + Jc.setZero(); dv_dq.setZero(); dv_dq_tmp.setZero(); Jc_tmp.setZero(); + + getJointVelocityDerivatives(model, data_ref, LF_id,LOCAL, + dv_dq.topRows<6>(), Jc.topRows<6>()); + + getJointVelocityDerivatives(model, data_ref, RF_id,LOCAL, + dv_dq_tmp, Jc_tmp); + + Jc.bottomRows<3>() = Jc_tmp.topRows<3>(); + dv_dq.bottomRows<3>() = dv_dq_tmp.topRows<3>(); + + BOOST_CHECK(data_ref.J.isApprox(data.J)); + + const Motion gravity_bk = model.gravity; + model.gravity.setZero(); + computeRNEADerivatives(model, data_ref, q, Eigen::VectorXd::Zero(model.nv), + data.ddq, iext); + model.gravity = gravity_bk; + + BOOST_CHECK(data.dac_da.isApprox(Jc)); +// BOOST_CHECK((data.dvc_dq-(dv_dq-Jc*data.Minv*data_ref.dtau_dq)).norm()<=1e-12); + + BOOST_CHECK((data.dlambda_dv+(1+r_coeff)*data.osim*Jc).isZero()); + +} + +BOOST_AUTO_TEST_CASE ( test_impulse_dynamics_derivatives_LOCAL_fd ) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_LF(CONTACT_6D,LF_id,LOCAL); + RigidConstraintModel ci_RF(CONTACT_3D,RF_id,LOCAL); + + contact_models.push_back(ci_LF); contact_data.push_back(RigidConstraintData(ci_LF)); + contact_models.push_back(ci_RF); contact_data.push_back(RigidConstraintData(ci_RF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + const double r_coeff = 0.5; + + initConstraintDynamics(model,data,contact_models); + impulseDynamics(model,data,q,v,contact_models,contact_data,r_coeff,mu0); + computeImpulseDynamicsDerivatives(model, data, contact_models, contact_data, r_coeff, mu0); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd dqafter_partial_dq_fd(model.nv,model.nv); dqafter_partial_dq_fd.setZero(); + MatrixXd dqafter_partial_dv_fd(model.nv,model.nv); dqafter_partial_dv_fd.setZero(); + + MatrixXd impulse_partial_dq_fd(constraint_dim,model.nv); impulse_partial_dq_fd.setZero(); + MatrixXd impulse_partial_dv_fd(constraint_dim,model.nv); impulse_partial_dv_fd.setZero(); + + const VectorXd dqafter0 = impulseDynamics(model,data_fd,q,v,contact_models, + contact_data,r_coeff,mu0); + const VectorXd impulse0 = data_fd.impulse_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd dqafter_plus(model.nv); + + const Eigen::MatrixXd Jc = data.contact_chol.matrix().topRightCorner(constraint_dim,model.nv); + const Eigen::VectorXd vel_jump = Jc*(dqafter0 + r_coeff*v); + + Data data_plus(model); + VectorXd impulse_plus(constraint_dim); + + Eigen::MatrixXd dvc_dq_fd(constraint_dim,model.nv); + const double alpha = 1e-8; + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + dqafter_plus = impulseDynamics(model,data_fd,q_plus,v,contact_models,contact_data,r_coeff,mu0); + + const Eigen::MatrixXd Jc_plus = data_fd.contact_chol.matrix().topRightCorner(constraint_dim,model.nv); + const Eigen::VectorXd vel_jump_plus = Jc_plus*(dqafter0 + r_coeff*v); + + dqafter_partial_dq_fd.col(k) = (dqafter_plus - dqafter0)/alpha; + impulse_partial_dq_fd.col(k) = (data_fd.impulse_c - impulse0)/alpha; + dvc_dq_fd.col(k) = (vel_jump_plus - vel_jump)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(Jc.isApprox(data.dac_da,sqrt(alpha))); + BOOST_CHECK(dqafter_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(impulse_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + dqafter_plus = impulseDynamics(model,data_fd,q,v_plus,contact_models,contact_data,r_coeff,mu0); + + dqafter_partial_dv_fd.col(k) = (dqafter_plus - dqafter0)/alpha; + impulse_partial_dv_fd.col(k) = (data_fd.impulse_c - impulse0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(dqafter_partial_dv_fd.isApprox(Eigen::MatrixXd::Identity(model.nv,model.nv)+data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(impulse_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); +} + +BOOST_AUTO_TEST_CASE ( test_impulse_dynamics_derivatives_LOCAL_WORLD_ALIGNED_fd ) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + Data data(model), data_fd(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; + const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; + const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; + + RigidConstraintModel ci_LF(CONTACT_6D,LF_id,LOCAL_WORLD_ALIGNED); + RigidConstraintModel ci_RF(CONTACT_3D,RF_id,LOCAL_WORLD_ALIGNED); + + contact_models.push_back(ci_LF); contact_data.push_back(RigidConstraintData(ci_LF)); + contact_models.push_back(ci_RF); contact_data.push_back(RigidConstraintData(ci_RF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + const double r_coeff = 0.5; + + initConstraintDynamics(model,data,contact_models); + impulseDynamics(model,data,q,v,contact_models,contact_data,r_coeff,mu0); + computeImpulseDynamicsDerivatives(model, data, contact_models, contact_data, r_coeff, mu0); + + //Data_fd + initConstraintDynamics(model,data_fd,contact_models); + + MatrixXd dqafter_partial_dq_fd(model.nv,model.nv); dqafter_partial_dq_fd.setZero(); + MatrixXd dqafter_partial_dv_fd(model.nv,model.nv); dqafter_partial_dv_fd.setZero(); + + MatrixXd impulse_partial_dq_fd(constraint_dim,model.nv); impulse_partial_dq_fd.setZero(); + MatrixXd impulse_partial_dv_fd(constraint_dim,model.nv); impulse_partial_dv_fd.setZero(); + + const VectorXd dqafter0 = impulseDynamics(model,data_fd,q,v,contact_models, + contact_data,r_coeff,mu0); + const VectorXd impulse0 = data_fd.impulse_c; + VectorXd v_eps(VectorXd::Zero(model.nv)); + VectorXd q_plus(model.nq); + VectorXd dqafter_plus(model.nv); + + VectorXd impulse_plus(constraint_dim); + const double alpha = 1e-8; + for(int k = 0; k < model.nv; ++k) + { + v_eps[k] += alpha; + q_plus = integrate(model,q,v_eps); + dqafter_plus = impulseDynamics(model,data_fd,q_plus,v,contact_models,contact_data,r_coeff,mu0); + dqafter_partial_dq_fd.col(k) = (dqafter_plus - dqafter0)/alpha; + impulse_partial_dq_fd.col(k) = (data_fd.impulse_c - impulse0)/alpha; + v_eps[k] = 0.; + } + + BOOST_CHECK(dqafter_partial_dq_fd.isApprox(data.ddq_dq,sqrt(alpha))); + BOOST_CHECK(impulse_partial_dq_fd.isApprox(data.dlambda_dq,sqrt(alpha))); + + VectorXd v_plus(v); + for(int k = 0; k < model.nv; ++k) + { + v_plus[k] += alpha; + dqafter_plus = impulseDynamics(model,data_fd,q,v_plus,contact_models,contact_data,r_coeff,mu0); + dqafter_partial_dv_fd.col(k) = (dqafter_plus - dqafter0)/alpha; + impulse_partial_dv_fd.col(k) = (data_fd.impulse_c - impulse0)/alpha; + v_plus[k] -= alpha; + } + + BOOST_CHECK(dqafter_partial_dv_fd.isApprox(Eigen::MatrixXd::Identity(model.nv,model.nv)+data.ddq_dv,sqrt(alpha))); + BOOST_CHECK(impulse_partial_dv_fd.isApprox(data.dlambda_dv,sqrt(alpha))); +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/impulse-dynamics.cpp b/unittest/impulse-dynamics.cpp new file mode 100644 index 0000000000..af3fe3cc8c --- /dev/null +++ b/unittest/impulse-dynamics.cpp @@ -0,0 +1,586 @@ +// +// Copyright (c) 2020 CNRS INRIA +// + +#include "pinocchio/algorithm/aba.hpp" +#include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/frames.hpp" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/centroidal.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/contact-info.hpp" +#include "pinocchio/algorithm/compute-all-terms.hpp" +#include "pinocchio/algorithm/impulse-dynamics.hpp" +#include "pinocchio/algorithm/contact-dynamics.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/parsers/sample-models.hpp" +#include "pinocchio/utils/timer.hpp" + +#include + +#include +#include + +BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) + +BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_empty) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + + const double mu0 = 0.; + const double r_coeff = 0.5; + computeAllTerms(model,data_ref,q,v); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + initConstraintDynamics(model,data,contact_models); + impulseDynamics(model,data,q,v,contact_models,contact_datas,r_coeff,mu0); + + data.M.triangularView() = + data.M.transpose().triangularView(); + + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.M.isApprox(data_ref.M)); + BOOST_CHECK(data.dq_after.isApprox(v)); +} + +BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; +// const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; +// const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),WORLD); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),WORLD); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + const double r_coeff = 0.5; + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + framesForwardKinematics(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + updateFramePlacements(model,data_ref); + getJointJacobian(model,data_ref,model.getJointId(RF),WORLD,J_ref.middleRows<6>(0)); + getJointJacobian(model,data_ref,model.getJointId(LF),WORLD,J_ref.middleRows<6>(6)); + + Eigen::VectorXd rhs_ref(constraint_dim); + + rhs_ref.segment<6>(0) = getFrameVelocity(model,data_ref, + model.getFrameId(RF),WORLD).toVector(); + rhs_ref.segment<6>(6) = getFrameVelocity(model,data_ref, + model.getFrameId(LF),WORLD).toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + impulseDynamics(model,data_ref,q,v,J_ref,r_coeff,mu0); + + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + BOOST_CHECK((data_ref.M*data_ref.dq_after-data_ref.M*v-J_ref.transpose()*data_ref.impulse_c).isZero()); + BOOST_CHECK((J_ref*data_ref.dq_after+r_coeff*J_ref*v).isZero()); + + initConstraintDynamics(model,data,contact_models); + impulseDynamics(model,data,q,v,contact_models,contact_datas,r_coeff,mu0); + BOOST_CHECK((J_ref*data.dq_after+r_coeff*J_ref*v).isZero()); + data.M.triangularView() = + data.M.transpose().triangularView(); + BOOST_CHECK((data.M*data.dq_after-data.M*v-J_ref.transpose()*data.impulse_c).isZero()); + Data data_ag(model); + ccrba(model,data_ag,q,v); + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.M.isApprox(data_ref.M)); + BOOST_CHECK(data.Ag.isApprox(data_ag.Ag)); + + for(Model::JointIndex k = 1; k < model.joints.size(); ++k) + { + BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); + BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k])); + BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k]))); + //BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k]))); + } + + // Check that the decomposition is correct + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after)); + BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c)); + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.impulse_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.impulse_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } +} + +BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D_LOCAL) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; +// const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; +// const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),LOCAL); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + const double r_coeff = 0.5; + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + framesForwardKinematics(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + updateFramePlacements(model,data_ref); + getJointJacobian(model,data_ref,model.getJointId(RF),LOCAL,J_ref.middleRows<6>(0)); + getJointJacobian(model,data_ref,model.getJointId(LF),LOCAL,J_ref.middleRows<6>(6)); + + Eigen::VectorXd rhs_ref(constraint_dim); + + rhs_ref.segment<6>(0) = getFrameVelocity(model,data_ref, + model.getFrameId(RF),LOCAL).toVector(); + rhs_ref.segment<6>(6) = getFrameVelocity(model,data_ref, + model.getFrameId(LF),LOCAL).toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + impulseDynamics(model,data_ref,q,v,J_ref,r_coeff,mu0); + + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + BOOST_CHECK((data_ref.M*data_ref.dq_after-data_ref.M*v-J_ref.transpose()*data_ref.impulse_c).isZero()); + BOOST_CHECK((J_ref*data_ref.dq_after+r_coeff*J_ref*v).isZero()); + + initConstraintDynamics(model,data,contact_models); + impulseDynamics(model,data,q,v,contact_models,contact_datas,r_coeff,mu0); + BOOST_CHECK((J_ref*data.dq_after+r_coeff*J_ref*v).isZero()); + data.M.triangularView() = + data.M.transpose().triangularView(); + BOOST_CHECK((data.M*data.dq_after-data.M*v-J_ref.transpose()*data.impulse_c).isZero()); + + Data data_ag(model); + ccrba(model,data_ag,q,v); + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.M.isApprox(data_ref.M)); + BOOST_CHECK(data.Ag.isApprox(data_ag.Ag)); + + for(Model::JointIndex k = 1; k < model.joints.size(); ++k) + { + BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); + BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k])); + BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k]))); + //BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k]))); + } + + // Check that the decomposition is correct + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after)); + BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c)); + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.impulse_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.impulse_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } +} + +BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D_LOCAL_WORLD_ALIGNED) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; +// const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; +// const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),LOCAL_WORLD_ALIGNED); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_6D,model.getJointId(LF),LOCAL_WORLD_ALIGNED); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + const double r_coeff = 0.5; + Eigen::MatrixXd J_ref(constraint_dim,model.nv); + J_ref.setZero(); + + computeAllTerms(model,data_ref,q,v); + framesForwardKinematics(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + updateFramePlacements(model,data_ref); + getJointJacobian(model,data_ref,model.getJointId(RF),LOCAL_WORLD_ALIGNED,J_ref.middleRows<6>(0)); + getJointJacobian(model,data_ref,model.getJointId(LF),LOCAL_WORLD_ALIGNED,J_ref.middleRows<6>(6)); + + Eigen::VectorXd rhs_ref(constraint_dim); + + rhs_ref.segment<6>(0) = getFrameVelocity(model,data_ref, + model.getFrameId(RF),LOCAL_WORLD_ALIGNED).toVector(); + rhs_ref.segment<6>(6) = getFrameVelocity(model,data_ref, + model.getFrameId(LF),LOCAL_WORLD_ALIGNED).toVector(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + impulseDynamics(model,data_ref,q,v,J_ref,r_coeff,mu0); + + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + BOOST_CHECK((data_ref.M*data_ref.dq_after-data_ref.M*v-J_ref.transpose()*data_ref.impulse_c).isZero()); + BOOST_CHECK((J_ref*data_ref.dq_after+r_coeff*J_ref*v).isZero()); + + initConstraintDynamics(model,data,contact_models); + impulseDynamics(model,data,q,v,contact_models,contact_datas,r_coeff,mu0); + BOOST_CHECK((J_ref*data.dq_after+r_coeff*J_ref*v).isZero()); + data.M.triangularView() = + data.M.transpose().triangularView(); + BOOST_CHECK((data.M*data.dq_after-data.M*v-J_ref.transpose()*data.impulse_c).isZero()); + + Data data_ag(model); + ccrba(model,data_ag,q,v); + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.M.isApprox(data_ref.M)); + BOOST_CHECK(data.Ag.isApprox(data_ag.Ag)); + + for(Model::JointIndex k = 1; k < model.joints.size(); ++k) + { + BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); + BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k])); + BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k]))); + //BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k]))); + } + + // Check that the decomposition is correct + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after)); + BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c)); + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.impulse_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.impulse_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } +} + +BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D_3D) +{ + using namespace Eigen; + using namespace pinocchio; + + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model,true); + pinocchio::Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + VectorXd q = randomConfiguration(model); + + VectorXd v = VectorXd::Random(model.nv); + VectorXd tau = VectorXd::Random(model.nv); + + const std::string RF = "rleg6_joint"; +// const Model::JointIndex RF_id = model.getJointId(RF); + const std::string LF = "lleg6_joint"; +// const Model::JointIndex LF_id = model.getJointId(LF); + + // Contact models and data + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; + RigidConstraintModel ci_RF(CONTACT_6D,model.getJointId(RF),WORLD); + contact_models.push_back(ci_RF); + contact_datas.push_back(RigidConstraintData(ci_RF)); + RigidConstraintModel ci_LF(CONTACT_3D,model.getJointId(LF),WORLD); + contact_models.push_back(ci_LF); + contact_datas.push_back(RigidConstraintData(ci_LF)); + + Eigen::DenseIndex constraint_dim = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + constraint_dim += contact_models[k].size(); + + const double mu0 = 0.; + const double r_coeff = 0.5; + Eigen::MatrixXd J_ref(constraint_dim,model.nv), Jtmp(6,model.nv); + J_ref.setZero(); Jtmp.setZero(); + + computeAllTerms(model,data_ref,q,v); + framesForwardKinematics(model,data_ref,q); + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + + + updateFramePlacements(model,data_ref); + getJointJacobian(model,data_ref,model.getJointId(RF),WORLD,J_ref.middleRows<6>(0)); + getJointJacobian(model,data_ref,model.getJointId(LF),WORLD,Jtmp); + J_ref.middleRows<3>(6) = Jtmp.middleRows<3>(Motion::LINEAR); + + Eigen::VectorXd rhs_ref(constraint_dim); + + rhs_ref.segment<6>(0) = getFrameVelocity(model,data_ref, + model.getFrameId(RF),WORLD).toVector(); + rhs_ref.segment<3>(6) = getFrameVelocity(model,data_ref, + model.getFrameId(LF),WORLD).linear(); + + Eigen::MatrixXd KKT_matrix_ref + = Eigen::MatrixXd::Zero(model.nv+constraint_dim,model.nv+constraint_dim); + KKT_matrix_ref.bottomRightCorner(model.nv,model.nv) = data_ref.M; + KKT_matrix_ref.topRightCorner(constraint_dim,model.nv) = J_ref; + KKT_matrix_ref.bottomLeftCorner(model.nv,constraint_dim) = J_ref.transpose(); + + impulseDynamics(model,data_ref,q,v,J_ref,r_coeff,mu0); + + data_ref.M.triangularView() = + data_ref.M.transpose().triangularView(); + BOOST_CHECK((data_ref.M*data_ref.dq_after-data_ref.M*v-J_ref.transpose()*data_ref.impulse_c).isZero()); + BOOST_CHECK((J_ref*data_ref.dq_after+r_coeff*J_ref*v).isZero()); + + initConstraintDynamics(model,data,contact_models); + impulseDynamics(model,data,q,v,contact_models,contact_datas,r_coeff,mu0); + BOOST_CHECK((J_ref*data.dq_after+r_coeff*J_ref*v).isZero()); + data.M.triangularView() = + data.M.transpose().triangularView(); + BOOST_CHECK((data.M*data.dq_after-data.M*v-J_ref.transpose()*data.impulse_c).isZero()); + + Data data_ag(model); + ccrba(model,data_ag,q,v); + BOOST_CHECK(data.J.isApprox(data_ref.J)); + BOOST_CHECK(data.M.isApprox(data_ref.M)); + BOOST_CHECK(data.Ag.isApprox(data_ag.Ag)); + + for(Model::JointIndex k = 1; k < model.joints.size(); ++k) + { + BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); + BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k])); + BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k]))); + //BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k]))); + } + + // Check that the decomposition is correct + const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; + Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); + + BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv,model.nv).isApprox(KKT_matrix_ref.bottomRightCorner(model.nv,model.nv))); + BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref)); + + // Check solutions + BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after)); + BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c)); + + Eigen::DenseIndex constraint_id = 0; + for(size_t k = 0; k < contact_models.size(); ++k) + { + const RigidConstraintModel & cmodel = contact_models[k]; + const RigidConstraintData & cdata = contact_datas[k]; + + switch(cmodel.type) + { + case pinocchio::CONTACT_3D: + { + BOOST_CHECK(cdata.contact_force.linear().isApprox(data_ref.impulse_c.segment(constraint_id,cmodel.size()))); + break; + } + + case pinocchio::CONTACT_6D: + { + ForceRef::Type> f_ref(data_ref.impulse_c.segment<6>(constraint_id)); + BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); + break; + } + + default: + break; + } + + constraint_id += cmodel.size(); + } +} + + + +BOOST_AUTO_TEST_SUITE_END () diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 6f04dab3cf..78bf362644 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -6,6 +6,7 @@ #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/kinematics.hpp" +#include #include #include @@ -87,8 +88,9 @@ void test_joint_methods(const JointModelBase & jmodel, JointModelCom Inertia::Matrix6 I1 = I0; Inertia::Matrix6 I2 = I0; - jmodel.calc_aba(jdata,I1,true); - jmodel_composite.calc_aba(jdata_composite,I2,true); + const Eigen::VectorXd armature = Eigen::VectorXd::Random(jmodel.nv()) + Eigen::VectorXd::Ones(jmodel.nv()); + jmodel.calc_aba(jdata,armature,I1,true); + jmodel_composite.calc_aba(jdata_composite,armature,I2,true); double prec = 1e-10; // higher tolerance to errors due to possible numerical imprecisions @@ -112,7 +114,7 @@ void test_joint_methods(const JointModelBase & jmodel, JointModelCom jmodel_composite2.calc(jdata_composite2,q,v); Inertia::Matrix6 I3 = I0; - jmodel_composite2.calc_aba(jdata_composite2,I3,true); + jmodel_composite2.calc_aba(jdata_composite2,armature,I3,true); if(jmodel.shortname() == "JointModelFreeFlyer") BOOST_CHECK((I3-I2).lpNorm() < prec); diff --git a/unittest/joint-free-flyer.cpp b/unittest/joint-free-flyer.cpp index 99177a188f..1e4a6671c0 100644 --- a/unittest/joint-free-flyer.cpp +++ b/unittest/joint-free-flyer.cpp @@ -22,7 +22,7 @@ BOOST_AUTO_TEST_CASE(spatial) { Motion v(Motion::Random()); - ConstraintIdentityTpl constraint; + JointMotionSubspaceIdentityTpl constraint; Motion Sv = constraint * v.toVector(); BOOST_CHECK(Sv == v); diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index c3300c4616..ce7f715aab 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -10,6 +10,7 @@ #include "pinocchio/algorithm/joint-configuration.hpp" +#include #include #include @@ -24,6 +25,8 @@ void test_joint_methods(JointModelBase & jmodel, std::cout << "Testing Joint over " << jmodel.shortname() << std::endl; Eigen::VectorXd q1, q2; + Eigen::VectorXd armature = Eigen::VectorXd::Random(jdata.S().nv()) + Eigen::VectorXd::Ones(jdata.S().nv()); + q1 = LieGroupType().random(); q2 = LieGroupType().random(); @@ -38,31 +41,30 @@ void test_joint_methods(JointModelBase & jmodel, jmodel.calc(jdata.derived(), q1, v1); - jmodel.calc_aba(jdata.derived(), Ia, update_I); + jmodel.calc_aba(jdata.derived(), armature, Ia, update_I); pinocchio::JointModel jma(jmodel); BOOST_CHECK(jmodel == jma); BOOST_CHECK(jma == jmodel); BOOST_CHECK(jma.hasSameIndexes(jmodel)); - pinocchio::JointData jda(jdata); + pinocchio::JointData jda(jdata.derived()); BOOST_CHECK(jda == jdata); BOOST_CHECK(jdata == jda); jma.calc(jda, q1, v1); - jma.calc_aba(jda, Ia, update_I); - + jma.calc_aba(jda, armature, Ia, update_I); pinocchio::JointData jda_other(jdata); jma.calc(jda_other, q2, v2); - jma.calc_aba(jda_other, Ia2, update_I); + jma.calc_aba(jda_other, armature, Ia2, update_I); BOOST_CHECK(jda_other != jda); BOOST_CHECK(jda != jda_other); BOOST_CHECK(jda_other != jdata); BOOST_CHECK(jdata != jda_other); - const std::string error_prefix("JointModel on " + jma.shortname()); + std::string error_prefix("JointModel on " + jma.shortname()); BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq ")); BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv ")); @@ -70,7 +72,7 @@ void test_joint_methods(JointModelBase & jmodel, BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v() ,std::string(error_prefix + " - Idx_v ")); BOOST_CHECK_MESSAGE(jmodel.id() == jma.id() ,std::string(error_prefix + " - JointId ")); - BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S().matrix()),std::string(error_prefix + " - ConstraintXd ")); + BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S().matrix()),std::string(error_prefix + " - JointMotionSubspaceXd ")); BOOST_CHECK_MESSAGE( (jda.M()).isApprox((jdata.M())),std::string(error_prefix + " - Joint transforms ")); // == or isApprox ? BOOST_CHECK_MESSAGE( (jda.v()).isApprox( (pinocchio::Motion(jdata.v()))),std::string(error_prefix + " - Joint motions ")); BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c()),std::string(error_prefix + " - Joint bias ")); @@ -232,13 +234,19 @@ namespace pinocchio template class JointCollectionTpl> struct JointModelTest; template class JointCollectionTpl> struct JointDataTest; - template class JointCollectionTpl> - struct traits< JointDataTest > - { typedef JointTpl JointDerived; }; + template class JointCollectionTpl> + struct traits< JointDataTest<_Scalar,_Options,JointCollectionTpl> > + { + typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + typedef _Scalar Scalar; + }; - template class JointCollectionTpl> - struct traits< JointModelTest > - { typedef JointTpl JointDerived; }; + template class JointCollectionTpl> + struct traits< JointModelTest<_Scalar,_Options,JointCollectionTpl> > + { + typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + typedef _Scalar Scalar; + }; template class JointCollectionTpl> struct traits< JointTest<_Scalar,_Options,JointCollectionTpl> > @@ -252,7 +260,7 @@ namespace pinocchio typedef JointDataTpl JointDataDerived; typedef JointModelTpl JointModelDerived; - typedef ConstraintTpl Constraint_t; + typedef JointMotionSubspaceTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionTpl Motion_t; typedef MotionTpl Bias_t; @@ -343,7 +351,7 @@ BOOST_AUTO_TEST_CASE(isEqual) BOOST_CHECK(jmodelx_copy == jmodelx.derived()); JointModel jmodely(joint_revolutey); - // TODO: the comparison of two variants is not supported by some old version of BOOST + // TDDO: the comparison of two variants is not supported by some old version of BOOST // BOOST_CHECK(jmodely.toVariant() != jmodelx.toVariant()); BOOST_CHECK(jmodely != jmodelx); } @@ -381,8 +389,10 @@ struct TestJointOperatorEqual JointData jdata = jmodel.createData(); jmodel.calc(jdata,q,v); + + Eigen::VectorXd armature = Eigen::VectorXd::Random(jmodel.nv()) + Eigen::VectorXd::Ones(jmodel.nv()); Inertia::Matrix6 I = Inertia::Matrix6::Identity(); - jmodel.calc_aba(jdata,I,false); + jmodel.calc_aba(jdata,armature,I,false); test(jdata); } diff --git a/unittest/joint-jacobian.cpp b/unittest/joint-jacobian.cpp index 215ade1acc..4538fd847d 100644 --- a/unittest/joint-jacobian.cpp +++ b/unittest/joint-jacobian.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2019 CNRS INRIA +// Copyright (c) 2015-2020 CNRS INRIA // #include "pinocchio/multibody/model.hpp" @@ -96,6 +96,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation ) // Regarding to the WORLD origin getJointJacobian(model,data,idx,WORLD,J); + BOOST_CHECK(J.isApprox(getJointJacobian(model,data,idx,WORLD))); getJointJacobianTimeVariation(model,data,idx,WORLD,dJ); Motion v_idx(J*v); @@ -107,6 +108,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation ) // Regarding to the LOCAL frame getJointJacobian(model,data,idx,LOCAL,J); + BOOST_CHECK(J.isApprox(getJointJacobian(model,data,idx,LOCAL))); getJointJacobianTimeVariation(model,data,idx,LOCAL,dJ); v_idx = (Motion::Vector6)(J*v); @@ -117,6 +119,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation ) // Regarding to the LOCAL_WORLD_ALIGNED frame getJointJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,J); + BOOST_CHECK(J.isApprox(getJointJacobian(model,data,idx,LOCAL_WORLD_ALIGNED))); getJointJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ); Data::SE3 worldMlocal = data.oMi[idx]; diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 3bc1db8e12..30ec71f067 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -20,13 +20,13 @@ void test_constraint_mimic(const JointModelBase & jmodel) typedef typename traits::JointDerived Joint; typedef typename traits::Constraint_t ConstraintType; typedef typename traits::JointDataDerived JointData; - typedef ScaledConstraint ScaledConstraintType; + typedef ScaledJointMotionSubspace ScaledJointMotionSubspaceType; JointData jdata = jmodel.createData(); const double scaling_factor = 2.; ConstraintType constraint_ref(jdata.S), constraint_ref_shared(jdata.S); - ScaledConstraintType scaled_constraint(constraint_ref_shared,scaling_factor); + ScaledJointMotionSubspaceType scaled_constraint(constraint_ref_shared,scaling_factor); BOOST_CHECK(constraint_ref.nv() == scaled_constraint.nv()); @@ -40,23 +40,23 @@ void test_constraint_mimic(const JointModelBase & jmodel) { SE3 M = SE3::Random(); - typename ScaledConstraintType::DenseBase S = M.act(scaled_constraint); - typename ScaledConstraintType::DenseBase S_ref = scaling_factor * M.act(constraint_ref); + typename ScaledJointMotionSubspaceType::DenseBase S = M.act(scaled_constraint); + typename ScaledJointMotionSubspaceType::DenseBase S_ref = scaling_factor * M.act(constraint_ref); BOOST_CHECK(S.isApprox(S_ref)); } { - typename ScaledConstraintType::DenseBase S = scaled_constraint.matrix(); - typename ScaledConstraintType::DenseBase S_ref = scaling_factor * constraint_ref.matrix(); + typename ScaledJointMotionSubspaceType::DenseBase S = scaled_constraint.matrix(); + typename ScaledJointMotionSubspaceType::DenseBase S_ref = scaling_factor * constraint_ref.matrix(); BOOST_CHECK(S.isApprox(S_ref)); } { Motion v = Motion::Random(); - typename ScaledConstraintType::DenseBase S = v.cross(scaled_constraint); - typename ScaledConstraintType::DenseBase S_ref = scaling_factor * v.cross(constraint_ref); + typename ScaledJointMotionSubspaceType::DenseBase S = v.cross(scaled_constraint); + typename ScaledJointMotionSubspaceType::DenseBase S_ref = scaling_factor * v.cross(constraint_ref); BOOST_CHECK(S.isApprox(S_ref)); } diff --git a/unittest/constraint.cpp b/unittest/joint-motion-subspace.cpp similarity index 96% rename from unittest/constraint.cpp rename to unittest/joint-motion-subspace.cpp index 4a9189ec50..8fd6607b02 100644 --- a/unittest/constraint.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -85,14 +85,14 @@ void test_jmodel_nq_against_nq_ref(const JointModelMimic & jmodel, template void test_nv_against_jmodel(const JointModelBase & jmodel, - const ConstraintBase & constraint) + const JointMotionSubspaceBase & constraint) { BOOST_CHECK(constraint.nv() == jmodel.nv()); } template void test_nv_against_jmodel(const JointModelMimic & jmodel, - const ConstraintBase & constraint) + const JointMotionSubspaceBase & constraint) { BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv()); } @@ -262,6 +262,13 @@ void test_constraint_operations(const JointModelBase & jmodel) BOOST_CHECK(YS.isApprox(YS_ref)); } + // Test constrainst operations + { + Eigen::MatrixXd StS = constraint.transpose() * constraint; + Eigen::MatrixXd StS_ref = constraint_mat.transpose() * constraint_mat; + BOOST_CHECK(StS.isApprox(StS_ref)); + } + } template struct init; diff --git a/unittest/kinematics-derivatives.cpp b/unittest/kinematics-derivatives.cpp index 0b1f6e460b..77d4b1cd2b 100644 --- a/unittest/kinematics-derivatives.cpp +++ b/unittest/kinematics-derivatives.cpp @@ -2,6 +2,8 @@ // Copyright (c) 2017-2020 CNRS INRIA // +#include + #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" #include "pinocchio/algorithm/jacobian.hpp" @@ -10,8 +12,6 @@ #include "pinocchio/algorithm/kinematics-derivatives.hpp" #include "pinocchio/parsers/sample-models.hpp" -#include - #include #include @@ -540,6 +540,269 @@ BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_against_classic_formula) } +BOOST_AUTO_TEST_CASE(test_classic_acceleration_derivatives) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + + Data data(model), data_ref(model), data_plus(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + VectorXd v = VectorXd::Random(model.nv); + VectorXd a = VectorXd::Random(model.nv); + + const Model::JointIndex joint_id = model.existJointName("rarm4_joint")?model.getJointId("rarm4_joint"):(Model::Index)(model.njoints-1); + Data::Matrix3x v3_partial_dq(3,model.nv); v3_partial_dq.setZero(); + Data::Matrix3x a3_partial_dq(3,model.nv); a3_partial_dq.setZero(); + Data::Matrix3x a3_partial_dv(3,model.nv); a3_partial_dv.setZero(); + Data::Matrix3x a3_partial_da(3,model.nv); a3_partial_da.setZero(); + + Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero(); + Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero(); + Data::Matrix6x a_partial_dq_ref(6,model.nv); a_partial_dq_ref.setZero(); + Data::Matrix6x a_partial_dv_ref(6,model.nv); a_partial_dv_ref.setZero(); + Data::Matrix6x a_partial_da_ref(6,model.nv); a_partial_da_ref.setZero(); + + computeForwardKinematicsDerivatives(model,data_ref,q,0*v,a); + computeForwardKinematicsDerivatives(model,data,q,0*v,a); + + // LOCAL + getJointAccelerationDerivatives(model,data_ref,joint_id,LOCAL, + v_partial_dq_ref,v_partial_dv_ref, + a_partial_dq_ref,a_partial_dv_ref,a_partial_da_ref); + + + getPointClassicAccelerationDerivatives(model,data,joint_id,SE3::Identity(),LOCAL, + v3_partial_dq,a3_partial_dq,a3_partial_dv,a3_partial_da); + + BOOST_CHECK(v3_partial_dq.isApprox(v_partial_dq_ref.middleRows<3>(Motion::LINEAR))); + BOOST_CHECK(a3_partial_dq.isApprox(a_partial_dq_ref.middleRows<3>(Motion::LINEAR))); + BOOST_CHECK(a3_partial_dv.isApprox(a_partial_dv_ref.middleRows<3>(Motion::LINEAR))); + BOOST_CHECK(a3_partial_da.isApprox(a_partial_da_ref.middleRows<3>(Motion::LINEAR))); + + // LOCAL_WORLD_ALIGNED + v_partial_dq_ref.setZero(); + v_partial_dv_ref.setZero(); + a_partial_dq_ref.setZero(); + a_partial_dv_ref.setZero(); + a_partial_da_ref.setZero(); + getJointAccelerationDerivatives(model,data_ref,joint_id,LOCAL_WORLD_ALIGNED, + v_partial_dq_ref,v_partial_dv_ref, + a_partial_dq_ref,a_partial_dv_ref,a_partial_da_ref); + + v3_partial_dq.setZero(); + a3_partial_dq.setZero(); + a3_partial_dv.setZero(); + a3_partial_da.setZero(); + getPointClassicAccelerationDerivatives(model,data,joint_id,SE3::Identity(),LOCAL_WORLD_ALIGNED, + v3_partial_dq,a3_partial_dq,a3_partial_dv,a3_partial_da); + + BOOST_CHECK(v3_partial_dq.isApprox(v_partial_dq_ref.middleRows<3>(Motion::LINEAR))); +// BOOST_CHECK(a3_partial_dq.isApprox(a_partial_dq_ref.middleRows<3>(Motion::LINEAR))); + BOOST_CHECK(a3_partial_dv.isApprox(a_partial_dv_ref.middleRows<3>(Motion::LINEAR))); + BOOST_CHECK(a3_partial_da.isApprox(a_partial_da_ref.middleRows<3>(Motion::LINEAR))); + +// std::cout << "a3_partial_dq:\n" << a3_partial_dq << std::endl; +// std::cout << "a3_partial_dq_ref:\n" << a_partial_dq_ref.middleRows<3>(Motion::LINEAR) << std::endl; + + const SE3 iMpoint = SE3::Random(); + computeForwardKinematicsDerivatives(model,data,q,v,a); + + v3_partial_dq.setZero(); + a3_partial_dq.setZero(); + a3_partial_dv.setZero(); + a3_partial_da.setZero(); + getPointClassicAccelerationDerivatives(model,data,joint_id,iMpoint,LOCAL, + v3_partial_dq,a3_partial_dq,a3_partial_dv,a3_partial_da); + + Data::Matrix3x v3_partial_dq_LWA(3,model.nv); v3_partial_dq_LWA.setZero(); + Data::Matrix3x a3_partial_dq_LWA(3,model.nv); a3_partial_dq_LWA.setZero(); + Data::Matrix3x a3_partial_LWA_dv(3,model.nv); a3_partial_LWA_dv.setZero(); + Data::Matrix3x a3_partial_LWA_da(3,model.nv); a3_partial_LWA_da.setZero(); + + getPointClassicAccelerationDerivatives(model,data,joint_id,iMpoint,LOCAL_WORLD_ALIGNED, + v3_partial_dq_LWA,a3_partial_dq_LWA,a3_partial_LWA_dv,a3_partial_LWA_da); + + const double eps = 1e-8; + Eigen::VectorXd v_plus = Eigen::VectorXd::Zero(model.nv); + + Data::Matrix3x v3_partial_dq_fd(3,model.nv); + Data::Matrix3x v3_partial_dv_fd(3,model.nv); + Data::Matrix3x a3_partial_dq_fd(3,model.nv); + Data::Matrix3x a3_partial_dv_fd(3,model.nv); + Data::Matrix3x a3_partial_da_fd(3,model.nv); + + Data::Matrix3x v3_partial_dq_LWA_fd(3,model.nv); + Data::Matrix3x v3_partial_dv_LWA_fd(3,model.nv); + Data::Matrix3x a3_partial_dq_LWA_fd(3,model.nv); + Data::Matrix3x a3_partial_dv_LWA_fd(3,model.nv); + Data::Matrix3x a3_partial_da_LWA_fd(3,model.nv); + + const SE3 oMpoint = data.oMi[joint_id] * iMpoint; + const Motion::Vector3 point_vec_L = iMpoint.actInv(data.v[joint_id]).linear(); // LOCAL + const Motion::Vector3 point_acc_L = classicAcceleration(data.v[joint_id],data.a[joint_id],iMpoint); // LOCAL + const Motion::Vector3 point_vec_LWA = oMpoint.rotation() * point_vec_L; // LOCAL_WORLD_ALIGNED + const Motion::Vector3 point_acc_LWA = oMpoint.rotation() * point_acc_L; // LOCAL_WORLD_ALIGNED + + // Derivatives w.r.t q + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + v_plus[k] = eps; + const VectorXd q_plus = integrate(model,q,v_plus); + forwardKinematics(model,data_plus,q_plus,v,a); + + const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint; + const Motion::Vector3 point_vec_L_plus = iMpoint.actInv(data_plus.v[joint_id]).linear(); + const Motion::Vector3 point_acc_L_plus = classicAcceleration(data_plus.v[joint_id],data_plus.a[joint_id],iMpoint); + + const Motion::Vector3 point_vec_LWA_plus = oMpoint_plus.rotation() * point_vec_L_plus; + const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus; + + v3_partial_dq_fd.col(k) = (point_vec_L_plus - point_vec_L)/eps; + a3_partial_dq_fd.col(k) = (point_acc_L_plus - point_acc_L)/eps; + + v3_partial_dq_LWA_fd.col(k) = (point_vec_LWA_plus - point_vec_LWA)/eps; + a3_partial_dq_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA)/eps; + + v_plus[k] = 0.; + } + + BOOST_CHECK(v3_partial_dq_fd.isApprox(v3_partial_dq,sqrt(eps))); + BOOST_CHECK(a3_partial_dq_fd.isApprox(a3_partial_dq,sqrt(eps))); + + BOOST_CHECK(v3_partial_dq_LWA_fd.isApprox(v3_partial_dq_LWA,sqrt(eps))); + BOOST_CHECK(a3_partial_dq_LWA_fd.isApprox(a3_partial_dq_LWA,sqrt(eps))); + + // Derivatives w.r.t v + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + v_plus = v; + v_plus[k] += eps; + forwardKinematics(model,data_plus,q,v_plus,a); + + const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint; + const Motion::Vector3 point_vec_L_plus = iMpoint.actInv(data_plus.v[joint_id]).linear(); + const Motion::Vector3 point_acc_L_plus = classicAcceleration(data_plus.v[joint_id],data_plus.a[joint_id],iMpoint); + + const Motion::Vector3 point_vec_LWA_plus = oMpoint_plus.rotation() * point_vec_L_plus; + const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus; + + v3_partial_dv_fd.col(k) = (point_vec_L_plus - point_vec_L)/eps; + a3_partial_dv_fd.col(k) = (point_acc_L_plus - point_acc_L)/eps; + + v3_partial_dv_LWA_fd.col(k) = (point_vec_LWA_plus - point_vec_LWA)/eps; + a3_partial_dv_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA)/eps; + } + + BOOST_CHECK(v3_partial_dv_fd.isApprox(a3_partial_da,sqrt(eps))); + BOOST_CHECK(a3_partial_dv_fd.isApprox(a3_partial_dv,sqrt(eps))); + + // Derivatives w.r.t v + Eigen::VectorXd a_plus = Eigen::VectorXd::Zero(model.nv); + for(Eigen::DenseIndex k = 0; k < model.nv; ++k) + { + a_plus = a; + a_plus[k] += eps; + forwardKinematics(model,data_plus,q,v,a_plus); + + const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint; + const Motion::Vector3 point_acc_L_plus = classicAcceleration(data_plus.v[joint_id],data_plus.a[joint_id],iMpoint); + + const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus; + + a3_partial_da_fd.col(k) = (point_acc_L_plus - point_acc_L)/eps; + a3_partial_da_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA)/eps; + } + + BOOST_CHECK(a3_partial_da_fd.isApprox(a3_partial_da,sqrt(eps))); + + // Test other signature + Data data_other(model); + Data::Matrix3x v3_partial_dq_other(3,model.nv); v3_partial_dq_other.setZero(); + Data::Matrix3x v3_partial_dv_other(3,model.nv); v3_partial_dv_other.setZero(); + Data::Matrix3x a3_partial_dq_other(3,model.nv); a3_partial_dq_other.setZero(); + Data::Matrix3x a3_partial_dv_other(3,model.nv); a3_partial_dv_other.setZero(); + Data::Matrix3x a3_partial_da_other(3,model.nv); a3_partial_da_other.setZero(); + + computeForwardKinematicsDerivatives(model,data_other,q,v,a); + getPointClassicAccelerationDerivatives(model,data_other,joint_id,iMpoint,LOCAL, + v3_partial_dq_other,v3_partial_dv_other, + a3_partial_dq_other,a3_partial_dv_other,a3_partial_da_other); + + BOOST_CHECK(v3_partial_dq_other.isApprox(v3_partial_dq)); + BOOST_CHECK(v3_partial_dv_other.isApprox(a3_partial_da)); + BOOST_CHECK(a3_partial_dq_other.isApprox(a3_partial_dq)); + BOOST_CHECK(a3_partial_dv_other.isApprox(a3_partial_dv)); + BOOST_CHECK(a3_partial_da_other.isApprox(a3_partial_da)); +} + +BOOST_AUTO_TEST_CASE(test_classic_velocity_derivatives) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model,true); + + Data data(model), data_ref(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + VectorXd q = randomConfiguration(model); + VectorXd v = VectorXd::Random(model.nv); + VectorXd a = VectorXd::Random(model.nv); + + const SE3 iMpoint = SE3::Random(); + const Model::JointIndex joint_id = model.existJointName("rarm4_joint")?model.getJointId("rarm4_joint"):(Model::Index)(model.njoints-1); + Data::Matrix3x v3_partial_dq_L(3,model.nv); v3_partial_dq_L.setZero(); + Data::Matrix3x v3_partial_dv_L(3,model.nv); v3_partial_dv_L.setZero(); + + computeForwardKinematicsDerivatives(model,data,q,v,0*a); + getPointVelocityDerivatives(model,data,joint_id,iMpoint,LOCAL, + v3_partial_dq_L,v3_partial_dv_L); + + Data::Matrix3x v_partial_dq_ref_L(3,model.nv); v_partial_dq_ref_L.setZero(); + Data::Matrix3x v_partial_dv_ref_L(3,model.nv); v_partial_dv_ref_L.setZero(); + Data::Matrix3x a_partial_dq_ref_L(3,model.nv); a_partial_dq_ref_L.setZero(); + Data::Matrix3x a_partial_dv_ref_L(3,model.nv); a_partial_dv_ref_L.setZero(); + Data::Matrix3x a_partial_da_ref_L(3,model.nv); a_partial_da_ref_L.setZero(); + + computeForwardKinematicsDerivatives(model,data_ref,q,v,a); + getPointClassicAccelerationDerivatives(model,data_ref,joint_id,iMpoint,LOCAL, + v_partial_dq_ref_L,v_partial_dv_ref_L, + a_partial_dq_ref_L,a_partial_dv_ref_L, + a_partial_da_ref_L); + + BOOST_CHECK(v3_partial_dq_L.isApprox(v_partial_dq_ref_L)); + BOOST_CHECK(v3_partial_dv_L.isApprox(v_partial_dv_ref_L)); + + // LOCAL_WORLD_ALIGNED + Data::Matrix3x v3_partial_dq_LWA(3,model.nv); v3_partial_dq_LWA.setZero(); + Data::Matrix3x v3_partial_dv_LWA(3,model.nv); v3_partial_dv_LWA.setZero(); + + getPointVelocityDerivatives(model,data,joint_id,iMpoint,LOCAL_WORLD_ALIGNED, + v3_partial_dq_LWA,v3_partial_dv_LWA); + + Data::Matrix3x v_partial_dq_ref_LWA(3,model.nv); v_partial_dq_ref_LWA.setZero(); + Data::Matrix3x v_partial_dv_ref_LWA(3,model.nv); v_partial_dv_ref_LWA.setZero(); + Data::Matrix3x a_partial_dq_ref_LWA(3,model.nv); a_partial_dq_ref_LWA.setZero(); + Data::Matrix3x a_partial_dv_ref_LWA(3,model.nv); a_partial_dv_ref_LWA.setZero(); + Data::Matrix3x a_partial_da_ref_LWA(3,model.nv); a_partial_da_ref_LWA.setZero(); + + getPointClassicAccelerationDerivatives(model,data_ref,joint_id,iMpoint,LOCAL_WORLD_ALIGNED, + v_partial_dq_ref_LWA,v_partial_dv_ref_LWA, + a_partial_dq_ref_LWA,a_partial_dv_ref_LWA, + a_partial_da_ref_LWA); + + BOOST_CHECK(v3_partial_dq_LWA.isApprox(v_partial_dq_ref_LWA)); + BOOST_CHECK(v3_partial_dv_LWA.isApprox(v_partial_dv_ref_LWA)); +} + BOOST_AUTO_TEST_CASE(test_kinematics_hessians) { using namespace Eigen; diff --git a/unittest/liegroups.cpp b/unittest/liegroups.cpp index 872b76456c..979e31fe6d 100644 --- a/unittest/liegroups.cpp +++ b/unittest/liegroups.cpp @@ -93,7 +93,7 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &) BOOST_CHECK_MESSAGE(q_interpolate.isApprox(q1), std::string("Error when interpolating " + jmodel.shortname())); q_interpolate = LieGroupType().interpolate(q1,q2,1.); - BOOST_CHECK_MESSAGE(q_interpolate.isApprox(q2), std::string("Error when interpolating " + jmodel.shortname())); + BOOST_CHECK_MESSAGE(q_interpolate.isApprox(q2,1e1*prec), std::string("Error when interpolating " + jmodel.shortname())); if(jmodel.shortname() != "JointModelSphericalZYX") { diff --git a/unittest/model.cpp b/unittest/model.cpp index 7d450c108f..8be31f8f44 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2016-2019 CNRS INRIA +// Copyright (c) 2016-2020 CNRS INRIA // #include "pinocchio/multibody/data.hpp" @@ -28,9 +28,18 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) Model::JointIndex idx_larm1 = model.getJointId("larm1_joint"); BOOST_CHECK(idx_larm1<(Model::JointIndex)model.njoints); - Model::IndexVector subtree = model.subtrees[idx_larm1]; + Model::IndexVector & subtree = model.subtrees[idx_larm1]; BOOST_CHECK(subtree.size()==6); + for(size_t joint_id= 0; joint_id < model.joints.size(); ++joint_id) + { + const Model::IndexVector & children = model.children[joint_id]; + for(size_t i = 0; i < children.size(); ++i) + { + BOOST_CHECK(model.parents[children[i]] == joint_id); + } + } + for(size_t i=1; i() == model.cast()); BOOST_CHECK(model.cast().cast() == model.cast()); + + typedef ModelTpl Modelld; + + Modelld model2(model); + BOOST_CHECK(model2 == model.cast()); } BOOST_AUTO_TEST_CASE(test_std_vector_of_Model) diff --git a/unittest/python/CMakeLists.txt b/unittest/python/CMakeLists.txt index 7d03357a9b..a33b6767fe 100644 --- a/unittest/python/CMakeLists.txt +++ b/unittest/python/CMakeLists.txt @@ -78,3 +78,7 @@ FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) MAKE_DIRECTORY("${CMAKE_CURRENT_BINARY_DIR}/serialization-data") + +IF(BUILD_WITH_CASADI_SUPPORT) + ADD_SUBDIRECTORY(casadi) +ENDIF(BUILD_WITH_CASADI_SUPPORT) diff --git a/unittest/python/bindings_joints.py b/unittest/python/bindings_joints.py index e77af74e50..6e5335cba3 100644 --- a/unittest/python/bindings_joints.py +++ b/unittest/python/bindings_joints.py @@ -1,28 +1,81 @@ import unittest +from test_case import PinocchioTestCase as TestCase + import pinocchio as pin import numpy as np -joint_types = [ - pin.JointModelRX, pin.JointModelRY, pin.JointModelRZ, - pin.JointModelPX, pin.JointModelPY, pin.JointModelPZ, - pin.JointModelFreeFlyer, pin.JointModelSpherical, pin.JointModelSphericalZYX, - pin.JointModelPlanar, pin.JointModelTranslation -] +class TestJointsAlgo(TestCase): + + def setUp(self): + self.model = pin.buildSampleModelHumanoidRandom() + + qmax = np.full((self.model.nq,1),np.pi) + self.q = pin.randomConfiguration(self.model,-qmax,qmax) + self.v = np.random.rand(self.model.nv) + + def test_basic(self): + model = self.model + + q_ones = np.ones((model.nq)) + self.assertFalse(pin.isNormalized(model, q_ones)) + self.assertFalse(pin.isNormalized(model, q_ones, 1e-8)) + self.assertTrue(pin.isNormalized(model, q_ones, 1e2)) + + q_rand = np.random.rand((model.nq)) + q_rand = pin.normalize(model,q_rand) + self.assertTrue(pin.isNormalized(model, q_rand)) + self.assertTrue(pin.isNormalized(model, q_rand, 1e-8)) + + self.assertTrue(abs(np.linalg.norm(q_rand[3:7])-1.) <= 1e-8) + + q_next = pin.integrate(model,self.q,np.zeros((model.nv))) + self.assertApprox(q_next,self.q) + + v_diff = pin.difference(model,self.q,q_next) + self.assertApprox(v_diff,np.zeros((model.nv))) + + q_next = pin.integrate(model,self.q,self.v) + q_int = pin.interpolate(model,self.q,q_next,0.5) + + self.assertApprox(q_int,q_int) + + value = pin.squaredDistance(model,self.q,self.q) + self.assertTrue((value <= 1e-8).all()) + + dist = pin.distance(model,self.q,self.q) + self.assertTrue(dist <= 1e-8) + + q_neutral = pin.neutral(model) + self.assertApprox(q_neutral,q_neutral) + + q_rand1 = pin.randomConfiguration(model) + q_rand2 = pin.randomConfiguration(model,-np.ones((model.nq)),np.ones((model.nq))) + + self.assertTrue(pin.isSameConfiguration(model,self.q,self.q,1e-8)) + + self.assertFalse(pin.isSameConfiguration(model,q_rand1,q_rand2,1e-8)) + + def test_derivatives(self): + model = self.model + + q = self.q + v = self.v -class TestJoints(unittest.TestCase): + J0, J1 = pin.dIntegrate(model,q,v) + res_0 = pin.dIntegrate(model,q,v,pin.ARG0) + res_1 = pin.dIntegrate(model,q,v,pin.ARG1) - def test_comparison_operators(self): - for joint_type in joint_types: + self.assertApprox(J0,res_0) + self.assertApprox(J1,res_1) - j = joint_type() - joint_model = pin.JointModel(j) + q_next = pin.integrate(model,q,v) - self.assertTrue(j == joint_type()) - self.assertTrue(j == joint_model) + J0, J1 = pin.dDifference(model,q,q_next) + res_0 = pin.dDifference(model,q,q_next,pin.ARG0) + res_1 = pin.dDifference(model,q,q_next,pin.ARG1) - j.setIndexes(0,0,0) - self.assertFalse(j == joint_model) - self.assertTrue(j != joint_model) + self.assertApprox(J0,res_0) + self.assertApprox(J1,res_1) if __name__ == '__main__': unittest.main() diff --git a/unittest/python/casadi/CMakeLists.txt b/unittest/python/casadi/CMakeLists.txt new file mode 100644 index 0000000000..578893d8f0 --- /dev/null +++ b/unittest/python/casadi/CMakeLists.txt @@ -0,0 +1,11 @@ +# +# Copyright (c) 2020-2021 INRIA +# + +SET(${PROJECT_NAME}_PYTHON_CASADI_TESTS + bindings_main_algo +) + +FOREACH(test ${${PROJECT_NAME}_PYTHON_CASADI_TESTS}) + ADD_PYTHON_UNIT_TEST("test-py-casadi-${test}" "unittest/python/casadi/${test}.py" "bindings/python") +ENDFOREACH(test ${${PROJECT_NAME}_PYTHON_CASADI_TESTS}) diff --git a/unittest/python/casadi/bindings_main_algo.py b/unittest/python/casadi/bindings_main_algo.py new file mode 100644 index 0000000000..ac725fea7a --- /dev/null +++ b/unittest/python/casadi/bindings_main_algo.py @@ -0,0 +1,134 @@ +import unittest +import sys, os +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +from test_case import PinocchioTestCase as TestCase + +import pinocchio as pin +from pinocchio import casadi as cpin + +import casadi +from casadi import SX +import numpy as np + +class TestMainAlgos(TestCase): + + def setUp(self): + self.model = pin.buildSampleModelHumanoidRandom() + self.data = self.model.createData() + + self.cmodel = cpin.Model(self.model) + self.cdata = self.cmodel.createData() + + qmax = np.full((self.model.nq,1),np.pi) + self.q = pin.randomConfiguration(self.model,-qmax,qmax) + self.v = np.random.rand(self.model.nv) + self.a = np.random.rand(self.model.nv) + self.tau = np.random.rand(self.model.nv) + + self.cq = SX.sym("q",self.cmodel.nq,1) + self.cv = SX.sym("v",self.cmodel.nv,1) + self.ca = SX.sym("a",self.cmodel.nv,1) + self.ctau = SX.sym("tau",self.cmodel.nv,1) + + self.fext = [] + self.cfext = [] + for i in range(self.model.njoints): + self.fext.append(pin.Force.Random()) + fname = "f" + str(i) + self.cfext.append(cpin.Force(SX.sym(fname,6,1))) + + self.fext[0].setZero() + + def test_crba(self): + model = self.model + data = self.data + + cmodel = self.cmodel + cdata = self.cdata + + cM = cpin.crba(cmodel,cdata,self.cq) + f_crba = casadi.Function("crba",[self.cq],[cM]) + + arg_crba = [list(self.q)] + M = f_crba.call(arg_crba)[0].full() + + M_ref = pin.crba(model,data,self.q) + + self.assertApprox(M,M_ref) + + def test_rnea(self): + model = self.model + data = self.data + + cmodel = self.cmodel + cdata = self.cdata + + ctau = cpin.rnea(cmodel,cdata,self.cq,self.cv,self.ca) + f_rnea = casadi.Function("rnea",[self.cq,self.cv,self.ca],[ctau]) + + arg_rnea = [list(self.q),list(self.v),list(self.a)] + tau = f_rnea.call(arg_rnea)[0].full() + + tau_ref = pin.rnea(model,data,self.q,self.v,self.a) + + self.assertApprox(tau,tau_ref) + + ctau_fext = cpin.rnea(cmodel,cdata,self.cq,self.cv,self.ca,self.cfext) + carg_in = [self.cq,self.cv,self.ca] + for f in self.cfext: + carg_in += [f.vector] + f_rnea_fext = casadi.Function("rnea_fext",carg_in,[ctau_fext]) + + arg_rnea_fext = arg_rnea + for f in self.fext: + arg_rnea_fext += [f.vector] + tau_fext = f_rnea_fext.call(arg_rnea_fext)[0].full() + + tau_fext_ref = pin.rnea(model,data,self.q,self.v,self.a,self.fext) + + self.assertApprox(tau_fext,tau_fext_ref) + + def test_aba(self): + model = self.model + data = self.data + + cmodel = self.cmodel + cdata = self.cdata + + cddq = cpin.aba(cmodel,cdata,self.cq,self.cv,self.ctau) + f_aba = casadi.Function("aba",[self.cq,self.cv,self.ctau],[cddq]) + + arg_aba = [list(self.q),list(self.v),list(self.tau)] + a = f_aba.call(arg_aba)[0].full() + + a_ref = pin.aba(model,data,self.q,self.v,self.tau) + + self.assertApprox(a,a_ref) + + cddq_fext = cpin.aba(cmodel,cdata,self.cq,self.cv,self.ctau,self.cfext) + carg_in = [self.cq,self.cv,self.ctau] + for f in self.cfext: + carg_in += [f.vector] + f_aba_fext = casadi.Function("aba_fext",carg_in,[cddq_fext]) + + arg_aba_fext = arg_aba + for f in self.fext: + arg_aba_fext += [f.vector] + a_fext = f_aba_fext.call(arg_aba_fext)[0].full() + + a_fext_ref = pin.aba(model,data,self.q,self.v,self.tau,self.fext) + + self.assertApprox(a_fext,a_fext_ref) + + + def test_computeMinverse(self): + model = self.model + Minv = pin.computeMinverse(model,self.data,self.q) + + data2 = model.createData() + M = pin.crba(model,data2,self.q) + + self.assertApprox(np.linalg.inv(M),Minv) + +if __name__ == '__main__': + unittest.main() diff --git a/unittest/rnea-derivatives.cpp b/unittest/rnea-derivatives.cpp index 5dd4c024d5..3db37844fc 100644 --- a/unittest/rnea-derivatives.cpp +++ b/unittest/rnea-derivatives.cpp @@ -36,7 +36,7 @@ BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives) VectorXd v(VectorXd::Zero(model.nv)); VectorXd a(VectorXd::Zero(model.nv)); - /// Check againt non-derivative algo + // Check againt non-derivative algo MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero(); computeGeneralizedGravityDerivatives(model,data,q,g_partial_dq); @@ -87,9 +87,6 @@ BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives_fext) VectorXd tau0 = computeStaticTorque(model,data_fd,q,fext); BOOST_CHECK(data.tau.isApprox(tau0)); - - std::cout << "data.tau: " << data.tau.transpose() << std::endl; - std::cout << "tau0: " << tau0.transpose() << std::endl; MatrixXd static_vec_partial_dq_fd(model.nv,model.nv); @@ -117,6 +114,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives) Model model; buildModels::humanoidRandom(model); + model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv); Data data(model), data_fd(model), data_ref(model); @@ -280,11 +278,11 @@ BOOST_AUTO_TEST_CASE(test_rnea_derivatives) computeJointJacobiansTimeVariation(model,data_ref,q,v); BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); crba(model,data_ref,q); + data_ref.M.triangularView() + = data_ref.M.transpose().triangularView(); rnea_partial_da.triangularView() = rnea_partial_da.transpose().triangularView(); - data_ref.M.triangularView() - = data_ref.M.transpose().triangularView(); BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M)); BOOST_CHECK(data.tau.isApprox(tau0)); @@ -496,7 +494,7 @@ BOOST_AUTO_TEST_CASE(test_get_coriolis) BOOST_CHECK(data.J.isApprox(data_ref.J)); BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); - BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdv)); + BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); for(JointIndex k = 1; k < model.joints.size(); ++k) { BOOST_CHECK(data.vxI[k].isApprox(data_ref.vxI[k])); diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index 1d9c894654..401498d181 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -168,6 +168,35 @@ BOOST_AUTO_TEST_CASE (test_rnea_with_fext) BOOST_CHECK(tau_ref.isApprox(data_rnea_fext.tau)); } + +BOOST_AUTO_TEST_CASE(test_rnea_with_armature) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoidRandom(model); + model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill( 1.); + + Data data(model); + Data data_ref(model); + + VectorXd q = randomConfiguration(model); + VectorXd v(VectorXd::Random(model.nv)); + VectorXd a(VectorXd::Random(model.nv)); + + crba(model,data_ref,q); + data_ref.M.triangularView() = data_ref.M.transpose().triangularView(); + const VectorXd nle = nonLinearEffects(model,data_ref,q,v); + + const VectorXd tau_ref = data_ref.M * a + nle; + + rnea(model,data,q,v,a); + BOOST_CHECK(tau_ref.isApprox(data.tau)); +} BOOST_AUTO_TEST_CASE(test_compute_gravity) { diff --git a/unittest/rotation.cpp b/unittest/rotation.cpp index 10ae784911..0a082bb1f4 100644 --- a/unittest/rotation.cpp +++ b/unittest/rotation.cpp @@ -18,26 +18,37 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_toRotationMatrix) { using namespace pinocchio; - const int max_tests = 1e5; + #ifndef NDEBUG + const int max_tests = 1e5; + #else + const int max_tests = 1e2; + #endif for(int k = 0; k < max_tests; ++k) { const double theta = std::rand(); double cos_value, sin_value; pinocchio::SINCOS(theta,&sin_value,&cos_value); - Eigen::Vector3d axis(Eigen::Vector3d::Random().normalized()); + const Eigen::Vector3d axis(Eigen::Vector3d::Random().normalized()); Eigen::Matrix3d rot; toRotationMatrix(axis,cos_value,sin_value,rot); - Eigen::Matrix3d rot_ref = Eigen::AngleAxisd(theta,axis).toRotationMatrix(); + Eigen::Matrix3d rot2; toRotationMatrix(axis,theta,rot2); + + const Eigen::Matrix3d rot_ref = Eigen::AngleAxisd(theta,axis).toRotationMatrix(); BOOST_CHECK(rot.isApprox(rot_ref)); + BOOST_CHECK(rot2.isApprox(rot_ref)); } } BOOST_AUTO_TEST_CASE(test_orthogonal_projection) { using namespace pinocchio; +#ifndef NDEBUG const int max_tests = 1e5; +#else + const int max_tests = 1e2; +#endif typedef Eigen::Vector4d Vector4; typedef Eigen::Quaterniond Quaternion; @@ -65,5 +76,3 @@ BOOST_AUTO_TEST_CASE(test_orthogonal_projection) } BOOST_AUTO_TEST_SUITE_END() - - diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 682cc13c48..3ccd1995a5 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -545,7 +545,7 @@ struct TestJointData jmodel.calc(jdata,q_random,v_random); pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); - jmodel.calc_aba(jdata,I,false); + jmodel.calc_aba(jdata,Eigen::VectorXd::Zero(jmodel.nv()),I,false); test(jdata); } @@ -572,7 +572,7 @@ struct TestJointData jmodel.calc(jdata,q_random,v_random); pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); - jmodel.calc_aba(jdata,I,false); + jmodel.calc_aba(jdata,Eigen::VectorXd::Zero(jmodel.nv()),I,false); test(jdata); } @@ -599,7 +599,7 @@ struct TestJointData jmodel_mimic.calc(jdata_mimic,q_random,v_random); pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); - jmodel_mimic.calc_aba(jdata_mimic,I,false); + jmodel_mimic.calc_aba(jdata_mimic,Eigen::VectorXd::Zero(jmodel.nv()),I,false); test(jdata_mimic); } diff --git a/unittest/spatial.cpp b/unittest/spatial.cpp index 0da7ce1946..e7a1e8b32b 100644 --- a/unittest/spatial.cpp +++ b/unittest/spatial.cpp @@ -89,6 +89,9 @@ BOOST_AUTO_TEST_CASE ( test_SE3 ) SE3f amb_float = amb.cast(); BOOST_CHECK(amb_float.isApprox(amb.cast())); + SE3f amb_float2(amb); + BOOST_CHECK(amb_float.isApprox(amb_float2)); + // Test actInv const SE3 M = SE3::Random(); const SE3 Minv = M.inverse(); @@ -258,6 +261,9 @@ BOOST_AUTO_TEST_CASE ( test_Motion ) Motion a(Motion::Random()); Motionf a_float = a.cast(); BOOST_CHECK(a_float.isApprox(a.cast())); + + Motionf a_float2(a); + BOOST_CHECK(a_float.isApprox(a_float2)); } } @@ -430,6 +436,9 @@ BOOST_AUTO_TEST_CASE ( test_Force ) Force a(Force::Random()); Forcef a_float = a.cast(); BOOST_CHECK(a_float.isApprox(a.cast())); + + Forcef a_float2(a); + BOOST_CHECK(a_float.isApprox(a_float2)); } // Test scalar multiplication @@ -694,8 +703,13 @@ BOOST_AUTO_TEST_CASE(cast_inertia) using namespace pinocchio; Inertia Y(Inertia::Random()); + typedef InertiaTpl Inertiald; + BOOST_CHECK(Y.cast() == Y); BOOST_CHECK(Y.cast().cast() == Y); + + Inertiald Y2(Y); + BOOST_CHECK(Y2.isApprox(Y.cast())); } BOOST_AUTO_TEST_CASE ( test_ActOnSet ) @@ -956,16 +970,21 @@ BOOST_AUTO_TEST_CASE(test_cartesian_axis) const double alpha = 3; Vector3d v2(alpha*v); - BOOST_CHECK(AxisX::cross(v).isApprox(Vector3d::Unit(0).cross(v))); - BOOST_CHECK(AxisY::cross(v).isApprox(Vector3d::Unit(1).cross(v))); - BOOST_CHECK(AxisZ::cross(v).isApprox(Vector3d::Unit(2).cross(v))); - BOOST_CHECK(AxisX::alphaCross(alpha,v).isApprox(Vector3d::Unit(0).cross(v2))); - BOOST_CHECK(AxisY::alphaCross(alpha,v).isApprox(Vector3d::Unit(1).cross(v2))); - BOOST_CHECK(AxisZ::alphaCross(alpha,v).isApprox(Vector3d::Unit(2).cross(v2))); + BOOST_CHECK(XAxis::cross(v).isApprox(Vector3d::Unit(0).cross(v))); + BOOST_CHECK(YAxis::cross(v).isApprox(Vector3d::Unit(1).cross(v))); + BOOST_CHECK(ZAxis::cross(v).isApprox(Vector3d::Unit(2).cross(v))); + BOOST_CHECK(XAxis::alphaCross(alpha,v).isApprox(Vector3d::Unit(0).cross(v2))); + BOOST_CHECK(YAxis::alphaCross(alpha,v).isApprox(Vector3d::Unit(1).cross(v2))); + BOOST_CHECK(ZAxis::alphaCross(alpha,v).isApprox(Vector3d::Unit(2).cross(v2))); test_scalar_multiplication_cartesian_axis<0>::run(); test_scalar_multiplication_cartesian_axis<1>::run(); test_scalar_multiplication_cartesian_axis<2>::run(); + + // test Vector value + BOOST_CHECK(XAxis::vector() == Vector3d::UnitX()); + BOOST_CHECK(YAxis::vector() == Vector3d::UnitY()); + BOOST_CHECK(ZAxis::vector() == Vector3d::UnitZ()); } template diff --git a/unittest/srdf.cpp b/unittest/srdf.cpp index f10cda0c43..1e747b8ab5 100644 --- a/unittest/srdf.cpp +++ b/unittest/srdf.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2016-2021 CNRS INRIA +// Copyright (c) 2016-2018 CNRS // #include diff --git a/unittest/symmetric.cpp b/unittest/symmetric.cpp index b0e4b365e6..c9db05596f 100644 --- a/unittest/symmetric.cpp +++ b/unittest/symmetric.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2015-2016,2018 CNRS +// Copyright (c) 2015-2020 CNRS INRIA // /* --- Unitary test symmetric.cpp This code tests and compares two ways of @@ -130,6 +130,15 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 ) { BOOST_CHECK(Symmetric3::Identity().matrix().isApprox(Matrix3::Identity(), 1e-12)); } + + // Set diagonal + { + Symmetric3 S0 = Symmetric3::Zero(); + const Symmetric3::Vector3 diag_elt = (Symmetric3::Vector3::Constant(1.) + Symmetric3::Vector3::Random()); + S0.setDiagonal(diag_elt); + + BOOST_CHECK(S0.matrix().diagonal().isApprox(diag_elt)); + } // Skew2 { diff --git a/utils/CMakeLists.txt b/utils/CMakeLists.txt index acee0419e4..f2e24633fd 100644 --- a/utils/CMakeLists.txt +++ b/utils/CMakeLists.txt @@ -39,3 +39,4 @@ IF(urdfdom_FOUND) TARGET_COMPILE_DEFINITIONS(pinocchio_read_model PRIVATE -DNOMINMAX) ENDIF(WIN32) ENDIF(urdfdom_FOUND) +