robot_state_benchmark.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2018, CITEC Bielefeld
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Robert Haschke */
40 #include <gtest/gtest.h>
41 #include <chrono>
42 
43 // Helper class to measure time within a scoped block and output the result
45 {
46  const char* const msg_;
47  double* const gold_standard_;
48  const std::chrono::time_point<std::chrono::steady_clock> start_;
49 
50 public:
51  // if gold_standard is provided, a relative increase/decrease is shown too
52  ScopedTimer(const char* msg = "", double* gold_standard = nullptr)
53  : msg_(msg), gold_standard_(gold_standard), start_(std::chrono::steady_clock::now())
54  {
55  }
56 
58  {
59  std::chrono::duration<double> elapsed = std::chrono::steady_clock::now() - start_;
60  std::cerr << msg_ << elapsed.count() * 1000. << "ms ";
61 
62  if (gold_standard_)
63  {
64  if (*gold_standard_ == 0)
65  *gold_standard_ = elapsed.count();
66  std::cerr << 100 * elapsed.count() / *gold_standard_ << "%";
67  }
68  std::cerr << std::endl;
69  }
70 };
71 
72 class Timing : public testing::Test
73 {
74 protected:
75  void SetUp() override
76  {
77  Eigen::Isometry3d iso = Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) *
78  Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) *
79  Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ());
80  transforms_.push_back(Eigen::Isometry3d::Identity()); // result
81  transforms_.push_back(iso); // input
82  }
83 
84  void TearDown() override
85  {
86  }
87 
88 public:
89  const Eigen::Isometry3d id = Eigen::Isometry3d::Identity();
90  // put transforms into a vector to avoid compiler optimization on variables
92  volatile size_t result_idx_ = 0;
93  volatile size_t input_idx_ = 1;
94 };
95 
96 TEST_F(Timing, stateUpdate)
97 {
98  robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2_description");
99  ASSERT_TRUE(bool(model));
100  robot_state::RobotState state(model);
101  ScopedTimer t("RobotState updates: ");
102  for (unsigned i = 0; i < 1e5; ++i)
103  {
104  state.setToRandomPositions();
105  state.update();
106  }
107 }
108 
109 TEST_F(Timing, multiply)
110 {
111  size_t RUNS = 1e7;
112  double gold_standard = 0;
113  {
114  ScopedTimer t("Eigen::Affine * Eigen::Matrix: ", &gold_standard);
115  for (size_t i = 0; i < RUNS; ++i)
116  transforms_[result_idx_].affine().noalias() = transforms_[input_idx_].affine() * transforms_[input_idx_].matrix();
117  }
118  {
119  ScopedTimer t("Eigen::Matrix * Eigen::Matrix: ", &gold_standard);
120  for (size_t i = 0; i < RUNS; ++i)
121  transforms_[result_idx_].matrix().noalias() = transforms_[input_idx_].matrix() * transforms_[input_idx_].matrix();
122  }
123  {
124  ScopedTimer t("Eigen::Isometry * Eigen::Isometry: ", &gold_standard);
125  for (size_t i = 0; i < RUNS; ++i)
126  transforms_[result_idx_] = transforms_[input_idx_] * transforms_[input_idx_];
127  }
128 }
129 
130 TEST_F(Timing, inverse)
131 {
132  EigenSTL::vector_Affine3d affine(1);
133  affine[0].matrix() = transforms_[input_idx_].matrix();
134  size_t RUNS = 1e7;
135  double gold_standard = 0;
136  {
137  ScopedTimer t("Isometry3d::inverse(): ", &gold_standard);
138  for (size_t i = 0; i < RUNS; ++i)
139  transforms_[result_idx_] = transforms_[input_idx_].inverse();
140  }
141  volatile size_t input_idx = 0;
142  {
143  ScopedTimer t("Affine3d::inverse(Eigen::Isometry): ", &gold_standard);
144  for (size_t i = 0; i < RUNS; ++i)
145  transforms_[result_idx_].affine().noalias() = affine[input_idx].inverse(Eigen::Isometry).affine();
146  }
147  {
148  ScopedTimer t("Affine3d::inverse(): ", &gold_standard);
149  for (size_t i = 0; i < RUNS; ++i)
150  transforms_[result_idx_].affine().noalias() = affine[input_idx].inverse().affine();
151  }
152  {
153  ScopedTimer t("Matrix4d::inverse(): ", &gold_standard);
154  for (size_t i = 0; i < RUNS; ++i)
155  transforms_[result_idx_].matrix().noalias() = affine[input_idx].matrix().inverse();
156  }
157 }
158 
159 int main(int argc, char** argv)
160 {
161  testing::InitGoogleTest(&argc, argv);
162  return RUN_ALL_TESTS();
163 }
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
const std::chrono::time_point< std::chrono::steady_clock > start_
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
TEST_F(Timing, stateUpdate)
EigenSTL::vector_Isometry3d transforms_
#define M_PI
geometry_msgs::TransformStamped t
int main(int argc, char **argv)
ScopedTimer(const char *msg="", double *gold_standard=nullptr)
void TearDown() override
const char *const msg_
void update(bool force=false)
Update all transforms.
TF2SIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
double *const gold_standard_
void SetUp() override
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Nov 11 2019 04:01:09