robot_state_benchmark.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2023, PickNik Robotics.
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, Mario Prats */
36 
37 // This file contains various benchmarks related to RobotState and matrix multiplication and inverse with Eigen types.
38 // To run this benchmark, 'cd' to the build/moveit_core/robot_state directory and directly run the binary.
39 
40 #include <benchmark/benchmark.h>
41 #include <random>
43 #include <kdl/treejnttojacsolver.hpp>
47 
48 // Robot and planning group for benchmarks.
49 constexpr char PANDA_TEST_ROBOT[] = "panda";
50 constexpr char PANDA_TEST_GROUP[] = "panda_arm";
51 
52 namespace
53 {
54 Eigen::Isometry3d createTestIsometry()
55 {
56  // An arbitrary Eigen::Isometry3d object.
57  return Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) *
58  Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) *
59  Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ());
60 }
61 } // namespace
62 
63 // Benchmark time to multiply an Eigen::Affine3d with an Eigen::Matrix4d.
64 static void multiplyAffineTimesMatrixNoAlias(benchmark::State& st)
65 {
66  Eigen::Isometry3d isometry = createTestIsometry();
67  Eigen::Affine3d result;
68  for (auto _ : st)
69  {
70  benchmark::DoNotOptimize(result.affine().noalias() = isometry.affine() * isometry.matrix());
71  benchmark::ClobberMemory();
72  }
73 }
74 
75 // Benchmark time to multiply an Eigen::Matrix4d with an Eigen::Matrix4d.
76 static void multiplyMatrixTimesMatrixNoAlias(benchmark::State& st)
77 {
78  Eigen::Isometry3d isometry = createTestIsometry();
79  Eigen::Isometry3d result;
80  for (auto _ : st)
81  {
82  benchmark::DoNotOptimize(result.matrix().noalias() = isometry.matrix() * isometry.matrix());
83  benchmark::ClobberMemory();
84  }
85 }
86 
87 // Benchmark time to multiply an Eigen::Isometry3d with an Eigen::Isometry3d.
88 static void multiplyIsometryTimesIsometryNoAlias(benchmark::State& st)
89 {
90  Eigen::Isometry3d isometry = createTestIsometry();
91  Eigen::Isometry3d result;
92  for (auto _ : st)
93  {
94  benchmark::DoNotOptimize(result.affine().noalias() = isometry.affine() * isometry.matrix());
95  benchmark::ClobberMemory();
96  }
97 }
98 
99 // Benchmark time to multiply an Eigen::Matrix4d with an Eigen::Matrix4d.
100 static void multiplyMatrixTimesMatrix(benchmark::State& st)
101 {
102  Eigen::Isometry3d isometry = createTestIsometry();
103  Eigen::Isometry3d result;
104  for (auto _ : st)
105  {
106  benchmark::DoNotOptimize(result.matrix() = isometry.matrix() * isometry.matrix());
107  benchmark::ClobberMemory();
108  }
109 }
110 // Benchmark time to multiply an Eigen::Isometry3d with an Eigen::Isometry3d.
111 static void multiplyIsometryTimesIsometry(benchmark::State& st)
112 {
113  Eigen::Isometry3d isometry = createTestIsometry();
114  Eigen::Isometry3d result;
115  for (auto _ : st)
116  {
117  benchmark::DoNotOptimize(result = isometry * isometry);
118  benchmark::ClobberMemory();
119  }
120 }
121 
122 // Benchmark time to invert an Eigen::Isometry3d.
123 static void inverseIsometry3d(benchmark::State& st)
124 {
125  Eigen::Isometry3d isometry = createTestIsometry();
126  Eigen::Isometry3d result;
127  for (auto _ : st)
128  {
129  benchmark::DoNotOptimize(result = isometry.inverse());
130  benchmark::ClobberMemory();
131  }
132 }
133 
134 // Benchmark time to invert an Eigen::Affine3d(Eigen::Isometry).
135 static void inverseAffineIsometry(benchmark::State& st)
136 {
137  Eigen::Affine3d affine = createTestIsometry();
138  Eigen::Affine3d result;
139  for (auto _ : st)
140  {
141  benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry));
142  benchmark::ClobberMemory();
143  }
144 }
145 
146 // Benchmark time to invert an Eigen::Affine3d.
147 static void inverseAffine(benchmark::State& st)
148 {
149  Eigen::Affine3d affine = createTestIsometry();
150  Eigen::Affine3d result;
151  for (auto _ : st)
152  {
153  benchmark::DoNotOptimize(result = affine.inverse());
154  benchmark::ClobberMemory();
155  }
156 }
157 
158 // Benchmark time to invert an Eigen::Matrix4d.
159 static void inverseMatrix4d(benchmark::State& st)
160 {
161  Eigen::Affine3d affine = createTestIsometry();
162  Eigen::Affine3d result;
163  for (auto _ : st)
164  {
165  benchmark::DoNotOptimize(result = affine.matrix().inverse());
166  benchmark::ClobberMemory();
167  }
168 }
169 
170 struct RobotStateBenchmark : ::benchmark::Fixture
171 {
172  void SetUp(const ::benchmark::State& /*state*/) override
173  {
176 
178  }
179 
180  std::vector<moveit::core::RobotState> constructStates(size_t num)
181  {
182  std::vector<moveit::core::RobotState> states;
183  states.reserve(num);
184  for (size_t i = 0; i < num; i++)
185  states.emplace_back(robot_model);
186  return states;
187  }
188  std::vector<moveit::core::RobotState> constructStates(size_t num, const moveit::core::RobotState& state)
189  {
190  std::vector<moveit::core::RobotState> states;
191  states.reserve(num);
192  for (size_t i = 0; i < num; i++)
193  states.emplace_back(state);
194  return states;
195  }
196 
197  std::vector<size_t> randomPermudation(size_t num)
198  {
199  std::vector<size_t> result;
200  result.reserve(num);
201  for (size_t i = 0; i < num; i++)
202  result.push_back(i);
203 
204  std::random_device random_device;
205  std::mt19937 generator(random_device());
206 
207  std::shuffle(result.begin(), result.end(), generator);
208  return result;
209  }
210 
211  moveit::core::RobotModelPtr robot_model;
212 };
213 
214 // Benchmark time to construct RobotStates
215 BENCHMARK_DEFINE_F(RobotStateBenchmark, construct)(benchmark::State& st)
216 {
217  for (auto _ : st)
218  {
219  auto states = constructStates(st.range(0));
220  benchmark::DoNotOptimize(states);
221  benchmark::ClobberMemory();
222  }
223 }
224 
225 // Benchmark time to copy-construct a RobotState.
226 BENCHMARK_DEFINE_F(RobotStateBenchmark, copyConstruct)(benchmark::State& st)
227 {
229  state.setToDefaultValues();
230  state.update();
231 
232  for (auto _ : st)
233  {
234  auto states = constructStates(st.range(0), state);
235  benchmark::DoNotOptimize(states);
236  benchmark::ClobberMemory();
237  }
238 }
239 
240 // Benchmark time to call `setToRandomPositions` and `update` on RobotState.
241 BENCHMARK_DEFINE_F(RobotStateBenchmark, update)(benchmark::State& st)
242 {
243  auto states = constructStates(st.range(0));
244  auto permutation = randomPermudation(states.size());
245  for (auto _ : st)
246  {
247  for (auto i : permutation) // process states in random order to challenge the cache
248  {
249  states[i].setToRandomPositions();
250  states[i].update();
251  }
252  }
253 }
254 
255 // Benchmark time to compute the Jacobian, using MoveIt's `getJacobian` function.
256 BENCHMARK_DEFINE_F(RobotStateBenchmark, jacobianMoveIt)(benchmark::State& st)
257 {
260  if (!jmg)
261  {
262  st.SkipWithError("The planning group doesn't exist.");
263  return;
264  }
265 
266  // Manually seeded RandomNumberGenerator for deterministic results
268 
269  for (auto _ : st)
270  {
271  // Time only the jacobian computation, not the forward kinematics.
272  st.PauseTiming();
273  state.setToRandomPositions(jmg, rng);
274  state.updateLinkTransforms();
275  st.ResumeTiming();
276  state.getJacobian(jmg);
277  }
278 }
279 
280 // Benchmark time to compute the Jacobian using KDL.
281 BENCHMARK_DEFINE_F(RobotStateBenchmark, jacobianKDL)(benchmark::State& st)
282 {
285  if (!jmg)
286  {
287  st.SkipWithError("The planning group doesn't exist.");
288  return;
289  }
290  KDL::Tree kdl_tree;
291  if (!kdl_parser::treeFromUrdfModel(*robot_model->getURDF(), kdl_tree))
292  {
293  st.SkipWithError("Can't create KDL tree.");
294  return;
295  }
296 
297  KDL::TreeJntToJacSolver jacobian_solver(kdl_tree);
298  KDL::Jacobian jacobian(kdl_tree.getNrOfJoints());
299  KDL::JntArray kdl_q(kdl_tree.getNrOfJoints());
300  const std::string tip_link = jmg->getLinkModelNames().back();
301 
302  // Manually seeded RandomNumberGenerator for deterministic results
304 
305  for (auto _ : st)
306  {
307  // Time only the jacobian computation, not the forward kinematics.
308  st.PauseTiming();
309  state.setToRandomPositions(jmg, rng);
310  state.copyJointGroupPositions(jmg, &kdl_q.data[0]);
311  st.ResumeTiming();
312  jacobian_solver.JntToJac(kdl_q, jacobian, tip_link);
313  }
314 }
315 
321 
326 
328  ->RangeMultiplier(10)
329  ->Range(100, 10000)
330  ->Unit(benchmark::kMillisecond);
332  ->RangeMultiplier(10)
333  ->Range(100, 10000)
334  ->Unit(benchmark::kMillisecond);
335 BENCHMARK_REGISTER_F(RobotStateBenchmark, update)->RangeMultiplier(10)->Range(10, 10000)->Unit(benchmark::kMillisecond);
336 
339 
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
PANDA_TEST_ROBOT
constexpr char PANDA_TEST_ROBOT[]
Definition: robot_state_benchmark.cpp:49
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
inverseMatrix4d
static void inverseMatrix4d(benchmark::State &st)
Definition: robot_state_benchmark.cpp:159
RobotStateBenchmark::SetUp
void SetUp(const ::benchmark::State &) override
Definition: robot_state_benchmark.cpp:172
moveit::core::RobotState::getJacobian
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
Definition: robot_state.cpp:1374
inverseIsometry3d
static void inverseIsometry3d(benchmark::State &st)
Definition: robot_state_benchmark.cpp:123
BENCHMARK_MAIN
BENCHMARK_MAIN()
multiplyMatrixTimesMatrixNoAlias
static void multiplyMatrixTimesMatrixNoAlias(benchmark::State &st)
Definition: robot_state_benchmark.cpp:76
PANDA_TEST_GROUP
constexpr char PANDA_TEST_GROUP[]
Definition: robot_state_benchmark.cpp:50
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::RobotState::copyJointGroupPositions
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:757
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Definition: robot_state.cpp:353
ros::console::set_logger_level
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
RobotStateBenchmark::randomPermudation
std::vector< size_t > randomPermudation(size_t num)
Definition: robot_state_benchmark.cpp:197
kdl_parser::treeFromUrdfModel
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
robot_model.h
BENCHMARK
BENCHMARK(multiplyAffineTimesMatrixNoAlias)
multiplyIsometryTimesIsometryNoAlias
static void multiplyIsometryTimesIsometryNoAlias(benchmark::State &st)
Definition: robot_state_benchmark.cpp:88
inverseAffineIsometry
static void inverseAffineIsometry(benchmark::State &st)
Definition: robot_state_benchmark.cpp:135
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:729
inverseAffine
static void inverseAffine(benchmark::State &st)
Definition: robot_state_benchmark.cpp:147
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:434
RobotStateBenchmark
Definition: robot_state_benchmark.cpp:170
kdl_parser.hpp
random_numbers::RandomNumberGenerator
moveit::core::JointModelGroup::getLinkModelNames
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
Definition: joint_model_group.h:284
multiplyMatrixTimesMatrix
static void multiplyMatrixTimesMatrix(benchmark::State &st)
Definition: robot_state_benchmark.cpp:100
moveit::core::RobotState::updateLinkTransforms
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
Definition: robot_state.cpp:770
RobotStateBenchmark::constructStates
std::vector< moveit::core::RobotState > constructStates(size_t num)
Definition: robot_state_benchmark.cpp:180
robot_model_test_utils.h
ros::console::notifyLoggerLevelsChanged
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
M_PI
#define M_PI
multiplyAffineTimesMatrixNoAlias
static void multiplyAffineTimesMatrixNoAlias(benchmark::State &st)
Definition: robot_state_benchmark.cpp:64
ros::console::levels::Warn
Warn
BENCHMARK_DEFINE_F
BENCHMARK_DEFINE_F(RobotStateBenchmark, construct)(benchmark
Definition: robot_state_benchmark.cpp:215
RobotStateBenchmark::constructStates
std::vector< moveit::core::RobotState > constructStates(size_t num, const moveit::core::RobotState &state)
Definition: robot_state_benchmark.cpp:188
RobotStateBenchmark::robot_model
moveit::core::RobotModelPtr robot_model
Definition: robot_state_benchmark.cpp:211
ROSCONSOLE_DEFAULT_NAME
#define ROSCONSOLE_DEFAULT_NAME
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:200
BENCHMARK_REGISTER_F
BENCHMARK_REGISTER_F(RobotStateBenchmark, update) -> RangeMultiplier(10) ->Range(10, 10000) ->Unit(benchmark::kMillisecond)
robot_state.h
multiplyIsometryTimesIsometry
static void multiplyIsometryTimesIsometry(benchmark::State &st)
Definition: robot_state_benchmark.cpp:111
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:117


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jul 21 2024 02:24:29