Bipedal-locomotion-framework: Discus the possibility to implement a wrapper around kindyn to better handle const methods

[GiulioRomualdi]

This follows https://github.com/dic-iit/bipedal-locomotion-framework/issues/273

Currently, TSID/IK (and probably other classes) takes as input a shared_ptr to a kindyn object. This approach was chosen to increase the performances of some classes. For instance, it's not necessary to run several times the RNE algorithm on the same set of data. However, it is not super clear who is in charge to set the robot state and how the LinearTasks can modify the kindyn object. Indeed given the current implementation of KinDyn several getters are not const functions (e.g. getFreeFloatingMassMatrix) and it is not possible to pass a std::shared_ptr<const KinDynComputations> object to LinearTask.

Discussing with @S-Dafarra, we came out with a possible solution. We may implement an interface to kindyn that stores a pointer to kindyn. In this case, we can easily fake "constness". In other words, we may access to kindyn object methods only through this new wrapper, furthermore it will also simplify the possibility to switch to other libraries for the dynamics computation (e.g. Pinocchio)

class IRigidBodyAlgorithms
{
public:
  virtual bool setRobotState(....)  = 0;
  virtual bool getMassMatrix(....) const = 0;
};


class KinDynComputationWrapper : public IRigidBodyAlgorithms
{
std::shared_ptr<KinDynComputations> m_kinDyn;

public:
  bool setRobotState(....)  final
  {
      return m_kinDyn->setRobotState(....);  
  }

  bool getMassMatrix(....)  const final
  {
      return m_kinDyn->getMassMatrix(....);  
  }
};

Thanks to this we can pass a const pointer to IRigidBodyAlgorithms to the LinearTask class so we are sure that setRobotState is not called.

cc @traversaro

[traversaro]

I guess setRobotState is not meant to be const, while getMassMatrix is, right?

[GiulioRomualdi]

I guess setRobotState is not meant to be const, while getMassMatrix is, right?

Thank you @traversaro. https://github.com/dic-iit/bipedal-locomotion-framework/issues/305#issue-884238658 updated

[traversaro]

By the way, note that you can easily force away const specifiers via const_cast .

[S-Dafarra]

Maybe, via a proper interface, it is possible to make sure that there is only one object that can set the state :thinking:

[traversaro]

You can have an object that inherits from two interfaces (i.e. pure C++ interfaces), one that contains the methods to set the state and another to retrieve the results, and pass to the object that can set the state some reference to the interface that can be used to set the state, while to the other just the interface to get the results.

[S-Dafarra]

But this won't prevent having multiple "writers". I mean, maybe two objects need to have both read and write access and, by mistake, they could be sharing the same KinDynObject.

[traversaro]

But this won't prevent having multiple "writers". I mean, maybe two objects need to have both read and write access and, by mistake, they could be sharing the same KinDynObject.

Yes, what I described it was just meant to avoid that someone that is writing a Task by error sets the state, not preventing all possible misuses.

[GiulioRomualdi]

Probably one way to ensure one single writer is using a Singleton but I'm not supersure.

[S-Dafarra]

A singleton ensures that there is a single object of that kind in the application. Not sure this is what we need. I don't think this has to be determined at compilation level.

[diegoferigo]

By the way, note that you can easily force away const specifiers via const_cast .

To me it seems that this classic workaround is the best trade-off between preventing user's mistakes (enforcing const), flexibility, and simplicity in the implementation. When needed, advanced users that know what they're doing can std::static|dynamic_pointer_cast and access the object in rw. This could be also done from the Python bindings e.g. by adding a new mutable() method to the KinDynComputations wrapper that returns a non-const shared pointer.

We may implement an interface to kindyn that stores a pointer to kindyn. In this case, we can easily fake "constness".

In other words, if we have to fake constness, at least we can do it explicitly when needed.