# Piper Arm Kinematics Implementation

**URL:** <https://discourse.openrobotics.org/t/piper-arm-kinematics-implementation/52479>\
**Category:** ROS General\
**Created:** [February 12, 2026, 3:03am UTC](https://discourse.openrobotics.org/t/piper-arm-kinematics-implementation/52479 "2026-02-12T03:03:50Z")\
**Posts on this page:** 1\
**Page:** 1

<div class="post-metadata">

**Author:** ![Agilex\_Robotics](https://sea2.discourse-cdn.com/flex022/user_avatar/discourse.openrobotics.org/agilex_robotics/32/10554_2.png) [@Agilex\_Robotics](https://discourse.openrobotics.org/u/Agilex_Robotics)\
**Post date:** [February 12, 2026, 3:03am UTC](https://discourse.openrobotics.org/t/piper-arm-kinematics-implementation/52479/1 "2026-02-12T03:03:50Z")

</div>

# Piper Arm Kinematics Implementation

### Abstract

This chapter implements the forward kinematics (FK) and Jacobian-based inverse kinematics (IK) for the AgileX PIPER robotic arm using the Eigen linear algebra library, as well as the implementation of custom interactive markers via `interactive_marker_utils`.

### Tags

Forward Kinematics, Jacobian-based Inverse Kinematics, RVIZ Simulation, Robotic Arm DH Parameters, Interactive Markers, AgileX PIPER

### Function Demonstration

### Code Repository

GitHub Link: [**https://github.com/agilexrobotics/Agilex-College.git**](https://github.com/agilexrobotics/Agilex-College.git)

* * *

### 1. Preparations Before Use

Reference Videos:

> **[松灵PIPER机械臂实现Eigen线代库解算正逆运动学-1-CSDN直播](https://live.csdn.net/v/492468)**
>
> 章实现基于线性代数库Eigen实现松灵PIPER机械臂的正解，逆解的雅各比方法，自定义交互式标记interactive\_marker\_utils的实现

> **[松灵PIPER机械臂实现Eigen线代库解算正逆运动学-2-CSDN直播](https://live.csdn.net/v/492470)**
>
> 本视频实现基于线性代数库Eigen实现松灵PIPER机械臂的正解，逆解的雅各比方法，自定义交互式标记interactive\_marker\_utils的实现

#### 1.1 Hardware Preparation

- AgileX Robotics Piper robotic arm

#### 1.2 Software Environment Configuration

1. For Piper arm driver deployment, refer to: [https://github.com/agilexrobotics/piper\_sdk/blob/1\_0\_0\_beta/README(ZH).MD](https://github.com/agilexrobotics/piper_sdk/blob/1_0_0_beta/README(ZH).MD)
2. For Piper arm ROS control node deployment, refer to: [https://github.com/agilexrobotics/piper\_ros/blob/noetic/README.MD](https://github.com/agilexrobotics/piper_ros/blob/noetic/README.MD)
3. Install the Eigen linear algebra library:

```bash
sudo apt install libeigen3-dev

```

#### 1.3 Prepare DH Parameters and Joint Limits for AgileX PIPER

The modified DH parameter table and joint limits of the PIPER arm can be found in the AgileX PIPER user manual:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/7/3/73065b56babf5f411b4d9f7523fbd96ecaaebf02.png)

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/0/a/0a2e1aec7988ec3cbcbcc7abaed7f6bedd16d697.png)

* * *

### 2. Forward Kinematics (FK) Calculation

The FK calculation process essentially converts angle values of each joint into the pose of a specific joint of the robotic arm in 3D space. This chapter takes `joint6` (the last rotary joint of the arm) as an example.

#### 2.1 Prepare DH Parameters

1. Build the FK calculation program based on the PIPER DH parameter table. From the modified DH parameter table of AgileX PIPER in Section 1.3, we obtain:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/2/e/2e667c18d72fdd275d3adcdbe049b7192e37d678.png)

```c
// Modified DH parameters [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
};

```

For conversion to Standard DH parameters, refer to the following rules:

**Standard DH ↔ Modified DH Conversion Rules:**

- Standard DH → Modified DH:  
αᵢ₋₁ (Standard) = αᵢ (Modified)  
aᵢ₋₁ (Standard) = aᵢ (Modified)  
dᵢ (Standard) = dᵢ (Modified)  
θᵢ (Standard) = θᵢ (Modified)

- Modified DH → Standard DH:  
αᵢ (Standard) = αᵢ₊₁ (Modified)  
aᵢ (Standard) = aᵢ₊₁ (Modified)  
dᵢ (Standard) = dᵢ (Modified)  
θᵢ (Standard) = θᵢ (Modified)

The converted Standard DH parameters are:

```c
// Standard DH parameters [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
};

```

1. Prepare DH Transformation Matrices
  - Modified DH Transformation Matrix:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/f/c/fc04fa8048f598a4ef51cd25d505b319478b75d1.png)

- Rewrite the modified DH transformation matrix using Eigen:

```c
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:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/0/d/0dacde917b583b09045e7b0bf863f4fa1eba30f4.png)

- Rewrite the standard DH transformation matrix using Eigen:

```c
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;

```

1. Implement the core function `computeFK()` for FK calculation. See the complete code in the repository: [**https://github.com/agilexrobotics/Agilex-College.git**](https://github.com/agilexrobotics/Agilex-College.git)

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

    // Initialize 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
    // Calculate the transformation matrix of the current joint and accumulate 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 = fixed d value (for rotary joints)

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

    // Return the final transformation matrix
    return T;
}

```

#### 2.2 Verify FK Calculation Accuracy

1. Launch the FK verification program:

```bash
ros2 launch piper_kinematics test_fk.launch.py

```

1. Launch the RVIZ simulation program, enable TF tree display, and check if the pose of `link6_from_fk` (the arm end-effector calculated by FK) coincides with the original `link6` (calculated by joint\_state\_publisher):

```bash
ros2 launch piper_description display_piper_with_joint_state_pub_gui.launch.py 

```

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/4/9/49f2d8f1382d154a462c54fb2e4b8b3f557c3a31.jpeg)

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/9/9/9949a26ba7a5ae21cf03f738405544f4bce2734f.jpeg)

High coincidence is observed, and the error between `link6_from_fk` and `link6` is basically within four decimal places.

* * *

### 3. Inverse Kinematics (IK) Calculation

The IK calculation process essentially determines the position of each joint of the robotic arm required to move the arm’s end-effector to a given target point.

#### 3.1 Confirm Joint Limits

- Joint limits of the PIPER arm must be defined to ensure the IK solution path does not exceed physical constraints (preventing arm damage or safety hazards).
- From Section 1.3, the joint limits of the PIPER arm are:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/9/7/97a03b8674a404a318049b5524828c66926d61e1.png)

- The joint limit matrix is defined as:

```cpp
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
};

```

#### 3.2 Step-by-Step Implementation of Jacobian Matrix Method for IK

Solution Process:

1. **Calculate Error e** :  
Difference between current pose and target pose (6-dimensional vector: 3 for position + 3 for orientation).
2. **Is Error e below tolerance?**
  - **Yes** : Return current θ as the solution.
  - **No** : Proceed to iterative optimization.

3. **Calculate Jacobian Matrix J** : 6×6 matrix.
4. **Calculate Damped Pseudoinverse** :

J⁺ = Jᵀ(JJᵀ + λ²I)⁻¹

λ is the damping coefficient (avoids numerical instability in singular configurations).  
5. **Calculate Joint Angle Increment** :

Δθ = J⁺e

Adjust joint angles using error e and pseudoinverse.  
6. **Update Joint Angles** :

θ = θ + Δθ

Apply adjustment to current joint angles.  
7. **Apply Joint Limits**.  
8. **Normalize Joint Angles**.  
9. **Reach Maximum Iterations?**

- **No** : Return to Step 2 for further iteration.
- **Yes** : Throw non-convergence error.

Core Function `computeIK()`:

```cpp
std::vector<double> computeIK(const std::vector<double>& initial_guess, 
                                 const Eigen::Matrix4d& target_pose,
                                 bool verbose = false,
                                 Eigen::VectorXd* final_error = nullptr) {
    // Initialize with initial guess pose
    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;

    // Start iterative calculation
    for (int iter = 0; iter < max_iterations_; ++iter) {
        // Calculate FK for initial state to get position and orientation
        current_pose = fk_.computeFK(joint_values);
        // Calculate error between initial state and target pose
        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";
        }

        // Check if error is below tolerance (separate for position and orientation)
        if (error.head<3>().norm() < position_tolerance_ && 
            error.tail<3>().norm() < orientation_tolerance_) {
            success = true;
            break;
        }

        // Calculate Jacobian matrix (analytical by default)
        Eigen::MatrixXd J = use_analytical_jacobian_ ? 
            computeAnalyticalJacobian(joint_values, current_pose) :
            computeNumericalJacobian(joint_values);

        // Use Levenberg-Marquardt (damped least squares)
        // Δθ = Jᵀ(JJᵀ + λ²I)⁻¹e
        // θ_new = θ + Δθ
        Eigen::MatrixXd Jt = J.transpose();
        Eigen::MatrixXd JJt = J * Jt;
        // lambda_: damping coefficient (default 0.1) to avoid numerical instability in singular configurations
        JJt.diagonal().array() += lambda_ * lambda_;
        Eigen::VectorXd delta_theta = Jt * JJt.ldlt().solve(error);

        // Update joint angles
        for (int i = 0; i < 6; ++i) {
            // Apply adjustment to current joint angle
            double new_value = joint_values[i] + delta_theta(i);
            // Ensure updated θ is within physical joint limits
            joint_values[i] = std::clamp(new_value, joint_limits_[i].first, joint_limits_[i].second);
        }

        // Normalize joint angles to [-π, π] (avoid unnecessary multi-turn rotation)
        normalizeJointAngles(joint_values);
    }

    // Throw exception if no solution is found within max iterations (100)
    if (!success) {
        throw std::runtime_error("IK did not converge within maximum iterations");
    }

    // Calculate final error (if required)
    if (final_error != nullptr) {
        current_pose = fk_.computeFK(joint_values);
        *final_error = computePoseError(current_pose, target_pose);
    }

    return joint_values;
}

```

#### 3.3 Publish 3D Target Points for the Arm Using Interactive Markers

1. Install ROS2 dependency packages:

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

```

1. Launch `interactive_marker_utils` to publish 3D target points:

```cpp
ros2 launch interactive_marker_utils marker.launch.py 

```

1. Launch RVIZ2 to observe the marker:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/f/e/feedbd7d2a2f3659ee65cead19bed865fe70f6be.png)

1. Drag the marker and use `ros2 topic echo` to verify if the published target point updates:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/4/6/462caf005a8bda63c9d49e71692dc7ee27bb96dc.png)

#### 3.4 Verify IK Correctness in RVIZ via Interactive Markers

1. Launch the AgileX PIPER RVIZ simulation demo (the model will not display correctly without `joint_state_publisher`):

```cpp
ros2 launch piper_description display_piper.launch.py 

```

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/4/f/4f2add2e893fc2682262a841a919521a2d12c3f7.png)

1. Launch the IK node and `interactive_marker` node (in the same launch file). The arm will display correctly after successful launch:

```cpp
ros2 launch piper_kinematics piper_ik.launch.py

```

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/f/2/f2a448caa4cf1bcfed9dedf8160152594a339eee.jpeg)

1. Control the arm for IK calculation using `interactive_marker`:  
 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/8/1/81849ae223195b84e5195e43bc9b5af2f9be3b83.png)

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/5/3/53705917f097160cdca62ff8dc9e3b667da1e650.png)

1. Drag the `interactive_marker` to see the IK solver calculate joint angles in real time:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/e/3/e354943da54c78c61b01b2ffdcf03ed9ec4e060c.jpeg)

1. If the `interactive_marker` is dragged to an unsolvable position, an exception will be thrown:

 ![image](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/2/2/224d7f85aacc2cbe69594cd59491f60c1d0dedaa.jpeg)

* * *

### 4. Verify IK on the Physical PIPER Arm

1. First, launch the script for CAN communication with the PIPER arm:

```cpp
cd piper_ros
./find_all_can_port.sh 
./can_activate.sh 

```

 ![](https://us1.discourse-cdn.com/flex022/uploads/ros/original/3X/7/8/78b4cfe15c8261cf75abcbea06aee0fc36ce1e06.png)

1. Launch the physical PIPER control node:

```cpp
ros2 launch piper my_start_single_piper_rviz.launch.py 

```

1. Launch the IK node and `interactive_marker` node (in the same launch file). The arm will move to the HOME position:

```cpp
ros2 launch piper_kinematics piper_ik.launch.py

```

1. Drag the `interactive_marker` and observe the movement of the physical PIPER arm.