Commit 44a768d5 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[pinocchio3] commit for Preview of Pinocchio3

parent b134b25f
Pipeline #15202 failed with stage
in 101 minutes and 38 seconds
......@@ -2,6 +2,9 @@ name: Build Pinocchio for Windows (CLANG) via Conda
on:
pull_request:
push:
branches:
- master
- devel
jobs:
build:
......
......@@ -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
......
......@@ -4,4 +4,4 @@ Xcode*
*.pyc
coverage*
.travis
.vscode*
.vscode*
\ No newline at end of file
......@@ -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")
......
......@@ -31,6 +31,39 @@ If you want to directly dive into **Pinocchio**, only one single line is suffici
conda install pinocchio -c conda-forge
</p>
## 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:
......
......@@ -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)
......@@ -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<double> rnea_code_gen(model);
......@@ -87,7 +105,11 @@ int main(int argc, const char ** argv)
CodeGenABADerivatives<double> aba_derivatives_code_gen(model);
aba_derivatives_code_gen.initLib();
aba_derivatives_code_gen.loadLib();
CodeGenConstraintDynamicsDerivatives<double> 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<VectorXd> qs (NBT);
pinocchio::container::aligned_vector<VectorXd> qdots (NBT);
pinocchio::container::aligned_vector<VectorXd> 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;
}
//
// 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 <iostream>
#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<NBT;++i)
{
qs[i] = randomConfiguration(model,-qmax,qmax);
qdots[i] = Eigen::VectorXd::Random(model.nv);
qddots[i] = Eigen::VectorXd::Random(model.nv);
taus[i] = Eigen::VectorXd::Random(model.nv);
}
timer.tic();
SMOOTH(NBT)
{
computeABADerivatives(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
}
std::cout << "ABA derivatives= \t\t\t"; timer.toc(std::cout,NBT);
double total_time = 0;
initConstraintDynamics(model,data,contact_models_empty);
SMOOTH(NBT)
{
constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_empty,contact_data_empty);
timer.tic();
computeConstraintDynamicsDerivatives(model, data,
contact_models_empty, contact_data_empty);
total_time += timer.toc(timer.DEFAULT_UNIT);
}
std::cout << "constraintDynamicsDerivs {} = \t\t" << (total_time/NBT)<<std::endl;
total_time = 0;
initConstraintDynamics(model,data,contact_models_6D);
SMOOTH(NBT)
{
constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_6D,contact_data_6D);
timer.tic();
computeConstraintDynamicsDerivatives(model,data,contact_models_6D,contact_data_6D);
total_time += timer.toc(timer.DEFAULT_UNIT);
}
std::cout << "constraintDynamicsDerivs {6D} = \t\t" << (total_time/NBT)<<std::endl;
total_time = 0;
initConstraintDynamics(model,data,contact_models_6D6D);
SMOOTH(NBT)
{
constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_6D6D,contact_data_6D6D);
timer.tic();
computeConstraintDynamicsDerivatives(model,data, contact_models_6D6D, contact_data_6D6D);
total_time += timer.toc(timer.DEFAULT_UNIT);
}
std::cout << "constraintDynamicsDerivs {6D,6D} = \t" << (total_time/NBT)<<std::endl;
return 0;
}
//
// Copyright (c) 2019-2020 INRIA
//
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/contact-dynamics.hpp"
#include "pinocchio/algorithm/constrained-dynamics.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include <iostream>
#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<NBT;++i)
{
qs[i] = randomConfiguration(model,-qmax,qmax);
qdots[i] = Eigen::VectorXd::Random(model.nv);
qddots[i] = Eigen::VectorXd::Random(model.nv);
taus[i] = Eigen::VectorXd::Random(model.nv);
}
timer.tic();
SMOOTH(NBT)
{
aba(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
}
std::cout << "ABA = \t\t"; timer.toc(std::cout,NBT);
double total_time = 0;
SMOOTH(NBT)
{
crba(model,data,qs[_smooth]);
timer.tic();
cholesky::decompose(model,data);
total_time += timer.toc(timer.DEFAULT_UNIT);
}
std::cout << "Sparse Cholesky = \t\t" << (total_time/NBT)
<< " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
total_time = 0;
SMOOTH(NBT)
{
computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
timer.tic();
contact_chol_empty.compute(model,data,contact_models_empty,contact_data_empty);
total_time += timer.toc(timer.DEFAULT_UNIT);
}
std::cout << "contactCholesky {} = \t\t" << (total_time/NBT)
<< " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
total_time = 0;
MatrixXd H_inverse(contact_chol_empty.size(),contact_chol_empty.size());
SMOOTH(NBT)
{
computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
contact_chol_empty.compute(model,data,contact_models_empty,contact_data_empty);
timer.tic();
contact_chol_empty.inverse(H_inverse);
total_time += timer.toc(timer.DEFAULT_UNIT);
}
std::cout << "contactCholeskyInverse {} = \t\t" << (total_time/NBT)
<< " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
initConstraintDynamics(model,data,contact_models_empty);
timer.tic();
SMOOTH(NBT)
{
constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_empty,contact_data_empty);
}
std::cout << "constraintDynamics {} = \t\t"; timer.toc(std::cout,NBT);
total_time = 0;
SMOOTH(NBT)
{
computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
timer.tic();
contact_chol_6D.compute(model,data,contact_models_6D,contact_data_6D);
total_time += timer.toc(timer.DEFAULT_UNIT);
}
std::cout << "contactCholesky {6D} = \t\t" << (total_time/NBT)
<< " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
total_time = 0;
H_inverse.resize(contact_chol_6D.size(),contact_chol_6D.size());
SMOOTH(NBT)
{
computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
contact_chol_6D.compute(model,data,contact_models_6D,contact_data_6D);
timer.tic();
contact_chol_6D.inverse(H_inverse);
total_time += timer.toc(timer.DEFAULT_UNIT);
}
std::cout << "contactCholeskyInverse {6D} = \t\t" << (total_time/NBT)
<< " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
MatrixXd J(contact_chol_6D.constraintDim(),model.nv);
J.setZero();
MatrixXd MJtJ_inv(model.nv+contact_chol_6D.constraintDim(),
model.nv+contact_chol_6D.constraintDim());
MJtJ_inv.setZero();