[constraint] makes BarycentricCenterEffector inherit from PositionEff…

…ector (#42) * [constraint] makes BarycentricCenterEffector inherit from PositionEffector: * [constraint] minor cleaning * [constraint] cleaning * Update src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h Co-authored-by: Alex Bilger <[email protected]> * [constraint] applies requested changes --------- Co-authored-by: Alex Bilger <[email protected]>
SofaDefrost · Oct 18, 2024 · c2dfb51 · c2dfb51
1 parent b0b8702
commit c2dfb51
Show file tree

Hide file tree

Showing 5 changed files with 143 additions and 236 deletions.
diff --git a/...python3/component/constraint/BarycentricCenterEffector/BarycentricCenterEffector_Rigid.py b/...python3/component/constraint/BarycentricCenterEffector/BarycentricCenterEffector_Rigid.py
@@ -1,71 +1,75 @@
+from math import sin, cos, pi
+
 def createScene(rootNode):
     rootNode.addObject('RequiredPlugin', name='SoftRobots')
     rootNode.addObject('RequiredPlugin', name='SoftRobots.Inverse')
     rootNode.addObject('RequiredPlugin', name='BeamAdapter')
     rootNode.addObject('VisualStyle',
                        displayFlags='showVisualModels showBehaviorModels showCollisionModels '
                                     'hideBoundingCollisionModels showForceFields '
-                                    'hideInteractionForceFields hideWireframe')
+                                    'showInteractionForceFields hideWireframe')
+    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 [GenericConstraintCorrection,UncoupledConstraintCorrection]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct')  # Needed to use components [SparseLDLSolver]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Iterative')  # Needed to use components [CGLinearSolver]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.Mapping.NonLinear')  # Needed to use components [RigidMapping]
+    rootNode.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward')  # Needed to use components [EulerImplicitSolver]
+    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.findData('dt').value = 0.01
     rootNode.findData('gravity').value = [0, -9810, 0]
     rootNode.addObject('FreeMotionAnimationLoop')
-    rootNode.addObject('QPInverseProblemSolver', epsilon=1, tolerance=1e-8, maxIterations=2500)
+    rootNode.addObject('QPInverseProblemSolver', epsilon=1e-3, tolerance=1e-8, maxIterations=2500)
 
-    #########################################
+    ##########################################
     # goal
     ##########################################
     goal = rootNode.addChild('goal')
     goal.addObject('VisualStyle', displayFlags="showCollisionModels")
     goal.addObject('EulerImplicitSolver', firstOrder=True)
     goal.addObject('CGLinearSolver', iterations=100, tolerance=1e-5, threshold=1e-5)
-    goal.addObject('MechanicalObject', name='goalMO', template="Vec3", showObject=True, drawMode=1,
-                   showObjectScale=3, position=[10, 0, 0])
-    goal.addObject('UncoupledConstraintCorrection')
+    goal.addObject('MechanicalObject', name='goalMO', template="Rigid3", showObject=True,
+                   showObjectScale=3, position=[[10, 0, 0, 0, 0, -sin(pi/16), cos(pi/16)]])
+    goal.addObject('UncoupledConstraintCorrection', defaultCompliance=[1e-7, 0, 0, 0, 0, 0, 0])
 
-    #########################################
+    ##########################################
     # solver
     ##########################################
     solverNode = rootNode.addChild('Solver')
-    solverNode.addObject('EulerImplicitSolver', firstOrder=False, rayleighStiffness=40.0, rayleighMass=40.0)
-    solverNode.addObject('SparseLDLSolver')
+    solverNode.addObject('EulerImplicitSolver', firstOrder=False, rayleighStiffness=1)
+    solverNode.addObject('SparseLDLSolver', template="CompressedRowSparseMatrixd")
     solverNode.addObject('GenericConstraintCorrection')
 
     ##########################################
-    # Beam Model                             #
+    # Beam Model
     ##########################################
-    posNode = [[0, 0, 0], [10, 0, 0], [20, 0, 0]]
-    edgeList = [[0, 1], [1, 2]]
+    positions = [[i*10, 0, 0] for i in range(3)]
+    edges = [[i, i+1] for i in range(2)]
     FramesNode = solverNode.addChild("framesNode")
-    FramesNode.addObject('MeshTopology', position=posNode, edges=edgeList)
-    FramesNode.addObject('MechanicalObject', template="Rigid3",
-                         showObject=True,
-                         showObjectScale=0.001)
-    FramesNode.addObject('BarycentricCenterEffector', template="Rigid3",
-                         effectorGoal=goal.goalMO.position.linkpath,
-                         axis=[1, 1, 1])
+    FramesNode.addObject('MeshTopology', position=positions, edges=edges)
+    FramesNode.addObject('MechanicalObject', template="Rigid3")
+    FramesNode.addObject('BarycentricCenterEffector', name="BCEPosition", template="Rigid3", useDirections=[0, 1, 0, 0, 0, 0],
+                         effectorGoal=goal.getMechanicalState().position.linkpath, drawBarycenter=True)
+    FramesNode.addObject('BarycentricCenterEffector', name="BCEOrientation", template="Rigid3", useDirections=[0, 0, 0, 0, 0, 1], weight=100,
+                         effectorGoal=goal.getMechanicalState().position.linkpath)
 
     topo = FramesNode.addChild("topo")
-    topo.addObject('MeshTopology', position=posNode, edges=edgeList)
-    topo.addObject('BeamInterpolation', name="BeamInterpolation", defaultYoungModulus=1e6,
-                   dofsAndBeamsAligned=True, straight=True)
+    topo.addObject('MeshTopology', position=positions, edges=edges)
+    topo.addObject('BeamInterpolation', name="BeamInterpolation", defaultYoungModulus=1e7,
+                   dofsAndBeamsAligned=True, straight=True, radius=5)
     topo.addObject('AdaptiveBeamForceFieldAndMass', computeMass=True, massDensity=0.0001)
 
     ##########################################
-    # Cable Model                            #
+    # Cable Model
     ##########################################
     cable = FramesNode.addChild('Cables')
     cable.addObject('VisualStyle', displayFlags="showInteractionForceFields")
     cable.addObject('MechanicalObject', template='Vec3', position=[[0., 0., 0.], [0., 0., 0.]])
-    for i in range(4):
-        cable.addObject('CableActuator', name="cable" + str(i), indices=i % 2,
-                        pullPoint=[[-20, 20, 20],
-                                   [40, 20, 20],
-                                   [-20, 20, -20],
-                                   [40, 20, -20]][i],
-                        minForce=0,
-                        maxPositiveDisp=10,
-                        maxDispVariation=0.1)
+    for i in range(2):
+        cable.addObject('CableActuator', name="cable" + str(i), indices=i,
+                        pullPoint=[[0, 20, 0], [20, 20, 0]][i], maxDispVariation=0.1)
     cable.addObject('RigidMapping', rigidIndexPerPoint=[0, 2])
 
     return rootNode
diff --git a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.cpp b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.cpp
@@ -38,55 +38,26 @@ using sofa::defaulttype::Vec3Types;
 using sofa::defaulttype::Rigid3Types;
 using sofa::core::RegisterObject ;
 
-
 template<>
-void BarycentricCenterEffector<Rigid3Types>::buildConstraintMatrix(const ConstraintParams* cParams,
-                                                                   DataMatrixDeriv &cMatrix,
-                                                                   unsigned int &cIndex,
-                                                                   const DataVecCoord &x)
+void BarycentricCenterEffector<Rigid3Types>::draw(const VisualParams* vparams)
 {
-    SOFA_UNUSED(cParams);
-    SOFA_UNUSED(x);
-
-    d_constraintIndex.setValue(cIndex);
-    const auto& constraintIndex = sofa::helper::getReadAccessor(d_constraintIndex);
-
-    const unsigned int nbp = m_state->getSize();
-
-    MatrixDeriv& matrix = *cMatrix.beginEdit();
-
-    unsigned int index = 0;
-    const Vec<3, Real> cx(1.0/Real(nbp),0,0), cy(0,1.0/Real(nbp),0), cz(0,0,1.0/Real(nbp));
-    const Vec<3, Real> vZero(0,0,0);
+    if(d_componentState.getValue() != ComponentState::Valid)
+        return;
 
-    if(d_axis.getValue()[0])
-    {
-        MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
-        for (unsigned int i=0; i<nbp; i++)
-            rowIterator.setCol(i, Deriv(cx,vZero));
-        index++;
-    }
+    if (!d_drawBarycenter.getValue())
+        return;
 
-    if(d_axis.getValue()[1])
-    {
-        MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
-        for (unsigned int i=0; i<nbp; i++)
-            rowIterator.setCol(i, Deriv(cy,vZero));
-        index++;
-    }
-
-    if(d_axis.getValue()[2])
-    {
-        MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
-        for (unsigned int i=0; i<nbp; i++)
-            rowIterator.setCol(i, Deriv(cz,vZero));
-        index++;
-    }
-
-    cIndex+=index;
+    computeBarycenter();
+    Coord barycenter = d_barycenter.getValue();
+    vparams->drawTool()->drawFrame(barycenter.getCenter(), barycenter.getOrientation(), sofa::type::Vec3f(5., 5., 5.));
+}
 
-    cMatrix.endEdit();
-    m_nbLines = cIndex - constraintIndex;
+template<>
+void BarycentricCenterEffector<Rigid3Types>::setBarycenter(const Coord& _barycenter)
+{
+    d_barycenter.setValue(_barycenter);
+    auto barycenter = sofa::helper::getWriteAccessor(d_barycenter);
+    barycenter->getOrientation().normalize();
 }
 
 

diff --git a/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h b/src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h
@@ -28,11 +28,11 @@
 ******************************************************************************/
 #pragma once
 
-#include <SoftRobots.Inverse/component/behavior/Effector.h>
+#include <SoftRobots.Inverse/component/constraint/PositionEffector.h>
 
 namespace softrobotsinverse::constraint
 {
-    using softrobotsinverse::behavior::Effector ;
+    using softrobotsinverse::constraint::PositionEffector ;
     using sofa::core::ConstraintParams ;
     using sofa::linearalgebra::BaseVector ;
     using sofa::core::visual::VisualParams ;
@@ -45,11 +45,11 @@ namespace softrobotsinverse::constraint
  * https://softrobotscomponents.readthedocs.io
 */
 template<class DataTypes>
-class BarycentricCenterEffector : public Effector<DataTypes>
+class SOFA_SOFTROBOTS_INVERSE_API BarycentricCenterEffector : public PositionEffector<DataTypes>
 {
 public:
     SOFA_CLASS(SOFA_TEMPLATE(BarycentricCenterEffector,DataTypes),
-               SOFA_TEMPLATE(Effector,DataTypes));
+               SOFA_TEMPLATE(PositionEffector,DataTypes));
 
     typedef typename DataTypes::VecCoord    VecCoord;
     typedef typename DataTypes::VecDeriv    VecDeriv;
@@ -58,7 +58,7 @@ class BarycentricCenterEffector : public Effector<DataTypes>
     typedef typename DataTypes::Deriv       Deriv;
     typedef typename Coord::value_type      Real;
 
-    typedef typename sofa::core::behavior::MechanicalState<DataTypes>          MechanicalState;
+    typedef typename sofa::core::behavior::MechanicalState<DataTypes> MechanicalState;
     typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator;
 
     typedef sofa::Data<VecCoord>		 DataVecCoord;
@@ -75,15 +75,6 @@ class BarycentricCenterEffector : public Effector<DataTypes>
     /// traversal of graph creation and modification,
     void init() override;
 
-    /// According to BaseObject::reinit
-    /// this method should be used to update the object when the variables used
-    /// in precomputation are modified.
-    void reinit() override;
-
-    /// According to BaseObject::reset
-    /// this method should be used to reset the object in its initial state.
-    void reset() override;
-
     /// According to BaseObject::draw
     /// this method should be used to render internal data of this object,
     /// for debugging purposes.
@@ -101,10 +92,6 @@ class BarycentricCenterEffector : public Effector<DataTypes>
                                 const BaseVector *Jdx) override;
     /////////////////////////////////////////////////////////////////////////
 
-    /////////////// Inherited from BaseSoftRobotsConstraint ////////////////
-    void storeResults(sofa::type::vector<double> &delta) override;
-    ///////////////////////////////////////////////////////////////////////////
-
 protected:
 
     ////////////////////////// Inherited attributes ////////////////////////////
@@ -118,24 +105,32 @@ class BarycentricCenterEffector : public Effector<DataTypes>
     using Effector<DataTypes>::d_constraintIndex ;
     using Effector<DataTypes>::f_listening ;
     using Effector<DataTypes>::getTarget ;
-
+    using PositionEffector<DataTypes>::d_useDirections ;
+    using PositionEffector<DataTypes>::d_directions ;
+    using PositionEffector<DataTypes>::d_effectorGoal ;
+    using PositionEffector<DataTypes>::d_delta ;
+    using PositionEffector<DataTypes>::d_weight ;
+    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;
 
-    sofa::Data<sofa::type::vector<double>> d_delta;
-
-    void initData();
-
     void computeBarycenter();
+    void setBarycenter(const Coord& barycenter){d_barycenter.setValue(barycenter);}
 };
 
 template<> SOFA_SOFTROBOTS_INVERSE_API
-void BarycentricCenterEffector<sofa::defaulttype::Rigid3Types>::buildConstraintMatrix(const ConstraintParams* cParams,
-                                                                                DataMatrixDeriv& cMatrix,
-                                                                                unsigned int& cIndex,
-                                                                                const DataVecCoord& x);
+void BarycentricCenterEffector<sofa::defaulttype::Rigid3Types>::draw(const VisualParams* vparams);
+
+template<> SOFA_SOFTROBOTS_INVERSE_API
+void BarycentricCenterEffector<sofa::defaulttype::Rigid3Types>::setBarycenter(const Coord& barycenter);
 
 #if !defined(SOFTROBOTS_INVERSE_BARYCENTRICCENTEREFFECTOR_CPP)
 extern template class SOFA_SOFTROBOTS_INVERSE_API BarycentricCenterEffector<sofa::defaulttype::Vec3Types >;