Add Position direct control to the control board plugin

[xela-95]

We want to add a low level, high frequency position control, corresponding to the Position Control Direct present in gazebo-yarp-plugins control board.

[xela-95]

CC @traversaro

[xela-95]

@traversaro while implementing the IPositionDirectinterface I'm not sure I've fully understood the documentation of the getRefPositionRaw method. It mentions that:

This is the dual of setPositionsRaw and shall return only values sent using IPositionDirect interface. If other interfaces like IPositionControl are implemented by the device, this call must ignore their values, i.e. this call must never return a reference sent using IPositionControl::PositionMove.

I've seen that controlboard plugin in gazebo-yarp-plugins implements this methods and does not check the control mode.

Should I implement this method even if I will implement the IPoisitionControl interface afterwards? Maybe just checking the actual control mode before returning the position reference value could be enough?

[xela-95]

By looking at the gazebo-yarp-plugin implementation, I've seen that the force computation given the position reference is made with a PID: https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L744-L749

Has this issue a dependency on the implementation of IPidControl?

[traversaro]

By looking at the gazebo-yarp-plugin implementation, I've seen that the force computation given the position reference is made with a PID: https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L744-L749

Has this issue a dependency on the implementation of IPidControl?

No, IPIDControl only permits to set the gains, but if we do not want to change them at runtime, we can just set them only from configuration files.

[traversaro]

@traversaro while implementing the IPositionDirectinterface I'm not sure I've fully understood the documentation of the getRefPositionRaw method. It mentions that:

This is the dual of setPositionsRaw and shall return only values sent using IPositionDirect interface. If other interfaces like IPositionControl are implemented by the device, this call must ignore their values, i.e. this call must never return a reference sent using IPositionControl::PositionMove.

I've seen that controlboard plugin in gazebo-yarp-plugins implements this methods and does not check the control mode.

Should I implement this method even if I will implement the IPoisitionControl interface afterwards? Maybe just checking the actual control mode before returning the position reference value could be enough?

The documentation is clear, probably the gazebo-yarp-plugins behaviour predates that version of the documentation. Let's implement what the documentation prescribe.

[xela-95]

By looking at the gazebo-yarp-plugin implementation, I've seen that the force computation given the position reference is made with a PID: https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L744-L749 Has this issue a dependency on the implementation of IPidControl?

No, IPIDControl only permits to set the gains, but if we do not want to change them at runtime, we can just set them only from configuration files.

Ok thank you, I misunderstood the scope of such interface. I will remove the dependency on issue #91 and put it back on backlog.

[xela-95]

PID class is implemented in gz-math: https://github.com/gazebosim/gz-math/blob/gz-math7/include/gz/math/PID.hh

[xela-95]

I have a doubt on how it is supposed to be passed the configuration of the PID from a yarp configuration file: in https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L1569-L1593 there's the parsing of the configuration and there is a predefined POSITION_CONTROL group and a fallback deprecated GAZEBO_PIDS group that will be used if the former is not found.

@traversaro do you suggest porting this logic? Should we support only the POSITION_CONTROL way?

[traversaro]

@traversaro do you suggest porting this logic? Should we support only the POSITION_CONTROL way?

Yes, that would be great. I think we should support just POSITION_CONTROL, that I guess it should be aligned with how the gains are specified for the real robots (but I am not 100% sure about this).

[traversaro]

@traversaro do you suggest porting this logic? Should we support only the POSITION_CONTROL way?

Yes, that would be great. I think we should support just POSITION_CONTROL, that I guess it should be aligned with how the gains are specified for the real robots (but I am not 100% sure about this).

It is also what is currently used in iCub/ergoCub models: https://github.com/icub-tech-iit/ergocub-software/blob/10670acf8120bdaad19c6cb01388eff555f4f356/urdf/ergoCub/conf/gazebo_ergocub_left_leg.ini#L17 .

[xela-95]

@traversaro I've a doubt regarding how the PID parameters are initialized in gazebo-yarp-plugins.

In https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L1176-L1179 the gazebo PID parameters are set by calling, in case of revolute joints, the convertUserGainToGazeboGain method.

This method does the opposite of what I was thinking: it converts the scale parameter from radians to degrees: https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L1772-L1773

Is it correct? I was thinking that user uses degrees and gazebo radians.

[traversaro]

I think what those methods are doing is correct, but the use of those functions are misleading. The unit of measurements used in YARP config files for revolute joints is $\frac{Nm}{deg}$, while the one used in the code is $\frac{Nm}{[rad]}$. So, converting a gain from $\frac{Nm}{deg}$ to $\frac{Nm}{[rad]}$ uses the same formula used to convert a quantity from $[rad]$ to $deg$ (try to write it down), but indeed it is quite misleading to use the same function, probably we could use a function called convertDegreeGainsToRadianGains, or something similar to avoid confusion.

[xela-95]

I think what those methods are doing is correct, but the use of those functions are misleading. The unit of measurements used in YARP config files for revolute joints is Nmdeg, while the one used in the code is Nm[rad]. So, converting a gain from Nmdeg to Nm[rad] uses the same formula used to convert a quantity from [rad] to deg (try to write it down), but indeed it is quite misleading to use the same function, probably we could use a function called convertDegreeGainsToRadianGains, or something similar to avoid confusion.

😱

Yep definitely a better name!

[xela-95]

@traversaro what can be a useful test to validate the direct position implementation? Applying a position reference to the single pendulum (via unit test or using yarpmotorgui) and verifying by reading the actual joint position that it follows the setpoint?

[traversaro]

@traversaro what can be a useful test to validate the direct position implementation? Applying a position reference to the single pendulum (via unit test or using yarpmotorgui) and verifying by reading the actual joint position that it follows the setpoint?

Yes, and for the tests we can check that the average and/or max tracking error is below a given threshold.

[xela-95]

@traversaro I'm getting closer to have a first working implementation, however I'm encountering strange behaviors using the yarpmotorgui.

Here is when I simulate the single pendulum using gazebo classic and gazebo-yarp-plugins:

https://github.com/robotology/gz-sim-yarp-plugins/assets/57228872/72c2b183-1df0-426e-a8fe-ce50d37232e5

Here what happens with Gazebo and gz-yarp-plugins:

https://github.com/robotology/gz-sim-yarp-plugins/assets/57228872/0b19e1d0-d45b-42ef-8677-9ffebf53e051

As you can see the value the yarpmotorgui reports when trying to change the reference position is really large (also the slider it is not initialized in the middle position, but on the left end).

[xela-95]

The issue appears to be connected to missing joint limits that make yarpmotorgui position slider resize badly.

By looking at the code I have not understood if the joint limits (position and velocity) are connected with the ones specified in the urdf (e.g. https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/tutorial/model/single_pendulum/model.urdf#L38) or if they are set from yarp ports calling SetLimits() method from IControlLimits:

https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h#L318-L319

[xela-95]

By manually changing the joint position limit default values

https://github.com/robotology/gz-sim-yarp-plugins/blob/d91ceb0f50caebd7351eca0bc29df528d2ca5c89/plugins/controlboard/include/ControlBoardData.hh#L44-L45

to +/- 10° I made the yarpmotorgui work, as in the image below.

@traversaro who should set these limits and based on what values? I could suppose that the limit should be initialized from the values of the gazebo model (that in turn are specified in the model sdf/urdf). Then, using the IControlLimits interface it will be possible to change them (this should in turn call also SetPositionLimits )

BTW I noticed a steady state error between the reader encoder value and the setpoint.

[traversaro]

BTW I noticed a steady state error between the reader encoder value and the setpoint.

Probably you need to increase the integral gain then?

@traversaro who should set these limits and based on what values? I could suppose that the limit should be initialized from the values of the gazebo model (that in turn are specified in the model sdf/urdf). Then, using the IControlLimits interface it will be possible to change them (this should in turn call also SetPositionLimits )

https://github.com/icub-tech-iit/ergocub-software/blob/10670acf8120bdaad19c6cb01388eff555f4f356/urdf/ergoCub/conf/gazebo_ergocub_head.ini#L48

[xela-95]

@traversaro who should set these limits and based on what values? I could suppose that the limit should be initialized from the values of the gazebo model (that in turn are specified in the model sdf/urdf). Then, using the IControlLimits interface it will be possible to change them (this should in turn call also SetPositionLimits )

Answer: there's a distinction between (simulated) hardware limits and software limits:

• Hardware limits are the ones specified in robot description (<limit> tag in model sdf/urdf) and are managed by the Gazebo simulator
• Software limits are limits, generally more conservative that are managed by the controlboard (https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L813-L860) and they are configured in the control board ini configuration file (e.g. for ergocub model https://github.com/icub-tech-iit/ergocub-software/blob/10670acf8120bdaad19c6cb01388eff555f4f356/urdf/ergoCub/conf/gazebo_ergocub_left_leg.ini#L50C1-L53C46) specified with the [LIMITS] group.

[xela-95]

I'm not able to read the gazebo joint limits, since from their joint API wrapper it is marked as TODO (it was possible in gazebo classic) and from the ECM it seems not querable since the joint limit component is missing (only the components to set new limits are available).

Getting these limits would be useful since we should check if the sw limits are coherent with the hw limits set in the model description and in case the LIMITS group is not defined from the control board we could use them as a fallback.

Related robotics stack exchange question: https://robotics.stackexchange.com/questions/108512/how-to-get-joint-position-limits-in-gazebo

[traversaro]

Ok, but I guess for now we can just use the [LIMITS] one from the .ini file, right?

[xela-95]

Ok, but I guess for now we can just use the [LIMITS] one from the .ini file, right?

yep!

[xela-95]

In commit e9a8a63602a494385dfd18e379b1259bdd6264bc the joint limits are configured from the configuration file.

In the following recording a demo of the control mode:

https://github.com/robotology/gz-sim-yarp-plugins/assets/57228872/470cbccb-579c-4758-9ef7-40c3cb10fbbf

It still remains the fact that in yarpmotorgui the encoder block is red (what does it mean?) and there is a steady state error decreasing with decreasing effect of the joint torque.

The PID tuning is the following: https://github.com/robotology/gz-sim-yarp-plugins/blob/e9a8a63602a494385dfd18e379b1259bdd6264bc/tutorial/single_pendulum/conf/gazebo_controlboard.ini#L7-L18

[traversaro]

the encoder block is red (what does it mean?)

Not sure, I would need to check the code.

[S-Dafarra]

the encoder block is red (what does it mean?)

Not sure, I would need to check the code.

It is because the motion appears not "done". Basically it depends on the output of checkMotionDone. See https://github.com/robotology/yarp/blob/6ca59397df63374d7112b977b9bd425208b6f5c6/src/yarpmotorgui/jointitem.cpp#L1560-L1565 and https://github.com/robotology/yarp/blob/6ca59397df63374d7112b977b9bd425208b6f5c6/src/yarpmotorgui/partitem.cpp#L2234-L2292

[xela-95]

It is because the motion appears not "done". Basically it depends on the output of checkMotionDone. See https://github.com/robotology/yarp/blob/6ca59397df63374d7112b977b9bd425208b6f5c6/src/yarpmotorgui/jointitem.cpp#L1560-L1565 and https://github.com/robotology/yarp/blob/6ca59397df63374d7112b977b9bd425208b6f5c6/src/yarpmotorgui/partitem.cpp#L2234-L2292

Thank you!!

[xela-95]

As suggested in https://github.com/robotology/gz-sim-yarp-plugins/issues/82#issuecomment-1959469967, in a11d5f4ff4b0a694756fbc292e9461b9e95b8e54 I've implemented a unit test in which a sinusoidal position reference is set for the single pendulum joint, and the tracking error (instantaneous in this case) is checked that is below a threshold.

Here is a video of the simulation performed in the test:

https://github.com/robotology/gz-sim-yarp-plugins/assets/57228872/e7df1f44-c977-42bf-bab6-7d1f80116550

[traversaro]

I would increase the amplitude of the sine, at least for scenographic effect. :D

[xela-95]

I would increase the amplitude of the sine, at least for scenographic effect. :D

ahah sure! I will also change the tracking error expectation to check the average error, since I'm encountering issues in a very few steps having a tracking error much higher than the average (like 5 steps in 5000 fail the expectations).

[xela-95]

ahah sure! I will also change the tracking error expectation to check the average error, since I'm encountering issues in a very few steps having a tracking error much higher than the average (like 5 steps in 5000 fail the expectations).

Done in 5cb01a7

https://github.com/robotology/gz-sim-yarp-plugins/assets/57228872/4554526e-13e1-47f6-84ee-520a1e350dc8

[traversaro]

I would increase the amplitude of the sine, at least for scenographic effect. :D

ahah sure! I will also change the tracking error expectation to check the average error, since I'm encountering issues in a very few steps having a tracking error much higher than the average (like 5 steps in 5000 fail the expectations).

Interesting! Probably we can think of investigating this in the future, for example plotting this. However, for the time being we can proceed.