Trajectory MultiDOF to file

edit

The solution is made of two answer:

• question #q277954 -> store data into file
• question #q246678 -> transform the link to the global pose

The solution is also in this repo.

// Store the points in a matrix
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // The solution is here: https://answers.ros.org/question/246678
  // To get the data without Eigen, the solution is here (line 460): https://github.com/kunal15595/ros/blob/master/moveit/src/moveit_core/robot_state/test/test_kinematic.cpp
  // To store all the points of the trajectory into a vector of vectors (matrix with x,y,z)
  // These are the necessary steps:
  // 1- Select the point in the trajectory (i)
  // 2- Set the robot on that pose (setVariablePositions && update)
  // 3- Obtain th3 values of the point from the link that is desired (kinematic_state->getGlobalLinkTransform("link"))
  // 3*- Also is possible for the orientation

  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
  //ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
  robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

  std::vector<std::vector<double>> trajectory_pos(trajectory.joint_trajectory.points.size());
  std::vector<std::vector<double>> finger_state(trajectory.joint_trajectory.points.size());

  for(int i = 0; i < trajectory.joint_trajectory.points.size(); i++)
  {
      trajectory_pos.at(i) = {trajectory.joint_trajectory.points[i].positions[0],trajectory.joint_trajectory.points[i].positions[1],trajectory.joint_trajectory.points[i].positions[2],trajectory.joint_trajectory.points[i].positions[3],trajectory.joint_trajectory.points[i].positions[4],trajectory.joint_trajectory.points[i].positions[5],trajectory.joint_trajectory.points[i].positions[6]};
      kinematic_state->setVariablePositions(trajectory_pos[i]);
      kinematic_state->update();

      double finger_state_x = kinematic_state->getGlobalLinkTransform("finger").translation().x();
      double finger_state_y = kinematic_state->getGlobalLinkTransform("finger").translation().y();
      double finger_state_z = kinematic_state->getGlobalLinkTransform("finger").translation().z();

      finger_state.at(i) = {finger_state_x, finger_state_y, finger_state_z};


      // This is from the original code. It prints the translation and orientation of the link in each position
      // Affine3D is a Matrix with 3 rows * 4 columns
      //const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("link_6");
      //ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
      //ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());
  }


    // Store the points in txt file
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    // The solution is from here: https://answers.ros.org/question/277954/
    // 1- Create a stream with all the data from the link coordinates.
    //    Each line has the 3 values separates by comma.
    // 2- When all the data is stored in the stream is time to open the file
    // 3- If the file is open correctly the data is copy in the file
    std::stringstream ss;

    for (unsigned i=0; i< trajectory.joint_trajectory.points.size(); i++)
    {
      ss
          << finger_state[i][0]
          << "," << finger_state[i][1]
          << "," << finger_state[i][2]
          << std::endl;

      if(i == (trajectory.joint_trajectory.points.size()-1))
      {
          std::ofstream outfile ("/home/crasar/points.txt",std::ios::app);
          if(!outfile.is_open())
          {
            ROS_INFO("open failed");
          }
          else
          {
              outfile << "There are a total of " << i << " points in the trajectory" <<std::endl; //<<count<<std::endl;
              //outfile<<finger_state[i][0]<<","<<finger_state[i][1]<<","<<finger_state[i][2]<<std::endl;
              //multi_dof_joint_trajectory.joint_names[0]<<std::endl;
              outfile<<ss.str()<<std::endl;
              outfile.close();
              ROS_INFO("File created");
          }
      }
    }