Two frames contact pr

[egordv]

This PR is related to the issue "TSID for "pantograph/biarticular" robot leg design" #165

I want to share the code I wrote to create a new type of contact following the discussion in the mentioned PR.
The main aim of this PR is to give the TSID dynamics formulation the ability to work with closed kinematic chains specified via user code (not URDF).

This PR adds two new items:

1. New motion task: TwoFramesEquality. This task can be used to force some frames of the robot to keep their relative displacement zero. The main difference with SE-Equality is that the TwoFramesEquality works with the relative placement of the frames, so the frames will be 'glued' to each other, not to the specified world position.
2. New contact type: TwoFramesContact. This contact uses the TwoFramesEquality motion task as it's base emulating a ball joint via using only translation part of the mask to non-zero.

The usage of TwoFramesContact alongside with zero actuation bounds allows to create a closed kinematic chains in current TSID formulation (with current version of Pinocchio and URDF parser). The contact should be created bu user via TwoFramesContact creation.

The PR also includes a minimal example showing how to add a closed kinematic chain to Talos gripper mechanism via demo/demo_talos_gripper_closed_kinematic_chain.py

This is the sample how this gripper works without additional contact:

This is the sample how this gripper works with additional contact via TwoFramesContact thus creating a closed kinematic chain:

Regarding the code: there are many virtual methods in conact-base.hpp not relevant to the concept of TwoFramesContact like setContactNormal, etc. Also the underlying motion task can be only the SE3Equality so the PR code contains some rudimentary methods to mitigate this in not very elegant way, may be some refactoring is required?

[hrp2-14]

Hi ! This project doesn't usually accept pull requests on the main branch.
If this wasn't intentionnal, you can change the base branch of this PR to devel
(No need to close it for that). Best, a bot.

[andreadelprete]

Thanks for this PR. I'm currently very busy, but I'll try to review this next week.

[andreadelprete]

I've reviewed the code and overall it looks great. I've left some comments on small things that should be fixed before merging this PR. The main one is removing the STL/urdf files, which should not stay in this repo in my opinion. I know we have the model of a robot in TSID, but that was introduced a long time ago, and it was justified by the need to give students an easy way to use the software. Nowadays, with packages as example-robot-data, I think this is no longer necessary. Let me know your thoughts, and thanks for the great work!

[egordv]

Hello Andrea!
Thank you for a review and comments, I'm glad to participate in this great project.
I renamed the class to ContactTwoFramePositions and moved URDF/STL to external repo as you recommended.
BTW is everything OK with m_dummyMotionTask approach?

[andreadelprete]

Nice job addressing all my comments, thanks! I've left just one additional comment related to the dummyMotionTask you mentioned.

[andreadelprete]

I had not noticed this in my first review, thanks for pointing it out. Indeed using this dummyMotionTask is not ideal. It would probably be better to change the API of ContactBase, so that the method getMotionTask returns a MotionTaskBase rather than an SE3EqualityTask. Would you agree with that?

[egordv]

For me the MotionTask approach looks neat, so I implemented it.
The resultant ContactTwoFramePositions contact works OK.
The implementation requires static_cast here inTaskCopEquality, this task is not covered by unit tests, can you check if its OK too?

[andreadelprete]

Thanks for implementing these changes. There's still something unclear to me (See comments below), but apart from this, everything looks fine!

[andreadelprete]

Shouldn't the getMotionTask method return a TaskMotion?

[andreadelprete]

The pure virtual method TaskMotion& getMotionTask of the parent class ContactBase should be implemented in this class, shouldn't it?

[egordv]

Hello Andrea!
Sorry for the delay. I tried both variants (getMotionTask returns TaskTwoFramesEquality or TaskMotion) and both works.
The reason I'm using TaskTwoFramesEquality returning variant is because I'm trying to be as close as possible in my realization with existing realizations of the same methods in contact-6d class and others.
So in contact-6d.cpp the pure virtual method TaskMotion& getMotionTask of the parent class ContactBase is also implemented via returning TaskSE3Equality.
Please advice which variant should I use in this PR.

[andreadelprete]

It's really weird that both work. In theory, if a class does not implement all the pure virtual methods of the parent class, then it is an abstract class and therefore cannot be instantiated. Implementing a method with the same name but with a different return type is also weird and should give you an error because you can't overload methods based on return type.

Therefore, you should get a compilation error when you try to create an object of class Contact6d or ContactTwoFramesPositions. Do you have some tests where objects of these classes are created?

[egordv]

Hello Andrea!
Yes, I'm checking my code before pushing with this minimalistic example mentioned above by creating ContactTwoFramePositions object here. Everything works as expected.
Should I create a more sophisticated test?
AFAIK the only part which uses getMotionTask() method of contact object is the TaskCopEquality, which casts the returned object to TaskSE3Equality. So returning the TaskSE3Equality class (as vanilla contact-6d does) by GetMotionTask method is the solution from the "classic" TSID code without my modifications.

[andreadelprete]

You're right. It works. I've recreated the same situation with this minimalistic code, and indeed the compiler doesn't complain. I didn't know you could do this. At this point I am fine with this PR and I am gonna merge it. Thanks again for the great contribution!

#include <iostream>
using namespace std;

class TaskMotion {};
class TaskTwoFramesEquality : public TaskMotion {};

class ContactBase {
    public:
    virtual const TaskMotion& getMotionTask() const = 0;
};

class ContactTwoFramePositions : public ContactBase {
    public:
    TaskTwoFramesEquality m_task;

    const TaskTwoFramesEquality& getMotionTask() const{
        cout<<"TaskTwoFramesEquality& getMotionTask"<<endl;
        return m_task;
    }
};

int main(){
    ContactTwoFramePositions c;
    const TaskMotion& task = c.getMotionTask();
    const TaskTwoFramesEquality& task2 = c.getMotionTask();
    return 0;
}

[egordv]

Good, thank you once again for this great repository!