Add position control (with interpolation) to control board plugin

[xela-95]

We want to add a position control mode to the control board plugin, corresponding to the same control mode present in gazebo-yarp-plugins.

Acceptance criteria:

• Write a unit test that sets a reference position and validate that when checkMotionDonereturns true the actual joint position is close to the reference one
• Validate logic of the reset of trajectory generator with and without initialConfiguration configuration parameter in .ini file

[xela-95]

CC @traversaro

[xela-95]

Looking at the code implementing IPositionControl the main task are carried out by the trajectory generators (TVP, MJT, constant speed, ...), implemented in https://github.com/robotology/gazebo-yarp-plugins/blob/master/plugins/controlboard/src/ControlBoardDriverTrajectory.cpp .

@traversaro do you suggest to port this class as-is (updating only the calls to gazebo APIs) or there is some modification you have in mind?

[traversaro]

@traversaro do you suggest to port this class as-is (updating only the calls to gazebo APIs) or there is some modification you have in mind?

Yes, I think we can copy directly the class, we can leave future evolutions in the future.

[xela-95]

Note: the default trajectory generation type is minimum jerk. https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L131-L155

[xela-95]

Note: a single generation type can be specified for all joints: https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L136-L144

[xela-95]

I needed to modify the initTrajectory() method of the classes implementing TrajectoryGenerator since the way in which the update period of the simulation can be obtained changed in the new version of Gazebo.

Here is an example for the minimum jerk generator: https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriverTrajectory.cpp#L207-L208

Now the data about simulation time is passed to the system plugin interfaces like the PreUpdate, Update, PostUpdate methods.

I'm thinking about how to arrange the flow of the simulation period to trajectory generators. @traversaro the simulation period in Gazebo can change at each iteration? i.e. there is the possibility to use variable step solvers?

[traversaro]

I'm thinking about how to arrange the flow of the simulation period to trajectory generators. @traversaro the simulation period in Gazebo can change at each iteration? i.e. there is the possibility to use variable step solvers?

In theory yes, in practice I guess we can stick to the simple case for now of assuming that the update period is constant. In any case it probably make sense to decouple the trajectory sampling time (that should be coupled to the control period) from the simulation step time. The control period should be fixed and should not depend on from the simulation step time.

[traversaro]

The control period should be fixed and should not depend on from the simulation step time.

In particular, I am not sure how I missed this, but what I mean is that the PID should not run at every simulation step (as done in https://github.com/robotology/gz-sim-yarp-plugins/blob/main/plugins/controlboard/src/ControlBoard.cpp#L359), but instead we should have an independent sampling, and check if it needs to run again given the simulation time. Something like:

void ControlBoard::PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm)
{

    // The class should have a m_controlUpdatePeriod attribute, parsed from config files
    // and with a reasonable default (like 1ms)
    // There should also be an attribute called m_lastControlUpdateInstant to capture the
    // last time the update have been run (and should be reset)
    if (_info.simTime-m_lastControlUpdateInstant > m_controlUpdatePeriod)
    {
        auto dt = _info.simTime-m_lastControlUpdateInstant;
        if (!updateReferences(_info, _ecm, dt))
        {
            yError() << "Error while updating control references";
        }
        m_lastControlUpdateInstant = _info.simTime;
    }
}

[traversaro]

See also robotology/gazebo-yarp-plugins#69 for the related bug on gazebo-yarp-plugins side.

[traversaro]

See also robotology/gazebo-yarp-plugins#69 for the related bug on gazebo-yarp-plugins side.

Interestingly, probably it could also make sense to only sample sensors measurements when the control loop is running, effectively subsampling the output of the simulator. By the way, this is similar to what we discussed yesterday with @DavideBarbone and @Gio-DS .

[xela-95]

Thank you @traversaro, now it's clear! while porting huge code portions and refactoring them is difficult to understand which are the specifications that the new plugin should maintain.

I will try to be more verbose when reviewing the gazebo-yarp-plugins to try not to skip this kind of requirements.

[xela-95]

I opened:

• Decouple the control update period from simulation one in control board plugin #100 to track the decoupling of control period from simulation one;
• Decouple sensor reading frequency from simulation one #101 to track the decoupling of sensor frequency from simulation one.

[traversaro]

refactoring them is difficult to understand which are the specifications that the new plugin should maintain.

Yes, in this particular case it was quite difficult as the old logic was "wrong" in a sense.

[xela-95]

While porting the resetPositionsAndTrajectoryGenerators method I've not understood the role of this line:
https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L561-L563

I did not find any reference to the m_oldReferencePositions vector in the code, so I'm assuming it's a legacy variable not used anymore.

[xela-95]

Also I've noted that some variables are rest only in the if branch in which the initial configuration is found, but they are not reset when using the actual positions. Is this wanted? It is not done since it is assumed that the trajectory generator reference positions are already equal to the actual joint position?

https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L484-L488

[traversaro]

Also I've noted that some variables are rest only in the if branch in which the initial configuration is found, but they are not reset when using the actual positions. Is this wanted? It is not done since it is assumed that the trajectory generator reference positions are already equal to the actual joint position?

https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L484-L488

I guess this worked just as the initial position of both without configuration file is 0. However, better to explicitly reset them, as this could one of the source of problems that people have in resets, fyi @rob-mau @S-Dafarra .

[xela-95]

Udpate: I should have ported the needed functionality to make the position control work with the single pendulum.

Now I'm facing an issue when trying to control the single pendulum model with the yarpmotorgui, since when I launch it and try to change the reference position the yarpmotorgui and gazebo freeze and I have to kill them, most probably a deadlock condition.

2024-02-29_11-04-00.mp4

[xela-95]

Now I'm facing an issue when trying to control the single pendulum model with the yarpmotorgui, since when I launch it and try to change the reference position the yarpmotorgui and gazebo freeze and I have to kill them, most probably a deadlock condition.

Fixed in ca3778f

[xela-95]

This is one of the first working demos:

2024-02-29_14-11-32.mp4

I still have to test that the trajectory generation works with every type of generator and test different configurations and defaults.

[xela-95]

With 5528c12 I've added a simple unit test: I set a reference position and run the simulation (at batches of 1000 steps) until checkMotionDone returns true. At that point I check that the measured joint position is really close to the reference one.

Here's a video of the test running:

2024-03-01_12-15-50.mp4

In this case I'm using the default settings for the trajectory generation (min jerk trajectory with reference speed of 10.0 deg/s and a reference acceleration of 10.0 deg/s^2). In future it can be set up a test trying different configurations, by supplying different .ini files and sdf files (maybe generated from a template using cmake or some c++ library).