validation.py

from modules.targets import Targets
import Sofa
import csv
import numpy as np

resultsDirectory = "data/results/"
WITH_POLHEMUS = False
WITH_CAMERA = False

class TargetController(Sofa.Core.Controller):
    """
        A Controller to change the target of Emio, and save the collected data in a CSV file.

        emio: Sofa node of Emio
        target: Sofa node containing a MechanicalObject with the targets position
        effector: PositionEffector component
        assembly: Controller component for the assembly of Emio (set up animation of the legs and center part)
        steps: number of simulation steps to wait before going to the next target  
    """

    def __init__(self, emio, target, effector, assembly, steps=20):
        Sofa.Core.Controller.__init__(self)
        self.name="TargetController"

        self.emio = emio
        self.targetsPosition = target.getMechanicalState().position.value
        self.targetIndex = len(self.targetsPosition) - 1

        self.effector = effector
        self.assembly = assembly
        self.firstTargetReached = False

        self.animationSteps = steps 
        self.animationStep = self.animationSteps
        self.createCSVFile()

        if WITH_POLHEMUS:
            from modules.polhemusUSB import PolhemusUSB
            self.polhemus = PolhemusUSB()

    def onAnimateBeginEvent(self, _):
        """
            Change the target when it's time
        """
        delta = np.array(self.emio.effector.getMechanicalState().position.value[0][0:3]) - np.array(self.targetsPosition[self.targetIndex])
        if np.linalg.norm(delta) < 1:
            self.firstTargetReached = True

        if self.assembly.done and self.firstTargetReached:

            if WITH_POLHEMUS:
                self.polhemus.UpdateSensors()

            self.animationStep -= 1
            if self.targetIndex >= 0 and self.animationStep == 0:
                self.writeToCSVFile()
                self.targetIndex -= 1
                self.animationStep = self.animationSteps
                self.effector.effectorGoal = [list(self.targetsPosition[self.targetIndex]) + [0, 0, 0, 1]]

    def getFilename(self):
        legname = self.emio.legsName[0]
        legmodel = self.emio.legsModel[0]
        return resultsDirectory + legname + "_" + legmodel + '_sphere.csv'

    def createCSVFile(self):
        """
            Clear or create the csv file in which we'll save the data
        """
        with open(self.getFilename(), 'w', newline='') as csvfile:
            csvwriter = csv.writer(csvfile, delimiter=';')
            csvwriter.writerow(["# extended ", self.emio.extended.value])
            csvwriter.writerow(["# legs ", self.emio.legsName.value])
            csvwriter.writerow(["# legs model ", self.emio.legsModel.value])
            csvwriter.writerow(["# legs young modulus ", self.emio.legsYoungModulus.value])
            csvwriter.writerow(["# legs poisson ratio ", self.emio.legsPoissonRatio.value])
            csvwriter.writerow(["# legs position on motor ", self.emio.legsPositionOnMotor.value])
            csvwriter.writerow(["# connector ", self.emio.centerPartName.value])
            csvwriter.writerow(["# connector type ", self.emio.centerPartType.value])
            csvwriter.writerow(["Target", "Simulation", "DepthCamera", "Polhemus"])

    def writeToCSVFile(self):
        """
            Save the data in a csv file
        """
        with open(self.getFilename(), 'a', newline='') as csvfile:
            csvwriter = csv.writer(csvfile, delimiter=';')
            csvwriter.writerow([self.targetsPosition[self.targetIndex], 
                                 self.emio.effector.getMechanicalState().position.value[0][0:3],
                                 self.emio.getRoot().DepthCamera.getMechanicalState().position.value[0][0:3] if WITH_CAMERA else [0, 0, 0],
                                 self.polhemus.sensors[0].GetLastPosition() if WITH_POLHEMUS else [0, 0, 0]])

def createScene(rootnode):
    """
        Emio simulation
    """
    from emio.utils.header import addHeader, addSolvers
    from emio.parts.controllers.assemblycontroller import AssemblyController
    from emio.parts.controllers.trackercontroller import DotTracker
    from emio import Emio

    settings, modelling, simulation = addHeader(rootnode, inverse=True)

    rootnode.dt = 0.04
    rootnode.gravity = [0., -9810., 0.]
    addSolvers(simulation)

    # Add Emio to the scene
    emio = Emio(name="Emio",
                legsName=["blueleg"],
                legsModel=["tetra"],
                legsPositionOnMotor=["counterclockwisedown","clockwisedown","counterclockwisedown","clockwisedown"],
                centerPartName="bluepart",
                centerPartType="rigid",
                extended=True)
    if not emio.isValid():
        return
    
    # get all legs and set yougModulus in TetrahedronFEMForceField
    for leg in emio.legs:
        leg.getChild(leg.name.value + "DeformablePart").Leg.getObject("TetrahedronFEMForceField").youngModulus.value = [7000.]

    simulation.addChild(emio)
    emio.attachCenterPartToLegs()
    assembly = AssemblyController(emio)
    emio.addObject(assembly)

    # Generation of the targets
    spherePositions = Targets(ratio=0.1, center=[0, -130, 0], size=80).sphere()
    sphere = modelling.addChild("SphereTargets")
    sphere.addObject("MechanicalObject", position=spherePositions, showObject=True, showObjectScale=10, drawMode=0)

    # Effector
    emio.effector.addObject("MechanicalObject", template="Rigid3", position=[0, 0, 0, 0, 0, 0, 1])
    emio.effector.addObject("RigidMapping", index=0)

    # Inverse components and GUI
    emio.addInverseComponentAndGUI(spherePositions[-1]+[0, 0, 0, 1], withGUI=False)
    emio.effector.EffectorCoord.maxSpeed.value = 100 # Limit the speed of the effector's motion

    # Components for the connection to the real robot 
    emio.addConnectionComponents()

    # We add a controller to go through the targets
    rootnode.addObject(TargetController(emio=emio,
                                        target=sphere, 
                                        effector=emio.effector.EffectorCoord, 
                                        assembly=assembly,
                                        steps=30))
    
    if WITH_CAMERA:
        # Add depth camera tracker (distributed with Emio) 
        rootnode.addObject(DotTracker(name="DotTracker",
                                    root=rootnode,
                                    configuration="extended",
                                    nb_tracker=1, # We only look for one marker
                                    show_video_feed=True,
                                    track_colors=True)) # We track the color of the marker (green by default)

    return rootnode