Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[Collision] ADD optional viscous friction at sliding contact points #4991

Open
wants to merge 16 commits into
base: master
Choose a base branch
from
Open

[Collision] ADD optional viscous friction at sliding contact points #4991

Show file tree
Hide file tree
Changes from 14 commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
40da34a
Added Viscous friction to the factory
bakpaul Jul 3, 2024
1f071df
add aliases
bakpaul Jul 4, 2024
233dd89
Add constaxpr
bakpaul Jul 4, 2024
c9102fd
ADD first version of viscous friction. Known bug : in the benchmark, …
Jul 19, 2024
66464be
ADD benchmark scene. Must be removed before merge
Jul 19, 2024
6558737
ADD full benchmark
Jul 24, 2024
49ba166
FIX wrong viscous friction force. ADD benchmark plots.
Jul 31, 2024
cb485d1
WIP
Sep 9, 2024
b6c6082
WIP
Sep 9, 2024
f0e4bd3
Rework to combine viscous and dry friction
Sep 9, 2024
39c9c02
Update factory registration mechanism
Sep 10, 2024
879a5c4
RENAME variable alpha to drag, FIX benchmark scene to pass proper par…
Sep 11, 2024
3793a68
Update benchmark to validate friction force
Sep 12, 2024
8717028
[Collision] ADD optional viscous friction at sliding contact points
Sep 12, 2024
7dab766
REVERT registration of unilateral lagrangian constraint to the factory
Sep 25, 2024
8743e41
REMOVE viscous_friction/cube.obj to instad rely on the resource provi…
Oct 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ...ollision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ class FrictionContact : public core::collision::Contact, public ContactIdentifie
sofa::core::objectmodel::RenamedData<double> tol;

Data<double> d_mu; ///< friction coefficient (0 for frictionless contacts)
Data<double> d_drag; ///< viscosity coefficient (0 for frictionless contacts)
Data<double> d_tol; ///< tolerance for the constraints resolution (0 for default tolerance)
std::vector< sofa::core::collision::DetectionOutput* > contacts;
std::vector< std::pair< std::pair<int, int>, double > > mappedContacts;
Expand Down
40 changes: 21 additions & 19 deletions ...lision/Response/Contact/src/sofa/component/collision/response/contact/FrictionContact.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,21 +32,22 @@
namespace sofa::component::collision::response::contact
{

template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::FrictionContact()
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::FrictionContact()
: FrictionContact(nullptr, nullptr, nullptr)
{
}


template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::FrictionContact(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod)
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::FrictionContact(CollisionModel1* model1, CollisionModel2* model2, Intersection* intersectionMethod)
: model1(model1)
, model2(model2)
, intersectionMethod(intersectionMethod)
, m_constraint(nullptr)
, parent(nullptr)
, d_mu (initData(&d_mu, 0.8, "mu", "friction coefficient (0 for frictionless contacts)"))
, d_drag (initData(&d_drag, 0., "drag", "viscosity coefficient (0 for frictionless contacts)"))
, d_tol (initData(&d_tol, 0.0, "tol", "tolerance for the constraints resolution (0 for default tolerance)"))
{
selfCollision = ((core::CollisionModel*)model1 == (core::CollisionModel*)model2);
Expand All @@ -60,13 +61,13 @@ FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::FrictionCo

}

template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::~FrictionContact()
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::~FrictionContact()
{
}

template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::cleanup()
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::cleanup()
{
if (m_constraint)
{
Expand All @@ -89,8 +90,8 @@ void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::clean
}


template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::setDetectionOutputs(OutputVector* o)
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::setDetectionOutputs(OutputVector* o)
{
TOutputVector& outputs = *static_cast<TOutputVector*>(o);
// We need to remove duplicate contacts
Expand Down Expand Up @@ -130,8 +131,8 @@ void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::setDe
}


template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::activateMappers()
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::activateMappers()
{
if (!m_constraint)
{
Expand Down Expand Up @@ -202,12 +203,13 @@ void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::activ
if (!selfCollision) mapper2.updateXfree();
}

template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::createResponse(core::objectmodel::BaseContext* group)
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::createResponse(core::objectmodel::BaseContext* group)
{

activateMappers();
const double mu_ = this->d_mu.getValue();
const double drag_ = this->d_drag.getValue();
// Checks if friction is considered
if ( mu_ < 0.0 )
msg_error() << "mu has to take positive values";
Expand All @@ -226,7 +228,7 @@ void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::creat
const long index = cantorPolynomia(o->id /*cantorPolynomia(index1, index2)*/,id);

// Add contact in unilateral constraint
m_constraint->addContact(mu_, o->normal, distance, index1, index2, index, o->id);
m_constraint->addContact(mu_, drag_, o->normal, distance, index1, index2, index, o->id);
}

if (parent!=nullptr)
Expand All @@ -244,8 +246,8 @@ void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::creat
}
}

template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::removeResponse()
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::removeResponse()
{
if (m_constraint)
{
Expand All @@ -260,8 +262,8 @@ void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::remov
}
}

template < class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1,TCollisionModel2,ResponseDataTypes>::setInteractionTags(MechanicalState1* mstate1, MechanicalState2* mstate2)
template <class TCollisionModel1, class TCollisionModel2, class ResponseDataTypes >
void FrictionContact<TCollisionModel1, TCollisionModel2, ResponseDataTypes>::setInteractionTags(MechanicalState1* mstate1, MechanicalState2* mstate2)
{
sofa::core::objectmodel::TagSet tagsm1 = mstate1->getTags();
sofa::core::objectmodel::TagSet tagsm2 = mstate2->getTags();
Expand Down
5 changes: 3 additions & 2 deletions ...ian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralConstraintResolution.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,9 +80,9 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralConstraintResolut
: public core::behavior::ConstraintResolution
{
public:
UnilateralConstraintResolutionWithFriction(SReal mu, PreviousForcesContainer* prev = nullptr,
UnilateralConstraintResolutionWithFriction(SReal mu, SReal drag, PreviousForcesContainer* prev = nullptr,
bool* active = nullptr)
: core::behavior::ConstraintResolution(3), _mu(mu), _prev(prev), _active(active)
: core::behavior::ConstraintResolution(3), _mu(mu), _drag(drag), _prev(prev), _active(active)
{
}

Expand All @@ -92,6 +92,7 @@ class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralConstraintResolut

protected:
SReal _mu;
SReal _drag;
SReal _W[6];
PreviousForcesContainer* _prev;
bool* _active; // Will set this after the resolution
Expand Down
14 changes: 7 additions & 7 deletions ...n/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,11 @@ namespace sofa::component::constraint::lagrangian::model
using namespace sofa::defaulttype;
using namespace sofa::helper;

//TODO(dmarchal) What does this TODO mean ?
int UnilateralLagrangianConstraintClass = core::RegisterObject("TODO-UnilateralLagrangianConstraint")
.add< UnilateralLagrangianConstraint<Vec3Types> >()

;

void registerUnilateralLagrangianConstraint(sofa::core::ObjectFactory* factory)
alxbilger marked this conversation as resolved.
Show resolved Hide resolved
{
factory->registerObjects(core::ObjectRegistrationData("Unilateral Lagrangian Constraint")
.add<UnilateralLagrangianConstraint<Vec3Types>>());
}

template class SOFA_COMPONENT_CONSTRAINT_LAGRANGIAN_MODEL_API UnilateralLagrangianConstraint<Vec3Types>;

Expand Down Expand Up @@ -87,6 +86,8 @@ void UnilateralConstraintResolutionWithFriction::resolution(int line, SReal** /*
const SReal factor = fN / normFt;
force[line+1] *= factor;
force[line+2] *= factor;
force[line+1] -= _drag*d[line+1];
force[line+2] -= _drag*d[line+2];
}
}

Expand All @@ -106,5 +107,4 @@ void UnilateralConstraintResolutionWithFriction::store(int line, SReal* force, b
}
}


} //namespace sofa::component::constraint::lagrangian::model
8 changes: 4 additions & 4 deletions ...ian/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ class UnilateralLagrangianConstraint : public core::behavior::PairInteractionCon
long contactId;
PersistentID localId;
SReal mu; ///< angle for friction

SReal drag; ///< viscous friction coefficient
Coord P, Q;

mutable Real dfree;
Expand Down Expand Up @@ -129,10 +129,10 @@ class UnilateralLagrangianConstraint : public core::behavior::PairInteractionCon

void clear(int reserve = 0);

virtual void addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord Pfree, Coord Qfree, long id=0, PersistentID localid=0);
virtual void addContact(SReal mu, SReal drag, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord Pfree, Coord Qfree, long id=0, PersistentID localid=0);

void addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0);
void addContact(SReal mu, Deriv norm, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0);
void addContact(SReal mu, SReal drag, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0);
void addContact(SReal mu, SReal drag, Deriv norm, Real contactDistance, int m1, int m2, long id=0, PersistentID localid=0);

void buildConstraintMatrix(const core::ConstraintParams* cParams, DataMatrixDeriv &c1, DataMatrixDeriv &c2, unsigned int &cIndex
, const DataVecCoord &x1, const DataVecCoord &x2) override;
Expand Down
48 changes: 25 additions & 23 deletions ...n/Model/src/sofa/component/constraint/lagrangian/model/UnilateralLagrangianConstraint.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,18 +55,18 @@ void UnilateralLagrangianConstraint<DataTypes>::clear(int reserve)
}

template<class DataTypes>
void UnilateralLagrangianConstraint<DataTypes>::addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id, PersistentID localid)
void UnilateralLagrangianConstraint<DataTypes>::addContact(SReal mu, SReal drag, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, long id, PersistentID localid)
{
addContact(mu, norm, P, Q, contactDistance, m1, m2,
addContact(mu, drag, norm, P, Q, contactDistance, m1, m2,
this->getMState2()->read(core::ConstVecCoordId::freePosition())->getValue()[m2],
this->getMState1()->read(core::ConstVecCoordId::freePosition())->getValue()[m1],
id, localid);
}

template<class DataTypes>
void UnilateralLagrangianConstraint<DataTypes>::addContact(SReal mu, Deriv norm, Real contactDistance, int m1, int m2, long id, PersistentID localid)
void UnilateralLagrangianConstraint<DataTypes>::addContact(SReal mu, SReal drag, Deriv norm, Real contactDistance, int m1, int m2, long id, PersistentID localid)
{
addContact(mu, norm,
addContact(mu, drag, norm,
this->getMState2()->read(core::ConstVecCoordId::position())->getValue()[m2],
this->getMState1()->read(core::ConstVecCoordId::position())->getValue()[m1],
contactDistance, m1, m2,
Expand All @@ -76,23 +76,24 @@ void UnilateralLagrangianConstraint<DataTypes>::addContact(SReal mu, Deriv norm,
}

template<class DataTypes>
void UnilateralLagrangianConstraint<DataTypes>::addContact(SReal mu, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord /*Pfree*/, Coord /*Qfree*/, long id, PersistentID localid)
void UnilateralLagrangianConstraint<DataTypes>::addContact(SReal mu, SReal drag, Deriv norm, Coord P, Coord Q, Real contactDistance, int m1, int m2, Coord /*Pfree*/, Coord /*Qfree*/, long id, PersistentID localid)
{
contacts.resize(contacts.size() + 1);
Contact &c = contacts.back();

c.P = P;
c.Q = Q;
c.m1 = m1;
c.m2 = m2;
c.norm = norm;
c.t = Deriv(norm.z(), norm.x(), norm.y());
c.s = cross(norm, c.t);
c.s = c.s / c.s.norm();
c.t = cross((-norm), c.s);
c.mu = mu;
c.P = P;
c.Q = Q;
c.m1 = m1;
c.m2 = m2;
c.norm = norm;
c.t = Deriv(norm.z(), norm.x(), norm.y());
c.s = cross(norm, c.t);
c.s = c.s / c.s.norm();
c.t = cross((-norm), c.s);
c.mu = mu;
c.drag = drag;
c.contactId = id;
c.localId = localid;
c.localId = localid;
c.contactDistance = contactDistance;
}

Expand All @@ -119,7 +120,7 @@ void UnilateralLagrangianConstraint<DataTypes>::buildConstraintMatrix(const core
c1_it.addCol(c.m1, -c.norm);
c1_it.addCol(c.m2, c.norm);

if (c.mu > 0.0)
if (c.mu > 0.0 || c.drag > 0.0)
{
c1_it = c1.writeLine(c.id + 1);
c1_it.setCol(c.m1, -c.t);
Expand Down Expand Up @@ -152,7 +153,7 @@ void UnilateralLagrangianConstraint<DataTypes>::buildConstraintMatrix(const core
MatrixDerivRowIterator c2_it = c2.writeLine(c.id);
c2_it.addCol(c.m2, c.norm);

if (c.mu > 0.0)
if (c.mu > 0.0 || c.drag > 0.0)
{
c1_it = c1.writeLine(c.id + 1);
c1_it.setCol(c.m1, -c.t);
Expand Down Expand Up @@ -247,7 +248,7 @@ void UnilateralLagrangianConstraint<DataTypes>::getPositionViolation(linearalgeb

c.dfree = dfree; // PJ : For isActive() method. Don't know if it's still usefull.

if (c.mu > 0.0)
if (c.mu > 0.0 || c.drag > 0.0)
{
v->set(c.id + 1, dfree_t);
v->set(c.id + 2, dfree_s);
Expand Down Expand Up @@ -280,7 +281,7 @@ void UnilateralLagrangianConstraint<DataTypes>::getVelocityViolation(linearalgeb

v->set(c.id, dot(dFreeVec, c.norm) - c.contactDistance*invDt ); // dvfree = 1/dt * [ dot ( P - Q, n) - contactDist ] + dot(v_P - v_Q , n ) ]

if (c.mu > 0.0)
if (c.mu > 0.0 || c.drag > 0.0)
{
v->set(c.id + 1, dot(QP_vfree, c.t)); // dfree_t
v->set(c.id + 2, dot(QP_vfree, c.s)); // dfree_s
Expand Down Expand Up @@ -362,13 +363,14 @@ void UnilateralLagrangianConstraint<DataTypes>::getConstraintResolution(const co
for(unsigned int i=0; i<contacts.size(); i++)
{
Contact& c = contacts[i];
if(c.mu > 0.0)
if(c.mu > 0.0 || c.drag > 0.0)
{
UnilateralConstraintResolutionWithFriction* ucrwf = new UnilateralConstraintResolutionWithFriction(c.mu, nullptr, &contactsStatus[i]);
UnilateralConstraintResolutionWithFriction* ucrwf;
ucrwf = new UnilateralConstraintResolutionWithFriction(c.mu, c.drag, nullptr, &contactsStatus[i]);
ucrwf->setTolerance(customTolerance);
resTab[offset] = ucrwf;

// TODO : cette méthode de stockage des forces peu mal fonctionner avec 2 threads quand on utilise l'haptique
// TODO : cette méthode de stockage des forces peut mal fonctionner avec 2 threads quand on utilise l'haptique
offset += 3;
}
else
Expand Down
Loading
Loading