diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 3306fd0063..93435c5069 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -31,8 +31,10 @@ jobs: - IMAGE: jazzy-ci ROS_DISTRO: jazzy env: + # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option + # https://stackoverflow.com/a/41673702 CXXFLAGS: >- - -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls + -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: > diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 90b2851ded..608babbebc 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -92,6 +92,8 @@ install( moveit_robot_model moveit_robot_state moveit_robot_trajectory + moveit_ruckig_filter + moveit_ruckig_filter_parameters moveit_smoothing_base moveit_test_utils moveit_trajectory_processing @@ -142,9 +144,10 @@ pluginlib_export_plugin_description_file(moveit_core collision_detector_fcl_description.xml) pluginlib_export_plugin_description_file( moveit_core collision_detector_bullet_description.xml) -pluginlib_export_plugin_description_file(moveit_core - filter_plugin_butterworth.xml) pluginlib_export_plugin_description_file(moveit_core filter_plugin_acceleration.xml) +pluginlib_export_plugin_description_file(moveit_core + filter_plugin_butterworth.xml) +pluginlib_export_plugin_description_file(moveit_core filter_plugin_ruckig.xml) ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_core/filter_plugin_ruckig.xml b/moveit_core/filter_plugin_ruckig.xml new file mode 100644 index 0000000000..47ffd347a1 --- /dev/null +++ b/moveit_core/filter_plugin_ruckig.xml @@ -0,0 +1,8 @@ + + + + Jerk/acceleration/velocity-limited command smoothing. + + + diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt index bcfca1c75e..1f1912c6d9 100644 --- a/moveit_core/online_signal_smoothing/CMakeLists.txt +++ b/moveit_core/online_signal_smoothing/CMakeLists.txt @@ -13,6 +13,23 @@ set_target_properties(moveit_smoothing_base ament_target_dependencies(moveit_smoothing_base rclcpp tf2_eigen) # Plugin implementations +add_library(moveit_acceleration_filter SHARED src/acceleration_filter.cpp) +generate_export_header(moveit_acceleration_filter) +target_include_directories( + moveit_acceleration_filter + PUBLIC $) +set_target_properties(moveit_acceleration_filter + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +generate_parameter_library(moveit_acceleration_filter_parameters + src/acceleration_filter_parameters.yaml) +target_link_libraries( + moveit_acceleration_filter moveit_acceleration_filter_parameters + moveit_robot_state moveit_smoothing_base osqp::osqp) +ament_target_dependencies( + moveit_acceleration_filter srdfdom # include dependency from + # moveit_robot_model + pluginlib) + add_library(moveit_butterworth_filter SHARED src/butterworth_filter.cpp) generate_export_header(moveit_butterworth_filter) target_include_directories( @@ -20,10 +37,8 @@ target_include_directories( PUBLIC $) set_target_properties(moveit_butterworth_filter PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - generate_parameter_library(moveit_butterworth_filter_parameters src/butterworth_parameters.yaml) - target_link_libraries( moveit_butterworth_filter moveit_butterworth_filter_parameters moveit_robot_model moveit_smoothing_base) @@ -32,32 +47,30 @@ ament_target_dependencies( srdfdom # include dependency from moveit_robot_model pluginlib) -add_library(moveit_acceleration_filter SHARED src/acceleration_filter.cpp) -generate_export_header(moveit_acceleration_filter) +add_library(moveit_ruckig_filter SHARED src/ruckig_filter.cpp) +generate_export_header(moveit_ruckig_filter) target_include_directories( - moveit_acceleration_filter - PUBLIC $) -set_target_properties(moveit_acceleration_filter + moveit_ruckig_filter PUBLIC $) +set_target_properties(moveit_ruckig_filter PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - -generate_parameter_library(moveit_acceleration_filter_parameters - src/acceleration_filter_parameters.yaml) - +generate_parameter_library(moveit_ruckig_filter_parameters + src/ruckig_filter_parameters.yaml) target_link_libraries( - moveit_acceleration_filter moveit_acceleration_filter_parameters - moveit_robot_state moveit_smoothing_base osqp::osqp) + moveit_ruckig_filter moveit_robot_state moveit_ruckig_filter_parameters + moveit_smoothing_base ruckig::ruckig) ament_target_dependencies( - moveit_acceleration_filter srdfdom # include dependency from - # moveit_robot_model + moveit_ruckig_filter srdfdom # include dependency from moveit_robot_model pluginlib) # Installation install(DIRECTORY include/ DESTINATION include/moveit_core) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_smoothing_base_export.h DESTINATION include/moveit_core) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_acceleration_filter_export.h + DESTINATION include/moveit_core) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_butterworth_filter_export.h DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_acceleration_filter_export.h +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ruckig_filter_export.h DESTINATION include/moveit_core) # Testing 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 new file mode 100644 index 0000000000..13cf36a742 --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h @@ -0,0 +1,106 @@ +/********************************************************************* + * 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 + +#include +#include +#include + +#include + +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: + /** + * A utility to print Ruckig's internal state + */ + void printRuckigState(); + + /** + * A utility to get velocity/acceleration/jerk bounds from the robot model + * @return true if all bounds are defined + */ + bool getVelAccelJerkBounds(std::vector& joint_velocity_bounds, std::vector& joint_acceleration_bounds, + std::vector& joint_jerk_bounds); + + /** \brief Parameters loaded from yaml file at runtime */ + online_signal_smoothing::Params params_; + /** \brief The robot model contains the vel/accel/jerk limits that Ruckig requires */ + moveit::core::RobotModelConstPtr robot_model_; + bool have_initial_ruckig_output_ = false; + std::optional> ruckig_; + std::optional> ruckig_input_; + std::optional> ruckig_output_; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h index e870e493f0..e2dc886aaf 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h @@ -89,6 +89,5 @@ class SmoothingBaseClass */ virtual bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, const Eigen::VectorXd& accelerations) = 0; - ; }; } // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp index 4bfc69ed14..5e47aaec69 100644 --- a/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp +++ b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp @@ -34,7 +34,6 @@ #include #include -#include // Disable -Wold-style-cast because all _THROTTLE macros trigger this #pragma GCC diagnostic ignored "-Wold-style-cast" @@ -345,5 +344,4 @@ bool AccelerationLimitedPlugin::reset(const Eigen::VectorXd& positions, const Ei } // namespace online_signal_smoothing #include - PLUGINLIB_EXPORT_CLASS(online_signal_smoothing::AccelerationLimitedPlugin, online_signal_smoothing::SmoothingBaseClass) diff --git a/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp new file mode 100644 index 0000000000..b689620761 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp @@ -0,0 +1,219 @@ +/********************************************************************* + * 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 +#include +#include + +// 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"); +} +} // namespace + +bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t 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 the joint vel/accel bounds + // TODO: Ruckig says the jerk bounds can be optional. We require them, for now. + ruckig::InputParameter ruckig_input(num_joints); + if (!getVelAccelJerkBounds(ruckig_input.max_velocity, ruckig_input.max_acceleration, ruckig_input.max_jerk)) + { + return false; + } + 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.synchronization = ruckig::Synchronization::Phase; + + ruckig_input_ = ruckig_input; + + ruckig_output_.emplace(ruckig::OutputParameter(num_joints)); + + ruckig_.emplace(ruckig::Ruckig(num_joints, params_.update_period)); + + return true; +} + +bool RuckigFilterPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, + Eigen::VectorXd& accelerations) +{ + if (!ruckig_input_ || !ruckig_output_ || !ruckig_) + { + RCLCPP_ERROR_STREAM(getLogger(), "The Ruckig smoother was not initialized"); + return false; + } + + if (have_initial_ruckig_output_) + { + ruckig_output_->pass_to_input(*ruckig_input_); + } + + // Update Ruckig target state + ruckig_input_->target_position = std::vector(positions.data(), positions.data() + positions.size()); + // We don't know what the next command will be. Assume velocity continues forward based on current state, + // target_acceleration is zero. + const size_t num_joints = ruckig_input_->current_acceleration.size(); + for (size_t i = 0; i < num_joints; ++i) + { + ruckig_input_->target_velocity.at(i) = + ruckig_input_->current_velocity.at(i) + ruckig_input_->current_acceleration.at(i) * params_.update_period; + } + // target_acceleration remains a vector of zeroes + + // 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; +} + +bool RuckigFilterPlugin::getVelAccelJerkBounds(std::vector& joint_velocity_bounds, + std::vector& joint_acceleration_bounds, + std::vector& joint_jerk_bounds) +{ + if (!robot_model_) + { + RCLCPP_ERROR(getLogger(), "The robot model was not initialized."); + return false; + } + + joint_velocity_bounds.clear(); + joint_acceleration_bounds.clear(); + joint_jerk_bounds.clear(); + + 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 + joint_velocity_bounds.push_back(bound.max_velocity_); + } + else + { + RCLCPP_ERROR_STREAM(getLogger(), "No joint velocity limit defined for " << joint->getName() << "."); + return false; + } + + if (bound.acceleration_bounded_) + { + // Assume symmetric limits + joint_acceleration_bounds.push_back(bound.max_acceleration_); + } + else + { + RCLCPP_WARN_STREAM(getLogger(), "No joint acceleration limit defined for " << joint->getName() << "."); + return false; + } + + if (bound.jerk_bounded_) + { + // Assume symmetric limits + joint_jerk_bounds.push_back(bound.max_jerk_); + } + // else, return false + else + { + RCLCPP_WARN_STREAM(getLogger(), "No joint jerk limit was defined for " + << joint->getName() << ". The output from Ruckig will not be jerk-limited."); + return false; + } + } + + return true; +} + +void RuckigFilterPlugin::printRuckigState() +{ + RCLCPP_INFO_STREAM(getLogger(), ruckig_->delta_time << "\nRuckig input:\n" + << ruckig_input_->to_string() << "\nRuckig output:\n" + << ruckig_output_->to_string()); +} +} // namespace online_signal_smoothing + +#include +PLUGINLIB_EXPORT_CLASS(online_signal_smoothing::RuckigFilterPlugin, online_signal_smoothing::SmoothingBaseClass) diff --git a/moveit_core/online_signal_smoothing/src/ruckig_filter_parameters.yaml b/moveit_core/online_signal_smoothing/src/ruckig_filter_parameters.yaml new file mode 100644 index 0000000000..92575e5059 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/ruckig_filter_parameters.yaml @@ -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." + } diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index d1af7d41bf..089eba6b8f 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -44,6 +44,14 @@ namespace moveit { namespace core { +/** Struct defining linear and rotational precision */ +struct CartesianPrecision +{ + double translational = 0.001; //< max deviation in translation (meters) + double rotational = 0.01; //< max deviation in rotation (radians) + double max_resolution = 1e-5; //< max resolution for waypoints (fraction of total path) +}; + /** \brief Struct with options for defining joint-space jump thresholds. */ struct JumpThreshold { @@ -144,6 +152,75 @@ class CartesianInterpolator double meters; }; + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. + + The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. + This vector is assumed to be specified either in the global reference frame or in the local + reference frame of the link. + The resulting joint values are stored in the vector \e traj, one by one. The interpolation distance in + Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which + provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal + call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to + the distance that was achieved and for which corresponding states were added to the path. + + The struct CartesianPrecision specifies the precision to which the path should follow the Cartesian straight line. + If the deviation at the mid point of two consecutive waypoints is larger than the specified precision, another waypoint + will be inserted at that mid point. The precision is specified separately for translation and rotation. + The maximal resolution to consider (as fraction of the total path length) is specified by max_resolution. + */ + static Distance computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link. + + In contrast to the previous function, the translation vector is specified as a (unit) direction vector and + a distance. */ + static Distance computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) + { + return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, + precision, validCallback, options, cost_function); + } + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. + + In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) + for a virtual frame attached to the robot \e link with the given \e link_offset. + The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame. + This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */ + static Percentage computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + + /** \brief Compute the sequence of joint values that perform a general Cartesian path. + + In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially + reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global + reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs + to move in a straight line between two consecutive waypoints. All other comments apply. */ + static Percentage computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. @@ -158,12 +235,19 @@ class CartesianInterpolator During the computation of the path, it is usually preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. - To account for this, the \e jump_threshold argument is provided. If a jump detection is enabled and a jump is - found, the path is truncated up to just before the jump. + To account for this, the \e jump_threshold struct is provided, which comprises three fields: + \e jump_threshold_factor, \e revolute_jump_threshold and \e prismatic_jump_threshold. + If either \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. + If \e jump_threshold_factor is non-zero, we test for relative jumps. To this end, the average joint-space distance + between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger than + this average distance multiplied by \e jump_threshold_factor, this step is considered a jump. + + Otherwise (if all params are zero), jump detection is disabled. + If a jump is detected, the path is truncated up to just before the jump. Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e cost_function. */ - static Distance computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, @@ -175,7 +259,7 @@ class CartesianInterpolator In contrast to the previous function, the translation vector is specified as a (unit) direction vector and a distance. */ - static Distance computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, @@ -183,8 +267,11 @@ class CartesianInterpolator const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" return computeCartesianPath(start_state, group, path, link, distance * direction, global_reference_frame, max_step, jump_threshold, validCallback, options, cost_function); +#pragma GCC diagnostic pop } /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. @@ -194,7 +281,7 @@ class CartesianInterpolator The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame (\e global_reference_frame is false). This function returns the percentage (0..1) of the path that was achieved. All other comments from the previous function apply. */ - static Percentage computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, @@ -209,7 +296,7 @@ class CartesianInterpolator reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs to move in a straight line between two consecutive waypoints. All other comments apply. */ - static Percentage computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 02707c3481..995338badd 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ +/* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman, Robert Haschke */ #include #include @@ -60,6 +60,54 @@ rclcpp::Logger getLogger() return moveit::getLogger("moveit.core.cartesian_interpolator"); } +bool validateAndImproveInterval(const RobotState& start_state, const RobotState& end_state, + const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& end_pose, + std::vector& traj, double& percentage, const double width, + const JointModelGroup* group, const LinkModel* link, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, + const kinematics::KinematicsBase::IKCostFn& cost_function, + const Eigen::Isometry3d& link_offset) +{ + // compute pose at joint-space midpoint between start_state and end_state + RobotState mid_state(start_state.getRobotModel()); + start_state.interpolate(end_state, 0.5, mid_state); + Eigen::Isometry3d fk_pose = mid_state.getGlobalLinkTransform(link) * link_offset; + + // compute pose at Cartesian-space midpoint between start_pose and end_pose + Eigen::Isometry3d mid_pose(Eigen::Quaterniond(start_pose.linear()).slerp(0.5, Eigen::Quaterniond(end_pose.linear()))); + mid_pose.translation() = 0.5 * (start_pose.translation() + end_pose.translation()); + + // if deviation between both poses, fk_pose and mid_pose is within precision, we are satisfied + double linear_distance = (mid_pose.translation() - fk_pose.translation()).norm(); + double angular_distance = Eigen::Quaterniond(mid_pose.linear()).angularDistance(Eigen::Quaterniond(fk_pose.linear())); + if (linear_distance <= precision.translational && angular_distance <= precision.rotational) + { + traj.push_back(std::make_shared(end_state)); + return true; + } + + if (width < precision.max_resolution) + return false; // failed to find linear interpolation within max_resolution + + // otherwise subdivide interval further, computing IK for mid_pose + if (!mid_state.setFromIK(group, mid_pose * link_offset.inverse(), link->getName(), 0.0, validCallback, options, + cost_function)) + return false; + + // and recursively processing the two sub-intervals + const auto half_width = width / 2.0; + const auto old_percentage = percentage; + percentage = percentage - half_width; + if (!validateAndImproveInterval(start_state, mid_state, start_pose, mid_pose, traj, percentage, half_width, group, + link, precision, validCallback, options, cost_function, link_offset)) + return false; + + percentage = old_percentage; + return validateAndImproveInterval(mid_state, end_state, mid_pose, end_pose, traj, percentage, half_width, group, link, + precision, validCallback, options, cost_function, link_offset); +} + std::optional hasRelativeJointSpaceJump(const std::vector& waypoints, const moveit::core::JointModelGroup& group, double jump_threshold_factor) { @@ -136,6 +184,129 @@ std::optional hasAbsoluteJointSpaceJump(const std::vector& traj, + const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function) +{ + const double distance = translation.norm(); + // The target pose is obtained by adding the translation vector to the link's current pose + Eigen::Isometry3d pose = start_state->getGlobalLinkTransform(link); + + // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame) + pose.translation() += global_reference_frame ? translation : pose.linear() * translation; + + // call computeCartesianPath for the computed target pose in the global reference frame + return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, pose, true, + max_step, precision, validCallback, options, + cost_function); +} + +CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function, + const Eigen::Isometry3d& link_offset) +{ + // check unsanitized inputs for non-isometry + ASSERT_ISOMETRY(target) + ASSERT_ISOMETRY(link_offset) + + RobotState state(*start_state); + + const std::vector& cjnt = group->getContinuousJointModels(); + // make sure that continuous joints wrap + for (const JointModel* joint : cjnt) + state.enforceBounds(joint); + + // Cartesian pose we start from + Eigen::Isometry3d start_pose = state.getGlobalLinkTransform(link) * link_offset; + Eigen::Isometry3d inv_offset = link_offset.inverse(); + + // the target can be in the local reference frame (in which case we rotate it) + Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; + + Eigen::Quaterniond start_quaternion(start_pose.linear()); + Eigen::Quaterniond target_quaternion(rotated_target.linear()); + + double rotation_distance = start_quaternion.angularDistance(target_quaternion); + double translation_distance = (rotated_target.translation() - start_pose.translation()).norm(); + + // decide how many steps we will need for this trajectory + std::size_t translation_steps = 0; + if (max_step.translation > 0.0) + translation_steps = floor(translation_distance / max_step.translation); + + std::size_t rotation_steps = 0; + if (max_step.rotation > 0.0) + rotation_steps = floor(rotation_distance / max_step.rotation); + std::size_t steps = std::max(translation_steps, rotation_steps) + 1; + + traj.clear(); + traj.push_back(std::make_shared(*start_state)); + + double last_valid_percentage = 0.0; + Eigen::Isometry3d prev_pose = start_pose; + RobotState prev_state(state); + for (std::size_t i = 1; i <= steps; ++i) + { + double percentage = static_cast(i) / static_cast(steps); + + Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion)); + pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation(); + + if (!state.setFromIK(group, pose * inv_offset, link->getName(), 0.0, validCallback, options, cost_function) || + !validateAndImproveInterval(prev_state, state, prev_pose, pose, traj, percentage, + 1.0 / static_cast(steps), group, link, precision, validCallback, options, + cost_function, link_offset)) + break; + + prev_pose = pose; + prev_state = state; + last_valid_percentage = percentage; + } + + return last_valid_percentage; +} + +CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function, + const Eigen::Isometry3d& link_offset) +{ + double percentage_solved = 0.0; + for (std::size_t i = 0; i < waypoints.size(); ++i) + { + std::vector waypoint_traj; + double wp_percentage_solved = + computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, + precision, validCallback, options, cost_function, link_offset); + if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) + { + percentage_solved = static_cast(i + 1) / static_cast(waypoints.size()); + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + } + else + { + percentage_solved += wp_percentage_solved / static_cast(waypoints.size()); + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + break; + } + } + + return percentage_solved; +} + JumpThreshold JumpThreshold::disabled() { return JumpThreshold(); @@ -184,10 +355,13 @@ CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath( // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame) pose.translation() += global_reference_frame ? translation : pose.linear() * translation; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" // call computeCartesianPath for the computed target pose in the global reference frame return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, path, link, pose, true, max_step, jump_threshold, validCallback, options, cost_function); +#pragma GCC diagnostic pop } CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( @@ -310,9 +484,12 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( // Don't test joint space jumps for every waypoint, test them later on the whole path. static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST = JumpThreshold::disabled(); std::vector waypoint_path; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" double wp_percentage_solved = computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step, NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function, link_offset); +#pragma GCC diagnostic pop if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = static_cast((i + 1)) / static_cast(waypoints.size()); diff --git a/moveit_py/moveit/core/planning_scene.pyi b/moveit_py/moveit/core/planning_scene.pyi index e5f2363780..9cc633ddcb 100644 --- a/moveit_py/moveit/core/planning_scene.pyi +++ b/moveit_py/moveit/core/planning_scene.pyi @@ -19,6 +19,8 @@ class PlanningScene: def process_planning_scene_world(self, *args, **kwargs) -> Any: ... def remove_all_collision_objects(self, *args, **kwargs) -> Any: ... def set_object_color(self, *args, **kwargs) -> Any: ... + def save_geometry_to_file(self, file_name_and_path) -> Any: ... + def load_geometry_from_file(self, file_name_and_path) -> Any: ... def __copy__(self) -> Any: ... def __deepcopy__(self) -> Any: ... @property diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp index 5df1c18f30..e55b0f15d4 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -38,6 +38,33 @@ #include #include +#include + +namespace +{ +bool saveGeometryToFile(std::shared_ptr& planning_scene, + const std::string& file_path_and_name) +{ + std::ofstream file(file_path_and_name); + if (!file.is_open()) + { + return false; + } + planning_scene->saveGeometryToStream(file); + file.close(); + return true; +} + +bool loadGeometryFromFile(std::shared_ptr& planning_scene, + const std::string& file_path_and_name) +{ + std::ifstream file(file_path_and_name); + planning_scene->loadGeometryFromStream(file); + file.close(); + return true; +} +} // namespace + namespace moveit_py { namespace bind_planning_scene @@ -431,6 +458,28 @@ void initPlanningScene(py::module& m) Returns: bool: true if state is in self collision otherwise false. + )") + + .def("save_geometry_to_file", &saveGeometryToFile, py::arg("file_path_and_name"), + R"( + Save the CollisionObjects in the PlanningScene to a file + + Args: + file_path_and_name (str): The file to save the CollisionObjects to. + + Returns: + bool: true if save to file was successful otherwise false. + )") + + .def("load_geometry_from_file", &loadGeometryFromFile, py::arg("file_path_and_name"), + R"( + Load the CollisionObjects from a file to the PlanningScene + + Args: + file_path_and_name (str): The file to load the CollisionObjects from. + + Returns: + bool: true if load from file was successful otherwise false. )"); } } // namespace bind_planning_scene diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index c868454fed..34af2a8094 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -185,7 +185,7 @@ bool MoveGroupCartesianPathService::computeService( std::vector traj; res->fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, - moveit::core::MaxEEFStep(req->max_step), jump_threshold, constraint_fn); + moveit::core::MaxEEFStep(req->max_step), moveit::core::CartesianPrecision{}, constraint_fn); moveit::core::robotStateToRobotStateMsg(start_state, res->start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req->group_name); diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index 5037999513..980370ca18 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -28,7 +28,7 @@ publish_joint_accelerations: false ## Plugins for smoothing outgoing commands use_smoothing: true -smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin" +smoothing_filter_plugin_name: "online_signal_smoothing::RuckigFilterPlugin" # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, # which other nodes can use as a source for information about the planning environment. @@ -38,13 +38,16 @@ is_primary_planning_scene_monitor: true check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available) ## MoveIt properties -move_group_name: panda_arm # Often 'manipulator' or 'arm' +move_group_name: panda_arm # Often 'manipulator' or 'arm' ## Configure handling of singularities and joint limits -lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margins: [0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) +# Added as a buffer to joint variable position bounds [in that joint variable's respective units]. +# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. +# If moving quickly, make these values larger. +joint_limit_margins: [0.12] ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index ddffaf0c97..497252714c 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -326,8 +326,8 @@ servo: joint_limit_margins: { type: double_array, - default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - description: "Added as a buffer to joint limits [radians]. If moving quickly, make this larger.", + default_value: [0.1], + description: "Added as a buffer to joint variable position bounds [in that joint variable's respective units]. Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. If moving quickly, make these values larger.", validation: { lower_element_bounds<>: 0.0 } @@ -336,7 +336,7 @@ servo: override_velocity_scaling_factor: { type: double, default_value: 0.0, - description: "Scaling factor when servo overrides the velocity (eg: near singularities)", + description: "Scaling factor when servo overrides the velocity (e.g, near singularities)", validation: { bounds<>: [0.0, 1.0] } diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 5324ce9bf1..34e735875a 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -37,13 +37,16 @@ smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" is_primary_planning_scene_monitor: true ## MoveIt properties -move_group_name: panda_arm # Often 'manipulator' or 'arm' +move_group_name: panda_arm # Often 'manipulator' or 'arm' ## Configure handling of singularities and joint limits -lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) +# Added as a buffer to joint variable position bounds [in that joint variable's respective units]. +# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. +# If moving quickly, make these values larger. +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp index 0ea73ec9cd..43a0e94fa2 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp @@ -91,11 +91,11 @@ int main(int argc, char* argv[]) std::chrono::seconds timeout_duration(3); std::chrono::seconds time_elapsed(0); - auto start_time = std::chrono::steady_clock::now(); + const auto start_time = std::chrono::steady_clock::now(); // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; - KinematicState current_state = servo.getCurrentRobotState(); + KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */); updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now()); RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage()); @@ -104,7 +104,7 @@ int main(int argc, char* argv[]) KinematicState joint_state = servo.getNextJointState(robot_state, joint_jog); const StatusCode status = servo.getStatus(); - auto current_time = std::chrono::steady_clock::now(); + const auto current_time = std::chrono::steady_clock::now(); time_elapsed = std::chrono::duration_cast(current_time - start_time); if (time_elapsed > timeout_duration) { diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp index aa71bda8b1..4c713fd8b6 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -119,7 +119,7 @@ int main(int argc, char* argv[]) // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; - KinematicState current_state = servo.getCurrentRobotState(); + KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */); updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now()); bool satisfies_linear_tolerance = false; diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp index cd310b596e..8c92fa4cfc 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -94,11 +94,11 @@ int main(int argc, char* argv[]) std::chrono::seconds timeout_duration(4); std::chrono::seconds time_elapsed(0); - auto start_time = std::chrono::steady_clock::now(); + const auto start_time = std::chrono::steady_clock::now(); // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; - KinematicState current_state = servo.getCurrentRobotState(); + KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */); updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now()); RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage()); @@ -107,7 +107,7 @@ int main(int argc, char* argv[]) KinematicState joint_state = servo.getNextJointState(robot_state, target_twist); const StatusCode status = servo.getStatus(); - auto current_time = std::chrono::steady_clock::now(); + const auto current_time = std::chrono::steady_clock::now(); time_elapsed = std::chrono::duration_cast(current_time - start_time); if (time_elapsed > timeout_duration) { diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index d50c831f98..d946bdc833 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -118,9 +118,11 @@ 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. + * @param block_for_current_state If true, we explicitly wait for a new robot state * @return The current state of the robot. */ - KinematicState getCurrentRobotState() const; + KinematicState getCurrentRobotState(bool block_for_current_state) const; /** * \brief Smoothly halt at a commanded state when command goes stale. @@ -187,7 +189,7 @@ class Servo * @param servo_params The servo parameters * @return True if parameters are valid, else False */ - bool validateParams(const servo::Params& servo_params) const; + bool validateParams(const servo::Params& servo_params); /** * \brief Updates the servo parameters and performs validations. @@ -206,7 +208,7 @@ class Servo * @param target_state The target kinematic state. * @return The bounded kinematic state. */ - KinematicState haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, + KinematicState haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, const KinematicState& target_state) const; // Variables @@ -230,6 +232,9 @@ class Servo // Map between joint subgroup names and corresponding joint name - move group indices map std::unordered_map joint_name_to_index_maps_; + + // The current joint limit safety margins for each active joint position variable. + std::vector joint_limit_margins_; }; } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp index 5fd5091dbd..060a113f56 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -166,15 +166,16 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, const moveit::core::JointBoundsVector& joint_bounds, double scaling_override); /** - * \brief Finds the joints that are exceeding allowable position limits. + * \brief Finds the joint variable indices corresponding to joints exceeding allowable position limits. * @param positions The joint positions. * @param velocities The current commanded velocities. * @param joint_bounds The allowable limits for the robot joints. * @param margins Additional buffer on the actual joint limits. - * @return The joints that are violating the specified position limits. + * @return The joint variable indices that violate the specified position limits. */ -std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, const std::vector& margins); +std::vector jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, + const std::vector& margins); /** * \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index bfac5ae194..329e4a84a8 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"); } @@ -158,7 +159,6 @@ void Servo::setSmoothingPlugin() RCLCPP_ERROR(logger_, "Smoothing plugin could not be initialized"); std::exit(EXIT_FAILURE); } - resetSmoothing(getCurrentRobotState()); } void Servo::doSmoothing(KinematicState& state) @@ -182,7 +182,7 @@ void Servo::setCollisionChecking(const bool check_collision) check_collision ? collision_monitor_->start() : collision_monitor_->stop(); } -bool Servo::validateParams(const servo::Params& servo_params) const +bool Servo::validateParams(const servo::Params& servo_params) { bool params_valid = true; auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); @@ -247,30 +247,27 @@ bool Servo::validateParams(const servo::Params& servo_params) const << servo_params.move_group_name << "'" << check_yaml_string); params_valid = false; } - if (servo_params.joint_limit_margins.size() != - robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()) - { - RCLCPP_ERROR_STREAM( - logger_, - "The parameter 'joint_limit_margins' must have the same number of elements as the number of joints in the " - "move group. The size of 'joint_limit_margins' is '" - << servo_params.joint_limit_margins.size() << "' but the number of joints of the move group '" - << servo_params.move_group_name << "' is '" - << robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount() << "'" - << check_yaml_string); - params_valid = false; + const auto num_dofs = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount(); + if (servo_params.joint_limit_margins.size() == 1u) + { + joint_limit_margins_.clear(); + for (size_t idx = 0; idx < num_dofs; ++idx) + { + joint_limit_margins_.push_back(servo_params.joint_limit_margins[0]); + } } - if (servo_params.joint_limit_margins.size() != - robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()) + else if (servo_params.joint_limit_margins.size() == num_dofs) + { + joint_limit_margins_ = servo_params.joint_limit_margins; + } + else { - RCLCPP_ERROR(logger_, - "Parameter 'joint_limit_margins' must have the same number of elements as the number of joints in the " - "move_group. " - "Size of 'joint_limit_margins' is '%li', but number of joints in '%s' is '%i'. " - "Check the parameters YAML file used to launch this node.", - servo_params.joint_limit_margins.size(), servo_params.move_group_name.c_str(), - robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()); + RCLCPP_ERROR_STREAM( + logger_, "The parameter 'joint_limit_margins' must have either a single element or the same number of " + "elements as the degrees of freedom in the active joint group. The size of 'joint_limit_margins' is '" + << servo_params.joint_limit_margins.size() << "' but the number of degrees of freedom in group '" + << servo_params.move_group_name << "' is '" << num_dofs << "'." << check_yaml_string); params_valid = false; } @@ -337,14 +334,14 @@ void Servo::setCommandType(const CommandType& command_type) expected_command_type_ = command_type; } -KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, - const KinematicState& target_state) const +KinematicState Servo::haltJoints(const std::vector& joint_variables_to_halt, + const KinematicState& current_state, const KinematicState& target_state) const { KinematicState bounded_state(target_state.joint_names.size()); bounded_state.joint_names = target_state.joint_names; std::stringstream halting_joint_names; - for (const int idx : joints_to_halt) + for (const auto idx : joint_variables_to_halt) { halting_joint_names << bounded_state.joint_names[idx] + " "; } @@ -364,7 +361,7 @@ KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const K // Halt only the joints that are out of bounds bounded_state.positions = target_state.positions; bounded_state.velocities = target_state.velocities; - for (const int idx : joints_to_halt) + for (const auto idx : joint_variables_to_halt) { bounded_state.positions[idx] = current_state.positions[idx]; bounded_state.velocities[idx] = 0.0; @@ -526,9 +523,6 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot // Adjust joint position based on scaled down velocity target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period); - // Apply smoothing to the positions if a smoother was provided. - doSmoothing(target_state); - // Apply collision scaling to the joint position delta target_state.positions = current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions); @@ -536,20 +530,20 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot // Compute velocities based on smoothed joint positions target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period; - // Check if any joints are going past joint position limits - const std::vector joints_to_halt = - jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margins); + // Check if any joints are going past joint position limits. + const std::vector joint_variables_to_halt = + jointVariablesToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_); // Apply halting if any joints need to be halted. - if (!joints_to_halt.empty()) + if (!joint_variables_to_halt.empty()) { servo_status_ = StatusCode::JOINT_BOUND; - target_state = haltJoints(joints_to_halt, current_state, target_state); + target_state = haltJoints(joint_variables_to_halt, current_state, target_state); } } - // Update internal state of filter with final calculated command. - resetSmoothing(target_state); + // Apply smoothing to the positions if a smoother was provided. + doSmoothing(target_state); return target_state; } @@ -647,8 +641,21 @@ std::optional Servo::toPlanningFrame(const PoseCommand& command, co return PoseCommand{ planning_frame, planning_to_command_tf * command.pose }; } -KinematicState Servo::getCurrentRobotState() const +KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const { + if (block_for_current_state) + { + 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(), 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(); return extractRobotState(robot_state, servo_params_.move_group_name); } @@ -665,9 +672,6 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta // set target velocity target_state.velocities *= 0.0; - // apply smoothing: this will change target position/velocity to make slow down gradual - doSmoothing(target_state); - // scale velocity in case of obstacle target_state.velocities *= collision_velocity_scale_; @@ -681,7 +685,9 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta } } - resetSmoothing(target_state); + // apply smoothing: this will change target position/velocity to make slow down gradual + doSmoothing(target_state); + return std::make_pair(stopped, target_state); } diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index deb8b02d01..bfc989adf3 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -152,12 +152,11 @@ void ServoNode::pauseServo(const std::shared_ptrgetCurrentRobotState(); + last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); servo_->resetSmoothing(last_commanded_state_); // clear out the command rolling window and reset last commanded state to be the current state joint_cmd_rolling_window_.clear(); - last_commanded_state_ = servo_->getCurrentRobotState(); // reactivate collision checking servo_->setCollisionChecking(true); @@ -301,8 +300,10 @@ void ServoNode::servoLoop() RCLCPP_INFO(node_->get_logger(), "Waiting to receive robot state update."); rclcpp::sleep_for(std::chrono::seconds(1)); } - KinematicState current_state = servo_->getCurrentRobotState(); + KinematicState current_state = servo_->getCurrentRobotState(true /* block for current robot state */); last_commanded_state_ = current_state; + // Ensure the filter is up to date + servo_->resetSmoothing(current_state); // Get the robot state and joint model group info. moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); @@ -314,6 +315,7 @@ void ServoNode::servoLoop() // Skip processing if servoing is disabled. if (servo_paused_) { + servo_->resetSmoothing(current_state); servo_frequency.sleep(); continue; } @@ -329,7 +331,7 @@ void ServoNode::servoLoop() { // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state joint_cmd_rolling_window_.clear(); - current_state = servo_->getCurrentRobotState(); + current_state = servo_->getCurrentRobotState(false /* block for current robot state */); current_state.velocities *= 0.0; } @@ -381,7 +383,6 @@ 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); } 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 66b0866819..7ce13c1805 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -408,24 +408,46 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, return min_scaling_factor; } -std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, const std::vector& margins) +std::vector jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, + const std::vector& margins) { - std::vector joint_idxs_to_halt; - for (size_t i = 0; i < joint_bounds.size(); i++) + std::vector variable_indices_to_halt; + + // Now get the scaling factor from joint velocity limits. + size_t variable_idx = 0; + for (const auto& joint_bound : joint_bounds) { - const auto joint_bound = (joint_bounds[i])->front(); - if (joint_bound.position_bounded_) + bool halt_joint = false; + for (const auto& variable_bound : *joint_bound) { - const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margins[i]); - const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margins[i]); - if (negative_bound || positive_bound) + // First, loop through all the joint variables to see if the entire joint should be halted. + if (variable_bound.position_bounded_) + { + const bool approaching_negative_bound = + velocities[variable_idx] < 0 && + positions[variable_idx] < (variable_bound.min_position_ + margins[variable_idx]); + const bool approaching_positive_bound = + velocities[variable_idx] > 0 && + positions[variable_idx] > (variable_bound.max_position_ - margins[variable_idx]); + if (approaching_negative_bound || approaching_positive_bound) + { + halt_joint |= true; + } + } + ++variable_idx; + + // If the joint needs to be halted, add all variable indices corresponding to that joint. + if (halt_joint) { - joint_idxs_to_halt.push_back(i); + for (size_t k = variable_idx - joint_bound->size(); k < variable_idx; ++k) + { + variable_indices_to_halt.push_back(k); + } } } } - return joint_idxs_to_halt; + return variable_indices_to_halt; } /** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/ @@ -499,7 +521,16 @@ 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 + // TODO: fix the root cause so this isn't necessary (#2958) + 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; } diff --git a/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py index 6cb56bfa5b..fd14c8bb9d 100644 --- a/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py @@ -22,8 +22,9 @@ def generate_test_description(): .to_dict() } - # This filter parameter should be >1. Increase it for greater smoothing but slower motion. - low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( @@ -91,7 +92,8 @@ def generate_test_description(): ), parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py index b5123ba530..86848fd696 100644 --- a/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py @@ -29,8 +29,9 @@ def generate_test_description(): .to_dict() } - # This filter parameter should be >1. Increase it for greater smoothing but slower motion. - low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( @@ -82,7 +83,8 @@ def generate_test_description(): name="servo_node", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, @@ -112,7 +114,8 @@ def generate_test_description(): name="servo_node", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index ed23f4d69e..3022b8a278 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -74,7 +74,7 @@ TEST_F(ServoRosFixture, testJointJog) // Call input type service auto request = std::make_shared(); - request->command_type = 0; + request->command_type = moveit_msgs::srv::ServoCommandType::Request::JOINT_JOG; auto response = switch_input_client_->async_send_request(request); ASSERT_EQ(response.get()->success, true); @@ -116,7 +116,7 @@ TEST_F(ServoRosFixture, testTwist) "/servo_node/delta_twist_cmds", rclcpp::SystemDefaultsQoS()); auto request = std::make_shared(); - request->command_type = 1; + request->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST; auto response = switch_input_client_->async_send_request(request); ASSERT_EQ(response.get()->success, true); @@ -155,7 +155,7 @@ TEST_F(ServoRosFixture, testPose) "/servo_node/pose_target_cmds", rclcpp::SystemDefaultsQoS()); auto request = std::make_shared(); - request->command_type = 2; + request->command_type = moveit_msgs::srv::ServoCommandType::Request::POSE; auto response = switch_input_client_->async_send_request(request); ASSERT_EQ(response.get()->success, true); diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 5e2fceb8a0..b9cd185d67 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -438,7 +438,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; debug_msg.step = w * sizeof(float); debug_msg.data.resize(img_size * sizeof(float)); - mesh_filter_->getModelDepth(reinterpret_cast(&debug_msg.data[0])); + mesh_filter_->getModelDepth(reinterpret_cast(&debug_msg.data[0])); pub_model_depth_image_.publish(debug_msg, *info_msg); sensor_msgs::msg::Image filtered_depth_msg; @@ -449,7 +449,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; filtered_depth_msg.step = w * sizeof(float); filtered_depth_msg.data.resize(img_size * sizeof(float)); - mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_depth_msg.data[0])); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_depth_msg.data[0])); pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg); sensor_msgs::msg::Image label_msg; @@ -481,7 +481,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: if (filtered_data.size() < img_size) filtered_data.resize(img_size); - mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); unsigned short* msg_data = reinterpret_cast(&filtered_msg.data[0]); for (std::size_t i = 0; i < img_size; ++i) { diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h index 3532389869..64b8dadfca 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h @@ -106,7 +106,7 @@ class GLRenderer * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] buffer pointer to memory where the depth values need to be stored */ - void getDepthBuffer(double* buffer) const; + void getDepthBuffer(float* buffer) const; /** * \brief loads, compiles, links and adds GLSL shaders from files to the current OpenGL context. diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index 646c3ca569..879a9d87bc 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -131,7 +131,7 @@ class MeshFilterBase * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] depth pointer to buffer to be filled with depth values. */ - void getFilteredDepth(double* depth) const; + void getFilteredDepth(float* depth) const; /** * \brief retrieves the labels of the rendered model @@ -149,7 +149,7 @@ class MeshFilterBase * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] depth pointer to buffer to be filled with depth values. */ - void getModelDepth(double* depth) const; + void getModelDepth(float* depth) const; /** * \brief set the shadow threshold. points that are further away than the rendered model are filtered out. diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h index 0ba4405281..607fb663c0 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h @@ -103,13 +103,13 @@ class SensorModel * \brief transforms depth values from rendered model to metric depth values * \param[in,out] depth pointer to floating point depth buffer */ - virtual void transformModelDepthToMetricDepth(double* depth) const; + virtual void transformModelDepthToMetricDepth(float* depth) const; /** * \brief transforms depth values from filtered depth to metric depth values * \param[in,out] depth pointer to floating point depth buffer */ - virtual void transformFilteredDepthToMetricDepth(double* depth) const; + virtual void transformFilteredDepthToMetricDepth(float* depth) const; /** * \brief sets the image size diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 3c2d7d29e3..0dea00e55c 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -213,7 +213,7 @@ void mesh_filter::GLRenderer::getColorBuffer(unsigned char* buffer) const glBindFramebuffer(GL_FRAMEBUFFER, 0); } -void mesh_filter::GLRenderer::getDepthBuffer(double* buffer) const +void mesh_filter::GLRenderer::getDepthBuffer(float* buffer) const { glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_); glBindTexture(GL_TEXTURE_2D, depth_id_); diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 793058297e..6b4b551b9f 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -227,7 +227,7 @@ void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const job->wait(); } -void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const +void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const { JobPtr job1 = std::make_shared>([&renderer = *mesh_renderer_, depth] { renderer.getDepthBuffer(depth); }); @@ -243,7 +243,7 @@ void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const job2->wait(); } -void mesh_filter::MeshFilterBase::getFilteredDepth(double* depth) const +void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const { JobPtr job1 = std::make_shared>([&filter = *depth_filter_, depth] { filter.getDepthBuffer(depth); }); JobPtr job2 = std::make_shared>( diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index 562e7b4359..9ff5cdbf0c 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp @@ -102,7 +102,7 @@ inline bool isAligned16(const void* pointer) #endif } // namespace -void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(double* depth) const +void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(float* depth) const { #if HAVE_SSE_EXTENSIONS const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_); @@ -161,7 +161,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(doub const float nf = near * far; const float f_n = far - near; - const double* depth_end = depth + width_ * height_; + const float* depth_end = depth + width_ * height_; while (depth < depth_end) { if (*depth != 0 && *depth != 1) @@ -178,7 +178,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(doub #endif } -void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(double* depth) const +void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(float* depth) const { #if HAVE_SSE_EXTENSIONS //* SSE version @@ -223,7 +223,7 @@ void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(d ++mmDepth; } #else - const double* depth_end = depth + width_ * height_; + const float* depth_end = depth + width_ * height_; const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_; const float offset = near_clipping_plane_distance_; while (depth < depth_end) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 4f82643017..be02e8c6d2 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -765,9 +765,16 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. */ + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, + double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, + bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, avoid_collisions, error_code); + } double computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); + moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the @@ -781,8 +788,16 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. */ + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, + double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, error_code); + } double computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, + moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 3fbf9e3c63..84a46005ce 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -860,7 +860,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } double computeCartesianPath(const std::vector& waypoints, double step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& msg, + moveit_msgs::msg::RobotTrajectory& msg, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes& error_code) { @@ -874,7 +874,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl req->header.stamp = getClock()->now(); req->waypoints = waypoints; req->max_step = step; - req->jump_threshold = jump_threshold; req->path_constraints = path_constraints; req->avoid_collisions = avoid_collisions; req->link_name = getEndEffectorLink(); @@ -1553,31 +1552,29 @@ moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan) //} double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) + moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions, + moveit_msgs::msg::MoveItErrorCodes* error_code) { moveit_msgs::msg::Constraints path_constraints_tmp; - return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::msg::Constraints(), - avoid_collisions, error_code); + return computeCartesianPath(waypoints, eef_step, trajectory, moveit_msgs::msg::Constraints(), avoid_collisions, + error_code); } double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, + moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) { if (error_code) { - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, *error_code); + return impl_->computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, *error_code); } else { moveit_msgs::msg::MoveItErrorCodes err_tmp; err_tmp.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; moveit_msgs::msg::MoveItErrorCodes& err = error_code ? *error_code : err_tmp; - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, err); + return impl_->computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, err); } } diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 370d2f72df..2b0595005e 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -238,6 +238,10 @@ TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest) // clear path constraints move_group_->clearPathConstraints(); + + // move back to ready pose + move_group_->setNamedTarget("ready"); + planAndMove(); } TEST_F(MoveGroupTestFixture, ModifyPlanningSceneAsyncInterfaces) @@ -311,11 +315,10 @@ TEST_F(MoveGroupTestFixture, CartPathTest) waypoints.push_back(target_waypoint); // up and left moveit_msgs::RobotTrajectory trajectory; - const auto jump_threshold = 0.0; const auto eef_step = 0.01; // test below is meaningless if Cartesian planning did not succeed - ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0); + ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, trajectory), 1.0); // Execute trajectory EXPECT_EQ(move_group_->execute(trajectory), moveit::core::MoveItErrorCode::SUCCESS); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index e79a026806..746685ed43 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -135,13 +135,11 @@ bool MotionPlanningFrame::computeCartesianPlan() // setup default params double cart_step_size = 0.01; - double cart_jump_thresh = 0.0; bool avoid_collisions = true; // compute trajectory moveit_msgs::msg::RobotTrajectory trajectory; - double fraction = - move_group_->computeCartesianPath(waypoints, cart_step_size, cart_jump_thresh, trajectory, avoid_collisions); + double fraction = move_group_->computeCartesianPath(waypoints, cart_step_size, trajectory, avoid_collisions); if (fraction >= 1.0) {