Issue with cable displacement values output of a multi-segment continuum robot value (Inverse Model).

[spicyram3n]

hello guys, I need your help regarding an issue in SOFA, the following is the code implemented to get the displacement of the cable1 when the end effector is moved to a certain goal (inverse model), but I'm getting NaN as the output value for the displacement can somene guide me with what's wrong with my code

from math import cos, sin, pi
import os
import Sofa.Core
from splib3.loaders import loadPointListFromFile
path = os.path.dirname(os.path.abspath(__file__)) + '/mesh/'
import Sofa

class DataSender(Sofa.Core.Controller):
    
    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        
        self.node = kwargs["node"]
        self.finger = self.node.getChild("finger")
        self.cable1 = self.finger.getChild('cable1')
        self.cable1Actuator = self.cable1.cable
        
    def onAnimateBeginEvent(self, event):
        cable1Disp = self.cable1Actuator.findData('displacement').value
        print(f'Cable 1 displacement = {cable1Disp}')
         

def createScene(rootNode):
    rootNode.addObject('RequiredPlugin',
                       pluginName='SoftRobots SoftRobots.Inverse SofaCUDA')
    rootNode.addObject('VisualStyle',
                       displayFlags='showVisualModels hideBehaviorModels showCollisionModels '
                                    'hideBoundingCollisionModels hideForceFields showInteractionForceFields '
                                    'hideWireframe')

    rootNode.addObject('FreeMotionAnimationLoop')
    rootNode.addObject('QPInverseProblemSolver', printLog=False)

    rootNode.gravity = [0, -981.0, 0]
    rootNode.dt = 1.0

    ##########################################
    # Effector goal for interactive control  #
    ##########################################
    goal = rootNode.addChild('goal')
    goal.addObject('EulerImplicitSolver', firstOrder=True)
    goal.addObject('CGLinearSolver', iterations=1000,
                   tolerance=1e-5, threshold=1e-5)
    goal.addObject('MechanicalObject', name='goalMO',
                   position=[0, 170, 0])
    goal.addObject('SphereCollisionModel', radius=5)
    goal.addObject('UncoupledConstraintCorrection', defaultCompliance=1e-5)

    ##########################################
    # FEM Model                              #
    ##########################################
    finger = rootNode.addChild('finger')
    finger.addObject('EulerImplicitSolver', firstOrder=True,
                     rayleighMass=0.1, rayleighStiffness=0.1)
    finger.addObject('SparseLDLSolver',
                     template="CompressedRowSparseMatrixMat3x3d")
    finger.addObject('MeshGmshLoader', name='loader',
                     filename=path + 'pookie5.msh')
    finger.addObject('MeshTopology', src='@loader', name='container')
    finger.addObject('MechanicalObject', name='myMechObject',
                     template='Vec3d', showObject='1')
    finger.addObject('UniformMass', totalMass=0.2)
    finger.addObject('TetrahedronFEMForceField', poissonRatio=0.3897, youngModulus=2410, computeVonMisesStress="0", showVonMisesStressPerNode="false")
    finger.addObject('BoxROI', name='ROI1',
                     box=[-20, -6, -20, 20, 4, 20], drawBoxes=True)
    finger.addObject('RestShapeSpringsForceField',
                     points='@ROI1.indices', stiffness=1e12)
    finger.addObject('LinearSolverConstraintCorrection')

    ##########################################
    # Visualization                          #
    ##########################################
    fingerVisu = finger.addChild('visu')
    fingerVisu.addObject('MeshSTLLoader', filename=path +
                         "pookie5.stl", name="loader")
    fingerVisu.addObject('OglModel', src="@loader", color=[0.0, 0.7, 0.7, 1])
    fingerVisu.addObject('BarycentricMapping')

    ##########################################
    # Cable                                  #
    ##########################################
    idx = 1
    cables = []
    for i in range(1, 4):
        for j in range(1, 5):
            cable = finger.addChild(f'cable{idx}')
            cable.addObject('MechanicalObject', position=loadPointListFromFile(
                f"/home/comrade/sofa_codes/src/test/cables/cs{i}_{j}.json"))
            cable.addObject('CableActuator', name='cable', indices=list(range(
                16)), pullPoint=[0, -20, 0], maxPositiveDisp=10, maxDispVariation=0.5, minForce=0)
            cable.addObject('BarycentricMapping')
            cables.append(cable)
            idx += 1
    
    # cable.addObject('BarycentricMapping')

    ##########################################
    # Effector                               #
    ##########################################
    effector = finger.addChild('fingertip')
    effector.addObject('MechanicalObject', position=([0, 168, 0]))
    effector.addObject('PositionEffector', template='Vec3d',
                       indices=0,
                       effectorGoal="@../../goal/goalMO.position")
    effector.addObject('BarycentricMapping', mapForces=False, mapMasses=False)

    rootNode.addObject(DataSender(node=rootNode))

here is the code written for the multi segment continuum robot

[hugtalbot]

Hi @Poorna-Sasank

Are you having these NaN directly at simulation start? or do these NaN appear after some time steps ?
Best,

[spicyram3n]

Hello,

I tried direct modeling too and I can see the tendons(cables) when actuated are not in contact with the visual model properly. I followed a similar modeling strategy as in the modeling of a cable actuated finger given in SOFA examples.
I will possibly attach a few pics by tomorrow.

Thank you for your time 😄

[hugtalbot]

Have you tried decreasing the time step ?

[spicyram3n]

It was 0.6 when I tried the simulation. What would you recommend?

[hugtalbot]

Time is something that can no be guessed. It depends :

• on the frequency of the phenomenon you simulate (resonance frequency of your bodies)
• on the mesh size
• on the integration scheme (ODESolver) you use

A good practice is to start with a very small one with regards to the phenomenon you simulate and increase it progressively as long as it does not affect the accuracy of your simulation. This work is usually called a sensibility analysis

[spicyram3n]

Ok i will try to find it out and update the situation here 😃

[github-actions[bot]]

⚠️ ⚠️ ⚠️
@Poorna-Sasank
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]]

⚠️ ⚠️ ⚠️
@Poorna-Sasank
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.