Skip to content

Commit

Permalink
Allow customization of contact surface properties (#267)
Browse files Browse the repository at this point in the history 
Adds support for registering callbacks that modify properties of all contacts generated by the dynamics engine. Example implementation for DART is provided.

Signed-off-by: Martin Pecka <peckama2@fel.cvut.cz>
Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>
  • Loading branch information
@peci1 @azeey
peci1 and azeey committed Oct 28, 2021
1 parent f1aee5a commit 1fd83e0
Show file tree
Hide file tree
Showing 7 changed files with 713 additions and 50 deletions.
25 changes: 24 additions & 1 deletion dartsim/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

# This component expresses custom features of the dartsim plugin, which can
# expose native dartsim data types.
ign_add_component(dartsim INTERFACE
Expand Down Expand Up @@ -33,6 +32,26 @@ target_link_libraries(${dartsim_plugin}
ignition-common${IGN_COMMON_VER}::profiler
)

# The ignition fork of DART contains additional code that allows customizing
# contact constraints. We check for the presence of "ContactSurface.hpp", which
# was added to enable these customizations, to detect if the feature is
# available.
include(CheckIncludeFileCXX)
if (MSVC)
set(CMAKE_REQUIRED_FLAGS "/std:c++14")
else()
set(CMAKE_REQUIRED_FLAGS "-std=c++14")
endif()
set(CMAKE_REQUIRED_INCLUDES "${DART_INCLUDE_DIRS};${EIGEN3_INCLUDE_DIRS}")
set(CMAKE_REQUIRED_QUIET false)

CHECK_INCLUDE_FILE_CXX(dart/constraint/ContactSurface.hpp DART_HAS_CONTACT_SURFACE_HEADER)
if (DART_HAS_CONTACT_SURFACE_HEADER)
target_compile_definitions(${dartsim_plugin} PRIVATE DART_HAS_CONTACT_SURFACE)
else()
message(STATUS "The version of DART does not support Contact constraint customizations.")
endif()

# Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
install(TARGETS ${dartsim_plugin} DESTINATION ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR})

Expand Down Expand Up @@ -67,6 +86,10 @@ foreach(test ${tests})
"TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\""
"IGNITION_PHYSICS_RESOURCE_DIR=\"${IGNITION_PHYSICS_RESOURCE_DIR}\"")

if (DART_HAS_CONTACT_SURFACE_HEADER)
target_compile_definitions(${test} PRIVATE DART_HAS_CONTACT_SURFACE)
endif()

# Helps when we want to build a single test after making changes to dartsim_plugin
add_dependencies(${test} ${dartsim_plugin})
endforeach()
Expand Down
218 changes: 190 additions & 28 deletions dartsim/src/SimulationFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,17 @@
*
*/

#include <memory>
#include <string>
#include <unordered_map>

#include <dart/collision/CollisionObject.hpp>
#include <dart/collision/CollisionResult.hpp>
#include <dart/constraint/ConstraintSolver.hpp>
#include <dart/constraint/ContactConstraint.hpp>
#ifdef DART_HAS_CONTACT_SURFACE
#include <dart/constraint/ContactSurface.hpp>
#endif

#include "SimulationFeatures.hh"

Expand Down Expand Up @@ -64,41 +73,194 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const

for (const auto &dtContact : colResult.getContacts())
{
dart::collision::CollisionObject *dtCollObj1 = dtContact.collisionObject1;
dart::collision::CollisionObject *dtCollObj2 = dtContact.collisionObject2;
auto contact = this->convertContact(dtContact);
if (contact)
outContacts.push_back(contact.value());
}

return outContacts;
}

std::optional<SimulationFeatures::ContactInternal>
SimulationFeatures::convertContact(
const dart::collision::Contact& _contact) const
{
auto *dtCollObj1 = _contact.collisionObject1;
auto *dtCollObj2 = _contact.collisionObject2;

const dart::dynamics::ShapeFrame *dtShapeFrame1 =
dtCollObj1->getShapeFrame();
const dart::dynamics::ShapeFrame *dtShapeFrame2 =
dtCollObj2->getShapeFrame();
auto *dtShapeFrame1 = dtCollObj1->getShapeFrame();
auto *dtShapeFrame2 = dtCollObj2->getShapeFrame();

dart::dynamics::ConstBodyNodePtr dtBodyNode1;
dart::dynamics::ConstBodyNodePtr dtBodyNode2;
if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) &&
this->shapes.HasEntity(dtShapeFrame2->asShapeNode()))
{
std::size_t shape1ID =
this->shapes.IdentityOf(dtShapeFrame1->asShapeNode());
std::size_t shape2ID =
this->shapes.IdentityOf(dtShapeFrame2->asShapeNode());

CompositeData extraData;

// Add normal, depth and wrench to extraData.
auto& extraContactData =
extraData.Get<SimulationFeatures::ExtraContactData>();
extraContactData.force = _contact.force;
extraContactData.normal = _contact.normal;
extraContactData.depth = _contact.penetrationDepth;


return SimulationFeatures::ContactInternal {
this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)),
this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)),
_contact.point, extraData
};
}

return std::nullopt;
}

if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) &&
this->shapes.HasEntity(dtShapeFrame2->asShapeNode()))
#ifdef DART_HAS_CONTACT_SURFACE
void SimulationFeatures::AddContactPropertiesCallback(
const Identity& _worldID, const std::string& _callbackID,
SurfaceParamsCallback _callback)
{
auto *world = this->ReferenceInterface<DartWorld>(_worldID);

auto handler = std::make_shared<IgnContactSurfaceHandler>();
handler->surfaceParamsCallback = _callback;
handler->convertContact = [this](const dart::collision::Contact& _contact) {
return this->convertContact(_contact);
};

this->contactSurfaceHandlers[_callbackID] = handler;
world->getConstraintSolver()->addContactSurfaceHandler(handler);
}

bool SimulationFeatures::RemoveContactPropertiesCallback(
const Identity& _worldID, const std::string& _callbackID)
{
auto *world = this->ReferenceInterface<DartWorld>(_worldID);

if (this->contactSurfaceHandlers.find(_callbackID) !=
this->contactSurfaceHandlers.end())
{
const auto handler = this->contactSurfaceHandlers[_callbackID];
this->contactSurfaceHandlers.erase(_callbackID);
return world->getConstraintSolver()->removeContactSurfaceHandler(handler);
}
else
{
ignerr << "Could not find the contact surface handler to be removed"
<< std::endl;
return false;
}
}

dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams(
const dart::collision::Contact& _contact,
const size_t _numContactsOnCollisionObject) const
{
auto pDart = ContactSurfaceHandler::createParams(
_contact, _numContactsOnCollisionObject);

if (!this->surfaceParamsCallback)
return pDart;

typedef SetContactPropertiesCallbackFeature F;
typedef FeaturePolicy3d P;
typename F::ContactSurfaceParams<P> pIgn;

pIgn.frictionCoeff = pDart.mFrictionCoeff;
pIgn.secondaryFrictionCoeff = pDart.mSecondaryFrictionCoeff;
pIgn.slipCompliance = pDart.mSlipCompliance;
pIgn.secondarySlipCompliance = pDart.mSecondarySlipCompliance;
pIgn.restitutionCoeff = pDart.mRestitutionCoeff;
pIgn.firstFrictionalDirection = pDart.mFirstFrictionalDirection;
pIgn.contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity;

auto contactInternal = this->convertContact(_contact);
if (contactInternal)
{
this->surfaceParamsCallback(contactInternal.value(),
_numContactsOnCollisionObject, pIgn);

if (pIgn.frictionCoeff)
pDart.mFrictionCoeff = pIgn.frictionCoeff.value();
if (pIgn.secondaryFrictionCoeff)
pDart.mSecondaryFrictionCoeff = pIgn.secondaryFrictionCoeff.value();
if (pIgn.slipCompliance)
pDart.mSlipCompliance = pIgn.slipCompliance.value();
if (pIgn.secondarySlipCompliance)
pDart.mSecondarySlipCompliance = pIgn.secondarySlipCompliance.value();
if (pIgn.restitutionCoeff)
pDart.mRestitutionCoeff = pIgn.restitutionCoeff.value();
if (pIgn.firstFrictionalDirection)
pDart.mFirstFrictionalDirection = pIgn.firstFrictionalDirection.value();
if (pIgn.contactSurfaceMotionVelocity)
pDart.mContactSurfaceMotionVelocity =
pIgn.contactSurfaceMotionVelocity.value();

static bool warnedRollingFrictionCoeff = false;
if (!warnedRollingFrictionCoeff && pIgn.rollingFrictionCoeff)
{
ignwarn << "DART doesn't support rolling friction setting" << std::endl;
warnedRollingFrictionCoeff = true;
}

static bool warnedSecondaryRollingFrictionCoeff = false;
if (!warnedSecondaryRollingFrictionCoeff &&
pIgn.secondaryRollingFrictionCoeff)
{
ignwarn << "DART doesn't support secondary rolling friction setting"
<< std::endl;
warnedSecondaryRollingFrictionCoeff = true;
}

static bool warnedTorsionalFrictionCoeff = false;
if (!warnedTorsionalFrictionCoeff && pIgn.torsionalFrictionCoeff)
{
std::size_t shape1ID =
this->shapes.IdentityOf(dtShapeFrame1->asShapeNode());
std::size_t shape2ID =
this->shapes.IdentityOf(dtShapeFrame2->asShapeNode());

CompositeData extraData;

// Add normal, depth and wrench to extraData.
auto &extraContactData = extraData.Get<ExtraContactData>();
extraContactData.force = dtContact.force;
extraContactData.normal = dtContact.normal;
extraContactData.depth = dtContact.penetrationDepth;

outContacts.push_back(
{this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)),
this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)),
dtContact.point, extraData});
ignwarn << "DART doesn't support torsional friction setting"
<< std::endl;
warnedTorsionalFrictionCoeff = true;
}
}
return outContacts;

this->lastIgnParams = pIgn;

return pDart;
}

dart::constraint::ContactConstraintPtr
IgnContactSurfaceHandler::createConstraint(
dart::collision::Contact& _contact,
const size_t _numContactsOnCollisionObject,
const double _timeStep) const
{
// this call sets this->lastIgnParams
auto constraint = dart::constraint::ContactSurfaceHandler::createConstraint(
_contact, _numContactsOnCollisionObject, _timeStep);

typedef SetContactPropertiesCallbackFeature F;
typedef FeaturePolicy3d P;
typename F::ContactSurfaceParams<P>& p = this->lastIgnParams;

if (this->lastIgnParams.errorReductionParameter)
constraint->setErrorReductionParameter(p.errorReductionParameter.value());

if (this->lastIgnParams.maxErrorAllowance)
constraint->setErrorAllowance(p.maxErrorAllowance.value());

if (this->lastIgnParams.maxErrorReductionVelocity)
constraint->setMaxErrorReductionVelocity(
p.maxErrorReductionVelocity.value());

if (this->lastIgnParams.constraintForceMixing)
constraint->setConstraintForceMixing(p.constraintForceMixing.value());

return constraint;
}
#endif

}
}
}
71 changes: 71 additions & 0 deletions dartsim/src/SimulationFeatures.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,25 +18,80 @@
#ifndef IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_
#define IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_

#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include <dart/config.hpp>
#ifdef DART_HAS_CONTACT_SURFACE
#include <dart/constraint/ContactSurface.hpp>
#endif

#include <ignition/physics/ForwardStep.hh>
#include <ignition/physics/GetContacts.hh>
#include <ignition/physics/ContactProperties.hh>

#include "Base.hh"

namespace dart
{
namespace collision
{
class Contact;
}
}

namespace ignition {
namespace physics {
namespace dartsim {

struct SimulationFeatureList : FeatureList<
ForwardStep,
#ifdef DART_HAS_CONTACT_SURFACE
SetContactPropertiesCallbackFeature,
#endif
GetContactsFromLastStepFeature
> { };

#ifdef DART_HAS_CONTACT_SURFACE
class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler
{
public: dart::constraint::ContactSurfaceParams createParams(
const dart::collision::Contact& _contact,
size_t _numContactsOnCollisionObject) const override;

public: dart::constraint::ContactConstraintPtr createConstraint(
dart::collision::Contact& _contact,
size_t _numContactsOnCollisionObject,
double _timeStep) const override;

public: typedef SetContactPropertiesCallbackFeature Feature;
public: typedef Feature::Implementation<FeaturePolicy3d> Impl;

public: Impl::SurfaceParamsCallback surfaceParamsCallback;

public: std::function<
std::optional<Impl::ContactImpl>(const dart::collision::Contact&)
> convertContact;

public: mutable typename Feature::ContactSurfaceParams<FeaturePolicy3d>
lastIgnParams;
};

using IgnContactSurfaceHandlerPtr = std::shared_ptr<IgnContactSurfaceHandler>;
#endif

class SimulationFeatures :
public virtual Base,
public virtual Implements3d<SimulationFeatureList>
{
public: using GetContactsFromLastStepFeature::Implementation<FeaturePolicy3d>
::ContactInternal;

public: SimulationFeatures() = default;
public: ~SimulationFeatures() override = default;

public: void WorldForwardStep(
const Identity &_worldID,
ForwardStep::Output &_h,
Expand All @@ -45,6 +100,22 @@ class SimulationFeatures :

public: std::vector<ContactInternal> GetContactsFromLastStep(
const Identity &_worldID) const override;

private: std::optional<ContactInternal> convertContact(
const dart::collision::Contact& _contact) const;

#ifdef DART_HAS_CONTACT_SURFACE
public: void AddContactPropertiesCallback(
const Identity &_worldID,
const std::string &_callbackID,
SurfaceParamsCallback _callback) override;

public: bool RemoveContactPropertiesCallback(
const Identity &_worldID, const std::string &_callbackID) override;

private: std::unordered_map<
std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers;
#endif
};

}
Expand Down
Loading

0 comments on commit 1fd83e0

Please sign in to comment.