# Revision history [back]

The solution is made of two answer: - question #277954 -> store data into file - question #246678 -> 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::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
//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");
}
}
}

 2 No.2 Revision jayess 5155 ●17 ●60 ●72

• question #277954 #q277954 -> store data into file - file
• question #246678 #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::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
//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");
}
}
}