# Trajectory MultiDOF to file

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


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


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 close merge delete

Sort by » oldest newest most voted

• 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*- Also is possible for the orientation

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

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
//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");
}
}
}

more

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?

more

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?

( 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.

( 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...

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