Trunk tutorial: problem in interactions between robot and objects

[camilla-agabiti]

Hi everyone,

I am starting to make some simulations with the cable-driven trunk-like soft robot (https://github.com/SofaDefrost/SoftRobots/tree/master/docs/sofapython3/tutorials/Trunk).

I would like to start from this design a soft manipulator to simulate a whole-arm grasping of an object. I am using the inverse mode in the code, so I set the target position in space that has to be reached by the robot with its end-effector. In this initial phase, I would like to add a simple cube like an obstacle in the achievement of this task: in particular, I added the cube to the simulation with its mechanical, collision and visual models.
However, when I animate the simulation and the robot bends to reach the target, the cube is completely penetrated by the robot, so it seems that there is no interaction between them. Do you have any suggestion about how to solve this ?

trunk_grasp (copy).TXT

Thank you very much!
Camilla

[EulalieCoevoet]

Hi Camilla,

It seems to me that you just forgot to add the collision model of the trunk:

    trunk = Trunk(simulation, inverseMode=inverseMode)
    trunk.addVisualModel(color=[1., 1., 1., 0.8])
    trunk.addCollisionModel()  # <-- forgotten line
    trunk.fixExtremity()

Eulalie

[camilla-agabiti]

Hello Eulalie,

Thanks a lot for your help, I found what I was looking for :)

I also have another thing I would like to discuss with you: I would like to set the simulation of the trunk motion so that the trunk end-effector follows a predefined path. To this aim, I generated a path as a cubic spline by imposing the passing through a start point (P0), and end point (P3) and 2 intermediate points (P1, P2).

The coordinates of these points are:
P0 = [0, 0, 195]
P1 = [60, 0, 150]
P2 = [90, 0, 100]
P3 = [50, 0, 60]

In order to generate this trajectory, I fixed the parameter t taking values in the interval [0, 1]: in this way, the equation of the 3D trajectory is:

x(t) = 180*(t^3) + 45*(t^2) + 185t;
y(t) = 0; (the trajectory in this case is in the x-z plane)
z(t) = 180(t^3) - 90*(t^2) -112.5*t + 195;

I am modifying the python script in order to make the end-effector follow this trajectory: to do this, I would give x(t), y(t) and z(t) instead of a fixed target position.
To do this, I think I should link the simulation time when I launch the animation and the parameter t of the trajectory. I had a look at the SOFA AdvancedTimer, but I don't know if it is correct to act on it to do this. Could you suggest me which would be the right way to implement this?

I hope I explained well my issue.

Thank you very much!

Camilla

[EulalieCoevoet]

Hello Camilla,

I think the AdvancedTimer is a statistics tool to get computation time performances. In your case, if I understood well, I would used the animation tool of STLIB and do something like that:

def myAnimation(target, factor):  # here you define your animation, factor being the simulation time
    x = 180 * (factor ** 3) + 45 * (factor ** 2) + 185 * factor
    y = 180 * (factor ** 3) - 90 * (factor ** 2) - 112.5 * factor
    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))

    target = rootnode.addChild('Target')
    target.addObject('MechanicalObject', position=[[0, 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, {'target': target.MechanicalObject}, duration=1)  # here you give your animation method, 
    # set your custom parameters, and give the duration of the animation. 
    # You can have a look at the link I gave above to see all the other parameters, and some other examples.

[camilla-agabiti]

Hello Eulalie,

Thanks a lot for your suggestion and for your fast reply.

Here below you can find the script of my simulation. I generated the target trajectory, you can visualize it, but the animation is not working, in the sense that the trunk end-effector does not follow that trajectory. I don't know, probably there is something I am missing.. For example, regarding the function addEffectors(self, target, position), which is the argument position I have to pass to it ?

I would be happy if you could have a look at my script.

Thank you again
trunk_grasping.TXT

[EulalieCoevoet]

There are two ways of doing this:

1- You create a node, let's call it Target, with a MechanicalObject that you link to the effectorGoal datafield of the component PositionEffector: effectorGoal=Target.MechanicalObject.position.getLinkPath(). We usually do that when we want to move the target interactively, that is with the mouse while the simulation is running. In this case we need solvers to solve the force the mouse spring applies on the single point to move it. In the trunk tutorial it corresponds to that:

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

What is happening is that, when you're moving the sphere in the simulation, you're changing the position of the MechanicalObject in the node Target which will also change the effectorGoal since both data are linked.

2- If you don't want to move the target interactively but instead use an animation, you can directly feed the datafield effectorGoal. Using for instance the animation tool from STLIB, in the trunk tutorial you could simply change the addEffectors method with:

def addEffectors(self, position=[0., 0., 195.]):
    
        def myAnimation(target, factor):  # here you define your animation, factor being the simulation time
            x = 180 * (factor ** 3) + 45 * (factor ** 2) + 185 * factor
            z = 180 * (factor ** 3) - 90 * (factor ** 2) - 112.5 * factor + 200
            target.effectorGoal.value = [[x, 0, z]]  # update the position
    
        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)
        animate(myAnimation, {'target': effectors.PositionEffector}, duration=1)

and don't forget to add rootnode.addObject(AnimationManager(rootnode)).

[camilla-agabiti]

Hi Eulalie,

Thanks a lot for your support, I really appreciate it.
I implemented your suggestions. Now, the robot animates, but it moves just a little bit without following the whole trajectory. I tried to play a little bit with some parameters, like "duration", or simulation "dt" but it does not improve. I also checked if there are errors or messages in the component of the SOFA graph, but everything looks working.

I have a question: the "factor"parameter is the simulation time step, right?
There could be a conflict between the parametrization of the trajectory I made and the animation time?

I attach again my script so that you can directly see there.
trunk_grasping.TXT

Best,
Camilla