Convert MDOF trajectories to joint trajectories in planning interfaces

Author: henningkayser

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();