Skip to content

Commit

Permalink
[Lagrangian.Correction] Add callbacks to check zero compliance (#4205)
Browse files Browse the repository at this point in the history 
* Create one single callback checking non-zero values for compliance

* add info message in rigid case

* test compliance vector only if non-rigid using boiler plate snippet

* move in a dedicated file the boiler plate code

* fix compil

* add checks for zero compliance vector in case of rigid body

* minor formatting

* Add comment on rigid compliance vector

* Update Sofa/Component/Constraint/Lagrangian/Correction/src/sofa/component/constraint/lagrangian/correction/UncoupledConstraintCorrection.inl

---------

Co-authored-by: Paul Baksic <30337881+bakpaul@users.noreply.github.com>
  • Loading branch information
@hugtalbot @bakpaul
hugtalbot and bakpaul committed Jan 10, 2024
1 parent 821f8ed commit 3af47db
Show file tree
Hide file tree
Showing 5 changed files with 132 additions and 31 deletions.
3 changes: 2 additions & 1 deletion ...ion/src/sofa/component/constraint/lagrangian/correction/UncoupledConstraintCorrection.cpp
View file
Expand Up @@ -89,7 +89,6 @@ SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_CORRECTION_API void UncoupledConstraintCorr
massValue = um->getVertexMass();
usedComp.push_back(odeFactor / massValue.mass);
msg_info() << "Compliance matrix is evaluated using the UniformMass";

}
else
{
Expand All @@ -109,6 +108,8 @@ SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_CORRECTION_API void UncoupledConstraintCorr
usedComp.push_back( odeFactor * massValue.invInertiaMassMatrix[1][2]);
usedComp.push_back( odeFactor * massValue.invInertiaMassMatrix[2][2]);
compliance.setValue(usedComp);

msg_info() << "\'compliance\' equals: " << compliance.getValue();
}
else
{
Expand Down
93 changes: 92 additions & 1 deletion ...ion/src/sofa/component/constraint/lagrangian/correction/UncoupledConstraintCorrection.inl
View file
Expand Up @@ -28,6 +28,7 @@
#include <sofa/linearalgebra/BaseMatrix.h>
#include <sofa/core/topology/TopologyData.inl>
#include <sofa/core/ConstraintParams.h>
#include <sofa/type/isRigidType.h>

namespace sofa::component::constraint::lagrangian::correction
{
Expand Down Expand Up @@ -110,6 +111,68 @@ UncoupledConstraintCorrection<DataTypes>::UncoupledConstraintCorrection(sofa::co
, l_topology(initLink("topology", "link to the topology container"))
, m_pOdeSolver(nullptr)
{
// Check defaultCompliance and entries of the compliance vector are not zero
core::objectmodel::Base::addUpdateCallback("checkNonZeroComplianceInput", {&defaultCompliance, &compliance}, [this](const core::DataTracker& t)
{
// Update of the defaultCompliance data
if(t.hasChanged(defaultCompliance))
{
if(defaultCompliance.getValue() == 0.0)
{
msg_error() << "Zero defaultCompliance is set: this will cause the constraint resolution to diverge";
return sofa::core::objectmodel::ComponentState::Invalid;
}
return sofa::core::objectmodel::ComponentState::Valid;
}
// Update of the compliance data
else
{
// Case: soft body
if constexpr (!sofa::type::isRigidType<DataTypes>())
{
const VecReal &comp = compliance.getValue();
if (std::any_of(comp.begin(), comp.end(), [](const Real c) { return c == 0; }))
{
msg_error() << "Zero values set in the compliance vector: this will cause the constraint resolution to diverge";
return sofa::core::objectmodel::ComponentState::Invalid;
}
}
// Case: rigid body
else
{
const VecReal &comp = compliance.getValue();
sofa::Size compSize = comp.size();

if (compSize % 7 != 0)
{
msg_error() << "Compliance vector should be a multiple of 7 in rigid case (1 for translation dofs, and 6 for the rotation matrix)";
return sofa::core::objectmodel::ComponentState::Invalid;
}

for(auto i = 0; i < comp.size() ; i += 7)
{
if(comp[i] == 0.)
{
msg_error() << "Zero compliance set on translation dofs: this will cause the constraint resolution to diverge (compliance[" << i << "])";
return sofa::core::objectmodel::ComponentState::Invalid;
}
// Check if the translational compliance and the diagonal values of the rotation compliance matrix are non zero
// In Rigid case, the inertia matrix generates this 3x3 rotation compliance matrix
// In the compliance vector comp, SOFA stores:
// - the translational compliance (comp[0])
// - the triangular part of the rotation compliance matrix: r[0,0]=comp[1],r[0,1],r[0,2],r[1,1]=comp[4],r[1,2],r[2,2]=comp[6]
if(comp[i+1] == 0. || comp[i+4] == 0. || comp[i+6] == 0.)
{
msg_error() << "Zero compliance set on rotation dofs (matrix diagonal): this will cause the constraint resolution to diverge (compliance[" << i << "])";
return sofa::core::objectmodel::ComponentState::Invalid;
}
}
}
return sofa::core::objectmodel::ComponentState::Valid;
}

}, {}
);
}

template<class DataTypes>
Expand All @@ -123,7 +186,7 @@ void UncoupledConstraintCorrection<DataTypes>::init()
{
Inherit::init();

if( !defaultCompliance.isSet() && !compliance.isSet() )
if (!defaultCompliance.isSet() && !compliance.isSet())
{
msg_warning() << "Neither the \'defaultCompliance\' nor the \'compliance\' data is set, please set one to define your compliance matrix";
}
Expand Down Expand Up @@ -205,6 +268,8 @@ void UncoupledConstraintCorrection<DataTypes>::init()
}
d_useOdeSolverIntegrationFactors.setReadOnly(true);
}

this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid);
}

template <class DataTypes>
Expand All @@ -216,6 +281,9 @@ void UncoupledConstraintCorrection<DataTypes>::reinit()
template<class DataTypes>
void UncoupledConstraintCorrection<DataTypes>::getComplianceWithConstraintMerge(linearalgebra::BaseMatrix* Wmerged, std::vector<int> &constraint_merge)
{
if(!this->isComponentStateValid())
return;

helper::WriteAccessor<Data<MatrixDeriv> > constraintsData = *this->mstate->write(core::MatrixDerivId::constraintJacobian());
MatrixDeriv& constraints = constraintsData.wref();

Expand Down Expand Up @@ -286,6 +354,9 @@ void UncoupledConstraintCorrection<DataTypes>::getComplianceWithConstraintMerge(
template<class DataTypes>
void UncoupledConstraintCorrection<DataTypes>::addComplianceInConstraintSpace(const sofa::core::ConstraintParams * cparams, sofa::linearalgebra::BaseMatrix *W)
{
if(!this->isComponentStateValid())
return;

const MatrixDeriv& constraints = cparams->readJ(this->mstate)->getValue() ;
VecReal comp = compliance.getValue();
Real comp0 = defaultCompliance.getValue();
Expand Down Expand Up @@ -402,6 +473,9 @@ void UncoupledConstraintCorrection<DataTypes>::addComplianceInConstraintSpace(co
template<class DataTypes>
void UncoupledConstraintCorrection<DataTypes>::getComplianceMatrix(linearalgebra::BaseMatrix *m) const
{
if(!this->isComponentStateValid())
return;

const VecReal& comp = compliance.getValue();
const Real comp0 = defaultCompliance.getValue();
const unsigned int s = this->mstate->getSize(); // comp.size();
Expand Down Expand Up @@ -438,6 +512,9 @@ void UncoupledConstraintCorrection<DataTypes>::computeMotionCorrection(const cor
{
SOFA_UNUSED(cparams);

if(!this->isComponentStateValid())
return;

auto writeDx = sofa::helper::getWriteAccessor( *dx[this->getMState()].write() );
const Data<VecDeriv>& f_d = *f[this->getMState()].read();
computeDx(f_d, writeDx.wref());
Expand All @@ -447,6 +524,8 @@ void UncoupledConstraintCorrection<DataTypes>::computeMotionCorrection(const cor
template<class DataTypes>
void UncoupledConstraintCorrection<DataTypes>::applyMotionCorrection(const core::ConstraintParams *cparams, Data< VecCoord > &x_d, Data< VecDeriv > &v_d, Data<VecDeriv>& dx_d, const Data< VecDeriv > &correction_d)
{
if(!this->isComponentStateValid())
return;

auto dx = sofa::helper::getWriteAccessor(dx_d);
auto correction = sofa::helper::getReadAccessor(correction_d);
Expand Down Expand Up @@ -479,6 +558,9 @@ void UncoupledConstraintCorrection<DataTypes>::applyMotionCorrection(const core:
template<class DataTypes>
void UncoupledConstraintCorrection<DataTypes>::applyPositionCorrection(const core::ConstraintParams *cparams, Data< VecCoord > &x_d, Data< VecDeriv >& dx_d, const Data< VecDeriv > &correction_d)
{
if(!this->isComponentStateValid())
return;

auto dx = sofa::helper::getWriteAccessor(dx_d);
auto correction = sofa::helper::getReadAccessor(correction_d);

Expand All @@ -504,6 +586,9 @@ void UncoupledConstraintCorrection<DataTypes>::applyPositionCorrection(const cor
template<class DataTypes>
void UncoupledConstraintCorrection<DataTypes>::applyVelocityCorrection(const core::ConstraintParams *cparams, Data< VecDeriv > &v_d, Data<VecDeriv>& dv_d, const Data< VecDeriv > &correction_d)
{
if(!this->isComponentStateValid())
return;

auto dx = sofa::helper::getWriteAccessor(dv_d);
auto correction = sofa::helper::getReadAccessor(correction_d);

Expand All @@ -529,6 +614,9 @@ void UncoupledConstraintCorrection<DataTypes>::applyVelocityCorrection(const cor
template<class DataTypes>
void UncoupledConstraintCorrection<DataTypes>::applyContactForce(const linearalgebra::BaseVector *f)
{
if(!this->isComponentStateValid())
return;

helper::WriteAccessor<Data<VecDeriv> > forceData = *this->mstate->write(core::VecDerivId::externalForce());
VecDeriv& force = forceData.wref();
const MatrixDeriv& constraints = this->mstate->read(core::ConstMatrixDerivId::constraintJacobian())->getValue();
Expand Down Expand Up @@ -657,6 +745,9 @@ void UncoupledConstraintCorrection<DataTypes>::addConstraintDisplacement(SReal *
/// constraint_force contains the force applied on dof involved with the contact
/// TODO : compute a constraint_disp that is updated each time a new force is provided !

if(!this->isComponentStateValid())
return;

const MatrixDeriv& constraints = this->mstate->read(core::ConstMatrixDerivId::constraintJacobian())->getValue();

for (int id = begin; id <= end; id++)
Expand Down
35 changes: 6 additions & 29 deletions ...dMechanics/Spring/src/sofa/component/solidmechanics/spring/RestShapeSpringsForceField.inl
View file
Expand Up @@ -28,34 +28,11 @@
#include <sofa/defaulttype/RigidTypes.h>
#include <sofa/type/RGBAColor.h>
#include <sofa/core/behavior/BaseLocalForceFieldMatrix.h>
#include <sofa/type/isRigidType.h>

#include <string_view>
#include <type_traits>

namespace // anonymous
{
// Boiler-plate code to test if a type implements a method
// explanation https://stackoverflow.com/a/30848101

template <typename...>
using void_t = void;

// Primary template handles all types not supporting the operation.
template <typename, template <typename> class, typename = void_t<>>
struct detect : std::false_type {};

// Specialization recognizes/validates only types supporting the archetype.
template <typename T, template <typename> class Op>
struct detect<T, Op, void_t<Op<T>>> : std::true_type {};

// Actual test if DataType::Coord implements getOrientation() (hence is a RigidType)
template <typename T>
using isRigid_t = decltype(std::declval<typename T::Coord>().getOrientation());

template <typename T>
using isRigidType = detect<T, isRigid_t>;
} // anonymous

namespace sofa::component::solidmechanics::spring
{

Expand Down Expand Up @@ -170,7 +147,7 @@ void RestShapeSpringsForceField<DataTypes>::bwdInit()
/// Compile time condition to check if we are working with a Rigid3Types or a type that does not
/// need the Angular Stiffness parameters.
//if constexpr (isRigid())
if constexpr (isRigidType<DataTypes>())
if constexpr (sofa::type::isRigidType<DataTypes>())
{
sofa::helper::ReadAccessor<Data<VecReal>> s = d_stiffness;
sofa::helper::WriteOnlyAccessor<Data<VecReal>> as = d_angularStiffness;
Expand Down Expand Up @@ -392,7 +369,7 @@ void RestShapeSpringsForceField<DataTypes>::addForce(const MechanicalParams* mp
const auto activeDirections = d_activeDirections.getValue();

// rigid case
if constexpr (isRigidType<DataTypes>())
if constexpr (sofa::type::isRigidType<DataTypes>())
{
// translation
if (i >= m_pivots.size())
Expand Down Expand Up @@ -475,7 +452,7 @@ void RestShapeSpringsForceField<DataTypes>::addDForce(const MechanicalParams* mp
const sofa::Index curIndex = m_indices[i];
const auto stiffness = k[static_cast<std::size_t>(i < k.size()) * i];

if constexpr (isRigidType<DataTypes>())
if constexpr (sofa::type::isRigidType<DataTypes>())
{
const auto angularStiffness = k_a[static_cast<std::size_t>(i < k_a.size()) * i];

Expand Down Expand Up @@ -597,7 +574,7 @@ void RestShapeSpringsForceField<DataTypes>::addKToMatrix(const MechanicalParams*
}

// rotation (if applicable)
if constexpr (isRigidType<DataTypes>())
if constexpr (sofa::type::isRigidType<DataTypes>())
{
const auto vr = -kFact * k_a[(index < k_a.size()) * index];
for (sofa::Size i = space_size; i < total_size; i++)
Expand Down Expand Up @@ -634,7 +611,7 @@ void RestShapeSpringsForceField<DataTypes>::buildStiffnessMatrix(core::behavior:
}

// rotation (if applicable)
if constexpr (isRigidType<DataTypes>())
if constexpr (sofa::type::isRigidType<DataTypes>())
{
const auto vr = -k_a[(index < k_a.size()) * index];
for (sofa::Size i = space_size; i < total_size; ++i)
Expand Down
1 change: 1 addition & 0 deletions Sofa/framework/Type/CMakeLists.txt
View file
Expand Up @@ -60,6 +60,7 @@ set(SOURCE_FILES
${SOFATYPESRC_ROOT}/vector_Integral.cpp
${SOFATYPESRC_ROOT}/vector_String.cpp
${SOFATYPESRC_ROOT}/SVector.cpp
${SOFATYPESRC_ROOT}/isRigidType.h
)

sofa_find_package(Sofa.Config REQUIRED)
Expand Down
31 changes: 31 additions & 0 deletions Sofa/framework/Type/src/sofa/type/isRigidType.h
View file
@@ -0,0 +1,31 @@
//
// Created by hugo on 28/11/23.
//

#ifndef SOFA_ISRIGIDTYPE_H
#define SOFA_ISRIGIDTYPE_H

namespace sofa::type
{
// Boiler-plate code to test if a type implements a method
// explanation https://stackoverflow.com/a/30848101

template <typename...>
using void_t = void;

// Primary template handles all types not supporting the operation.
template <typename, template <typename> class, typename = void_t<>>
struct detect : std::false_type {};

// Specialization recognizes/validates only types supporting the archetype.
template <typename T, template <typename> class Op>
struct detect<T, Op, void_t<Op<T>>> : std::true_type {};

// Actual test if DataType::Coord implements getOrientation() (hence is a RigidType)
template <typename T>
using isRigid_t = decltype(std::declval<typename T::Coord>().getOrientation());

template <typename T>
using isRigidType = detect<T, isRigid_t>;
}
#endif //SOFA_ISRIGIDTYPE_H

0 comments on commit 3af47db

Please sign in to comment.