Skip to content

Commit

Permalink
[Constraint.Lagrangian] Add fixed lagrangian constraint (#4646)
Browse files Browse the repository at this point in the history 
* ADD FixedLagrangianConstraint, still need to test and complete the rigid implementation

* Add example

* Finalize rigids and add example scenes

* clean

* Add most of alxbilger comments

* Alxbilger comments part2

* Change beam solver and vector

* Add missing call to super init in Constraint class + component state setting

* Add requested templates
  • Loading branch information
@bakpaul
bakpaul committed May 8, 2024
1 parent 7dfb651 commit 76f5b5d
Show file tree
Hide file tree
Showing 10 changed files with 588 additions and 0 deletions.
3 changes: 3 additions & 0 deletions Sofa/Component/Constraint/Lagrangian/Model/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@ set(HEADER_FILES

${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralLagrangianConstraint.h
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralLagrangianConstraint.inl
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/FixedLagrangianConstraint.h
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/FixedLagrangianConstraint.inl
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/SlidingLagrangianConstraint.h
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/SlidingLagrangianConstraint.inl
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/StopperLagrangianConstraint.h
Expand All @@ -34,6 +36,7 @@ set(HEADER_FILES
set(SOURCE_FILES
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/init.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/BilateralLagrangianConstraint.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/FixedLagrangianConstraint.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/SlidingLagrangianConstraint.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/StopperLagrangianConstraint.cpp
${SOFACOMPONENTCONSTRAINTLAGRANGIANMODEL_SOURCE_DIR}/UniformLagrangianConstraint.cpp
Expand Down
253 changes: 253 additions & 0 deletions ...angian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,253 @@
/******************************************************************************
* 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: contact@sofa-framework.org *
******************************************************************************/
#define SOFA_COMPONENT_CONSTRAINTSET_FIXEDLAGRANGIANCONSTRAINT_CPP
#include <sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.inl>

#include <sofa/defaulttype/VecTypes.h>
#include <sofa/core/ObjectFactory.h>
#include <sofa/component/constraint/lagrangian/model/BilateralConstraintResolution.h>

namespace sofa::component::constraint::lagrangian::model
{

using namespace sofa::defaulttype;
using namespace sofa::helper;

// Vec6 specialization
template <>
void FixedLagrangianConstraint< Vec6Types >::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &c, unsigned int lineNumber)
{
constexpr Coord c0(1,0,0,0,0,0), c1(0,1,0,0,0,0), c2(0,0,1,0,0,0), c3(0,0,0,1,0,0), c4(0,0,0,0,1,0), c5(0,0,0,0,0,1);
const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];

MatrixDerivRowIterator c_it = c->writeLine(m_cid[lineNumber]);
c_it.setCol(dofIdx, c0);

c_it = c->writeLine(m_cid[lineNumber] + 1);
c_it.setCol(dofIdx, c1);

c_it = c->writeLine(m_cid[lineNumber] + 2);
c_it.setCol(dofIdx, c2);

c_it = c->writeLine(m_cid[lineNumber] + 3);
c_it.setCol(dofIdx, c3);

c_it = c->writeLine(m_cid[lineNumber] + 4);
c_it.setCol(dofIdx, c4);

c_it = c->writeLine(m_cid[lineNumber] + 5);
c_it.setCol(dofIdx, c5);


}

template<>
void FixedLagrangianConstraint<Vec6Types>::doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePos, const DataVecCoord * restPos,unsigned int lineNumber)
{
unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];
const Coord dfree = freePos->getValue()[dofIdx] - restPos->getValue()[dofIdx];

resV->set(m_cid[lineNumber] , dfree[0]);
resV->set(m_cid[lineNumber]+1, dfree[1]);
resV->set(m_cid[lineNumber]+2, dfree[2]);
resV->set(m_cid[lineNumber]+3, dfree[3]);
resV->set(m_cid[lineNumber]+4, dfree[4]);
resV->set(m_cid[lineNumber]+5, dfree[5]);

}

template<>
void FixedLagrangianConstraint<Vec6Types>::doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber)
{
resTab[offset] = new BilateralConstraintResolutionNDof(6);
offset += 6;
}

// Vec3 specialization
template <>
void FixedLagrangianConstraint< Vec3Types >::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &c, unsigned int lineNumber)
{
constexpr Coord cx(1,0,0), cy(0,1,0), cz(0,0,1);
const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];

MatrixDerivRowIterator c_it = c->writeLine(m_cid[lineNumber]);
c_it.setCol(dofIdx, cx);

c_it = c->writeLine(m_cid[lineNumber] + 1);
c_it.setCol(dofIdx, cy);

c_it = c->writeLine(m_cid[lineNumber] + 2);
c_it.setCol(dofIdx, cz);

}

template<>
void FixedLagrangianConstraint<Vec3Types>::doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePos, const DataVecCoord * restPos,unsigned int lineNumber)
{
unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];
const Coord dfree = freePos->getValue()[dofIdx] - restPos->getValue()[dofIdx];

resV->set(m_cid[lineNumber] , dfree[0]);
resV->set(m_cid[lineNumber]+1, dfree[1]);
resV->set(m_cid[lineNumber]+2, dfree[2]);

}

template<>
void FixedLagrangianConstraint<Vec3Types>::doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber)
{
resTab[offset] = new BilateralConstraintResolution3Dof(&m_prevForces[lineNumber]);
offset += 3;
}


// Vec2 specialization
template <>
void FixedLagrangianConstraint< Vec2Types >::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &c, unsigned int lineNumber)
{
constexpr Coord cx(1, 0), cy(0, 1);
const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];

MatrixDerivRowIterator c_it = c->writeLine(m_cid[lineNumber]);
c_it.setCol(dofIdx, cx);

c_it = c->writeLine(m_cid[lineNumber] + 1);
c_it.setCol(dofIdx, cy);
}

template<>
void FixedLagrangianConstraint<Vec2Types>::doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePos, const DataVecCoord * restPos,unsigned int lineNumber)
{
unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];
const Coord dfree = freePos->getValue()[dofIdx] - restPos->getValue()[dofIdx];

resV->set(m_cid[lineNumber] , dfree[0]);
resV->set(m_cid[lineNumber]+1, dfree[1]);

}

template<>
void FixedLagrangianConstraint<Vec2Types>::doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber)
{
resTab[offset] = new BilateralConstraintResolutionNDof(2);
offset += 2;
}

// Vec1 specialization
template <>
void FixedLagrangianConstraint< Vec1Types >::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &c, unsigned int lineNumber)
{
constexpr Coord cx(1);
const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];

MatrixDerivRowIterator c_it = c->writeLine(m_cid[lineNumber]);
c_it.setCol(dofIdx, cx);
}

template<>
void FixedLagrangianConstraint<Vec1Types>::doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePos, const DataVecCoord * restPos,unsigned int lineNumber)
{
unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];
const Coord dfree = freePos->getValue()[dofIdx] - restPos->getValue()[dofIdx];

resV->set(m_cid[lineNumber] , dfree[0]);
}

template<>
void FixedLagrangianConstraint<Vec1Types>::doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber)
{
resTab[offset] = new BilateralConstraintResolutionNDof(1);
offset += 1;
}

// Rigid3 specialization
template<>
void FixedLagrangianConstraint<Rigid3Types>::doBuildConstraintLine( helper::WriteAccessor<DataMatrixDeriv> &c, unsigned int lineNumber)
{
constexpr type::Vec3 cx(1,0,0), cy(0,1,0), cz(0,0,1), zero(0,0,0);
const unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];

MatrixDerivRowIterator c_it = c->writeLine(m_cid[lineNumber]);
c_it.setCol(dofIdx, Deriv(cx, zero));

c_it = c->writeLine(m_cid[lineNumber] + 1);
c_it.setCol(dofIdx, Deriv(cy, zero));

c_it = c->writeLine(m_cid[lineNumber] + 2);
c_it.setCol(dofIdx, Deriv(cz, zero));

c_it = c->writeLine(m_cid[lineNumber] + 3);
c_it.setCol(dofIdx, Deriv(zero, cx));

c_it = c->writeLine(m_cid[lineNumber] + 4);
c_it.setCol(dofIdx, Deriv(zero, cy));

c_it = c->writeLine(m_cid[lineNumber] + 5);
c_it.setCol(dofIdx, Deriv(zero, cz));

}

template<>
void FixedLagrangianConstraint<Rigid3Types>::doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePose, const DataVecCoord * restPose,unsigned int lineNumber)
{
unsigned dofIdx = d_fixAll.getValue() ? lineNumber : d_indices.getValue()[lineNumber];


const sofa::type::Vec3 pfree = (freePose->getValue()[dofIdx].getCenter() - restPose->getValue()[dofIdx].getCenter());
const sofa::type::Vec3 ofree = ( freePose->getValue()[dofIdx].getOrientation()*restPose->getValue()[dofIdx].getOrientation().inverse()).toEulerVector();


resV->set(m_cid[lineNumber] , pfree[0]);
resV->set(m_cid[lineNumber]+1, pfree[1]);
resV->set(m_cid[lineNumber]+2, pfree[2]);
resV->set(m_cid[lineNumber]+3, ofree[0]);
resV->set(m_cid[lineNumber]+4, ofree[1]);
resV->set(m_cid[lineNumber]+5, ofree[2]);

}

template<>
void FixedLagrangianConstraint<Rigid3Types>::doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber)
{
resTab[offset] = new BilateralConstraintResolution3Dof(&getVCenter(m_prevForces[lineNumber]));
offset += 3;
resTab[offset] = new BilateralConstraintResolution3Dof(&getVOrientation(m_prevForces[lineNumber]));
offset += 3;
}



int FixedLagrangianConstraintClass = core::RegisterObject("Lagrangian-based fixation of DOFs of the model")
.add< FixedLagrangianConstraint<Vec6Types> >()
.add< FixedLagrangianConstraint<Vec3Types> >()
.add< FixedLagrangianConstraint<Vec2Types> >()
.add< FixedLagrangianConstraint<Vec1Types> >()
.add< FixedLagrangianConstraint<Rigid3Types> >();

template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Vec6Types>;
template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Vec3Types>;
template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Vec2Types>;
template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Vec1Types>;
template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint<Rigid3Types>;


} //namespace sofa::component::constraint::lagrangian::model
96 changes: 96 additions & 0 deletions ...grangian/Model/src/sofa/component/constraint/lagrangian/model/FixedLagrangianConstraint.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
/******************************************************************************
* 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: contact@sofa-framework.org *
******************************************************************************/
#pragma once
#include <sofa/component/constraint/lagrangian/model/config.h>

#include <sofa/core/behavior/Constraint.h>
#include <sofa/core/behavior/ConstraintResolution.h>
#include <sofa/core/behavior/MechanicalState.h>
#include <sofa/core/behavior/OdeSolver.h>
#include <sofa/core/topology/TopologySubsetIndices.h>

namespace sofa::component::constraint::lagrangian::model
{

template< class DataTypes >
class FixedLagrangianConstraint : public core::behavior::Constraint<DataTypes>
{
public:
SOFA_CLASS(SOFA_TEMPLATE(FixedLagrangianConstraint,DataTypes), SOFA_TEMPLATE(core::behavior::Constraint,DataTypes));

typedef typename DataTypes::VecCoord VecCoord;
typedef typename DataTypes::VecDeriv VecDeriv;
typedef typename DataTypes::Coord Coord;
typedef typename DataTypes::Deriv Deriv;
typedef typename DataTypes::MatrixDeriv MatrixDeriv;
typedef typename core::behavior::MechanicalState<DataTypes> MechanicalState;
typedef typename core::behavior::Constraint<DataTypes> Inherit;

typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator;
typedef core::objectmodel::Data<VecCoord> DataVecCoord;
typedef core::objectmodel::Data<VecDeriv> DataVecDeriv;
typedef core::objectmodel::Data<MatrixDeriv> DataMatrixDeriv;

typedef sofa::core::topology::TopologySubsetIndices SetIndex;

protected:

sofa::type::vector<unsigned int> m_cid;
sofa::type::vector<Deriv> m_prevForces;

SetIndex d_indices;
Data<bool> d_fixAll;

FixedLagrangianConstraint(MechanicalState* object = nullptr);
virtual ~FixedLagrangianConstraint() {}

virtual type::vector<std::string> getConstraintIdentifiers() override final
{
type::vector<std::string> ids = getFixedIdentifiers();
ids.push_back("Bilateral");
return ids;
}

virtual type::vector<std::string> getFixedIdentifiers(){ return {}; }

public:
void init() override;
void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c_d, unsigned int &cIndex, const DataVecCoord &x) override;
void getConstraintViolation(const core::ConstraintParams* cParams, linearalgebra::BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) override;
void getConstraintResolution(const core::ConstraintParams *, std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset) override;

private:
void doBuildConstraintLine(helper::WriteAccessor<DataMatrixDeriv> &c, unsigned int lineNumber);
void doGetSingleConstraintViolation(linearalgebra::BaseVector *resV, const DataVecCoord * freePos, const DataVecCoord * restPos,unsigned int lineNumber);
void doGetSingleConstraintResolution(std::vector<core::behavior::ConstraintResolution*>& resTab, unsigned int& offset, unsigned int lineNumber);

};

#if !defined(SOFA_COMPONENT_CONSTRAINTSET_FIXEDLAGRANGIANCONSTRAINT_CPP)
extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Vec6Types >;
extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Vec3Types >;
extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Vec2Types >;
extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Vec1Types >;
extern template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API FixedLagrangianConstraint< defaulttype::Rigid3Types >;
#endif

} //namespace sofa::component::constraint::lagrangian::model

0 comments on commit 76f5b5d

Please sign in to comment.