Enable mdof trajectory execution (#2740)

* Add RobotTrajectory conversion from MDOF to joints

* Convert MDOF trajectories to joint trajectories in planning interfaces

* Treat mdof joint variables as common joints in
TrajectoryExecutionManager

* Convert multi-DOF trajectories to joints in TEM

* Revert "Convert MDOF trajectories to joint trajectories in planning interfaces"

This reverts commit 885ee27.

* Handle multi-DOF variables in TEM's bound checking

* Add parameter to optionally enable multi-dof conversion

* Improve error message about unknown controllers

* Fix name ordering in JointTrajectory conversion

* Improve DEBUG output in TEM

* Comment RobotTrajectory test

* add acceleration to avoid out of bounds read

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

---------

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
Co-authored-by: Paul Gesel <paulgesel@gmail.com>
Co-authored-by: Abishalini Sivaraman <abi.gpuram@gmail.com>
Co-authored-by: Ezra Brooks <ezra@brooks.cx>

Author: 4 people

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

@@ -410,4 +410,13 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory);
 /// or nullopt if it is not possible to calculate the density
 [[nodiscard]] std::optional<double> waypointDensity(const RobotTrajectory& trajectory);
 
+/// \brief Converts a RobotTrajectory to a JointTrajectory message
+//  \param[in] trajectory Given robot trajectory
+//  \param[in] include_mdof_joints Treat Multi-DOF variables as joints, e.g. position/x position/y position/theta
+//  \param[in] joint_filter Exclude joints with the provided names
+//  \return JointTrajectory message including all waypoints
+//  or nullopt if the provided RobotTrajectory or RobotModel is empty
+[[nodiscard]] std::optional<trajectory_msgs::msg::JointTrajectory>
+toJointTrajectory(const RobotTrajectory& trajectory, bool include_mdof_joints = false,
+                  const std::vector<std::string>& joint_filter = {});
 }  // namespace robot_trajectory

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -373,29 +373,36 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t
         {
           const std::vector<std::string> names = mdof[j]->getVariableNames();
           const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]);
+          const double* accelerations = waypoints_[i]->getJointAccelerations(mdof[j]);
 
           geometry_msgs::msg::Twist point_velocity;
+          geometry_msgs::msg::Twist point_acceleration;
 
           for (std::size_t k = 0; k < names.size(); ++k)
           {
             if (names[k].find("/x") != std::string::npos)
             {
               point_velocity.linear.x = velocities[k];
+              point_acceleration.linear.x = accelerations[k];
             }
             else if (names[k].find("/y") != std::string::npos)
             {
               point_velocity.linear.y = velocities[k];
+              point_acceleration.linear.y = accelerations[k];
             }
             else if (names[k].find("/z") != std::string::npos)
             {
               point_velocity.linear.z = velocities[k];
+              point_acceleration.linear.z = accelerations[k];
             }
             else if (names[k].find("/theta") != std::string::npos)
             {
               point_velocity.angular.z = velocities[k];
+              point_acceleration.angular.z = accelerations[k];
             }
           }
           trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity);
+          trajectory.multi_dof_joint_trajectory.points[i].accelerations.push_back(point_acceleration);
         }
       }
       if (duration_from_previous_.size() > i)
@@ -707,4 +714,129 @@ std::optional<double> waypointDensity(const RobotTrajectory& trajectory)
   return std::nullopt;
 }
 
+std::optional<trajectory_msgs::msg::JointTrajectory> toJointTrajectory(const RobotTrajectory& trajectory,
+                                                                       bool include_mdof_joints,
+                                                                       const std::vector<std::string>& joint_filter)
+{
+  const auto group = trajectory.getGroup();
+  const auto robot_model = trajectory.getRobotModel();
+  const std::vector<const moveit::core::JointModel*>& jnts =
+      group ? group->getActiveJointModels() : robot_model->getActiveJointModels();
+
+  if (trajectory.empty() || jnts.empty())
+    return std::nullopt;
+
+  trajectory_msgs::msg::JointTrajectory joint_trajectory;
+  std::vector<const moveit::core::JointModel*> onedof;
+  std::vector<const moveit::core::JointModel*> mdof;
+
+  for (const moveit::core::JointModel* active_joint : jnts)
+  {
+    // only consider joints listed in joint_filter
+    if (!joint_filter.empty() &&
+        std::find(joint_filter.begin(), joint_filter.end(), active_joint->getName()) == joint_filter.end())
+      continue;
+
+    if (active_joint->getVariableCount() == 1)
+    {
+      onedof.push_back(active_joint);
+    }
+    else if (include_mdof_joints)
+    {
+      mdof.push_back(active_joint);
+    }
+  }
+
+  for (const auto& joint : onedof)
+  {
+    joint_trajectory.joint_names.push_back(joint->getName());
+  }
+  for (const auto& joint : mdof)
+  {
+    for (const auto& name : joint->getVariableNames())
+    {
+      joint_trajectory.joint_names.push_back(name);
+    }
+  }
+
+  if (!onedof.empty() || !mdof.empty())
+  {
+    joint_trajectory.header.frame_id = robot_model->getModelFrame();
+    joint_trajectory.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
+    joint_trajectory.points.resize(trajectory.getWayPointCount());
+  }
+
+  static const auto ZERO_DURATION = rclcpp::Duration::from_seconds(0);
+  double total_time = 0.0;
+  for (std::size_t i = 0; i < trajectory.getWayPointCount(); ++i)
+  {
+    total_time += trajectory.getWayPointDurationFromPrevious(i);
+    joint_trajectory.points[i].time_from_start = rclcpp::Duration::from_seconds(total_time);
+    const auto& waypoint = trajectory.getWayPoint(i);
+
+    if (!onedof.empty())
+    {
+      joint_trajectory.points[i].positions.resize(onedof.size());
+      joint_trajectory.points[i].velocities.reserve(onedof.size());
+
+      for (std::size_t j = 0; j < onedof.size(); ++j)
+      {
+        joint_trajectory.points[i].positions[j] = waypoint.getVariablePosition(onedof[j]->getFirstVariableIndex());
+        // if we have velocities/accelerations/effort, copy those too
+        if (waypoint.hasVelocities())
+        {
+          joint_trajectory.points[i].velocities.push_back(
+              waypoint.getVariableVelocity(onedof[j]->getFirstVariableIndex()));
+        }
+        if (waypoint.hasAccelerations())
+        {
+          joint_trajectory.points[i].accelerations.push_back(
+              waypoint.getVariableAcceleration(onedof[j]->getFirstVariableIndex()));
+        }
+        if (waypoint.hasEffort())
+        {
+          joint_trajectory.points[i].effort.push_back(waypoint.getVariableEffort(onedof[j]->getFirstVariableIndex()));
+        }
+      }
+      // clear velocities if we have an incomplete specification
+      if (joint_trajectory.points[i].velocities.size() != onedof.size())
+        joint_trajectory.points[i].velocities.clear();
+      // clear accelerations if we have an incomplete specification
+      if (joint_trajectory.points[i].accelerations.size() != onedof.size())
+        joint_trajectory.points[i].accelerations.clear();
+      // clear effort if we have an incomplete specification
+      if (joint_trajectory.points[i].effort.size() != onedof.size())
+        joint_trajectory.points[i].effort.clear();
+    }
+
+    if (!mdof.empty())
+    {
+      for (const auto joint : mdof)
+      {
+        // Add variable placeholders
+        const std::vector<std::string> names = joint->getVariableNames();
+        joint_trajectory.points[i].positions.reserve(joint_trajectory.points[i].positions.size() + names.size());
+
+        joint_trajectory.points[i].velocities.reserve(joint_trajectory.points[i].velocities.size() + names.size());
+
+        for (const auto& name : names)
+        {
+          joint_trajectory.points[i].positions.push_back(waypoint.getVariablePosition(name));
+
+          if (waypoint.hasVelocities())
+          {
+            joint_trajectory.points[i].velocities.push_back(waypoint.getVariableVelocity(name));
+          }
+          if (waypoint.hasAccelerations())
+          {
+            joint_trajectory.points[i].accelerations.push_back(waypoint.getVariableAcceleration(name));
+          }
+        }
+      }
+    }
+  }
+
+  return joint_trajectory;
+}
+
 }  // end of namespace robot_trajectory

moveit_core/robot_trajectory/test/test_robot_trajectory.cpp

@@ -634,6 +634,38 @@ TEST_F(OneRobot, UnwindFromState)
   }
 }
 
+TEST_F(OneRobot, MultiDofTrajectoryToJointStates)
+{
+  // GIVEN a RobotTrajectory with two waypoints of a robot model that has a multi-dof base joint
+  robot_trajectory::RobotTrajectory trajectory(robot_model_);
+  trajectory.addSuffixWayPoint(robot_state_, 0.01 /* dt */);
+  trajectory.addSuffixWayPoint(robot_state_, 0.01 /* dt */);
+
+  // WHEN converting the RobotTrajectory to a JointTrajectory message, including mdof variables
+  auto maybe_trajectory_msg = toJointTrajectory(trajectory, true /* include_mdof_joints */);
+
+  // WHEN the optional trajectory result is valid (always assumed)
+  ASSERT_TRUE(maybe_trajectory_msg.has_value());
+
+  const auto traj = maybe_trajectory_msg.value();
+  const auto& joint_names = traj.joint_names;
+
+  size_t joint_variable_count = 0u;
+  for (const auto& active_joint : robot_model_->getActiveJointModels())
+  {
+    joint_variable_count += active_joint->getVariableCount();
+  }
+
+  // THEN all joints names should include the base joint variables
+  EXPECT_EQ(joint_names.size(), joint_variable_count);
+  EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), "base_joint/x") != joint_names.end());
+  // THEN the size of the trajectory should equal the input size
+  ASSERT_EQ(traj.points.size(), 2u);
+  // THEN all positions size should equal the variable size
+  EXPECT_EQ(traj.points.at(0).positions.size(), joint_variable_count);
+  EXPECT_EQ(traj.points.at(1).positions.size(), joint_variable_count);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h

@@ -333,6 +333,7 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
   // 'global' values
   double allowed_execution_duration_scaling_;
   double allowed_goal_duration_margin_;
+  bool control_multi_dof_joint_variables_;
   // controller-specific values
   // override the 'global' values
   std::map<std::string, double> controller_allowed_execution_duration_scaling_;