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..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 @@ -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 all active variables in the group. + * + * @return std::pair Containing the lower and upper joint limits for all active variables. + */ + [[nodiscard]] std::pair getLowerAndUpperLimits() const; + + /** + * @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; + 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, diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index a9986368e3..2776bb0c9d 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -831,5 +831,56 @@ bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const d return true; } + +std::pair JointModelGroup::getLowerAndUpperLimits() const +{ + // Get the group joints lower/upper position limits. + Eigen::VectorXd lower_limits(active_variable_count_); + Eigen::VectorXd upper_limits(active_variable_count_); + 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[variable_index] = variable_bounds.min_position_; + upper_limits[variable_index] = variable_bounds.max_position_; + variable_index++; + } + } + return { lower_limits, upper_limits }; +} + +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); + // Check if variable count matches number of joint model bounds + if (active_joint_models_bounds_.size() != active_variable_count_) + { + // 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 }; + } + // Check if the joint group contains multi-dof joints + for (const auto& bound : active_joint_models_bounds_) + { + if (bound->size() != 1) + { + 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_, + active_joint_models_bounds_[i]->at(0).max_acceleration_); + } + return { max_joint_velocities, max_joint_accelerations }; +} } // end of namespace core } // end of namespace moveit 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/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