Add utility functions to get limits and trajectory message (#2861)

Co-authored-by: Mario Prats <[email protected]>

moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h

@@ -586,6 +586,21 @@ class JointModelGroup
   bool computeJointVariableIndices(const std::vector<std::string>& joint_names,
                                    std::vector<size_t>& joint_bijection) const;
 
+  /**
+   * @brief Get the lower and upper position limits of all active variables in the group.
+   *
+   * @return std::pair<Eigen::VectorXd, Eigen::VectorXd> Containing the lower and upper joint limits for all active variables.
+   */
+  [[nodiscard]] std::pair<Eigen::VectorXd, Eigen::VectorXd> 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<Eigen::VectorXd, Eigen::VectorXd> Containing the velocity and acceleration limits
+   */
+  [[nodiscard]] std::pair<Eigen::VectorXd, Eigen::VectorXd> 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,

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<Eigen::VectorXd, Eigen::VectorXd> 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<Eigen::VectorXd, Eigen::VectorXd> 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

moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h

@@ -38,6 +38,7 @@
 
 #include <moveit_msgs/msg/robot_trajectory.hpp>
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 
 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<std::string>& joint_names,
+                        const trajectory_processing::Trajectory& trajectory, const int sampling_rate);
 }  // namespace trajectory_processing

moveit_core/trajectory_processing/src/trajectory_tools.cpp

@@ -37,8 +37,20 @@
 #include <moveit/trajectory_processing/trajectory_tools.h>
 #include <moveit/trajectory_processing/ruckig_traj_smoothing.h>
 #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
+
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
 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<std::string>& 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<double>(sampling_rate);
+  const int n_samples = static_cast<int>(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<double>(t));
+    trajectory_msg.points.push_back(std::move(point));
+  }
+  return trajectory_msg;
+}
 }  // namespace trajectory_processing