Skip to content

Refactoring #88

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

Open
EulalieCoevoet wants to merge 10 commits into
base: main
Choose a base branch
from
Open

Refactoring #88

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
fdb3305
[binding] register the solver
EulalieCoevoet Aug 6, 2026
33eed22
[solver] wip solver mode
EulalieCoevoet Aug 6, 2026
0162a92
Merge branch 'main' into pr_solvermode
EulalieCoevoet Aug 10, 2026
fabe702
[constraint] cleaning BarycentricCenterEffector
EulalieCoevoet Aug 10, 2026
67cfce4
[constraint] refactoring
EulalieCoevoet Aug 10, 2026
043c12b
fix
EulalieCoevoet Aug 10, 2026
06319d9
fix
EulalieCoevoet Aug 11, 2026
2f0ec7e
cleaning
EulalieCoevoet Aug 12, 2026
4e14a67
following requested changes in SoftRobots
EulalieCoevoet Aug 17, 2026
fb2a4b6
requested changes
EulalieCoevoet Aug 17, 2026
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
6 changes: 6 additions & 0 deletions src/SoftRobots.Inverse/binding/Binding_QPInverseProblemSolver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,12 @@ void moduleAddQPInverseProblemSolver(py::module &m)

s.def(py::init<>());
s.def("solveSystem", &softrobotsinverse::solver::QPInverseProblemSolver::solveSystem);

/// register the binding in the downcasting subsystem
sofapython3::PythonFactory::registerType<softrobotsinverse::solver::QPInverseProblemSolver>([](sofa::core::objectmodel::Base* object)
{
return py::cast(dynamic_cast<softrobotsinverse::solver::QPInverseProblemSolver*>(object));
});
}

}
6 changes: 0 additions & 6 deletions src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,12 +113,6 @@ class SOFA_SOFTROBOTS_INVERSE_API BarycentricCenterEffector : public PositionEff
using PositionEffector<DataTypes>::d_indices ;
using PositionEffector<DataTypes>::d_componentState ;

SOFA_ATTRIBUTE_DEPRECATED("v24.12", "v25.06", "Use d_useDirections instead.")
sofa::Data<Vec<3,bool> > d_axis;

SOFA_ATTRIBUTE_DEPRECATED("v24.12", "v25.06", "Use d_effectorGoal instead.")
sofa::Data<Coord > d_effectorGoalPosition;

sofa::Data<bool> d_drawBarycenter;
sofa::Data<Coord > d_barycenter;

Expand Down
15 changes: 0 additions & 15 deletions src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,18 +51,13 @@ using sofa::core::objectmodel::ComponentState;
template<class DataTypes>
BarycentricCenterEffector<DataTypes>::BarycentricCenterEffector(MechanicalState* object)
: Inherit1(object)
, d_axis(initData(&d_axis, sofa::type::Vec<3,bool>(true,true,true), "axis",
"The parameter axis is of type Vec3<bool> and allows to specify the directions in \n"
"which you want to solve the effector. If unspecified, the default \n"
"values are {true, true, true})."))

, d_drawBarycenter(initData(&d_drawBarycenter,false,"drawBarycenter",
"If true, draw the barycenter" ))

, d_barycenter(initData(&d_barycenter,"barycenter",
"Position of barycenter." ))
{
d_axis.setDisplayed(false);
d_indices.setDisplayed(false); // inherited from PositionModel but not used here
d_barycenter.setReadOnly(true);
}
Expand All @@ -81,16 +76,6 @@ void BarycentricCenterEffector<DataTypes>::init()
indices.resize(1);

Inherit1::init();

if (d_axis.isSet())
{
msg_deprecated() << "The data axis is deprecated. To fix your scene please use useDirections instead. It will be remove in v25.06.";
auto useDirections = sofa::helper::getWriteAccessor(d_useDirections);
const auto& axis = sofa::helper::getReadAccessor(d_axis);
useDirections[0] = axis[0];
useDirections[1] = axis[1];
useDirections[2] = axis[2];
}
}

template<class DataTypes>
Expand Down
45 changes: 23 additions & 22 deletions src/SoftRobots.Inverse/component/constraint/CableActuator.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,13 +61,6 @@ CableActuator<DataTypes>::CableActuator(MechanicalState* object)
// These data from CableModel have no sense for actuator
d_eqForce.setDisplayed(false);
d_eqDisplacement.setDisplayed(false);

// QP on only one value, we set dimension to one
m_lambdaInit.resize(1);
m_deltaMax.resize(1);
m_deltaMin.resize(1);
m_lambdaMax.resize(1);
m_lambdaMin.resize(1);
}


Expand Down Expand Up @@ -103,8 +96,10 @@ void CableActuator<DataTypes>::reset()
template<class DataTypes>
void CableActuator<DataTypes>::initData()
{
d_displacement.setValue(0.0);
auto l = sofa::helper::getWriteAccessor(this->d_lambda);
l[0] = d_initForce.getValue();
d_force.setValue(d_initForce.getValue());

if(d_initForce.isSet())
{
m_hasLambdaInit = true;
Expand All @@ -119,7 +114,7 @@ void CableActuator<DataTypes>::initLimit()
if(time < d_constrainAtTime.getValue())
return;

ReadAccessor<sofa::Data<double>> displacement = d_displacement;
ReadAccessor<sofa::Data<sofa::type::vector<double>>> displacement = this->d_delta;
ReadAccessor<sofa::Data<Real>> maxDispVariation = d_maxDispVariation;
ReadAccessor<sofa::Data<Real>> maxPositiveDisplacement = d_maxPositiveDisplacement;
ReadAccessor<sofa::Data<Real>> maxNegativeDisplacement = d_maxNegativeDisplacement;
Expand Down Expand Up @@ -157,10 +152,10 @@ void CableActuator<DataTypes>::initLimit()
{
m_hasDeltaMax = true;
m_hasDeltaMin = true;
if(rabs(m_deltaMin[0] - displacement) >= maxDispVariation || !d_maxNegativeDisplacement.isSet())
m_deltaMin[0] = displacement - maxDispVariation;
if(rabs(m_deltaMax[0] - displacement) >= maxDispVariation || !d_maxPositiveDisplacement.isSet())
m_deltaMax[0] = displacement + maxDispVariation;
if(rabs(m_deltaMin[0] - displacement[0]) >= maxDispVariation || !d_maxNegativeDisplacement.isSet())
m_deltaMin[0] = displacement[0] - maxDispVariation;
if(rabs(m_deltaMax[0] - displacement[0]) >= maxDispVariation || !d_maxPositiveDisplacement.isSet())
m_deltaMax[0] = displacement[0] + maxDispVariation;
}
}

Expand All @@ -178,7 +173,7 @@ void CableActuator<DataTypes>::updateLimit()
return;
}

ReadAccessor<sofa::Data<double>> displacement = d_displacement;
ReadAccessor<sofa::Data<sofa::type::vector<double>>> displacement = this->d_delta;
ReadAccessor<sofa::Data<Real>> maxDispVariation = d_maxDispVariation;
ReadAccessor<sofa::Data<Real>> maxPositiveDisplacement = d_maxPositiveDisplacement;
ReadAccessor<sofa::Data<Real>> maxNegativeDisplacement = d_maxNegativeDisplacement;
Expand All @@ -191,33 +186,33 @@ void CableActuator<DataTypes>::updateLimit()

if(d_maxDispVariation.isSet())
{
if(rabs(m_deltaMin[0] - displacement) >= maxDispVariation || !d_maxNegativeDisplacement.isSet())
m_deltaMin[0] = displacement - maxDispVariation;
if(rabs(m_deltaMax[0] - displacement) >= maxDispVariation || !d_maxPositiveDisplacement.isSet())
m_deltaMax[0] = displacement + maxDispVariation;
if(rabs(m_deltaMin[0] - displacement[0]) >= maxDispVariation || !d_maxNegativeDisplacement.isSet())
m_deltaMin[0] = displacement[0] - maxDispVariation;
if(rabs(m_deltaMax[0] - displacement[0]) >= maxDispVariation || !d_maxPositiveDisplacement.isSet())
m_deltaMax[0] = displacement[0] + maxDispVariation;
}
}


template<class DataTypes>
void CableActuator<DataTypes>::updateVisualization()
{
ReadAccessor<sofa::Data<double>> displacement = d_displacement;
ReadAccessor<sofa::Data<double>> force = d_force;
ReadAccessor<sofa::Data<sofa::type::vector<double>>> displacement = this->d_delta;
ReadAccessor<sofa::Data<sofa::type::vector<double>>> force = this->d_lambda;
ReadAccessor<sofa::Data<Real>> maxPositiveDisplacement = d_maxPositiveDisplacement;
ReadAccessor<sofa::Data<Real>> maxForce = d_maxForce;

if(d_maxPositiveDisplacement.isSet())
{
if(rabs(displacement-maxPositiveDisplacement) < 1e-5)
if(rabs(displacement[0]-maxPositiveDisplacement) < 1e-5)
d_color.setValue(sofa::type::RGBAColor::red());
else
d_color.setValue(m_color);
}

if(d_maxForce.isSet())
{
if(rabs(force-maxForce) < 1e-5)
if(rabs(force[0]-maxForce) < 1e-5)
d_color.setValue(sofa::type::RGBAColor::red());
else
d_color.setValue(m_color);
Expand All @@ -228,7 +223,13 @@ template<class DataTypes>
void CableActuator<DataTypes>::storeResults(sofa::type::vector<double> &lambda,
sofa::type::vector<double> &delta)
{
auto l = sofa::helper::getWriteAccessor(this->d_lambda);
auto d = sofa::helper::getWriteAccessor(this->d_delta);

l[0] = lambda[0];
d_force.setValue(lambda[0]);

d[0] = delta[0];
d_displacement.setValue(delta[0]);

updateLimit();
Expand Down
6 changes: 0 additions & 6 deletions src/SoftRobots.Inverse/component/constraint/CableEquality.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,10 +55,6 @@ CableEquality<DataTypes>::CableEquality(MechanicalState* object)
d_maxPositiveDisplacement.setDisplayed(false);
d_maxNegativeDisplacement.setDisplayed(false);
d_maxDispVariation.setDisplayed(false);

// QP on only one value, we set dimension to one
m_lambdaEqual.resize(1);
m_deltaEqual.resize(1);
}


Expand All @@ -85,8 +81,6 @@ void CableEquality<DataTypes>::reinit()
template<class DataTypes>
void CableEquality<DataTypes>::reset()
{
d_displacement.setValue(0.0);
d_force.setValue(0.0);
updateConstraint();
}

Expand Down
4 changes: 2 additions & 2 deletions src/SoftRobots.Inverse/component/constraint/CableSensor.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,12 +61,12 @@ void CableSensor<DataTypes>::setUpData()
d_maxForce.setDisplayed(false);
d_minForce.setDisplayed(false);
d_eqForce.setDisplayed(false);
d_force.setDisplayed(false);
this->d_lambda.setDisplayed(false);
d_maxPositiveDisplacement.setDisplayed(false);
d_maxNegativeDisplacement.setDisplayed(false);
d_maxDispVariation.setDisplayed(false);
d_eqDisplacement.setDisplayed(false);
d_displacement.setDisplayed(false);
this->d_delta.setDisplayed(false);
}


Expand Down
2 changes: 2 additions & 0 deletions src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,9 @@ class ForcePointActuator : public Actuator<DataTypes>
sofa::Data<Real> d_minForce;
sofa::Data<Real> d_initForce;
sofa::Data<Real> d_maxForceVariation;
SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_lambda instead.")
sofa::Data<sofa::type::vector<Real>> d_force;
SOFA_ATTRIBUTE_DEPRECATED("v26.06", "v27.06", "Use d_delta instead.")
sofa::Data<Real> d_displacement;
sofa::Data<Deriv> d_direction;
sofa::Data<Real> d_energyWeight;
Expand Down
54 changes: 27 additions & 27 deletions src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,23 +51,17 @@ ForcePointActuator<DataTypes>::ForcePointActuator(MechanicalState* object)
"Index of the point of the model on which we want to apply the force"))

, d_maxForce(initData(&d_maxForce, "maxForce",
""))
"Maximum force."))

, d_minForce(initData(&d_minForce, "minForce",
""))
"Minimum force."))

, d_initForce(initData(&d_initForce, Real(0.0), "initForce",
"Initial force if any. Default is 0."))

, d_maxForceVariation(initData(&d_maxForceVariation, "maxForceVariation",
"Only available if the direction is set."))

, d_force(initData(&d_force, "force",
"Warning: to get the actual force you should divide this value by dt."))

, d_displacement(initData(&d_displacement, Real(0.0), "displacement",
""))

, d_direction(initData(&d_direction, "direction",
"Direction of the force we want to apply. If d=[0,0,0], the direction \n"
"will be optimized."))
Expand All @@ -78,10 +72,10 @@ ForcePointActuator<DataTypes>::ForcePointActuator(MechanicalState* object)
"transfered by this actuator. The default value used is the energyWeight defined in the inverse problem solver."))

, d_showForce(initData(&d_showForce, false, "showForce",
""))
"Either to show the force or not."))

, d_visuScale(initData(&d_visuScale, Real(0.1), "visuScale",
""))
"Visualization scale."))

{
setUpData();
Expand All @@ -94,9 +88,6 @@ void ForcePointActuator<DataTypes>::setUpData()
d_epsilon.setOriginalData(&d_energyWeight);
this->addAlias(&d_energyWeight, "penalty");

d_force.setReadOnly(true);
d_displacement.setReadOnly(true);

d_showForce.setGroup("Visualization");
d_visuScale.setGroup("Visualization");
}
Expand Down Expand Up @@ -135,6 +126,7 @@ template<class DataTypes>
void ForcePointActuator<DataTypes>::initData()
{
m_dim = (d_direction.getValue().norm()<1e-10)? Deriv::total_size: 1;
this->resizeConstraints(m_dim);

if(d_energyWeight.isSet())
{
Expand All @@ -150,12 +142,8 @@ void ForcePointActuator<DataTypes>::initData()

sofa::type::vector<Real> force;
force.resize(m_dim, d_initForce.getValue());
this->d_lambda.setValue(force);
d_force.setValue(force);

// QP on only one value, we set dimension to one
m_lambdaInit.resize(m_dim);
m_lambdaMax.resize(m_dim);
m_lambdaMin.resize(m_dim);
}


Expand Down Expand Up @@ -193,24 +181,26 @@ void ForcePointActuator<DataTypes>::updateLimit()
lambda = d_minForce.getValue();
}

auto lambda = sofa::helper::getReadAccessor(this->d_lambda);

if(d_maxForceVariation.isSet())
{
if (m_dim>1)
{
for(unsigned int j=0; j<Deriv::total_size; j++)
{
if(rabs(m_lambdaMin[j] - d_force.getValue()[j]) >= d_maxForceVariation.getValue() || !d_minForce.isSet())
m_lambdaMin[j] = d_force.getValue()[j] - d_maxForceVariation.getValue();
if(rabs(m_lambdaMax[j] - d_force.getValue()[j]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet())
m_lambdaMax[j] = d_force.getValue()[j] + d_maxForceVariation.getValue();
if(rabs(m_lambdaMin[j] - lambda[j]) >= d_maxForceVariation.getValue() || !d_minForce.isSet())
m_lambdaMin[j] = lambda[j] - d_maxForceVariation.getValue();
if(rabs(m_lambdaMax[j] - lambda[j]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet())
m_lambdaMax[j] = lambda[j] + d_maxForceVariation.getValue();
}
}
else
{
if(rabs(m_lambdaMin[0] - d_force.getValue()[0]) >= d_maxForceVariation.getValue() || !d_minForce.isSet())
m_lambdaMin[0] = d_force.getValue()[0] - d_maxForceVariation.getValue();
if(rabs(m_lambdaMax[0] - d_force.getValue()[0]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet())
m_lambdaMax[0] = d_force.getValue()[0] + d_maxForceVariation.getValue();
if(rabs(m_lambdaMin[0] - lambda[0]) >= d_maxForceVariation.getValue() || !d_minForce.isSet())
m_lambdaMin[0] = lambda[0] - d_maxForceVariation.getValue();
if(rabs(m_lambdaMax[0] - lambda[0]) >= d_maxForceVariation.getValue() || !d_maxForce.isSet())
m_lambdaMax[0] = lambda[0] + d_maxForceVariation.getValue();
}
}
}
Expand Down Expand Up @@ -287,17 +277,27 @@ void ForcePointActuator<DataTypes>::getConstraintViolation(const ConstraintParam
template<class DataTypes>
void ForcePointActuator<DataTypes>::storeResults(vector<double> &lambda, vector<double> &delta)
{
auto l = sofa::helper::getWriteAccessor(this->d_lambda);
auto d = sofa::helper::getWriteAccessor(this->d_delta);

WriteAccessor<sofa::Data<vector<Real>>> force = d_force;

d[0] = delta[0];
d_displacement.setValue(delta[0]);

if(m_dim > 1) // No fixed direction
{
for(unsigned int j=0; j<Deriv::total_size; j++)
{
l[j] = lambda[j];
force[j]=lambda[j];
}
}
else
{
l[0] = lambda[0];
force[0] = lambda[0];
}

updateLimit();

Expand All @@ -316,7 +316,7 @@ void ForcePointActuator<DataTypes>::draw(const VisualParams* vparams)
ReadAccessor<sofa::Data<sofa::type::vector<sofa::Index>>> indices = sofa::helper::getReadAccessor(d_indices);
ReadAccessor<sofa::Data<Real>> visuScale = sofa::helper::getReadAccessor(d_visuScale);
ReadAccessor<sofa::Data<VecCoord> > positions = m_state->readPositions();
ReadAccessor<sofa::Data<vector<Real>>> force = d_force;
ReadAccessor<sofa::Data<vector<Real>>> force = this->d_lambda;
Deriv direction = d_direction.getValue();

static const sofa::type::RGBAColor color(0,0,0.8,1);
Expand Down
Loading
Loading