forked from moveit/moveit2
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add jerk-limited online smoothing plugin
- Loading branch information
Showing
9 changed files
with
292 additions
and
22 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,8 @@ | ||
| <library path="moveit_acceleration_filter"> | ||
| <class type="online_signal_smoothing::RuckigFilterPlugin" | ||
| base_class_type="online_signal_smoothing::SmoothingBaseClass"> | ||
| <description> | ||
| Jerk/acceleration/velocity-limited command smoothing. | ||
| </description> | ||
| </class> | ||
| </library> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
102 changes: 102 additions & 0 deletions
102
moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,102 @@ | ||
| /********************************************************************* | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2024, Andrew Zelenak | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of PickNik Inc. nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| *********************************************************************/ | ||
|
|
||
| /* Author: Andy Zelenak | ||
| Description: Applies jerk/acceleration/velocity limits to online motion commands | ||
| */ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <cstddef> | ||
|
|
||
| #include <moveit/robot_model/robot_model.h> | ||
| #include <moveit/online_signal_smoothing/smoothing_base_class.h> | ||
| #include <moveit_ruckig_filter_parameters.hpp> | ||
|
|
||
| #include <ruckig/ruckig.hpp> | ||
|
|
||
| namespace online_signal_smoothing | ||
| { | ||
|
|
||
| class RuckigFilterPlugin : public SmoothingBaseClass | ||
| { | ||
| public: | ||
| /** | ||
| * Initialize the smoothing algorithm | ||
| * @param node ROS node, used for parameter retrieval | ||
| * @param robot_model used to retrieve vel/accel/jerk limits | ||
| * @param num_joints number of actuated joints in the JointGroup | ||
| * @return True if initialization was successful | ||
| */ | ||
| bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, | ||
| size_t num_joints) override; | ||
|
|
||
| /** | ||
| * Smooth the command signals for all DOF | ||
| * @param positions array of joint position commands | ||
| * @param velocities array of joint velocity commands | ||
| * @param accelerations array of joint acceleration commands | ||
| * @return True if initialization was successful | ||
| */ | ||
| bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; | ||
|
|
||
| /** | ||
| * Reset to a given joint state | ||
| * @param positions reset the filters to these joint positions | ||
| * @param velocities (unused) | ||
| * @param accelerations (unused) | ||
| * @return True if reset was successful | ||
| */ | ||
| bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, | ||
| const Eigen::VectorXd& accelerations) override; | ||
|
|
||
| private: | ||
| rclcpp::Node::SharedPtr node_; | ||
| void getDefaultJointVelAccelBounds(); | ||
|
|
||
| online_signal_smoothing::Params params_; | ||
|
|
||
| size_t num_joints_; | ||
| moveit::core::RobotModelConstPtr robot_model_; | ||
|
|
||
| bool have_initial_ruckig_output_ = false; | ||
| std::optional<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_; | ||
| std::optional<ruckig::InputParameter<ruckig::DynamicDOFs>> ruckig_input_; | ||
| std::optional<ruckig::OutputParameter<ruckig::DynamicDOFs>> ruckig_output_; | ||
|
|
||
| std::vector<double> default_joint_velocity_bounds_; | ||
| std::vector<double> default_joint_acceleration_bounds_; | ||
| std::vector<double> default_joint_jerk_bounds_; | ||
| }; | ||
| } // namespace online_signal_smoothing |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
131 changes: 131 additions & 0 deletions
131
moveit_core/online_signal_smoothing/src/ruckig_filter.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,131 @@ | ||
| /********************************************************************* | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Copyright (c) 2024, Andrew Zelenak | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of PickNik Inc. nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| *********************************************************************/ | ||
|
|
||
| #include <moveit/online_signal_smoothing/ruckig_filter.h> | ||
| #include <rclcpp/clock.hpp> | ||
| #include <rclcpp/logging.hpp> | ||
|
|
||
| // Disable -Wold-style-cast because all _THROTTLE macros trigger this | ||
| #pragma GCC diagnostic ignored "-Wold-style-cast" | ||
|
|
||
| namespace online_signal_smoothing | ||
| { | ||
| namespace | ||
| { | ||
| 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, | ||
| size_t num_joints) | ||
| { | ||
| node_ = node; | ||
| num_joints_ = num_joints; | ||
| robot_model_ = robot_model; | ||
|
|
||
| // get node parameters and store in member variables | ||
| auto param_listener = online_signal_smoothing::ParamListener(node_); | ||
| params_ = param_listener.get_params(); | ||
|
|
||
| // Ruckig needs to know the joint vel/accel/jerk bounds. | ||
| getDefaultJointVelAccelBounds(); | ||
| ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input(num_joints_); | ||
| ruckig_input.max_velocity = default_joint_velocity_bounds_; | ||
| ruckig_input.max_acceleration = default_joint_acceleration_bounds_; | ||
| ruckig_input.max_jerk = default_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_ = | ||
|
|
||
| ruckig_.emplace(ruckig::Ruckig<ruckig::DynamicDOFs>(num_joints_, params_.update_period)); | ||
|
|
||
| return ruckig_.has_value(); | ||
| } | ||
|
|
||
| bool RuckigFilterPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, | ||
| Eigen::VectorXd& accelerations) | ||
| { | ||
| return true; | ||
| } | ||
|
|
||
| bool RuckigFilterPlugin::reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, | ||
| const Eigen::VectorXd& accelerations) | ||
| { | ||
| return true; | ||
| } | ||
|
|
||
| void RuckigFilterPlugin::getDefaultJointVelAccelBounds() | ||
| { | ||
| auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name); | ||
| for (const auto& joint : joint_model_group->getActiveJointModels()) | ||
| { | ||
| const auto& bound = joint->getVariableBounds(joint->getName()); | ||
|
|
||
| if (bound.velocity_bounded_) | ||
| { | ||
| // Assume symmetric limits | ||
| default_joint_velocity_bounds_.push_back(bound.max_velocity_); | ||
| } | ||
| else | ||
| { | ||
| RCLCPP_ERROR(getLogger(), "No joint vel limit defined. Exiting for safety."); | ||
| std::exit(EXIT_FAILURE); | ||
| } | ||
|
|
||
| if (bound.acceleration_bounded_) | ||
| { | ||
| // Assume symmetric limits | ||
| default_joint_acceleration_bounds_.push_back(bound.max_acceleration_); | ||
| } | ||
| else | ||
| { | ||
| RCLCPP_ERROR(getLogger(), "WARNING: No joint accel limit defined. Very large accelerations will be " | ||
| "possible."); | ||
| default_joint_acceleration_bounds_.push_back(DBL_MAX); | ||
| } | ||
|
|
||
| // MoveIt and the URDF don't support jerk limits, so use a safe default always | ||
| default_joint_jerk_bounds_.push_back(DEFAULT_JERK_BOUND); | ||
| } | ||
|
|
||
| assert(default_joint_jerk_bounds_.size() == num_joints_); | ||
| } | ||
| } // namespace online_signal_smoothing | ||
|
|
||
| #include <pluginlib/class_list_macros.hpp> | ||
| PLUGINLIB_EXPORT_CLASS(online_signal_smoothing::RuckigFilterPlugin, online_signal_smoothing::SmoothingBaseClass) |
16 changes: 16 additions & 0 deletions
16
moveit_core/online_signal_smoothing/src/ruckig_filter_parameters.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,16 @@ | ||
| online_signal_smoothing: | ||
| update_period: { | ||
| type: double, | ||
| description: "The expected time in seconds between calls to `doSmoothing` method.", | ||
| read_only: true, | ||
| validation: { | ||
| gt<>: 0.0 | ||
| } | ||
| } | ||
| planning_group_name: { | ||
| type: string, | ||
| read_only: true, | ||
| description: "The name of the MoveIt planning group of the robot \ | ||
| This parameter does not have a default value and \ | ||
| must be passed to the node during launch time." | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters