Revision history [back]

See the above comments for notes about why MoveIt, barring a pull request, won't compute time optimal cartesian paths from within RViz. With that said, you can still program your own time optimal cartesian paths in a move_group node. This worked for me:

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

  const moveit::core::LinkModel* link_model6 = move_group.getCurrentState()->getLinkModel("link6");
  visual_tools.publishTrajectoryLine(totg_trajectory, link_model6, joint_model_group, rviz_visual_tools::ORANGE);

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

See the above comments for notes about why MoveIt, barring a pull request, won't compute time optimal cartesian paths from within RViz. With that said, you can still program your own time optimal cartesian paths in a move_group node. This worked for me:

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

  const moveit::core::LinkModel* link_model6 = move_group.getCurrentState()->getLinkModel("link6");
  visual_tools.publishTrajectoryLine(totg_trajectory, link_model6, joint_model_group, rviz_visual_tools::ORANGE);

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