From c8a26b66d3e520f6268c9f5bf11cb9f27233fcf2 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 7 Aug 2024 17:08:35 -0500 Subject: [PATCH] Implement jerk-limited smoothing for Servo --- .../online_signal_smoothing/ruckig_filter.h | 7 +- .../src/ruckig_filter.cpp | 69 +++++++++++++++++-- .../include/moveit_servo/servo.hpp | 1 + moveit_ros/moveit_servo/src/servo.cpp | 31 ++++++--- moveit_ros/moveit_servo/src/servo_node.cpp | 4 +- moveit_ros/moveit_servo/src/utils/common.cpp | 8 +++ 6 files changed, 103 insertions(+), 17 deletions(-) diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h index 047afc1e60d..dd487270dd0 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h +++ b/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_; std::optional> ruckig_input_; std::optional> ruckig_output_; diff --git a/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp index b546fe94337..3a72be35ff0 100644 --- a/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp +++ b/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(num_joints_, 0.0); + ruckig_input.current_velocity = std::vector(num_joints_, 0.0); + ruckig_input.current_acceleration = std::vector(num_joints_, 0.0); ruckig_input_ = ruckig_input; ruckig_output_.emplace(ruckig::OutputParameter(num_joints_)); @@ -82,12 +83,56 @@ 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_); + } + + // Update Ruckig target state + // This assumes stationary at the target (zero vel, zero accel) + ruckig_input_->target_position = std::vector(positions.data(), positions.data() + positions.size()); + + // Call the Ruckig algorithm + 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(ruckig_output_->new_position.data(), + ruckig_output_->new_position.size()); + velocities = Eigen::Map(ruckig_output_->new_velocity.data(), + ruckig_output_->new_velocity.size()); + accelerations = Eigen::Map(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 + ruckig_input_->current_position = std::vector(positions.data(), positions.data() + positions.size()); + ruckig_input_->current_velocity = std::vector(velocities.data(), velocities.data() + velocities.size()); + ruckig_input_->current_acceleration = + std::vector(accelerations.data(), accelerations.data() + accelerations.size()); + + have_initial_ruckig_output_ = false; return true; } @@ -121,12 +166,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 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index d50c831f987..a1859aac36e 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -118,6 +118,7 @@ class Servo /** * \brief Get the current state of the robot as given by planning scene monitor. + * This may block if a current robot state is not available immediately. * @return The current state of the robot. */ KinematicState getCurrentRobotState() const; diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index d0ac00652ab..50078eca54f 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -79,16 +79,6 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptrgetStateMonitor()->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(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_)); @@ -125,6 +115,17 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr(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,16 @@ std::optional Servo::toPlanningFrame(const PoseCommand& command, co KinematicState Servo::getCurrentRobotState() const { + bool have_current_state = false; + while (rclcpp::ok() && !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); } diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index f5f7e137fb9..6ff98d530a9 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -298,7 +298,7 @@ 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(); @@ -383,7 +383,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(servo_->getStatus()); diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index 66b0866819c..d659a2f2d0d 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -499,7 +499,15 @@ KinematicState extractRobotState(const moveit::core::RobotStatePtr& robot_state, 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); })) + { + robot_state->zeroAccelerations(); + robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations); + } return current_state; }