timings-geometry.cpp

Go to the documentation of this file.

//
// Copyright (c) 2015-2019 CNRS INRIA
//
 
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#ifdef PINOCCHIO_WITH_HPP_FCL
  #include "pinocchio/collision/collision.hpp"
#endif // PINOCCHIO_WITH_HPP_FCL
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/multibody/sample-models.hpp"
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/utils/timer.hpp"
 
#include <iostream>
 
int main()
{
  using namespace Eigen;
  using namespace pinocchio;
 
  PinocchioTicToc timer(PinocchioTicToc::US);
#ifdef NDEBUG
  const unsigned int NBT = 1000 * 100;
  const unsigned int NBD = 1000; // for heavy tests, like computeDistances()
#else
  const unsigned int NBT = 1;
  const unsigned int NBD = 1;
  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
#endif
 
  std::string romeo_filename =
    PINOCCHIO_MODEL_DIR
    + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
  std::vector<std::string> package_dirs;
  std::string romeo_meshDir = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots");
  package_dirs.push_back(romeo_meshDir);
 
  pinocchio::Model model;
  pinocchio::urdf::buildModel(romeo_filename, pinocchio::JointModelFreeFlyer(), model);
  pinocchio::GeometryModel geom_model;
  pinocchio::urdf::buildGeom(model, romeo_filename, COLLISION, geom_model, package_dirs);
#ifdef PINOCCHIO_WITH_HPP_FCL
  geom_model.addAllCollisionPairs();
#endif // PINOCCHIO_WITH_HPP_FCL
 
  Data data(model);
  GeometryData geom_data(geom_model);
  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
 
  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qs_romeo(NBT);
  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qdots_romeo(NBT);
  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) qddots_romeo(NBT);
  for (size_t i = 0; i < NBT; ++i)
  {
    qs_romeo[i] = randomConfiguration(model, -qmax, qmax);
    qdots_romeo[i] = Eigen::VectorXd::Random(model.nv);
    qddots_romeo[i] = Eigen::VectorXd::Random(model.nv);
  }
 
  timer.tic();
  SMOOTH(NBT)
  {
    forwardKinematics(model, data, qs_romeo[_smooth]);
  }
  double geom_time = timer.toc(PinocchioTicToc::US) / NBT;
 
  timer.tic();
  SMOOTH(NBT)
  {
    updateGeometryPlacements(model, data, geom_model, geom_data, qs_romeo[_smooth]);
  }
  double update_col_time = timer.toc(PinocchioTicToc::US) / NBT - geom_time;
  std::cout << "Update Collision Geometry < false > = \t" << update_col_time << " "
            << PinocchioTicToc::unitName(PinocchioTicToc::US) << std::endl;
 
#ifdef PINOCCHIO_WITH_HPP_FCL
  timer.tic();
  SMOOTH(NBT)
  {
    updateGeometryPlacements(model, data, geom_model, geom_data, qs_romeo[_smooth]);
    for (std::vector<pinocchio::CollisionPair>::iterator it = geom_model.collisionPairs.begin();
         it != geom_model.collisionPairs.end(); ++it)
    {
      computeCollision(geom_model, geom_data, std::size_t(it - geom_model.collisionPairs.begin()));
    }
  }
  double collideTime = timer.toc(PinocchioTicToc::US) / NBT - (update_col_time + geom_time);
  std::cout << "Collision test between two geometry objects (mean time) = \t"
            << collideTime / double(geom_model.collisionPairs.size())
            << PinocchioTicToc::unitName(PinocchioTicToc::US) << std::endl;
 
  timer.tic();
  SMOOTH(NBT)
  {
    computeCollisions(geom_model, geom_data, true);
  }
  double is_colliding_time = timer.toc(PinocchioTicToc::US) / NBT - (update_col_time + geom_time);
  std::cout << "Collision Test : robot in collision? = \t" << is_colliding_time
            << PinocchioTicToc::unitName(PinocchioTicToc::US) << std::endl;
 
  timer.tic();
  SMOOTH(NBD)
  {
    computeDistances(model, data, geom_model, geom_data, qs_romeo[_smooth]);
  }
  double computeDistancesTime =
    timer.toc(PinocchioTicToc::US) / NBD - (update_col_time + geom_time);
  std::cout << "Compute distance between two geometry objects (mean time) = \t"
            << computeDistancesTime / double(geom_model.collisionPairs.size()) << " "
            << PinocchioTicToc::unitName(PinocchioTicToc::US) << " "
            << geom_model.collisionPairs.size() << " col pairs" << std::endl;
 
#endif // PINOCCHIO_WITH_HPP_FCL
 
  return 0;
}