Implement jerk-limited smoothing for Servo

Author: AndyZe

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h

@@ -82,6 +82,11 @@ class RuckigFilterPlugin : public SmoothingBaseClass
              const Eigen::VectorXd& accelerations) override;
 
 private:
+  /**
+   * A utility to print Ruckig's internal state
+   */
+  void printRuckigState();
+
   rclcpp::Node::SharedPtr node_;
   void getDefaultJointVelAccelBounds();
 
@@ -90,7 +95,7 @@ class RuckigFilterPlugin : public SmoothingBaseClass
   size_t num_joints_;
   moveit::core::RobotModelConstPtr robot_model_;
 
-  bool have_initial_ruckig_output_ = false;
+  bool have_initial_ruckig_output_;
   std::optional<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_;
   std::optional<ruckig::InputParameter<ruckig::DynamicDOFs>> ruckig_input_;
   std::optional<ruckig::OutputParameter<ruckig::DynamicDOFs>> ruckig_output_;

moveit_core/online_signal_smoothing/src/ruckig_filter.cpp

@@ -56,6 +56,7 @@ bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::
   node_ = node;
   num_joints_ = num_joints;
   robot_model_ = robot_model;
+  have_initial_ruckig_output_ = false;
 
   // get node parameters and store in member variables
   auto param_listener = online_signal_smoothing::ParamListener(node_);
@@ -67,9 +68,9 @@ bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::
   ruckig_input.max_velocity = joint_velocity_bounds_;
   ruckig_input.max_acceleration = joint_acceleration_bounds_;
   ruckig_input.max_jerk = joint_jerk_bounds_;
-  // initialize positions. All other quantities are initialized to zero in the constructor.
-  // This will be overwritten in the first control loop
   ruckig_input.current_position = std::vector<double>(num_joints_, 0.0);
+  ruckig_input.current_velocity = std::vector<double>(num_joints_, 0.0);
+  ruckig_input.current_acceleration = std::vector<double>(num_joints_, 0.0);
   ruckig_input_ = ruckig_input;
 
   ruckig_output_.emplace(ruckig::OutputParameter<ruckig::DynamicDOFs>(num_joints_));
@@ -82,12 +83,68 @@ bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::
 bool RuckigFilterPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities,
                                      Eigen::VectorXd& accelerations)
 {
+  if (have_initial_ruckig_output_ && ruckig_input_ && ruckig_output_)
+  {
+    ruckig_output_->pass_to_input(*ruckig_input_);
+  }
+  // else
+  // {
+  //   // Force a new calculation
+  //   ruckig_.reset();
+  // }
+
+  // Update Ruckig target state
+  // This assumes stationary at the target (zero vel, zero accel)
+  std::cerr << "Updating target position" << std::endl;
+  ruckig_input_->target_position = std::vector<double>(positions.data(), positions.data() + positions.size());
+
+  // Call the Ruckig algorithm
+  std::cerr << "State prior to ruckig_.update()" << std::endl;
+  printRuckigState();
+  ruckig::Result ruckig_result = ruckig_->update(*ruckig_input_, *ruckig_output_);
+
+  // Finished means the target state can be reached in this timestep.
+  // Working means the target state can be reached but not in this timestep.
+  // ErrorSynchronizationCalculation means smoothing was successful but the robot will deviate a bit from the desired
+  // path.
+  // See https://github.com/pantor/ruckig/blob/master/include/ruckig/input_parameter.hpp
+  if (ruckig_result != ruckig::Result::Finished && ruckig_result != ruckig::Result::Working &&
+      ruckig_result != ruckig::Result::ErrorSynchronizationCalculation)
+
+  {
+    RCLCPP_ERROR_STREAM(getLogger(), "Ruckig jerk-limited smoothing failed with code: " << ruckig_result);
+    printRuckigState();
+    // Return without modifying the position/vel/accel
+    have_initial_ruckig_output_ = false;
+    return true;
+  }
+
+  // Update the target state with Ruckig output
+  positions = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(ruckig_output_->new_position.data(),
+                                                            ruckig_output_->new_position.size());
+  velocities = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(ruckig_output_->new_velocity.data(),
+                                                             ruckig_output_->new_velocity.size());
+  accelerations = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(ruckig_output_->new_acceleration.data(),
+                                                                ruckig_output_->new_acceleration.size());
+  have_initial_ruckig_output_ = true;
+
   return true;
 }
 
 bool RuckigFilterPlugin::reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
                                const Eigen::VectorXd& accelerations)
 {
+  // Initialize Ruckig
+  std::cerr << "Setting the current position in reset()!" << std::endl;
+  ruckig_input_->current_position = std::vector<double>(positions.data(), positions.data() + positions.size());
+  ruckig_input_->current_velocity = std::vector<double>(velocities.data(), velocities.data() + velocities.size());
+  ruckig_input_->current_acceleration =
+      std::vector<double>(accelerations.data(), accelerations.data() + accelerations.size());
+
+  std::cerr << ruckig_input_->current_acceleration[0] << "  " << ruckig_input_->current_acceleration[1] << "  "
+            << ruckig_input_->current_acceleration[2] << std::endl;
+
+  have_initial_ruckig_output_ = false;
   return true;
 }
 
@@ -121,12 +178,28 @@ void RuckigFilterPlugin::getDefaultJointVelAccelBounds()
       joint_acceleration_bounds_.push_back(DBL_MAX);
     }
 
-    // MoveIt and the URDF don't support jerk limits, so use a safe default always
-    joint_jerk_bounds_.push_back(DEFAULT_JERK_BOUND);
+    if (bound.jerk_bounded_)
+    {
+      // Assume symmetric limits
+      joint_jerk_bounds_.push_back(bound.max_jerk_);
+    }
+    else
+    {
+      RCLCPP_ERROR_STREAM(getLogger(), "WARNING: No joint jerk limit defined. A default jerk limit of "
+                                           << DEFAULT_JERK_BOUND << " rad/s^3 has been applied.");
+      joint_jerk_bounds_.push_back(DEFAULT_JERK_BOUND);
+    }
   }
 
   assert(joint_jerk_bounds_.size() == num_joints_);
 }
+
+void RuckigFilterPlugin::printRuckigState()
+{
+  RCLCPP_ERROR_STREAM(getLogger(), ruckig_->delta_time << "\nRuckig input:\n"
+                                                       << ruckig_input_->to_string() << "\nRuckig output:\n"
+                                                       << ruckig_output_->to_string());
+}
 }  // namespace online_signal_smoothing
 
 #include <pluginlib/class_list_macros.hpp>

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -117,7 +117,7 @@ class Servo
   servo::Params& getParams();
 
   /**
-   * \brief Get the current state of the robot as given by planning scene monitor.
+   * \brief Get the current state of the robot as given by planning scene monitor. This is blocking.
    * @return The current state of the robot.
    */
   KinematicState getCurrentRobotState() const;

moveit_ros/moveit_servo/src/servo.cpp

@@ -79,16 +79,6 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
 
   moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
 
-  // Load the smoothing plugin
-  if (servo_params_.use_smoothing)
-  {
-    setSmoothingPlugin();
-  }
-  else
-  {
-    RCLCPP_WARN(logger_, "No smoothing plugin loaded");
-  }
-
   // Create the collision checker and start collision checking.
   collision_monitor_ =
       std::make_unique<CollisionMonitor>(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_));
@@ -125,6 +115,17 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
     joint_name_to_index_maps_.insert(
         std::make_pair<std::string, JointNameToMoveGroupIndexMap>(std::string(sub_group_name), std::move(new_map)));
   }
+
+  // Load the smoothing plugin
+  if (servo_params_.use_smoothing)
+  {
+    setSmoothingPlugin();
+  }
+  else
+  {
+    RCLCPP_WARN(logger_, "No smoothing plugin loaded");
+  }
+
   RCLCPP_INFO_STREAM(logger_, "Servo initialized successfully");
 }
 
@@ -645,6 +646,17 @@ std::optional<PoseCommand> Servo::toPlanningFrame(const PoseCommand& command, co
 
 KinematicState Servo::getCurrentRobotState() const
 {
+  // waitForCurrentState(const rclcpp::Time& t = rclcpp::Clock(RCL_ROS_TIME).now(), double wait_time_s = 1.0) const;
+  bool have_current_state = false;
+  while (!have_current_state)
+  {
+    have_current_state =
+        planning_scene_monitor_->getStateMonitor()->waitForCurrentState(rclcpp::Clock(RCL_ROS_TIME).now(), 1.0 /* s */);
+    if (!have_current_state)
+    {
+      RCLCPP_WARN(logger_, "Waiting for the current state");
+    }
+  }
   moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
   return extractRobotState(robot_state, servo_params_.move_group_name);
 }

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -298,12 +298,13 @@ void ServoNode::servoLoop()
   while (planning_scene_monitor_->getLastUpdateTime().get_clock_type() != node_->get_clock()->get_clock_type() ||
          servo_node_start > planning_scene_monitor_->getLastUpdateTime())
   {
-    RCLCPP_INFO(node_->get_logger(), "Waiting to receive robot state update.");
+    RCLCPP_INFO(node_->get_logger(), "Waiting to receive a valid robot state update.");
     rclcpp::sleep_for(std::chrono::seconds(1));
   }
   KinematicState current_state = servo_->getCurrentRobotState();
   last_commanded_state_ = current_state;
   // Ensure the filter is up to date
+  std::cerr << "Initial update of the filter with resetSmoothing()" << std::endl;
   servo_->resetSmoothing(current_state);
 
   // Get the robot state and joint model group info.
@@ -383,7 +384,7 @@ void ServoNode::servoLoop()
     {
       // if no new command was created, use current robot state
       updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time);
-      servo_->resetSmoothing(current_state);
+      // servo_->resetSmoothing(current_state);
     }
 
     status_msg.code = static_cast<int8_t>(servo_->getStatus());

moveit_ros/moveit_servo/src/utils/common.cpp

@@ -493,13 +493,23 @@ planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const
 
 KinematicState extractRobotState(const moveit::core::RobotStatePtr& robot_state, const std::string& move_group_name)
 {
+  std::cerr << "Doing extractRobotState" << std::endl;
   const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(move_group_name);
   const auto joint_names = joint_model_group->getActiveJointModelNames();
   KinematicState current_state(joint_names.size());
   current_state.joint_names = joint_names;
   robot_state->copyJointGroupPositions(joint_model_group, current_state.positions);
   robot_state->copyJointGroupVelocities(joint_model_group, current_state.velocities);
+
   robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations);
+  // If any acceleration is nan, set all accelerations to zero
+  if (std::any_of(current_state.accelerations.begin(), current_state.accelerations.end(),
+                  [](double v) { return isnan(v); }))
+  {
+    std::cerr << "Zeroing accelerations" << std::endl;
+    robot_state->zeroAccelerations();
+    robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations);
+  }
 
   return current_state;
 }