Moveit: output Cartesian trajectories

edit

Hi,

I am wondering if it is possible to use moveit (ROS1) to generate Cartesian trajectories (not joint ones), that then will be handled separately.

For example, I was looking at the cartesian_trajectory_controller, which should accept Cartesian Trajectories, but how to provide them from the moveit pipelines?

I looked at the custom controller manager but the sendTrajectory method of moveit_controller_manager::MoveItControllerHandle accepts a moveit_msgs::RobotTrajectory which is a joint trajectory.

At the moment, after planning, I simply extract the waypoints from the RobotTrajectory and I publish them:

auto solution = planning_components->plan();

moveit_msgs::CartesianTrajectory cartesian_traj_msg;
cartesian_traj_msg.header.frame_id = moveit_cpp_ptr->getRobotModel()->getRootLinkName();
cartesian_traj_msg.header.stamp = ros::Time::now();
cartesian_traj_msg.tracked_frame = "arm1_6";

for (int i =0; i< solution.trajectory_->getWayPointCount(); i++ ) {
    moveit_msgs::CartesianTrajectoryPoint cartesian_traj_point_msg;
    cartesian_traj_point_msg.time_from_start = ros::Duration(solution.trajectory_->getWayPointDurationFromStart(i));

    const Eigen::Isometry3d& tip_pose = solution.trajectory_->getWayPoint(i).getGlobalLinkTransform(cartesian_traj_msg.tracked_frame);

    tf::poseEigenToMsg(tip_pose, cartesian_traj_point_msg.point.pose);

    cartesian_traj_msg.points.push_back(cartesian_traj_point_msg);
}

to_cartesian_pub.publish(cartesian_traj_msg);

but it does not seem the right way to do it