Issues using Closed-Loop PID autotuner

I am trying to autotune a PID controllers for Dynamixel MX-106 motors. I need to control the position by adjusting torque. I'm using ROS to run the rest of this project and the PID controller I need to tune is implemented in C++ using ROS Noetic. There are two relevant ROS nodes.

The first controls the actual motors and listens to a topic /torques that is published by the PID controller node and adjusts them accordingly. It also publishes the positions of the motors. I am controlling 6 different motors, but they each have their own controller and I've changed the code here to only work with one for testing purpouses. I've also added a subscriber to a ROS topic /disturb, which allows MatLab to write inputs the same way as my own PID controller. I'm removed a lot of code for readbility and this will not run as is, but would be happy to upload the rest if needed.

void disturb(const publisher::Position msg)
{

  float oldTorque = torques[msg.motor-1];
  float torque = msg.pos;
  if(torque > maxtorque)
  {
      //printf("Torque %g greater than max torque %d. Setting torque to %d\n", torque, maxtorque, maxtorque);
      torque = maxtorque;
  } 
  else if(torque < -maxtorque)
  {
      //printf("Torque %g less than min torque %d. Setting torque to %d\n", torque, -maxtorque, -maxtorque);
      torque = -maxtorque;
  }

  //DMX torque: 0-1024 clockwise, 1024-2048 counterclockwise
  if(torque < 0)
  {
      torque = -torque + 1024;
  }
  torques[msg.motor-1] = torque;
  printf("DISTURBANCE motor %d torque %g -> %g\n", msg.motor, oldTorque, torques[msg.motor-1]);
  
}

void newTorque(const publisher::Position msg)
{
  float oldTorque = torques[msg.motor-1];
  torques[msg.motor-1] = msg.pos;
  //printf("Motor %d torque %g -> %g\n", msg.motor, oldTorque, torques[msg.motor-1]);
}

int main(int argc, char **argv)
{

  dmxWrite(1, DXL6_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
  dmxWrite(1, DXL6_ID, ADDR_MX_TORQUE_CONTROL_MODE, TORQUE_ENABLE);

//-------------------ROS SETUP---------------------------//
  ros::init(argc, argv, "torque_writer");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("torques", 6, newTorque);
  ros::Subscriber subdis = n.subscribe("disturb", 6, disturb);
  ros::Publisher pub = n.advertise<publisher::Position>("positions", 6);
//---------------END ROS SETUP---------------------------//
  printf("Waiting for %d motor control nodes to connect to position publisher...\n", 1);
  while(pub.getNumSubscribers() < 1)
  {
    usleep(100000);
  }
  printf("All nodes connected!\n");

  while(1)
  { 
    //Fetch current positions
    getMotorValues();

    publisher::Position position;
    position.motor = 6;
    position.pos = dxl6_present_position;
    pub.publish(position);
    
    //Subscribe torque
    ros::spinOnce();
    //Write torque
    dmxWrite(2, ADDR_MOTOR[5], ADDR_MX_GOAL_TORQUE, torques[5]);
  }
  return 0;
}

The other node is the actual PID controller. It listens to a topic /positions and calculates what torque to apply, then publishes that on /torques.

void pidControlLoop()
{
    //Setup ROS nh, publisher, subscriber
    ros:: NodeHandle n6;
    ros::Subscriber sub = n6.subscribe("positions", 6, updatePosition);
    ros::Subscriber goalSub = n6.subscribe("goalpositions", 1000, updateGoalPosition);
    ros::Publisher pub = n6.advertise<publisher::Position>("torques", 6);
    ros::Rate rate(5);
    publisher::Position torque_msg;
    torque_msg.motor = MOTOR;
    
    //INITIALIZE error values, error sums, gains
    int pError = 0;
    int iError = 0;
    int dError = 0;
    int errorSum= 0;
    int errorOld= 0;

    float torque;

    float pk= 0.01;
    float ik= 0.08;
    float dk= 0.00;
    

    while(position == -1)
    {
        ros::spinOnce();
    }
    pError = goalPosition - position;
    errorOld = pError;

    while(1){
       
        pError = goalPosition - position;
        //printf("pError: %d\n", pError);
        if(abs(pError) > 50)
        {
            if(abs(pError) > 2048 && goalPosition > position)
            {
                pError = (goalPosition - 4096) - position;
                printf("crossing ccw: %d\n", pError); 
            }       
            else if (abs(pError) > 2048 && goalPosition < position)
            {
                pError = (goalPosition + 4096) - position;
                printf("crossing ccw: %d\n", pError);
            }

            errorSum += pError;
            dError = pError - errorOld;
            torque = pk*pError + dk*dError + ik*errorSum;
            if(torque > maxtorque)
            {
                torque = maxtorque;
            } 
            else if(torque < -maxtorque)
            {
                torque = -maxtorque;
            }

            //DMX torque: 0-1024 clockwise, 1024-2048 counterclockwise
            if(torque < 0)
            {
                torque = -torque + 1024;
            }
            
            torque_msg.pos = torque;
            
            
        } 
        else
        {
            torque_msg.pos = 0;
        }

        pub.publish(torque_msg);
        ros::spinOnce();
        rate.sleep();
    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "motor6control");
    pidControlLoop();
}

The PID controller functions. Obviously it is not well tuned, but after spending some time tuning by hand, some motors are able to get to the goal position, stay there, and respond to any disturbances. I am attempting to use the following diagram from help center as a model:

This is the Simulink model I've created:

Here are my steps during testing. I think the issue is likely related to a mistake I'm making here, as I'm pretty clueless about control design.

1. Run both above ROS nodes, and make sure that the PID controller is adjusting torque as expected
2. Click the Monitor and Tune button in Simulink
3. Watch the ROS topics and see the test run

When I do this, a few weird things happen.

• The %conv almost immediately displays 100
• All 3 PID gains display 0
• Eventually, the autotuner starts writing to the disturb topic and adjusting the torques as expected. However, it will continue making seemingly random adjustments until the simulation reaches its stop time.
• All PID gains still read 0, even when the data is exported to MATLAB.
• Switching the start/stop signal off after the test starts does nothing.

Once the simulation ends, the motor will stop moving erratically, but no gains are ever displayed anywhere that I can find. Any help or advice is much appreciated.

[Jagadeesh Konakalla]

Hi Desmond Harris,

Thanks for the detailed explanation on your setup. Looks like it is difficult to reproduce the same setup on our end.

Here are few questions and suggestions to help your further.

• You have mentioned about two ROS nodes which are implemented in c++. One node is to control the motor and another one is to the PID controller. What is the role of the Simulink ROS model in this setup ? Is this theird node ? Can you elaborate on this ?
• It is also worth if you can simply the problem with a setup that we can reproduce on our end to help you further. I mean provide us the simple model where you reproduce the issue.
• Can you also try without external mode, i mean simply using the Normal mode Simulation. Please note that normal model simulation also, you can Read the ROS topics and publish the data.

Thanks,

Jagadeesh K.

[Desmond Harris]

Thanks for responding! I probably should've mentioned that I'm coming from a software-only background, so I'm pretty clueless about control design.

• I'm honestly not sure. It seems that when the simulation is being built, it creates a third ROS node for MatLab, but I'm not creating any new nodes manually. I did introduce a new topic(/disturb) however.
• It may be possible to troubleshoot this without the actual robot. Esentially, I have one ROS node that is communicating with the robot, and one that is doing calculations. To get a similar setup, you could set up ROS nodes that feed dummy numbers or random data into the controller. I don't think this would be a problem, since PID logic/math isn't my issue, I just need to figure out how I can get an actual output.
• How do I do this?

[Jagadeesh Konakalla]

Hi,

In the Simulink model, can you confirm that you receive the values of /torques and /positions ? I suggest you to use the trigger subsystem to utilize the output of subscriber block. I mean connect the Is New out put to trigger subsytem and move your logic inside the trigger subsystem. This ensures that your PID Autotuner block will be calculating output values only when the new data is received. Please update the model and elt me know how it works.

Thanks,

Jagadeesh K.

[Simone Cortinovis]

Hi,

I would like to comment about the issue on %conv reaching immediately 100 %.

I investigated the Closed Loop PID Autotuner and how conv % is calculated.

This value is determined solely based on the modulus of the estimated frequency response of the system, with the monitored frequency limited to the target bandwith.

The figure below shows the algorithm used to calculate convergence:

Convergence is the result of dividing between two terms calculated from the modulus of the frequency response. The difference between the current value and the previous sample's value, and its average over 64 samples.

Below is the progress of the parameters during a simulation. It is noteworthy that the estimated frequency response only varies at the target bandwith frequency.

In conclusion, based on the Simulink structure utilized, it is inevitable that the percentage of conversions will immediately increase and then end the simulation at 100%. The only terms of interest are likely the four instants of time when the frequency response estimate updates, although it is uncertain how informative they may actually be.

I hope you find it useful!

Simone Cortinovis

[Simone Cortinovis]

Since the topic covered in this question is more general, I have opened a new question at this link.

https://ch.mathworks.com/matlabcentral/answers/2057069-conv-in-closed-loop-pid-autotuner

[Arkadiy Turevskiy]

Thanks Simone for your analysis, we provided the answer in your stand-alone question.

On the topic of the original question: I wonder if using ROS nodes in this setup is introducing such significant delays into the setup that it creates a problem for the autotuner.

PID Autotuner works in two steps- first it estimate a frequency response of the plant at a few frequency points. Then it uses this frequency response to compute PID gains.

I suggest you try setting Experiment Mode to PRBS (this will use pseudo-random binary sequence as perturbation), which will hopefully produce frequency response faster. I also suggest adding optional outpit of plant frequency response - see if you get any non-zero values there.