From 2d8c4c1394687bbd2d3de7bb608fe2771fb3cfdf Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 5 Jun 2024 13:46:49 +0200 Subject: [PATCH 1/8] Add utility functionality to extract limits from JMG --- .../moveit/robot_model/joint_model_group.h | 19 ++++++++++ .../robot_model/src/joint_model_group.cpp | 36 +++++++++++++++++++ 2 files changed, 55 insertions(+) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index ad8ff4a0cf..66de98153f 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -754,5 +754,24 @@ class JointModelGroup /** \brief The names of the default states specified for this group in the SRDF */ std::vector default_states_names_; }; + +/** + * @brief Get the lower and upper position limits of a given joint group. + * + * @param group Joint model group from which the limits are read + * @return std::pair Containing the joint limits + */ +[[nodiscard]] std::pair +getLowerAndUpperLimits(const moveit::core::JointModelGroup& group); + +/** + * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains + * only single-variable joints, + * + * @param group Joint model group from which the limits are read + * @return std::pair Containing the velocity and acceleration limits + */ +[[nodiscard]] std::pair +getMaximumVelocitiesAndAccelerations(const moveit::core::JointModelGroup& group); } // namespace core } // namespace moveit diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index a9986368e3..2dedb9ecac 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -831,5 +831,41 @@ bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const d return true; } + +std::pair getLowerAndUpperLimits(const moveit::core::JointModelGroup& group) +{ + // Get the group joints lower/upper position limits. + Eigen::VectorXd lower_limits(group.getActiveVariableCount()); + Eigen::VectorXd upper_limits(group.getActiveVariableCount()); + const moveit::core::JointBoundsVector& group_bounds = group.getActiveJointModelsBounds(); + int joint_index = 0; + for (const moveit::core::JointModel::Bounds* joint_bounds : group_bounds) + { + for (const moveit::core::VariableBounds& variable_bounds : *joint_bounds) + { + lower_limits[joint_index] = variable_bounds.min_position_; + upper_limits[joint_index] = variable_bounds.max_position_; + joint_index++; + } + } + return { lower_limits, upper_limits }; +} + +std::pair +getMaximumVelocitiesAndAccelerations(const moveit::core::JointModelGroup& group) +{ + const std::size_t n_dofs = group.getActiveVariableCount(); + Eigen::VectorXd max_joint_velocities = Eigen::VectorXd::Constant(n_dofs, 0.0); + Eigen::VectorXd max_joint_accelerations = Eigen::VectorXd::Constant(n_dofs, 0.0); + const moveit::core::JointBoundsVector& bounds = group.getActiveJointModelsBounds(); + assert(bounds.size() == n_dofs); + for (std::size_t i = 0; i < bounds.size(); ++i) + { + assert(bounds[i]->size() == 1); // single-variable joints. + max_joint_velocities[i] = std::min(-bounds[i]->at(0).min_velocity_, bounds[i]->at(0).max_velocity_); + max_joint_accelerations[i] = std::min(-bounds[i]->at(0).min_acceleration_, bounds[i]->at(0).max_acceleration_); + } + return { max_joint_velocities, max_joint_accelerations }; +} } // end of namespace core } // end of namespace moveit From 855231f674f3bd911cf95b37b35e599b612e19c1 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 5 Jun 2024 14:16:36 +0200 Subject: [PATCH 2/8] Add convenience function to transform trajectory into msg type --- .../time_optimal_trajectory_generation.h | 7 +++++ .../time_optimal_trajectory_generation.cpp | 29 +++++++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 8f7b90ec03..e44ac26d71 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -189,6 +189,13 @@ class Trajectory mutable std::list::const_iterator cached_trajectory_segment_; }; +/** + * @brief Converts a `trajectory_processing::Trajectory` into a `JointTrajectory` message with a given sampling rate. + */ +[[nodiscard]] trajectory_msgs::msg::JointTrajectory +createTrajectoryMessage(const std::vector& joint_names, + const trajectory_processing::Trajectory& trajectory, const int sampling_rate); + MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration); class TimeOptimalTrajectoryGeneration : public TimeParameterization { diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index ad9225b34a..f5dccf78db 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -1310,4 +1310,33 @@ double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double request } return scaling_factor; } + +trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector& joint_names, + const trajectory_processing::Trajectory& trajectory, + const int sampling_rate) +{ + trajectory_msgs::msg::JointTrajectory trajectory_msg; + trajectory_msg.joint_names = joint_names; + assert(sampling_rate > 0); + const double time_step = 1.0 / static_cast(sampling_rate); + const int n_samples = static_cast(trajectory.getDuration() / time_step) + 1; + trajectory_msg.points.reserve(n_samples); + for (int sample = 0; sample < n_samples; ++sample) + { + const double t = sample * time_step; + trajectory_msgs::msg::JointTrajectoryPoint point; + auto position = trajectory.getPosition(t); + auto velocity = trajectory.getVelocity(t); + auto acceleration = trajectory.getAcceleration(t); + for (std::size_t i = 0; i < joint_names.size(); i++) + { + point.positions.push_back(position[i]); + point.velocities.push_back(velocity[i]); + point.accelerations.push_back(acceleration[i]); + } + point.time_from_start = rclcpp::Duration(std::chrono::duration(t)); + trajectory_msg.points.push_back(std::move(point)); + } + return trajectory_msg; +} } // namespace trajectory_processing From 99b49e6aeec9742b5f686efb15e1fd7f0e12c61b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 6 Jun 2024 12:05:51 +0200 Subject: [PATCH 3/8] Address review --- .../moveit/robot_model/joint_model_group.h | 34 +++++++------- .../robot_model/src/joint_model_group.cpp | 43 +++++++++++------- .../time_optimal_trajectory_generation.h | 7 --- .../trajectory_processing/trajectory_tools.h | 8 ++++ .../time_optimal_trajectory_generation.cpp | 29 ------------ .../src/trajectory_tools.cpp | 45 +++++++++++++++++++ 6 files changed, 95 insertions(+), 71 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 66de98153f..2fd8f48f8b 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -586,6 +586,21 @@ class JointModelGroup bool computeJointVariableIndices(const std::vector& joint_names, std::vector& joint_bijection) const; + /** + * @brief Get the lower and upper position limits of a given joint group. + * + * @return std::pair Containing the joint limits + */ + [[nodiscard]] std::pair getLowerAndUpperLimits(); + + /** + * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains + * only single-variable joints, + * + * @return std::pair Containing the velocity and acceleration limits + */ + [[nodiscard]] std::pair getMaximumVelocitiesAndAccelerations(); + protected: /** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group, @@ -754,24 +769,5 @@ class JointModelGroup /** \brief The names of the default states specified for this group in the SRDF */ std::vector default_states_names_; }; - -/** - * @brief Get the lower and upper position limits of a given joint group. - * - * @param group Joint model group from which the limits are read - * @return std::pair Containing the joint limits - */ -[[nodiscard]] std::pair -getLowerAndUpperLimits(const moveit::core::JointModelGroup& group); - -/** - * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains - * only single-variable joints, - * - * @param group Joint model group from which the limits are read - * @return std::pair Containing the velocity and acceleration limits - */ -[[nodiscard]] std::pair -getMaximumVelocitiesAndAccelerations(const moveit::core::JointModelGroup& group); } // namespace core } // namespace moveit diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 2dedb9ecac..46ec1c22d2 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -832,14 +832,13 @@ bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const d return true; } -std::pair getLowerAndUpperLimits(const moveit::core::JointModelGroup& group) +std::pair JointModelGroup::getLowerAndUpperLimits() { // Get the group joints lower/upper position limits. - Eigen::VectorXd lower_limits(group.getActiveVariableCount()); - Eigen::VectorXd upper_limits(group.getActiveVariableCount()); - const moveit::core::JointBoundsVector& group_bounds = group.getActiveJointModelsBounds(); + Eigen::VectorXd lower_limits(active_variable_count_); + Eigen::VectorXd upper_limits(active_variable_count_); int joint_index = 0; - for (const moveit::core::JointModel::Bounds* joint_bounds : group_bounds) + for (const moveit::core::JointModel::Bounds* joint_bounds : active_joint_models_bounds_) { for (const moveit::core::VariableBounds& variable_bounds : *joint_bounds) { @@ -851,19 +850,31 @@ std::pair getLowerAndUpperLimits(const moveit: return { lower_limits, upper_limits }; } -std::pair -getMaximumVelocitiesAndAccelerations(const moveit::core::JointModelGroup& group) +std::pair JointModelGroup::getMaximumVelocitiesAndAccelerations() { - const std::size_t n_dofs = group.getActiveVariableCount(); - Eigen::VectorXd max_joint_velocities = Eigen::VectorXd::Constant(n_dofs, 0.0); - Eigen::VectorXd max_joint_accelerations = Eigen::VectorXd::Constant(n_dofs, 0.0); - const moveit::core::JointBoundsVector& bounds = group.getActiveJointModelsBounds(); - assert(bounds.size() == n_dofs); - for (std::size_t i = 0; i < bounds.size(); ++i) + Eigen::VectorXd max_joint_velocities = Eigen::VectorXd::Constant(active_variable_count_, 0.0); + Eigen::VectorXd max_joint_accelerations = Eigen::VectorXd::Constant(active_variable_count_, 0.0); + if (active_joint_models_bounds_.size() != active_variable_count_) { - assert(bounds[i]->size() == 1); // single-variable joints. - max_joint_velocities[i] = std::min(-bounds[i]->at(0).min_velocity_, bounds[i]->at(0).max_velocity_); - max_joint_accelerations[i] = std::min(-bounds[i]->at(0).min_acceleration_, bounds[i]->at(0).max_acceleration_); + // TODO(sjahr) Support multiple variables + RCLCPP_ERROR(getLogger(), "Number of active joint models does not match number of active joint model bounds. " + "Returning bound vectors.with zeros"); + return { max_joint_velocities, max_joint_accelerations }; + } + for (std::size_t i = 0; i < active_joint_models_bounds_.size(); ++i) + { + if (active_joint_models_bounds_[i]->size() == 1) + { + RCLCPP_ERROR( + getLogger(), + "Joint '%li' has '%li' bounds and only joints with one bounds set are supported by this function. In " + "the returned vector this element will be 0.", + i, active_joint_models_bounds_.size()); + } + max_joint_velocities[i] = std::min(-active_joint_models_bounds_[i]->at(0).min_velocity_, + active_joint_models_bounds_[i]->at(0).max_velocity_); + max_joint_accelerations[i] = std::min(-active_joint_models_bounds_[i]->at(0).min_acceleration_, + active_joint_models_bounds_[i]->at(0).max_acceleration_); } return { max_joint_velocities, max_joint_accelerations }; } diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index e44ac26d71..8f7b90ec03 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -189,13 +189,6 @@ class Trajectory mutable std::list::const_iterator cached_trajectory_segment_; }; -/** - * @brief Converts a `trajectory_processing::Trajectory` into a `JointTrajectory` message with a given sampling rate. - */ -[[nodiscard]] trajectory_msgs::msg::JointTrajectory -createTrajectoryMessage(const std::vector& joint_names, - const trajectory_processing::Trajectory& trajectory, const int sampling_rate); - MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration); class TimeOptimalTrajectoryGeneration : public TimeParameterization { diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 2e8d600cb3..876f3af06c 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -38,6 +38,7 @@ #include #include +#include namespace trajectory_processing { @@ -79,4 +80,11 @@ bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, double acceleration_scaling_factor, bool mitigate_overshoot = false, double overshoot_threshold = 0.01); + +/** + * @brief Converts a `trajectory_processing::Trajectory` into a `JointTrajectory` message with a given sampling rate. + */ +[[nodiscard]] trajectory_msgs::msg::JointTrajectory +createTrajectoryMessage(const std::vector& joint_names, + const trajectory_processing::Trajectory& trajectory, const int sampling_rate); } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index f5dccf78db..ad9225b34a 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -1310,33 +1310,4 @@ double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double request } return scaling_factor; } - -trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector& joint_names, - const trajectory_processing::Trajectory& trajectory, - const int sampling_rate) -{ - trajectory_msgs::msg::JointTrajectory trajectory_msg; - trajectory_msg.joint_names = joint_names; - assert(sampling_rate > 0); - const double time_step = 1.0 / static_cast(sampling_rate); - const int n_samples = static_cast(trajectory.getDuration() / time_step) + 1; - trajectory_msg.points.reserve(n_samples); - for (int sample = 0; sample < n_samples; ++sample) - { - const double t = sample * time_step; - trajectory_msgs::msg::JointTrajectoryPoint point; - auto position = trajectory.getPosition(t); - auto velocity = trajectory.getVelocity(t); - auto acceleration = trajectory.getAcceleration(t); - for (std::size_t i = 0; i < joint_names.size(); i++) - { - point.positions.push_back(position[i]); - point.velocities.push_back(velocity[i]); - point.accelerations.push_back(acceleration[i]); - } - point.time_from_start = rclcpp::Duration(std::chrono::duration(t)); - trajectory_msg.points.push_back(std::move(point)); - } - return trajectory_msg; -} } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp index 6ce5a412e6..b15f0ca9a3 100644 --- a/moveit_core/trajectory_processing/src/trajectory_tools.cpp +++ b/moveit_core/trajectory_processing/src/trajectory_tools.cpp @@ -37,8 +37,20 @@ #include #include #include + +#include +#include namespace trajectory_processing { + +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.trajectory_processing.trajectory_tools"); +} +} // namespace + bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory) { return trajectory.joint_trajectory.points.empty() && trajectory.multi_dof_joint_trajectory.points.empty(); @@ -62,4 +74,37 @@ bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double return time_param.applySmoothing(trajectory, velocity_scaling_factor, acceleration_scaling_factor, mitigate_overshoot, overshoot_threshold); } + +trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector& joint_names, + const trajectory_processing::Trajectory& trajectory, + const int sampling_rate) +{ + trajectory_msgs::msg::JointTrajectory trajectory_msg; + if (sampling_rate <= 0) + { + RCLCPP_ERROR(getLogger(), "Cannot sample trajectory with sampling rate <= 0. Returning empty trajectory."); + return trajectory_msg; + } + trajectory_msg.joint_names = joint_names; + const double time_step = 1.0 / static_cast(sampling_rate); + const int n_samples = static_cast(trajectory.getDuration() / time_step) + 1; + trajectory_msg.points.reserve(n_samples); + for (int sample = 0; sample < n_samples; ++sample) + { + const double t = sample * time_step; + trajectory_msgs::msg::JointTrajectoryPoint point; + auto position = trajectory.getPosition(t); + auto velocity = trajectory.getVelocity(t); + auto acceleration = trajectory.getAcceleration(t); + for (std::size_t i = 0; i < joint_names.size(); i++) + { + point.positions.push_back(position[i]); + point.velocities.push_back(velocity[i]); + point.accelerations.push_back(acceleration[i]); + } + point.time_from_start = rclcpp::Duration(std::chrono::duration(t)); + trajectory_msg.points.push_back(std::move(point)); + } + return trajectory_msg; +} } // namespace trajectory_processing From 2ebc0724fe3445e3db18b007066c233a007adde1 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 6 Jun 2024 12:22:02 +0200 Subject: [PATCH 4/8] Rename function --- .../robot_model/include/moveit/robot_model/joint_model_group.h | 2 +- moveit_core/robot_model/src/joint_model_group.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 2fd8f48f8b..5312b590c3 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -599,7 +599,7 @@ class JointModelGroup * * @return std::pair Containing the velocity and acceleration limits */ - [[nodiscard]] std::pair getMaximumVelocitiesAndAccelerations(); + [[nodiscard]] std::pair getMaxVelocitiesAndAccelerationBounds(); protected: /** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 46ec1c22d2..a42b37fa6b 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -850,7 +850,7 @@ std::pair JointModelGroup::getLowerAndUpperLim return { lower_limits, upper_limits }; } -std::pair JointModelGroup::getMaximumVelocitiesAndAccelerations() +std::pair JointModelGroup::getMaxVelocitiesAndAccelerationBounds() { Eigen::VectorXd max_joint_velocities = Eigen::VectorXd::Constant(active_variable_count_, 0.0); Eigen::VectorXd max_joint_accelerations = Eigen::VectorXd::Constant(active_variable_count_, 0.0); From 6552aa3c6491623ffff02651c9629a603307f193 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 6 Jun 2024 12:39:16 +0200 Subject: [PATCH 5/8] const! --- .../include/moveit/robot_model/joint_model_group.h | 4 ++-- moveit_core/robot_model/src/joint_model_group.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 5312b590c3..c522a4c432 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -591,7 +591,7 @@ class JointModelGroup * * @return std::pair Containing the joint limits */ - [[nodiscard]] std::pair getLowerAndUpperLimits(); + [[nodiscard]] std::pair getLowerAndUpperLimits() const; /** * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains @@ -599,7 +599,7 @@ class JointModelGroup * * @return std::pair Containing the velocity and acceleration limits */ - [[nodiscard]] std::pair getMaxVelocitiesAndAccelerationBounds(); + [[nodiscard]] std::pair getMaxVelocitiesAndAccelerationBounds() const; protected: /** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index a42b37fa6b..0717c2e91e 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -832,7 +832,7 @@ bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const d return true; } -std::pair JointModelGroup::getLowerAndUpperLimits() +std::pair JointModelGroup::getLowerAndUpperLimits() const { // Get the group joints lower/upper position limits. Eigen::VectorXd lower_limits(active_variable_count_); @@ -850,7 +850,7 @@ std::pair JointModelGroup::getLowerAndUpperLim return { lower_limits, upper_limits }; } -std::pair JointModelGroup::getMaxVelocitiesAndAccelerationBounds() +std::pair JointModelGroup::getMaxVelocitiesAndAccelerationBounds() const { Eigen::VectorXd max_joint_velocities = Eigen::VectorXd::Constant(active_variable_count_, 0.0); Eigen::VectorXd max_joint_accelerations = Eigen::VectorXd::Constant(active_variable_count_, 0.0); From e56e5d54cefcaf8cccec20ddbfcf6b498aa5996c Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 6 Jun 2024 13:03:55 +0200 Subject: [PATCH 6/8] Apply suggestions from code review Co-authored-by: Mario Prats --- .../include/moveit/robot_model/joint_model_group.h | 4 ++-- moveit_core/robot_model/src/joint_model_group.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index c522a4c432..fbe5ecdc45 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -587,7 +587,7 @@ class JointModelGroup std::vector& joint_bijection) const; /** - * @brief Get the lower and upper position limits of a given joint group. + * @brief Get the lower and upper position limits of all active variables in the group. * * @return std::pair Containing the joint limits */ @@ -596,7 +596,7 @@ class JointModelGroup /** * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains * only single-variable joints, - * + * @details In case of asymmetric velocity or acceleration limits, this function will return the most limiting component. * @return std::pair Containing the velocity and acceleration limits */ [[nodiscard]] std::pair getMaxVelocitiesAndAccelerationBounds() const; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 0717c2e91e..529061aa03 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -858,7 +858,7 @@ std::pair JointModelGroup::getMaxVelocitiesAnd { // TODO(sjahr) Support multiple variables RCLCPP_ERROR(getLogger(), "Number of active joint models does not match number of active joint model bounds. " - "Returning bound vectors.with zeros"); + "Returning bound vectors with zeros"); return { max_joint_velocities, max_joint_accelerations }; } for (std::size_t i = 0; i < active_joint_models_bounds_.size(); ++i) From 24900649a1adfaee360f4f63f83bab9f845c6ce8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 6 Jun 2024 13:13:40 +0200 Subject: [PATCH 7/8] Apply suggestions from code review Co-authored-by: Mario Prats --- .../robot_model/include/moveit/robot_model/joint_model_group.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index fbe5ecdc45..c63112fde4 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -589,7 +589,7 @@ class JointModelGroup /** * @brief Get the lower and upper position limits of all active variables in the group. * - * @return std::pair Containing the joint limits + * @return std::pair Containing the lower and upper joint limits for all active variables. */ [[nodiscard]] std::pair getLowerAndUpperLimits() const; From 26fdfd71d2b74e436db3f17458ed5bcbac06a9c7 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 6 Jun 2024 13:07:02 +0200 Subject: [PATCH 8/8] Address review #2 --- .../robot_model/src/joint_model_group.cpp | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 529061aa03..2776bb0c9d 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -837,14 +837,14 @@ std::pair JointModelGroup::getLowerAndUpperLim // Get the group joints lower/upper position limits. Eigen::VectorXd lower_limits(active_variable_count_); Eigen::VectorXd upper_limits(active_variable_count_); - int joint_index = 0; + int variable_index = 0; for (const moveit::core::JointModel::Bounds* joint_bounds : active_joint_models_bounds_) { for (const moveit::core::VariableBounds& variable_bounds : *joint_bounds) { - lower_limits[joint_index] = variable_bounds.min_position_; - upper_limits[joint_index] = variable_bounds.max_position_; - joint_index++; + lower_limits[variable_index] = variable_bounds.min_position_; + upper_limits[variable_index] = variable_bounds.max_position_; + variable_index++; } } return { lower_limits, upper_limits }; @@ -854,6 +854,7 @@ std::pair JointModelGroup::getMaxVelocitiesAnd { Eigen::VectorXd max_joint_velocities = Eigen::VectorXd::Constant(active_variable_count_, 0.0); Eigen::VectorXd max_joint_accelerations = Eigen::VectorXd::Constant(active_variable_count_, 0.0); + // Check if variable count matches number of joint model bounds if (active_joint_models_bounds_.size() != active_variable_count_) { // TODO(sjahr) Support multiple variables @@ -861,16 +862,19 @@ std::pair JointModelGroup::getMaxVelocitiesAnd "Returning bound vectors with zeros"); return { max_joint_velocities, max_joint_accelerations }; } - for (std::size_t i = 0; i < active_joint_models_bounds_.size(); ++i) + // Check if the joint group contains multi-dof joints + for (const auto& bound : active_joint_models_bounds_) { - if (active_joint_models_bounds_[i]->size() == 1) + if (bound->size() != 1) { - RCLCPP_ERROR( - getLogger(), - "Joint '%li' has '%li' bounds and only joints with one bounds set are supported by this function. In " - "the returned vector this element will be 0.", - i, active_joint_models_bounds_.size()); + RCLCPP_ERROR(getLogger(), "Multi-dof joints are currently not supported by " + "getMaxVelocitiesAndAccelerationBounds(). Returning bound vectors with zeros."); + return { max_joint_velocities, max_joint_accelerations }; } + } + // Populate max_joint_velocity and acceleration vectors + for (std::size_t i = 0; i < active_joint_models_bounds_.size(); ++i) + { max_joint_velocities[i] = std::min(-active_joint_models_bounds_[i]->at(0).min_velocity_, active_joint_models_bounds_[i]->at(0).max_velocity_); max_joint_accelerations[i] = std::min(-active_joint_models_bounds_[i]->at(0).min_acceleration_,