Skip to content

Commit 544cd7e

Browse files
juan-g-bonillaschaubh
juan-g-bonilla
authored and
schaubh
committed
[#1376] Rename PIDControllers folder to JointPIDController
Renames the folder, SWIG module (MJPIDControllers -> MJJointPIDController), and test file to match the single concrete class it contains. Adds JointPIDController.rst and updates scenarioBranchingPanels to import from the new module name.
1 parent b55d1d2 commit 544cd7e

6 files changed

Lines changed: 94 additions & 18 deletions

File tree

‎examples/mujoco/scenarioBranchingPanels.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@
3333
velocity profiles. In this scenario, the panels are deployed in a
3434
staged sequence, where the deployment of some panels happens after
3535
the deployment of others. Moreover, this scenario uses the C++
36-
implementation of the PID controller (`MJPIDControllers.JointPIDController`),
36+
implementation of the PID controller (`MJJointPIDController.JointPIDController`),
3737
which provides improved performance compared to the previous
3838
Python-based controller.
3939
@@ -79,7 +79,7 @@
7979
from Basilisk.simulation import StatefulSysModel
8080
from Basilisk.architecture import messaging
8181
from Basilisk.simulation import svIntegrators
82-
from Basilisk.simulation import MJPIDControllers
82+
from Basilisk.simulation import MJJointPIDController
8383

8484
import numpy as np
8585

@@ -254,7 +254,7 @@ def addJointController(panelID: str, initialAngle: float, timeOffset: int):
254254
)
255255

256256
# Use the C++ JointPIDController
257-
pidController = MJPIDControllers.JointPIDController()
257+
pidController = MJJointPIDController.JointPIDController()
258258
pidController.ModelTag = f"{actuatorName}_controller"
259259
pidController.setProportionalGain(0.1)
260260
pidController.setDerivativeGain(0.002)

‎src/simulation/mujocoDynamics/JointPIDController/JointPIDController.cpp‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
/*
2+
ISC License
3+
4+
Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
5+
6+
Permission to use, copy, modify, and/or distribute this software for any
7+
purpose with or without fee is hereby granted, provided that the above
8+
copyright notice and this permission notice appear in all copies.
9+
10+
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
11+
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
12+
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
13+
ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
14+
WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15+
ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16+
OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17+
18+
*/
19+
20+
#include "JointPIDController.h"
21+
22+
double JointPIDController::readMeasuredPosition(const ScalarJointStateMsgPayload& i) const
23+
{
24+
return i.state;
25+
}
26+
27+
double JointPIDController::readMeasuredVelocity(const ScalarJointStateMsgPayload& i) const
28+
{
29+
return i.state;
30+
}
31+
32+
void JointPIDController::writeOutput(SingleActuatorMsgPayload& o, double val)
33+
{
34+
o.input = val;
35+
}

src/simulation/mujocoDynamics/PIDControllers/JointPIDController.h renamed to src/simulation/mujocoDynamics/JointPIDController/JointPIDController.h

Lines changed: 3 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -37,30 +37,21 @@ class JointPIDController : public PIDController<ScalarJointStateMsgPayload, Scal
3737
* @param i Joint state message payload.
3838
* @return The joint position (radians or meters).
3939
*/
40-
double readMeasuredPosition(const ScalarJointStateMsgPayload& i) const override
41-
{
42-
return i.state;
43-
}
40+
double readMeasuredPosition(const ScalarJointStateMsgPayload& i) const override;
4441

4542
/**
4643
* @brief Read the measured velocity from the joint state payload.
4744
* @param i Joint state message payload.
4845
* @return The joint velocity (radians/sec or meters/sec).
4946
*/
50-
double readMeasuredVelocity(const ScalarJointStateMsgPayload& i) const override
51-
{
52-
return i.state;
53-
}
47+
double readMeasuredVelocity(const ScalarJointStateMsgPayload& i) const override;
5448

5549
/**
5650
* @brief Write the computed actuator input to the output payload.
5751
* @param o Actuator message payload.
5852
* @param val Value to write.
5953
*/
60-
void writeOutput(SingleActuatorMsgPayload& o, double val) override
61-
{
62-
o.input = val;
63-
}
54+
void writeOutput(SingleActuatorMsgPayload& o, double val) override;
6455
};
6556

6657
#endif

src/simulation/mujocoDynamics/PIDControllers/PIDControllers.i renamed to src/simulation/mujocoDynamics/JointPIDController/JointPIDController.i

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
*/
1919

2020

21-
%module MJPIDControllers
21+
%module MJJointPIDController
2222

2323
%include "architecture/utilities/bskException.swg"
2424
%default_bsk_exception();

‎src/simulation/mujocoDynamics/JointPIDController/JointPIDController.rst‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
Executive Summary
2+
-----------------
3+
The ``JointPIDController`` module implements a proportional-integral-derivative (PID) controller for scalar MuJoCo
4+
joints (rotational or translational). It reads measured and desired joint state messages, computes a control output,
5+
and publishes it as a ``SingleActuatorMsgPayload``.
6+
7+
Message Interfaces
8+
------------------
9+
.. bsk-module-io:: JointPIDController
10+
11+
input measuredPosInMsg ScalarJointStateMsgPayload
12+
Measured joint position. ``state`` field is used as the measured position.
13+
14+
input measuredVelInMsg ScalarJointStateMsgPayload
15+
Measured joint velocity. ``state`` field is used as the measured velocity.
16+
17+
input desiredPosInMsg ScalarJointStateMsgPayload
18+
Desired joint position. Required when ``Kp`` is non-zero.
19+
20+
input desiredVelInMsg ScalarJointStateMsgPayload
21+
Desired joint velocity. Required when ``Kd`` is non-zero.
22+
23+
output outputOutMsg SingleActuatorMsgPayload
24+
Control output written to ``input`` field.
25+
26+
Module Description
27+
------------------
28+
The controller computes
29+
30+
.. math::
31+
32+
u = K_p\,(q_d - q) + K_d\,(\dot{q}_d - \dot{q}) + K_i\,\int(q_d - q)\,\text{d}t
33+
34+
where :math:`q`, :math:`\dot{q}` are the measured position and velocity, :math:`q_d`, :math:`\dot{q}_d` are the
35+
desired values, and :math:`K_p`, :math:`K_d`, :math:`K_i` are the proportional, derivative, and integral gains.
36+
37+
The integral error is stored as a registered state and evolved by the simulation integrator.
38+
39+
The module inherits from the generic ``PIDController`` base class (in ``_GeneralModuleFiles/PIDController.h``),
40+
which is templated on ``ScalarJointStateMsgPayload`` (measured and desired) and ``SingleActuatorMsgPayload`` (output).
41+
42+
Verification and Testing
43+
------------------------
44+
The module is verified in
45+
``src/simulation/mujocoDynamics/JointPIDController/_UnitTest/test_JointPIDController.py`` by simulating a
46+
single-joint arm with a linearly ramping desired position. The test checks that:
47+
48+
- The PID output matches the expected formula at every time step.
49+
- The joint position converges to the desired final position within 1%.
50+
- The joint velocity settles near the desired final velocity within 1%.

src/simulation/mujocoDynamics/PIDControllers/_UnitTest/test_PIDControllers.py renamed to src/simulation/mujocoDynamics/JointPIDController/_UnitTest/test_JointPIDController.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010

1111
try:
1212
from Basilisk.simulation import mujoco
13-
from Basilisk.simulation import MJPIDControllers
13+
from Basilisk.simulation import MJJointPIDController
1414

1515
couldImportMujoco = True
1616
except:
@@ -60,7 +60,7 @@ def test_jointPIDController(showPlots: bool = False):
6060
])
6161
scene.AddModelToDynamicsTask(desiredPosInterpolator)
6262

63-
armController = MJPIDControllers.JointPIDController()
63+
armController = MJJointPIDController.JointPIDController()
6464
armController.setProportionalGain(1)
6565
armController.setDerivativeGain(2)
6666
armController.setIntegralGain(0.02)

0 commit comments

Comments
 (0)