Add full PID control, plus small bug fixes

[AndyZe]

Use the control_toolbox PID controller instead of simple proportional control.

[AndyZe]

I moved this PID initialization stuff to the bottom of the function so that joint names would be available for the debug prints.

[AndyZe]

I don't believe it makes sense to initialize this to 0 when we have a command available.

[AndyZe]

Whoops, I duplicated this block of code. Plus, I think it is safer to use:

        const auto jointVelCmd =
          this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
          this->dataPtr->joints_[i].sim_joint);
        *jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
          {target_vel});

Than...

vel->Data()[0] = target_vel;

I might be wrong, though.

[livanov93]

@AndyZe I was looking at your changes in write - https://github.com/AndyZe/ign_ros2_control/blob/0c4da2204c27acdf97a3002a14507c39561d22fe/ign_ros2_control/src/ign_system.cpp#L621-L703 . I have been experimenting locally with changing whole approach here. What do you think about fully moving to foce commands. I get the inspiration from main ignition examples. Please take a look here:

• velocity control via pid with force output
• position control via pid with force output

We could also create cascade loop when using position interface from ros2_control with outer PD position controller and inner velocity loop with PID and final output with force/torque that is always propagated to the physics engine via JointForceCmd. Also using joint position and velocity limits as limiters in the loop.

IMHO this would be the correct way of doing this. What do you think?

[AndyZe]

That sounds like it is worth trying. I also see that (in the example you linked) they use a math::PID type of controller which has saturation (cmdMax, cmdMin) options. That would be nice to have.

I don't trust the derivative term from the control_toolbox PID controller. It gave some really weird output. Yet another reason to switch to math::PID type.

I see something weird with this PR and I haven't been able to track down the cause. After I make contact with a surface, the arm occasionally springs back rapidly. I don't know if this is coming from the PID controller or from Gazebo contact physics.

[livanov93]

That sounds like it is worth trying. I also see that (in the example you linked) they use a math::PID type of controller which has saturation (cmdMax, cmdMin) options. That would be nice to have.

Then I will try to add those, and maybe some kind of wrapper for cascade control. Please don't mind if I hijack some commits from here.

I don't trust the derivative term from the control_toolbox PID controller. It gave some really weird output. Yet another reason to switch to math::PID type.

Too bad. I wasn't aware about that issue.

I see something weird with this PR and I haven't been able to track down the cause. After I make contact with a surface, the arm occasionally springs back rapidly. I don't know if this is coming from the PID controller or from Gazebo contact physics.

I can't tell without testing :/

[ahcorde]

I merged the ign to gz PR, and you have some conflicts, Sorry, it was a huge change

[AndyZe]

Closing this because I think effort-based control like in #122 would be a lot better 👍