timings-geometry.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 //
4 
5 #include "pinocchio/algorithm/joint-configuration.hpp"
6 #include "pinocchio/algorithm/kinematics.hpp"
7 #include "pinocchio/algorithm/geometry.hpp"
8 #include "pinocchio/parsers/urdf.hpp"
9 #include "pinocchio/parsers/sample-models.hpp"
10 #include "pinocchio/multibody/geometry.hpp"
11 #include "pinocchio/utils/timer.hpp"
12 
13 #include <iostream>
14 
15 
16 int main()
17 {
18  using namespace Eigen;
19  using namespace pinocchio;
20 
22  #ifdef NDEBUG
23  const unsigned int NBT = 1000*100;
24  const unsigned int NBD = 1000; // for heavy tests, like computeDistances()
25  #else
26  const unsigned int NBT = 1;
27  const unsigned int NBD = 1;
28  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
29  #endif
30 
31  std::string romeo_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
32  std::vector < std::string > package_dirs;
33  std::string romeo_meshDir = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots");
34  package_dirs.push_back(romeo_meshDir);
35 
38  pinocchio::GeometryModel geom_model; pinocchio::urdf::buildGeom(model, romeo_filename, COLLISION, geom_model, package_dirs);
39 #ifdef PINOCCHIO_WITH_HPP_FCL
40  geom_model.addAllCollisionPairs();
41 #endif // PINOCCHIO_WITH_HPP_FCL
42 
43  Data data(model);
44  GeometryData geom_data(geom_model);
45  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
46 
47 
48  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qs_romeo (NBT);
49  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qdots_romeo (NBT);
50  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qddots_romeo (NBT);
51  for(size_t i=0;i<NBT;++i)
52  {
53  qs_romeo[i] = randomConfiguration(model,-qmax,qmax);
54  qdots_romeo[i] = Eigen::VectorXd::Random(model.nv);
55  qddots_romeo[i] = Eigen::VectorXd::Random(model.nv);
56  }
57 
58  timer.tic();
59  SMOOTH(NBT)
60  {
61  forwardKinematics(model,data,qs_romeo[_smooth]);
62  }
63  double geom_time = timer.toc(PinocchioTicToc::US)/NBT;
64 
65  timer.tic();
66  SMOOTH(NBT)
67  {
68  updateGeometryPlacements(model,data,geom_model,geom_data,qs_romeo[_smooth]);
69  }
70  double update_col_time = timer.toc(PinocchioTicToc::US)/NBT - geom_time;
71  std::cout << "Update Collision Geometry < false > = \t" << update_col_time << " " << PinocchioTicToc::unitName(PinocchioTicToc::US) << std::endl;
72 
73 #ifdef PINOCCHIO_WITH_HPP_FCL
74  timer.tic();
75  SMOOTH(NBT)
76  {
77  updateGeometryPlacements(model,data,geom_model,geom_data,qs_romeo[_smooth]);
78  for (std::vector<pinocchio::CollisionPair>::iterator it = geom_model.collisionPairs.begin(); it != geom_model.collisionPairs.end(); ++it)
79  {
80  computeCollision(geom_model,geom_data,std::size_t(it-geom_model.collisionPairs.begin()));
81  }
82  }
83  double collideTime = timer.toc(PinocchioTicToc::US)/NBT - (update_col_time + geom_time);
84  std::cout << "Collision test between two geometry objects (mean time) = \t" << collideTime / double(geom_model.collisionPairs.size())
86 
87 
88  timer.tic();
89  SMOOTH(NBT)
90  {
91  computeCollisions(geom_model,geom_data, true);
92  }
93  double is_colliding_time = timer.toc(PinocchioTicToc::US)/NBT - (update_col_time + geom_time);
94  std::cout << "Collision Test : robot in collision? = \t" << is_colliding_time
96 
97 
98  timer.tic();
99  SMOOTH(NBD)
100  {
101  computeDistances(model,data,geom_model,geom_data,qs_romeo[_smooth]);
102  }
103  double computeDistancesTime = timer.toc(PinocchioTicToc::US)/NBD - (update_col_time + geom_time);
104  std::cout << "Compute distance between two geometry objects (mean time) = \t" << computeDistancesTime / double(geom_model.collisionPairs.size())
105  << " " << PinocchioTicToc::unitName(PinocchioTicToc::US) << " " << geom_model.collisionPairs.size() << " col pairs" << std::endl;
106 
107 #endif // PINOCCHIO_WITH_HPP_FCL
108 
109  return 0;
110 }
double toc()
Definition: timer.hpp:68
static std::string unitName(Unit u)
Definition: timer.hpp:52
CollisionPairVector collisionPairs
Vector of collision pairs.
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.
#define PINOCCHIO_MODEL_DIR
void tic()
Definition: timer.hpp:63
bool computeCollisions(const int num_threads, const GeometryModel &geom_model, GeometryData &geom_data, const bool stopAtFirstCollision=false)
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
#define SMOOTH(s)
Definition: timer.hpp:38
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
int main()
void addAllCollisionPairs()
Add all possible collision pairs.
Main pinocchio namespace.
Definition: timings.cpp:28
int nv
Dimension of the velocity vector space.
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
int nq
Dimension of the configuration vector representation.


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:33