Enable mdof trajectory execution

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

@@ -707,4 +707,120 @@ 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);
+      joint_trajectory.joint_names.push_back(active_joint->getName());
+    }
+    else if (include_mdof_joints)
+    {
+      mdof.push_back(active_joint);
+      for (const auto& variable_name : active_joint->getVariableNames())
+      {
+        joint_trajectory.joint_names.push_back(variable_name);
+      }
+    }
+  }
+
+  if (!joint_trajectory.joint_names.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());
+
+        for (const auto& name : names)
+        {
+          joint_trajectory.points[i].positions.push_back(waypoint.getVariablePosition(name));
+        }
+
+        if (waypoint.hasVelocities())
+        {
+          joint_trajectory.points[i].velocities.reserve(joint_trajectory.points[i].velocities.size() + names.size());
+          for (const auto& name : names)
+          {
+            joint_trajectory.points[i].velocities.push_back(waypoint.getVariableVelocity(name));
+          }
+        }
+      }
+    }
+  }
+
+  return joint_trajectory;
+}
+
 }  // end of namespace robot_trajectory

moveit_core/robot_trajectory/test/test_robot_trajectory.cpp

@@ -634,6 +634,31 @@ TEST_F(OneRobot, UnwindFromState)
   }
 }
 
+TEST_F(OneRobot, MultiDofTrajectoryToJointStates)
+{
+  robot_trajectory::RobotTrajectory trajectory(robot_model_);
+  trajectory.addSuffixWayPoint(robot_state_, 0.01 /* dt */);
+  trajectory.addSuffixWayPoint(robot_state_, 0.01 /* dt */);
+  auto maybe_trajectory_msg = toJointTrajectory(trajectory, true /* include_mdof_joints */);
+
+  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();
+  }
+
+  EXPECT_EQ(joint_names.size(), joint_variable_count);
+  EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), "base_joint/x") != joint_names.end());
+  ASSERT_EQ(traj.points.size(), 2u);
+  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/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp

@@ -106,8 +106,29 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptr<ExecTra
 {
   RCLCPP_INFO(getLogger(), "Execution request received");
 
+  // Convert mdof trajectory to RobotTrajectory
+  auto trajectory_msg = goal->get_goal()->trajectory;
+  if (!trajectory_msg.multi_dof_joint_trajectory.points.empty())
+  {
+    std::vector<std::string> joint_names = {};  // trajectory_msg.joint_trajectory.joint_names;
+    for (const auto& name : trajectory_msg.multi_dof_joint_trajectory.joint_names)
+    {
+      joint_names.push_back(name);
+    }
+
+    robot_trajectory::RobotTrajectory trajectory(context_->moveit_cpp_->getRobotModel());
+    trajectory.setRobotTrajectoryMsg(*context_->moveit_cpp_->getCurrentState(0.05), trajectory_msg);
+    const auto joint_trajectory =
+        robot_trajectory::toJointTrajectory(trajectory, true /* include_mdof_joints */, joint_names);
+    if (joint_trajectory.has_value())
+    {
+      trajectory_msg.joint_trajectory = joint_trajectory.value();
+      trajectory_msg.multi_dof_joint_trajectory = trajectory_msgs::msg::MultiDOFJointTrajectory();
+    }
+  }
+
   context_->trajectory_execution_manager_->clear();
-  if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names))
+  if (context_->trajectory_execution_manager_->push(trajectory_msg, goal->get_goal()->controller_names))
   {
     setExecuteTrajectoryState(MONITOR, goal);
     context_->trajectory_execution_manager_->execute();

moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp

@@ -220,8 +220,20 @@ MoveItCpp::execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
   }
 
   // Execute trajectory
+  const auto maybe_traj = robot_trajectory::toJointTrajectory(*robot_trajectory, true /* include_mdof_joints */);
+  if (!maybe_traj.has_value())
+  {
+    RCLCPP_ERROR(logger_, "Execution failed! Could not convert robot trajectory to message");
+    return moveit_controller_manager::ExecutionStatus::ABORTED;
+  }
+
   moveit_msgs::msg::RobotTrajectory robot_trajectory_msg;
-  robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg);
+  robot_trajectory_msg.joint_trajectory = maybe_traj.value();
+
+  RCLCPP_ERROR(logger_, "Pushing trajectory for joints:");
+  for (const auto& joint_name : robot_trajectory_msg.joint_trajectory.joint_names)
+    RCLCPP_ERROR_STREAM(logger_, "- " << joint_name);
+
   trajectory_execution_manager_->push(robot_trajectory_msg, controllers);
   trajectory_execution_manager_->execute();
   return trajectory_execution_manager_->waitForExecution();

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -704,12 +704,12 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::msg::Ro
   std::set<std::string> actuated_joints_single;
   for (const std::string& joint_name : trajectory.joint_trajectory.joint_names)
   {
-    const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_name);
+    const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name);
     if (jm)
     {
       if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == moveit::core::JointModel::FIXED)
         continue;
-      actuated_joints_single.insert(jm->getName());
+      actuated_joints_single.insert(joint_name);
     }
   }
 
@@ -865,11 +865,12 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont
 
       for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
       {
+        // TODO support multi-dof joints
         const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]);
         if (!jm)
         {
           RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_names[i]);
-          return false;
+          continue;
         }
 
         double cur_position = current_state->getJointPositions(jm)[0];
@@ -947,7 +948,7 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
   std::set<std::string> actuated_joints;
 
   auto is_actuated = [this](const std::string& joint_name) -> bool {
-    const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_name);
+    const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name);
     return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != moveit::core::JointModel::FIXED);
   };
   for (const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
@@ -1490,6 +1491,7 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon
 
       for (std::size_t i = 0; i < n && !moved; ++i)
       {
+        // TODO: Update to respect mdof joint variables
         const moveit::core::JointModel* jm = cur_state->getJointModel(joint_names[i]);
         if (!jm)
           continue;  // joint vanished from robot state (shouldn't happen), but we don't care