Jacobian Magic: PiPER Arm Kinematics Unleashed

Abstract

This chapter achieves the forward kinematics solution and the Jacobian method for inverse kinematics solution of the AgileX PiPER Robotic Arm based on the Eigen linear algebra library, as well as the implementation of the custom interactive marker interactive_marker_utils.

Function Demonstration

Preparation Before Use

Hardware Preparation

• AgileX Robotics PiPER Robotic Arm

Software Environment Configuration

• For the deployment of the PIPER arm driver: piper_sdk
• For the deployment of the PIPER arm ROS control node: piper_ros
• Install the Eigen linear algebra library:

sudo apt install libeigen3-dev

Preparation of DH Parameters Table and Joint Limits for AgileX PiPER

The modified DH parameters table and joint limits of PIPER can be found in the AgileX PIPER User Manual:

Forward Kinematics Calculation (FK)

The calculation process of forward kinematics (FK) is to compute the pose of a certain joint of the robotic arm in the 3D world based on the angle values of each joint. This article takes the last rotating joint (joint6) of the robotic arm as an example.

Preparation of DH Parameters

• Construct the forward kinematics calculation program according to PiPER’s DH parameters table. Based on the modified DH parameters table of AgileX PiPER in Section 1.3, the following parameters are obtained:

//modified DH Parameter [alpha, a, d, theta_offset]
dh_params_ = {
    {0,         0,          0.123,      0},                     // Joint 1
    {-M_PI/2,   0,          0,          -172.22/180*M_PI},      // Joint 2 
    {0,         0.28503,    0,          -102.78/180*M_PI},      // Joint 3
    {M_PI/2,    -0.021984,  0.25075,    0},                     // Joint 4
    {-M_PI/2,   0,          0,          0},                     // Joint 5
    {M_PI/2,    0,          0.091,      0}                      // Joint 6
};

To convert to standard DH, the following conversion rules can be referred to:

• From Standard DH → Modified DH:αᵢ₋₁(Standard) = αᵢ(Modified)aᵢ₋₁(Standard) = aᵢ(Modified)dᵢ(Standard) = dᵢ(Modified)θᵢ(Standard) = θᵢ(Modified)
• From Modified DH → Standard DH:αᵢ(Standard) = αᵢ₊₁(Modified)aᵢ(Standard) = aᵢ₊₁(Modified)dᵢ(Standard) = dᵢ(Modified)θᵢ(Standard) = θᵢ(Modified)

Standard DH Parameters After Conversion:

// Standard DH  [alpha, a, d, theta_offset]
dh_params_ = {
    {-M_PI/2,   0,          0.123,      0},                     // Joint 1
    {0,         0.28503,    0,          -172.22/180*M_PI},      // Joint 2 
    {M_PI/2,    -0.021984,  0,          -102.78/180*M_PI},      // Joint 3
    {-M_PI/2,   0,          0.25075,    0},                     // Joint 4
    {M_PI/2,    0,          0,          0},                     // Joint 5
    {0,         0,          0.091,      0}                      // Joint 6
};

• Preparation of DH Transformation Matrix
• Modified DH Transformation Matrix:

Rewritten in Eigen:

T << cos(theta),            -sin(theta),            0,             a,
     sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -sin(alpha)*d,
     sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),  cos(alpha)*d,
     0,                     0,                      0,             1;

• Standard DH Transformation Matrix:

Rewritten in Eigen:

T << cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), a*cos(theta),
     sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta),
     0,           sin(alpha),             cos(alpha),            d,
     0,           0,                      0,                     1;

• Implementation of the Key Function computeFK()For the complete code, see the repository:https://github.com/agilexrobotics/Agilex-College.git

Eigen::Matrix4d computeFK(const std::vector<double>& joint_values) {
    //Check the number of input joints (at least six)
    if (joint_values.size() < 6) {
        throw std::runtime_error("Piper arm requires at least 6 joint values for FK");
    }

    //Initialize the identity matrix as the initial transformation
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();

// For each joint:
// Calculate actual joint angle = input value + offset
// Get fixed parameter d value
// Calculate the transformation matrix for the current joint and accumulate it to the total transformation
    for (size_t i = 0; i < 6; ++i) {
        double theta = joint_values[i] + dh_params_[i][3];  // θ = joint_value + θ_offset
        double d = dh_params_[i][2];                       // d = d_fixed (if it's rotating joint)

        T *= computeTransform(
            dh_params_[i][0],  // alpha
            dh_params_[i][1],  // a
            d,                 // d
            theta              // theta
            );
    }

    //Return to final transformation matrix
    return T;
}

Verification of FK Calculation Accuracy

• Start the forward kinematics verification program:

ros2 launch piper_kinematics test_fk.launch.py

• Start the RVIZ simulation, enable TF tree display, and check whether the pose of link6_from_fk (end of the arm computed by FK) coincides with link6 (pose from joint_state_publisher):

ros2 launch piper_description display_piper_with_joint_state_pub_gui.launch.py

The coincidence is very high; the error between link6_from_fk and link6 is basically within four decimal places.

Inverse Kinematics Solution (IK)

Inverse kinematics calculates the joint angles required for the manipulator to reach a given target pose.

Confirmation of Joint Limits

• Each joint limit must be confirmed to ensure the IK solution does not exceed the robot’s physical limits.
• The limits for the PiPER Robotic Arm (from Section 1.3) are:

• The joint limit matrix in code:

std::vector<std::pair<double, double>> limits = {
    {-154/180*M_PI, 154/180*M_PI},    // Joint 1
    {0, 195/180*M_PI},                 // Joint 2
    {-175/180*M_PI, 0},                // Joint 3
    {-102/180*M_PI, 102/180*M_PI},     // Joint 4
    {-75/180*M_PI, 75/180*M_PI},       // Joint 5
    {-120/180*M_PI, 120/180*M_PI}      // Joint 6
};

Brief Steps for Implementing IK Using the Jacobian Matrix Method

Solution Process:

• Compute the Pose ErrorCalculate the difference between the current pose and the target pose, represented in 6D (3 position + 3 orientation).e = target pose - current pose
• Check if the Error is Below ThresholdYes: Return the current joint angles θ as the solution.No: Continue iteration.
• Calculate the Jacobian Matrix (J)Compute the 6×6 Jacobian matrix.
• Compute the Damped PseudoinverseCalculate the damped pseudoinverse of the Jacobian matrix:J⁺ = Jᵀ (J Jᵀ + λ² I)⁻¹
Where:λ is the damping factor (avoids numerical instability near singularities).
• Compute the Joint IncrementΔθ = J⁺ * e

• Update Joint Anglesθ = θ + ΔθApply joint limits.
• Normalize AnglesNormalize joint angles to a standard range, e.g., [-π, π].
• Check Maximum IterationNo: Go back to step 2.Yes: Throw an "unconverged" error.

Key Function computeIK()

std::vector<double> computeIK(
    const std::vector<double>& initial_guess, 
    const Eigen::Matrix4d& target_pose,
    bool verbose = false,
    Eigen::VectorXd* final_error = nullptr) 
{
    if (initial_guess.size() < 6) {
        throw std::runtime_error("Initial guess must have at least 6 joint values");
    }

    std::vector<double> joint_values = initial_guess;
    Eigen::Matrix4d current_pose;
    Eigen::VectorXd error(6);
    bool success = false;

    for (int iter = 0; iter < max_iterations_; ++iter) {
        current_pose = fk_.computeFK(joint_values);
        error = computePoseError(current_pose, target_pose);

        if (verbose) {
            std::cout << "Iteration " << iter << ": error norm = " << error.norm() 
                      << " (pos: " << error.head<3>().norm() 
                      << ", orient: " << error.tail<3>().norm() << ")\n";
        }

        if (error.head<3>().norm() < position_tolerance_ && 
            error.tail<3>().norm() < orientation_tolerance_) {
            success = true;

Using interactive_marker to Publish 3D Target Points

• Install ROS2 dependency packages:

sudo apt install ros-${ROS_DISTRO}-interactive-markers ros-${ROS_DISTRO}-tf2-ros

• Start interactive_marker_utils to realize the publication of 3D space target points:

ros2 launch interactive_marker_utils marker.launch.py

• Start RVIZ2 to observe the Marker:

4.Drag the Marker and use ros2 topic echo to observe whether the target point published by the Marker changes

Verify IK Correctness in RVIZ via interactive_marker

• Start the RVIZ simulation demo of AgileX PiPER. Since there is no joint_state_publisher at this time, the model will not be displayed correctly:

ros2 launch piper_description display_piper.launch.py

• Next, start the IK node and the interactive_marker node (in the same launch file). After successful startup, the robotic arm can be displayed normally:

ros2 launch piper_kinematics piper_ik.launch.py

• Use the interactive_marker to control the robotic arm for IK solution:

By dragging the interactive_marker, it can be seen that the IK successfully solves the angle of each joint

• By dragging the interactive_marker, it can be seen that the IK successfully solves the angle of each joint

• If the interactive_marker is dragged to a position where the solution is impossible, an exception will be thrown

Verify IK on the Actual PiPER Robotic Arm

• First, start the script for connecting to the CAN communication of PIPER

cd piper_ros
./find_all_can_port.sh
./can_activate.sh

• Start the actual machine control node of PiPER

ros2 launch piper my_start_single_piper_rviz.launch.py

• Next, start the IK node and the interactive_marker node (in the same launch file). It can be seen that the robotic arm moves to the HOME position

ros2 launch piper_kinematics piper_ik.launch.py

• Drag the interactive_marker and observe the movement of the PIPER Robotic Arm.

That’s a wrap!

With this setup, you can:

• Compute and verify FK and IK for the PiPER arm.
• Use interactive_marker to move the arm in 3D space.
• Safely experiment in both simulation and on the actual robot.

Hope this inspires your own projects or experiments with PiPER!