moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 <kdl_parser/kdl_parser.hpp>
42 #include <kdl/chainjnttojacsolver.hpp>
46 #include <random_numbers/random_numbers.h>
47 
48 // Robot and planning group for benchmarks.
49 constexpr char PANDA_TEST_ROBOT[] = "panda";
50 constexpr char PANDA_TEST_GROUP[] = "panda_arm";
51 constexpr char PR2_TEST_ROBOT[] = "pr2";
52 constexpr char PR2_TIP_LINK[] = "r_wrist_roll_link";
53 
54 // Number of iterations to use in matrix multiplication / inversion benchmarks.
55 constexpr int MATRIX_OPS_N_ITERATIONS = 1e7;
56 
57 namespace
58 {
59 Eigen::Isometry3d createTestIsometry()
60 {
61  // An arbitrary Eigen::Isometry3d object.
62  return Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) *
63  Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) *
64  Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ());
65 }
66 } // namespace
67 
68 // Benchmark time to multiply an Eigen::Affine3d with an Eigen::Matrix4d.
69 static void multiplyAffineTimesMatrix(benchmark::State& st)
70 {
71  int n_iters = st.range(0);
72  Eigen::Isometry3d isometry = createTestIsometry();
73  for (auto _ : st)
74  {
75  for (int i = 0; i < n_iters; ++i)
76  {
77  Eigen::Affine3d result;
78  benchmark::DoNotOptimize(result = isometry.affine() * isometry.matrix());
79  benchmark::ClobberMemory();
80  }
81  }
82 }
83 
84 // Benchmark time to multiply an Eigen::Matrix4d with an Eigen::Matrix4d.
85 static void multiplyMatrixTimesMatrix(benchmark::State& st)
86 {
87  int n_iters = st.range(0);
88  Eigen::Isometry3d isometry = createTestIsometry();
89  for (auto _ : st)
90  {
91  for (int i = 0; i < n_iters; ++i)
92  {
93  Eigen::Matrix4d result;
94  benchmark::DoNotOptimize(result = isometry.matrix() * isometry.matrix());
95  benchmark::ClobberMemory();
96  }
97  }
98 }
99 
100 // Benchmark time to multiply an Eigen::Isometry3d with an Eigen::Isometry3d.
101 static void multiplyIsometryTimesIsometry(benchmark::State& st)
102 {
103  int n_iters = st.range(0);
104  Eigen::Isometry3d isometry = createTestIsometry();
105  for (auto _ : st)
106  {
107  for (int i = 0; i < n_iters; ++i)
108  {
109  Eigen::Isometry3d result;
110  benchmark::DoNotOptimize(result = isometry * isometry);
111  benchmark::ClobberMemory();
112  }
113  }
114 }
115 
116 // Benchmark time to invert an Eigen::Isometry3d.
117 static void inverseIsometry3d(benchmark::State& st)
118 {
119  int n_iters = st.range(0);
120  Eigen::Isometry3d isometry = createTestIsometry();
121  for (auto _ : st)
122  {
123  for (int i = 0; i < n_iters; ++i)
124  {
125  Eigen::Isometry3d result;
126  benchmark::DoNotOptimize(result = isometry.inverse());
127  benchmark::ClobberMemory();
128  }
129  }
130 }
131 
132 // Benchmark time to invert an Eigen::Affine3d(Eigen::Isometry).
133 static void inverseAffineIsometry(benchmark::State& st)
134 {
135  int n_iters = st.range(0);
136  Eigen::Isometry3d isometry = createTestIsometry();
137  Eigen::Affine3d affine;
138  affine.matrix() = isometry.matrix();
139 
140  for (auto _ : st)
141  {
142  for (int i = 0; i < n_iters; ++i)
143  {
144  Eigen::Affine3d result;
145  benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry).affine());
146  benchmark::ClobberMemory();
147  }
148  }
149 }
150 
151 // Benchmark time to invert an Eigen::Affine3d.
152 static void inverseAffine(benchmark::State& st)
153 {
154  int n_iters = st.range(0);
155  Eigen::Isometry3d isometry = createTestIsometry();
156  Eigen::Affine3d affine;
157  affine.matrix() = isometry.matrix();
158 
159  for (auto _ : st)
160  {
161  for (int i = 0; i < n_iters; ++i)
162  {
163  Eigen::Affine3d result;
164  benchmark::DoNotOptimize(result = affine.inverse().affine());
165  benchmark::ClobberMemory();
166  }
167  }
168 }
169 
170 // Benchmark time to invert an Eigen::Matrix4d.
171 static void inverseMatrix4d(benchmark::State& st)
172 {
173  int n_iters = st.range(0);
174  Eigen::Isometry3d isometry = createTestIsometry();
175  Eigen::Affine3d affine;
176  affine.matrix() = isometry.matrix();
177 
178  for (auto _ : st)
179  {
180  for (int i = 0; i < n_iters; ++i)
181  {
182  Eigen::Affine3d result;
183  benchmark::DoNotOptimize(result = affine.matrix().inverse());
184  benchmark::ClobberMemory();
185  }
186  }
187 }
188 
189 // Benchmark time to construct a RobotState given a RobotModel.
190 static void robotStateConstruct(benchmark::State& st)
191 {
192  int n_states = st.range(0);
193  const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT);
194 
195  // Make sure the group exists, otherwise exit early with an error.
196  if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP))
197  {
198  st.SkipWithError("The planning group doesn't exist.");
199  return;
200  }
201 
202  for (auto _ : st)
203  {
204  for (int i = 0; i < n_states; i++)
205  {
206  std::unique_ptr<moveit::core::RobotState> robot_state;
207  benchmark::DoNotOptimize(robot_state = std::make_unique<moveit::core::RobotState>(robot_model));
208  benchmark::ClobberMemory();
209  }
210  }
211 }
212 
213 // Benchmark time to copy a RobotState.
214 static void robotStateCopy(benchmark::State& st)
215 {
216  int n_states = st.range(0);
217  const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT);
218 
219  // Make sure the group exists, otherwise exit early with an error.
220  if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP))
221  {
222  st.SkipWithError("The planning group doesn't exist.");
223  return;
224  }
225 
226  // Robot state.
227  moveit::core::RobotState robot_state(robot_model);
228  robot_state.setToDefaultValues();
229 
230  for (auto _ : st)
231  {
232  for (int i = 0; i < n_states; i++)
233  {
234  std::unique_ptr<moveit::core::RobotState> robot_state_copy;
235  benchmark::DoNotOptimize(robot_state_copy = std::make_unique<moveit::core::RobotState>(robot_state));
236  benchmark::ClobberMemory();
237  }
238  }
239 }
240 
241 // Benchmark time to call `setToRandomPositions` and `update` on a RobotState.
242 static void robotStateUpdate(benchmark::State& st)
243 {
244  int n_states = st.range(0);
245  const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT);
246  moveit::core::RobotState state(robot_model);
247 
248  for (auto _ : st)
249  {
250  for (int i = 0; i < n_states; ++i)
251  {
252  state.setToRandomPositions();
253  state.update();
254  benchmark::ClobberMemory();
255  }
256  }
257 }
258 
259 // Benchmark time to call `setToRandomPositions` and `getGlobalLinkTransform` on a RobotState.
260 static void robotStateForwardKinematics(benchmark::State& st)
261 {
262  int n_states = st.range(0);
263  const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT);
264  moveit::core::RobotState state(robot_model);
265 
266  for (auto _ : st)
267  {
268  for (int i = 0; i < n_states; ++i)
269  {
270  state.setToRandomPositions();
271  Eigen::Isometry3d transform;
272  benchmark::DoNotOptimize(transform = state.getGlobalLinkTransform(robot_model->getLinkModel(PR2_TIP_LINK)));
273  benchmark::ClobberMemory();
274  }
275  }
276 }
277 
278 // Benchmark time to compute the Jacobian, using MoveIt's `getJacobian` function.
279 static void moveItJacobian(benchmark::State& st)
280 {
281  // Load a test robot model.
282  const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT);
283 
284  // Make sure the group exists, otherwise exit early with an error.
285  if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP))
286  {
287  st.SkipWithError("The planning group doesn't exist.");
288  return;
289  }
290 
291  // Robot state.
292  moveit::core::RobotState kinematic_state(robot_model);
293  const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(PANDA_TEST_GROUP);
294 
295  // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint
296  // configurations.
297  random_numbers::RandomNumberGenerator rng(0);
298 
299  for (auto _ : st)
300  {
301  // Time only the jacobian computation, not the forward kinematics.
302  st.PauseTiming();
303  kinematic_state.setToRandomPositions(jmg, rng);
304  kinematic_state.updateLinkTransforms();
305  st.ResumeTiming();
306  kinematic_state.getJacobian(jmg);
307  }
308 }
309 
310 // Benchmark time to compute the Jacobian using KDL.
311 static void kdlJacobian(benchmark::State& st)
312 {
313  const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT);
314 
315  // Make sure the group exists, otherwise exit early with an error.
316  if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP))
317  {
318  st.SkipWithError("The planning group doesn't exist.");
319  return;
320  }
321 
322  // Robot state.
323  moveit::core::RobotState kinematic_state(robot_model);
324  const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(PANDA_TEST_GROUP);
325 
326  // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint
327  // configurations.
328  random_numbers::RandomNumberGenerator rng(0);
329 
330  KDL::Tree kdl_tree;
331  if (!kdl_parser::treeFromUrdfModel(*robot_model->getURDF(), kdl_tree))
332  {
333  st.SkipWithError("Can't create KDL tree.");
334  return;
335  }
336 
337  KDL::Chain kdl_chain;
338  if (!kdl_tree.getChain(jmg->getJointModels().front()->getParentLinkModel()->getName(),
339  jmg->getLinkModelNames().back(), kdl_chain))
340  {
341  st.SkipWithError("Can't create KDL Chain.");
342  return;
343  }
344 
345  KDL::ChainJntToJacSolver jacobian_solver(kdl_chain);
346 
347  for (auto _ : st)
348  {
349  // Time only the jacobian computation, not the forward kinematics.
350  st.PauseTiming();
351  kinematic_state.setToRandomPositions(jmg, rng);
352  kinematic_state.updateLinkTransforms();
353  KDL::Jacobian jacobian(kdl_chain.getNrOfJoints());
354  KDL::JntArray kdl_q;
355  kdl_q.resize(kdl_chain.getNrOfJoints());
356  kinematic_state.copyJointGroupPositions(jmg, &kdl_q.data[0]);
357  st.ResumeTiming();
358  jacobian_solver.JntToJac(kdl_q, jacobian);
359  }
360 }
361 
362 BENCHMARK(multiplyAffineTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond);
363 BENCHMARK(multiplyMatrixTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond);
364 BENCHMARK(multiplyIsometryTimesIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond);
365 
366 BENCHMARK(inverseIsometry3d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond);
367 BENCHMARK(inverseAffineIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond);
368 BENCHMARK(inverseAffine)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond);
369 BENCHMARK(inverseMatrix4d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond);
370 
371 BENCHMARK(robotStateConstruct)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond);
372 BENCHMARK(robotStateCopy)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond);
373 BENCHMARK(robotStateUpdate)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond);
374 BENCHMARK(robotStateForwardKinematics)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond);
375 
376 BENCHMARK(moveItJacobian);
377 BENCHMARK(kdlJacobian);
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
constexpr char PR2_TEST_ROBOT[]
BENCHMARK(multiplyAffineTimesMatrix) -> Arg(MATRIX_OPS_N_ITERATIONS) ->Unit(benchmark::kMillisecond)
constexpr int MATRIX_OPS_N_ITERATIONS
constexpr char PANDA_TEST_ROBOT[]
constexpr char PR2_TIP_LINK[]
constexpr char PANDA_TEST_GROUP[]