diff --git a/rrrobot_src/src/arm_control/CMakeLists.txt b/rrrobot_src/src/arm_control/CMakeLists.txt index cf8517a..74e95f3 100644 --- a/rrrobot_src/src/arm_control/CMakeLists.txt +++ b/rrrobot_src/src/arm_control/CMakeLists.txt @@ -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 ) ############# diff --git a/rrrobot_src/src/arm_control/include/arm_control/arm.h b/rrrobot_src/src/arm_control/include/arm_control/arm.h index b01d77e..0ddb737 100644 --- a/rrrobot_src/src/arm_control/include/arm_control/arm.h +++ b/rrrobot_src/src/arm_control/include/arm_control/arm.h @@ -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 diff --git a/rrrobot_src/src/arm_control/src/arm.cpp b/rrrobot_src/src/arm_control/src/arm.cpp index dc1e726..6f0e0f6 100644 --- a/rrrobot_src/src/arm_control/src/arm.cpp +++ b/rrrobot_src/src/arm_control/src/arm.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -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("name"); cout << "Found " << model_name << " model" << endl; - unordered_map links; + unordered_map links; unordered_map link_ordering; string first_link; @@ -53,22 +56,25 @@ Arm::Arm(const std::string &sdf_file) cout << "Link: " << link->Get("name") << endl; const string name(link->Get("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("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("ixx"), inertia->Get("iyy"), inertia->Get("izz"), @@ -76,20 +82,20 @@ Arm::Arm(const std::string &sdf_file) inertia->Get("ixz"), inertia->Get("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("ixx") << endl; // cout << inertia->Get("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; } diff --git a/rrrobot_src/src/arm_control/src/arm_controller.cpp b/rrrobot_src/src/arm_control/src/arm_controller.cpp index e285f83..bc5e7cb 100644 --- a/rrrobot_src/src/arm_control/src/arm_controller.cpp +++ b/rrrobot_src/src/arm_control/src/arm_controller.cpp @@ -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("/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++) // { diff --git a/rrrobot_src/src/arm_control/test/test.cpp b/rrrobot_src/src/arm_control/test/test.cpp index 886a9f9..e71248c 100644 --- a/rrrobot_src/src/arm_control/test/test.cpp +++ b/rrrobot_src/src/arm_control/test/test.cpp @@ -11,16 +11,23 @@ #include #include #include +#include +#include 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"; } + // } } \ No newline at end of file