interbody-distance-time-of-computation.cc.in 3.25 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
// Copyright (C) 2014, 2014 LAAS-CNRS
// Author: Florent Lamiraux
//
// This file is part of the test-hpp.
//
// test-hpp is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// test-hpp is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with test-hpp.  If not, see <http://www.gnu.org/licenses/>.

#include <sstream>
#include <ostream>
#include <fstream>
#include <vector>

#include <Eigen/Core>

26
#include <hpp/fcl/shape/geometric_shapes.h>
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99

#include <hpp/util/debug.hh>
#include <hpp/model/body.hh>
#include <hpp/model/collision-object.hh>
#include <hpp/model/device.hh>
#include <hpp/model/joint.hh>
#include <hpp/model/urdf/util.hh>


#define BOOST_TEST_MODULE forward_kinematics
#include <boost/test/included/unit_test.hpp>

typedef boost::shared_ptr <const fcl::Capsule> CapsuleConstPtr_t;
typedef hpp::model::matrix_t::Scalar value_type;
typedef hpp::model::CollisionObjectPtr_t CollisionObjectPtr_t;
typedef hpp::model::vector3_t vector3_t;
typedef hpp::model::DistanceResult DistanceResult_t;
typedef hpp::model::Transform3f Transform3f;
typedef hpp::model::vector3_t vector3_t;

BOOST_AUTO_TEST_SUITE( test_suite )

BOOST_AUTO_TEST_CASE (interbody_distances_time_of_computation)
{
  using hpp::model::Device;
  using hpp::model::Joint;
  using hpp::model::Body;
  typedef std::vector<double> vector_t;
  std::ifstream fileConfig;
  vector_t::size_type configSize;
  fileConfig.open ("@PROJECT_SOURCE_DIR@/data/configuration.data");
  vector_t dofs;
  bool eof = false;
  double value;

  // Read configuration file
  while (!eof) {
    fileConfig >> value;
    if (fileConfig.fail ()) {
      eof = true;
    } else {
      dofs.push_back (value);
    }
  }
  fileConfig.close ();
  // Load hrp2
  hpp::model::HumanoidRobotPtr_t humanoidRobot =
    hpp::model::HumanoidRobot::create ("hrp2_14");
  hpp::model::urdf::loadHumanoidModel(humanoidRobot, "freeflyer",
				      "hrp2_14_description", "hrp2_14",
				      "_capsule", "_capsule");
  configSize = humanoidRobot->configSize ();
  // Check size of config
  if (dofs.size () != configSize) {
    std::ostringstream error;
    error << "wrong configuration size: " << dofs.size ()
	  << ", expecting " << configSize;
    throw std::runtime_error (error.str ());
  }
  hpp::model::Configuration_t config (configSize);
  for (vector_t::size_type i=0; i<configSize; ++i) {
    config [i] = dofs [i];
  }
  humanoidRobot->currentConfiguration (config);
  humanoidRobot->computeForwardKinematics ();
  hppDout (info, *humanoidRobot);

  timeval t0, t1;
  gettimeofday (&t0, NULL);
  for (std::size_t i=0; i<1000; ++i) {
    humanoidRobot->computeDistances ();
  }
  gettimeofday (&t1, NULL);
100
101
  double t = (double)(t1.tv_sec - t0.tv_sec) +
    1e-6*(double)(t1.tv_usec - t0.tv_usec);
102
103
104
105
106
  std::cout << "Time for 1000 self-collision tests in seconds: "
	    << t << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()