Generalize the way properties of a contact constraints are computed, allow for custom implementations

[peci1]

Before creating a pull request

• Document new methods and classes
• Format new code files using clang-format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change

Upstreaming gazebo-forks#22 . Here's the copied PR description:

---

This PR adds the possibility to adjust properties of contact constraints (joints) before they are added to the simulation loop.

It resolves a TODO from ContactConstraint.cpp that was reading:

TODO(JS): Consider providing various ways of the combined friction or allowing to override this method by a custom method

The contact surface structure was inspired by the existing contact constraint class members and by http://ode.org/wiki/index.php?title=Manual#Contact .

This PR also adds support for specifying friction-unrelated contact velocity (dContactMotion1 etc. in ODE words). This is needed for simulation of tracks or conveyor belts. It basically specifies that the velocity LCP tries to achieve should not be zero-up-to-friction-coefficients, but the specified velocity (very similar to how bouncing works). This velocity target can be set using the custom contact surface handler this PR adds.

Adding the custom contact surface handler also allowed to separate the odd-placed force-dependent slip settings from ConstraintSolver to the handler class, which looks to me like a desired separation of concerns.

PR gazebo-forks#22 in the Ignition fork of DART allowed adding more proper support for tracked vehicles to Ignition Gazebo with DART: gazebosim/gz-sim#869 .

I did not take much care about keeping ABI compatibility. API compatibility should remain untouched regarding public members of classes. There are some new classes and I deprecated a few static protected members of ContactConstraint and moved their implementation to DefaultContactSurfaceHandler. I also moved a few .cpp-only constants from ContactConstraint.cpp to ContactSurface.hpp (making them publicly available). If ABI compatibility is wanted, the commits are prepared in gazebo-forks#22 .

The ContactSurfaceParams class has a future-proof void* member that should allow adding more contact surface properties without breaking ABI. That should be good for downstream applications using DART. However, I'm not 100% sure whether raw pointer is the correct approach (it is null for now). Maybe a unique_ptr should be there together with custom copy and move constructors.

All existing tests passed on my computer. I can add a few more regarding the addition/removal of contact handlers.

The example API usage is like this:

class tmp : public dart::constraint::ContactSurfaceHandler
{
public:

  dart::constraint::ContactSurfaceParams createParams(const dart::collision::Contact& contact, const size_t numContactsOnCollisionObject) const override
  {
     // this automatically calls the default handler containing the original DART code handling contacts
    auto params = ContactSurfaceHandler::createParams(contact, numContactsOnCollisionObject);
    params.mFirstFrictionalDirection = Eigen::Vector3d::UnitY();
    params.mContactSurfaceMotionVelocity = Eigen::Vector3d::UnitY();
    return params;
  }
};

auto tmpPtr = std::make_shared<tmp>();

...

world->getConstraintSolver()->addContactSurfaceHandler(tmpPtr);

contactMotion.mp4

[peci1]

This PR also does not add the generalized approach to soft contacts. I've never worked with them, so I don't feel too confident editing around. But their API seems similar in many parts, so maybe it'd make sense to include them, too.

[jslee02]

Could you let me know what's the motivation for having the handlers in a chain structure? It seems there is no advantage of using the chain structure because ConstraintSolver uses the lastly "added" handler anyway. Why not simply set handler (instead of adding)?

If the user wants to use a sophisticated handler that utilizes multiple handlers (e.g., to use different handlers by BodyNode), they could create a composite handler like dart::utils::CompositeResourceRetriever.

[peci1]

The motivation was mainly to ease creating "small" contact handlers that just do one or two things more than the default one. It could alternatively be solved by subclassing the default handler, but the chain-like structure allows to not care what handler is already set. This should theoretically allow mutliple plugins add multiple handlers without conflicts (as long as there is no logical conflict in their tasks).

If the goal is to create a very customized handler and disable the default one, the custom handler will just not call ContactSurfaceHandler::createParams() in the override, which will effectively terminate the chain.

[jslee02]

Do you think the chain-like structure would be more frequently used than the other case? If not, I would prefer subclassing the default handler because addContactSurfaceHandler() could confuse the user without understanding of how this function "add"s handler (i.e., adding back to the existing handler) and how the chain works.

If the user wants to have the same effect of the current chain-like structure, they could achieve it through subclass, as you mentioned, or creating an wrapper class that works exactly the same as the current ContactSurfaceHandler something like:

class ContactSurfaceHandler {
public:
  ContactSurfaceHandler() = default;  // no parent parameter
  virtual ContactSurfaceParams createParams(
      const collision::Contact& contact,
      size_t numContactsOnCollisionObject) const = 0;  // pure virtual
};

class DefaultContactSurfaceHandler : public ContactSurfaceHandler {
public:
  DefaultContactSurfaceHandler() = default;
  ContactSurfaceParams DefaultContactSurfaceHandler::createParams(
      const collision::Contact& contact,
      size_t numContactsOnCollisionObject) const override {
    // ...
  }
};

class ChainContactSurfaceHandler : public ContactSurfaceHandler {
public:
  ChainContactSurfaceHandler(ContactSurfaceHandlerPtr parent = nullptr);
  ContactSurfaceParams createParams(
      const collision::Contact& contact,
      size_t numContactsOnCollisionObject) const override {
    if (mParent != nullptr)
      return mParent->createParams(contact, numContactsOnCollisionObject);
    return {};
  }
protected:
  ContactSurfaceHandlerPtr mParent;
};

class CustomHandler : public ChainContactSurfaceHandler {
public:
  ChainContactSurfaceHandler(ContactSurfaceHandlerPtr parent = nullptr)
    : ChainContactSurfaceHandler(std::move(parent)) {}
  ContactSurfaceParams createParams(
      const collision::Contact& contact,
      size_t numContactsOnCollisionObject) const override {
    // Utilize mParent as needed
  }
};

// If you need chain-like structure:
auto defaultHandler = std::make_shared<DefaultContactSurfaceHandler>()
auto customHandler = std::make_shared<CustomHandler>(defaultHandler);
constraintSolver->setContactSurfaceHandler(customHandler);

It could alternatively be solved by subclassing the default handler, but the chain-like structure allows to not care what handler is already set. This should theoretically allow mutliple plugins add multiple handlers without conflicts (as long as there is no logical conflict in their tasks).

I'm afraid that I'm not quite following this statement. To me, the chain-like structure should care what the handler is already set to be able to decide whether to utilize or/and how to utilize. Let me know if I'm missing something here.

[peci1]

Ok. Imagine I'm writing a 3rd party plugin that wants to add a handler. Now I don't know anything about which other plugins set what other handlers, so I can't be sure subclassing DefaultContactSurfaceHandler is the right thing to do. That would mean almost all plugin developers would need to come with the chaining handlers to be sure they do not break other plugins (unless they really want to get rid of whatever is there).

Providing ChainContactSurfaceHandler as you suggested would help that and give developers an easy-to-use base class. However, I'm not sure how would removing such handler work (if other handlers are hooked to it).

That is why I think the chaining logic should be implemented in DART.

addContactSurfaceHandler() could confuse the user without understanding of how this function "add"s handler (i.e., adding back to the existing handler) and how the chain works.

I was hoping the method names and the attached documentation are good enough to explain how the chain works. If we agree on keeping the chaining logic, I could try to improve it.

[peci1]

I assume that a large part of 3rd party handlers could be just "observers" or "single-value-changers". This is why I think the default behavior should be passing the work to the previous handler and why I want such simple handlers to be written as simple as possible.

[jslee02]

Alright, now I see your points. Thanks for adding the tests, which are also helpful understanding the motivation!

[jslee02]

Thanks for adding this feature!

I did not take much care about keeping ABI compatibility.

ABI compatibility is nice-to-have, but it's not required for major version updates for DART at the moment. It's currently not easy to keep it when adding new features unless DART adopts the PIMPL idiom, which is not planned. So I think it's okay.

The ContactSurfaceParams class has a future-proof void* member that should allow adding more contact surface properties without breaking ABI. That should be good for downstream applications using DART. However, I'm not 100% sure whether raw pointer is the correct approach (it is null for now). Maybe a unique_ptr should be there together with custom copy and move constructors.

void* seems the most generic way to me. Maybe we could add a note that it's the user's responsibility to manage the life of the external data.

I can add a few more regarding the addition/removal of contact handlers.

Yeah, more unit tests would be appreciated!

This PR also does not add the generalized approach to soft contacts. I've never worked with them, so I don't feel too confident editing around. But their API seems similar in many parts, so maybe it'd make sense to include them, too.

The soft body is not actively maintained. Let's save it for future changes (#1631).

[peci1]

Thank you for the thorough and prompt review and comments. I've commited most of your suggestions.

[peci1]

void* seems the most generic way to me. Maybe we could add a note that it's the user's responsibility to manage the life of the external data.

I extended mExtraData docs and added a remark that once this starts to be used, rule-of-five has to be obeyed and the memory management will be done automatically. Putting the burden of memory management on downstream users would be not only inconvenient, but maybe impossible (the pointer is private and points to memory of unknown size) and would not achieve the future-proofness because of which it exists in the first place (compile-time would be okay, but during runtime there would be instances that would need manual copying of the data, but the downstream app would not know about that).

[peci1]

Yeah, more unit tests would be appreciated!

I added a test for adding/removing the contact handlers. Maybe it will help you assess the usefulness of the chaining approach.

[jslee02]

Alright, now I see your points. Thanks for adding the tests, which are also helpful understanding the motivation!

[peci1]

Thanks!