Change the ownership of the KinDynComputation in ContinuousDynamicalSystem

[GiulioRomualdi]

Some classes of the ContinuousDynamicalSystem component required a kindyn object to work.

https://github.com/dic-iit/bipedal-locomotion-framework/blob/70c85ba1db9b27f59bfdb120cc73e2fa43d70c85/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h#L95-L100

Passing the object as shared_ptr could not be the right choice indeed someone may keep the object outside and call setRobotState.

Here there are three possible solutions:

1. use an unique_ptr. The ownership is well defined and it is simple to change handle the transition of already existing code.
2. build the kindyn computation object in the ContinuousDynamicalSystem class. In this case, the user should pass the model + base_frame name in the parameters.
3. Pass a reference of the kindyn computation object. However, I don't know if the copy assignment operator is defined for kindyncomputations

This issue is the consequence of the wrong usage of the FixedBaseDynamics in https://github.com/dic-iit/bipedal-locomotion-framework/pull/251 where the same kindyn object is passed to the TSID and the FixedBaseDynamics class.

What do you think @traversaro @S-Dafarra @isorrentino @prashanthr05

[traversaro]

I like option 2 as it kind of cleanup the API, with the following advantages:

• making it possible to eventually switch from KinDynComputation to something else (iDynFor::KinDynComputation) and it would simplify
• clarify the input/output structure, without conflating the rigid body model and its state (see point 3 in https://github.com/dic-iit/bipedal-locomotion-framework/pull/229#issuecomment-795247879)
• it simplifies exposing the blocks in higher level languages or with different paradigms such as BlockFactory blocks

A possible downside is that know we would have several duplicate KinDynComputations that perform the computations several times, but if that turns out to be a problem I think we can think of having a KinDynComputations(Buffer|State|Output) that contains the information that is typically buffered inside the KinDynComputatiojns, and use it as a proper input to the block.

[traversaro]

A possible downside is that know we would have several duplicate KinDynComputations that perform the computations several times, but if that turns out to be a problem I think we can think of having a KinDynComputations(Buffer|State|Output) that contains the information that is typically buffered inside the KinDynComputatiojns, and use it as a proper input to the block.

However, this downside is intrinsic if you want to avoid that SetRobotState is called indipendently for each block, and not a specific limitation of option 2.

[GiulioRomualdi]

When I ws implementing the ContinuousDynamicalSystem component I had in mind to keep the kindyn object inside the class. Indeed the setRobotState() is called by the dynamics() method.

https://github.com/dic-iit/bipedal-locomotion-framework/blob/70c85ba1db9b27f59bfdb120cc73e2fa43d70c85/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp#L121-L126

No one should touch the kindyn object from outside. So I think we can easily pass an iDynTree model to the ContinuousDynamicalSystem and build the kindyn object in the DynamicalSystem.

Notice that this is not valid for TSID/IK where set robotState should be called from the user. Also there it would be nice to avoid the user to call setRobotState from outside, howver all the tasks shares the same KinDynObject (using shared_ptr), and the user when create a task have to pass the pointer to a kinDyn object.

A possible solution there is to create an ILinearTask with all the methods privates https://github.com/dic-iit/bipedal-locomotion-framework/blob/70c85ba1db9b27f59bfdb120cc73e2fa43d70c85/src/System/include/BipedalLocomotion/System/LinearTask.h#L63-L111

and make IntegrationBasedIK and TSID friends of ILinearTask. In this way the user cannot play with the kindynobject

[S-Dafarra]

To be honest, I am a bit lost. What would be the desiderata? In the end, it depends on what you need. The reasons I see behind sharing the same KinDyn object would be:

• avoid to do dynamics computations n-times
• every object only need to read some data, without modifying anything.

In the second case, it is important to define who needs to update KinDyn. Hence, the concept of ownership is not sufficient. It is not just matter of who needs to allocate and deallocate the object, but also who takes care of updating its internal state. If this is the case, from my point of view we would need another interface that simply allows to read, and then only one is allowed to write.

[GiulioRomualdi]

To be honest, I am a bit lost. What would be the desiderata?

You're right let me better explain the problem. In #251 @isorrentino was using the FixedBaseDynamics class to simulate the robot model and the QPFixedBase to control the robot. Both the classes require a shared_ptr to a KinDynComputations object. In theory the pointers should point to two different KinDynComputations instances

This issue was mainly related to point 1

1. FixedBaseDynamics: the main goal of this class is to use it with Integrator. Inside dynamics() we set the state of KinDynComputations and we get some matrices (e.g. the mass matrix). The FixedBaseDynamics should be seen as a real mechanical system. So in my opinion the KinDynComputations should not be exposed.
2. TSID/IK: here each task requires a shared_ptr to kindyn object. This is useful to avoid computing the same quantity several times. We assume that the user set the robot state in the main. Now two problems rise:
   1. Right now there is no way to be sure that the user passes the same kinDyn object to all the tasks

      auto kinDyn1 = std::make_shared<KinDynComputations>();
      CoMTask.setKinDyn(kinDyn1);
      auto kinDyn2 = std::make_shared<KinDynComputations>();
      SE3Task.setKinDyn(kinDyn2);

   2. The user should call KinDynComputations::setRobotState() in the main.

Possible solution follows:

A possible solution to 1 replace setKinDyn() with setRobotModel() and pass an iDynTree::Model. The kinDyn object will be then stored in the DynamicalSystem

A possible solution to 2.i is to make impossible to call setKinDyn by the user. This can be done making setKinDyn() a private member function of LinearTask (and now it is not a function of LinearTask) and make IntegrationBaseIK a friend class of LinearTask. Something similar to what is done with VariableHandler, where the IK is in charge to set the handlers for all the tasks.

https://github.com/dic-iit/bipedal-locomotion-framework/blob/70c85ba1db9b27f59bfdb120cc73e2fa43d70c85/src/IK/src/QPInverseKinematics.cpp#L214

A possible solution to 2.ii implement setRobotState in the IK/TSID and set the kindyn computation object.

If I've to tell the truth, I like the solution to problem 1 (FixedBaseDynamics should be seen as a way to simulate the system and so a standalone block) , but I'm not a fan of the solution of problems 2.i and 2.ii. Indeed sharing the same kindyn object between different classes is really useful because it gives the possibility to some information (e.g. the CoM position) without having a copy of the KinDyn is really useful.

[traversaro]

I think we reached a consensus on 1, so I would focus on 2.i and 2.ii . If sharing the KinDynComputations object across tasks is useful, I think this is orchestrated by the IntegrationBaseIK or similar class. The user sets the iDynTree::Model in IntegrationBaseIK and then the IntegrationBaseIK class creates the KinDynComputations instance with all the tasks, via private methods or other techniques. If I understood correctly, this is what @GiulioRomualdi is proposing, so I am a bit confused with the last sentence:

but I'm not a fan of the solution of problems 2.i and 2.ii. Indeed sharing the same kindyn object between different classes is really useful because it gives the possibility to some information (e.g. the CoM position) without having a copy of the KinDyn is really useful.

The solution to problems 2.i and 2.ii still permits to share the KinDynComputations across tasks, no? Or I am missing something?