[solver-constraint] Rename epsilon by energyWeight

examples/sofapython3/component/constraint/BarycentricCenterEffector/BarycentricCenterEffector_Rigid.py

@@ -21,7 +21,7 @@ def createScene(rootNode):
     rootNode.findData('dt').value = 0.01
     rootNode.findData('gravity').value = [0, -9810, 0]
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('QPInverseProblemSolver', epsilon=1e-3, tolerance=1e-8, maxIterations=2500)
+    rootNode.addObject('QPInverseProblemSolver', energyWeight=1e-3, tolerance=1e-8, maxIterations=2500)
 
     ##########################################
     # goal

examples/sofapython3/component/constraint/CableEquality/CableEquality.py

@@ -15,7 +15,7 @@ def createScene(rootNode):
     rootNode.findData('gravity').value = [0, 0, 0]
     rootNode.findData('dt').value = 0.02
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('QPInverseProblemSolver', printLog=0, epsilon=1e-3, maxIterations=1000, tolerance=1e-4)
+    rootNode.addObject('QPInverseProblemSolver', printLog=0, energyWeight=1e-3, maxIterations=1000, tolerance=1e-4)
 
     VolumetricMeshPath = MeshesPath + 'Accordeon_Volumetric.vtk'
     SurfaceMeshPath = MeshesPath + 'Accordeon_Surface.stl'

examples/sofapython3/component/constraint/ForcePointActuator/Tissues.py

@@ -48,7 +48,7 @@ def createScene(rootNode):
     rootNode.dt = 0.02
 
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('QPInverseProblemSolver', printLog=False, epsilon=1e-3, tolerance=1e-4, maxIterations=2500)
+    rootNode.addObject('QPInverseProblemSolver', printLog=False, energyWeight=1e-3, tolerance=1e-4, maxIterations=2500)
     rootNode.addObject('CollisionPipeline')
     rootNode.addObject('BruteForceBroadPhase')
     rootNode.addObject('BVHNarrowPhase')

examples/sofapython3/component/constraint/ForceSurfaceActuator/ForceSurfaceActuator.py

@@ -21,7 +21,7 @@ def createScene(rootNode):
     rootNode.findData('dt').value = 0.02
 
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('QPInverseProblemSolver', epsilon=1e-3, tolerance=1e-2, maxIterations=500)
+    rootNode.addObject('QPInverseProblemSolver', energyWeight=1e-3, tolerance=1e-2, maxIterations=500)
 
     ##########################################
     # Effector goal for interactive control  #

examples/sofapython3/component/constraint/JointActuator/JointActuator.py

@@ -34,7 +34,7 @@ def createScene(rootNode):
     rootNode.addObject('VisualStyle', displayFlags='showBehaviorModels')
     rootNode.addObject('BackgroundSetting', color=[0, 0.168627, 0.211765, 1.])
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('QPInverseProblemSolver', epsilon=0)
+    rootNode.addObject('QPInverseProblemSolver', energyWeight=0)
 
     # Target position of the end effector
     goal = rootNode.addChild('Goal')

examples/sofapython3/component/constraint/SlidingActuator/actuatedBeam.py

@@ -35,7 +35,7 @@ def createScene(rootNode):
                                                    'showForceFields showInteractionForceFields')
 
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('QPInverseProblemSolver', epsilon=0.001, printLog=False, tolerance=1e-10, maxIterations=1000)
+    rootNode.addObject('QPInverseProblemSolver', energyWeight=0.001, printLog=False, tolerance=1e-10, maxIterations=1000)
 
     #########################################
     # Goal for end effector                 #

examples/sofapython3/component/constraint/SurfacePressureEquality/SurfacePressureEquality.py

@@ -15,7 +15,7 @@ def createScene(rootNode):
     rootNode.findData('gravity').value = [0, 0, 0]
     rootNode.findData('dt').value = 0.02
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('QPInverseProblemSolver', printLog=0, epsilon=1e-1, maxIterations=1000, tolerance=1e-5)
+    rootNode.addObject('QPInverseProblemSolver', printLog=0, energyWeight=1e-1, maxIterations=1000, tolerance=1e-5)
 
     rootNode.addObject('BackgroundSetting', color=[1, 1, 1, 1])
     rootNode.addObject('LightManager')

src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h

@@ -105,7 +105,9 @@ class ForcePointActuator : public Actuator<DataTypes>
     sofa::Data<sofa::type::vector<Real>>         d_force;
     sofa::Data<Real>                             d_displacement;
     sofa::Data<Deriv>                            d_direction;
-    sofa::Data<Real>                             d_epsilon;
+    sofa::Data<Real>                             d_energyWeight;
+    SOFA_ATTRIBUTE_DEPRECATED("v25.12", "v26.12", "Use d_energyWeight instead.")
+    sofa::Data<Real> d_penalty; // Used to deprecate the name of the data from the UI
 
     sofa::Data<bool>                             d_showForce;
     sofa::Data<Real>                             d_visuScale;
@@ -134,11 +136,11 @@ class ForcePointActuator : public Actuator<DataTypes>
     using Actuator<DataTypes>::m_lambdaMax ;
     using Actuator<DataTypes>::m_lambdaMin ;
     using Actuator<DataTypes>::m_lambdaInit ;
-    using Actuator<DataTypes>::m_epsilon ;
+    using Actuator<DataTypes>::m_energyWeight ;
     using Actuator<DataTypes>::m_hasLambdaMax ;
     using Actuator<DataTypes>::m_hasLambdaMin ;
     using Actuator<DataTypes>::m_hasLambdaInit ;
-    using Actuator<DataTypes>::m_hasEpsilon ;
+    using Actuator<DataTypes>::m_hasEnergyWeight ;
     using Actuator<DataTypes>::m_nbLines ;
     ////////////////////////////////////////////////////////////////////////////
 

src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl

@@ -72,9 +72,12 @@ ForcePointActuator<DataTypes>::ForcePointActuator(MechanicalState* object)
                            "Direction of the force we want to apply. If d=[0,0,0], the direction \n"
                            "will be optimized."))
 
-    , d_epsilon(initData(&d_epsilon, Real(1e-3), "penalty",
-                           "Use this value to prioritize the constraint. 0 means no limitation on the energy \n"
-                            "transfered by this actuator. Default is 1e-3."))
+    , d_energyWeight(initData(&d_energyWeight, Real(1e-3), "energyWeight",
+                              "An energy term is added in the minimization process. \n"
+                              "Specify the energy weight of the constraint. 0 means no limitation on the energy \n"
+                              "transfered by this actuator. The default value used is the energyWeight defined in the inverse problem solver."))
+
+    , d_penalty(initData(&d_penalty, Real(1e-3), "penalty", ""))
 
     , d_showForce(initData(&d_showForce, false, "showForce",
                            ""))
@@ -90,6 +93,8 @@ ForcePointActuator<DataTypes>::ForcePointActuator(MechanicalState* object)
 template<class DataTypes>
 void ForcePointActuator<DataTypes>::setUpData()
 {
+    d_penalty.setDisplayed(false);
+
     d_force.setReadOnly(true);
     d_displacement.setReadOnly(true);
 
@@ -115,6 +120,13 @@ void ForcePointActuator<DataTypes>::init()
                             "To remove this error message fix your scene possibly by "
                             "adding a MechanicalObject." ;
 
+    if (d_penalty.isSet())
+    {
+        msg_deprecated() << "The data penalty is deprecated. To fix your scene please use energyWeight instead. It will be removed in v26.12.";
+        const auto& penalty = sofa::helper::getReadAccessor(d_penalty);
+        d_energyWeight.setValue(penalty);
+    }
+
     initData();
     initLimit();
 }
@@ -132,10 +144,10 @@ void ForcePointActuator<DataTypes>::initData()
 {
     m_dim = (d_direction.getValue().norm()<1e-10)? Deriv::total_size: 1;
 
-    if(d_epsilon.isSet())
+    if(d_energyWeight.isSet())
     {
-        m_hasEpsilon = true;
-        m_epsilon = d_epsilon.getValue();
+        m_hasEnergyWeight = true;
+        m_energyWeight = d_energyWeight.getValue();
     }
 
     if(d_initForce.isSet())

src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp

@@ -132,16 +132,19 @@ QPInverseProblemSolver::QPInverseProblemSolver()
 
     , d_qpSolver(initData(&d_qpSolver, "qpSolver", "QP solver implementation to be used"))
 
-    , d_epsilon(initData(&d_epsilon, 1e-3, "epsilon",
-                         "An energy term is added in the minimization process. \n"
-                         "Epsilon has to be chosen sufficiently small so that the deformation \n"
-                         "energy does not disrupt the quality of the effector positioning. "
-                         "Default value 1e-3."))
+    , d_energyWeight(initData(&d_energyWeight, 1e-3, "energyWeight",
+                             "An energy term is added in the minimization process. \n"
+                             "This is the weight of this term. \n"
+                             "It has to be chosen sufficiently small so that the deformation \n"
+                             "energy does not disrupt the quality of the effector positioning. "
+                             "Default value 1e-3."))
+
+    , d_epsilon(initData(&d_epsilon, 1e-3, "epsilon", ""))
 
     , d_actuatorsOnly(initData(&d_actuatorsOnly, false, "actuatorsOnly",
-                         "An energy term is added in the minimization process. \n"
-                         "If true, only for actuators."
-                         "Default value false."))
+                                 "An energy term is added in the minimization process. \n"
+                                 "If true, add it only for actuators."
+                                 "Default value false."))
 
     , d_allowSliding(initData(&d_allowSliding,false,"allowSliding",
                               "In case of friction, this option enable/disable sliding contact."))
@@ -156,12 +159,12 @@ QPInverseProblemSolver::QPInverseProblemSolver()
                                   "If set, will contraints the sum of contact forces \n"
                                   "to be lesser or equal to the given value.") )
 
-    , d_objective(initData(&d_objective, 250.0, "objective", "Erreur between the target and the end effector "))
+    , d_objective(initData(&d_objective, 0.0, "objective", "Calculated optimal objective function value."))
 
-    , m_lastCP(NULL)
     , m_CP1(nullptr)
     , m_CP2(nullptr)
     , m_CP3(nullptr)
+    , m_lastCP(nullptr)
 {
     sofa::helper::OptionsGroup qpSolvers{"qpOASES" , "proxQP"};
 #if defined SOFTROBOTSINVERSE_ENABLE_PROXQP && !defined SOFTROBOTSINVERSE_ENABLE_QPOASES
@@ -170,6 +173,8 @@ QPInverseProblemSolver::QPInverseProblemSolver()
     qpSolvers.setSelectedItem(QPSolverImpl::QPOASES);
 #endif
 
+    d_objective.setReadOnly(true);
+
     d_qpSolver.setValue(qpSolvers);
 
     d_graph.setWidget("graph");
@@ -181,6 +186,14 @@ QPInverseProblemSolver::QPInverseProblemSolver()
         deleteProblems();
         createProblems();
     });
+
+    d_epsilon.setDisplayed(false);
+    if (d_epsilon.isSet())
+    {
+        msg_deprecated() << "The data epsilon is deprecated. To fix your scene please use energyWeight instead. It will be removed in v26.12.";
+        const auto& epsilon = sofa::helper::getReadAccessor(d_epsilon);
+        d_energyWeight.setValue(epsilon);
+    }
 }
 
 void QPInverseProblemSolver::createProblems()
@@ -469,19 +482,6 @@ inline void QPInverseProblemSolver::buildCompliance(const ConstraintParams *cPar
     AdvancedTimer::stepEnd("Get Compliance");
 }
 
-void QPInverseProblemSolver::rebuildSystem(double massFactor, double forceFactor)
-{
-    d_graph.beginEdit()->clear();
-    d_graph.endEdit();
-
-    //rebuildConstraintCorrectionSystem()
-    for (unsigned int i=0; i<m_constraintsCorrections.size(); i++)
-    {
-        BaseConstraintCorrection* CC = m_constraintsCorrections[i];
-        CC->rebuildSystem(massFactor, forceFactor);
-    }
-}
-
 bool QPInverseProblemSolver::solveSystem(const ConstraintParams * cParams,
                                          MultiVecId res1,
                                          MultiVecId res2)
@@ -494,7 +494,7 @@ bool QPInverseProblemSolver::solveSystem(const ConstraintParams * cParams,
 
     double time = getContext()->getTime();
     m_currentCP->setTime(time);
-    m_currentCP->setEpsilon(d_epsilon.getValue());
+    m_currentCP->setEnergyWeight(d_energyWeight.getValue());
     m_currentCP->setEnergyActuatorsOnly(d_actuatorsOnly.getValue());
     m_currentCP->setTolerance(d_tolerance.getValue());
     m_currentCP->setMaxIterations(d_maxIterations.getValue());

src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h

@@ -102,9 +102,6 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen
                      MultiVecId res1,
                      MultiVecId res2=MultiVecId::null()) override;
 
-    void rebuildSystem(double massFactor,
-                       double forceFactor) override;
-
     bool solveSystem(const ConstraintParams* cParams,
                      MultiVecId res1, MultiVecId res2=MultiVecId::null()) override;
 
@@ -148,7 +145,10 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen
     sofa::Data<double>    d_responseFriction;
     sofa::Data<sofa::helper::OptionsGroup> d_qpSolver;
 
-    sofa::Data<double>    d_epsilon;
+    sofa::Data<double>    d_energyWeight;
+    SOFA_ATTRIBUTE_DEPRECATED("v25.12", "v26.12", "Use d_energyWeight instead.")
+    sofa::Data<double>    d_epsilon; // Used to deprecate the name of the data from the UI
+
     sofa::Data<bool>      d_actuatorsOnly;
     sofa::Data<bool>      d_allowSliding;
     sofa::Data<map <string, vector<SReal> > > d_graph;

src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp

@@ -56,7 +56,7 @@ using sofa::core::objectmodel::Base;
 
 
 QPInverseProblem::QPInverseProblem()
-    : m_epsilon(1e-3)
+    : m_ernergyWeight(1e-3)
     , m_tolerance(1e-5)
     , m_maxIteration(200)
     , m_largestQNormVariation(0.0)

src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.h

@@ -100,7 +100,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblem : public sofa::component::con
     void solve(double& objective, int& iterations);
 
     void setTime(const double& time) {m_time = time;}
-    void setEpsilon(const double& epsilon) {m_epsilon = epsilon;}
+    void setEnergyWeight(const double& energyWeight) {m_ernergyWeight = energyWeight;}
     void setEnergyActuatorsOnly(const bool& actuatorsOnly) {m_actuatorsOnly = actuatorsOnly;}
     void setTolerance(const double& tolerance) {m_tolerance = tolerance;}
     void setMaxIterations(const int& maxIt) {m_maxIteration = maxIt;}
@@ -127,7 +127,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblem : public sofa::component::con
     QPSystem* m_qpSystem;
     QPConstraintLists* m_qpCLists;
 
-    double    m_epsilon;
+    double    m_ernergyWeight;
     bool      m_actuatorsOnly;
     double    m_tolerance;
     int       m_maxIteration;

src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp

@@ -109,9 +109,11 @@ void QPInverseProblemImpl::init(){
 }
 
 /// Build system
-void QPInverseProblemImpl:: computeEnergyWeight(double& weight)
+
+
+void QPInverseProblemImpl:: computeEnergyScalingFactor(double& scalingFactor)
 {
-    weight = 0.0;
+    scalingFactor = 0.0;
     unsigned int nbActuators = m_qpCLists->actuatorRowIds.size();
     unsigned int nbEquality = m_qpCLists->equalityRowIds.size();
 
@@ -148,7 +150,7 @@ void QPInverseProblemImpl:: computeEnergyWeight(double& weight)
     }
 
     if(normW > 1e-14)
-        weight = normQ/normW;
+        scalingFactor = normQ/normW;
 }
 
 
@@ -198,9 +200,9 @@ void QPInverseProblemImpl::buildQPMatrices()
             m_qpSystem->c[j] += m_qpSystem->W[m_qpCLists->effectorRowIds[i]][acIds[j]]*m_qpSystem->dFree[m_qpCLists->effectorRowIds[i]];
     }
 
-    // Add energy term to Q+=eps*||Q||/||Waa||*Waa, eps is set by user
-    double weight = 0.;
-    computeEnergyWeight(weight); // compute ||Q||/||Waa||
+    // Add energy term to Q+=eps*||Q||/||Waa||*Waa, eps (the weight of this term) is set by user
+    double scalingFactor = 0.;
+    computeEnergyScalingFactor(scalingFactor); // compute ||Q||/||Waa||
     int actuatorsId = 0;
     int equalityId = 0;
     unsigned int actuatorsNbLines = 0;
@@ -226,10 +228,10 @@ void QPInverseProblemImpl::buildQPMatrices()
             }
             for(unsigned int j=0; j<dim; j++)
             {
-                if(ac->hasEpsilon() && j==k) // energy of a specific actuator
-                    m_qpSystem->Q[k][j] += ac->getEpsilon()*weight*m_qpSystem->W[acIds[k]][acIds[j]];
+                if(ac->hasEnergyWeight() && j==k) // energy of a specific actuator
+                    m_qpSystem->Q[k][j] += ac->getEnergyWeight()*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]];
                 else
-                    m_qpSystem->Q[k][j] += m_epsilon*weight*m_qpSystem->W[acIds[k]][acIds[j]];
+                    m_qpSystem->Q[k][j] += m_ernergyWeight*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]];
             }
         }
         else if (k<nbActuators+nbEquality)
@@ -244,16 +246,16 @@ void QPInverseProblemImpl::buildQPMatrices()
             }
             for(unsigned int j=0; j<dim; j++)
             {
-                if(ac->hasEpsilon() && j==k) // energy of a specific actuator
-                    m_qpSystem->Q[k][j] += ac->getEpsilon()*weight*m_qpSystem->W[acIds[k]][acIds[j]];
+                if(ac->hasEnergyWeight() && j==k) // energy of a specific actuator
+                    m_qpSystem->Q[k][j] += ac->getEnergyWeight()*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]];
                 else
-                    m_qpSystem->Q[k][j] += m_epsilon*weight*m_qpSystem->W[acIds[k]][acIds[j]];
+                    m_qpSystem->Q[k][j] += m_ernergyWeight*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]];
             }
         }
         else // contacts
         {
             for(unsigned int j=0; j<dim; j++)
-                m_qpSystem->Q[k][j] += m_epsilon*weight*m_qpSystem->W[acIds[k]][acIds[j]];
+                m_qpSystem->Q[k][j] += m_ernergyWeight*scalingFactor*m_qpSystem->W[acIds[k]][acIds[j]];
         }
     }
 

src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.h

@@ -82,7 +82,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemImpl : public QPInverseProblem
                              vector<double> &result,
                              vector<double> &dual) = 0;
 
-    void computeEnergyWeight(double& weight);
+    void computeEnergyScalingFactor(double& weight);
     void buildQPMatrices();
 
     void solveWithContact(vector<double>& result, double &objective, int &iterations);

src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp

@@ -48,8 +48,8 @@ QPInverseProblemQPOases::QPInverseProblemQPOases() :
 }
 
 void QPInverseProblemQPOases::solveInverseProblem(double& objective,
-                                               vector<double> &result,
-                                               vector<double> &dual)
+                                                   vector<double> &result,
+                                                   vector<double> &dual)
 {
     m_qpSystem->previousResult = result;
 
@@ -213,4 +213,4 @@ void QPInverseProblemQPOases::updateQPMatrices(real_t * Q, real_t * c, real_t *
 
 } // namespace
 
-#endif
+#endif

tests/component/solver/QPInverseProblemSolverTest.cpp

@@ -92,7 +92,7 @@ struct QPInverseProblemSolverTest : public BaseTest,
         int nbTimeStep = 10;
         string deltaString;
         double tolerance = 1e-10;
-        m_root->getObject("QPInverseProblemSolver")->findData("epsilon")->read("0.0");
+        m_root->getObject("QPInverseProblemSolver")->findData("energyWeight")->read("0.0");
         m_root->getObject("QPInverseProblemSolver")->findData("qpSolver")->read(qpSolver);
         // Test inverse resolution (effector == target)