Skip to content

Commit

Permalink
[Constraint.Lagrangian] Add augmented lagrangians (#5003)
Browse files Browse the repository at this point in the history 
* Add AugmentedLagrangianConstraint response

* accelerate GS convergence

* Added virtualization layer for constraints, but linker issue arise. Need to fix it then add virutalization in Contact response

* FIX linking

* Add virtualization layer for contact response

* Fix OBBCapsule

* Fix Distance grid and simplify fix for OBB

* Fix default friction used by tests

* Try fix macos compilation

* Add tag for augmented lagrangian

* add details to the description of epsilon data

* remove multiple registration in factory

* MAke parameters struct final

* USe data accessor instead

* fix compilation

* Try fix scene tests

* Apply suggestions from code review

Co-authored-by: Hugo <[email protected]>

* Add reference

---------

Co-authored-by: Hugo <[email protected]>
  • Loading branch information
@bakpaul @hugtalbot
bakpaul and hugtalbot authored Dec 13, 2024
1 parent 8ba12f0 commit 4eda9c7
Show file tree
Hide file tree
Showing 26 changed files with 1,749 additions and 731 deletions.
6 changes: 6 additions & 0 deletions Sofa/Component/Collision/Response/Contact/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,10 @@ set(SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR "src/sofa/component/collisi
set(HEADER_FILES
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/config.h.in
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/init.h
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.h
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.inl
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BaseUnilateralContactResponse.h
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BaseUnilateralContactResponse.inl
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.h
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.inl
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricStickContact.h
Expand All @@ -26,6 +30,7 @@ set(HEADER_FILES

set(SOURCE_FILES
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/init.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricStickContact.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/CollisionResponse.cpp
Expand All @@ -36,6 +41,7 @@ set(SOURCE_FILES
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/RayContact.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/RuleBasedContactManager.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/StickContactConstraint.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronAugmentedLagrangianContact.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronBarycentricPenalityContact.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronFrictionContact.cpp
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronRayContact.cpp
Expand Down
85 changes: 85 additions & 0 deletions ...nse/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
/******************************************************************************
* SOFA, Simulation Open-Framework Architecture *
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
* *
* This program is free software; you can redistribute it and/or modify it *
* under the terms of the GNU Lesser General Public License as published by *
* the Free Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: [email protected] *
******************************************************************************/
#include <sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.inl>
#include <sofa/component/collision/response/contact/AugmentedLagrangianResponse.inl>
#include <sofa/component/collision/response/contact/BaseUnilateralContactResponse.inl>

#include <sofa/component/collision/response/mapper/RigidContactMapper.inl>
#include <sofa/component/collision/response/mapper/BarycentricContactMapper.inl>

namespace sofa::component::collision::response::contact
{

using namespace defaulttype;
using namespace sofa::helper;
using namespace sofa::component::collision::geometry;
using simulation::Node;

Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > PointPointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > LineSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > LinePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>> > LineLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > TrianglePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleTriangleAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > SphereSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > SpherePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<RigidSphereModel, RigidSphereModel> > RigidSphereRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > SphereRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > LineRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > TriangleRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>> > RigidSpherePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);

template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<RigidSphereModel, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;

template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<RigidSphereModel, RigidSphereModel>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>>;

} //namespace sofa::component::collision::response::contact
67 changes: 67 additions & 0 deletions ...ponse/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,67 @@
/******************************************************************************
* SOFA, Simulation Open-Framework Architecture *
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
* *
* This program is free software; you can redistribute it and/or modify it *
* under the terms of the GNU Lesser General Public License as published by *
* the Free Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: [email protected] *
******************************************************************************/
#pragma once
#include <sofa/component/collision/response/contact/config.h>

#include <sofa/helper/Factory.h>
#include <sofa/core/collision/Contact.h>
#include <sofa/core/collision/Intersection.h>
#include <sofa/core/behavior/MechanicalState.h>
#include <sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.h>
#include <sofa/component/collision/response/contact/BaseUnilateralContactResponse.h>
#include <sofa/component/collision/response/contact/ContactIdentifier.h>

#include <sofa/core/objectmodel/RenamedData.h>

namespace sofa::component::collision::response::contact
{
//Work inspired by: Zimmerman BK, Ateshian GA. "A Surface-to-Surface Finite Element Algorithm for Large Deformation Frictional Contact in febio."
// J Biomech Eng. 2018 Aug 1;140(8):0810131–08101315. doi: 10.1115/1.4040497. PMID: 30003262; PMCID: PMC6056201.
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes = sofa::defaulttype::Vec3Types >
class AugmentedLagrangianResponse : public BaseUnilateralContactResponse<TCollisionModel1, TCollisionModel2,constraint::lagrangian::model::AugmentedLagrangianContactParameters, ResponseDataTypes>
{
public:
SOFA_CLASS(SOFA_TEMPLATE3(AugmentedLagrangianResponse, TCollisionModel1, TCollisionModel2, ResponseDataTypes), SOFA_TEMPLATE4(BaseUnilateralContactResponse, TCollisionModel1, TCollisionModel2,constraint::lagrangian::model::AugmentedLagrangianContactParameters, ResponseDataTypes));

typedef typename Inherit1::DataTypes1 DataTypes1;
typedef typename Inherit1::DataTypes2 DataTypes2;
typedef typename Inherit1::CollisionModel1 CollisionModel1;
typedef typename Inherit1::CollisionModel2 CollisionModel2;
typedef typename Inherit1::Intersection Intersection;

typedef core::behavior::MechanicalState<DataTypes1> MechanicalState1;
typedef core::behavior::MechanicalState<DataTypes2> MechanicalState2;

Data<SReal> d_mu; ///< friction parameter
Data<SReal> d_epsilon; ///< Penalty parameter

AugmentedLagrangianResponse();
AugmentedLagrangianResponse(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod);

virtual ~AugmentedLagrangianResponse() = default;

virtual constraint::lagrangian::model::AugmentedLagrangianContactParameters getParameterFromDatas() const override;
virtual void setupConstraint(MechanicalState1 *,MechanicalState2 *) override;

};

} // namespace sofa::component::collision::response::contact
Loading

0 comments on commit 4eda9c7

Please sign in to comment.