Merge branch 'main' into pr-enable_multiple_planners

moveit_core/robot_model/src/floating_joint_model.cpp

@@ -36,6 +36,7 @@
 /* Author: Ioan Sucan */
 
 #include <cmath>
+#include <Eigen/Geometry>
 #include <geometric_shapes/check_isometry.h>
 #include <limits>
 #include <moveit/robot_model/floating_joint_model.h>
@@ -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<double>::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

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<moveit::core::RobotState>(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<double> 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<double> 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<double> 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)

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 "$<TARGET_FILE_DIR:${PROJECT_NAME}>;$<TARGET_FILE_DIR:${PROJECT_NAME}_TestPlugins1>")
@@ -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()

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 <benchmark/benchmark.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/utils/robot_model_test_utils.h>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
+
+// 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_trajectory::RobotTrajectory>(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_trajectory::RobotTrajectory>(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<std::string, double> 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);

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<bool, KinematicState> smoothHalt(const KinematicState& halt_state) const;
+  std::pair<bool, KinematicState> 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<double> collision_velocity_scale_ = 1.0;
   std::unique_ptr<CollisionMonitor> collision_monitor_;
 
+  // Pointer to the (optional) smoothing plugin.
   pluginlib::UniquePtr<online_signal_smoothing::SmoothingBaseClass> smoother_ = nullptr;
 
   // Map between joint subgroup names and corresponding joint name - move group indices map

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<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_state) const
+std::pair<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_state)
 {
   bool stopped = false;
   auto target_state = halt_state;
@@ -630,16 +640,12 @@ std::pair<bool, KinematicState> 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);
 }
 

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -162,6 +162,9 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request
   }
   else
   {
+    // Reset the smoothing plugin with the robot's current state in case the robot moved between pausing and unpausing.
+    servo_->resetSmoothing(servo_->getCurrentRobotState());
+
     servo_->setCollisionChecking(true);
     response->message = "Servoing enabled";
   }