invert waypoint velocities on reverse (#1335)

Otherwise the trajectories profile will be broken and robot controllers that respect velocities in specified trajectories, e.g. default PR2 and TIAGo, will generate oscillating profiles.

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -516,6 +516,9 @@ class RobotState
     return effort_[index];
   }
 
+  /** \brief Invert velocity if present. */
+  void invertVelocity();
+
   /** @} */
 
   /** \name Getting and setting joint positions, velocities, accelerations and effort

moveit_core/robot_state/src/robot_state.cpp

@@ -422,6 +422,15 @@ void RobotState::setVariableEffort(const std::vector<std::string>& variable_name
     effort_[robot_model_->getVariableIndex(variable_names[i])] = variable_effort[i];
 }
 
+void RobotState::invertVelocity()
+{
+  if (has_velocity_)
+  {
+    for (size_t i = 0; i < robot_model_->getVariableCount(); ++i)
+      velocity_[i] *= -1;
+  }
+}
+
 void RobotState::setJointEfforts(const JointModel* joint, const double* effort)
 {
   if (has_acceleration_)

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -101,6 +101,11 @@ void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t st
 void RobotTrajectory::reverse()
 {
   std::reverse(waypoints_.begin(), waypoints_.end());
+  for (robot_state::RobotStatePtr& waypoint : waypoints_)
+  {
+    // reversing the trajectory implies inverting the velocity profile
+    waypoint->invertVelocity();
+  }
   if (!duration_from_previous_.empty())
   {
     duration_from_previous_.push_back(duration_from_previous_.front());
@@ -470,4 +475,4 @@ bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration,
   return true;
 }
 
-}  // end of namespace robot_trajectory
+}  // end of namespace robot_trajectory