LQR control of Acrobot

Discussion in 'Modeling' started by atabak, Jul 8, 2018.

  1. atabak

    atabak

    I am trying to apply the LQR controller, introduced at the end of this Spong's classic on Acrobot. I've attached the model and the controller is simply:
    Code:
    d->actuator_force[0] = 242.52*(d->qpos[0]-M_PI/2.0) + 96.33*d->qpos[1] + 104.59 * d->qvel[0] + 49.05 * d->qvel[1]
    Which is called inside the control callback, however, the system seems to be unstable, when started at:
    Code:
    d->qpos[0] = M_PI/2.0 + 0.01;
d->qpos[1] = 0.0;
d->qvel[0] = 0.0;
d->qvel[1] = 0.0;
    I can't seem to understand which part of my model is wrong.

    Thanks in advance for any suggestion.
     

    Attached Files:

    atabak, Jul 8, 2018
    #1
  2. Emo Todorov

    Emo Todorov Administrator Staff Member

    You seem to have positive feedback gains. Try changing the sign of the control force.

    In general, better stability is achieved by reducing the timestep or by using the RK4 integrator (especially in systems without contact). But if you have positive feedback gains, the system will go unstable no matter what.
     
    Emo Todorov, Jul 9, 2018
    #2
  3. atabak

    atabak

    Thanks for the fast answer professor. I have changed the sign of the controller (duh!), used RK4 integrator and reduced timestep, but the controller is still unstable :-?

    Code:
    ctrl = 242.52*(d->qpos[0]-M_PI/2.0) + 96.33*d->qpos[1] + 104.59 * d->qvel[0] + 49.05 * d->qvel[1];
d->actuator_force[0] = -ctrl;
     

    Attached Files:

    atabak, Jul 9, 2018
    #3
  4. Emo Todorov

    Emo Todorov Administrator Staff Member

    The passive simulation is stable, so your controller must be unstable. Maybe you entered the wrong numbers somewhere, or the book you are using has errors, not sure. For example, you are setting mass =1000. These are kilograms... are you sure you want the links to be that heavy?

    In general, when your feedback controller is unstable, reduce the gains until it becomes stable. Although in this case, I am guessing the book analyzed the linearized system and found the controller to be stable, so the more likely explanation is that you entered the wrong numbers somewhere.
     
    Emo Todorov, Jul 10, 2018
    #4
    atabak likes this.
  5. atabak

    atabak

    That is correct.

    As mentioned in the first post, I am trying to replicate Spong's LQR controller in MuJoCo. According to his paper, for an acrobot with first link 1m, second link 2m, and both links with the mass of 1kg, a linear state feedback controller should stabilize the system around x = (q1-M_PI/2.0, 0, 0, 0):
    upload_2018-7-10_23-22-54.png
    where the state is x=(q1, q2, q1', q2') with the control law:
    upload_2018-7-10_23-27-22.png
    which I have implemented as
    Code:
    mjtNum ctrl = 242.52*(d->qpos[0]-M_PI/2.0) + 96.33*d->qpos[1] + 104.59 * d->qvel[0] + 49.05 * d->qvel[1];
d->actuator_force[0] = ctrl;
    and I am under the impression that it is correctly modeled, but the close loop system when started at a slightly purturbed position is unstable (It is unstable even at (q1-M_PI/2.0, 0, 0, 0) but I think that's because of numeric approximation of M_PI).

    UPDATE:
    With the sign changed, it finally did get stable.
     

    Attached Files:

    Last edited: Jul 11, 2018
    atabak, Jul 10, 2018
    #5
  6. Emo Todorov

    Emo Todorov Administrator Staff Member

    I meant the (implied) units of mass are kilograms. So you are setting the mass to 1000 kg instead of 1 kg.

    The approximation of pi is not a problem in this case; there error is close to the floating point precision.
     
    Emo Todorov, Jul 12, 2018
    #6
    atabak likes this.