Debug Ruckig JointLimiter.

Debug and optimize Rucking JointLimiter.

Author: destogl

joint_limits/include/joint_limits/joint_limiter_interface.hpp

@@ -55,6 +55,7 @@ class JointLimiterInterface
   JOINT_LIMITS_PUBLIC virtual bool configure(
     const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states)
   {
+    // TODO(destogl): add checks for position
     return on_configure(current_joint_states);
   }
 

joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp

@@ -18,9 +18,11 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "joint_limits/joint_limits_rosparam.hpp"
 #include "rcutils/logging_macros.h"
+#include "ruckig/brake.hpp"
 #include "ruckig/input_parameter.hpp"
 #include "ruckig/output_parameter.hpp"
 #include "ruckig/ruckig.hpp"
@@ -34,34 +36,48 @@ RuckigJointLimiter<joint_limits::JointLimits>::RuckigJointLimiter()
 }
 
 template <>
-bool RuckigJointLimiter<joint_limits::JointLimits>::on_init()
+bool RuckigJointLimiter<joint_limits::JointLimits>::on_init(/*const rclcpp::Duration & dt*/)
 {
+  // TODO(destogl): This should be used from parameter
+  const rclcpp::Duration dt = rclcpp::Duration::from_seconds(0.005);
+
   // Initialize Ruckig
-  ruckig_ = std::make_shared<ruckig::Ruckig<0>>(number_of_joints_, 0.01 /*timestep*/);
+  ruckig_ = std::make_shared<ruckig::Ruckig<0>>(number_of_joints_, dt.seconds());
   ruckig_input_ = std::make_shared<ruckig::InputParameter<0>>(number_of_joints_);
   ruckig_output_ = std::make_shared<ruckig::OutputParameter<0>>(number_of_joints_);
 
   // Velocity mode works best for smoothing one waypoint at a time
   ruckig_input_->control_interface = ruckig::ControlInterface::Velocity;
+  ruckig_input_->synchronization = ruckig::Synchronization::Time;
 
   for (auto joint = 0ul; joint < number_of_joints_; ++joint)
   {
     if (joint_limits_[joint].has_jerk_limits)
     {
       ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration;
     }
+    else
+    {
+      ruckig_input_->max_jerk.at(joint) = DEFAULT_MAX_JERK;
+    }
     if (joint_limits_[joint].has_acceleration_limits)
     {
       ruckig_input_->max_acceleration.at(joint) = joint_limits_[joint].max_acceleration;
     }
+    else
+    {
+      ruckig_input_->max_acceleration.at(joint) = DEFAULT_MAX_ACCELERATION;
+    }
     if (joint_limits_[joint].has_velocity_limits)
     {
       ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity;
     }
+    else
+    {
+      ruckig_input_->max_velocity.at(joint) = DEFAULT_MAX_VELOCITY;
+    }
   }
 
-  RCUTILS_LOG_INFO_NAMED("ruckig_joint_limiter", "Successfully initialized.");
-
   return true;
 }
 
@@ -70,15 +86,24 @@ bool RuckigJointLimiter<joint_limits::JointLimits>::on_configure(
   const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states)
 {
   // Initialize Ruckig with current_joint_states
-  std::copy_n(
-    current_joint_states.positions.begin(), number_of_joints_,
-    ruckig_input_->current_position.begin());
-  std::copy_n(
-    current_joint_states.velocities.begin(), number_of_joints_,
-    ruckig_input_->current_velocity.begin());
-  std::copy_n(
-    current_joint_states.accelerations.begin(), number_of_joints_,
-    ruckig_input_->current_acceleration.begin());
+  ruckig_input_->current_position = current_joint_states.positions;
+
+  if (current_joint_states.velocities.size() == number_of_joints_)
+  {
+    ruckig_input_->current_velocity = current_joint_states.velocities;
+  }
+  else
+  {
+    ruckig_input_->current_velocity = std::vector<double>(number_of_joints_, 0.0);
+  }
+  if (current_joint_states.accelerations.size() == number_of_joints_)
+  {
+    ruckig_input_->current_acceleration = current_joint_states.accelerations;
+  }
+  else
+  {
+    ruckig_input_->current_acceleration = std::vector<double>(number_of_joints_, 0.0);
+  }
 
   // Initialize output data
   ruckig_output_->new_position = ruckig_input_->current_position;
@@ -96,20 +121,54 @@ bool RuckigJointLimiter<joint_limits::JointLimits>::on_enforce(
 {
   // We don't use current_joint_states. For stability, it is recommended to feed previous Ruckig
   // output back in as input for the next iteration. This assumes the robot tracks the command well.
+  ruckig_input_->current_position = ruckig_output_->new_position;
+  ruckig_input_->current_velocity = ruckig_output_->new_velocity;
+  ruckig_input_->current_acceleration = ruckig_output_->new_acceleration;
+
+  // Target state is the next waypoint
+  ruckig_input_->target_position = desired_joint_states.positions;
+  // TODO(destogl): in current use-case we use only velocity
+  if (desired_joint_states.velocities.size() == number_of_joints_)
+  {
+    ruckig_input_->target_velocity = desired_joint_states.velocities;
+  }
+  else
+  {
+    ruckig_input_->target_velocity = std::vector<double>(number_of_joints_, 0.0);
+  }
+  if (desired_joint_states.accelerations.size() == number_of_joints_)
+  {
+    ruckig_input_->target_acceleration = desired_joint_states.accelerations;
+  }
+  else
+  {
+    ruckig_input_->target_acceleration = std::vector<double>(number_of_joints_, 0.0);
+  }
+
+  ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_);
+  RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result);
+
+  desired_joint_states.positions = ruckig_output_->new_position;
+  desired_joint_states.velocities = ruckig_output_->new_velocity;
+  desired_joint_states.accelerations = ruckig_output_->new_acceleration;
 
   // Feed output from the previous timestep back as input
   for (auto joint = 0ul; joint < number_of_joints_; ++joint)
   {
-    ruckig_input_->current_position.at(joint) = ruckig_output_->new_position.at(joint);
-    ruckig_input_->current_velocity.at(joint) = ruckig_output_->new_velocity.at(joint);
-    ruckig_input_->current_acceleration.at(joint) = ruckig_output_->new_acceleration.at(joint);
-
-    // Target state is the next waypoint
-    ruckig_input_->target_velocity.at(joint) = desired_joint_states.velocities.at(joint);
-    ruckig_input_->target_acceleration.at(joint) = desired_joint_states.accelerations.at(joint);
+    RCUTILS_LOG_DEBUG_NAMED(
+      "ruckig_joint_limiter",
+      "Desired position: %e \nDesired velocity: %e\n Desired acceleration: %e.",
+      ruckig_input_->target_position.at(joint), ruckig_input_->target_velocity.at(joint),
+      ruckig_input_->target_acceleration.at(joint));
   }
 
-  ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_);
+  for (auto joint = 0ul; joint < number_of_joints_; ++joint)
+  {
+    RCUTILS_LOG_DEBUG_NAMED(
+      "ruckig_joint_limiter", "New position: %e \nNew velocity: %e\nNew acceleration: %e.",
+      ruckig_output_->new_position.at(joint), ruckig_output_->new_velocity.at(joint),
+      ruckig_output_->new_acceleration.at(joint));
+  }
 
   return result == ruckig::Result::Finished;
 }