[SOLVED] Moving the starting point of a beam

[Benjamin]

Hello everyone,

I currently try to create a forward dynamic model of a continuum robot. The robot is actuated by slinding actuator that push and pull a beam. To create the forward dynamic model of this robot I have to move the point presenting the actuation system which is linked to the beam. To do this, I remove the sliding actuator component because if I well understood, its displacement property is only an output but it can not be controlled. So, I decided to directly move the first position of the beam inside a controller. The problem is that I don’t succeed to move it and i don’t know why. I try several thing but i think it is a link with this point that is missing.

For example, I looked at @damien-marchaluniv-lille1-fr ‘s answer of the forum : Moving_model_with_controller. I try to understand it and I modify it successfully in order to do an automatique cissor. I think what my code lacking is a link like: r.createObject("RigidRigidMapping", name="map", initialPoints=r.transform.findData("rest_position").getLinkPath()) but I did not succeed to find the right command in order to link the position of only one index of a MechanicalObject’s position. Do you have any idea ?

Here is the current short code that i am using to develop it before integrating to the robot:

import Sofa
import numpy
from splib.constants import Key
from math import sin, cos, sqrt
from splib.numerics import vec3
from splib.objectmodel import SofaPrefab
from stlib.visuals import VisualModel
from stlib.components import addOrientedBoxRoi
from splib.animation import AnimationManager

PI = 3.14159265359

deltaT=1
indexQ1=0
class MyController(Sofa.PythonScriptController):
        def __init__(self,mecaMod):
                self.mecaMod=mecaMod
                self.time=0

        def onBeginAnimationStep(self,dt):
            self.time=self.time+dt
            # print(self.mecaMod.getObject('MoPos').position[0])
            print(self.mecaMod.getObject('MoPos').findData("position").value[0])
            
            posAct1List=self.mecaMod.getObject('MoPos').findData("position").value[indexQ1]
            print(posAct1List)

            if self.time>deltaT:
                jb=(self.time-deltaT)*1.0
                posAct1List[0]=jb
                print(posAct1List)

            print('positionQ1:',self.mecaMod.getObject('MoPos').position[indexQ1])
            # self.mecaMod.getObject('MoPos').findData("position").value[indexQ1]=posAct1List
            # self.mecaMod.getObject('MoPos').findData("rest_position").value[indexQ1]=posAct1List
            self.mecaMod.MoPos.position[indexQ1]=posAct1List
            self.mecaMod.MoPos.rest_position[indexQ1]=posAct1List

def createScene(rootNode):
    
    rootNode.gravity=('0 0 0')  #(N)
    rootNode.createObject('RequiredPlugin',pluginName='SoftRobots BeamAdapter SofaPython SofaSparseSolver')
    
    AnimationManager(rootNode)
    
    rootNode.createObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels hideMappings showForceFields showInteractionForceFields')
    rootNode.dt=0.01

    MecaModelNode = rootNode.createChild('MecaModel')
    MecaModelNode.createObject('EulerImplicitSolver', rayleighStiffness=0.0, printLog=False, rayleighMass=0.0)      #Parametres de convergence
    MecaModelNode.createObject('SparseLDLSolver', name='ldl')
    MecaModelNode.createObject('GenericConstraintCorrection', name='GCC', solverName='ldl')
    MecaModelNode.createObject('MeshTopology', name="MeshEdges", edges=['0 1','1 2','2 3'])
    positionT=['0.0 0.0 0.0    0.0 0.0 0.0 1.0', '10.0 0.0 0.0      0.0 0.0 0.0 1.0', '20.0 0.0 0.0    0.0 0.0 0.0 1.0', '30.0 0.0 0.0      0.0 0.0 0.0 1.0']

    MoPos=MecaModelNode.createObject('MechanicalObject', template='Rigid3d',name="MoPos", showObject='true', position=positionT)
    # MecaModelNode.createObject("RigidRigidMapping", name="map", initialPoints=MecaModelNode.MoPos.findData("rest_position").getLinkPath(), index='0') 

    DOF0TransformNode0T=['0.0 0.0 0.0    0.0 0.0 0.0 1.0', '0.0 0.0 0.0    0.0 0.0 0.0 1.0', '0.0 0.0 0.0    0.0 0.0 0.0 1.0']
    DOF1TransformNode1T=['0.0 0.0 0.0    0.0 0.0 0.0 1.0', '0.0 0.0 0.0    0.0 0.0 0.0 1.0', '0.0 0.0 0.0    0.0 0.0 0.0 1.0']
    
    # MecaModelNode.createObject('LinearMovementConstraint', template='Rigid3d', name='LinCnst', indices='0', keyTimes='1 1.5 2',movements="0 0 0   0 0 0  1 0 0   0 0 0")

    # MecaModelNode.createObject('PartialFixedConstraint', name='ConstraintPlat', indices='0', fixedDirections='0 1 1 1 1 1')

    Interpol = MecaModelNode.createObject('BeamInterpolation', name='interpolation', crossSectionShape='circular', radius='0.5', innerRadius='0.0', dofsAndBeamsAligned='false', defaultYoungModulus='160e6' , DOF0TransformNode0=DOF0TransformNode0T, DOF1TransformNode1=DOF1TransformNode1T)

    MecaModelNode.createObject('AdaptiveBeamForceFieldAndMass', massDensity=8.0e-6, name='LinkForceField', interpolation='@interpolation', reinforceLength=1)
    MyController(MecaModelNode)

    return rootNode

Have a good day,
Benjamin

[Benjamin]

I solved the problem. To move the mecanical object, all the involved positions need to be specified and not only one.

[Author]

Posts