C++ Module: JointPIDController

Executive Summary

The JointPIDController module implements a proportional-integral-derivative (PID) controller for scalar MuJoCo joints (rotational or translational). It reads measured and desired joint state messages, computes a control output, and publishes it as a SingleActuatorMsgPayload.

Message Interfaces

Module I/O Messages

Msg Variable Name	Msg Type	Description
measuredPosInMsg	ScalarJointStateMsgPayload	Measured joint position. state field is used as the measured position.
measuredVelInMsg	ScalarJointStateMsgPayload	Measured joint velocity. state field is used as the measured velocity.
desiredPosInMsg	ScalarJointStateMsgPayload	Desired joint position. Required when Kp is non-zero.
desiredVelInMsg	ScalarJointStateMsgPayload	Desired joint velocity. Required when Kd is non-zero.
outputOutMsg	SingleActuatorMsgPayload	Control output written to input field.

Module Description

The controller computes

\[u = K_p\,(q_d - q) + K_d\,(\dot{q}_d - \dot{q}) + K_i\,\int(q_d - q)\,\text{d}t\]

where \(q\), \(\dot{q}\) are the measured position and velocity, \(q_d\), \(\dot{q}_d\) are the desired values, and \(K_p\), \(K_d\), \(K_i\) are the proportional, derivative, and integral gains.

The integral error is stored as a registered state and evolved by the simulation integrator.

The module inherits from the generic PIDController base class (in _GeneralModuleFiles/PIDController.h), which is templated on ScalarJointStateMsgPayload (measured and desired) and SingleActuatorMsgPayload (output).

Verification and Testing

The module is verified in src/simulation/mujocoDynamics/JointPIDController/_UnitTest/test_JointPIDController.py by simulating a single-joint arm with a linearly ramping desired position. The test checks that:

• The PID output matches the expected formula at every time step.

• The joint position converges to the desired final position within 1%.

• The joint velocity settles near the desired final velocity within 1%.

---

class JointPIDController : public PIDController<ScalarJointStateMsgPayload, ScalarJointStateMsgPayload, SingleActuatorMsgPayload>

#include <JointPIDController.h>

PID controller for scalar joints (rotational or translational).

Implements the required virtual methods for reading joint state and writing actuator input.

Protected Functions

double readMeasuredPosition(const ScalarJointStateMsgPayload &i) const override

Read the measured position from the joint state payload.

Parameters:

i – Joint state message payload.

Returns:

The joint position (radians or meters).

double readMeasuredVelocity(const ScalarJointStateMsgPayload &i) const override

Read the measured velocity from the joint state payload.

Parameters:

i – Joint state message payload.

Returns:

The joint velocity (radians/sec or meters/sec).

void writeOutput(SingleActuatorMsgPayload &o, double val) override

Write the computed actuator input to the output payload.

Parameters:

• o – Actuator message payload.

• val – Value to write.