sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-18 23:22: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) find_package(PkgConfig REQUIRED)
pkg_check_modules(SDF sdformat 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 ## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed ## modules and global scripts declared therein get installed
@@ -127,7 +127,7 @@ include_directories(
${roscpp_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS}
/usr/include/bullet /usr/include/bullet
/usr/include/sdformat-6.2 /usr/include/sdformat-6.2
/usr/include/ignition/math6 /usr/include/ignition/math4
/home/rrrobot/rrrobot_src/devel/include/ /home/rrrobot/rrrobot_src/devel/include/
) )
include_directories( include_directories(
@@ -148,7 +148,7 @@ include_directories(
## With catkin_make all packages are built within a single CMake context ## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide ## 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(${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 ## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the ## 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} ${kdl_parser_LIBRARIES}
${orocos_kdl_LIBRARIES} ${orocos_kdl_LIBRARIES}
${SDF_LIBRARIES} ${SDF_LIBRARIES}
ignition-math6::ignition-math6 ignition-math4::ignition-math4
) )
target_link_libraries(test target_link_libraries(test
@@ -174,7 +174,7 @@ target_link_libraries(test
${kdl_parser_LIBRARIES} ${kdl_parser_LIBRARIES}
${orocos_kdl_LIBRARIES} ${orocos_kdl_LIBRARIES}
${SDF_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: private:
KDL::Chain arm; KDL::Chain arm;
KDL::Chain simple_arm;
/* /*
* i_com: the inertia about the center of mass of this link * 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 <sdf/sdf.hh>
#include <ignition/math/Pose3.hh> #include <ignition/math/Pose3.hh>
#include <LinearMath/btTransform.h> #include <LinearMath/btTransform.h>
#include <kdl/frames_io.hpp>
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
@@ -17,11 +18,13 @@ using std::vector;
struct FrameInformation struct FrameInformation
{ {
string link_name; string link_name; // link name
KDF::Joint joint; KDL::Joint joint; // joint information (name, type)
KDF::Frame link_frame; KDL::Frame link_frame; // link location (world coordinates)
KDF::Frame joint_frame; KDL::Frame joint_frame; // joint location relative to parent(link_name)
RigidBodyInertia inertia; 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"); const string model_name = model->Get<string>("name");
cout << "Found " << model_name << " model" << endl; cout << "Found " << model_name << " model" << endl;
unordered_map<string, KDL::Segment> links; unordered_map<string, FrameInformation> links;
unordered_map<string, string> link_ordering; unordered_map<string, string> link_ordering;
string first_link; string first_link;
@@ -53,22 +56,25 @@ Arm::Arm(const std::string &sdf_file)
cout << "Link: " << link->Get<string>("name") << endl; cout << "Link: " << link->Get<string>("name") << endl;
const string name(link->Get<string>("name")); 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"); const sdf::ElementPtr inertial_data = link->GetElement("inertial");
float mass = inertial_data->Get<float>("mass"); float mass = inertial_data->Get<float>("mass");
links[name].mass = mass;
cout << "Mass: " << mass << endl; cout << "Mass: " << mass << endl;
string inertial_data_s = inertial_data->GetElement("pose")->GetValue()->GetAsString(); stringstream inertial_data_ss(inertial_data->GetElement("pose")->GetValue()->GetAsString());
stringstream inertial_data_ss(inertial_data_s); // stringstream inertial_data_ss(inertial_data_s);
KDL::Vector inertial_frame_info; // KDL::Vector inertial_frame_info;
inertial_data_ss >> inertial_frame_info.data[0]; 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[1]; // inertial_data_ss >> inertial_frame_info.data[0];
inertial_data_ss >> inertial_frame_info.data[2]; // inertial_data_ss >> inertial_frame_info.data[1];
// inertial_data_ss >> inertial_frame_info.data[2];
const sdf::ElementPtr inertia = inertial_data->GetElement("inertia"); 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>("ixx"),
inertia->Get<float>("iyy"), inertia->Get<float>("iyy"),
inertia->Get<float>("izz"), inertia->Get<float>("izz"),
@@ -76,20 +82,20 @@ Arm::Arm(const std::string &sdf_file)
inertia->Get<float>("ixz"), inertia->Get<float>("ixz"),
inertia->Get<float>("iyz")); 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 // Transformation from world to link coordinates
// stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString()); stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString());
// float x, y, z; float x, y, z;
// float roll, pitch, yaw; float roll, pitch, yaw;
// frame_location >> x >> y >> z >> roll >> pitch >> yaw; frame_location >> x >> y >> z >> roll >> pitch >> yaw;
// KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw); KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw);
// KDL::Vector link_position(x, y, z); KDL::Vector link_position(x, y, z);
// KDL::Frame frame(rotation, link_position); 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); //arm.addSegment(cur_link);
links[name] = cur_link; //links[name] = cur_link;
// cout << inertia->Get<float>("ixx") << endl; // cout << inertia->Get<float>("ixx") << endl;
// cout << inertia->Get<float>("iyy") << endl; // cout << inertia->Get<float>("iyy") << endl;
@@ -166,7 +172,7 @@ Arm::Arm(const std::string &sdf_file)
else if (axis_num == 2) else if (axis_num == 2)
joint_type = KDL::Joint::JointType::RotZ; 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()); stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString());
float x, y, z; 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::Rotation frame_rotation = KDL::Rotation::RPY(roll, pitch, yaw);
KDL::Vector frame_location(x, y, z); 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 //KDL::Segment cur_segment = arm.getSegment
joint = joint->GetNextElement("joint"); joint = joint->GetNextElement("joint");
@@ -189,51 +196,118 @@ Arm::Arm(const std::string &sdf_file)
{ {
cur_link_name = link_ordering[cur_link_name]; cur_link_name = link_ordering[cur_link_name];
const KDL::Segment &cur = links[cur_link_name]; // get the world coordinates of the joint
const KDL::Frame &cur_frame = cur.getFrameToTip(); // KDL::Vector global_joint_location = links[link_name].link_frame * links[link_name].joint_frame * KDL::Vector(0, 0, 0);
double cur_roll, cur_pitch, cur_yaw; // KDL::Vector previous_global_joint = prev_frame * KDL::Vector(0, 0, 0);
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 translation = global_joint_location - previous_global_joint;
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; KDL::Frame joint_in_world = links[cur_link_name].link_frame * links[cur_link_name].joint_frame;
// const KDL::Vector &cur_translation = cur_frame.p; KDL::Frame prev_frame_inverse = prev_frame.Inverse();
// const KDL::Vector &rot_x = cur_rotation.UnitX(); KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world;
// const KDL::Vector &rot_y = cur_rotation.UnitY(); // joint_relative_to_prev = joint_relative_to_prev.Inverse();
// const KDL::Vector &rot_z = cur_rotation.UnitZ();
// btTransform cur_frame_world_transform( // const KDL::Segment &cur = links[cur_link_name];
// btMatrix3x3(rot_x[0], rot_y[0], rot_z[0], // const KDL::Frame &cur_frame = cur.getFrameToTip();
// rot_x[1], rot_y[1], rot_z[1], // double cur_roll, cur_pitch, cur_yaw;
// rot_x[2], rot_y[2], rot_z[2]), // cur_frame.M.GetRPY(cur_roll, cur_pitch, cur_yaw);
// btVector3(btScalar(cur_translation.x()), // double prev_roll, prev_pitch, prev_yaw;
// btScalar(cur_translation.y()), // prev_frame.M.GetRPY(prev_roll, prev_pitch, prev_yaw);
// btScalar(cur_translation.z()))); // KDL::Rotation new_rotation = KDL::Rotation::RPY(cur_roll - prev_roll, cur_pitch - prev_pitch, cur_yaw - prev_yaw);
// btTransform transform; // double roll, pitch, yaw;
// //transform.mult(prev_frame_world_transform, cur_frame_world_transform /*.inverse()*/); // new_rotation.GetRPY(roll, pitch, yaw);
// transform = cur_frame float roll, pitch, yaw;
// transform.getRotation().getEulerZYX(yaw, pitch, roll); // KDL::Vector cur_pos = cur_frame.p;
// KDL::Rotation rot(KDL::Rotation::RPY(roll, pitch, yaw)); // KDL::Vector prev_pos = prev_frame.p;
// btVector3 translation = transform.getOrigin(); // KDL::Vector new_position(cur_pos.x() /* - prev_pos.x()*/, cur_pos.y() /* - prev_pos.y()*/, cur_pos.z() /* - prev_pos.z()*/);
// KDL::Vector trans((float)translation.getX(), (float)translation.getY(), (float)translation.getZ());
// // 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 << cur_link_name << endl;
// cout << "\tprev_frame world transform ("; // 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; // joint_relative_to_prev.M.GetRPY(roll, pitch, yaw);
cout << "\tFrame rotation (roll, pitch, yaw): (" << roll << ", " << pitch << ", " << yaw << ")" << endl; // 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); 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; // 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"); Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::Chain chain = arm.getArm(); KDL::Chain chain = arm.getArm();
KDL::Chain correct_chain; KDL::Chain simple_chain;
void angle_callback(const simulation_env::arm_angles &msg) 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); kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
if (kinematics_status >= 0) 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!"); //printf("%s \n", "Succes, thanks KDL!");
} }
else else
@@ -65,6 +79,22 @@ int main(int argc, char **argv)
// publisher = nh.advertise<simulation_env::arm_command>("/arm_node/arm_commands", 1000); // publisher = nh.advertise<simulation_env::arm_command>("/arm_node/arm_commands", 1000);
ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback); 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 // Assign some values to the joint positions
// for (unsigned int i = 0; i < nj; i++) // 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 <kdl/frames_io.hpp>
#include <stdio.h> #include <stdio.h>
#include <iostream> #include <iostream>
#include <arm.h>
#include <fstream>
using namespace KDL; using namespace KDL;
using std::cin;
using std::cout;
using std::endl;
using std::ofstream;
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
//Definition of a kinematic chain & add segments to the chain //Definition of a kinematic chain & add segments to the chain
KDL::Chain chain; 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");
chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0)))); KDL::Chain chain = arm.getArm();
chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1)))); // 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, 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.480))));
// chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.645)))); // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.645))));
// chain.addSegment(Segment(Joint(Joint::RotZ))); // chain.addSegment(Segment(Joint(Joint::RotZ)));
@@ -33,29 +40,83 @@ int main(int argc, char **argv)
// Create joint array // Create joint array
unsigned int nj = chain.getNrOfJoints(); unsigned int nj = chain.getNrOfJoints();
KDL::JntArray jointpositions = JntArray(nj); KDL::JntArray jointpositions = JntArray(nj);
for (int pos = 0; pos < nj; ++pos)
// Assign some values to the joint positions
for (unsigned int i = 0; i < nj; i++)
{ {
float myinput; jointpositions(pos) = 0;
printf("Enter the position of joint %i: ", i);
scanf("%e", &myinput);
jointpositions(i) = (double)myinput;
} }
// // 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 // Create the frame that will contain the results
KDL::Frame cartpos; KDL::Frame cartpos;
ofstream f_raw("configurations.txt");
ofstream f_corrected("corrected_configurations.txt");
// Calculate forward position kinematics for (double shoulder_pivot = 0.0; shoulder_pivot <= 6.28; shoulder_pivot += 0.1)
bool kinematics_status;
kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
if (kinematics_status >= 0)
{ {
std::cout << cartpos << std::endl; // Calculate forward position kinematics
printf("%s \n", "Succes, thanks KDL!"); bool kinematics_status;
} // for (int segments = 0; segments <= chain.getNrOfSegments(); ++segments)
else // {
{ // kinematics_status = fksolver.JntToCart(jointpositions, cartpos, segments);
printf("%s \n", "Error: could not calculate forward kinematics :("); // 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";
} }
// }
} }