Add methods to compute the Jacobian of a rigid contact point

[kbonnet001]

Add several methods to the Contacts class:

• contactId: returns the index of a contact from its name.

• rigidContactJacobian (and its 3 overloads): similar to markersJacobian implemented in the Markers class, these functions compute the Jacobian of a rigid contact point. The removeAxis parameter has been removed because it modifies the position of the contact point, which is not the intended behavior. Instead, we only want to ignore axes that are not active without altering the geometry of the point.

• projectContactJacobian: this method has been added to preserve only the active axes of the contact point Jacobian. It performs the projection of the full 3D Jacobian onto the set of active contact directions.

  • Example: for a contact point defined in the biomod file with "axis xy", only the rows corresponding to the x and y directions are kept (i.e., rows [:2, :] of the full Jacobian).

---

This change is 

[pariterre]

That seems about right
Can you add Test for this please.

At some point, we probably want a proper python wrapper around the contacts too. If you want to tackle this, let me know, I can help you start the process!

@pariterre reviewed 2 files and all commit messages, and made 7 comments.
Reviewable status: all files reviewed, 6 unresolved discussions (waiting on kbonnet001).

---

src/RigidBody/Contacts.cpp line 273 at r1 (raw file):

}

size_t BIORBD_NAMESPACE::rigidbody::Contacts::contactId(

That seems to be the internal biorbd id

---

src/RigidBody/Contacts.cpp line 455 at r1 (raw file):

            updateKin ? &Q : nullptr);

    utils::Matrix J = updatedModel.CalcPointJacobian(

from that point, it should be:
return rigidContactJacobian(updatedMode, Q, idx); (and contact few lines prior can be removed)

---

src/RigidBody/Contacts.cpp line 401 at r1 (raw file):

}

utils::Matrix BiorbdEigen3::rigidbody::Contacts::projectContactJacobian(

Is this because if a contact is declared on one or two axes, the contactJacobian returns a 3xN matrix regadless? If so, I feel that this method can be fused with the contactJacobian and simply add a flag "bool keepAxes = true" would do the trick.

EDIT: I just had a look and biorbd does not interface the contact Jacobian, so I assume you are calling the RBDL method to get the jacobian? If so, I suggest we create a dedicate contactJacobian in biorbd (so without the "project" in the name) and it returns the expected jacobian matrix (with the flag for keepAxes). This would be similar to markersJacobian in Markers.cpp.

RE-EDIT: Okay so "rigidContactJacobian" is actually what I was referring to in the first "EDIT". As I understand it, projectContactJacobian should not be called by the user as it is more a utility method than anything else. So in the header file, the method should be surrounded by:

protected:
// DOC
methodName(...)

public:

This last edit is the only thing that should be done regarding this long comment (I keep my reasoning here just in case though)

---

src/RigidBody/Contacts.cpp line 490 at r1 (raw file):

    for (size_t idx = 0; idx < nbRigidContacts(); ++idx) {

        const auto& contact = rigidContact(idx);

The inner for-loop could be simplified by:
G.push_back(rigidContactsJacobian(updatedModel, Q, idx));

---

include/RigidBody/Contacts.h line 200 at r1 (raw file):

  /// \throw std::runtime_error if the contact does not exist
  ///
  size_t contactId(const utils::String& name) ;

Is there a reason to access contactId? They can be confusing... because there is basically two id types accross biorbd: the internal biorbd ids and the rbdl ids. They can match or not... (I can't remember for the contacts right now)

If it is necessary, and the current ids are for biorbd (as opposed to rbdl), then there is nothing to change. If however this is the rbdl id, change the name for "contactRbdlId"

---

include/RigidBody/Contacts.h line 370 at r1 (raw file):

  /// \return Vector of Jacobians for all rigid contacts (projected onto active axes)
  ///
  std::vector<utils::Matrix> rigidContactsJacobian(

Swap this overload with the next, so it is consistent with the previous block

[pariterre]

@pariterre made 2 comments.
Reviewable status: all files reviewed, 8 unresolved discussions (waiting on kbonnet001).

---

src/RigidBody/Contacts.cpp line 401 at r1 (raw file):

}

utils::Matrix BiorbdEigen3::rigidbody::Contacts::projectContactJacobian(

BiorbdEigen3 is the wrong namespace as it won't work with the Casadi Backend. you should use BIRORBD_NAMESPACE which automatically takes the value the is required

---

src/RigidBody/Contacts.cpp line 416 at r1 (raw file):

    for (size_t i = 0; i < 3; ++i) {
        if (axes[i]) {
            Jout.row(r++) = J.row(i);

The test currently fail here, but for now, I suspect it is because of the namespace naming discussed earlier

[pariterre]

@kbonnet001
Thanks for this contribution! I took the liberty to update the windows compiler in the github actions. Please consider updated to the latest commit when answering the comments!

[kbonnet001]

Thanks @pariterre for your comments :) Below are the different fixes and some explanations:

• From that point, it should be: return rigidContactJacobian(updatedModel, Q, idx); and The inner for-loop could be simplified by: G.push_back(rigidContactsJacobian(updatedModel, Q, idx));

  • Done.

• [...] This last edit is the only thing that should be done regarding this long comment (I keep my reasoning here just in case though).

  • If I remember correctly, the markersJacobian method returns the Jacobian for all axes. There is a removeAxis flag, and at first I thought it would remove the unused axes and keep only the active ones. However, removeAxes() does not actually remove the axes as I initially thought—it simply sets the corresponding values to zero. That is why I needed to add the new projectContactJacobian method. I hope this makes things clearer now.

    I agree that users should probably not need to use this method. However, I had some doubts, so I initially left it public. I have now moved it to protected.

• Is there a reason to access contactId? They can be confusing... [...]

  • Yes, I agree—it was confusing for me at first as well. For example, if you add a contact point with the xy axes in a biorbd model, biorbd_model.nbContacts() returns 2, and you can get the name of each contact with biorbd_model.contactName(idx).to_string(). However, biorbd_model.nbRigidContacts() returns 1.

    Initially, I added a marker at the same position as the contact point to compute the Jacobian using markersJacobian. I wanted to automatically retrieve the corresponding marker/contact ID, so I thought this method would be useful for that purpose. In the end, I solved the problem by introducing another method, but I left this one in place anyway.

    I kept the method and renamed it as suggested, but perhaps it would be better to remove it altogether?

• Swap this overload with the next, so it is consistent with the previous block.

  • Done.

• [...] You should use BIORBD_NAMESPACE, which automatically takes the required value.

  • Done.

I will take a one-month break and continue adding tests in September.

[pariterre]

Have a good break :)

@pariterre reviewed 7 files and all commit messages, made 1 comment, and resolved 8 discussions.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on kbonnet001).