Skip to content

Commit 885ee27

Browse files
committed
Convert MDOF trajectories to joint trajectories in planning interfaces
1 parent 3cad609 commit 885ee27

File tree

2 files changed

+35
-2
lines changed

2 files changed

+35
-2
lines changed

moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp

+22-1
Original file line numberDiff line numberDiff line change
@@ -106,8 +106,29 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptr<ExecTra
106106
{
107107
RCLCPP_INFO(getLogger(), "Execution request received");
108108

109+
// Convert mdof trajectory to RobotTrajectory
110+
auto trajectory_msg = goal->get_goal()->trajectory;
111+
if (!trajectory_msg.multi_dof_joint_trajectory.points.empty())
112+
{
113+
std::vector<std::string> joint_names = {}; // trajectory_msg.joint_trajectory.joint_names;
114+
for (const auto& name : trajectory_msg.multi_dof_joint_trajectory.joint_names)
115+
{
116+
joint_names.push_back(name);
117+
}
118+
119+
robot_trajectory::RobotTrajectory trajectory(context_->moveit_cpp_->getRobotModel());
120+
trajectory.setRobotTrajectoryMsg(*context_->moveit_cpp_->getCurrentState(0.05), trajectory_msg);
121+
const auto joint_trajectory =
122+
robot_trajectory::toJointTrajectory(trajectory, true /* include_mdof_joints */, joint_names);
123+
if (joint_trajectory.has_value())
124+
{
125+
trajectory_msg.joint_trajectory = joint_trajectory.value();
126+
trajectory_msg.multi_dof_joint_trajectory = trajectory_msgs::msg::MultiDOFJointTrajectory();
127+
}
128+
}
129+
109130
context_->trajectory_execution_manager_->clear();
110-
if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names))
131+
if (context_->trajectory_execution_manager_->push(trajectory_msg, goal->get_goal()->controller_names))
111132
{
112133
setExecuteTrajectoryState(MONITOR, goal);
113134
context_->trajectory_execution_manager_->execute();

moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp

+13-1
Original file line numberDiff line numberDiff line change
@@ -220,8 +220,20 @@ MoveItCpp::execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
220220
}
221221

222222
// Execute trajectory
223+
const auto maybe_traj = robot_trajectory::toJointTrajectory(*robot_trajectory, true /* include_mdof_joints */);
224+
if (!maybe_traj.has_value())
225+
{
226+
RCLCPP_ERROR(logger_, "Execution failed! Could not convert robot trajectory to message");
227+
return moveit_controller_manager::ExecutionStatus::ABORTED;
228+
}
229+
223230
moveit_msgs::msg::RobotTrajectory robot_trajectory_msg;
224-
robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg);
231+
robot_trajectory_msg.joint_trajectory = maybe_traj.value();
232+
233+
RCLCPP_ERROR(logger_, "Pushing trajectory for joints:");
234+
for (const auto& joint_name : robot_trajectory_msg.joint_trajectory.joint_names)
235+
RCLCPP_ERROR_STREAM(logger_, "- " << joint_name);
236+
225237
trajectory_execution_manager_->push(robot_trajectory_msg, controllers);
226238
trajectory_execution_manager_->execute();
227239
return trajectory_execution_manager_->waitForExecution();

0 commit comments

Comments
 (0)