Implement TaskSpaceInverseDynamics interface

[GiulioRomualdi]

This follows from #251 (comment)

Before merging we should change ContactWrench struct since it's not containing the value of the contact wrench.

In your opinion does it make sense to store it in an Eigen::Vector6d?

[S-Dafarra]

Just a typo

[S-Dafarra]

Suggested change

	* TaskSpaceInverseDynamics implements the interface for the task sapce inverse
	* TaskSpaceInverseDynamics implements the interface for the task space inverse

[S-Dafarra]

In your opinion does it make sense to store it in an Eigen::Vector6d?

The only problem would be the possibility to get the linear and angular part. Maybe extending Eigen::Vector6d 🤔?

[prashanthr05]

In your opinion does it make sense to store it in an Eigen::Vector6d?

The only problem would be the possibility to get the linear and angular part. Maybe extending Eigen::Vector6d ?

x.head<3>(), x.tail<3>() should be enough right? maybe not descriptive enough.

[traversaro]

In your opinion does it make sense to store it in an Eigen::Vector6d?

The only problem would be the possibility to get the linear and angular part. Maybe extending Eigen::Vector6d ?

x.head<3>(), x.tail<3>() should be enough right? maybe not descriptive enough.

Yes, I think those would risk to be quite error prone, even not extending Eigen::Vector6d it could make sense to wrap x.tail<3>() in functions that take Eigen::Vector6d in input.

[GiulioRomualdi]

manif::se3d::tanget? I don't really like it

[prashanthr05]

manif::se3d::tanget? I don't really like it

technically a co-tangent.

[GiulioRomualdi]

technically a co-tangent.

Yes you're right but manif does not define cotangent vector. 😢

[GiulioRomualdi]

Otherwise iDynTree::Wrench

[S-Dafarra]

Otherwise iDynTree::Wrench

I think we are too centered around Eigen objects. A wrench could part of a set of optimization variables, so it is better to be consistent across the library.

[S-Dafarra]

I think that for the time being we can stay simple, and use, as @traversaro suggested, some helper functions to "view" the linear and angular part given a 6D vector.

[S-Dafarra]

The take home message is: the assumption "the first 3 elements are the linear part" is error-prone, and we want to remove that assumption. Internally we can still use a 6D vector though.

[GiulioRomualdi]

A possible solution is to define (i don't know where) this class See: http://eigen.tuxfamily.org/dox-3.2/TopicCustomizingEigen.html

class Wrench : public  Eigen::Vector6d
{
public:
    Wrench(void):Eigen::Vector6d() {}
    typedef Eigen::Vector6d Base;
    // This constructor allows you to construct Wrench from Eigen expressions
    template<typename OtherDerived>
    Wrench(const Eigen::MatrixBase<OtherDerived>& other)
        : Eigen::Vector6d(other)
    { }
    // This method allows you to assign Eigen expressions to MyVectorType
    template<typename OtherDerived>
    Wrench & operator= (const Eigen::MatrixBase <OtherDerived>& other)
    {
        this->Base::operator=(other);
        return *this;
    }


Eigen::Block<Wrench, 3, 1>
force()
{
  return this->Base::head<3>();
}
 
Eigen::Block<Wrench, 3, 1> torque()
{
  return this->Base::tail<3>();
}
 
};

[S-Dafarra]

A possible solution is to define (i don't know where)

Here https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/src/GenericContainer/include/BipedalLocomotion/GenericContainer?

[GiulioRomualdi]

Hi all, I implement the Wrench class in the Math component. I also defined two operators that can be used to rotate/transform a wrench using manif objects.

I also introduced in ContactWrench in the Contacts component.

@isorrentino, @S-Dafarra @prashanthr05 and/or @traversaro could you please review the PR again?

[GiulioRomualdi]

Before merging I should

• edit the changelog
• edit the readme

Furthermore this PR add manif as dependencies of Math. I don’t know if this is a problem.
notice that Wrench is a template class and also the operators that I defined are template. So in theory we need manif only if the user includes Wrench.h and not to compile the math component.

@traversaro and @S-Dafarra

[traversaro]

Furthermore this PR add manif as dependencies of Math. I don’t know if this is a problem.

I don't think this is a problem, manif is already an almost compulsory dependency.

[GiulioRomualdi]

I don't think this is a problem, manif is already an almost compulsory dependency.

There are actually two possible solutions:

1. Keep the operator*() methods in the header and add define guards to avoid installing it if manif is not used.
2. Move the operator*() methods in a separate header and install it only if manif is used. This is similar to what is done in `conversions

[prashanthr05]

Minor comment.

[S-Dafarra]

I don't think this is a problem, manif is already an almost compulsory dependency.

There are actually two possible solutions:

1. Keep the operator*() methods in the header and add define guards to avoid installing it if manif is not used.
2. Move the operator*() methods in a separate header and install it only if manif is used. This is similar to what is done in `conversions

To be honest I don't like too much APIs that may change according to a build configuration. I think that at this point, either we add manif as a required dependency, or we don't define the operator.

[S-Dafarra]

Yet another possibility could be to create another library in Math that is compiled only if manif is present, and put Wrench in it.

[GiulioRomualdi]

After a discussion with @S-Dafarra, we decided to add manif as dependencies for math. So I updated the cmake 374a5c7, the readme e9b713e and the changelog eba86c7