Optional jerk bounds

Author: AndyZe

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

@@ -89,8 +89,9 @@ class RuckigFilterPlugin : public SmoothingBaseClass
 
   /**
    * A utility to get velocity/acceleration/jerk bounds from the robot model
+   * @return true if all bounds are defined
    */
-  void getVelAccelJerkBounds();
+  bool getVelAccelJerkBounds();
 
   rclcpp::Node::SharedPtr node_;
   /** \brief Parameters loaded from yaml file at runtime */
@@ -105,6 +106,6 @@ class RuckigFilterPlugin : public SmoothingBaseClass
 
   std::vector<double> joint_velocity_bounds_;
   std::vector<double> joint_acceleration_bounds_;
-  std::vector<double> joint_jerk_bounds_;
+  std::optional<std::vector<double>> joint_jerk_bounds_;
 };
 }  // namespace online_signal_smoothing

moveit_core/online_signal_smoothing/src/ruckig_filter.cpp

@@ -47,7 +47,6 @@ rclcpp::Logger getLogger()
 {
   return moveit::getLogger("moveit.core.ruckig_filter_plugin");
 }
-inline constexpr double DEFAULT_JERK_BOUND = 300;  // rad/s^3
 }  // namespace
 
 bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model,
@@ -62,15 +61,25 @@ bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::
   auto param_listener = online_signal_smoothing::ParamListener(node_);
   params_ = param_listener.get_params();
 
-  // Ruckig needs the joint vel/accel/jerk bounds.
+  // Ruckig needs the joint vel/accel bounds, jerk bounds are optional
   getVelAccelJerkBounds();
   ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input(num_joints_);
   ruckig_input.max_velocity = joint_velocity_bounds_;
   ruckig_input.max_acceleration = joint_acceleration_bounds_;
-  ruckig_input.max_jerk = joint_jerk_bounds_;
   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);
+
+  // These quantities can be omitted if jerk limits are not applied
+  if (joint_jerk_bounds_)
+  {
+    ruckig_input.max_jerk = *joint_jerk_bounds_;
+  }
+  else
+  {
+    ruckig_input.max_jerk = std::vector<double>(num_joints_, 0.0);
+  }
+
   ruckig_input_ = ruckig_input;
 
   ruckig_output_.emplace(ruckig::OutputParameter<ruckig::DynamicDOFs>(num_joints_));
@@ -83,14 +92,33 @@ 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_)
+  if (have_initial_ruckig_output_)
   {
-    ruckig_output_->pass_to_input(*ruckig_input_);
+    if (joint_jerk_bounds_)
+    {
+      ruckig_output_->pass_to_input(*ruckig_input_);
+    }
+    else
+    {
+      ruckig_input_->current_position = ruckig_output_->new_position;
+      ruckig_input_->current_velocity = ruckig_output_->new_velocity;
+    }
+  }
+  else
+  {
+    // TODO
+    ;
   }
 
   // Update Ruckig target state
   // This assumes stationary at the target (zero vel, zero accel)
   ruckig_input_->target_position = std::vector<double>(positions.data(), positions.data() + positions.size());
+  ruckig_input_->target_velocity = std::vector<double>(num_joints_, 0);
+  ruckig_input_->target_acceleration = std::vector<double>(num_joints_, 0);
+  if (!joint_jerk_bounds_)
+  {
+    ruckig_input_->current_acceleration = std::vector<double>(num_joints_, 0.0);
+  }
 
   // Call the Ruckig algorithm
   ruckig::Result ruckig_result = ruckig_->update(*ruckig_input_, *ruckig_output_);
@@ -129,16 +157,21 @@ bool RuckigFilterPlugin::reset(const Eigen::VectorXd& positions, const Eigen::Ve
   // Initialize Ruckig
   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());
+  if (joint_jerk_bounds_)
+  {
+    ruckig_input_->current_acceleration =
+        std::vector<double>(accelerations.data(), accelerations.data() + accelerations.size());
+  }
 
   have_initial_ruckig_output_ = false;
   return true;
 }
 
-void RuckigFilterPlugin::getVelAccelJerkBounds()
+bool RuckigFilterPlugin::getVelAccelJerkBounds()
 {
   auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name);
+  bool have_all_jerk_bounds = true;
+  std::vector<double> jerk_bounds;
   for (const auto& joint : joint_model_group->getActiveJointModels())
   {
     const auto& bound = joint->getVariableBounds(joint->getName());
@@ -169,17 +202,26 @@ void RuckigFilterPlugin::getVelAccelJerkBounds()
     if (bound.jerk_bounded_)
     {
       // Assume symmetric limits
-      joint_jerk_bounds_.push_back(bound.max_jerk_);
+      jerk_bounds.push_back(bound.max_jerk_);
     }
+    // else, joint_jerk_bounds_ will be a std::nullopt and Ruckig doesn't apply jerk limits
     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);
+      have_all_jerk_bounds = false;
     }
   }
 
-  assert(joint_jerk_bounds_.size() == num_joints_);
+  if (!have_all_jerk_bounds)
+  {
+    joint_jerk_bounds_ = std::nullopt;
+    RCLCPP_WARN(getLogger(), "No joint jerk limit was defined. The output from Ruckig will not be jerk-limited.");
+  }
+  else
+  {
+    joint_jerk_bounds_ = jerk_bounds;
+  }
+
+  return true;
 }
 
 void RuckigFilterPlugin::printRuckigState()

moveit_ros/moveit_servo/src/servo.cpp

@@ -646,14 +646,17 @@ std::optional<PoseCommand> Servo::toPlanningFrame(const PoseCommand& command, co
 
 KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const
 {
-  bool have_current_state = false;
-  while (rclcpp::ok() && !have_current_state)
+  if (block_for_current_state)
   {
-    have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState(
-        rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME /* s */);
-    if (!have_current_state)
+    bool have_current_state = false;
+    while (rclcpp::ok() && !have_current_state)
     {
-      RCLCPP_WARN(logger_, "Waiting for the current state");
+      have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState(
+          rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME /* s */);
+      if (!have_current_state)
+      {
+        RCLCPP_WARN(logger_, "Waiting for the current state");
+      }
     }
   }
   moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();