How to make a continuous path for a continuum robot tip in inverse mode?

[TeoREN0217]

Hi~ I have solved the problem by setting my own AnimationController and onBeginAnimationStep. But here is another question.
Here is the situation: I build a continuum robot with 3 cable actuators and used the inverse mode by setting a target to make my robot tip to touch it. I have achieved one target and I want to set more targets to touch one by one. I have no idea about setting a continuous path to make my robot get. Should I still use the Animation controller to achieve that?

[EulalieCoevoet]

Hi @TeoREN0217,

I think you could have a look at the responses under this question. I suggest to use the animation tool from the plugin STLIB.

[TeoREN0217]

Hi~Eulalie:
I have tried the answer you mentioned above and I did achieve to draw a path. But here is another problem that is I hope to draw a circle path but I can only draw like 1/4 circle. I'm confused about how to draw a full circle path. Should I adjust my x,y formula or the animate() function params?

[EulalieCoevoet]

Great!
Can you show us your animate() function, with your formula?

[TeoREN0217]

Hi! Here is my animate(). It can draw a circle now but I have to set the duration to 20, otherwise the target will move so fast. I'm confused about what the factor is? is the factor 1sec? or others?

def addEffectors(self, target, position=[0., 0., 195.]):
        def myAnimation(target, factor):  # here you define your animation, factor being the simulation time
            t=factor*180/3.14
            x = 20*cos(t)
            y = 20*sin(t)
            target.effectorGoal.value = [[x, y, 115]]
            # print(target.effectorGoal.value)
        effectors = self.node.addChild('Effectors')
        effectors.addObject('MechanicalObject', position=position)
        effectors.addObject('PositionEffector', indices=list(range(len(position))), effectorGoal=position)
        effectors.addObject('BarycentricMapping', mapForces=False, mapMasses=False)
        effectors.addObject('Monitor', indices=0, showTrajectories=True) 
        animate(myAnimation, {'target': effectors.PositionEffector}, duration=20)

[EulalieCoevoet]

Here's what I would do, see my comments:

from math import pi, sin, cos


def myAnimation(target, radius, factor):  # here you define your animation
    # factor will go from 0 to 1 and depends on the duration of your animation
    # you can have a look at the corresponding code in STLIB, splib3/animation/animate.py
    # factor = (currentTime-startTime) / duration
    print(factor)  # you can print the value to check
    x = radius * cos(factor * 2 * pi)
    y = radius * sin(factor * 2 * pi)
    target.position.value = [[x, y, 0]]  # update the position


def createScene(rootnode):
    from splib3.animation import animate, AnimationManager  # you need these two things
    rootnode.addObject(AnimationManager(rootnode))

    radius = 5

    target = rootnode.addChild('Target')
    target.addObject('MechanicalObject', position=[[radius, 0, 0]], showObject=True, drawMode=2, showObjectScale=1)
    target.addObject('Monitor', indices=0, showTrajectories=True)  # you can use this to display the trajectory
    animate(myAnimation,  # here you give your animation method,
            {'target': target.MechanicalObject, 'radius': radius},  # set your custom parameters, 
            duration=5)  # and give the duration of the animation

[TeoREN0217]

Thanks for your reply! I tried your code and the path looks perfect but my robot doesn't trace that. I build this model from the Trunk demo. It had a func effectorTarget() already to set a point target. Here is the function:

def effectorTarget(parentNode, position=target_position):
    target = parentNode.addChild('Target')
    target.addObject('EulerImplicitSolver', firstOrder=True)
    target.addObject('CGLinearSolver')
    target.addObject('MechanicalObject', name='dofs', position=position, showObject=True, showObjectScale=2, drawMode=2, showColor=[1., 1., 1., 1.])
    target.addObject('UncoupledConstraintCorrection')
    return target

And in it's createScene(), it has these two lines：

target = effectorTarget(rootNode)
trunk.addEffectors(target=target.dofs.getData('position').getLinkPath(), position=[[0., 0., 195]])

I changed my code before:
First, I didn't change the effectorTarget() because it will lead the scene lost.

def effectorTarget(parentNode, position=target_position):
    target = parentNode.addChild('Target')
    target.addObject('EulerImplicitSolver', firstOrder=True)
    target.addObject('CGLinearSolver')
    target.addObject('MechanicalObject', name='dofs', position=position, showObject=True, showObjectScale=2, drawMode=2, showColor=[1., 1., 1., 1.])
    target.addObject('UncoupledConstraintCorrection')
    return target

Second, I change addEffectors() in my class trunk like I showed you before.

def addEffectors(self, target, position=[0., 0., 195.]):
        def myAnimation(target, factor):  # here you define your animation, factor being the simulation time
            t=factor*180/3.14
            x = 20*cos(t)
            y = 20*sin(t)
            target.effectorGoal.value = [[x, y, 115]]
            print(target.effectorGoal.value)
        effectors = self.node.addChild('Effectors')
        effectors.addObject('MechanicalObject', position=position)
        effectors.addObject('PositionEffector', indices=list(range(len(position))), effectorGoal=position)
        effectors.addObject('BarycentricMapping', mapForces=False, mapMasses=False)
        effectors.addObject('Monitor', indices=0, showTrajectories=True) 
        animate(myAnimation, {'target': effectors.PositionEffector}, duration=10)

And in the createScene() has two lines：

target = effectorTarget(rootNode)
trunk.addEffectors(target=target.dofs.getData('position').getLinkPath(), position=robot_decisionpoint)

In this scene, the robot could trace the path I set and the original taget point still there.

I change your demo just like your format and here is only a perfect circle without tracing.