Verified Commit 097ae1f3 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo: add parallel computations algo for ABA, RNEA and Collisions

parent 47499908
......@@ -217,6 +217,7 @@ 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) 2021 INRIA
//
#ifndef __pinocchio_algorithm_parallel_aba_hpp__
#define __pinocchio_algorithm_parallel_aba_hpp__
#include <omp.h>
#include "pinocchio/multibody/pool/model.hpp"
#include "pinocchio/algorithm/aba.hpp"
namespace pinocchio
{
///
/// \brief A parallel version of the Articulated Body algorithm. It computes the forward dynamics, aka the joint acceleration according to the current state of the system and the desired joint torque.
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorPool Matrix type of the joint configuration vector.
/// \tparam TangentVectorPool1 Matrix type of the joint velocity vector.
/// \tparam TangentVectorPool2 Matrix type of the joint torque vector.
/// \tparam TangentVectorPool3 Matrix type of the joint acceleration vector.
///
/// \param[in] pool Pool containing model and data for parallel computations.
/// \param[in] num_threads Number of threads used for parallel computations.
/// \param[in] q The joint configuration vector (dim model.nq x batch_size).
/// \param[in] v The joint velocity vector (dim model.nv x batch_size).
/// \param[in] tau The joint acceleration vector (dim model.nv x batch_size).
/// \param[out] a The joint torque vector (dim model.nv x batch_size).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorPool, typename TangentVectorPool1, typename TangentVectorPool2, typename TangentVectorPool3>
void aba(const int num_threads,
ModelPoolTpl<Scalar,Options,JointCollectionTpl> & pool,
const Eigen::MatrixBase<ConfigVectorPool> & q,
const Eigen::MatrixBase<TangentVectorPool1> & v,
const Eigen::MatrixBase<TangentVectorPool2> & tau,
const Eigen::MatrixBase<TangentVectorPool3> & a)
{
typedef ModelPoolTpl<Scalar,Options,JointCollectionTpl> Pool;
typedef typename Pool::Model Model;
typedef typename Pool::Data Data;
typedef typename Pool::DataVector DataVector;
const Model & model = pool.model();
DataVector & datas = pool.datas();
TangentVectorPool3 & res = a.const_cast_derived();
PINOCCHIO_CHECK_INPUT_ARGUMENT(num_threads <= pool.size(), "The pool is too small");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.rows(), model.nq);
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), v.cols());
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), a.cols());
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), res.cols());
omp_set_num_threads(num_threads);
const Eigen::DenseIndex batch_size = res.cols();
Eigen::DenseIndex i = 0;
#pragma omp parallel for
for(i = 0; i < batch_size; i++)
{
const int thread_id = omp_get_thread_num();
Data & data = datas[(size_t)thread_id];
res.col(i) = aba(model,data,q.col(i),v.col(i),tau.col(i));
}
}
}
#endif // ifndef __pinocchio_algorithm_parallel_aba_hpp__
//
// Copyright (c) 2021 INRIA
//
#ifndef __pinocchio_algorithm_parallel_geometry_hpp__
#define __pinocchio_algorithm_parallel_geometry_hpp__
#include <omp.h>
#include "pinocchio/multibody/pool/geometry.hpp"
#include "pinocchio/algorithm/geometry.hpp"
namespace pinocchio
{
inline bool computeCollisions(const int num_threads,
const GeometryModel & geom_model,
GeometryData & geom_data,
const bool stopAtFirstCollision = false)
{
bool is_colliding = false;
omp_set_num_threads(num_threads);
std::size_t cp_index = 0;
#pragma omp parallel for
for(cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
{
if(stopAtFirstCollision && is_colliding) continue;
const CollisionPair & cp = geom_model.collisionPairs[cp_index];
if(geom_data.activeCollisionPairs[cp_index]
&& !( geom_model.geometryObjects[cp.first].disableCollision
|| geom_model.geometryObjects[cp.second].disableCollision))
{
fcl::CollisionRequest & collision_request = geom_data.collisionRequests[cp_index];
collision_request.distance_upper_bound = collision_request.security_margin + 1e-6; // TODO: change the margin
fcl::CollisionResult & collision_result = geom_data.collisionResults[cp_index];
collision_result.clear();
fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[cp.first ])),
oM2 (toFclTransform3f(geom_data.oMg[cp.second]));
const GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[cp_index];
std::size_t res = do_computations(oM1, oM2, collision_request, collision_result);
if(!is_colliding && res)
{
is_colliding = true;
geom_data.collisionPairIndex = cp_index; // first pair to be in collision
}
}
}
return is_colliding;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
bool computeCollisions(const int num_threads,
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const GeometryModel & geom_model,
GeometryData & geom_data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool stopAtFirstCollision = false)
{
updateGeometryPlacements(model, data, geom_model, geom_data, q);
return computeCollisions(num_threads, geom_model, geom_data, stopAtFirstCollision);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorPool, typename CollisionVectorResult>
void computeCollisions(const int num_threads,
GeometryPoolTpl<Scalar,Options,JointCollectionTpl> & pool,
const Eigen::MatrixBase<ConfigVectorPool> & q,
const Eigen::MatrixBase<CollisionVectorResult> & res,
const bool stopAtFirstCollision = false)
{
typedef GeometryPoolTpl<Scalar,Options,JointCollectionTpl> Pool;
typedef typename Pool::Model Model;
typedef typename Pool::Data Data;
typedef typename Pool::GeometryModel GeometryModel;
typedef typename Pool::GeometryData GeometryData;
typedef typename Pool::DataVector DataVector;
typedef typename Pool::GeometryDataVector GeometryDataVector;
const Model & model = pool.model();
const GeometryModel & geometry_model = pool.geometry_model();
DataVector & datas = pool.datas();
GeometryDataVector & geometry_datas = pool.geometry_datas();
CollisionVectorResult & res_ = res.const_cast_derived();
PINOCCHIO_CHECK_INPUT_ARGUMENT(num_threads <= pool.size(), "The pool is too small");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.rows(), model.nq);
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), res.size());
res_.fill(false);
omp_set_num_threads(num_threads);
const Eigen::DenseIndex batch_size = res.size();
Eigen::DenseIndex i = 0;
#pragma omp parallel for
for(i = 0; i < batch_size; i++)
{
const int thread_id = omp_get_thread_num();
Data & data = datas[(size_t)thread_id];
GeometryData & geometry_data = geometry_datas[(size_t)thread_id];
res_[i] = computeCollisions(model,data,geometry_model,geometry_data,q.col(i),stopAtFirstCollision);
}
}
}
#endif // ifndef __pinocchio_algorithm_parallel_geometry_hpp__
//
// Copyright (c) 2021 INRIA
//
#ifndef __pinocchio_algorithm_parallel_rnea_hpp__
#define __pinocchio_algorithm_parallel_rnea_hpp__
#include <omp.h>
#include "pinocchio/multibody/pool/model.hpp"
#include "pinocchio/algorithm/rnea.hpp"
namespace pinocchio
{
///
/// \brief The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques according to the current state of the system and the desired joint accelerations.
///
/// \tparam JointCollection Collection of Joint types.
/// \tparam ConfigVectorPool Matrix type of the joint configuration vector.
/// \tparam TangentVectorPool1 Matrix type of the joint velocity vector.
/// \tparam TangentVectorPool2 Matrix type of the joint acceleration vector.
/// \tparam TangentVectorPool3 Matrix type of the joint torque vector.
///
/// \param[in] pool Pool containing model and data for parallel computations.
/// \param[in] num_threads Number of threads used for parallel computations.
/// \param[in] q The joint configuration vector (dim model.nq x batch_size).
/// \param[in] v The joint velocity vector (dim model.nv x batch_size).
/// \param[in] a The joint acceleration vector (dim model.nv x batch_size).
/// \param[out] tau The joint torque vector (dim model.nv x batch_size).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorPool, typename TangentVectorPool1, typename TangentVectorPool2, typename TangentVectorPool3>
void rnea(const int num_threads,
ModelPoolTpl<Scalar,Options,JointCollectionTpl> & pool,
const Eigen::MatrixBase<ConfigVectorPool> & q,
const Eigen::MatrixBase<TangentVectorPool1> & v,
const Eigen::MatrixBase<TangentVectorPool2> & a,
const Eigen::MatrixBase<TangentVectorPool3> & tau)
{
typedef ModelPoolTpl<Scalar,Options,JointCollectionTpl> Pool;
typedef typename Pool::Model Model;
typedef typename Pool::Data Data;
typedef typename Pool::DataVector DataVector;
const Model & model = pool.model();
DataVector & datas = pool.datas();
TangentVectorPool3 & res = tau.const_cast_derived();
PINOCCHIO_CHECK_INPUT_ARGUMENT(num_threads <= pool.size(), "The pool is too small");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.rows(), model.nq);
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), v.cols());
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), a.cols());
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), res.cols());
omp_set_num_threads(num_threads);
const Eigen::DenseIndex batch_size = res.cols();
Eigen::DenseIndex i = 0;
#pragma omp parallel for
for(i = 0; i < batch_size; i++)
{
const int thread_id = omp_get_thread_num();
Data & data = datas[(size_t)thread_id];
res.col(i) = rnea(model,data,q.col(i),v.col(i),a.col(i));
}
}
}
#endif // ifndef __pinocchio_algorithm_parallel_rnea_hpp__
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment