Ask Your Question
0

Trajectory MultiDOF to file

asked 2018-10-08 10:37:19 -0600

jdeleon gravatar image

Hi all!

I'm getting stuck trying to save the trajectory generate by MoveIt to a file.

The trajectory is generated correctly and the robot moves right in RViz.

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

image description

Also, I can open a file and write that in it. But I can't obtain the data of the trajectory.

If I read the data with:

trajectory.joint_trajectory.points[i].position[6];

I get a value, that I don't know what it means, and also is only one point, no three for the X, Y, and Z of the joint.

If I try with:

Eigen::Affine3d t;
tf::transformMsgToEigen(trajectory.multi_dof_joint_trajectory.points[i].transforms[j], t);

I can compile my code, but I get an error when executing the program. The error code is -11.

[move_group_interface_tutorial-1] process has died [pid 19367, exit code -11, cmd /home/crasar/WS/fotokite_ws/devel/lib/moveit_tutorials/fotokite_move_path __name:=move_group_interface_tutorial __log:=/home/crasar/.ros/log/f8fe907a-cb06-11e8-bd7b-d89ef3923760/move_group_interface_tutorial-1.log].
log file: /home/crasar/.ros/log/f8fe907a-cb06-11e8-bd7b-d89ef3923760/move_group_interface_tutorial-1*.log

Here is the workspace of my project.

And to launch it you need two terminals:

  1. roslaunch fotokite_moveit demo.launch
  2. roslaunch moveit_tutorials fotokite_move_path.launch

Thank you, Jorge

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2018-10-09 16:16:56 -0600

jdeleon gravatar image

updated 2018-10-09 19:25:14 -0600

jayess gravatar image

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");
          }
      }
    }
edit flag offensive delete link more
0

answered 2018-10-09 03:11:36 -0600

Hi Jorge,

There are a few things that need clarifying here. The moveit_msgs::RobotTrajectory message doesn't store Cartesian waypoints it stores joint angles. To convert these trajectories into Cartesian EEF positions you'll need to use the kinematic model of the robot.

The line:

trajectory.joint_trajectory.points[i].position[6];

Is the angle of the 7th joint of the robot at step i in the trajectory, this value is in radians. This is why your attempt to transform it using TF fails before because it's a single angle not a 3D point.

What type of robot model are you using in this case?

edit flag offensive delete link more

Comments

Hi @PeteBlacketThe3rd, thank you for your answer.

I have a 7 DOF arm. Which is RRR-P-RRR.

And what about using the trajectory_msgs/MultiDOF? Finally it gives:

geometry_msgs/Vector3 translation
geometry_msgs/Quaternion rotation

Should the position be stored at the Vector3 translation?

jdeleon gravatar imagejdeleon ( 2018-10-09 08:17:13 -0600 )edit

If the MultiDOF message is populated instead you can use that, however the translation is relative to the previous joint, it is not in the base link frame. You'll need to create a set of transforms for each joint and transform through all of them to get back to the base link frame. Can be done.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-10-09 16:07:00 -0600 )edit

Hi @PeteBlackerThe3rd I have get it with the help of this question https://answers.ros.org/question/2466...

jdeleon gravatar imagejdeleon ( 2018-10-09 16:12:39 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2018-10-08 10:37:19 -0600

Seen: 311 times

Last updated: Oct 09 '18