diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index f234728ec61..2c7f60b8d2c 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -121,16 +122,10 @@ double FloatingJointModel::distanceTranslation(const double* values1, const doub double FloatingJointModel::distanceRotation(const double* values1, const double* values2) const { - double dq = - fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]); - if (dq + std::numeric_limits::epsilon() >= 1.0) - { - return 0.0; - } - else - { - return acos(dq); - } + // The values are in "xyzw" order but Eigen expects "wxyz". + const auto q1 = Eigen::Quaterniond(values1[6], values1[3], values1[4], values1[5]).normalized(); + const auto q2 = Eigen::Quaterniond(values2[6], values2[3], values2[4], values2[5]).normalized(); + return q2.angularDistance(q1); } void FloatingJointModel::interpolate(const double* from, const double* to, const double t, double* state) const diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 1d0a246eac5..4a22a813ce3 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -55,7 +55,7 @@ class RobotTrajectoryTestFixture : public testing::Test { robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_); robot_state_ = std::make_shared(robot_model_); - robot_state_->setToDefaultValues(arm_jmg_name_, arm_state_name_); + robot_state_->setToDefaultValues(); robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0); robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1); robot_state_->update(); @@ -67,7 +67,7 @@ class RobotTrajectoryTestFixture : public testing::Test void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory) { - // Init a traj + // Init a trajectory ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) << "Robot model does not have group: " << arm_jmg_name_; @@ -79,7 +79,10 @@ class RobotTrajectoryTestFixture : public testing::Test double duration_from_previous = 0.1; std::size_t waypoint_count = 5; for (std::size_t ix = 0; ix < waypoint_count; ++ix) + { trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous); + } + // Quick check that getDuration is working correctly EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count) << "Generated trajectory duration incorrect"; @@ -528,6 +531,17 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength) { robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); + EXPECT_FLOAT_EQ(robot_trajectory::pathLength(*trajectory), 0.0); + + // modify joint values so the smoothness is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } EXPECT_GT(robot_trajectory::pathLength(*trajectory), 0.0); } @@ -536,6 +550,16 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); + // modify joint values so the smoothness is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + const auto smoothness = robot_trajectory::smoothness(*trajectory); ASSERT_TRUE(smoothness.has_value()); EXPECT_GT(smoothness.value(), 0.0); @@ -550,13 +574,28 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity) robot_trajectory::RobotTrajectoryPtr trajectory; initTestTrajectory(trajectory); - const auto density = robot_trajectory::waypointDensity(*trajectory); + // If trajectory has all equal state, and length zero, density should be null. + auto density = robot_trajectory::waypointDensity(*trajectory); + ASSERT_FALSE(density.has_value()); + + // modify joint values so the density is nonzero + std::vector positions; + for (size_t i = 0; i < trajectory->size(); ++i) + { + auto waypoint = trajectory->getWayPointPtr(i); + waypoint->copyJointGroupPositions(arm_jmg_name_, positions); + positions[0] += 0.01 * i; + waypoint->setJointGroupPositions(arm_jmg_name_, positions); + } + + density = robot_trajectory::waypointDensity(*trajectory); ASSERT_TRUE(density.has_value()); EXPECT_GT(density.value(), 0.0); // Check for empty trajectory trajectory->clear(); - EXPECT_FALSE(robot_trajectory::waypointDensity(*trajectory).has_value()); + density = robot_trajectory::waypointDensity(*trajectory); + EXPECT_FALSE(density.has_value()); } TEST_F(OneRobot, Unwind) diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index fc68b43b9a1..50c1ac9ddf7 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 00000000000..c0eb07be119 --- /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); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 4b01e8cd7ce..180b17543b2 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -125,7 +125,19 @@ class Servo * @param The last commanded joint states. * @return The next state stepping towards the required halting state. */ - std::pair smoothHalt(const KinematicState& halt_state) const; + std::pair smoothHalt(const KinematicState& halt_state); + + /** + * \brief Applies smoothing to an input state, if a smoothing plugin is set. + * @param state The state to be updated by the smoothing plugin. + */ + void doSmoothing(KinematicState& state); + + /** + * \brief Resets the smoothing plugin, if set, to a specified state. + * @param state The desired state to reset the smoothing plugin to. + */ + void resetSmoothing(const KinematicState& state); private: /** @@ -215,6 +227,7 @@ class Servo std::atomic collision_velocity_scale_ = 1.0; std::unique_ptr collision_monitor_; + // Pointer to the (optional) smoothing plugin. pluginlib::UniquePtr smoother_ = nullptr; // Map between joint subgroup names and corresponding joint name - move group indices map diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 8022762eebc..55839747174 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -168,8 +168,23 @@ void Servo::setSmoothingPlugin() RCLCPP_ERROR(logger_, "Smoothing plugin could not be initialized"); std::exit(EXIT_FAILURE); } - const KinematicState current_state = getCurrentRobotState(); - smoother_->reset(current_state.positions, current_state.velocities, current_state.accelerations); + resetSmoothing(getCurrentRobotState()); +} + +void Servo::doSmoothing(KinematicState& state) +{ + if (smoother_) + { + smoother_->doSmoothing(state.positions, state.velocities, state.accelerations); + } +} + +void Servo::resetSmoothing(const KinematicState& state) +{ + if (smoother_) + { + smoother_->reset(state.positions, state.velocities, state.accelerations); + } } void Servo::setCollisionChecking(const bool check_collision) @@ -467,13 +482,8 @@ KinematicState Servo::getNextJointState(const ServoInput& command) // Compute the next joint positions based on the joint position deltas target_state.positions = current_state.positions + joint_position_delta; - // TODO : apply filtering to the velocity instead of position // Apply smoothing to the positions if a smoother was provided. - // Update filter state and apply filtering in position domain - if (smoother_) - { - smoother_->doSmoothing(target_state.positions, target_state.velocities, target_state.accelerations); - } + doSmoothing(target_state); // Compute velocities based on smoothed joint positions target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period; @@ -615,7 +625,7 @@ KinematicState Servo::getCurrentRobotState() const return current_state; } -std::pair Servo::smoothHalt(const KinematicState& halt_state) const +std::pair Servo::smoothHalt(const KinematicState& halt_state) { bool stopped = false; auto target_state = halt_state; @@ -630,16 +640,12 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta (target_state.velocities[i] - current_state.velocities[i]) / servo_params_.publish_period; } + doSmoothing(target_state); + // If all velocities are near zero, robot has decelerated to a stop. stopped = (std::accumulate(target_state.velocities.begin(), target_state.velocities.end(), 0.0) <= STOPPED_VELOCITY_EPS); - if (smoother_) - { - smoother_->reset(current_state.positions, current_state.velocities, current_state.accelerations); - smoother_->doSmoothing(target_state.positions, target_state.velocities, target_state.accelerations); - } - return std::make_pair(stopped, target_state); } diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 4b23d1aba93..bf6017c741e 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -162,6 +162,9 @@ void ServoNode::pauseServo(const std::shared_ptrresetSmoothing(servo_->getCurrentRobotState()); + servo_->setCollisionChecking(true); response->message = "Servoing enabled"; }