Unverified Commit f3e0aecc authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1420 from jcarpent/topic/collision

Enhance collision support + serialization + parallelization
parents cfc2e4bc fd54daec
Pipeline #13968 passed with stage
in 256 minutes and 15 seconds
......@@ -47,6 +47,7 @@ jobs:
echo $APT_DEPENDENCIES
sudo apt-get update -qq
sudo apt-get install -qq curl cppcheck ${APT_DEPENDENCIES}
sudo apt install libomp-dev libomp5
- name: Free disk space
run: |
sudo apt clean
......@@ -72,7 +73,7 @@ jobs:
export MAKEFLAGS="-j1"
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Debug -DBUILD_WITH_COLLISION_SUPPORT=ON -DBUILD_ADVANCED_TESTING=ON -DBUILD_WITH_CASADI_SUPPORT=ON -DPYTHON_EXECUTABLE=$(which python3)
cmake .. -DCMAKE_BUILD_TYPE=Debug -DBUILD_WITH_COLLISION_SUPPORT=ON -DBUILD_ADVANCED_TESTING=ON -DBUILD_WITH_CASADI_SUPPORT=ON -DPYTHON_EXECUTABLE=$(which python3) -DBUILD_WITH_OPENMP_SUPPORT=ON
make
make build_tests
export CTEST_OUTPUT_ON_FAILURE=1
......
......@@ -2,9 +2,6 @@ name: Build Pinocchio for Windows (CLANG) via Conda
on:
pull_request:
push:
branches:
- master
- devel
jobs:
build:
......
......@@ -2,9 +2,6 @@ name: Build Pinocchio for Windows (v142) via Conda
on:
pull_request:
push:
branches:
- master
- devel
jobs:
build:
......@@ -33,6 +30,10 @@ jobs:
- name: Install cmake and update conda
run: |
conda install cmake -c main
- name: Display the path
run: echo %cd%
shell: cmd
- name: Build Pinocchio
shell: cmd /C CALL {0}
......@@ -43,7 +44,6 @@ 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
......@@ -61,14 +61,31 @@ jobs:
-DBUILD_WITH_URDF_SUPPORT=ON ^
-DBUILD_PYTHON_INTERFACE=OFF ^
-DBUILD_WITH_COLLISION_SUPPORT=ON ^
-DBUILD_TESTING=ON ^
..
:: Build
cmake --build . --config Release --target install
# - name: Start SSH session
# uses: luchihoratiu/debug-via-ssh@main
# with:
# NGROK_AUTH_TOKEN: ${{ secrets.NGROK_AUTH_TOKEN }}
# SSH_PASS: ${{ secrets.SSH_PASS }}
#
- name: Testing
shell: cmd /C CALL {0}
env:
ACTIONS_ALLOW_UNSECURE_COMMANDS: 'true'
run: |
:: Testing
pushd build
set PATH=%PATH%;%CONDA_PREFIX%\Lib\site-packages\pinocchio
ctest --output-on-failure -C Release -V
set PATH=%PATH%;%CONDA_PREFIX%\Library\Lib
set PATH=%PATH%;%CONDA_PREFIX%\Library\bin
.\unittest\Release\test-cpp-urdf.exe
.\examples\Release\example-cpp-geometry-models.exe
ctest --output-on-failure -C Release -V --repeat until-pass:2
# :: Test Python import
# cd ..
......
......@@ -5,5 +5,5 @@
path = .travis
url = https://github.com/jrl-umi3218/jrl-travis
[submodule "models/others"]
path = models/others
path = models/example-robot-data
url = https://github.com/Gepetto/example-robot-data.git
#
# Copyright (c) 2015-2020 CNRS INRIA
# Copyright (c) 2015-2021 CNRS INRIA
# Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
#
......@@ -32,6 +32,10 @@ INCLUDE(${CMAKE_CURRENT_LIST_DIR}/cmake/boost.cmake)
INCLUDE(${CMAKE_CURRENT_LIST_DIR}/cmake/python.cmake)
INCLUDE(${CMAKE_CURRENT_LIST_DIR}/cmake/ide.cmake)
INCLUDE(${CMAKE_CURRENT_LIST_DIR}/cmake/apple.cmake)
IF(APPLE) # Use the handmade approach
SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake/find-external/OpenMP ${CMAKE_MODULE_PATH})
ENDIF(APPLE)
INCLUDE(CMakeDependentOption)
# If needed, set CMake policy for APPLE systems
APPLY_DEFAULT_APPLE_CONFIGURATION()
......@@ -63,6 +67,8 @@ OPTION(BUILD_WITH_COLLISION_SUPPORT "Build the library with the collision suppor
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)
OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the support of code generation (via CppADCodeGen)" OFF)
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)
......@@ -123,6 +129,10 @@ IF(BUILD_WITH_CASADI_SUPPORT)
ADD_PROJECT_DEPENDENCY(casadi 3.4.5 REQUIRED PKG_CONFIG_REQUIRES "casadi >= 3.4.5")
ENDIF(BUILD_WITH_CASADI_SUPPORT)
IF(BUILD_WITH_OPENMP_SUPPORT)
FIND_PACKAGE(OpenMP REQUIRED)
ENDIF(BUILD_WITH_OPENMP_SUPPORT)
SET(BOOST_REQUIRED_COMPONENTS filesystem serialization system)
SET_BOOST_DEFAULT_OPTIONS()
......@@ -133,7 +143,7 @@ IF(BUILD_PYTHON_INTERFACE)
MESSAGE(STATUS "The Python bindings of Pinocchio will be compiled along the main library. If you want to disable this feature, please set the option BUILD_PYTHON_INTERFACE to OFF.")
FINDPYTHON(REQUIRED)
SEARCH_FOR_BOOST_PYTHON(REQUIRED)
ADD_PROJECT_DEPENDENCY(eigenpy 2.5.0 REQUIRED)
ADD_PROJECT_DEPENDENCY(eigenpy 2.6.2 REQUIRED)
ELSE(BUILD_PYTHON_INTERFACE)
MESSAGE(STATUS "Pinocchio won't be compiled with its Python bindings. If you want to enable this feature, please set the option BUILD_PYTHON_INTERFACE to ON.")
ENDIF(BUILD_PYTHON_INTERFACE)
......@@ -141,7 +151,7 @@ ENDIF(BUILD_PYTHON_INTERFACE)
IF(BUILD_WITH_HPP_FCL_SUPPORT)
ADD_DEFINITIONS(-DPINOCCHIO_WITH_HPP_FCL)
LIST(APPEND CFLAGS_DEPENDENCIES "-DPINOCCHIO_WITH_HPP_FCL")
ADD_PROJECT_DEPENDENCY(hpp-fcl 1.4.0 REQUIRED PKG_CONFIG_REQUIRES "hpp-fcl >= 1.4.0")
ADD_PROJECT_DEPENDENCY(hpp-fcl 1.7.1 REQUIRED PKG_CONFIG_REQUIRES "hpp-fcl >= 1.7.1")
# Check whether hpp-fcl python bindings are available.
SET(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS FALSE)
IF(BUILD_PYTHON_INTERFACE)
......@@ -203,12 +213,14 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/liegroup")
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/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/container")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/codegen")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/autodiff")
......
#
# Copyright (c) 2015-2020 CNRS INRIA
# Copyright (c) 2015-2021 CNRS INRIA
#
# ----------------------------------------------------
......@@ -51,6 +51,11 @@ IF(CPPADCG_FOUND)
TARGET_LINK_LIBRARIES(timings-cg PUBLIC ${CMAKE_DL_LIBS})
ENDIF(CPPADCG_FOUND)
IF(BUILD_WITH_OPENMP_SUPPORT)
ADD_BENCH(timings-parallel TRUE)
TARGET_LINK_LIBRARIES(timings-parallel PRIVATE OpenMP::OpenMP_CXX)
ENDIF(BUILD_WITH_OPENMP_SUPPORT)
# timings
#
ADD_BENCH(timings-cholesky TRUE)
......
//
// Copyright (c) 2021 INRIA
//
#include "pinocchio/algorithm/parallel/rnea.hpp"
#include "pinocchio/algorithm/parallel/aba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#ifdef PINOCCHIO_WITH_HPP_FCL
#include "pinocchio/algorithm/parallel/geometry.hpp"
#endif
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include <iostream>
#include "pinocchio/utils/timer.hpp"
#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
int main(int /*argc*/, const char ** /*argv*/)
{
using namespace Eigen;
using namespace pinocchio;
typedef Eigen::Matrix<bool,Eigen::Dynamic,1> VectorXb;
// typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic> MatrixXb;
PinocchioTicToc timer(PinocchioTicToc::US);
#ifdef NDEBUG
const int NBT = 4000;
#else
const int NBT = 1;
std::cout << "(the time score in debug mode is not relevant) " << std::endl;
#endif
const int BATCH_SIZE = 256;
const int NUM_THREADS = omp_get_max_threads();
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");
pinocchio::Model model;
pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
std::cout << "nq = " << model.nq << std::endl;
std::cout << "nv = " << model.nv << std::endl;
std::cout << "name = " << model.name << std::endl;
#ifdef PINOCCHIO_WITH_HPP_FCL
const std::string package_path = PINOCCHIO_MODEL_DIR;
hpp::fcl::MeshLoaderPtr mesh_loader = boost::make_shared<hpp::fcl::CachedMeshLoader>();
const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
std::vector<std::string> package_paths(1,package_path);
pinocchio::GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,COLLISION,geometry_model,package_paths,mesh_loader);
geometry_model.addAllCollisionPairs();
pinocchio::srdf::removeCollisionPairs(model,geometry_model,srdf_filename,false);
GeometryData geometry_data(geometry_model);
{
int num_active_collision_pairs = 0;
for(size_t k = 0; k < geometry_data.activeCollisionPairs.size(); ++k)
{
if(geometry_data.activeCollisionPairs[k])
num_active_collision_pairs++;
}
std::cout << "active collision pairs = " << num_active_collision_pairs << std::endl;
}
#endif
std::cout << "--" << std::endl;
std::cout << "NUM_THREADS: " << NUM_THREADS << std::endl;
std::cout << "BATCH_SIZE: " << BATCH_SIZE << std::endl;
std::cout << "--" << std::endl;
pinocchio::Data data(model);
const VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
MatrixXd qs(model.nq,BATCH_SIZE);
MatrixXd vs(model.nv,BATCH_SIZE);
MatrixXd as(model.nv,BATCH_SIZE);
MatrixXd taus(model.nv,BATCH_SIZE);
MatrixXd res(model.nv,BATCH_SIZE);
// VectorXb cols()
PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) q_vec(NBT);
for(size_t i=0; i < NBT; ++i)
{
q_vec[i] = randomConfiguration(model,-qmax,qmax);
}
for(Eigen::DenseIndex i=0; i < BATCH_SIZE; ++i)
{
qs.col(i) = randomConfiguration(model,-qmax,qmax);
vs.col(i) = Eigen::VectorXd::Random(model.nv);
as.col(i) = Eigen::VectorXd::Random(model.nv);
taus.col(i) = Eigen::VectorXd::Random(model.nv);
}
ModelPool pool(model,NUM_THREADS);
timer.tic();
SMOOTH(NBT)
{
for(Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
res.col(i) = rnea(model,data,qs.col(i),vs.col(i),as.col(i));
}
std::cout << "mean RNEA = \t\t\t\t"; timer.toc(std::cout,NBT*BATCH_SIZE);
for(int num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
{
timer.tic();
SMOOTH(NBT)
{
rnea(num_threads,pool,qs,vs,as,res);
}
double elapsed = timer.toc()/(NBT*BATCH_SIZE);
std::stringstream ss;
ss << "mean RNEA pool (";
ss << num_threads;
ss << " threads) = \t\t";
ss << elapsed << " us" << std::endl;
std::cout << ss.str();
}
std::cout << "--" << std::endl;
timer.tic();
SMOOTH(NBT)
{
for(Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
res.col(i) = aba(model,data,qs.col(i),vs.col(i),taus.col(i));
}
std::cout << "mean ABA = \t\t\t\t"; timer.toc(std::cout,NBT*BATCH_SIZE);
for(int num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
{
timer.tic();
SMOOTH(NBT)
{
aba(num_threads,pool,qs,vs,taus,res);
}
double elapsed = timer.toc()/(NBT*BATCH_SIZE);
std::stringstream ss;
ss << "mean ABA pool (";
ss << num_threads;
ss << " threads) = \t\t";
ss << elapsed << " us" << std::endl;
std::cout << ss.str();
}
#ifdef PINOCCHIO_WITH_HPP_FCL
std::cout << "--" << std::endl;
const int NBT_COLLISION = math::max(NBT,1);
timer.tic();
SMOOTH((size_t)NBT_COLLISION)
{
computeCollisions(model,data,geometry_model,geometry_data,q_vec[_smooth]);
}
std::cout << "non parallel collision = \t\t\t"; timer.toc(std::cout,NBT_COLLISION);
for(int num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
{
timer.tic();
SMOOTH((size_t)NBT_COLLISION)
{
computeCollisions(num_threads,model,data,geometry_model,geometry_data,q_vec[_smooth]);
}
double elapsed = timer.toc()/(NBT_COLLISION);
std::stringstream ss;
ss << "parallel collision (";
ss << num_threads;
ss << " threads) = \t\t";
ss << elapsed << " us" << std::endl;
std::cout << ss.str();
}
std::cout << "--" << std::endl;
GeometryPool geometry_pool(model,geometry_model,NUM_THREADS);
VectorXb collision_res(BATCH_SIZE);
collision_res.fill(false);
timer.tic();
SMOOTH((size_t)(NBT_COLLISION/BATCH_SIZE))
{
for(Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
computeCollisions(model,data,geometry_model,geometry_data,qs.col(i));
}
std::cout << "non parallel collision = \t\t\t"; timer.toc(std::cout,NBT_COLLISION);
for(int num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
{
timer.tic();
SMOOTH((size_t)(NBT_COLLISION/BATCH_SIZE))
{
computeCollisions(num_threads,geometry_pool,qs,collision_res);
}
double elapsed = timer.toc()/(NBT_COLLISION);
std::stringstream ss;
ss << "pool parallel collision (";
ss << num_threads;
ss << " threads) = \t\t";
ss << elapsed << " us" << std::endl;
std::cout << ss.str();
}
#endif
std::cout << "--" << std::endl;
return 0;
}
#
# Copyright (c) 2015-2020 CNRS INRIA
# Copyright (c) 2015-2021 CNRS INRIA
# Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
#
......@@ -27,6 +27,15 @@ FUNCTION(REMOVE_PATH_FROM_LIST list_name path_name dest_list)
SET(${dest_list} ${list_name_} PARENT_SCOPE)
ENDFUNCTION(REMOVE_PATH_FROM_LIST)
FUNCTION(LIST_FILTER list regular_expression dest_list)
FOREACH(elt ${list})
IF(${elt} MATCHES ${regular_expression})
LIST(REMOVE_ITEM list ${elt})
ENDIF()
ENDFOREACH(elt ${list})
SET(${dest_list} ${list} PARENT_SCOPE)
ENDFUNCTION(LIST_FILTER)
# --- PYTHON TARGET --- #
SET(PYWRAP ${PROJECT_NAME}_pywrap)
SET(PYWRAP ${PYWRAP} PARENT_SCOPE)
......@@ -64,6 +73,24 @@ IF(NOT BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
)
ENDIF(NOT BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
IF(NOT BUILD_WITH_OPENMP_SUPPORT)
LIST_FILTER("${${PROJECT_NAME}_PYTHON_HEADERS}" "^multibody/pool" ${PROJECT_NAME}_PYTHON_HEADERS)
LIST_FILTER("${${PROJECT_NAME}_PYTHON_SOURCES}" "^multibody/pool" ${PROJECT_NAME}_PYTHON_SOURCES)
LIST_FILTER("${${PROJECT_NAME}_PYTHON_HEADERS}" "^algorithm/parallel" ${PROJECT_NAME}_PYTHON_HEADERS)
LIST_FILTER("${${PROJECT_NAME}_PYTHON_SOURCES}" "^algorithm/parallel" ${PROJECT_NAME}_PYTHON_SOURCES)
#LIST(FILTER ${PROJECT_NAME}_PYTHON_HEADERS EXCLUDE REGEX "^multibody/pool")
#LIST(FILTER ${PROJECT_NAME}_PYTHON_SOURCES EXCLUDE REGEX "^multibody/pool")
#LIST(FILTER ${PROJECT_NAME}_PYTHON_HEADERS EXCLUDE REGEX "^algorithm/parallel")
#LIST(FILTER ${PROJECT_NAME}_PYTHON_SOURCES EXCLUDE REGEX "^algorithm/parallel")
ELSE(NOT BUILD_WITH_OPENMP_SUPPORT)
IF(NOT BUILD_WITH_HPP_FCL_SUPPORT)
LIST_FILTER("${${PROJECT_NAME}_PYTHON_HEADERS}" "^multibody/pool/geometry.hpp" ${PROJECT_NAME}_PYTHON_HEADERS)
LIST_FILTER("${${PROJECT_NAME}_PYTHON_SOURCES}" "^algorithm/parallel/geometry.cpp" ${PROJECT_NAME}_PYTHON_SOURCES)
#LIST(FILTER ${PROJECT_NAME}_PYTHON_HEADERS EXCLUDE REGEX "^multibody/pool/geometry.hpp")
#LIST(FILTER ${PROJECT_NAME}_PYTHON_SOURCES EXCLUDE REGEX "^algorithm/parallel/geometry.cpp")
ENDIF(NOT BUILD_WITH_HPP_FCL_SUPPORT)
ENDIF(NOT BUILD_WITH_OPENMP_SUPPORT)
LIST(APPEND HEADERS ${${PROJECT_NAME}_PYTHON_HEADERS})
# Headers of the Python bindings
......@@ -71,12 +98,16 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python"
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/joint")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/pool")
IF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/fcl")
ENDIF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/parsers")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/serialization")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/algorithm")
IF(BUILD_WITH_OPENMP_SUPPORT)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/algorithm/parallel")
ENDIF(BUILD_WITH_OPENMP_SUPPORT)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/utils")
SYMLINK_AND_INSTALL_HEADERS("${${PROJECT_NAME}_PYTHON_HEADERS}" "bindings/python")
......@@ -94,8 +125,16 @@ IF(BUILD_PYTHON_INTERFACE)
ADD_CUSTOM_TARGET(python)
SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 2.2.0")
SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 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})
IF(LINK_PYTHON_INTERFACE_TO_OPENM)
TARGET_LINK_LIBRARIES(${PYWRAP} PRIVATE ${OpenMP_CXX_LIBRARIES})
ENDIF(LINK_PYTHON_INTERFACE_TO_OPENM)
ENDIF(BUILD_WITH_OPENMP_SUPPORT)
ADD_DEPENDENCIES(python ${PYWRAP})
SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES PREFIX "") # Remove lib prefix for the target
......@@ -117,7 +156,7 @@ IF(BUILD_PYTHON_INTERFACE)
TARGET_LINK_LIBRARIES(${PYWRAP} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy)
TARGET_LINK_BOOST_PYTHON(${PYWRAP} PUBLIC)
IF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DPINOCCHIO_WITH_HPP_FCL_PYTHON_BINDINGS)
TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_HPP_FCL_PYTHON_BINDINGS)
ENDIF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
IF(WIN32)
TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DNOMINMAX)
......
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
......@@ -17,7 +17,7 @@ namespace pinocchio
bp::def("rnea",
&rnea<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
bp::args("model","Data","q","v","a"),
bp::args("model","data","q","v","a"),
"Compute the RNEA, store the result in Data and return it.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
......@@ -29,7 +29,7 @@ namespace pinocchio
bp::def("rnea",
&rnea<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd,Force>,
bp::args("model","Data","q","v","a","fext"),
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"
"\tmodel: model of the kinematic tree\n"
......@@ -42,7 +42,7 @@ namespace pinocchio
bp::def("nonLinearEffects",
&nonLinearEffects<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("model","Data","q","v"),
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"
"\tmodel: model of the kinematic tree\n"
......@@ -53,7 +53,7 @@ namespace pinocchio
bp::def("computeGeneralizedGravity",
&computeGeneralizedGravity<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model","Data","q"),
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"
"\tmodel: model of the kinematic tree\n"
......@@ -63,7 +63,7 @@ namespace pinocchio
bp::def("computeStaticTorque",
&computeStaticTorque<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model","Data","q","fext"),
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"
"\tmodel: model of the kinematic tree\n"
......@@ -74,7 +74,7 @@ namespace pinocchio
bp::def("computeCoriolisMatrix",
&computeCoriolisMatrix<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("model","Data","q","v"),
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"
"\tmodel: model of the kinematic tree\n"
......@@ -85,7 +85,7 @@ namespace pinocchio
bp::def("getCoriolisMatrix",
&getCoriolisMatrix<double,0,JointCollectionDefaultTpl>,
bp::args("model","Data"),
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"
"\tmodel: model of the kinematic tree\n"
......
//
// Copyright (c) 2021 INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/parallel/aba.hpp"
#include <eigenpy/eigen-from-python.hpp>
namespace pinocchio
{
namespace python
{
static void aba_proxy_res(const int num_thread, ModelPool & pool,
const Eigen::MatrixXd & q, const Eigen::MatrixXd & v, const Eigen::MatrixXd & tau,
Eigen::Ref<Eigen::MatrixXd> a)
{
aba(num_thread,pool,q,v,tau,a);
}
static Eigen::MatrixXd aba_proxy(const int num_thread, ModelPool & pool,
const Eigen::MatrixXd & q, const Eigen::MatrixXd & v, const Eigen::MatrixXd & tau)
{
Eigen::MatrixXd a(v.rows(),v.cols());
aba(num_thread,pool,q,v,tau,a);
return a;
}
void exposeParallelABA()
{
namespace bp = boost::python;
using namespace Eigen;
bp::def("aba",
aba_proxy,
bp::args("num_thread","pool","q","v","a"),
"Computes in parallel the ABA and returns the result.\n\n"
"Parameters:\n"
"\tnum_thread: number of threads used for the computation\n"
"\tpool: pool of model/data\n"
"\tq: the joint configuration vector (size model.nq x batch_size)\n"
"\tv: the joint velocity vector (size model.nv x batch_size)\n"
"\ttau: the joint torque vector (size model.nv x batch_size)\n");