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

Summary of Controller

Neuromeka offers a built-in position controller for IndySDK users. The following examples demonstrate the implementation of $\mathcal{H}_\infty$ optimal controller, one of the built-in position controller, in the joint space and task space. In the $\mathcal{H}_\infty$ optimal controller, there are mainly two terms, feedforward and feedback terms. Feedforward term is calculated from the reference velocity ($\dot{q}_{ref}$) and robot model. The feedback term is added to attenuate the 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}$$

where $\widehat{M}$, $\widehat{C}$, and $\widehat{g}$ denote inertia matrix, Coriolis matrix, and gravity vector, respectively. The reference velocity errors can be computed as follows, $\dot{e}_{ref} = \dot{q}_{ref} - \dot{q}$. The reference acceleration ($\ddot{q}_{ref}$) is defined as follows.

$$\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}$$

where $e = q_{des} - q$, is joint position error and $q_{des}$ is the desired position trajectory. $K$, $K_{v}$, $K_{p}$ are control gains.

We refer the reader to the more in-depth discussions on the following paper.

• Design of a Robust $\mathcal{H}_\infty$ PID Control for Industrial Manipulators

Example Code

Joint-space Position Controller

First, joint control component template should be generated ( refer [''Create IndySDK Componen''] (section2.md) ). Once user generate joint control component, the next step is, adding the following codes to the specified files. Add the following variables, control gains and flag to reset controller, in the header file.

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'').

/***** License Information *****/
#define USERNAME "NEUROMEKA"
#define EMAIL "NEUROMEKA@neuromeka.com"
#define SERIAL "NEUROMEKA1234567890"
/******************************/

The next step is implementing computeControlTorq() function. This function computes the control input (torque). The following three functions will be used during the computation of the control 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.

To generate control torque successfully, add the following code to computeControlTorq() function in the cpp file.

    if (reset_ctrl)
    {
        robot.resetHinfController();
        reset_ctrl = false;
    }

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

Now, we will add the following code in the constructor and destruction functions. These codes will initialize and reset the controller. The function initHinfController() is used to set the sampling time; the controller use this sampling time to handle the differential and integral computation.

Modify the constructor and destructor functions according to the following codes.

JointControlComponent::JointControlComponent()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{
    reset_ctrl = true;
}

void JointControlComponent::initialize(ROBOT & robot, double delt)
{
    _robotNom = AbstractController::createRobotObj();
    robot.initHinfController(delt);
}

Since we have to reset the integral action of the controller when the robot start new waypoint, the reset() function in the cpp file should be modified as follows.

void JointControlComponent::reset()
{
    reset_ctrl = true;
}

Subsequently, we are going to add the following codes in the setGains() function (in the cpp file), which enable us to tune the control gains using Conty (Neuromekas' teaching pendant).

    _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:

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.

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);


 return 0;
}