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/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h

@@ -315,6 +315,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_;

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -36,6 +36,7 @@
 
 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
 #include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
 #include <geometric_shapes/check_isometry.h>
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <moveit/utils/logger.hpp>
@@ -51,6 +52,7 @@ static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5;  // allow 0.5
                                                                     // after scaling)
 static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING =
     1.1;  // allow the execution of a trajectory to take more time than expected (scaled by a value > 1)
+static const bool DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES = false;
 
 TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node,
                                                        const moveit::core::RobotModelConstPtr& robot_model,
@@ -94,6 +96,7 @@ void TrajectoryExecutionManager::initialize()
   execution_velocity_scaling_ = 1.0;
   allowed_start_tolerance_ = 0.01;
   wait_for_trajectory_completion_ = true;
+  control_multi_dof_joint_variables_ = DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES;
 
   allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
   allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
@@ -200,6 +203,7 @@ void TrajectoryExecutionManager::initialize()
   controller_mgr_node_->get_parameter("trajectory_execution.allowed_goal_duration_margin",
                                       allowed_goal_duration_margin_);
   controller_mgr_node_->get_parameter("trajectory_execution.allowed_start_tolerance", allowed_start_tolerance_);
+  controller_mgr_node_->get_parameter("trajectory_execution.control_multi_dof_joint_variables", control_multi_dof_joint_variables_);
 
   if (manage_controllers_)
   {
@@ -350,8 +354,45 @@ bool TrajectoryExecutionManager::push(const moveit_msgs::msg::RobotTrajectory& t
     return false;
   }
 
+  // Optionally, convert multi dof waypoints to joint states and replace trajectory for execution
+  std::optional<moveit_msgs::msg::RobotTrajectory> replaced_trajectory;
+  if (control_multi_dof_joint_variables_ && !trajectory.multi_dof_joint_trajectory.points.empty())
+  {
+    // We convert the trajectory message into a RobotTrajectory first,
+    // since the conversion to a combined JointTrajectory depends on the local variables
+    // of the Multi-DOF joint types.
+    moveit::core::RobotState reference_state(robot_model_);
+    reference_state.setToDefaultValues();
+    robot_trajectory::RobotTrajectory tmp_trajectory(robot_model_);
+    tmp_trajectory.setRobotTrajectoryMsg(reference_state, trajectory);
+
+    // Combine all joints for filtering the joint trajectory waypoints
+    std::vector<std::string> all_trajectory_joints = trajectory.joint_trajectory.joint_names;
+    for (const auto& mdof_joint : trajectory.multi_dof_joint_trajectory.joint_names)
+    {
+      all_trajectory_joints.push_back(mdof_joint);
+    }
+
+    // Convert back to single joint trajectory including the MDOF joint variables, e.g. position/x, position/y, ...
+    const auto joint_trajectory =
+        robot_trajectory::toJointTrajectory(tmp_trajectory, true /* include_mdof_joints */, all_trajectory_joints);
+
+    // Check success of conversion
+    // This should never happen when using MoveIt's interfaces, but users can pass anything into TEM::push() directly
+    if (!joint_trajectory.has_value())
+    {
+      RCLCPP_ERROR(logger_, "Failed to convert multi-DOF trajectory to joint trajectory, aborting execution!");
+      return false;
+    }
+
+    // Create a new robot trajectory message that only contains the combined joint trajectory
+    RCLCPP_INFO(logger_, "Successfully converted multi-DOF trajectory to joint trajectory for execution.");
+    replaced_trajectory = moveit_msgs::msg::RobotTrajectory();
+    replaced_trajectory->joint_trajectory = joint_trajectory.value();
+  }
+
   TrajectoryExecutionContext* context = new TrajectoryExecutionContext();
-  if (configure(*context, trajectory, controllers))
+  if (configure(*context, replaced_trajectory.value_or(trajectory), controllers))
   {
     if (verbose_)
     {
@@ -704,12 +745,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);
     }
   }
 
@@ -849,7 +890,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont
     RCLCPP_WARN(logger_, "Failed to validate trajectory: couldn't receive full current joint state within 1s");
     return false;
   }
-
+  moveit::core::RobotState reference_state(*current_state);
   for (const auto& trajectory : context.trajectory_parts_)
   {
     if (!trajectory.joint_trajectory.points.empty())
@@ -863,30 +904,45 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont
         return false;
       }
 
+      std::set<const moveit::core::JointModel*> joints;
       for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
       {
-        const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]);
+        const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_names[i]);
         if (!jm)
         {
           RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_names[i]);
           return false;
         }
 
-        double cur_position = current_state->getJointPositions(jm)[0];
-        double traj_position = positions[i];
-        // normalize positions and compare
-        jm->enforcePositionBounds(&cur_position);
-        jm->enforcePositionBounds(&traj_position);
-        if (jm->distance(&cur_position, &traj_position) > allowed_start_tolerance_)
+        joints.insert(jm);
+      }
+
+      // Copy all variable positions to reference state, and then compare start state joint distance within bounds
+      // Note on multi-DOF joints: Instead of comparing the translation and rotation distances like it's done for
+      // the multi-dof trajectory, this check will use the joint's internal distance implementation instead.
+      // This is more accurate, but may require special treatment for cases like the diff drive's turn path geometry.
+      reference_state.setVariablePositions(joint_names, positions);
+      for (const auto joint : joints)
+      {
+        reference_state.enforcePositionBounds(joint);
+        current_state->enforcePositionBounds(joint);
+        if (reference_state.distance(*current_state, joint) > allowed_start_tolerance_)
         {
           RCLCPP_ERROR(logger_,
-                       "\nInvalid Trajectory: start point deviates from current robot state more than %g"
-                       "\njoint '%s': expected: %g, current: %g",
-                       allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position);
+                       "Invalid Trajectory: start point deviates from current robot state more than %g at joint '%s'."
+                       "\nEnable DEBUG for detailed state info.",
+                       allowed_start_tolerance_, joint->getName().c_str());
+          // TODO(henningkayser): print state info, or treat multi-dof joint separately using the
+          //  current_state->printStatePositions()
+          //  RCLCPP_DEBUG(logger_,
+          //               "Current state
+          //               "\njoint '%s': expected: %g, current: %g",
+          //               allowed_start_tolerance_, joint->getName().c_str(), traj_position, cur_position);
           return false;
         }
       }
     }
+
     if (!trajectory.multi_dof_joint_trajectory.points.empty())
     {
       // Check multi-dof trajectory
@@ -947,7 +1003,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)
@@ -1015,7 +1071,12 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
       {
         if (known_controllers_.find(controller) == known_controllers_.end())
         {
-          RCLCPP_ERROR(logger_, "Controller '%s' is not known", controller.c_str());
+          std::stringstream stream;
+          for (const auto& controller : known_controllers_)
+          {
+            stream << " `" << controller.first << '`';
+          }
+          RCLCPP_ERROR_STREAM(logger_, "Controller " << controller << " is not known. Known controllers: " << stream.str());
           return false;
         }
       }
@@ -1045,6 +1106,13 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
     }
   }
   RCLCPP_ERROR(logger_, "Known controllers and their joints:\n%s", ss2.str().c_str());
+
+  if (!trajectory.multi_dof_joint_trajectory.joint_names.empty())
+  {
+    RCLCPP_WARN(logger_, "Hint: You can control multi-dof waypoints as joint trajectory by setting "
+                         "`trajectory_execution.control_multi_dof_joint_variables=True`");
+  }
+
   return false;
 }
 
@@ -1490,11 +1558,12 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon
 
       for (std::size_t i = 0; i < n && !moved; ++i)
       {
-        const moveit::core::JointModel* jm = cur_state->getJointModel(joint_names[i]);
+        const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_names[i]);
         if (!jm)
           continue;  // joint vanished from robot state (shouldn't happen), but we don't care
 
-        if (fabs(cur_state->getJointPositions(jm)[0] - prev_state->getJointPositions(jm)[0]) > allowed_start_tolerance_)
+        if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) >
+            allowed_start_tolerance_)
         {
           moved = true;
           no_motion_count = 0;