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
265 additions
and
20 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
101 changes: 101 additions & 0 deletions
101
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,101 @@ | ||
| /********************************************************************* | ||
| * 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 typically used to retrieve vel/accel/jerk limits | ||
| * @param num_joints number of actuated joints in the JointGroup Servo controls | ||
| * @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: | ||
| void getDefaultJointVelAccelBounds(); | ||
|
|
||
| online_signal_smoothing::Params params_; | ||
|
|
||
| size_t num_joints_; | ||
| moveit::core::RobotModelConstPtr robot_model_; | ||
|
|
||
| bool have_initial_ruckig_output_ = false; | ||
| ruckig::Ruckig<ruckig::DynamicDOFs> ruckig_; | ||
| ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input_; | ||
| 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
117 changes: 117 additions & 0 deletions
117
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,117 @@ | ||
| /********************************************************************* | ||
| * 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 | ||
| { | ||
| bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, | ||
| size_t num_joints) | ||
| { | ||
| // get node parameters and store in member variables | ||
| auto param_listener = online_signal_smoothing::ParamListener(node_); | ||
| params_ = param_listener.get_params(); | ||
|
|
||
| num_joints_ = num_joints; | ||
| robot_model_ = robot_model; | ||
|
|
||
| // Ruckig needs to know the joint vel/accel/jerk bounds. | ||
| getDefaultJointVelAccelBounds(); | ||
| 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); | ||
|
|
||
| return true; | ||
| } | ||
|
|
||
| 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 | ||
| { | ||
| ROS_ERROR_STREAM("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 | ||
| { | ||
| ROS_ERROR_STREAM("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) |
8 changes: 8 additions & 0 deletions
8
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,8 @@ | ||
| online_signal_smoothing: | ||
| 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