diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index fc68b43b9a..50c1ac9ddf 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -26,6 +26,9 @@ target_link_libraries(moveit_trajectory_processing install(DIRECTORY include/ DESTINATION include/moveit_core) if(BUILD_TESTING) + find_package(ament_cmake_google_benchmark REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(benchmark REQUIRED) if(WIN32) # TODO add windows paths # set(append_library_dirs "$;$") @@ -41,4 +44,15 @@ if(BUILD_TESTING) moveit_trajectory_processing moveit_test_utils ) + + ament_add_google_benchmark( + robot_trajectory_benchmark + test/robot_trajectory_benchmark.cpp) + target_link_libraries(robot_trajectory_benchmark + moveit_robot_model + moveit_test_utils + moveit_robot_state + moveit_robot_trajectory + moveit_trajectory_processing + ) endif() diff --git a/moveit_core/trajectory_processing/test/robot_trajectory_benchmark.cpp b/moveit_core/trajectory_processing/test/robot_trajectory_benchmark.cpp new file mode 100644 index 0000000000..c0eb07be11 --- /dev/null +++ b/moveit_core/trajectory_processing/test/robot_trajectory_benchmark.cpp @@ -0,0 +1,135 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Robotics. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Mario Prats */ + +// To run this benchmark, 'cd' to the build/moveit_core/trajectory_processing directory and directly run the binary. + +#include +#include +#include +#include +#include +#include + +// Robot and planning group to use in the benchmarks. +constexpr char TEST_ROBOT[] = "panda"; +constexpr char TEST_GROUP[] = "panda_arm"; + +// Benchmark manual creation of a trajectory with a given number of waypoints. +// This includes creating and updating the individual RobotState's. +static void robotTrajectoryCreate(benchmark::State& st) +{ + int n_states = st.range(0); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(TEST_GROUP)) + { + st.SkipWithError("The planning group doesn't exist."); + return; + } + auto* group = robot_model->getJointModelGroup(TEST_GROUP); + + // Robot state. + moveit::core::RobotState robot_state(robot_model); + robot_state.setToDefaultValues(); + + for (auto _ : st) + { + auto trajectory = std::make_shared(robot_model, group); + for (int i = 0; i < n_states; ++i) + { + // Create a sinusoidal test trajectory for all the joints. + const double joint_value = std::sin(0.001 * i); + const double duration_from_previous = 0.1; + + moveit::core::RobotState robot_state_waypoint(robot_state); + Eigen::VectorXd joint_values = Eigen::VectorXd::Constant(group->getActiveVariableCount(), joint_value); + robot_state_waypoint.setJointGroupActivePositions(group, joint_values); + trajectory->addSuffixWayPoint(robot_state_waypoint, duration_from_previous); + } + } +} + +// Benchmark timing of a trajectory with a given number of waypoints, via TOTG. +static void robotTrajectoryTiming(benchmark::State& st) +{ + int n_states = st.range(0); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(TEST_GROUP)) + { + st.SkipWithError("The planning group doesn't exist."); + return; + } + auto* group = robot_model->getJointModelGroup(TEST_GROUP); + + // Robot state. + moveit::core::RobotState robot_state(robot_model); + robot_state.setToDefaultValues(); + + // Trajectory. + auto trajectory = std::make_shared(robot_model, group); + Eigen::VectorXd joint_values = Eigen::VectorXd::Zero(group->getActiveVariableCount()); + for (int i = 0; i < n_states; ++i) + { + // Create a sinusoidal test trajectory for all the joints. + const double joint_value = std::sin(0.001 * i); + const double duration_from_previous = 0.0; + + moveit::core::RobotState robot_state_waypoint(robot_state); + joint_values = Eigen::VectorXd::Constant(group->getActiveVariableCount(), joint_value); + robot_state_waypoint.setJointGroupActivePositions(group, joint_values); + trajectory->addSuffixWayPoint(robot_state_waypoint, duration_from_previous); + } + + // Add some velocity / acceleration limits, which are needed for TOTG. + std::unordered_map velocity_limits, acceleration_limits; + for (const auto& joint_name : group->getActiveJointModelNames()) + { + velocity_limits[joint_name] = 1.0; + acceleration_limits[joint_name] = 2.0; + } + + for (auto _ : st) + { + trajectory_processing::TimeOptimalTrajectoryGeneration totg(/*path_tolerance=*/0.0); + totg.computeTimeStamps(*trajectory, velocity_limits, acceleration_limits); + } +} + +BENCHMARK(robotTrajectoryCreate)->RangeMultiplier(10)->Range(10, 100000)->Unit(benchmark::kMillisecond); +BENCHMARK(robotTrajectoryTiming)->RangeMultiplier(10)->Range(10, 20000)->Unit(benchmark::kMillisecond);