Ensure to reset the smoothing plugin when resuming Servo (#2537)

moveit · Nov 20, 2023 · 6f43588 · 6f43588
1 parent e064a84
commit 6f43588
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 16 deletions.
diff --git 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<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

diff --git 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<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);
 }
 

diff --git 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_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";
   }