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).

2025-08-19 23:32:44 +00:00 · 2020-04-09 17:05:06 -04:00
parent 496a7351c2
commit 6acb7158a0
4 changed files with 148 additions and 36 deletions
--- 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()
--- 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));
 }
--- a/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp
+++ b/rrrobot_src/src/arm_control/test/test_internal_arm_representation.cpp
@@ -0,0 +1,98 @@
+#include <arm.h>
+
+#include "gtest/gtest.h"
+#include <kdl/frames_io.hpp>
+#include <kdl/chain.hpp>
+#include <cmath>
+
+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();
+}
--- a/rrrobot_src/test.sh
+++ b/rrrobot_src/test.sh
@@ -0,0 +1,2 @@
+source build.sh
+catkin_make run_tests