parallel-aba.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2021 INRIA
3 //
4 
5 #include "pinocchio/spatial/fwd.hpp"
6 #include "pinocchio/algorithm/parallel/aba.hpp"
7 #include "pinocchio/algorithm/aba.hpp"
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 #include "pinocchio/parsers/sample-models.hpp"
10 #include "pinocchio/utils/timer.hpp"
11 
12 #include <iostream>
13 
14 #include <boost/test/unit_test.hpp>
15 #include <boost/utility/binary.hpp>
16 
17 using namespace pinocchio;
18 
19 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
20 
21 BOOST_AUTO_TEST_CASE(test_parallel_aba)
22 {
24  Data data_ref(model);
25 
26  model.lowerPositionLimit.head<3>().fill(-1.);
27  model.upperPositionLimit.head<3>().fill( 1.);
28 
29  const Eigen::DenseIndex batch_size = 128;
30  const int num_threads = omp_get_max_threads();
31 
32  Eigen::MatrixXd q(model.nq,batch_size);
33  Eigen::MatrixXd v(model.nv,batch_size);
34  Eigen::MatrixXd tau(model.nv,batch_size);
35  Eigen::MatrixXd a(model.nv,batch_size);
36  Eigen::MatrixXd a_ref(model.nv,batch_size);
37 
38  for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
39  {
40  q.col(i) = randomConfiguration(model);
41  v.col(i) = Eigen::VectorXd::Random(model.nv);
42  tau.col(i) = Eigen::VectorXd::Random(model.nv);
43  }
44 
45  ModelPool pool(model);
46  aba(num_threads,pool,q,v,tau,a);
47 
48  for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
49  {
50  a_ref.col(i) = aba(model,data_ref,q.col(i),v.col(i),tau.col(i));
51  }
52 
53  BOOST_CHECK(a == a_ref);
54 }
55 
56 BOOST_AUTO_TEST_SUITE_END()
57 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
BOOST_AUTO_TEST_CASE(test_parallel_aba)
Main pinocchio namespace.
Definition: timings.cpp:30
int nv
Dimension of the velocity vector space.
list a
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
JointCollectionTpl & model
int nq
Dimension of the configuration vector representation.
void fill(Eigen::Ref< MatType > mat, const typename MatType::Scalar &value)


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04