Skip to content

Nonlinear \mathcal{H}_\infty Optimal Position Controller

Summary of Controller

Neuromeka offers a built-in position controller for IndySDK users. This example describe how to use \mathcal{H}_\infty optimal controller in both joint space and task space. \mathcal{H}_\infty optimal controller consists of two parts. The first one is the feedforward term calculated from the reference velocity (\dot{q}_{ref}) and robot model. The other is the feedback term which attenuates disturbances and model uncertainties in the sense of \mathcal{H}_\infty optimality.

\begin{align} \tau = \widehat{M}(q)\ddot{q}_{ref} + \widehat{C}(q,\dot{q})\dot{q}_{ref} + \widehat{g}(q) + K\dot{e}_{ref}, \end{align}

Here, \widehat{M}, \widehat{C}, and \widehat{g} denote inertia matrix, Coriolis matrix, and gravity vector respectively. Also, \dot{e}_{ref} = \dot{q}_{ref} - \dot{q},

\begin{align} \ddot{q}_{ref} = \ddot{q}_{des} + K_{v}\dot{e} + K_{p}e,\quad \dot{q}_{ref} = \dot{q}_{des} + K_{v}e + K_{p}\int{e}, \end{align}

e = q_{des} - q, q_{des} represents the desired position trajectory. K, K_{v}, K_{p} are control gains.

For more details, refer to the following paper.

Example Code

Joint-space Position Controller

As a first step, generate the joint control component template. Then, in the header, define several variables to set control gains and reset controllers like follows:

1
2
3
private:
    bool reset_ctrl;         // for controller reset
    JointVec _kp, _kv, _ki;  // for control gain settings

Note that the user needs to enter the license information at the top of the source file so that the cross complied joint control component is successfully transplanted (the license is issued to IndySDK users. For more information, refer to ''Getting Started'').

1
2
3
4
5
/***** License Information *****/
#define USERNAME "NEUROMEKA"
#define EMAIL "NEUROMEKA@neuromeka.com"
#define SERIAL "NEUROMEKA1234567890"
/******************************/

Then, fill in the computeControlTorq() function to calculate the desired control input (torque).

  • resetHinfController() is used to reset the integral action of the controller.
  • HinfController() is the nonlinear optimal position controller Neorumeka provides.
  • setHinfControlGain() exists to adjust feedback control gains.
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
int JointControlComponent::computeControlTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec const & qDesired,
JointVec const & qdotDesired, JointVec const & qddotDesired,
JointVec & torque)
{
    if (reset_ctrl)
    {
        robot.resetHinfController();
        reset_ctrl = false;
    }

    robot.setHinfControlGain(_kp, _kv, _ki);
    robot.HinfController(gravDir, qDesired, qdotDesired, qddotDesired, torque);

    return 0;
}

In constructor and destructor, add the following code to initialize and reset the controller. Note that the initHinfController() is used to set the sampling time on the controller for differential and integral calculations.

1
2
3
4
5
6
JointControlComponent::JointControlComponent()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{
    reset_ctrl = true;
}
1
2
3
4
5
void JointControlComponent::initialize(ROBOT & robot, double delt)
{
    _robotNom = AbstractController::createRobotObj();
    robot.initHinfController(delt);
}

As shown in below, in the reset() function, reset the variable reset_ctrl when the robot starts to move to new waypoint.

1
2
3
4
void JointControlComponent::reset()
{
    reset_ctrl = true;
}

Configure the setGains() function as follows. Then, the users can easily tune the control gains using a Neuromeka's teaching pendant Conty (In Gain tuning menu of Conty, select Manual tuning menu and tune control gains).

1
2
3
4
5
6
7
void JointControlComponent::setGains(JointVec const & kp, JointVec const & kv,
JointVec const & ki)
{
    _kp = kp;
    _kv = kv;
    _ki = ki;
}

In the joint control component, in addition to the definition of position controller, the use of direct teaching mode should be defined. In this example, for simplicity, the gravity compensation will be applied as follows:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
int JointControlComponent::computeGravityTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, JointVec & torque)
{
    JointVec resTorq;
    robot.idyn_gravity(gravDir);
    resTorq = robot.tau();

    torque = resTorq;

    return 0;
}

Task-space Position Controller

The way to use task-space position controller is basically same as that of joint-space controller. However, as shown in below, the end-effector's position, velocity, and acceleration values are used as controller inputs instead of joint angle, joint velocity, and joint acceleration.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
int TaskControlComponent::computeControlTorq(ROBOT & robot,
LieGroup::Vector3D const & gravDir, TaskPosition const & posDesired,
TaskVelocity const & velDesired, TaskAcceleration const & accDesired,
JointVec & torque)
{
 if (reset_ctrl)
 {
   robot.resetHinfController();
   reset_ctrl = false;
 }

 robot.setHinfControlGain(_kp, _kv, _ki);
 robot.HinfController(gravDir, posDesired, velDesired, accDesired, torque);


 return 0;
}