how to save and read / load a trajectory

edit

Hi,

I want to capture a trajectory in one run of the program saving it to a file, and try to load the same trajectory the next time. So, actually skipping the move_group.setJointValueTarget(jointVals) or move_group.setPoseTarget(sample_pose) on next execution and trying to read the trajectory points directly from the file.

Why we are looking to this option The real robotic arm is available for tests only occasionally for short times. Since planners are random sampling based planners, the planners might compute a different path to the goal point during different runs of the program. At certain times, we noticed that a certain trajectory found and played in rviz, took a different path when executed on the real robotic arm. Since, workspace of the robotic arm have some physical constraints in place, it was not possible to carry out the deviated trajectory execution. An example, a movement went through the negative z-axis before reaching the goal pose. The real robotic arm workspace is constrained that way. Hence, the robotic arm had to be emergency stopped using the red button on the panel.

Below is the code that I use to save the trajectory, where my_plan is the successfully computed plan to the goal pose/joint-space goal.

moveit_msgs::RobotTrajectory path_points;
  trajectory_msgs::JointTrajectory jt;
  std::stringstream ss;
  size_t size_trajectory;

  path_points = my_plan.trajectory_;
  jt = path_points.joint_trajectory;
  size_trajectory = jt.points.size();

  for (unsigned i=0; i<size_trajectory; i++)
  {
          ss << "point_index: " << i << std::endl
             << "positions: "
             << "[" << jt.points[i].positions[0]
             << "," << jt.points[i].positions[1]
             << "," << jt.points[i].positions[2]
             << "," << jt.points[i].positions[3]
             << "," << jt.points[i].positions[4]
             << "," << jt.points[i].positions[5]
             << "]" << endl;
  }

My file contains entries of the following form (vector of joint-angles in radians).

point_index: 3
positions: [-0.286826,-0.120969,0.155815,-0.0343112,-0.139144,0.295076]

Next, I read the above file. I parse it and create a std::vector<std::vector<double>> traj_matrix where each row is a joint-configuration vector. And, the total number of rows in this matrix is the total number of points in the original trajectory i.e. size_trajectory. Given each row of this matrix, I calculate the corresponding end-effector pose (forward kinematics). I can save all these poses as waypoints to compute a cartesian path through these points. Following the tutorial on kinematics, I write the following code:

void SimpleMotionPlanning::computeFK()
{
   std::vector<double> joint_values;
   geometry_msgs::Pose a_pose;

  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
  robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
  joint_model_group = kinematic_model->getJointModelGroup(PLANNING_GROUP);

  for (int i = 0; i < this->traj_matrix.size(); i++)
  {
    kinematic_state->setJointGroupPositions(joint_model_group, traj_matrix[i]);
    kinematic_state->enforceBounds();
    tf::poseEigenToMsg(kinematic_state->getGlobalLinkTransform("ee_link"), a_pose);
    this->waypoints.push_back(a_pose);
  }

}

Here is the launch file

<launch>
  <include file="$(find ur5_moveit_path_planner)/launch/planning_context.launch" >
    <arg name="load_robot_description" value="true" />
  </include>

  <!--
    first try, in which, we do not have a robot connected. so, we publish fake joint states
  -->
<node
    name="joint_state_publisher"
    pkg="joint_state_publisher"
    type ...