Dynamic Node Constraint for Deformable Object？

[sodasoda520]

Hi SOFA community,

I have a deformable object with mixed rigid-deformable structure. The rigid region control works fine, but I need to dynamically constrain individual nodes for shape correction.

Goal: Add/remove constraints on specific nodes at runtime to correct deformation.

Current approach: Using RestShapeSpringsForceField with dynamic child node creation:

# Create child node for constraint
self.node618_spring_node = self.pi.ElasticBody.MechanicalModel.addChild("Node618Spring")

# Add spring constraint
self.node618_constraint = self.node618_spring_node.addObject(
    'RestShapeSpringsForceField',
    name='node618Spring',
    points=[self.node618_index],
    stiffness=1e12,
    external_rest_shape=dofs.getLinkPath(),
    external_points=[self.node618_index]
)

Error: node618Spring is not associated to any mechanical state

Previous attempts:

• FixedConstraint - template matching issues and mechanical state errors
• Various constraint placement strategies - all failed with mstate association

Question: What's the correct way to dynamically constrain individual nodes in a deformable object? Should I use a different constraint type or modify the mechanical model structure?

Any guidance would be appreciated!

Thanks

My code is here

import Sofa
import Sofa.Core
import math
import numpy as np
from stlib3.scene import Scene
from splib3.numerics.quat import Quat
from splib3.numerics import getOrientedBoxFromTransform, Vec3, sdiv
from splib3.constants import Key
from splib3.objectmodel import setData


# ============================================================================
# Component functions
# ============================================================================
def addOrientedBoxRoi(parentNode, position, name="BoxROI", translation=[0., 0., 0.], 
                     eulerRotation=[0., 0., 0.], scale=[1., 1., 1.], drawBoxes=True):
    orientedBox = getOrientedBoxFromTransform(translation=translation, 
                                             eulerRotation=eulerRotation, 
                                             scale=scale)
    return parentNode.addObject("BoxROI", name=name, position=position, 
                               orientedBox=orientedBox, drawBoxes=drawBoxes)

# ============================================================================
# Elastic body definition
# ============================================================================
def ElasticBody(name="ElasticBody", rotation=[0, 0, 0], translation=[0, 0, 0], 
               color=[0.82, 0.41, 0.12]):
    self = Sofa.Core.Node(name)
    mechanicalmodel = self.addChild("MechanicalModel")
    
    mechanicalmodel.addObject('MeshGmshLoader',
                              name='loader',
                              rotation=rotation,
                              translation=translation,
                              filename="mesh/dense.msh")
    mechanicalmodel.addObject('MeshTopology',
                              src='@loader',
                              name='container')

    mechanicalmodel.addObject('MechanicalObject',
                              name='dofs',
                              position=mechanicalmodel.loader.position.getLinkPath(),
                              showObject=False,
                              showObjectScale=5.0)
    mechanicalmodel.addObject('UniformMass',
                              name="mass",
                              totalMass=0.00355)

    mechanicalmodel.addObject('TetrahedronFEMForceField',
                              name="linearElasticBehavior",
                              youngModulus=2500000,
                              poissonRatio=0.34,
                              method="large",
                              computeVonMisesStress="0",
                              showVonMisesStressPerNode="false")
    
    visualmodel = Sofa.Core.Node("VisualModel")
    visualmodel.addObject('MeshGmshLoader',
                          name='loader',
                          filename="mesh/dense.msh",
                          rotation=rotation,
                          translation=translation)
    visualmodel.addObject('OglModel',
                          src=visualmodel.loader.getLinkPath(),
                          name='renderer',
                          color=color)
    self.addChild(visualmodel)

    visualmodel.addObject('IdentityMapping',
                          input=mechanicalmodel.dofs.getLinkPath(),
                          output=visualmodel.renderer.getLinkPath())
    return self

# ============================================================================
# Mixed material rigidification function
# ============================================================================
def getBarycenter(selectedPoints):
    poscenter = [0., 0., 0.]
    if len(selectedPoints) != 0:
        poscenter = sdiv(sum(selectedPoints), float(len(selectedPoints)))
    return poscenter

def Rigidify(targetObject, sourceObject, groupIndices, frames=None, name=None):
    if frames is None:
        frames = [[0., 0., 0.]]*len(groupIndices)

    assert len(groupIndices) == len(frames), "size mismatch."

    if name is None:
        name = sourceObject.name

    ero = targetObject.addChild(name)

    allPositions = sourceObject.container.position.value
    allIndices = list(range(len(allPositions)))

    rigids = []
    indicesMap = []

    def mfilter(si, ai, pts):
        tmp = []
        for i in ai:
            if i in si:
                tmp.append(pts[i])
        return tmp

    selectedIndices = []
    for i in range(len(groupIndices)):
        selectedPoints = mfilter(groupIndices[i], allIndices, allPositions)
        if len(frames[i]) == 3:
            orientation = Quat.createFromEuler(frames[i], inDegree=True)
            poscenter = getBarycenter(selectedPoints)
        elif len(frames[i]) == 4:
            orientation = frames[i]
            poscenter = getBarycenter(selectedPoints)
        elif len(frames[i]) == 6:
            orientation = Quat.createFromEuler([frames[i][3], frames[i][4], frames[i][5]], inDegree=True)
            poscenter = [frames[i][0], frames[i][1], frames[i][2]]
        elif len(frames[i]) == 7:
            orientation = [frames[i][3], frames[i][4], frames[i][5], frames[i][6]]
            poscenter = [frames[i][0], frames[i][1], frames[i][2]]
        else:
            Sofa.msg_error("Do not understand the size of a frame.")

        rigids.append(poscenter + list(orientation))

        selectedIndices += map(lambda x: x, groupIndices[i])
        indicesMap += [i] * len(groupIndices[i])

    otherIndices = list(filter(lambda x: x not in selectedIndices, allIndices))
    Kd = {v: None for k, v in enumerate(allIndices)}
    Kd.update({v: [0, k] for k, v in enumerate(otherIndices)})
    Kd.update({v: [1, k] for k, v in enumerate(selectedIndices)})
    indexPairs = [v for kv in Kd.values() for v in kv]

    freeParticules = ero.addChild("DeformableParts")
    freeParticules.addObject("MechanicalObject", template="Vec3", name="dofs",
                           position=[allPositions[i] for i in otherIndices])

    rigidParts = ero.addChild("RigidParts")
    rigidParts.addObject("MechanicalObject", template="Rigid3", name="dofs", 
                        reserve=len(rigids), position=rigids)

    rigidifiedParticules = rigidParts.addChild("RigidifiedParticules")
    rigidifiedParticules.addObject("MechanicalObject", template="Vec3", name="dofs",
                                  position=[allPositions[i] for i in selectedIndices])
    rigidifiedParticules.addObject("RigidMapping", name="mapping", 
                                  globalToLocalCoords=True, rigidIndexPerPoint=indicesMap)

    if "solver" in sourceObject.objects:
        sourceObject.removeObject(sourceObject.solver)
    if "integration" in sourceObject.objects:
        sourceObject.removeObject(sourceObject.integration)
    if "correction" in sourceObject.objects:
        sourceObject.removeObject(sourceObject.correction)

    sourceObject.addObject("SubsetMultiMapping", name="mapping", template="Vec3,Vec3",
                          input=[freeParticules.dofs.getLinkPath(),rigidifiedParticules.dofs.getLinkPath()],
                          output=sourceObject.dofs.getLinkPath(),
                          indexPairs=indexPairs)

    rigidifiedParticules.addChild(sourceObject)
    freeParticules.addChild(sourceObject)
    return ero

# ============================================================================
# Fixed box constraint function
# ============================================================================
def FixingBox(parent, target, name='FixingBox',
              translation=[0.0, 0.0, 0.0], eulerRotation=[0.0, 0.0, 0.0], 
              scale=[1.0, 1.0, 1.0]):
    ob = getOrientedBoxFromTransform(translation=translation, 
                                    eulerRotation=eulerRotation, 
                                    scale=scale)

    self = parent.addChild(name)
    self.addObject('BoxROI',
                   orientedBox=ob,
                   name='BoxROI',
                   position=target.dofs.getData('rest_position').getLinkPath(),
                   drawBoxes=False)

    c = self.addChild('Constraint')
    target.addChild(c)

    c.addObject('RestShapeSpringsForceField',
               points=self.BoxROI.getData('indices').getLinkPath(),
               stiffness=1e12)
    return self

# ============================================================================
# PI Plate Model
# ============================================================================
def PI(name="PI"):
    
    def rigidify(self):
        deformableObject = self.ElasticBody.MechanicalModel
        self.ElasticBody.init()

        box = addOrientedBoxRoi(
            self,
            position=deformableObject.dofs.rest_position.value,
            name="RigidBoxROI",
            translation=[20, 2.5, 0.125],
            scale=[1.6, 6, 0.4],
            drawBoxes=True
        )
        box.init()

        rigidIndices = [ind for ind in box.indices.value]
        
        rigidFrame = [20, 2.5, 0.125, 0.0, 0.0, 0.0, 1.0]

        rigidified_struct = Rigidify(
            self,
            sourceObject=deformableObject,
            groupIndices=[rigidIndices],
            frames=[rigidFrame],
            name="RigidifiedStructure"
        )

        rigidified_struct.RigidParts.addObject('FixedConstraint', 
                                             name='fixRigidPart',
                                             indices=[0])

    def addRendering(self):
        setData(self.RigidifiedStructure.RigidParts.dofs, 
                showObject=True,
                showObjectScale=1,
                drawMode=3)
        
        setData(self.RigidifiedStructure.RigidParts.RigidifiedParticules.dofs,
                showObject=False,
                showObjectScale=0.4,
                drawMode=1, 
                showColor=[1., 1., 0., 1.])
        
        setData(self.RigidifiedStructure.DeformableParts.dofs,
                showObject=False,
                showObjectScale=0.3,
                drawMode=2)
   
    def addFixedConstraint(self):
        self.fixedConstraint = FixingBox(
            self,
            self.ElasticBody.MechanicalModel,
            scale=[1.7, 6, 0.4],
            translation=[0, 2.5, 0.125]
        )
        self.fixedConstraint.BoxROI.drawBoxes = True
       
    self = Sofa.Core.Node(name)
    
    self.addChild(ElasticBody(
        color=[0.82, 0.41, 0.12]
    ))
    
    rigidify(self)
    addRendering(self)
    addFixedConstraint(self)
    
    return self

# ============================================================================
# Controller with Node Constraint Issues
# ============================================================================
class MyController(Sofa.Core.Controller):

    def __init__(self, *, mechanicalmodel, **_):
        super().__init__()
        self.pi = mechanicalmodel
        self.displacement  = [-2.0, 2.0, 2.0]
        self.rotation_deg  = [0.0, 10.0, 0.0]
        
        self.node618_displacement = [1.0, 0.5, -0.5]
        self.node618_index = 618
        
        self.node618_constraint = None
        self.node618_spring_node = None

    def onKeypressedEvent(self, event):
        if event["key"] == Key.P:
            self.apply_gripper_transform()
        elif event["key"] == Key.F:
            self.apply_node618_transform()
        elif event["key"] == Key.G:
            self.release_node618_constraint()

    def apply_gripper_transform(self):
        rp = self.pi.RigidifiedStructure.RigidParts.dofs
        cur = list(rp.position.value[0])

        new_xyz = [cur[i] + self.displacement[i] for i in range(3)]

        q_cur = Quat(cur[3:])

        inc_quat = Quat.createFromEuler(
            [math.radians(a) for a in self.rotation_deg])

        q_new = Quat.product(q_cur, inc_quat)
        q_new.normalize()

        rp.position = [new_xyz + list(q_new)]

    def apply_node618_transform(self):
        try:
            self.release_node618_constraint()
            
            mechanical_model = self.pi.ElasticBody.MechanicalModel
            dofs = mechanical_model.dofs
            current_positions = list(dofs.position.value)
            
            if len(current_positions) <= self.node618_index:
                return
            
            current_pos = current_positions[self.node618_index]
            new_pos = [current_pos[i] + self.node618_displacement[i] for i in range(3)]
            
            current_positions[self.node618_index] = new_pos
            dofs.position = current_positions
            
            self.node618_spring_node = self.pi.ElasticBody.MechanicalModel.addChild("Node618Spring")
            
            self.node618_constraint = self.node618_spring_node.addObject(
                'RestShapeSpringsForceField',
                name='node618Spring',
                points=[self.node618_index],
                stiffness=1e12,
                angularStiffness=0,
                external_rest_shape=dofs.getLinkPath(),
                external_points=[self.node618_index]
            )
            
        except Exception as e:
            pass

    def release_node618_constraint(self):
        try:
            if self.node618_spring_node is not None:
                self.pi.ElasticBody.MechanicalModel.removeChild(self.node618_spring_node)
                self.node618_spring_node = None
                self.node618_constraint = None
        except Exception as e:
            pass

# ============================================================================
# Scene creation function
# ============================================================================
def createScene(rootNode):
    pluginList = [
        "Sofa.Component.Constraint.Projective",
        "Sofa.Component.SolidMechanics.Spring"
    ]
    
    scene = Scene(rootNode, gravity=[0, -9810, 0], iterative=False, plugins=pluginList)
    scene.addMainHeader()

    pi = scene.Modelling.addChild(PI())
    scene.Simulation.addChild(pi)
    
    rootNode.addObject(
        MyController(name="Ctrl", mechanicalmodel=pi)
    )
    
    visual_node = rootNode.addChild("Node618Visual")
    visual_node.addObject('MechanicalObject', name='visualMO', position=[[0,0,0]])
    visual_node.addObject('SphereCollisionModel', radius=0.3, color=[1,0,0,1])
    
    return rootNode

# ============================================================================
# Program entry point
# ============================================================================
if __name__ == '__main__':
    pass

[bakpaul]

Hello !

You don't need to add new child if you only want to add a new ForceField to the node. You could directly add it to the original node. But it would require you to call init on this new object after its creation if it is during the simulation.

But I don't think thuis is what you want. The RestShapeSpringForceField has got a 'points' data that specify on which point to apply a spring. Why not just modify this list dynamically ?

Tell me if this solves your issue.

[github-actions[bot]]

⚠️ ⚠️ ⚠️
@sodasoda520
Feedback has been given to you by the project reviewers, however we have not received a response from you. Without further news in the coming weeks, this discussion will be automatically closed in order to keep this forum clean and fresh 🌱 Thank you for your understanding

[github-actions[bot]]

⚠️ ⚠️ ⚠️
@sodasoda520
In accordance with our forum management policy, the last reply is more than 4 months old and is therefore closed. Our objective is to keep the forum up to date and offer the best support experience.

Please feel free to reopen it if the topic is still active by providing us with an update. Please feel free to open a new thread at any time - we'll be happy to help where and when we can.