Topic/contact force - [merged]

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 14:05

Merges topic/contact-force -> devel

Solves #272

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 14:09

marked as a Work In Progress

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 14:10

This PR doesn't deal yet with the problem of linking the contactdata to cost->contactdata. I've added the wip status in order to discuss how to implement that.

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 14:17

Earlier, in the python version, I was doing something like this:

for d in problem.runningDatas:
   # Because we also have the impact models without differntial.
   if hasattr(d, "differential"):
       for (patchname, contactData) in d.differential.contact.contacts.iteritems():
           if "forces_" + patchname in d.differential.costs.costs:
                d.differential.costs["forces_" + patchname].contact = contactData

I could try to do something similar in the shooting problem class constructor. After problem.createData() is called, we search for integrated action datas, and then in these action datas, we search if a CostModelContactForce class exists. I don't really see what other solution I could use.

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 14:20

Although it is a really ugly solution.

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 14:24

added 5 commits

• ab95b041...29495486 - 2 commits from branch loco-3d:devel
• ff7de204 - [cost][contact-force] add contact-force costs
• 806f674a - [bindings][cost][contact force] add contact force
• 3e168443 - [cost][cmake][contact force] install contact force

Compare with previous version

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:42

Could you add asserts in the construction that checks fref dimension and contactc.get_nc() are the same?

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:42

same here

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:42

and here

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:42

same here

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:47

I think it's better to use Eigen::VectorXd so we can handle 3D cases too. Do you agree?

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 14:49

Yes, I missed that. Thanks.

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:50

If we agree with https://gepgitlab.laas.fr/loco-3d/crocoddyl/merge_requests/256#note_7646, then we could create a function in ContactModelAbstract to retrieve the force vector.

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:51

I think CostModelForce is better name since we could use this functions outside contact context.

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 14:56

I think this should be a standard policy in all cost classes. Currently, I see that different cost classes have different names and types for references (com has cref(Eigen::Vector), framevelocity has vref(FrameMotion) etc). All these classes have different getter functions as a result(get_cref, get_vref). The user needs to remember what the internal name of the variable is, in order to extract the reference. A standardized name and type for all cost classes would be useful.

If you are okay, I'll create an issue to rename all these getters to get_ref(). This doesn't have to be priority.

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:56

Why do you expose contact_ too? Use this policy bp::return_value_policy<bp::return_by_value>() with boost::shared_ptr

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 14:59

We should provide sort of unittest code, however @MaximilienNaveau did arrive here yet. Maybe you could add it as Python unittest as I did for other cases.

Additionally, you need

1. to investigate what happens in your CI
2. to format the code as explained in the wiki

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 15:02

It sounds a good idea, but not for this PR. Don't put it as priority neither, however, it would be great to do it before releasing the code (within a three weeks).

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:06

What do you mean?

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:07

I added ContactForce, since the class is based completely on the ContactModel class. The same way that the FrameVelocity class is based completely on the Frame class.

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 15:10

Fair enough, I missed this point!

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 15:13

Never mind, let's keep it as it is.

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:14

changed this line in version 3 of the diff

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:14

changed this line in version 3 of the diff

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:14

changed this line in version 3 of the diff

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:14

changed this line in version 3 of the diff

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:14

changed this line in version 3 of the diff

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:14

changed this line in version 3 of the diff

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:14

added 1 commit

• 21ab5449 - [format] clang format code

Compare with previous version

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 15:18

No needed this. Let's keep everything with Pinocchio::Force.

It's good as you did it

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:19

contactdata->f is stored as pinocchio::Force. This is true even for ContactModel3d. If I use Eigen::VectorXd here, I need to know what type of contactmodel we are using. I think we need to start saving contactdata->f as an Eigen::VectorXd as well. (This was the approach being followed in the python version.)

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:23

I did add an assert to check that activation->get_nr()==contact->get_nc()

[wxmerkt]

In GitLab by @cmastalli on Nov 6, 2019, 15:27

I don't think modifying the ShootingProblem is a good idea, it should be agnostic.

Instead I proposed to have a sharedMemory function (similar to this https://gepgitlab.laas.fr/loco-3d/crocoddyl/blob/devel/include/crocoddyl/multibody/actions/contact-fwddyn.hpp#L72) that shares (whenever with enable the force flag in action constructor) the same memory between cost and contact, i.e.:

  costs->shareMemory(contacts);

where contacts is the data.

It requires to make some tricks that might easily create mess. So, we could first release the code working with the ugly Python trick that you used to do, and later we add this functionality

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:27

added 1 commit

• a08d1a44 - [cost][contact force] add assert to check that the size of activation matches contact

Compare with previous version

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:29

Do you mean that I should expose contact?

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 15:45

I'm changing contactdata->f to Eigen::VectorXd. This should make it possible to use this CostModel.

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 17:16

added 2 commits

• 5a9a7cfb - [contact] save the force applied at the frame as an vectorxd. needed for costmodelcontactforce
• c3d7a9ac - [cost][contact force] save reference as eigen vector

Compare with previous version

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 17:17

Done. I'll resolve this.

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 17:18

I didn't change ContactData->f. Instead I created frame_force, which stores the contact force applied at the frame level. The contactdata->f now stores the force at the joint level. This is the minimal amount of change possible.

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 23:38

mentioned in merge request !258

[wxmerkt]

In GitLab by @proyan on Nov 6, 2019, 23:39

added 29 commits

• c3d7a9ac...22fd74ac - 20 commits from branch loco-3d:devel
• 5683d20e - [CMake] sync submodule
• 11b034b1 - [CMake] fix submodule detection
• f21b847d - [cost][contact-force] add contact-force costs
• 408e2c0f - [bindings][cost][contact force] add contact force
• 6d151d76 - [cost][cmake][contact force] install contact force
• 4d18b0b5 - [format] clang format code
• 49b9b54d - [cost][contact force] add assert to check that the size of activation matches contact
• 4d78b8e0 - [contact] save the force applied at the frame as an vectorxd. needed for costmodelcontactforce
• b979309d - [cost][contact force] save reference as eigen vector

Compare with previous version

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 08:27

added 1 commit

• c39eaacc - [bindings][contact force] expose costdata.contactdata element

Compare with previous version

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 08:27

DOne.

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 09:03

added 2 commits

• 651f7b55 - [unittest][contacts][WIP] copy the unittest from python into the cpp version. update api
• 3537c1a5 - [a2m,m2a] add helper functions in util

Compare with previous version

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 09:06

Regarding the unittest:

I'm starting to copy the unittest test_contacts.py from the python version, and updating the API to match the cpp version. I think we have everything to make this work, and compare against the num-diff classes in python.

This is not yet finished. I've added the unittest, and updated the imports. I still need to change the API to make it suitable for the cpp version.

[wxmerkt]

In GitLab by @cmastalli on Nov 7, 2019, 09:20

Let me know when I should review this PR.

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 19:43

added 15 commits

• 3537c1a5...df4f47cc - 5 commits from branch loco-3d:devel
• e7ab19ef - [cost][contact-force] add contact-force costs
• 326a71cf - [bindings][cost][contact force] add contact force
• 68d44325 - [cost][cmake][contact force] install contact force
• 5b8f4b23 - [format] clang format code
• a4e2a9f6 - [cost][contact force] add assert to check that the size of activation matches contact
• efbb1895 - [contact] save the force applied at the frame as an vectorxd. needed for costmodelcontactforce
• 5b1865fa - [cost][contact force] save reference as eigen vector
• 71d64e51 - [bindings][contact force] expose costdata.contactdata element
• 794f49d2 - [unittest][contacts][WIP] copy the unittest from python into the cpp version. update api
• e2fcd769 - [unittest][contact] convert test_contacts to current api

Compare with previous version

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 19:44

added 1 commit

• 79327f57 - [format]

Compare with previous version

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 19:44

unmarked as a Work In Progress

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 19:44

I don't think we'll get a solution in this PR. Let's keep a separate issue on it then. I'll resolve this.

[wxmerkt]

In GitLab by @proyan on Nov 7, 2019, 19:47

I've added the unittest. It is the test_contacts.py which we used to verify the python version. Since @MaximilienNaveau is already working on the unittests, this one is just as a makeshift personal satisfaction that the code actually works. Meantime, it also makes sure that the contact models are working. This unittest is not added to the CI.