# Time Optimal Cartesian Paths

I have made some progress since my last few questions here. I now have a fully functional robot arm accepting trajectories from MoveIt. Currently, I rely on Time Optimal Parameterization with a fairly small resample_dt. My stepper motor controller (running entirely on a Teensy 3.6) takes these waypoints and fills in the rest.

I was hoping I might be able to post the move_group code I've been using to get time optimal cartesian paths and get some style points and feedback on how I am approaching this.

Here's what I have right now:

  move_group.setStartStateToCurrentState();
geometry_msgs::Pose start_pose = move_group.getCurrentPose().pose;

std::vector<geometry_msgs::Pose> waypoints;
waypoints.push_back(start_pose);

geometry_msgs::Pose target_pose3 = start_pose;

target_pose3.position.x += inch_to_m(8);
target_pose3.position.z += inch_to_m(-10);
target_pose3.position.y += inch_to_m(-8);
waypoints.push_back(target_pose3);

moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

robot_trajectory::RobotTrajectory trajectory_for_totg(move_group.getRobotModel(), PLANNING_GROUP);

robot_state::RobotState start_state(*move_group.getCurrentState());

trajectory_for_totg.setRobotTrajectoryMsg(start_state, trajectory);

//Get these from param server later, plz
double path_tolerance = 0.002;
double resample_dt = 0.03;
double min_angle_change = 0.001;

trajectory_processing::TimeOptimalTrajectoryGeneration time_param(path_tolerance, resample_dt, min_angle_change);
time_param.computeTimeStamps(trajectory_for_totg, 1.0);  //1.0 accel scaling

moveit_msgs::RobotTrajectory totg_trajectory;
trajectory_for_totg.getRobotTrajectoryMsg(totg_trajectory);

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);

for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window move arm.");

move_group.execute(totg_trajectory);


I've trimmed the fat here to just show the relevant code. Is there a way for me to get these paths to publish like regular non-Cartesian paths that I can view in RQT before they become goals? I am also wondering if someone might help me to get rid of this messy business of converting a moveit_msgs::RobotTrajectory into a robot_trajectory::RobotTrajectory, running Time Optimal on it, and then converting back to a moveit_msgs::RobotTrajectory.