Skip to content

Commit 4eda9c7

Browse files
bakpaulhugtalbot
bakpaul
and
hugtalbot
authored
[Constraint.Lagrangian] Add augmented lagrangians (#5003)
* 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 <hugo.talbot@sofa-framework.org> * Add reference --------- Co-authored-by: Hugo <hugo.talbot@sofa-framework.org>
1 parent 8ba12f0 commit 4eda9c7

26 files changed

+1749
-731
lines changed

‎Sofa/Component/Collision/Response/Contact/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,10 @@ set(SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR "src/sofa/component/collisi
66
set(HEADER_FILES
77
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/config.h.in
88
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/init.h
9+
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.h
10+
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.inl
11+
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BaseUnilateralContactResponse.h
12+
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BaseUnilateralContactResponse.inl
913
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.h
1014
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.inl
1115
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricStickContact.h
@@ -26,6 +30,7 @@ set(HEADER_FILES
2630

2731
set(SOURCE_FILES
2832
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/init.cpp
33+
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/AugmentedLagrangianResponse.cpp
2934
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricPenalityContact.cpp
3035
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/BarycentricStickContact.cpp
3136
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/CollisionResponse.cpp
@@ -36,6 +41,7 @@ set(SOURCE_FILES
3641
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/RayContact.cpp
3742
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/RuleBasedContactManager.cpp
3843
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/StickContactConstraint.cpp
44+
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronAugmentedLagrangianContact.cpp
3945
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronBarycentricPenalityContact.cpp
4046
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronFrictionContact.cpp
4147
${SOFACOMPONENTCOLLISIONRESPONSECONTACT_SOURCE_DIR}/TetrahedronRayContact.cpp

‎Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.cpp‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
/******************************************************************************
2+
* SOFA, Simulation Open-Framework Architecture *
3+
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Authors: The SOFA Team and external contributors (see Authors.txt) *
19+
* *
20+
* Contact information: contact@sofa-framework.org *
21+
******************************************************************************/
22+
#include <sofa/component/constraint/lagrangian/model/BaseContactLagrangianConstraint.inl>
23+
#include <sofa/component/collision/response/contact/AugmentedLagrangianResponse.inl>
24+
#include <sofa/component/collision/response/contact/BaseUnilateralContactResponse.inl>
25+
26+
#include <sofa/component/collision/response/mapper/RigidContactMapper.inl>
27+
#include <sofa/component/collision/response/mapper/BarycentricContactMapper.inl>
28+
29+
namespace sofa::component::collision::response::contact
30+
{
31+
32+
using namespace defaulttype;
33+
using namespace sofa::helper;
34+
using namespace sofa::component::collision::geometry;
35+
using simulation::Node;
36+
37+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > PointPointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
38+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > LineSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
39+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > LinePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
40+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>> > LineLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
41+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
42+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > TrianglePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
43+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleLineAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
44+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>> > TriangleTriangleAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
45+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>> > SphereSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
46+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>> > SpherePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
47+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<RigidSphereModel, RigidSphereModel> > RigidSphereRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
48+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > SphereRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
49+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > LineRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
50+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel> > TriangleRigidSphereAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
51+
Creator<sofa::core::collision::Contact::Factory, AugmentedLagrangianResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>> > RigidSpherePointAugmentedLagrangianResponseClass("AugmentedLagrangianResponseConstraint",true);
52+
53+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
54+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
55+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
56+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
57+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
58+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
59+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
60+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
61+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
62+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
63+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<RigidSphereModel, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
64+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
65+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
66+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
67+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API BaseUnilateralContactResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>,constraint::lagrangian::model::AugmentedLagrangianContactParameters>;
68+
69+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<PointCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
70+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
71+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
72+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>>;
73+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
74+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
75+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, LineCollisionModel<sofa::defaulttype::Vec3Types>>;
76+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, TriangleCollisionModel<sofa::defaulttype::Vec3Types>>;
77+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, SphereCollisionModel<sofa::defaulttype::Vec3Types>>;
78+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
79+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<RigidSphereModel, RigidSphereModel>;
80+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<SphereCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
81+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<LineCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
82+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<TriangleCollisionModel<sofa::defaulttype::Vec3Types>, RigidSphereModel>;
83+
template class SOFA_COMPONENT_COLLISION_RESPONSE_CONTACT_API AugmentedLagrangianResponse<RigidSphereModel, PointCollisionModel<sofa::defaulttype::Vec3Types>>;
84+
85+
} //namespace sofa::component::collision::response::contact

‎Sofa/Component/Collision/Response/Contact/src/sofa/component/collision/response/contact/AugmentedLagrangianResponse.h‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
/******************************************************************************
2+
* SOFA, Simulation Open-Framework Architecture *
3+
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Authors: The SOFA Team and external contributors (see Authors.txt) *
19+
* *
20+
* Contact information: contact@sofa-framework.org *
21+
******************************************************************************/
22+
#pragma once
23+
#include <sofa/component/collision/response/contact/config.h>
24+
25+
#include <sofa/helper/Factory.h>
26+
#include <sofa/core/collision/Contact.h>
27+
#include <sofa/core/collision/Intersection.h>
28+
#include <sofa/core/behavior/MechanicalState.h>
29+
#include <sofa/component/constraint/lagrangian/model/AugmentedLagrangianConstraint.h>
30+
#include <sofa/component/collision/response/contact/BaseUnilateralContactResponse.h>
31+
#include <sofa/component/collision/response/contact/ContactIdentifier.h>
32+
33+
#include <sofa/core/objectmodel/RenamedData.h>
34+
35+
namespace sofa::component::collision::response::contact
36+
{
37+
//Work inspired by: Zimmerman BK, Ateshian GA. "A Surface-to-Surface Finite Element Algorithm for Large Deformation Frictional Contact in febio."
38+
// J Biomech Eng. 2018 Aug 1;140(8):0810131–08101315. doi: 10.1115/1.4040497. PMID: 30003262; PMCID: PMC6056201.
39+
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes = sofa::defaulttype::Vec3Types >
40+
class AugmentedLagrangianResponse : public BaseUnilateralContactResponse<TCollisionModel1, TCollisionModel2,constraint::lagrangian::model::AugmentedLagrangianContactParameters, ResponseDataTypes>
41+
{
42+
public:
43+
SOFA_CLASS(SOFA_TEMPLATE3(AugmentedLagrangianResponse, TCollisionModel1, TCollisionModel2, ResponseDataTypes), SOFA_TEMPLATE4(BaseUnilateralContactResponse, TCollisionModel1, TCollisionModel2,constraint::lagrangian::model::AugmentedLagrangianContactParameters, ResponseDataTypes));
44+
45+
typedef typename Inherit1::DataTypes1 DataTypes1;
46+
typedef typename Inherit1::DataTypes2 DataTypes2;
47+
typedef typename Inherit1::CollisionModel1 CollisionModel1;
48+
typedef typename Inherit1::CollisionModel2 CollisionModel2;
49+
typedef typename Inherit1::Intersection Intersection;
50+
51+
typedef core::behavior::MechanicalState<DataTypes1> MechanicalState1;
52+
typedef core::behavior::MechanicalState<DataTypes2> MechanicalState2;
53+
54+
Data<SReal> d_mu; ///< friction parameter
55+
Data<SReal> d_epsilon; ///< Penalty parameter
56+
57+
AugmentedLagrangianResponse();
58+
AugmentedLagrangianResponse(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod);
59+
60+
virtual ~AugmentedLagrangianResponse() = default;
61+
62+
virtual constraint::lagrangian::model::AugmentedLagrangianContactParameters getParameterFromDatas() const override;
63+
virtual void setupConstraint(MechanicalState1 *,MechanicalState2 *) override;
64+
65+
};
66+
67+
} // namespace sofa::component::collision::response::contact

0 commit comments

Comments
 (0)