sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-17 23:02:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Forward kinematics verification is mostly working for individual joints. The axis of rotation is correct for all joints, but some cause incorrect amounts of motion. Specifically, moving the wrist_pivot joint causes a circular motion of the end effector, when (I believe) it should cause no movement, just rotation, of the end effector.

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-08 23:06:53 -04:00
parent 8e25bb09a3
commit 6a73cd2371
5 changed files with 255 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
rrrobot_src/src/arm_control/CMakeLists.txt
View File

@@ -21,7 +21,7 @@ find_package(kdl_parser)
find_package(PkgConfig REQUIRED)
pkg_check_modules(SDF sdformat REQUIRED)
find_package(ignition-math6 REQUIRED)
find_package(ignition-math4 REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
@@ -127,7 +127,7 @@ include_directories(
${roscpp_INCLUDE_DIRS}
/usr/include/bullet
/usr/include/sdformat-6.2
/usr/include/ignition/math6
/usr/include/ignition/math4
/home/rrrobot/rrrobot_src/devel/include/
)
include_directories(
@@ -148,7 +148,7 @@ include_directories(
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/arm_controller.cpp src/arm.cpp)
add_executable(test test/test.cpp)
add_executable(test test/test.cpp src/arm.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
@@ -166,7 +166,7 @@ target_link_libraries(${PROJECT_NAME}_node
${kdl_parser_LIBRARIES}
${orocos_kdl_LIBRARIES}
${SDF_LIBRARIES}
ignition-math6::ignition-math6
ignition-math4::ignition-math4
)
target_link_libraries(test
@@ -174,7 +174,7 @@ target_link_libraries(test
${kdl_parser_LIBRARIES}
${orocos_kdl_LIBRARIES}
${SDF_LIBRARIES}
ignition-math6::ignition-math6
ignition-math4::ignition-math4
)
#############

1
rrrobot_src/src/arm_control/include/arm_control/arm.h
View File

@@ -63,6 +63,7 @@ public:
private:
KDL::Chain arm;
KDL::Chain simple_arm;
/*
* i_com: the inertia about the center of mass of this link

196
rrrobot_src/src/arm_control/src/arm.cpp
View File

@@ -3,6 +3,7 @@
#include <sdf/sdf.hh>
#include <ignition/math/Pose3.hh>
#include <LinearMath/btTransform.h>
#include <kdl/frames_io.hpp>
#include <iostream>
#include <sstream>
@@ -17,11 +18,13 @@ using std::vector;
struct FrameInformation
{
string link_name;
KDF::Joint joint;
KDF::Frame link_frame;
KDF::Frame joint_frame;
RigidBodyInertia inertia;
string link_name; // link name
KDL::Joint joint; // joint information (name, type)
KDL::Frame link_frame; // link location (world coordinates)
KDL::Frame joint_frame; // joint location relative to parent(link_name)
float mass;
KDL::Vector com_location; // relative to frame origin
KDL::RotationalInertia rotational_inertia;
};
/*
@@ -42,7 +45,7 @@ Arm::Arm(const std::string &sdf_file)
const string model_name = model->Get<string>("name");
cout << "Found " << model_name << " model" << endl;
unordered_map<string, KDL::Segment> links;
unordered_map<string, FrameInformation> links;
unordered_map<string, string> link_ordering;
string first_link;
@@ -53,22 +56,25 @@ Arm::Arm(const std::string &sdf_file)
cout << "Link: " << link->Get<string>("name") << endl;
const string name(link->Get<string>("name"));
KDL::Joint cur_joint(KDL::Joint::None);
links[name].link_name = name;
// KDL::Joint cur_joint(KDL::Joint::None);
const sdf::ElementPtr inertial_data = link->GetElement("inertial");
float mass = inertial_data->Get<float>("mass");
links[name].mass = mass;
cout << "Mass: " << mass << endl;
string inertial_data_s = inertial_data->GetElement("pose")->GetValue()->GetAsString();
stringstream inertial_data_ss(inertial_data_s);
KDL::Vector inertial_frame_info;
stringstream inertial_data_ss(inertial_data->GetElement("pose")->GetValue()->GetAsString());
// stringstream inertial_data_ss(inertial_data_s);
// KDL::Vector inertial_frame_info;
inertial_data_ss >> inertial_frame_info.data[0];
inertial_data_ss >> inertial_frame_info.data[1];
inertial_data_ss >> inertial_frame_info.data[2];
inertial_data_ss >> links[name].com_location.data[0] >> links[name].com_location.data[1] >> links[name].com_location[2];
// inertial_data_ss >> inertial_frame_info.data[0];
// inertial_data_ss >> inertial_frame_info.data[1];
// inertial_data_ss >> inertial_frame_info.data[2];
const sdf::ElementPtr inertia = inertial_data->GetElement("inertia");
KDL::RotationalInertia rotational_inertia(
links[name].rotational_inertia = KDL::RotationalInertia(
inertia->Get<float>("ixx"),
inertia->Get<float>("iyy"),
inertia->Get<float>("izz"),
@@ -76,20 +82,20 @@ Arm::Arm(const std::string &sdf_file)
inertia->Get<float>("ixz"),
inertia->Get<float>("iyz"));
KDL::RigidBodyInertia link_inertia(mass, inertial_frame_info, rotational_inertia);
//KDL::RigidBodyInertia link_inertia(mass, inertial_frame_info, rotational_inertia);
// Transformation from world to link coordinates
// stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString());
// float x, y, z;
// float roll, pitch, yaw;
// frame_location >> x >> y >> z >> roll >> pitch >> yaw;
// KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw);
// KDL::Vector link_position(x, y, z);
// KDL::Frame frame(rotation, link_position);
stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString());
float x, y, z;
float roll, pitch, yaw;
frame_location >> x >> y >> z >> roll >> pitch >> yaw;
KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw);
KDL::Vector link_position(x, y, z);
links[name].link_frame = KDL::Frame(rotation, link_position);
KDL::Segment cur_link(name, cur_joint, KDL::Frame::Identity(), link_inertia);
//KDL::Segment cur_link(name, cur_joint, KDL::Frame::Identity(), link_inertia);
//arm.addSegment(cur_link);
links[name] = cur_link;
//links[name] = cur_link;
// cout << inertia->Get<float>("ixx") << endl;
// cout << inertia->Get<float>("iyy") << endl;
@@ -166,7 +172,7 @@ Arm::Arm(const std::string &sdf_file)
else if (axis_num == 2)
joint_type = KDL::Joint::JointType::RotZ;
KDL::Joint cur_joint(name, joint_type);
links[child].joint = KDL::Joint(name, joint_type);
stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString());
float x, y, z;
@@ -175,8 +181,9 @@ Arm::Arm(const std::string &sdf_file)
KDL::Rotation frame_rotation = KDL::Rotation::RPY(roll, pitch, yaw);
KDL::Vector frame_location(x, y, z);
links[child].joint_frame = KDL::Frame(frame_rotation, frame_location);
links[child] = KDL::Segment(links[child].getName(), cur_joint, KDL::Frame(frame_rotation, frame_location), links[child].getInertia());
//links[child] = KDL::Segment(links[child].getName(), cur_joint, KDL::Frame(frame_rotation, frame_location), links[child].getInertia());
//KDL::Segment cur_segment = arm.getSegment
joint = joint->GetNextElement("joint");
@@ -189,51 +196,118 @@ Arm::Arm(const std::string &sdf_file)
{
cur_link_name = link_ordering[cur_link_name];
const KDL::Segment &cur = links[cur_link_name];
const KDL::Frame &cur_frame = cur.getFrameToTip();
double cur_roll, cur_pitch, cur_yaw;
cur_frame.M.GetRPY(cur_roll, cur_pitch, cur_yaw);
double prev_roll, prev_pitch, prev_yaw;
prev_frame.M.GetRPY(prev_roll, prev_pitch, prev_yaw);
KDL::Rotation new_rotation = KDL::Rotation::RPY(cur_roll - prev_roll, cur_pitch - prev_pitch, cur_yaw - prev_yaw);
double roll, pitch, yaw;
new_rotation.GetRPY(roll, pitch, yaw);
// get the world coordinates of the joint
// KDL::Vector global_joint_location = links[link_name].link_frame * links[link_name].joint_frame * KDL::Vector(0, 0, 0);
// KDL::Vector previous_global_joint = prev_frame * KDL::Vector(0, 0, 0);
KDL::Vector cur_pos = cur_frame.p;
KDL::Vector prev_pos = prev_frame.p;
KDL::Vector new_position(cur_pos.x() /* - prev_pos.x()*/, cur_pos.y() /* - prev_pos.y()*/, cur_pos.z() /* - prev_pos.z()*/);
// KDL::Vector translation = global_joint_location - previous_global_joint;
// const KDL::Rotation &cur_rotation = cur_frame.M;
// const KDL::Vector &cur_translation = cur_frame.p;
// const KDL::Vector &rot_x = cur_rotation.UnitX();
// const KDL::Vector &rot_y = cur_rotation.UnitY();
// const KDL::Vector &rot_z = cur_rotation.UnitZ();
KDL::Frame joint_in_world = links[cur_link_name].link_frame * links[cur_link_name].joint_frame;
KDL::Frame prev_frame_inverse = prev_frame.Inverse();
KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world;
// joint_relative_to_prev = joint_relative_to_prev.Inverse();
// btTransform cur_frame_world_transform(
// btMatrix3x3(rot_x[0], rot_y[0], rot_z[0],
// rot_x[1], rot_y[1], rot_z[1],
// rot_x[2], rot_y[2], rot_z[2]),
// btVector3(btScalar(cur_translation.x()),
// btScalar(cur_translation.y()),
// btScalar(cur_translation.z())));
// btTransform transform;
// //transform.mult(prev_frame_world_transform, cur_frame_world_transform /*.inverse()*/);
// transform = cur_frame float roll, pitch, yaw;
// transform.getRotation().getEulerZYX(yaw, pitch, roll);
// KDL::Rotation rot(KDL::Rotation::RPY(roll, pitch, yaw));
// btVector3 translation = transform.getOrigin();
// KDL::Vector trans((float)translation.getX(), (float)translation.getY(), (float)translation.getZ());
// const KDL::Segment &cur = links[cur_link_name];
// const KDL::Frame &cur_frame = cur.getFrameToTip();
// double cur_roll, cur_pitch, cur_yaw;
// cur_frame.M.GetRPY(cur_roll, cur_pitch, cur_yaw);
// double prev_roll, prev_pitch, prev_yaw;
// prev_frame.M.GetRPY(prev_roll, prev_pitch, prev_yaw);
// KDL::Rotation new_rotation = KDL::Rotation::RPY(cur_roll - prev_roll, cur_pitch - prev_pitch, cur_yaw - prev_yaw);
// double roll, pitch, yaw;
// new_rotation.GetRPY(roll, pitch, yaw);
// KDL::Vector cur_pos = cur_frame.p;
// KDL::Vector prev_pos = prev_frame.p;
// KDL::Vector new_position(cur_pos.x() /* - prev_pos.x()*/, cur_pos.y() /* - prev_pos.y()*/, cur_pos.z() /* - prev_pos.z()*/);
// // const KDL::Rotation &cur_rotation = cur_frame.M;
// // const KDL::Vector &cur_translation = cur_frame.p;
// // const KDL::Vector &rot_x = cur_rotation.UnitX();
// // const KDL::Vector &rot_y = cur_rotation.UnitY();
// // const KDL::Vector &rot_z = cur_rotation.UnitZ();
// // btTransform cur_frame_world_transform(
// // btMatrix3x3(rot_x[0], rot_y[0], rot_z[0],
// // rot_x[1], rot_y[1], rot_z[1],
// // rot_x[2], rot_y[2], rot_z[2]),
// // btVector3(btScalar(cur_translation.x()),
// // btScalar(cur_translation.y()),
// // btScalar(cur_translation.z())));
// // btTransform transform;
// // //transform.mult(prev_frame_world_transform, cur_frame_world_transform /*.inverse()*/);
// // transform = cur_frame float roll, pitch, yaw;
// // transform.getRotation().getEulerZYX(yaw, pitch, roll);
// // KDL::Rotation rot(KDL::Rotation::RPY(roll, pitch, yaw));
// // btVector3 translation = transform.getOrigin();
// // KDL::Vector trans((float)translation.getX(), (float)translation.getY(), (float)translation.getZ());
// EXPERIMENTAL - update rotation axis
KDL::Vector uncorrected_axis(0, 0, 0);
if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotX)
{
uncorrected_axis = KDL::Vector(1, 0, 0);
}
else if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotY)
{
uncorrected_axis = KDL::Vector(0, 1, 0);
}
else if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotZ)
{
uncorrected_axis = KDL::Vector(0, 0, 1);
}
KDL::Vector corrected_axis = joint_relative_to_prev.M * uncorrected_axis;
double x = fabs(corrected_axis.x());
double y = fabs(corrected_axis.y());
double z = fabs(corrected_axis.z());
const string &name(links[cur_link_name].joint.getName());
if (x > y and x > z)
{
links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotX);
}
else if (y > x and y > z)
{
links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotY);
}
else if (z > x and z > y)
{
links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotZ);
}
// END EXPERIMENTAL
KDL::RigidBodyInertia inertia(links[cur_link_name].mass, links[cur_link_name].com_location, links[cur_link_name].rotational_inertia);
KDL::Segment to_add(links[cur_link_name].link_name, links[cur_link_name].joint, joint_relative_to_prev, inertia);
cout << cur_link_name << endl;
// cout << "\tprev_frame world transform (";
double roll, pitch, yaw;
cout << " joint in world coordinates:" << endl;
cout << joint_in_world << endl;
cout << " joint in previous joint's frame:" << endl;
cout << joint_relative_to_prev << endl;
cout << " previous frame inverse:" << endl;
cout << prev_frame_inverse << endl;
cout << "Joint type: " << links[cur_link_name].joint.getTypeName() << "\n\n"
<< endl;
cout << "\tOrigin position: " << new_position.x() << ", " << new_position.y() << ", " << new_position.z() << endl;
cout << "\tFrame rotation (roll, pitch, yaw): (" << roll << ", " << pitch << ", " << yaw << ")" << endl;
// joint_relative_to_prev.M.GetRPY(roll, pitch, yaw);
// cout << "\tOrigin position: " << joint_relative_to_prev.p.x() << ", " << joint_relative_to_prev.p.y() << ", " << joint_relative_to_prev.p.z() << endl;
// cout << "\tFrame rotation (roll, pitch, yaw): (" << roll << ", " << pitch << ", " << yaw << ")" << endl;
KDL::Segment to_add(cur.getName(), cur.getJoint(), KDL::Frame(new_rotation, new_position), cur.getInertia());
//KDL::Segment to_add(cur.getName(), cur.getJoint(), KDL::Frame(new_rotation, new_position), cur.getInertia());
arm.addSegment(to_add);
if (simple_arm.getNrOfSegments() < 4)
{
simple_arm.addSegment(to_add);
}
prev_frame = cur_frame;
prev_frame = joint_in_world;
// prev_frame_world_transform = cur_frame_world_transform;
}

34
rrrobot_src/src/arm_control/src/arm_controller.cpp
View File

@@ -17,7 +17,7 @@ using namespace KDL;
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::Chain chain = arm.getArm();
KDL::Chain correct_chain;
KDL::Chain simple_chain;
void angle_callback(const simulation_env::arm_angles &msg)
{
@@ -43,7 +43,21 @@ void angle_callback(const simulation_env::arm_angles &msg)
kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
if (kinematics_status >= 0)
{
std::cout << cartpos << std::endl;
cout << nj << endl;
cout << "shoulder_pivot: " << jointpositions(0) << endl;
cout << "shoulder_joint: " << jointpositions(1) << endl;
cout << "elbow_joint: " << jointpositions(2) << endl;
cout << "wrist_pivot: " << jointpositions(3) << endl;
cout << "wrist_joint: " << jointpositions(4) << endl;
cout << "end_effector_pivot: " << jointpositions(5) << endl;
std::cout << cartpos.p << std::endl;
double roll, pitch, yaw;
cartpos.M.GetRPY(roll, pitch, yaw);
cout << "roll: " << roll << endl;
cout << "pitch: " << pitch << endl;
cout << "yaw: " << yaw << endl;
cout << endl;
// std::cout << cartpos << std::endl;
//printf("%s \n", "Succes, thanks KDL!");
}
else
@@ -65,6 +79,22 @@ int main(int argc, char **argv)
// publisher = nh.advertise<simulation_env::arm_command>("/arm_node/arm_commands", 1000);
ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback);
cout << "0 positions for each segment" << endl;
cout << "Base:" << endl;
cout << chain.getSegment(0).pose(0.0) << endl;
cout << "Link_1:" << endl;
cout << chain.getSegment(0).pose(0.0) * chain.getSegment(1).pose(0.0) << endl;
cout << "Link_2:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) << endl;
cout << "Link_3:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) * chain.getSegment(3).pose(0.0) << endl;
cout << "Link_4:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) * chain.getSegment(3).pose(0.0) * chain.getSegment(4).pose(0.0) << endl;
cout << "Link_5:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) * chain.getSegment(3).pose(0.0) * chain.getSegment(4).pose(0.0) * chain.getSegment(5).pose(0.0) << endl;
cout << "Link_6:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) * chain.getSegment(3).pose(0.0) * chain.getSegment(4).pose(0.0) * chain.getSegment(5).pose(0.0) * chain.getSegment(6).pose(0.0) << endl;
// Assign some values to the joint positions
// for (unsigned int i = 0; i < nj; i++)
// {

103
rrrobot_src/src/arm_control/test/test.cpp
View File

@@ -11,16 +11,23 @@
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>
#include <arm.h>
#include <fstream>
using namespace KDL;
using std::cin;
using std::cout;
using std::endl;
using std::ofstream;
int main(int argc, char **argv)
{
//Definition of a kinematic chain & add segments to the chain
KDL::Chain chain;
chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0))));
chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
Arm arm(/*"/home/rrrobot/rrrobot_src/src/gazebo_models/basic_arm/model.sdf"); /(*/ "/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::Chain chain = arm.getArm();
// chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.480))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.645))));
// chain.addSegment(Segment(Joint(Joint::RotZ)));
@@ -33,29 +40,83 @@ int main(int argc, char **argv)
// Create joint array
unsigned int nj = chain.getNrOfJoints();
KDL::JntArray jointpositions = JntArray(nj);
// Assign some values to the joint positions
for (unsigned int i = 0; i < nj; i++)
for (int pos = 0; pos < nj; ++pos)
{
float myinput;
printf("Enter the position of joint %i: ", i);
scanf("%e", &myinput);
jointpositions(i) = (double)myinput;
jointpositions(pos) = 0;
}
// // Assign some values to the joint positions for (unsigned int i = 0; i < nj; i++)
// for (int i = 0; i < nj; ++i)
// {
// float myinput;
// printf("Enter the position of joint %i: ", i);
// scanf("%e", &myinput);
// jointpositions(i) = (double)myinput;
// }
// // Create the frame that will contain the results
// KDL::Frame cartpos;
// // Calculate forward position kinematics
// bool kinematics_status;
// kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
// if (kinematics_status >= 0)
// {
// // cout << segments << endl;
// std::cout << cartpos << std::endl;
// // printf("%s \n", "Succes, thanks KDL!");
// }
// else
// {
// // printf("%s \n", "Error: could not calculate forward kinematics :(");
// }
// while (1)
// {
// cout << "Enter the shoulder_pivot angle: ";
// cin >> jointpositions(0);
// cout << "Enter the shoulder_joint angle: ";
// cin >> jointpositions(1);
// cout << "Enter the elbow_joint angle: ";
// cin >> jointpositions(2);
// cout << "Enter the wrist_pivot angle: ";
// cin >> jointpositions(3);
// cout << "Enter the wrist_joint angle: ";
// cin >> jointpositions(4);
// cout << "Enter the end_effector_pivot angle: ";
// cin >> jointpositions(5);
// Create the frame that will contain the results
KDL::Frame cartpos;
ofstream f_raw("configurations.txt");
ofstream f_corrected("corrected_configurations.txt");
// Calculate forward position kinematics
bool kinematics_status;
kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
if (kinematics_status >= 0)
for (double shoulder_pivot = 0.0; shoulder_pivot <= 6.28; shoulder_pivot += 0.1)
{
std::cout << cartpos << std::endl;
printf("%s \n", "Succes, thanks KDL!");
}
else
{
printf("%s \n", "Error: could not calculate forward kinematics :(");
// Calculate forward position kinematics
bool kinematics_status;
// for (int segments = 0; segments <= chain.getNrOfSegments(); ++segments)
// {
// kinematics_status = fksolver.JntToCart(jointpositions, cartpos, segments);
// if (kinematics_status >= 0)
// {
// cout << segments << endl;
// std::cout << cartpos << std::endl;
// // printf("%s \n", "Succes, thanks KDL!");
// }
// else
// {
// // printf("%s \n", "Error: could not calculate forward kinematics :(");
// }
// }
jointpositions(5) = shoulder_pivot;
kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
cout << "Final position: " << endl;
cout << cartpos << endl;
KDL::Vector pos = cartpos.p;
f_raw << pos.x() << "," << pos.y() << "," << pos.z() << "\n";
KDL::Vector corrected = cartpos.M * cartpos.p;
f_corrected << corrected.x() << "," << corrected.y() << "," << corrected.z() << "\n";
}
// }
}