[constraint] list of changes:

- changes maxPositiveDisp/maxNegativeDisp to maxDisplacement/minDisplacement - removes references to angle - renames imposedValue to value - updates example wrt changes
SofaDefrost · Apr 30, 2024 · 4d88c4c · 4d88c4c
1 parent 0539767
commit 4d88c4c
Show file tree

Hide file tree

Showing 4 changed files with 74 additions and 63 deletions.
diff --git a/examples/component/constraint/JointConstraint/JointConstraint.py b/examples/component/constraint/JointConstraint/JointConstraint.py
@@ -1,56 +1,71 @@
 import os
+from math import pi
 
 dirPath = os.path.dirname(os.path.abspath(__file__)) + '/'
 
 
 def createScene(rootNode):
     rootNode.addObject('RequiredPlugin', name='SoftRobots')
     rootNode.addObject('RequiredPlugin', name='SoftRobots.Inverse')
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct')  # Needed to use components [SparseLDLSolver]
+    rootNode.addObject('RequiredPlugin', name='ArticulatedSystemPlugin')  # Needed to use components [ArticulatedHierarchyContainer,ArticulatedSystemMapping,Articulation,ArticulationCenter]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.AnimationLoop')  # Needed to use components [FreeMotionAnimationLoop]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Correction')  # Needed to use components [UncoupledConstraintCorrection]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Solver')  # Needed to use components [GenericConstraintSolver]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Projective')  # Needed to use components [FixedProjectiveConstraint]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Engine.Generate')  # Needed to use components [GenerateRigidMass]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.IO.Mesh')  # Needed to use components [MeshSTLLoader]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mapping.NonLinear')  # Needed to use components [RigidMapping]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mass')  # Needed to use components [UniformMass]  
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward')  # Needed to use components [EulerImplicitSolver]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Setting')  # Needed to use components [BackgroundSetting]  
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.FEM.Elastic')  # Needed to use components [BeamFEMForceField]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.StateContainer')  # Needed to use components [MechanicalObject]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant')  # Needed to use components [MeshTopology]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Visual')  # Needed to use components [VisualStyle]  
+    rootNode.addObject('RequiredPlugin', name='Sofa.GL.Component.Rendering3D')  # Needed to use components [OglModel] 
 
-    rootNode.dt = 0.1
-    rootNode.gravity = [0., -981., 0.]
+    rootNode.dt = 0.01
+    rootNode.gravity = [0., -9810., 0.]
     rootNode.addObject('VisualStyle', displayFlags='showBehaviorModels')
-    rootNode.addObject('BackgroundSetting', color=[0, 0.168627, 0.211765, 1.])
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('GenericConstraintSolver')
+    rootNode.addObject('GenericConstraintSolver', maxIterations=500, tolerance=1e-4)
 
     # Simulation node
     simulation = rootNode.addChild('Simulation')
     simulation.addObject('EulerImplicitSolver')
-    simulation.addObject('CGLinearSolver')
+    simulation.addObject('SparseLDLSolver', template="CompressedRowSparseMatrixd")
+    simulation.addObject('GenericConstraintCorrection')
 
     object = simulation.addChild('Object')
-    object.addObject('MechanicalObject', name='dofs', template='Vec1', position=0.)
-    # Try to change the imposed value from the GUI 
-    object.addObject('JointConstraint', template='Vec1', index=0, imposedValue=2, valueType="displacement")
+    object.addObject('MechanicalObject',  template='Vec1', position=0.)
+    object.addObject("RestShapeSpringsForceField", points=[0], stiffness=1e1)
+    # Try to change the data field "value" from the GUI
+    object.addObject('JointConstraint', template='Vec1', index=0, value=0, valueType="displacement",
+                     minDisplacement=-pi, maxDisplacement=pi)
 
     rigid = object.addChild('Rigid')
-    rigid.addObject('MechanicalObject', template='Rigid3', name='dofs',
-                    position=[[0., 0., 0., 0., 0., 0., 1.], [4.5, 0., 0., 0., 0., 0., 1.]])
-    rigid.addObject('BeamFEMForceField', name='FEM', radius=0.2, youngModulus=1e3, poissonRatio=0.45)
-    rigid.addObject('MeshTopology', lines=[0, 1])
+    rigid.addObject('MechanicalObject', template='Rigid3', position=[[0., 0., 0., 0., 0., 0., 1.]]*2)
+    rigid.addObject('UniformMass', totalMass=0.003)
+    rigid.addObject('ArticulatedSystemMapping', 
+                    input1=object.getMechanicalState().linkpath, 
+                    output=rigid.getMechanicalState().linkpath)
 
     visual = rigid.addChild('VisualModel')
-    visual.addObject('MeshSTLLoader', name='loader', filename=dirPath + 'mesh/arm.stl', translation=[-4.5, 0., 0.],
-                     scale3d=[0.1, 0.1, 0.1])
+    visual.addObject('MeshSTLLoader', name='loader', filename=dirPath + 'mesh/arm.stl', translation=[-45, 0., 0.])
     visual.addObject('MeshTopology', src='@loader')
     visual.addObject('OglModel', color=[0.9, 0.9, 0.9, 1.])
     visual.addObject('RigidMapping', index=1)
 
-    rigid.addObject('GenerateRigidMass', name='mass', density=0.002, src=visual.loader.getLinkPath())
-    rigid.addObject('FixedConstraint', template='Rigid3', indices=0)
-    rigid.addObject('ArticulatedSystemMapping', input1=object.dofs.getLinkPath(), output=rigid.dofs.getLinkPath())
-    rigid.addObject('UniformMass', vertexMass=rigid.mass.findData('rigidMass').getLinkPath())
-
-    object.addObject('UncoupledConstraintCorrection')
-    object.addObject('ArticulatedHierarchyContainer')
-
     articulationCenters = object.addChild('Articulation')
     articulationCenter = articulationCenters.addChild('ArticulationCenter')
-    articulationCenter.addObject('ArticulationCenter', parentIndex=0, childIndex=1, posOnParent=[0., 0., 0.],
-                                 posOnChild=[-4.5, 0., 0.], articulationProcess=0)
+    articulationCenter.addObject('ArticulationCenter', parentIndex=0, childIndex=1,
+                                 posOnParent=[0., 0., 0.],
+                                 posOnChild=[-45, 0., 0.], articulationProcess=1)
     articulation = articulationCenter.addChild('Articulations')
     articulation.addObject('Articulation', translation=False, rotation=True, rotationAxis=[0, 0, 1],
                            articulationIndex=0)
 
+    object.addObject('ArticulatedHierarchyContainer')
+
     return rootNode
diff --git a/src/SoftRobots/component/constraint/JointConstraint.cpp b/src/SoftRobots/component/constraint/JointConstraint.cpp
@@ -62,8 +62,7 @@ void JointDisplacementConstraintResolution::resolution(int line, double** w, dou
 
     // da=Waa*(lambda_a) + Sum Wai * lambda_i  = m_imposedDisplacement
     lambda[line] -= (d[line]-m_imposedDisplacement) / m_wActuatorActuator;
-
-    std::clamp(lambda[line], m_minForce, m_maxForce);
+    lambda[line] = std::clamp(lambda[line], m_minForce, m_maxForce);
 }
 
 
@@ -89,14 +88,14 @@ void JointForceConstraintResolution::resolution(int line, double** w, double* d,
 
     double displacement = m_wActuatorActuator*m_imposedForce + d[line];
 
-    if (displacement<m_minDisplacement)
+    if (displacement < m_minDisplacement)
     {
-        displacement=m_minDisplacement;
+        displacement = m_minDisplacement;
         lambda[line] -= (d[line]-displacement) / m_wActuatorActuator;
     }
-    else if (displacement>m_maxDisplacement)
+    else if (displacement > m_maxDisplacement)
     {
-        displacement=m_maxDisplacement;
+        displacement = m_maxDisplacement;
         lambda[line] -= (d[line]-displacement) / m_wActuatorActuator;
     }
     else
@@ -108,7 +107,7 @@ void JointForceConstraintResolution::resolution(int line, double** w, double* d,
 
 
 ////////////////////////////////////////////    FACTORY    //////////////////////////////////////////////
-int JointConstraintClass = RegisterObject("Lagrange multiplier approach to apply a force/angle on a Joint (Vec1)")
+int JointConstraintClass = RegisterObject("Lagrange multiplier approach to apply a force/displacement on a Joint (Vec1)")
 .add< JointConstraint<Vec1Types> >(true)
 ;
 ////////////////////////////////////////////////////////////////////////////////////////////////////////

diff --git a/src/SoftRobots/component/constraint/JointConstraint.h b/src/SoftRobots/component/constraint/JointConstraint.h
@@ -149,7 +149,6 @@ class JointConstraint : public SoftRobotsConstraint<DataTypes>
     using SoftRobotsConstraint<DataTypes>::d_componentState ;
     ////////////////////////////////////////////////////////////////////////////
 
-
     //Input data
     sofa::Data<unsigned int>       d_index; ///< indice of considered node
 
@@ -158,9 +157,9 @@ class JointConstraint : public SoftRobotsConstraint<DataTypes>
 
     sofa::Data<double>                  d_maxForce; ///< maximum force applied on the points
     sofa::Data<double>                  d_minForce; ///< minimum force applied on the points
-
-    sofa::Data<double>                  d_maxPositiveDisplacement; ///< maximum displacement of the points in the positive direction
-    sofa::Data<double>                  d_maxNegativeDisplacement; ///< maximum displacement of the points in the negative direction
+    
+    sofa::Data<double>                  d_maxDisplacement; ///< maximum displacement of the points
+    sofa::Data<double>                  d_minDisplacement; ///< minimum displacement of the points
 
     sofa::Data<double> d_value;
     sofa::Data<sofa::helper::OptionsGroup>  d_valueType;
@@ -169,8 +168,8 @@ class JointConstraint : public SoftRobotsConstraint<DataTypes>
     void internalInit();
     void checkIndicesRegardingState();
 
-    double m_initAngle;
-    double m_currentAngle;
+    double m_initDisplacement;
+    double m_currentDisplacement;
 
 private:
     void setUpDisplacementLimits(double& imposedValue, double& minForce, double& maxForce);

diff --git a/src/SoftRobots/component/constraint/JointConstraint.inl b/src/SoftRobots/component/constraint/JointConstraint.inl
@@ -50,7 +50,7 @@ JointConstraint<DataTypes>::JointConstraint(MechanicalState* object)
                                          "Output force. Warning: to get the actual force you should divide this value by dt."))
 
     , d_displacement(initData(&d_displacement,double(0.0), "displacement",
-                          "Output angle displacement (in radian) compared to the initial position."))
+                          "Output displacement compared to the initial position."))
 
     , d_maxForce(initData(&d_maxForce, "maxForce",
                           "Maximum force allowed. \n"
@@ -60,23 +60,22 @@ JointConstraint<DataTypes>::JointConstraint(MechanicalState* object)
                           "Minimum force allowed. \n"
                           "If unspecified no minimum value will be considered."))
 
-    , d_maxPositiveDisplacement(initData(&d_maxPositiveDisplacement,"maxPositiveDisp",
-                                         "Maximum displacement (in radian) in the positive direction. \n"
-                                         "If unspecified no maximum value will be considered."))
+    , d_maxDisplacement(initData(&d_maxDisplacement,"maxDisplacement",
+                                 "Maximum displacement. \n"
+                                 "If unspecified no maximum value will be considered."))
 
-    , d_maxNegativeDisplacement(initData(&d_maxNegativeDisplacement,"maxNegativeDisp",
-                                         "Maximum displacement (in radian) in the negative direction. \n"
-                                         "If unspecified no maximum value will be considered."))
+    , d_minDisplacement(initData(&d_minDisplacement,"minDisplacement",
+                                 "Minimum displacement. \n"
+                                 "If unspecified no minimum value will be considered."))
 
-    , d_value(initData(&d_value, "imposedValue",
+    , d_value(initData(&d_value, "value",
                                 "Displacement or force to impose.\n"))
 
     , d_valueType(initData(&d_valueType, {"displacement","force"}, "valueType",
                                           "displacement = the constraint will impose the displacement provided in data value[valueIndex] \n"
                                           "force = the constraint will impose the force provided in data value[valueIndex] \n"
                                           "If unspecified, the default value is displacement"))
 {
-    d_index.setGroup("Input");
     d_force.setReadOnly(true);
     d_displacement.setReadOnly(true);
 }
@@ -122,9 +121,8 @@ void JointConstraint<DataTypes>::reinit()
 template<class DataTypes>
 void JointConstraint<DataTypes>::internalInit()
 {
-    // Init angles
     ReadAccessor<sofa::Data<VecCoord> > positions = m_state->readPositions();
-    m_initAngle = positions[d_index.getValue()][0];
+    m_initDisplacement = positions[d_index.getValue()][0];
     d_displacement.setValue(0);
     d_force.setValue(0);
 
@@ -175,7 +173,7 @@ void JointConstraint<DataTypes>::getConstraintViolation(const ConstraintParams*
 
     SOFA_UNUSED(cParams);
 
-    Real dFree = Jdx->element(0) - m_initAngle + m_currentAngle;
+    Real dFree = Jdx->element(0) - m_initDisplacement + m_currentDisplacement;
     resV->set(m_constraintId, dFree);
 }
 
@@ -195,7 +193,7 @@ void JointConstraint<DataTypes>::getConstraintResolution(const ConstraintParams*
     {
         double maxForce = std::numeric_limits<double>::max();
         double minForce = -maxForce;
-        setUpDisplacementLimits(imposedValue,minForce,maxForce);
+        setUpDisplacementLimits(imposedValue, minForce, maxForce);
 
         JointDisplacementConstraintResolution *cr=  new JointDisplacementConstraintResolution(imposedValue, minForce, maxForce);
         resTab[offset++] = cr;
@@ -205,7 +203,7 @@ void JointConstraint<DataTypes>::getConstraintResolution(const ConstraintParams*
     {
         double maxDisplacement = std::numeric_limits<double>::max();
         double minDisplacement = -maxDisplacement;
-        setUpForceLimits(imposedValue,minDisplacement,maxDisplacement);
+        setUpForceLimits(imposedValue, minDisplacement, maxDisplacement);
 
         JointForceConstraintResolution *cr=  new JointForceConstraintResolution(imposedValue, minDisplacement, maxDisplacement);
         resTab[offset++] = cr;
@@ -227,21 +225,21 @@ void JointConstraint<DataTypes>::storeLambda(const ConstraintParams* cParams,
     // Update joint effort
     d_force.setValue(lambda->element(m_constraintId));
 
-    // Update joint angle
-    ReadAccessor<sofa::Data<VecCoord> > positions = m_state->readPositions();
-    m_currentAngle = positions[d_index.getValue()][0];
-    d_displacement.setValue(-m_initAngle+m_currentAngle);
+    // Update joint displacement
+    ReadAccessor<sofa::Data<VecCoord>> positions = m_state->readPositions();
+    m_currentDisplacement = positions[d_index.getValue()][0];
+    d_displacement.setValue(- m_initDisplacement + m_currentDisplacement);
 }
 
 
 template<class DataTypes>
 void JointConstraint<DataTypes>::setUpDisplacementLimits(double& imposedValue, double& minForce, double& maxForce)
 {
-    if(d_maxPositiveDisplacement.isSet() && imposedValue>d_maxPositiveDisplacement.getValue())
-        imposedValue = d_maxPositiveDisplacement.getValue();
+    if(d_maxDisplacement.isSet() && imposedValue>d_maxDisplacement.getValue())
+        imposedValue = d_maxDisplacement.getValue();
 
-    if(d_maxNegativeDisplacement.isSet() && imposedValue<-d_maxNegativeDisplacement.getValue())
-        imposedValue = -d_maxNegativeDisplacement.getValue();
+    if(d_minDisplacement.isSet() && imposedValue<d_minDisplacement.getValue())
+        imposedValue = d_minDisplacement.getValue();
 
     if(d_minForce.isSet())
         minForce=d_minForce.getValue();
@@ -258,10 +256,10 @@ void JointConstraint<DataTypes>::setUpForceLimits(double& imposedValue, double&
     if(d_minForce.isSet() && imposedValue<d_minForce.getValue())
         imposedValue = d_minForce.getValue();
 
-    if(d_maxNegativeDisplacement.isSet())
-        minDisplacement=-d_maxNegativeDisplacement.getValue();
-    if(d_maxPositiveDisplacement.isSet())
-        maxDisplacement=d_maxPositiveDisplacement.getValue();
+    if(d_minDisplacement.isSet())
+        minDisplacement=d_minDisplacement.getValue();
+    if(d_maxDisplacement.isSet())
+        maxDisplacement=d_maxDisplacement.getValue();
 }