Skip to content

Commit

Permalink
Merge branch 'ign-physics2' into backport_260
Browse files Browse the repository at this point in the history
  • Loading branch information
@azeey
azeey authored Oct 28, 2021
2 parents 998fa5d + 1fd83e0 commit d8035e7
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 d8035e7

Please sign in to comment.