40 #include <benchmark/benchmark.h>
41 #include <kdl_parser/kdl_parser.hpp>
42 #include <kdl/chainjnttojacsolver.hpp>
46 #include <random_numbers/random_numbers.h>
59 Eigen::Isometry3d createTestIsometry()
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());
69 static void multiplyAffineTimesMatrix(benchmark::State& st)
71 int n_iters = st.range(0);
72 Eigen::Isometry3d isometry = createTestIsometry();
75 for (
int i = 0; i < n_iters; ++i)
77 Eigen::Affine3d result;
78 benchmark::DoNotOptimize(result = isometry.affine() * isometry.matrix());
79 benchmark::ClobberMemory();
85 static void multiplyMatrixTimesMatrix(benchmark::State& st)
87 int n_iters = st.range(0);
88 Eigen::Isometry3d isometry = createTestIsometry();
91 for (
int i = 0; i < n_iters; ++i)
93 Eigen::Matrix4d result;
94 benchmark::DoNotOptimize(result = isometry.matrix() * isometry.matrix());
95 benchmark::ClobberMemory();
101 static void multiplyIsometryTimesIsometry(benchmark::State& st)
103 int n_iters = st.range(0);
104 Eigen::Isometry3d isometry = createTestIsometry();
107 for (
int i = 0; i < n_iters; ++i)
109 Eigen::Isometry3d result;
110 benchmark::DoNotOptimize(result = isometry * isometry);
111 benchmark::ClobberMemory();
117 static void inverseIsometry3d(benchmark::State& st)
119 int n_iters = st.range(0);
120 Eigen::Isometry3d isometry = createTestIsometry();
123 for (
int i = 0; i < n_iters; ++i)
125 Eigen::Isometry3d result;
126 benchmark::DoNotOptimize(result = isometry.inverse());
127 benchmark::ClobberMemory();
133 static void inverseAffineIsometry(benchmark::State& st)
135 int n_iters = st.range(0);
136 Eigen::Isometry3d isometry = createTestIsometry();
137 Eigen::Affine3d affine;
138 affine.matrix() = isometry.matrix();
142 for (
int i = 0; i < n_iters; ++i)
144 Eigen::Affine3d result;
145 benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry).affine());
146 benchmark::ClobberMemory();
152 static void inverseAffine(benchmark::State& st)
154 int n_iters = st.range(0);
155 Eigen::Isometry3d isometry = createTestIsometry();
156 Eigen::Affine3d affine;
157 affine.matrix() = isometry.matrix();
161 for (
int i = 0; i < n_iters; ++i)
163 Eigen::Affine3d result;
164 benchmark::DoNotOptimize(result = affine.inverse().affine());
165 benchmark::ClobberMemory();
171 static void inverseMatrix4d(benchmark::State& st)
173 int n_iters = st.range(0);
174 Eigen::Isometry3d isometry = createTestIsometry();
175 Eigen::Affine3d affine;
176 affine.matrix() = isometry.matrix();
180 for (
int i = 0; i < n_iters; ++i)
182 Eigen::Affine3d result;
183 benchmark::DoNotOptimize(result = affine.matrix().inverse());
184 benchmark::ClobberMemory();
190 static void robotStateConstruct(benchmark::State& st)
192 int n_states = st.range(0);
198 st.SkipWithError(
"The planning group doesn't exist.");
204 for (
int i = 0; i < n_states; i++)
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();
214 static void robotStateCopy(benchmark::State& st)
216 int n_states = st.range(0);
222 st.SkipWithError(
"The planning group doesn't exist.");
228 robot_state.setToDefaultValues();
232 for (
int i = 0; i < n_states; i++)
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();
242 static void robotStateUpdate(benchmark::State& st)
244 int n_states = st.range(0);
250 for (
int i = 0; i < n_states; ++i)
252 state.setToRandomPositions();
254 benchmark::ClobberMemory();
260 static void robotStateForwardKinematics(benchmark::State& st)
262 int n_states = st.range(0);
268 for (
int i = 0; i < n_states; ++i)
270 state.setToRandomPositions();
271 Eigen::Isometry3d transform;
272 benchmark::DoNotOptimize(transform = state.getGlobalLinkTransform(robot_model->getLinkModel(
PR2_TIP_LINK)));
273 benchmark::ClobberMemory();
279 static void moveItJacobian(benchmark::State& st)
287 st.SkipWithError(
"The planning group doesn't exist.");
297 random_numbers::RandomNumberGenerator rng(0);
303 kinematic_state.setToRandomPositions(jmg, rng);
304 kinematic_state.updateLinkTransforms();
306 kinematic_state.getJacobian(jmg);
311 static void kdlJacobian(benchmark::State& st)
318 st.SkipWithError(
"The planning group doesn't exist.");
328 random_numbers::RandomNumberGenerator rng(0);
331 if (!kdl_parser::treeFromUrdfModel(*robot_model->getURDF(), kdl_tree))
333 st.SkipWithError(
"Can't create KDL tree.");
337 KDL::Chain kdl_chain;
338 if (!kdl_tree.getChain(jmg->
getJointModels().front()->getParentLinkModel()->getName(),
341 st.SkipWithError(
"Can't create KDL Chain.");
345 KDL::ChainJntToJacSolver jacobian_solver(kdl_chain);
351 kinematic_state.setToRandomPositions(jmg, rng);
352 kinematic_state.updateLinkTransforms();
353 KDL::Jacobian jacobian(kdl_chain.getNrOfJoints());
355 kdl_q.resize(kdl_chain.getNrOfJoints());
356 kinematic_state.copyJointGroupPositions(jmg, &kdl_q.data[0]);
358 jacobian_solver.JntToJac(kdl_q, jacobian);
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);
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.
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[]