Segments removal

[francofusco]

This PR introduces new functions to the Chain and Tree classes to remove segments. This can be useful in some applications like pick and place: once an object is picked, adding the object to the chain/tree as a segment allows, e.g., to compensate for its dynamics while moving the robot around. Once the object is placed, the current implementation would require to reset the chain/tree to the original state using the assignment operator. Having dedicated functions to remove segments should instead be more efficient as the whole object does not need to be copied again. It also removes the need to store the "original" chain/tree (which has to be kept in order to reset the "working" instance used by solvers).

[meyerj]

I merged the changes from the latest master branch, but it seems that one unit test is still failing due to an Eigen assertion:

3: .LDL Solver Test
3: .solvertest: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:413: Eigen::DenseCoeffsBase<type-parameter-0-0, 1>::Scalar &Eigen::DenseCoeffsBase<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 1>::operator()(Eigen::Index) [Derived = Eigen::Matrix<double, -1, 1, 0, -1, 1>, Level = 1]: Assertion `index >= 0 && index < size()' failed.

[francofusco]

Thanks for having updated the PR!

I am really sorry but I cannot manage to figure out what the problem is. From my understanding, it seems that the failing test is SolverTest::LDLdecompTest. However, it should not be related to the code I added, and in addition it runs fine on my PC: I just compiled that bit of code and it seems to run with no issues.

I will try to better investigate the problem, if you have any suggestion it would surely help!

[francofusco]

Clearly, I spoke too quickly. The error happened after the test I mentioned, in the code that I added.

In particular, there are two lines which were failing: this and the following one. I am not sure if this is the case, but after removing the last segment of the chain, I guess there were some problems with the IK since the robot had less than 6 dof.

I modified the test so that more segments are added to the chain. If I now remove the segments, the robot still has 6dof and the two solvers succeed.