From 6acb7158a010fd17f5cd192bdbae55d7d924c87b Mon Sep 17 00:00:00 2001 From: Derek Witcpalek Date: Thu, 9 Apr 2020 17:05:06 -0400 Subject: [PATCH] Working kinematic representation. One caveat: there is no link representation between the world frame and the robot base frame. Sometimes when the simulation starts, the arm slides slightly off of (0, 0, 0) which causes joint positions to be slightly off (by the same amount). --- rrrobot_src/src/arm_control/CMakeLists.txt | 9 +- rrrobot_src/src/arm_control/src/arm.cpp | 75 +++++++------- .../test/test_internal_arm_representation.cpp | 98 +++++++++++++++++++ rrrobot_src/test.sh | 2 + 4 files changed, 148 insertions(+), 36 deletions(-) create mode 100644 rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp create mode 100755 rrrobot_src/test.sh 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