Custom controller gains

[carlosjoserg]

The gains must be specified in the YAML file configuration, and ideally, dynamically re-configurable. Default values should provide a decent performance.

[enricocorvaglia]

Hi guys, @carlosjoserg @Tabjones @manuelbonilla please review the implementation of the OneTaskInverseKinematics controller, now it reads the PID gains from the lwr_controllers.yaml and it has a topic call to update them (see README for command). Tell me if you have comments, if it's fine in the next days I will do it for the other controllers. Cheers, Enrico

[Tabjones]

So technically you wrote the gains into a yaml and read them instead of hardcoding them into the source. Nice work, i'll test it on the lwr as soon as i can.

[carlosjoserg]

It looks fine to me. However, I see you are using the pid class from ros control that already has an implementation for this, and it uses a dynamic reconfigure to set the gains, and we should make the most of it.

I would suggest to initialize each of the pid's like in line 68 of the position controller, that is, in our case, something like:

// Load PID Controller using gains set on parameter server
std::string pid_("pid_");
for (int i = 0; i < kdl_chain_.getNrOfJoints(); ++i)
{
    if (!PIDs_[i].init(ros::NodeHandle(n, pid_ +
        joint_handles_.getJoint(i).getName() )))
    {
        ROS_ERROR("Error initializing the PIDs");
        return false;
    }
}

That way, the yaml file can be written like:

  OneTaskInverseKinematics: 
    type: lwr_controllers/OneTaskInverseKinematics
    root_name: world
    tip_name: lwr_7_link
    pid_lwr_1_joint: {p: 250,  i: 10, d: 30}
    pid_lwr_1_joint: {p: 220,  i: 10, d: 30}
    pid_lwr_2_joint: {p: 150,  i: 6, d: 20}
    pid_lwr_3_joint: {p: 150,  i: 5, d: 12}
    pid_lwr_4_joint: {p: 90,  i: 5, d: 10}
    pid_lwr_5_joint: {p: 40,  i: 5, d: 7}
    pid_lwr_6_joint: {p: 15,  i: 2, d: 5}

[enricocorvaglia]

Ok Carlos. And regarding the dynamic reconfigure, I didn't get if the PIDs have a specific topic so that I can publish gains directly on them, or the reconfiguration is handled inside the controller through the set_gains topic routine.

[carlosjoserg]

The dynamic reconfigure and set gains is already being handled by the PID class if you use the bool Pid::init(const ros::NodeHandle &node, const bool quiet) function, take a look at the source at line 93.

Btw, check in the code I gave above that the sum of strings and the getName() call are correct.

[enricocorvaglia]

Okay! I still have to try to adapt that script to our case. I was getting wrong with the "reconfigure" thing, I thought that you were talking about the online reconfiguration of the gains, not just the initialization.

[carlosjoserg]

I think you are still getting it wrong, since it is about that. Doing it this way will provide both load&initialize gains from the yaml file and reconfigure them online using rosrun rqt_reconfigure rqt_reconfigure.

I didn't write it detailed enough when I posted the issue, my apologies, hope it is clear now.

[enricocorvaglia]

Well, now it is clear, thanks. I was missing the rosrun command.

[enricocorvaglia]

Update 1: now the controllers that use control_toolbox::PID meet the issue requirements. All the others still need tweaking and possibly the integration of dynamic-reconfigure routine.

[carlosjoserg]

That's good news! Btw, recall that you are allowed to close the issue as well, though I would wait until the others meet the requirements. Check this to do it via commit, and link the commit to the issue.

[enricocorvaglia]

Yes, it's ok for me too to wait for the others. At least these first controllers are the ones used by Federico.