Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 15 commits into from
Jan 14, 2022
Merged

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

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
6695192
Better generalization of contact constraint generation. Now it allows…
peci1 Dec 18, 2021
86add3a
Add possibility to remove contact surface handlers.
peci1 Jun 10, 2021
b3306bc
Better generalization of contact constraint generation. Now it allows…
peci1 Jun 10, 2021
bc1d9e9
Improve comment.
peci1 Jul 13, 2021
312d485
Addressed comments from review.
peci1 Jul 31, 2021
ea4a6fe
clang-format.
peci1 Dec 18, 2021
5a7049b
Apply suggestions from code review
peci1 Jan 5, 2022
3b27247
Merge branch 'main' into contact-surface-upstream
peci1 Jan 5, 2022
6fc2ecc
Changes according to review.
peci1 Jan 5, 2022
6fab7f3
More verbose docs.
peci1 Jan 5, 2022
375745f
Added test for ConstraintSolver surface handler.
peci1 Jan 5, 2022
e55144d
Check for a possible infinite recursion and prevent it.
peci1 Jan 5, 2022
4fcf9f9
Trying to fix build on Mac.
peci1 Jan 5, 2022
c52dcc8
Update dart/constraint/ContactSurface.hpp
jslee02 Jan 14, 2022
f17accb
Update dart/constraint/ContactSurface.cpp
jslee02 Jan 14, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CONTRIBUTING.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ The code doesn't need to be perfect right away, feel free to post work-in-progre
[Christoph Hinze](https://github.com/chhinze) | python bindings
[Erwin Coumans](https://github.com/erwincoumans) | build fix on Windows/MSVC
[Silvio Traversaro](https://github.com/traversaro) | build fix on Windows/MSVC, vcpkg packaging
[Martin Pecka](https://github.com/peci1) | contact surface generalization
peci1 marked this conversation as resolved.
Show resolved Hide resolved

You can find the complete contribution history in [here](https://github.com/dartsim/dart/graphs/contributors).

Expand Down
75 changes: 58 additions & 17 deletions dart/constraint/ConstraintSolver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#include "dart/common/Console.hpp"
#include "dart/constraint/ConstrainedGroup.hpp"
#include "dart/constraint/ContactConstraint.hpp"
#include "dart/constraint/ContactSurface.hpp"
#include "dart/constraint/JointConstraint.hpp"
#include "dart/constraint/JointCoulombFrictionConstraint.hpp"
#include "dart/constraint/LCPSolver.hpp"
Expand All @@ -64,7 +65,8 @@ ConstraintSolver::ConstraintSolver(double timeStep)
mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()),
mCollisionOption(collision::CollisionOption(
true, 1000u, std::make_shared<collision::BodyNodeCollisionFilter>())),
mTimeStep(timeStep)
mTimeStep(timeStep),
mContactSurfaceHandler(std::make_shared<DefaultContactSurfaceHandler>())
{
assert(timeStep > 0.0);

Expand All @@ -83,7 +85,8 @@ ConstraintSolver::ConstraintSolver()
mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()),
mCollisionOption(collision::CollisionOption(
true, 1000u, std::make_shared<collision::BodyNodeCollisionFilter>())),
mTimeStep(0.001)
mTimeStep(0.001),
mContactSurfaceHandler(std::make_shared<DefaultContactSurfaceHandler>())
{
auto cd = std::static_pointer_cast<collision::FCLCollisionDetector>(
mCollisionDetector);
Expand Down Expand Up @@ -389,6 +392,8 @@ void ConstraintSolver::setFromOtherConstraintSolver(

addSkeletons(other.getSkeletons());
mManualConstraints = other.mManualConstraints;

mContactSurfaceHandler = other.mContactSurfaceHandler;
}

//==============================================================================
Expand Down Expand Up @@ -501,6 +506,7 @@ void ConstraintSolver::updateConstraints()
};

std::map<ContactPair, size_t, ContactPairCompare> contactPairMap;
std::vector<collision::Contact*> contacts;

// Create new contact constraints
for (auto i = 0u; i < mCollisionResult.getNumContacts(); ++i)
Expand Down Expand Up @@ -543,31 +549,23 @@ void ConstraintSolver::updateConstraints()
++contactPairMap[std::make_pair(
contact.collisionObject1, contact.collisionObject2)];

mContactConstraints.push_back(
std::make_shared<ContactConstraint>(contact, mTimeStep));
contacts.push_back(&contact);
}
}

// Add the new contact constraints to dynamic constraint list
for (const auto& contactConstraint : mContactConstraints)
for (auto* contact : contacts)
{
// update the slip compliances of the contact constraints based on the
// number of contacts between the collision objects.
auto& contact = contactConstraint->getContact();
std::size_t numContacts = 1;
auto it = contactPairMap.find(
std::make_pair(contact.collisionObject1, contact.collisionObject2));
std::make_pair(contact->collisionObject1, contact->collisionObject2));
if (it != contactPairMap.end())
numContacts = it->second;

// The slip compliance acts like a damper at each contact point so the total
// damping for each collision is multiplied by the number of contact points
// (numContacts). To eliminate this dependence on numContacts, the inverse
// damping is multiplied by numContacts.
contactConstraint->setPrimarySlipCompliance(
contactConstraint->getPrimarySlipCompliance() * numContacts);
contactConstraint->setSecondarySlipCompliance(
contactConstraint->getSecondarySlipCompliance() * numContacts);
auto contactConstraint = mContactSurfaceHandler->createConstraint(
*contact, numContacts, mTimeStep);
mContactConstraints.push_back(contactConstraint);

contactConstraint->update();

if (contactConstraint->isActive())
Expand Down Expand Up @@ -740,5 +738,48 @@ bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const
return bodyNode1IsSoft || bodyNode2IsSoft;
}

//==============================================================================
ContactSurfaceHandlerPtr ConstraintSolver::getLastContactSurfaceHandler() const
{
return mContactSurfaceHandler;
}

//==============================================================================
void ConstraintSolver::addContactSurfaceHandler(
ContactSurfaceHandlerPtr handler)
{
handler->setParent(mContactSurfaceHandler);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member

@jslee02 jslee02 Jan 14, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

mContactSurfaceHandler = std::move(handler);
}

//==============================================================================
bool ConstraintSolver::removeContactSurfaceHandler(
const ContactSurfaceHandlerPtr& handler)
{
bool found = false;
ContactSurfaceHandlerPtr current = mContactSurfaceHandler;
ContactSurfaceHandlerPtr previous = nullptr;
while (current != nullptr)
{
if (current == handler)
{
if (previous != nullptr)
previous->mParent = current->mParent;
else
mContactSurfaceHandler = current->mParent;
found = true;
break;
}
previous = current;
current = current->mParent;
}

if (mContactSurfaceHandler == nullptr)
dterr << "No contact surface handler remained. This is an error. Add at "
<< "least DefaultContactSurfaceHandler." << std::endl;

return found;
}

} // namespace constraint
} // namespace dart
21 changes: 21 additions & 0 deletions dart/constraint/ConstraintSolver.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -184,6 +184,24 @@ class ConstraintSolver
/// properties and registered skeletons and constraints will be copied over.
virtual void setFromOtherConstraintSolver(const ConstraintSolver& other);

/// Get the handler used for computing contact surface parameters based on
/// the contact properties of the two colliding bodies.
ContactSurfaceHandlerPtr getLastContactSurfaceHandler() const;

/// Set the handler used for computing contact surface parameters based on
/// the contact properties of the two colliding bodies. This function
/// automatically sets the previous handler as parent of the given handler.
void addContactSurfaceHandler(ContactSurfaceHandlerPtr handler);

/// Remove the given contact surface handler. If it is not the last in the
/// chain of handlers, the neighbor handlers are automatically connected
/// when the given handler is removed. This function returns true when the
/// given handler was found. It returns false when the handler is not found.
/// The search procedure utilizes pointer equality (i.e. the shared pointers
/// have to point to the same address to be treated equal). Take special care
/// to make sure at least one handler is always available.
bool removeContactSurfaceHandler(const ContactSurfaceHandlerPtr& handler);

protected:
// TODO(JS): Docstring
virtual void solveConstrainedGroup(ConstrainedGroup& group) = 0;
Expand Down Expand Up @@ -256,6 +274,9 @@ class ConstraintSolver

/// Constraint group list
std::vector<ConstrainedGroup> mConstrainedGroups;

/// Factory for ContactSurfaceParams for each contact
ContactSurfaceHandlerPtr mContactSurfaceHandler;
};

} // namespace constraint
Expand Down
Loading