From aebcac17548a7bb8125a98b5924cbeb33016f652 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 31 May 2021 16:55:12 +0200 Subject: [PATCH 01/16] Added implementation of contact surface velocity. Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.cc | 247 ++++++++++++++++-- dartsim/src/SimulationFeatures.hh | 33 ++- .../physics/ContactJointProperties.hh | 174 ++++++++++++ .../physics/detail/ContactJointProperties.hh | 100 +++++++ 4 files changed, 525 insertions(+), 29 deletions(-) create mode 100644 include/ignition/physics/ContactJointProperties.hh create mode 100644 include/ignition/physics/detail/ContactJointProperties.hh diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 27f135212..72ccf8e44 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -17,6 +17,9 @@ #include #include +#include +#include +#include #include "SimulationFeatures.hh" @@ -27,6 +30,53 @@ namespace ignition { namespace physics { namespace dartsim { +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 SetContactJointPropertiesCallbackFeature Feature; + public: typedef Feature::Implementation Impl; + + public: Impl::SurfaceParamsCallback surfaceParamsCallback; + + public: std::function< + std::optional(const dart::collision::Contact&) + > convertContact; + + public: mutable typename Feature::ContactSurfaceParams + lastIgnParams; +}; +using IgnContactSurfaceHandlerPtr = std::shared_ptr; + +class SimulationFeaturesPrivate +{ + public: explicit SimulationFeaturesPrivate(SimulationFeatures* _main); + + public: std::optional convertContact( + const dart::collision::Contact& _contact) const; + + public: std::unordered_map< + std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers; + + private: SimulationFeatures* main; +}; + +SimulationFeatures::SimulationFeatures() + : dataPtr(std::make_unique(this)) +{ +} + +SimulationFeatures::~SimulationFeatures() +{ +} + void SimulationFeatures::WorldForwardStep( const Identity &_worldID, ForwardStep::Output & /*_h*/, @@ -64,41 +114,182 @@ 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->dataPtr->convertContact(dtContact); + if (contact) + outContacts.push_back(contact.value()); + } + + return outContacts; +} + +void SimulationFeatures::AddContactJointPropertiesCallback( + const Identity& _worldID, const std::string& _callbackID, + SurfaceParamsCallback _callback) +{ + auto *world = this->ReferenceInterface(_worldID); + + auto handler = std::make_shared(); + handler->surfaceParamsCallback = _callback; + handler->convertContact = [this](const dart::collision::Contact& _contact) { + return this->dataPtr->convertContact(_contact); + }; + + this->dataPtr->contactSurfaceHandlers[_callbackID] = handler; + world->getConstraintSolver()->setContactSurfaceHandler(handler); +} + +bool SimulationFeatures::RemoveContactJointPropertiesCallback( + const Identity& _worldID, const std::string& _callbackID) +{ + auto *world = this->ReferenceInterface(_worldID); + + if (this->dataPtr->contactSurfaceHandlers.find(_callbackID) != + this->dataPtr->contactSurfaceHandlers.end()) + { + const auto handler = this->dataPtr->contactSurfaceHandlers[_callbackID]; + this->dataPtr->contactSurfaceHandlers.erase(_callbackID); + return world->getConstraintSolver()->removeContactSurfaceHandler(handler); + } + else + { + ignerr << "Could not find the contact surface handler to be removed" + << std::endl; + return false; + } +} + +std::optional +SimulationFeaturesPrivate::convertContact( + const dart::collision::Contact& _contact) const +{ + auto *dtCollObj1 = _contact.collisionObject1; + auto *dtCollObj2 = _contact.collisionObject2; + + auto *dtShapeFrame1 = dtCollObj1->getShapeFrame(); + auto *dtShapeFrame2 = dtCollObj2->getShapeFrame(); + + if (this->main->shapes.HasEntity(dtShapeFrame1->asShapeNode()) && + this->main->shapes.HasEntity(dtShapeFrame2->asShapeNode())) + { + std::size_t shape1ID = + this->main->shapes.IdentityOf(dtShapeFrame1->asShapeNode()); + std::size_t shape2ID = + this->main->shapes.IdentityOf(dtShapeFrame2->asShapeNode()); + + CompositeData extraData; + + // Add normal, depth and wrench to extraData. + auto& extraContactData = + extraData.Get(); + extraContactData.force = _contact.force; + extraContactData.normal = _contact.normal; + extraContactData.depth = _contact.penetrationDepth; - const dart::dynamics::ShapeFrame *dtShapeFrame1 = - dtCollObj1->getShapeFrame(); - const dart::dynamics::ShapeFrame *dtShapeFrame2 = - dtCollObj2->getShapeFrame(); - dart::dynamics::ConstBodyNodePtr dtBodyNode1; - dart::dynamics::ConstBodyNodePtr dtBodyNode2; + return SimulationFeatures::ContactInternal { + this->main->GenerateIdentity(shape1ID, this->main->shapes.at(shape1ID)), + this->main->GenerateIdentity(shape2ID, this->main->shapes.at(shape2ID)), + _contact.point, extraData + }; + } + + return std::nullopt; +} + +SimulationFeaturesPrivate::SimulationFeaturesPrivate(SimulationFeatures* _main) + : main(_main) +{ +} + +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; - if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) && - this->shapes.HasEntity(dtShapeFrame2->asShapeNode())) + typedef SetContactJointPropertiesCallbackFeature F; + typedef FeaturePolicy3d P; + typename F::ContactSurfaceParams

pIgn; + + pIgn.Get>().frictionCoeff = pDart.mFrictionCoeff; + pIgn.Get>().secondaryFrictionCoeff = + pDart.mSecondaryFrictionCoeff; + pIgn.Get>().slipCompliance = pDart.mSlipCompliance; + pIgn.Get>().secondarySlipCompliance = + pDart.mSecondarySlipCompliance; + pIgn.Get>().restitutionCoeff = + pDart.mRestitutionCoeff; + pIgn.Get>().firstFrictionalDirection = + pDart.mFirstFrictionalDirection; + pIgn.Get>().contactSurfaceMotionVelocity = + pDart.mContactSurfaceMotionVelocity; + + if (this->surfaceParamsCallback) + { + auto contactInternal = this->convertContact(_contact); + if (contactInternal) { - 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.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}); + this->surfaceParamsCallback(contactInternal.value(), + _numContactsOnCollisionObject, pIgn); + + pDart.mFrictionCoeff = pIgn.Get>().frictionCoeff; + pDart.mSecondaryFrictionCoeff = + pIgn.Get>().secondaryFrictionCoeff; + pDart.mSlipCompliance = pIgn.Get>().slipCompliance; + pDart.mSecondarySlipCompliance = + pIgn.Get>().secondarySlipCompliance; + pDart.mRestitutionCoeff = + pIgn.Get>().restitutionCoeff; + pDart.mFirstFrictionalDirection = + pIgn.Get>().firstFrictionalDirection; + pDart.mContactSurfaceMotionVelocity = + pIgn.Get>(). + contactSurfaceMotionVelocity; } } - 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 SetContactJointPropertiesCallbackFeature F; + typedef FeaturePolicy3d P; + typename F::ContactSurfaceParams

& p = this->lastIgnParams; + + if (this->lastIgnParams.Has>()) + constraint->setErrorReductionParameter( + p.Get>().errorReductionParameter); + + if (this->lastIgnParams.Has>()) + constraint->setErrorAllowance( + p.Get>().maxErrorAllowance); + + if (this->lastIgnParams.Has>()) + constraint->setMaxErrorReductionVelocity( + p.Get>().maxErrorReductionVelocity); + + if (this->lastIgnParams.Has>()) + constraint->setConstraintForceMixing( + p.Get>().constraintForceMixing); + + return constraint; +} + } } } diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index 0ceb2cbe3..79ebd46e5 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -21,22 +21,40 @@ #include #include #include +#include #include "Base.hh" +namespace dart +{ +namespace collision +{ +class Contact; +} +} + namespace ignition { namespace physics { namespace dartsim { struct SimulationFeatureList : FeatureList< ForwardStep, - GetContactsFromLastStepFeature + GetContactsFromLastStepFeature, + SetContactJointPropertiesCallbackFeature > { }; +class SimulationFeaturesPrivate; + class SimulationFeatures : public virtual Base, public virtual Implements3d { + public: using GetContactsFromLastStepFeature::Implementation + ::ContactInternal; + + public: SimulationFeatures(); + public: ~SimulationFeatures() override; + public: void WorldForwardStep( const Identity &_worldID, ForwardStep::Output &_h, @@ -45,8 +63,21 @@ class SimulationFeatures : public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; + + public: void AddContactJointPropertiesCallback( + const Identity &_worldID, + const std::string &_callbackID, + SurfaceParamsCallback _callback) override; + + public: bool RemoveContactJointPropertiesCallback( + const Identity &_worldID, const std::string &_callbackID) override; + + private: std::unique_ptr dataPtr; + private: friend class SimulationFeaturesPrivate; }; + + } } } diff --git a/include/ignition/physics/ContactJointProperties.hh b/include/ignition/physics/ContactJointProperties.hh new file mode 100644 index 000000000..7d9f8c715 --- /dev/null +++ b/include/ignition/physics/ContactJointProperties.hh @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_PHYSICS_CONTACTJOINTPROPERTIES_HH_ +#define IGNITION_PHYSICS_CONTACTJOINTPROPERTIES_HH_ + +#include +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace physics +{ +/// \brief SetContactJointPropertiesCallbackFeature is a feature for setting the +/// properties of a contact joint after it is created but before it affects the +/// forward step. +class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature + : public virtual FeatureWithRequirements +{ + public: template struct FrictionCoeff + { + typename PolicyT::Scalar frictionCoeff; + }; + + public: template struct SecondaryFrictionCoeff + { + typename PolicyT::Scalar secondaryFrictionCoeff; + }; + + public: template struct RollingFrictionCoeff + { + typename PolicyT::Scalar rollingFrictionCoeff; + }; + + public: template struct SecondaryRollingFrictionCoeff + { + typename PolicyT::Scalar secondaryRollingFrictionCoeff; + }; + + public: template struct NormalRollingFrictionCoeff + { + typename PolicyT::Scalar normalRollingFrictionCoeff; + }; + + public: template struct SlipCompliance + { + typename PolicyT::Scalar slipCompliance; + }; + + public: template struct SecondarySlipCompliance + { + typename PolicyT::Scalar secondarySlipCompliance; + }; + + public: template struct RestitutionCoeff + { + typename PolicyT::Scalar restitutionCoeff; + }; + + public: template struct FirstFrictionalDirection + { + typename FromPolicy::template Use firstFrictionalDirection; + }; + + public: template struct ContactSurfaceMotionVelocity + { + typename FromPolicy::template Use + contactSurfaceMotionVelocity; + }; + + public: template struct ErrorReductionParameter + { + typename PolicyT::Scalar errorReductionParameter; + }; + + public: template struct MaxErrorReductionVelocity + { + typename PolicyT::Scalar maxErrorReductionVelocity; + }; + + public: template struct MaxErrorAllowance + { + typename PolicyT::Scalar maxErrorAllowance; + }; + + public: template struct ConstraintForceMixing + { + typename PolicyT::Scalar constraintForceMixing; + }; + + public: template using ContactSurfaceParams = + SpecifyData, SecondaryFrictionCoeff, + RollingFrictionCoeff, SecondaryRollingFrictionCoeff, + NormalRollingFrictionCoeff, + SlipCompliance, SecondarySlipCompliance, + RestitutionCoeff, + FirstFrictionalDirection, + ContactSurfaceMotionVelocity, + ErrorReductionParameter, + MaxErrorReductionVelocity, + MaxErrorAllowance, + ConstraintForceMixing > >; + + public: template + class World : public virtual Feature::World + { + public: using JointPtrType = JointPtr; + + public: using ShapePtrType = typename GetContactsFromLastStepFeature + ::World::ShapePtrType; + + public: using Contact = typename GetContactsFromLastStepFeature + ::World::Contact; + + public: typedef std::function< + void(const Contact&, size_t, ContactSurfaceParams&) + > SurfaceParamsCallback; + + /// \brief Add the callback. + public: void AddContactJointPropertiesCallback( + const std::string &_callbackID, SurfaceParamsCallback _callback); + + /// \brief Remove the callback. + public: bool RemoveContactJointPropertiesCallback( + const std::string &_callbackID); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: using ContactImpl = typename GetContactsFromLastStepFeature + ::Implementation::ContactInternal; + + public: typedef std::function< + void(const ContactImpl&, size_t, ContactSurfaceParams&) + > SurfaceParamsCallback; + + /// \brief Add the callback. + public: virtual void AddContactJointPropertiesCallback( + const Identity &_worldID, + const std::string &_callbackID, + SurfaceParamsCallback _callback) = 0; + + /// \brief Remove the callback. + public: virtual bool RemoveContactJointPropertiesCallback( + const Identity &_worldID, const std::string &_callbackID) = 0; + }; +}; + +} +} + +#include "ignition/physics/detail/ContactJointProperties.hh" + +#endif /* end of include guard: IGNITION_PHYSICS_CONTACTJOINTPROPERTIES_HH_ */ diff --git a/include/ignition/physics/detail/ContactJointProperties.hh b/include/ignition/physics/detail/ContactJointProperties.hh new file mode 100644 index 000000000..44590fc00 --- /dev/null +++ b/include/ignition/physics/detail/ContactJointProperties.hh @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_PHYSICS_DETAIL_CONTACTJOINTPROPERTIES_HH_ +#define IGNITION_PHYSICS_DETAIL_CONTACTJOINTPROPERTIES_HH_ + +#include +#include +#include + +namespace ignition +{ +namespace physics +{ + +///////////////////////////////////////////////// +template +void SetContactJointPropertiesCallbackFeature::World:: + AddContactJointPropertiesCallback( + const std::string &_callbackID, + SurfaceParamsCallback _callback) +{ + using Impl = Implementation; + using ContactInternal = typename Impl::ContactImpl; + + auto pimpl = this->pimpl; + auto convertContact = [pimpl](const ContactInternal& _internal) -> Contact + { + using ContactPoint = + typename GetContactsFromLastStepFeature::World + ::ContactPoint; + + ContactPoint contactPoint { + ShapePtrType(pimpl, _internal.collision1), + ShapePtrType(pimpl, _internal.collision2), + _internal.point + }; + + Contact contactOutput; + contactOutput.template Get() = std::move(contactPoint); + + using ExtraContactData = + GetContactsFromLastStepFeature::ExtraContactDataT; + + auto *extraContactData = + _internal.extraData.template Query(); + + if (extraContactData) + { + contactOutput.template Get() = + std::move(*extraContactData); + } + + return contactOutput; + }; + + typename Impl::SurfaceParamsCallback callbackInternal = nullptr; + if (_callback) + { + callbackInternal = [_callback, convertContact]( + const typename Impl::ContactImpl &_contact, + const size_t _numContactsOnCollision, + ContactSurfaceParams &_params) + { + _callback(convertContact(_contact), _numContactsOnCollision, _params); + }; + } + + this->template Interface() + ->AddContactJointPropertiesCallback( + this->identity, _callbackID, callbackInternal); +} + +///////////////////////////////////////////////// +template +bool SetContactJointPropertiesCallbackFeature::World:: + RemoveContactJointPropertiesCallback(const std::string& _callbackID) +{ + return this->template Interface()-> + RemoveContactJointPropertiesCallback(this->identity, _callbackID); +} + +} // namespace physics +} // namespace ignition + +#endif From dd7ac99c71dbe2119d8e69116f5bde7483e159ce Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 21 Jun 2021 14:27:06 +0200 Subject: [PATCH 02/16] Fixed codecheck issues. Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.cc | 4 ++++ dartsim/src/SimulationFeatures.hh | 3 +++ include/ignition/physics/ContactJointProperties.hh | 2 ++ include/ignition/physics/detail/ContactJointProperties.hh | 2 ++ 4 files changed, 11 insertions(+) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 72ccf8e44..004229a9c 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -15,6 +15,10 @@ * */ +#include +#include +#include + #include #include #include diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index 79ebd46e5..63c7b23fe 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -18,7 +18,10 @@ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_ +#include +#include #include + #include #include #include diff --git a/include/ignition/physics/ContactJointProperties.hh b/include/ignition/physics/ContactJointProperties.hh index 7d9f8c715..b8f0c70a7 100644 --- a/include/ignition/physics/ContactJointProperties.hh +++ b/include/ignition/physics/ContactJointProperties.hh @@ -18,7 +18,9 @@ #ifndef IGNITION_PHYSICS_CONTACTJOINTPROPERTIES_HH_ #define IGNITION_PHYSICS_CONTACTJOINTPROPERTIES_HH_ +#include #include + #include #include #include diff --git a/include/ignition/physics/detail/ContactJointProperties.hh b/include/ignition/physics/detail/ContactJointProperties.hh index 44590fc00..d21501ad5 100644 --- a/include/ignition/physics/detail/ContactJointProperties.hh +++ b/include/ignition/physics/detail/ContactJointProperties.hh @@ -18,8 +18,10 @@ #ifndef IGNITION_PHYSICS_DETAIL_CONTACTJOINTPROPERTIES_HH_ #define IGNITION_PHYSICS_DETAIL_CONTACTJOINTPROPERTIES_HH_ +#include #include #include + #include namespace ignition From d43eed2a3b2f94783a8fef5f41dfbf0f5e90957f Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 13 Jul 2021 12:58:29 +0200 Subject: [PATCH 03/16] Fixed issues from review. Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.cc | 119 +++++------------- dartsim/src/SimulationFeatures.hh | 38 +++++- .../physics/ContactJointProperties.hh | 15 ++- .../physics/detail/ContactJointProperties.hh | 2 +- 4 files changed, 80 insertions(+), 94 deletions(-) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 004229a9c..c21ea6eb3 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -34,53 +34,6 @@ namespace ignition { namespace physics { namespace dartsim { -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 SetContactJointPropertiesCallbackFeature Feature; - public: typedef Feature::Implementation Impl; - - public: Impl::SurfaceParamsCallback surfaceParamsCallback; - - public: std::function< - std::optional(const dart::collision::Contact&) - > convertContact; - - public: mutable typename Feature::ContactSurfaceParams - lastIgnParams; -}; -using IgnContactSurfaceHandlerPtr = std::shared_ptr; - -class SimulationFeaturesPrivate -{ - public: explicit SimulationFeaturesPrivate(SimulationFeatures* _main); - - public: std::optional convertContact( - const dart::collision::Contact& _contact) const; - - public: std::unordered_map< - std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers; - - private: SimulationFeatures* main; -}; - -SimulationFeatures::SimulationFeatures() - : dataPtr(std::make_unique(this)) -{ -} - -SimulationFeatures::~SimulationFeatures() -{ -} - void SimulationFeatures::WorldForwardStep( const Identity &_worldID, ForwardStep::Output & /*_h*/, @@ -118,7 +71,7 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const for (const auto &dtContact : colResult.getContacts()) { - auto contact = this->dataPtr->convertContact(dtContact); + auto contact = this->convertContact(dtContact); if (contact) outContacts.push_back(contact.value()); } @@ -135,10 +88,10 @@ void SimulationFeatures::AddContactJointPropertiesCallback( auto handler = std::make_shared(); handler->surfaceParamsCallback = _callback; handler->convertContact = [this](const dart::collision::Contact& _contact) { - return this->dataPtr->convertContact(_contact); + return this->convertContact(_contact); }; - this->dataPtr->contactSurfaceHandlers[_callbackID] = handler; + this->contactSurfaceHandlers[_callbackID] = handler; world->getConstraintSolver()->setContactSurfaceHandler(handler); } @@ -147,11 +100,11 @@ bool SimulationFeatures::RemoveContactJointPropertiesCallback( { auto *world = this->ReferenceInterface(_worldID); - if (this->dataPtr->contactSurfaceHandlers.find(_callbackID) != - this->dataPtr->contactSurfaceHandlers.end()) + if (this->contactSurfaceHandlers.find(_callbackID) != + this->contactSurfaceHandlers.end()) { - const auto handler = this->dataPtr->contactSurfaceHandlers[_callbackID]; - this->dataPtr->contactSurfaceHandlers.erase(_callbackID); + const auto handler = this->contactSurfaceHandlers[_callbackID]; + this->contactSurfaceHandlers.erase(_callbackID); return world->getConstraintSolver()->removeContactSurfaceHandler(handler); } else @@ -163,7 +116,7 @@ bool SimulationFeatures::RemoveContactJointPropertiesCallback( } std::optional -SimulationFeaturesPrivate::convertContact( +SimulationFeatures::convertContact( const dart::collision::Contact& _contact) const { auto *dtCollObj1 = _contact.collisionObject1; @@ -172,13 +125,13 @@ SimulationFeaturesPrivate::convertContact( auto *dtShapeFrame1 = dtCollObj1->getShapeFrame(); auto *dtShapeFrame2 = dtCollObj2->getShapeFrame(); - if (this->main->shapes.HasEntity(dtShapeFrame1->asShapeNode()) && - this->main->shapes.HasEntity(dtShapeFrame2->asShapeNode())) + if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) && + this->shapes.HasEntity(dtShapeFrame2->asShapeNode())) { std::size_t shape1ID = - this->main->shapes.IdentityOf(dtShapeFrame1->asShapeNode()); + this->shapes.IdentityOf(dtShapeFrame1->asShapeNode()); std::size_t shape2ID = - this->main->shapes.IdentityOf(dtShapeFrame2->asShapeNode()); + this->shapes.IdentityOf(dtShapeFrame2->asShapeNode()); CompositeData extraData; @@ -191,8 +144,8 @@ SimulationFeaturesPrivate::convertContact( return SimulationFeatures::ContactInternal { - this->main->GenerateIdentity(shape1ID, this->main->shapes.at(shape1ID)), - this->main->GenerateIdentity(shape2ID, this->main->shapes.at(shape2ID)), + this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)), + this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)), _contact.point, extraData }; } @@ -200,11 +153,6 @@ SimulationFeaturesPrivate::convertContact( return std::nullopt; } -SimulationFeaturesPrivate::SimulationFeaturesPrivate(SimulationFeatures* _main) - : main(_main) -{ -} - dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( const dart::collision::Contact& _contact, const size_t _numContactsOnCollisionObject) const @@ -232,28 +180,25 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( pIgn.Get>().contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity; - if (this->surfaceParamsCallback) + auto contactInternal = this->convertContact(_contact); + if (contactInternal) { - auto contactInternal = this->convertContact(_contact); - if (contactInternal) - { - this->surfaceParamsCallback(contactInternal.value(), - _numContactsOnCollisionObject, pIgn); - - pDart.mFrictionCoeff = pIgn.Get>().frictionCoeff; - pDart.mSecondaryFrictionCoeff = - pIgn.Get>().secondaryFrictionCoeff; - pDart.mSlipCompliance = pIgn.Get>().slipCompliance; - pDart.mSecondarySlipCompliance = - pIgn.Get>().secondarySlipCompliance; - pDart.mRestitutionCoeff = - pIgn.Get>().restitutionCoeff; - pDart.mFirstFrictionalDirection = - pIgn.Get>().firstFrictionalDirection; - pDart.mContactSurfaceMotionVelocity = - pIgn.Get>(). - contactSurfaceMotionVelocity; - } + this->surfaceParamsCallback(contactInternal.value(), + _numContactsOnCollisionObject, pIgn); + + pDart.mFrictionCoeff = pIgn.Get>().frictionCoeff; + pDart.mSecondaryFrictionCoeff = + pIgn.Get>().secondaryFrictionCoeff; + pDart.mSlipCompliance = pIgn.Get>().slipCompliance; + pDart.mSecondarySlipCompliance = + pIgn.Get>().secondarySlipCompliance; + pDart.mRestitutionCoeff = + pIgn.Get>().restitutionCoeff; + pDart.mFirstFrictionalDirection = + pIgn.Get>().firstFrictionalDirection; + pDart.mContactSurfaceMotionVelocity = + pIgn.Get>(). + contactSurfaceMotionVelocity; } this->lastIgnParams = pIgn; diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index 63c7b23fe..9a932541c 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -22,6 +22,8 @@ #include #include +#include + #include #include #include @@ -46,7 +48,30 @@ struct SimulationFeatureList : FeatureList< SetContactJointPropertiesCallbackFeature > { }; -class SimulationFeaturesPrivate; +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 SetContactJointPropertiesCallbackFeature Feature; + public: typedef Feature::Implementation Impl; + + public: Impl::SurfaceParamsCallback surfaceParamsCallback; + + public: std::function< + std::optional(const dart::collision::Contact&) + > convertContact; + + public: mutable typename Feature::ContactSurfaceParams + lastIgnParams; +}; +using IgnContactSurfaceHandlerPtr = std::shared_ptr; class SimulationFeatures : public virtual Base, @@ -55,8 +80,8 @@ class SimulationFeatures : public: using GetContactsFromLastStepFeature::Implementation ::ContactInternal; - public: SimulationFeatures(); - public: ~SimulationFeatures() override; + public: SimulationFeatures() = default; + public: ~SimulationFeatures() override = default; public: void WorldForwardStep( const Identity &_worldID, @@ -75,8 +100,11 @@ class SimulationFeatures : public: bool RemoveContactJointPropertiesCallback( const Identity &_worldID, const std::string &_callbackID) override; - private: std::unique_ptr dataPtr; - private: friend class SimulationFeaturesPrivate; + private: std::optional convertContact( + const dart::collision::Contact& _contact) const; + + private: std::unordered_map< + std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers; }; diff --git a/include/ignition/physics/ContactJointProperties.hh b/include/ignition/physics/ContactJointProperties.hh index b8f0c70a7..7017051e9 100644 --- a/include/ignition/physics/ContactJointProperties.hh +++ b/include/ignition/physics/ContactJointProperties.hh @@ -133,8 +133,21 @@ class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature public: using Contact = typename GetContactsFromLastStepFeature ::World::Contact; + /// \brief This callback is called for every detected contact point and + /// allows customizing properties of the contact surface. + /// \param _contact[in] The contact object containint contact point, + /// normal, force etc. + /// \param _numContactsOnCollision[in] Number of contact points on the same + /// collision object. This can be used + /// e.g. for force normalization. + /// \param _surfaceParams[in,out] Parameters of the contact surface. They + /// are pre-filled by the physics engine and + /// the callback can alter them. public: typedef std::function< - void(const Contact&, size_t, ContactSurfaceParams&) + void( + const Contact& /*_contact*/, + size_t /*_numContactsOnCollision*/, + ContactSurfaceParams& /*_surfaceParams*/) > SurfaceParamsCallback; /// \brief Add the callback. diff --git a/include/ignition/physics/detail/ContactJointProperties.hh b/include/ignition/physics/detail/ContactJointProperties.hh index d21501ad5..15f180e0d 100644 --- a/include/ignition/physics/detail/ContactJointProperties.hh +++ b/include/ignition/physics/detail/ContactJointProperties.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. From 54afd1946a6cf131e17d12d3d7d97103f1d0887d Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 14 Jul 2021 03:03:22 +0200 Subject: [PATCH 04/16] Exchanged CompositeData for a struct to improve performance. Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.cc | 89 ++++++++++------- .../physics/ContactJointProperties.hh | 99 ++++--------------- 2 files changed, 70 insertions(+), 118 deletions(-) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index c21ea6eb3..18180713d 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -167,18 +167,13 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( typedef FeaturePolicy3d P; typename F::ContactSurfaceParams

pIgn; - pIgn.Get>().frictionCoeff = pDart.mFrictionCoeff; - pIgn.Get>().secondaryFrictionCoeff = - pDart.mSecondaryFrictionCoeff; - pIgn.Get>().slipCompliance = pDart.mSlipCompliance; - pIgn.Get>().secondarySlipCompliance = - pDart.mSecondarySlipCompliance; - pIgn.Get>().restitutionCoeff = - pDart.mRestitutionCoeff; - pIgn.Get>().firstFrictionalDirection = - pDart.mFirstFrictionalDirection; - pIgn.Get>().contactSurfaceMotionVelocity = - pDart.mContactSurfaceMotionVelocity; + 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) @@ -186,19 +181,44 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( this->surfaceParamsCallback(contactInternal.value(), _numContactsOnCollisionObject, pIgn); - pDart.mFrictionCoeff = pIgn.Get>().frictionCoeff; - pDart.mSecondaryFrictionCoeff = - pIgn.Get>().secondaryFrictionCoeff; - pDart.mSlipCompliance = pIgn.Get>().slipCompliance; - pDart.mSecondarySlipCompliance = - pIgn.Get>().secondarySlipCompliance; - pDart.mRestitutionCoeff = - pIgn.Get>().restitutionCoeff; - pDart.mFirstFrictionalDirection = - pIgn.Get>().firstFrictionalDirection; - pDart.mContactSurfaceMotionVelocity = - pIgn.Get>(). - contactSurfaceMotionVelocity; + 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.rollingFrictionCoeff) + { + ignwarn << "DART doesn't support secondary rolling friction setting" + << std::endl; + warnedSecondaryRollingFrictionCoeff = true; + } + + static bool warnedNormalRollingFrictionCoeff = false; + if (!warnedNormalRollingFrictionCoeff && pIgn.rollingFrictionCoeff) + { + ignwarn << "DART doesn't support normal rolling friction setting" + << std::endl; + warnedNormalRollingFrictionCoeff = true; + } } this->lastIgnParams = pIgn; @@ -220,21 +240,18 @@ IgnContactSurfaceHandler::createConstraint( typedef FeaturePolicy3d P; typename F::ContactSurfaceParams

& p = this->lastIgnParams; - if (this->lastIgnParams.Has>()) - constraint->setErrorReductionParameter( - p.Get>().errorReductionParameter); + if (this->lastIgnParams.errorReductionParameter) + constraint->setErrorReductionParameter(p.errorReductionParameter.value()); - if (this->lastIgnParams.Has>()) - constraint->setErrorAllowance( - p.Get>().maxErrorAllowance); + if (this->lastIgnParams.maxErrorAllowance) + constraint->setErrorAllowance(p.maxErrorAllowance.value()); - if (this->lastIgnParams.Has>()) + if (this->lastIgnParams.maxErrorReductionVelocity) constraint->setMaxErrorReductionVelocity( - p.Get>().maxErrorReductionVelocity); + p.maxErrorReductionVelocity.value()); - if (this->lastIgnParams.Has>()) - constraint->setConstraintForceMixing( - p.Get>().constraintForceMixing); + if (this->lastIgnParams.constraintForceMixing) + constraint->setConstraintForceMixing(p.constraintForceMixing.value()); return constraint; } diff --git a/include/ignition/physics/ContactJointProperties.hh b/include/ignition/physics/ContactJointProperties.hh index 7017051e9..f2cd1b047 100644 --- a/include/ignition/physics/ContactJointProperties.hh +++ b/include/ignition/physics/ContactJointProperties.hh @@ -37,91 +37,26 @@ namespace physics class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature : public virtual FeatureWithRequirements { - public: template struct FrictionCoeff - { - typename PolicyT::Scalar frictionCoeff; - }; - - public: template struct SecondaryFrictionCoeff - { - typename PolicyT::Scalar secondaryFrictionCoeff; - }; - - public: template struct RollingFrictionCoeff - { - typename PolicyT::Scalar rollingFrictionCoeff; - }; - - public: template struct SecondaryRollingFrictionCoeff - { - typename PolicyT::Scalar secondaryRollingFrictionCoeff; - }; - - public: template struct NormalRollingFrictionCoeff - { - typename PolicyT::Scalar normalRollingFrictionCoeff; - }; - - public: template struct SlipCompliance - { - typename PolicyT::Scalar slipCompliance; - }; - - public: template struct SecondarySlipCompliance - { - typename PolicyT::Scalar secondarySlipCompliance; - }; - - public: template struct RestitutionCoeff - { - typename PolicyT::Scalar restitutionCoeff; - }; - - public: template struct FirstFrictionalDirection - { - typename FromPolicy::template Use firstFrictionalDirection; - }; - - public: template struct ContactSurfaceMotionVelocity - { - typename FromPolicy::template Use + public: template struct ContactSurfaceParams + { + std::optional frictionCoeff; + std::optional secondaryFrictionCoeff; + std::optional rollingFrictionCoeff; + std::optional secondaryRollingFrictionCoeff; + std::optional normalRollingFrictionCoeff; + std::optional slipCompliance; + std::optional secondarySlipCompliance; + std::optional restitutionCoeff; + std::optional::template Use> + firstFrictionalDirection; + std::optional::template Use> contactSurfaceMotionVelocity; + std::optional errorReductionParameter; + std::optional maxErrorReductionVelocity; + std::optional maxErrorAllowance; + std::optional constraintForceMixing; }; - public: template struct ErrorReductionParameter - { - typename PolicyT::Scalar errorReductionParameter; - }; - - public: template struct MaxErrorReductionVelocity - { - typename PolicyT::Scalar maxErrorReductionVelocity; - }; - - public: template struct MaxErrorAllowance - { - typename PolicyT::Scalar maxErrorAllowance; - }; - - public: template struct ConstraintForceMixing - { - typename PolicyT::Scalar constraintForceMixing; - }; - - public: template using ContactSurfaceParams = - SpecifyData, SecondaryFrictionCoeff, - RollingFrictionCoeff, SecondaryRollingFrictionCoeff, - NormalRollingFrictionCoeff, - SlipCompliance, SecondarySlipCompliance, - RestitutionCoeff, - FirstFrictionalDirection, - ContactSurfaceMotionVelocity, - ErrorReductionParameter, - MaxErrorReductionVelocity, - MaxErrorAllowance, - ConstraintForceMixing > >; - public: template class World : public virtual Feature::World { From 0bbb40886380735a7a0cada4a2c491e804560338 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 14 Jul 2021 03:07:55 +0200 Subject: [PATCH 05/16] Codecheck Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index 9a932541c..279a6d11f 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -20,6 +20,7 @@ #include #include +#include #include #include From 031fad8535ab238e311ef9888e481a4924391eb2 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 18 Aug 2021 00:56:45 +0200 Subject: [PATCH 06/16] Updated with latest DART changes Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 18180713d..4d726d991 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -92,7 +92,7 @@ void SimulationFeatures::AddContactJointPropertiesCallback( }; this->contactSurfaceHandlers[_callbackID] = handler; - world->getConstraintSolver()->setContactSurfaceHandler(handler); + world->getConstraintSolver()->addContactSurfaceHandler(handler); } bool SimulationFeatures::RemoveContactJointPropertiesCallback( From 278901159d60568dabdb5cbc5b3b8399ce231834 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 18 Aug 2021 00:58:47 +0200 Subject: [PATCH 07/16] Fixed bugs from review Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 4d726d991..131f1acc7 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -205,7 +205,8 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( } static bool warnedSecondaryRollingFrictionCoeff = false; - if (!warnedSecondaryRollingFrictionCoeff && pIgn.rollingFrictionCoeff) + if (!warnedSecondaryRollingFrictionCoeff && + pIgn.secondaryRollingFrictionCoeff) { ignwarn << "DART doesn't support secondary rolling friction setting" << std::endl; @@ -213,7 +214,7 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( } static bool warnedNormalRollingFrictionCoeff = false; - if (!warnedNormalRollingFrictionCoeff && pIgn.rollingFrictionCoeff) + if (!warnedNormalRollingFrictionCoeff && pIgn.normalRollingFrictionCoeff) { ignwarn << "DART doesn't support normal rolling friction setting" << std::endl; From 65397fb52de0d6d96792f11afaed0369bf4f61fa Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 11 Oct 2021 14:18:59 -0500 Subject: [PATCH 08/16] Detect if contact constraint customization is available This allows CI to pass on Focal. Signed-off-by: Addisu Z. Taddese --- dartsim/CMakeLists.txt | 16 +++++++ dartsim/src/SimulationFeatures.cc | 78 ++++++++++++++++--------------- dartsim/src/SimulationFeatures.hh | 22 ++++++--- 3 files changed, 72 insertions(+), 44 deletions(-) diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index 22bad22a5..9070688bd 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -33,6 +33,22 @@ 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) +set(CMAKE_REQUIRED_INCLUDES ${DART_INCLUDE_DIRS}) +set(CMAKE_REQUIRED_DEFINITIONS ${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}) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 131f1acc7..492c986a4 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -21,9 +21,11 @@ #include #include +#include #include +#ifdef DART_HAS_CONTACT_SURFACE #include -#include +#endif #include "SimulationFeatures.hh" @@ -79,42 +81,6 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const return outContacts; } -void SimulationFeatures::AddContactJointPropertiesCallback( - const Identity& _worldID, const std::string& _callbackID, - SurfaceParamsCallback _callback) -{ - auto *world = this->ReferenceInterface(_worldID); - - auto handler = std::make_shared(); - 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::RemoveContactJointPropertiesCallback( - const Identity& _worldID, const std::string& _callbackID) -{ - auto *world = this->ReferenceInterface(_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; - } -} - std::optional SimulationFeatures::convertContact( const dart::collision::Contact& _contact) const @@ -153,6 +119,43 @@ SimulationFeatures::convertContact( return std::nullopt; } +#ifdef DART_HAS_CONTACT_SURFACE +void SimulationFeatures::AddContactJointPropertiesCallback( + const Identity& _worldID, const std::string& _callbackID, + SurfaceParamsCallback _callback) +{ + auto *world = this->ReferenceInterface(_worldID); + + auto handler = std::make_shared(); + 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::RemoveContactJointPropertiesCallback( + const Identity& _worldID, const std::string& _callbackID) +{ + auto *world = this->ReferenceInterface(_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 @@ -256,6 +259,7 @@ IgnContactSurfaceHandler::createConstraint( return constraint; } +#endif } } diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index 279a6d11f..505d3b2ed 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -23,7 +23,10 @@ #include #include +#include +#if DART_VERSION_AT_LEAST(6, 10, 0) #include +#endif #include #include @@ -45,10 +48,13 @@ namespace dartsim { struct SimulationFeatureList : FeatureList< ForwardStep, - GetContactsFromLastStepFeature, - SetContactJointPropertiesCallbackFeature +#ifdef DART_HAS_CONTACT_SURFACE + SetContactJointPropertiesCallbackFeature, +#endif + GetContactsFromLastStepFeature > { }; +#ifdef DART_HAS_CONTACT_SURFACE class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler { public: dart::constraint::ContactSurfaceParams createParams( @@ -72,7 +78,9 @@ class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler public: mutable typename Feature::ContactSurfaceParams lastIgnParams; }; + using IgnContactSurfaceHandlerPtr = std::shared_ptr; +#endif class SimulationFeatures : public virtual Base, @@ -93,6 +101,10 @@ class SimulationFeatures : public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; + private: std::optional convertContact( + const dart::collision::Contact& _contact) const; + +#ifdef DART_HAS_CONTACT_SURFACE public: void AddContactJointPropertiesCallback( const Identity &_worldID, const std::string &_callbackID, @@ -101,15 +113,11 @@ class SimulationFeatures : public: bool RemoveContactJointPropertiesCallback( const Identity &_worldID, const std::string &_callbackID) override; - private: std::optional convertContact( - const dart::collision::Contact& _contact) const; - private: std::unordered_map< std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers; +#endif }; - - } } } From be31da3189c0dba348c6056d910c64e2a5995886 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Sun, 24 Oct 2021 12:36:31 +0200 Subject: [PATCH 09/16] Fixed CMake for finding whether DART supports contact surface customization. Signed-off-by: Martin Pecka --- dartsim/CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index 9070688bd..5063642cd 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -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 @@ -38,8 +37,7 @@ target_link_libraries(${dartsim_plugin} # was added to enable these customizations, to detect if the feature is # available. include(CheckIncludeFileCXX) -set(CMAKE_REQUIRED_INCLUDES ${DART_INCLUDE_DIRS}) -set(CMAKE_REQUIRED_DEFINITIONS ${EIGEN3_INCLUDE_DIRS}) +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) From 90254540c81cc3b1f78f8f52629d4093b5db6645 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 25 Oct 2021 03:08:29 +0200 Subject: [PATCH 10/16] Added a test for SetContactJointPropertiesCallback feature. Signed-off-by: Martin Pecka --- dartsim/CMakeLists.txt | 4 + dartsim/src/SimulationFeatures_TEST.cc | 171 ++++++++++++++++-- .../physics/ContactJointProperties.hh | 8 +- .../physics/detail/ContactJointProperties.hh | 11 +- tpe/plugin/src/SimulationFeatures_TEST.cc | 6 +- 5 files changed, 169 insertions(+), 31 deletions(-) diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index 5063642cd..b477a6b9c 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -81,6 +81,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() diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index 265a7b732..70a911cd8 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -48,14 +49,23 @@ struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::GetEntities, ignition::physics::GetShapeBoundingBox, ignition::physics::CollisionFilterMaskFeature, +#ifdef DART_HAS_CONTACT_SURFACE + ignition::physics::SetContactJointPropertiesCallbackFeature, +#endif ignition::physics::sdf::ConstructSdfWorld > { }; using TestWorldPtr = ignition::physics::World3dPtr; using TestShapePtr = ignition::physics::Shape3dPtr; -using ContactPoint = ignition::physics::World3d::ContactPoint; -using ExtraContactData = - ignition::physics::World3d::ExtraContactData; +using TestWorld = ignition::physics::World3d; +using TestContactPoint = TestWorld::ContactPoint; +using TestExtraContactData = TestWorld::ExtraContactData; +using TestContact = TestWorld::Contact; +#ifdef DART_HAS_CONTACT_SURFACE +using ContactSurfaceParams = + ignition::physics::SetContactJointPropertiesCallbackFeature:: + ContactSurfaceParams; +#endif std::unordered_set LoadWorlds( const std::string &_library, @@ -237,17 +247,6 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) auto groundPlane = world->GetModel("ground_plane"); auto groundPlaneCollision = groundPlane->GetLink(0)->GetShape(0); - // The first step already has contacts, but the contact force due to the - // impact does not match the steady-state force generated by the - // body's weight. - StepWorld(world); - - // After a second step, the contact force reaches steady-state - StepWorld(world); - - auto contacts = world->GetContactsFromLastStep(); - EXPECT_EQ(4u, contacts.size()); - // Use a set because the order of collisions is not determined. std::set possibleCollisions = { groundPlaneCollision, @@ -274,9 +273,14 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) {sphere->GetLink(3)->GetShape(0), 3.0 * gravity}, }; - for (auto &contact : contacts) + // This procedure checks the validity of a generated contact point. It is + // used both when checking the contacts after the step is finished and for + // checking them inside the contact joint properties callback. The callback + // is called after the contacts are generated but before they affect the + // physics. That is why contact force is zero during the callback. + auto checkContact = [&](const TestContact& _contact, const bool zeroForce) { - const auto &contactPoint = contact.Get(); + const auto &contactPoint = _contact.Get(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); @@ -302,7 +306,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) contactPoint.point, 1e-6)); // Check if the engine populated the extra contact data struct - const auto* extraContactData = contact.Query(); + const auto* extraContactData = _contact.Query(); ASSERT_NE(nullptr, extraContactData); // The normal of the contact force is a vector pointing up (z positive) @@ -315,8 +319,139 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_NEAR(extraContactData->force[0], 0.0, 1e-3); EXPECT_NEAR(extraContactData->force[1], 0.0, 1e-3); EXPECT_NEAR(extraContactData->force[2], - forceExpectations.at(testCollision), 1e-3); + zeroForce ? 0 : forceExpectations.at(testCollision), 1e-3); + }; + +#ifdef DART_HAS_CONTACT_SURFACE + size_t numContactCallbackCalls = 0u; + auto contactCallback = [&]( + const TestContact& _contact, + size_t _numContactsOnCollision, + ContactSurfaceParams& _surfaceParams) + { + numContactCallbackCalls++; + checkContact(_contact, true); + EXPECT_EQ(1u, _numContactsOnCollision); + // the values in _surfaceParams are implemented as std::optional to allow + // physics engines fill only those parameters that are actually + // implemented + ASSERT_TRUE(_surfaceParams.frictionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.secondaryFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.rollingFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.secondaryRollingFrictionCoeff.has_value()); + // not implemented in DART yet + EXPECT_FALSE(_surfaceParams.normalRollingFrictionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.slipCompliance.has_value()); + ASSERT_TRUE(_surfaceParams.secondarySlipCompliance.has_value()); + ASSERT_TRUE(_surfaceParams.restitutionCoeff.has_value()); + ASSERT_TRUE(_surfaceParams.firstFrictionalDirection.has_value()); + ASSERT_TRUE(_surfaceParams.contactSurfaceMotionVelocity.has_value()); + // these constraint parameters are implemented in DART but are not filled + // when the callback is called; they are only read after the callback ends + EXPECT_FALSE(_surfaceParams.errorReductionParameter.has_value()); + EXPECT_FALSE(_surfaceParams.maxErrorReductionVelocity.has_value()); + EXPECT_FALSE(_surfaceParams.maxErrorAllowance.has_value()); + EXPECT_FALSE(_surfaceParams.constraintForceMixing.has_value()); + + EXPECT_NEAR(_surfaceParams.frictionCoeff.value(), 1.0, 1e-6); + EXPECT_NEAR(_surfaceParams.secondaryFrictionCoeff.value(), 1.0, 1e-6); + EXPECT_NEAR(_surfaceParams.slipCompliance.value(), 0.0, 1e-6); + EXPECT_NEAR(_surfaceParams.secondarySlipCompliance.value(), 0.0, 1e-6); + EXPECT_NEAR(_surfaceParams.restitutionCoeff.value(), 0.0, 1e-6); + + EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 1), + _surfaceParams.firstFrictionalDirection.value(), 1e-6)); + + EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 0), + _surfaceParams.contactSurfaceMotionVelocity.value(), 1e-6)); + }; + world->AddContactJointPropertiesCallback("test", contactCallback); +#endif + + // The first step already has contacts, but the contact force due to the + // impact does not match the steady-state force generated by the + // body's weight. + StepWorld(world); + +#ifdef DART_HAS_CONTACT_SURFACE + // There are 4 collision bodies in the world all colliding at the same time + EXPECT_EQ(4u, numContactCallbackCalls); +#endif + + // After a second step, the contact force reaches steady-state + StepWorld(world); + +#ifdef DART_HAS_CONTACT_SURFACE + // There are 4 collision bodies in the world all colliding at the same time + EXPECT_EQ(8u, numContactCallbackCalls); +#endif + + auto contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(4u, contacts.size()); + + for (auto &contact : contacts) + { + checkContact(contact, false); + } + +#ifdef DART_HAS_CONTACT_SURFACE + // removing a non-existing callback yields no error but returns false + EXPECT_FALSE(world->RemoveContactJointPropertiesCallback("foo")); + + // removing an existing callback works and the callback is no longer called + EXPECT_TRUE(world->RemoveContactJointPropertiesCallback("test")); + + // Third step + StepWorld(world); + + // Number of callback calls is the same as after the 2nd call + EXPECT_EQ(8u, numContactCallbackCalls); + + // Now we check that changing _surfaceParams inside the contact properties + // callback affects the result of the simulation; we set + // contactSurfaceMotionVelocity to [1,0,0] which accelerates the contact + // points from 0 m/s to 1 m/s in a single simulation step. + + auto contactCallback2 = [&]( + const TestContact& /*_contact*/, + size_t /*_numContactsOnCollision*/, + ContactSurfaceParams& _surfaceParams) + { + numContactCallbackCalls++; + // friction direction is [0,0,1] and contact surface motion velocity uses + // the X value to denote the desired velocity along the friction direction + _surfaceParams.contactSurfaceMotionVelocity->x() = 1.0; + }; + world->AddContactJointPropertiesCallback("test2", contactCallback2); + + numContactCallbackCalls = 0u; + // Fourth step + StepWorld(world); + EXPECT_EQ(4u, numContactCallbackCalls); + + // Adjust the expected forces to account for the added acceleration along Z + forceExpectations = + { + // Contact force expectations are: + // link mass * (gravity + acceleration to 1 m.s^-1 in 1 ms) + {sphere->GetLink(0)->GetShape(0), 0.1 * gravity + 100}, + {sphere->GetLink(1)->GetShape(0), 1.0 * gravity + 999.99}, + {sphere->GetLink(2)->GetShape(0), 2.0 * gravity + 1999.98}, + {sphere->GetLink(3)->GetShape(0), 3.0 * gravity + 2999.97}, + }; + + // Verify that the detected contacts correspond to the adjusted expectations + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(4u, contacts.size()); + for (auto &contact : contacts) + { + checkContact(contact, false); } + + EXPECT_TRUE(world->RemoveContactJointPropertiesCallback("test2")); +#endif } } diff --git a/include/ignition/physics/ContactJointProperties.hh b/include/ignition/physics/ContactJointProperties.hh index f2cd1b047..fb9cce18c 100644 --- a/include/ignition/physics/ContactJointProperties.hh +++ b/include/ignition/physics/ContactJointProperties.hh @@ -65,12 +65,9 @@ class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature public: using ShapePtrType = typename GetContactsFromLastStepFeature ::World::ShapePtrType; - public: using Contact = typename GetContactsFromLastStepFeature - ::World::Contact; - /// \brief This callback is called for every detected contact point and /// allows customizing properties of the contact surface. - /// \param _contact[in] The contact object containint contact point, + /// \param _contact[in] The contact object containing contact point, /// normal, force etc. /// \param _numContactsOnCollision[in] Number of contact points on the same /// collision object. This can be used @@ -80,7 +77,8 @@ class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature /// the callback can alter them. public: typedef std::function< void( - const Contact& /*_contact*/, + const typename GetContactsFromLastStepFeature:: + World::Contact& /*_contact*/, size_t /*_numContactsOnCollision*/, ContactSurfaceParams& /*_surfaceParams*/) > SurfaceParamsCallback; diff --git a/include/ignition/physics/detail/ContactJointProperties.hh b/include/ignition/physics/detail/ContactJointProperties.hh index 15f180e0d..848df68c5 100644 --- a/include/ignition/physics/detail/ContactJointProperties.hh +++ b/include/ignition/physics/detail/ContactJointProperties.hh @@ -39,20 +39,21 @@ void SetContactJointPropertiesCallbackFeature::World:: using Impl = Implementation; using ContactInternal = typename Impl::ContactImpl; - auto pimpl = this->pimpl; - auto convertContact = [pimpl](const ContactInternal& _internal) -> Contact + auto pimplPtr = this->pimpl; + auto convertContact = [pimplPtr](const ContactInternal& _internal) { using ContactPoint = typename GetContactsFromLastStepFeature::World ::ContactPoint; ContactPoint contactPoint { - ShapePtrType(pimpl, _internal.collision1), - ShapePtrType(pimpl, _internal.collision2), + ShapePtrType(pimplPtr, _internal.collision1), + ShapePtrType(pimplPtr, _internal.collision2), _internal.point }; - Contact contactOutput; + typename GetContactsFromLastStepFeature::World::Contact + contactOutput; contactOutput.template Get() = std::move(contactPoint); using ExtraContactData = diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 17eb8aaab..aa4de7af4 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -54,7 +54,7 @@ struct TestFeatureList : ignition::physics::FeatureList< using TestWorldPtr = ignition::physics::World3dPtr; using TestShapePtr = ignition::physics::Shape3dPtr; -using ContactPoint = ignition::physics::World3d::ContactPoint; +using TestContactPoint = ignition::physics::World3d::ContactPoint; std::unordered_set LoadWorlds( const std::string &_library, @@ -327,7 +327,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) for (auto &contact : contacts) { - const auto &contactPoint = contact.Get(); + const auto &contactPoint = contact.Get(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_NE(contactPoint.collision1, contactPoint.collision2); @@ -375,7 +375,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) contactBoxCylinder = 0u; for (auto contact : contacts) { - const auto &contactPoint = contact.Get<::ContactPoint>(); + const auto &contactPoint = contact.Get<::TestContactPoint>(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_NE(contactPoint.collision1, contactPoint.collision2); From 967e5d3a84c13900650fb2920217ddd262fc6760 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 25 Oct 2021 03:44:15 +0200 Subject: [PATCH 11/16] Documented the ContactSurfaceParams struct. Signed-off-by: Martin Pecka --- .../physics/ContactJointProperties.hh | 51 ++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/include/ignition/physics/ContactJointProperties.hh b/include/ignition/physics/ContactJointProperties.hh index fb9cce18c..3d07e4872 100644 --- a/include/ignition/physics/ContactJointProperties.hh +++ b/include/ignition/physics/ContactJointProperties.hh @@ -37,23 +37,70 @@ namespace physics class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature : public virtual FeatureWithRequirements { + /// \brief This struct gets filled by the simulator and contains various + /// properties of a contact joint (surface, constraint). All of the values + /// are optional, which means that they are only filled if the physics engine + /// supports them. Some originally unfilled values may still be processed by + /// the physics engine if they are set - this just means there is no default + /// value for them. public: template struct ContactSurfaceParams { + /// \brief Coefficient of friction along the 1st friction direction. std::optional frictionCoeff; + + /// \brief Coefficient of friction along the 2nd friction direction. std::optional secondaryFrictionCoeff; + + /// \brief Coefficient of rolling friction along the 1st friction direction. std::optional rollingFrictionCoeff; + + /// \brief Coefficient of rolling friction along the 2nd friction direction. std::optional secondaryRollingFrictionCoeff; + + /// \brief Coefficient of rolling friction along the contact normal. std::optional normalRollingFrictionCoeff; + + /// \brief Force-dependent slip coefficient along the 1st friction + /// direction. std::optional slipCompliance; + + /// \brief Force-dependent slip coefficient along the 2nd friction + /// direction. std::optional secondarySlipCompliance; + + /// \brief Defines the bounciness of the contact. 0 is not bouncy. Values + /// between 0 and 1 are allowed. std::optional restitutionCoeff; + + /// \brief The first frictional direction. It should be perpendicular to the + /// contact normal. The second frictional direction can be computed as a + /// vector perpendicular both to the normal and to the first direction. std::optional::template Use> firstFrictionalDirection; + + /// \brief Desired velocity of the colliding bodies in the contact point. + /// Setting this to non-zero asks the physics engine to add such forces + /// that can achieve that the colliding bodies have the specified velocity. + /// The X component specifies velocity along 1st friction direction. + /// The Y component specifies velocity along 2nd friction direction. + /// The Z component specifies velocity along the contact normal. std::optional::template Use> contactSurfaceMotionVelocity; + + /// \brief Joint error reduction parameter. This is the fraction of the + /// joint error that will be attempted to be corrected in each simulation + /// step. Allowed values are 0 to 1. Default is usually somewhere between. std::optional errorReductionParameter; + + /// \brief Maximum velocity that can be used to reduce joint error. std::optional maxErrorReductionVelocity; + + /// \brief Maximum joint error for which no error reduction is performed. std::optional maxErrorAllowance; + + /// \brief Constraint force mixing. This should be a non-negative number. + /// If greater than 0, this number is added to the diagonal of the system + /// matrix making the contact softer and the solution more stable. std::optional constraintForceMixing; }; @@ -68,7 +115,9 @@ class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature /// \brief This callback is called for every detected contact point and /// allows customizing properties of the contact surface. /// \param _contact[in] The contact object containing contact point, - /// normal, force etc. + /// normal, force etc. Please note that the force will + /// be always zero because the forward step has not yet + /// been run to compute the force. /// \param _numContactsOnCollision[in] Number of contact points on the same /// collision object. This can be used /// e.g. for force normalization. From 5080c4001e577920b65269f9194e6e5befab47ac Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 25 Oct 2021 19:12:12 +0200 Subject: [PATCH 12/16] Fix include guard. Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index 505d3b2ed..b8defd0cb 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -24,7 +24,7 @@ #include #include -#if DART_VERSION_AT_LEAST(6, 10, 0) +#ifdef DART_HAS_CONTACT_SURFACE #include #endif From 7ca80078e4b95aa95e3f10e4f8ed5d39e82e8cae Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 25 Oct 2021 14:40:53 -0500 Subject: [PATCH 13/16] Fix detection of ContactSurface.hh in macOS Signed-off-by: Addisu Z. Taddese --- dartsim/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/dartsim/CMakeLists.txt b/dartsim/CMakeLists.txt index b477a6b9c..59396ab58 100644 --- a/dartsim/CMakeLists.txt +++ b/dartsim/CMakeLists.txt @@ -37,6 +37,11 @@ target_link_libraries(${dartsim_plugin} # 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) From 553edad2d3f0b620c730ffc87821c5260abf2122 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 26 Oct 2021 15:47:28 +0200 Subject: [PATCH 14/16] Renamed normalRolling friction to torsional friction. Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.cc | 8 ++++---- dartsim/src/SimulationFeatures_TEST.cc | 2 +- include/ignition/physics/ContactJointProperties.hh | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 492c986a4..fd8e900d5 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -216,12 +216,12 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( warnedSecondaryRollingFrictionCoeff = true; } - static bool warnedNormalRollingFrictionCoeff = false; - if (!warnedNormalRollingFrictionCoeff && pIgn.normalRollingFrictionCoeff) + static bool warnedTorsionalFrictionCoeff = false; + if (!warnedTorsionalFrictionCoeff && pIgn.torsionalFrictionCoeff) { - ignwarn << "DART doesn't support normal rolling friction setting" + ignwarn << "DART doesn't support torsional friction setting" << std::endl; - warnedNormalRollingFrictionCoeff = true; + warnedTorsionalFrictionCoeff = true; } } diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index 70a911cd8..17660500d 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -342,7 +342,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) // not implemented in DART yet EXPECT_FALSE(_surfaceParams.secondaryRollingFrictionCoeff.has_value()); // not implemented in DART yet - EXPECT_FALSE(_surfaceParams.normalRollingFrictionCoeff.has_value()); + EXPECT_FALSE(_surfaceParams.torsionalFrictionCoeff.has_value()); ASSERT_TRUE(_surfaceParams.slipCompliance.has_value()); ASSERT_TRUE(_surfaceParams.secondarySlipCompliance.has_value()); ASSERT_TRUE(_surfaceParams.restitutionCoeff.has_value()); diff --git a/include/ignition/physics/ContactJointProperties.hh b/include/ignition/physics/ContactJointProperties.hh index 3d07e4872..9bead0f95 100644 --- a/include/ignition/physics/ContactJointProperties.hh +++ b/include/ignition/physics/ContactJointProperties.hh @@ -57,8 +57,8 @@ class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature /// \brief Coefficient of rolling friction along the 2nd friction direction. std::optional secondaryRollingFrictionCoeff; - /// \brief Coefficient of rolling friction along the contact normal. - std::optional normalRollingFrictionCoeff; + /// \brief Coefficient of torsional friction. + std::optional torsionalFrictionCoeff; /// \brief Force-dependent slip coefficient along the 1st friction /// direction. From 854af93b1d3c6bd9fe7aa9b87a5bb890488a4d4c Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 28 Oct 2021 17:12:47 +0200 Subject: [PATCH 15/16] Rename ContactJointProperties to ContactProperties. Signed-off-by: Martin Pecka --- dartsim/src/SimulationFeatures.cc | 8 +++---- dartsim/src/SimulationFeatures.hh | 10 ++++----- dartsim/src/SimulationFeatures_TEST.cc | 16 +++++++------- ...ointProperties.hh => ContactProperties.hh} | 20 ++++++++--------- ...ointProperties.hh => ContactProperties.hh} | 22 +++++++++---------- 5 files changed, 38 insertions(+), 38 deletions(-) rename include/ignition/physics/{ContactJointProperties.hh => ContactProperties.hh} (91%) rename include/ignition/physics/detail/{ContactJointProperties.hh => ContactProperties.hh} (78%) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index fd8e900d5..4ca731892 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -120,7 +120,7 @@ SimulationFeatures::convertContact( } #ifdef DART_HAS_CONTACT_SURFACE -void SimulationFeatures::AddContactJointPropertiesCallback( +void SimulationFeatures::AddContactPropertiesCallback( const Identity& _worldID, const std::string& _callbackID, SurfaceParamsCallback _callback) { @@ -136,7 +136,7 @@ void SimulationFeatures::AddContactJointPropertiesCallback( world->getConstraintSolver()->addContactSurfaceHandler(handler); } -bool SimulationFeatures::RemoveContactJointPropertiesCallback( +bool SimulationFeatures::RemoveContactPropertiesCallback( const Identity& _worldID, const std::string& _callbackID) { auto *world = this->ReferenceInterface(_worldID); @@ -166,7 +166,7 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( if (!this->surfaceParamsCallback) return pDart; - typedef SetContactJointPropertiesCallbackFeature F; + typedef SetContactPropertiesCallbackFeature F; typedef FeaturePolicy3d P; typename F::ContactSurfaceParams

pIgn; @@ -240,7 +240,7 @@ IgnContactSurfaceHandler::createConstraint( auto constraint = dart::constraint::ContactSurfaceHandler::createConstraint( _contact, _numContactsOnCollisionObject, _timeStep); - typedef SetContactJointPropertiesCallbackFeature F; + typedef SetContactPropertiesCallbackFeature F; typedef FeaturePolicy3d P; typename F::ContactSurfaceParams

& p = this->lastIgnParams; diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index b8defd0cb..ac87dc691 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -30,7 +30,7 @@ #include #include -#include +#include #include "Base.hh" @@ -49,7 +49,7 @@ namespace dartsim { struct SimulationFeatureList : FeatureList< ForwardStep, #ifdef DART_HAS_CONTACT_SURFACE - SetContactJointPropertiesCallbackFeature, + SetContactPropertiesCallbackFeature, #endif GetContactsFromLastStepFeature > { }; @@ -66,7 +66,7 @@ class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler size_t _numContactsOnCollisionObject, double _timeStep) const override; - public: typedef SetContactJointPropertiesCallbackFeature Feature; + public: typedef SetContactPropertiesCallbackFeature Feature; public: typedef Feature::Implementation Impl; public: Impl::SurfaceParamsCallback surfaceParamsCallback; @@ -105,12 +105,12 @@ class SimulationFeatures : const dart::collision::Contact& _contact) const; #ifdef DART_HAS_CONTACT_SURFACE - public: void AddContactJointPropertiesCallback( + public: void AddContactPropertiesCallback( const Identity &_worldID, const std::string &_callbackID, SurfaceParamsCallback _callback) override; - public: bool RemoveContactJointPropertiesCallback( + public: bool RemoveContactPropertiesCallback( const Identity &_worldID, const std::string &_callbackID) override; private: std::unordered_map< diff --git a/dartsim/src/SimulationFeatures_TEST.cc b/dartsim/src/SimulationFeatures_TEST.cc index 17660500d..e987f56cb 100644 --- a/dartsim/src/SimulationFeatures_TEST.cc +++ b/dartsim/src/SimulationFeatures_TEST.cc @@ -34,7 +34,7 @@ #include #include #include -#include +#include #include #include @@ -50,7 +50,7 @@ struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::GetShapeBoundingBox, ignition::physics::CollisionFilterMaskFeature, #ifdef DART_HAS_CONTACT_SURFACE - ignition::physics::SetContactJointPropertiesCallbackFeature, + ignition::physics::SetContactPropertiesCallbackFeature, #endif ignition::physics::sdf::ConstructSdfWorld > { }; @@ -63,7 +63,7 @@ using TestExtraContactData = TestWorld::ExtraContactData; using TestContact = TestWorld::Contact; #ifdef DART_HAS_CONTACT_SURFACE using ContactSurfaceParams = - ignition::physics::SetContactJointPropertiesCallbackFeature:: + ignition::physics::SetContactPropertiesCallbackFeature:: ContactSurfaceParams; #endif @@ -367,7 +367,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 0), _surfaceParams.contactSurfaceMotionVelocity.value(), 1e-6)); }; - world->AddContactJointPropertiesCallback("test", contactCallback); + world->AddContactPropertiesCallback("test", contactCallback); #endif // The first step already has contacts, but the contact force due to the @@ -398,10 +398,10 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) #ifdef DART_HAS_CONTACT_SURFACE // removing a non-existing callback yields no error but returns false - EXPECT_FALSE(world->RemoveContactJointPropertiesCallback("foo")); + EXPECT_FALSE(world->RemoveContactPropertiesCallback("foo")); // removing an existing callback works and the callback is no longer called - EXPECT_TRUE(world->RemoveContactJointPropertiesCallback("test")); + EXPECT_TRUE(world->RemoveContactPropertiesCallback("test")); // Third step StepWorld(world); @@ -424,7 +424,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) // the X value to denote the desired velocity along the friction direction _surfaceParams.contactSurfaceMotionVelocity->x() = 1.0; }; - world->AddContactJointPropertiesCallback("test2", contactCallback2); + world->AddContactPropertiesCallback("test2", contactCallback2); numContactCallbackCalls = 0u; // Fourth step @@ -450,7 +450,7 @@ TEST_P(SimulationFeatures_TEST, RetrieveContacts) checkContact(contact, false); } - EXPECT_TRUE(world->RemoveContactJointPropertiesCallback("test2")); + EXPECT_TRUE(world->RemoveContactPropertiesCallback("test2")); #endif } } diff --git a/include/ignition/physics/ContactJointProperties.hh b/include/ignition/physics/ContactProperties.hh similarity index 91% rename from include/ignition/physics/ContactJointProperties.hh rename to include/ignition/physics/ContactProperties.hh index 9bead0f95..39b70a8d6 100644 --- a/include/ignition/physics/ContactJointProperties.hh +++ b/include/ignition/physics/ContactProperties.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_PHYSICS_CONTACTJOINTPROPERTIES_HH_ -#define IGNITION_PHYSICS_CONTACTJOINTPROPERTIES_HH_ +#ifndef IGNITION_PHYSICS_CONTACTPROPERTIES_HH_ +#define IGNITION_PHYSICS_CONTACTPROPERTIES_HH_ #include #include @@ -31,10 +31,10 @@ namespace ignition { namespace physics { -/// \brief SetContactJointPropertiesCallbackFeature is a feature for setting the +/// \brief SetContactPropertiesCallbackFeature is a feature for setting the /// properties of a contact joint after it is created but before it affects the /// forward step. -class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature +class IGNITION_PHYSICS_VISIBLE SetContactPropertiesCallbackFeature : public virtual FeatureWithRequirements { /// \brief This struct gets filled by the simulator and contains various @@ -133,11 +133,11 @@ class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature > SurfaceParamsCallback; /// \brief Add the callback. - public: void AddContactJointPropertiesCallback( + public: void AddContactPropertiesCallback( const std::string &_callbackID, SurfaceParamsCallback _callback); /// \brief Remove the callback. - public: bool RemoveContactJointPropertiesCallback( + public: bool RemoveContactPropertiesCallback( const std::string &_callbackID); }; @@ -152,13 +152,13 @@ class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature > SurfaceParamsCallback; /// \brief Add the callback. - public: virtual void AddContactJointPropertiesCallback( + public: virtual void AddContactPropertiesCallback( const Identity &_worldID, const std::string &_callbackID, SurfaceParamsCallback _callback) = 0; /// \brief Remove the callback. - public: virtual bool RemoveContactJointPropertiesCallback( + public: virtual bool RemoveContactPropertiesCallback( const Identity &_worldID, const std::string &_callbackID) = 0; }; }; @@ -166,6 +166,6 @@ class IGNITION_PHYSICS_VISIBLE SetContactJointPropertiesCallbackFeature } } -#include "ignition/physics/detail/ContactJointProperties.hh" +#include "ignition/physics/detail/ContactProperties.hh" -#endif /* end of include guard: IGNITION_PHYSICS_CONTACTJOINTPROPERTIES_HH_ */ +#endif /* end of include guard: IGNITION_PHYSICS_CONTACTPROPERTIES_HH_ */ diff --git a/include/ignition/physics/detail/ContactJointProperties.hh b/include/ignition/physics/detail/ContactProperties.hh similarity index 78% rename from include/ignition/physics/detail/ContactJointProperties.hh rename to include/ignition/physics/detail/ContactProperties.hh index 848df68c5..055693ffd 100644 --- a/include/ignition/physics/detail/ContactJointProperties.hh +++ b/include/ignition/physics/detail/ContactProperties.hh @@ -15,14 +15,14 @@ * */ -#ifndef IGNITION_PHYSICS_DETAIL_CONTACTJOINTPROPERTIES_HH_ -#define IGNITION_PHYSICS_DETAIL_CONTACTJOINTPROPERTIES_HH_ +#ifndef IGNITION_PHYSICS_DETAIL_CONTACTPROPERTIES_HH_ +#define IGNITION_PHYSICS_DETAIL_CONTACTPROPERTIES_HH_ #include #include #include -#include +#include namespace ignition { @@ -31,8 +31,8 @@ namespace physics ///////////////////////////////////////////////// template -void SetContactJointPropertiesCallbackFeature::World:: - AddContactJointPropertiesCallback( +void SetContactPropertiesCallbackFeature::World:: + AddContactPropertiesCallback( const std::string &_callbackID, SurfaceParamsCallback _callback) { @@ -83,18 +83,18 @@ void SetContactJointPropertiesCallbackFeature::World:: }; } - this->template Interface() - ->AddContactJointPropertiesCallback( + this->template Interface() + ->AddContactPropertiesCallback( this->identity, _callbackID, callbackInternal); } ///////////////////////////////////////////////// template -bool SetContactJointPropertiesCallbackFeature::World:: - RemoveContactJointPropertiesCallback(const std::string& _callbackID) +bool SetContactPropertiesCallbackFeature::World:: + RemoveContactPropertiesCallback(const std::string& _callbackID) { - return this->template Interface()-> - RemoveContactJointPropertiesCallback(this->identity, _callbackID); + return this->template Interface()-> + RemoveContactPropertiesCallback(this->identity, _callbackID); } } // namespace physics From 6a1c2576a024b059e3298388ba308ec10b331e68 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 28 Oct 2021 17:22:13 +0200 Subject: [PATCH 16/16] Removed JointPtrType. Signed-off-by: Martin Pecka --- include/ignition/physics/ContactProperties.hh | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/include/ignition/physics/ContactProperties.hh b/include/ignition/physics/ContactProperties.hh index 39b70a8d6..302e8dfee 100644 --- a/include/ignition/physics/ContactProperties.hh +++ b/include/ignition/physics/ContactProperties.hh @@ -32,7 +32,7 @@ namespace ignition namespace physics { /// \brief SetContactPropertiesCallbackFeature is a feature for setting the -/// properties of a contact joint after it is created but before it affects the +/// properties of a contact after it is created but before it affects the /// forward step. class IGNITION_PHYSICS_VISIBLE SetContactPropertiesCallbackFeature : public virtual FeatureWithRequirements @@ -107,8 +107,6 @@ class IGNITION_PHYSICS_VISIBLE SetContactPropertiesCallbackFeature public: template class World : public virtual Feature::World { - public: using JointPtrType = JointPtr; - public: using ShapePtrType = typename GetContactsFromLastStepFeature ::World::ShapePtrType;