Control Tuning

When Do You Need This Guide?

The default PID gains are optimized for Indy7. Tuning is only necessary in the following cases:

• When mounting heavy payloads (increased vibration or tracking error)
• When adding a new robot model (when default gains are not available)
• When high precision is required (RMS tracking error < 0.001 rad)

Start with the default values and refer to the content below only when problems occur.

---

Control Modes

PLEM supports multiple control modes managed by a state machine:

Mode	Tracking	Gravity Compensation	Friction Compensation	PID	Use Case
TRAJECTORY	O	O	O	O	Normal operation, path execution
FREEDRIVE	X	O	X	X	Manual teach, user guides robot
BRAKED	N/A	N/A	N/A	N/A	Torque 0, servo holds position with brake
P_STOP (protective)	X	O	X	X	Safety stop, limit exceeded

Why friction compensation is disabled in FREEDRIVE: When the user directly guides the robot, target_velocity ≈ actual_velocity, which creates a positive feedback loop (runaway).

---

Torque Control Overview

JointTorqueController implements the following dynamics equation:

$$\tau = G(q) + C(q, \dot{q})\dot{q} + M(q)\ddot{q} + F(\dot{q}) + \tau_{PID}$$

Component	Role	When Active
Gravity G(q)	Weight cancellation (KDL-based)	Always
Coriolis C(q,q̇)q̇	Velocity-dependent force cancellation	Always
Inertia M(q)q̈	Acceleration feedforward	When target_accelerations provided
Friction F(q̇)	Joint friction compensation	TRAJECTORY mode only
PID τ_PID	Tracking feedback	TRAJECTORY mode only

Core API:

#include <plem/control/joint_torque_controller.hpp>

// disable_tracking parameter of calculate_joint_torque():
//   false → Full control (TRAJECTORY)
//   true  → Gravity + Coriolis compensation only (FREEDRIVE, P_STOP)
auto torque = controller.calculate_joint_torque(
    joint_angles, joint_velocities,
    target_positions, target_velocities, target_accelerations,
    /*disable_tracking=*/false);

---

PID Gain Configuration

PID Parameter Structure

struct PIDParameter {
    double kp;            // Proportional gain [Nm/rad]
    double ki;            // Integral gain [Nm/(rad·s)]
    double kd;            // Derivative gain [Nm/(rad/s)]
    double max_integral;  // Anti-windup limit
    double max_output;    // Output saturation [Nm]
};

Default Gains (Indy7)

Joint	Kp	Ki	Kd	max_integral	max_output	Notes
J0 (base)	8100	10	420	50	200	High inertia, strong stiffness
J1 (shoulder)	8100	10	420	50	200	Maximum gravity torque
J2 (elbow)	2100	10	120	50	200	Medium inertia
J3 (wrist1)	2100	10	120	50	200	Low inertia
J4 (wrist2)	2100	10	120	50	200	Low inertia
J5 (wrist3)	2100	10	120	50	200	Minimal friction

Design rationale: Ki is small (10) because gravity compensation handles most of the static load. Base/shoulder require strong Kp (8100) and high damping Kd (420) due to high inertia.

Applying Custom Gains

#include <plem/control/joint_torque_controller.hpp>
#include <plem/control/defaults/indy7.hpp>

// Start with defaults and modify
auto custom_gains = indy7::PIDGains;

// Increase shoulder stiffness for heavy payload (5kg)
custom_gains[1].kp = 10000.0;  // 8100 → 10000
custom_gains[1].kd = 500.0;    // 420 → 500
custom_gains[1].max_output = 250.0;

// Reduce aggressiveness for wrist without payload
custom_gains[5].kp = 1500.0;   // 2100 → 1500

PIDController<6> pid(custom_gains);

Parameter Effect Summary

Parameter	Too Low	Too High	Typical Range
Kp	Slow response, large error	Oscillation, overshoot	1000-10000
Ki	Steady-state bias	Windup, slow oscillation	5-20
Kd	Overshoot, ringing	Noise amplification, chattering	100-500
max_integral	Slow recovery during saturation	Allows windup	50-200
max_output	Limits PID response	Risk of hardware damage	50-250

---

Friction Compensation Parameters

PLEM uses a smooth Coulomb friction model:

$$\tau_{friction} = F_c \cdot \tanh\left(\frac{\omega}{0.05}\right)$$

tanh provides smooth transition near zero velocity, preventing chattering (compared to sign()).

Default Friction Coefficients (Indy7)

Joint	Fc [Nm]	Notes
J0 (base)	6.5	Large bearing, low friction
J1 (shoulder)	36.68	Load-bearing, highest friction
J2 (elbow)	9.0	Medium
J3 (wrist1)	2.2	Low mass
J4 (wrist2)	2.6	Low mass
J5 (wrist3)	1.0	Minimal friction

Friction Coefficient Identification Method

1. Set robot to gravity compensation mode (or mount horizontally)
2. Command constant velocity for each joint (0.5 rad/s)
3. Measure average torque during steady-state motion
4. Fc = average_torque (at high velocity, tanh ≈ 1)

---

Tuning Workflow

Recommended Sequence

1. Tune Kp only (Ki=0, Kd=0): Increase until overshoot < 10% with 0.1 rad step input
2. Add Kd: Start at Kp/20, increase until overshoot disappears
3. Add Ki: Start at 0.1, increase until residual error < 0.001 rad
4. Set anti-windup: max_integral = 5 * (typical_error / Ki)
5. Set output limit: max_output = 0.8 * max_joint_torque

Joint-Specific Guidelines

Joint Group	Kp Range	Kd Range	Characteristics
Base/Shoulder (J0-J1)	6000-10000	300-500	High inertia, allows high Kd
Elbow (J2)	2000-3000	100-200	Caution near singularities
Wrist (J3-J5)	1000-2500	80-150	Friction-dominated, aggressive tuning possible

Monitoring Tools

# Monitor real-time joint status
ros2 topic echo /joint_states

# Visualize with PlotJuggler (tracking error, torque, etc.)
ros2 run plotjuggler plotjuggler
# ROS2 Subscriber → Add /joint_states, /rt_raw topics

# Check RT loop performance
ros2 topic echo /rt_raw --field loop_jitter_us
ros2 topic echo /rt_raw --field loop_exec_us

Good Response Criteria

Metric	Acceptable	Excellent
Overshoot	< 10%	< 5%
Settling time (0.1 rad step)	< 0.5s	< 0.2s
Steady-state error	< 0.005 rad	< 0.002 rad
RMS tracking error (during motion)	< 0.005 rad	< 0.002 rad

---

Performance Reference

W-RC measurements (Indy7, PREEMPT_RT):

Metric	Measured Value	Notes
Total loop execution time	60-80 µs	Including KDL dynamics
Loop jitter	12-42 µs	clock_nanosleep TIMER_ABSTIME
Gravity compensation	18-25 µs	~30% of total
Coriolis compensation	20-28 µs	~35% of total
Friction + PID	3-5 µs	~5% of total

There is sufficient margin compared to the 1kHz control period (1000 µs).

---

Next Steps

• Performance monitoring details: Performance Monitoring