diff --git a/rrrobot_src/src/arm_control/CMakeLists.txt b/rrrobot_src/src/arm_control/CMakeLists.txt index 74e95f3..4d10e11 100644 --- a/rrrobot_src/src/arm_control/CMakeLists.txt +++ b/rrrobot_src/src/arm_control/CMakeLists.txt @@ -224,7 +224,14 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_arm_control.cpp) +catkin_add_gtest(${PROJECT_NAME}-test test/test_internal_arm_representation.cpp src/arm.cpp) +target_link_libraries(${PROJECT_NAME}-test + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/rrrobot_src/src/arm_control/src/arm.cpp b/rrrobot_src/src/arm_control/src/arm.cpp index 13780cf..74b8278 100644 --- a/rrrobot_src/src/arm_control/src/arm.cpp +++ b/rrrobot_src/src/arm_control/src/arm.cpp @@ -119,7 +119,7 @@ Arm::Arm(const std::string &sdf_file) else if (axis_num == 2) joint_type = KDL::Joint::JointType::RotZ; - links[child].joint = KDL::Joint(name, joint_type); + links[child].joint = KDL::Joint(name, joint_type); // correct stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString()); float x, y, z; @@ -128,13 +128,14 @@ 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].joint_frame = KDL::Frame(frame_rotation, frame_location); // incorrect --> this is actually the joint = joint->GetNextElement("joint"); } string cur_link_name = "world"; KDL::Frame prev_frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(0, 0, 0)); + KDL::Joint::JointType to_set(KDL::Joint::JointType::None); while (link_ordering.find(cur_link_name) != link_ordering.end()) { @@ -145,45 +146,47 @@ Arm::Arm(const std::string &sdf_file) KDL::Frame prev_frame_inverse = prev_frame.Inverse(); KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world; - // EXPERIMENTAL - update rotation axis - KDL::Vector uncorrected_axis(0, 0, 0); + // // 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); - } + // 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; + // 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()); + // 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 + // 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::Joint::JointType next(links[cur_link_name].joint.getType()); 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); + KDL::Segment to_add(links[cur_link_name].link_name, KDL::Joint(links[cur_link_name].joint.getName(), to_set), joint_relative_to_prev, inertia); + to_set = next; // cout << cur_link_name << endl; @@ -202,6 +205,8 @@ Arm::Arm(const std::string &sdf_file) prev_frame = joint_in_world; } + arm.addSegment(KDL::Segment(string("pseudo_link"), KDL::Joint("pseudo_joint", to_set), KDL::Frame::Identity(), KDL::RigidBodyInertia())); + cout << "# of arm segments: " << arm.getNrOfSegments() << "\t# of arm joints: " << arm.getNrOfJoints() << endl; fksolver.reset(new KDL::ChainFkSolverPos_recursive(arm)); } diff --git a/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp b/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp new file mode 100644 index 0000000..18af99c --- /dev/null +++ b/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp @@ -0,0 +1,98 @@ +#include + +#include "gtest/gtest.h" +#include +#include +#include + +const double ALLOWED_ERROR = 0.05; + +double getDistance(const KDL::Vector &a, const KDL::Vector &b) +{ + return sqrt(pow(a.x() - b.x(), 2) + pow(a.y() - b.y(), 2) + pow(a.z() - b.z(), 2)); +} + +TEST(ArmRepresentationTests, shoulder_pivot) +{ + Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf"); + + // Create joint array + unsigned int nj = arm.getArm().getNrOfJoints(); + KDL::JntArray jointpositions = KDL::JntArray(nj); + for (int pos = 0; pos < nj; ++pos) + { + jointpositions(pos) = 0; + } + + // Create the frame that will contain the results + KDL::Frame cartpos; + KDL::Vector correct(0.000767, -1.87014, 2.0658); + + jointpositions(0) = 0.0; + arm.calculateForwardKinematics(jointpositions, cartpos); + + EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR); + + jointpositions(0) = 2 * M_PI; + arm.calculateForwardKinematics(jointpositions, cartpos); + EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR); + + jointpositions(0) = M_PI; + arm.calculateForwardKinematics(jointpositions, cartpos); + correct = KDL::Vector(0.000767, 1.87014, 2.0658); + EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR); +} + +TEST(ArmRepresentationTests, elbow_pivot) +{ + Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf"); + + // Create joint array + unsigned int nj = arm.getArm().getNrOfJoints(); + KDL::JntArray jointpositions = KDL::JntArray(nj); + for (int pos = 0; pos < nj; ++pos) + { + jointpositions(pos) = 0; + } + + // Create the frame that will contain the results + KDL::Frame cartpos; + KDL::Vector correct(0.000767, -1.87014, 2.0658); + + for (double pos = 0.0; pos <= M_2_PI; pos += 0.1) + { + jointpositions(3) = pos; + arm.calculateForwardKinematics(jointpositions, cartpos); + EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR); + } +} + +TEST(ArmRepresentationTests, end_effector_pivot) +{ + Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf"); + + // Create joint array + unsigned int nj = arm.getArm().getNrOfJoints(); + KDL::JntArray jointpositions = KDL::JntArray(nj); + for (int pos = 0; pos < nj; ++pos) + { + jointpositions(pos) = 0; + } + + // Create the frame that will contain the results + KDL::Frame cartpos; + KDL::Vector correct(0.000767, -1.87014, 2.0658); + + for (double pos = 0.0; pos <= M_2_PI; pos += 0.1) + { + jointpositions(5) = pos; + arm.calculateForwardKinematics(jointpositions, cartpos); + EXPECT_NEAR(getDistance(cartpos.p, correct), 0.0, ALLOWED_ERROR); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/rrrobot_src/test.sh b/rrrobot_src/test.sh new file mode 100755 index 0000000..f0068c4 --- /dev/null +++ b/rrrobot_src/test.sh @@ -0,0 +1,2 @@ +source build.sh +catkin_make run_tests