Time Optimal Cartesian Paths

edit

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

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

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.

Thanks in advance, Drew Hamilton

repository: https://github.com/drewhamiltonasdf/c...