cloth_place.py

"""
Scene file for cloth on table.
To check deformations of the cloth under Attach Constraint and Fixed Constraint
Current script uses only Attach constraint at the 2 red balls to grasp the cloth
To add Fixed constraint uncomment line 62

Note: The red balls are supposed to move together in simulation and thus the cloth must follow them with minimum deformations
at the grasped vertices
"""


import os
import numpy as np
import Sofa
import random
import time
import sys


class workspace (Sofa.PythonScriptController):

    def __init__(self, node, commandLineArguments) : 
        self.commandLineArguments = commandLineArguments
        print "Command line arguments for python : "+str(commandLineArguments)
        self.node = node
        self.createGraph(node)
        print 'INIT GRAPH CALLED'
    
        
        self.timeStep = 0
        self.random_start = 15
        self.node.animate = 1
        
        return None;

    def createGraph(self,rootNode):

        self.gripped = 1
        # rootNode
        rootNode.createObject('VisualStyle', displayFlags='hideBehaviorModels hideCollisionModels hideMappings hideForceFields showVisualModels hideInteractionForceFields')
        rootNode.createObject('RequiredPlugin', name='SofaMiscCollision')
        rootNode.createObject('RequiredPlugin', pluginName='SofaPython', name='SofaPython')
        rootNode.createObject('FreeMotionAnimationLoop', solveVelocityConstraintFirst='0')
        rootNode.createObject('LCPConstraintSolver', mu='0.9', name='LCPConstraintSolver', maxIt='1000', printLog='0', initial_guess='false', multi_grid='false', build_lcp='true', tolerance='1e-6')
        rootNode.createObject('CollisionPipeline', draw='0', depth='6', verbose='0')
        rootNode.createObject('BruteForceDetection', name='N2')
        rootNode.createObject('LocalMinDistance', useLMDFilters='0', contactDistance='01', alarmDistance='03', name='Proximity')
        #rootNode.createObject('MinProximityIntersection', contactDistance='01', alarmDistance='03', name='Proximity')
        rootNode.createObject('CollisionResponse', name='Response', response='FrictionContact')

        # rootNode/SquareGravity
        SquareGravity = rootNode.createChild('SquareGravity')
        self.SquareGravity = SquareGravity
        SquareGravity.createObject('EulerImplicitSolver', name='cg_odesolver', printLog='false')
        SquareGravity.createObject('CGLinearSolver', threshold='1.0e-100', tolerance='1.0e-9', name='linear solver', iterations='10')
        SquareGravity.createObject('MeshGmshLoader', scale='100', name='loader', translation='0 50 -80', createSubelements='true', rotation='90 0 0', filename='mesh/square3.msh')
        SquareGravity.createObject('TriangleSetTopologyContainer', src='@loader', name='topo')
        SquareGravity.createObject('TriangleSetGeometryAlgorithms', recomputeTrianglesOrientation='1', name='algo', template='Vec3d')
        SquareGravity.createObject('MechanicalObject', src='@loader', name='cloth', template='Vec3d')
        SquareGravity.createObject('UniformMass', template="Vec3d", mass="0.07")
        #SquareGravity.createObject('DiagonalMass', name='DiagonalMass', massDensity='0.01')
        SquareGravity.createObject('TriangularFEMForceField', name='TriangularFEMForceField', youngModulus='1000', template='Vec3d', method='large', poissonRatio='0.1')
        SquareGravity.createObject('TriangularBendingSprings', template='Vec3d', damping='2.0', stiffness='10', name='FEM-Bend')
        SquareGravity.createObject('UncoupledConstraintCorrection', compliance='100')
        #SquareGravity.createObject('FixedConstraint', indices='2 3') # -- Change Here

        # rootNode/SquareGravity/TriangularSurface
        TriangularSurface = SquareGravity.createChild('TriangularSurface')
        self.TriangularSurface = TriangularSurface
        TriangularSurface.createObject('TriangleSetTopologyContainer', name='Container')
        TriangularSurface.createObject('TriangleSetTopologyModifier', name='Modifier')
        TriangularSurface.createObject('Tetra2TriangleTopologicalMapping', input='@../topo', output='@Container')
        TriangularSurface.createObject('Triangle', name='models')
        TriangularSurface.createObject('Line')
        TriangularSurface.createObject('Point')

        # rootNode/SquareGravity/clothVisual
        clothVisual = SquareGravity.createChild('clothVisual')
        self.clothVisual = clothVisual
        clothVisual.createObject('OglModel', color='red', src='@../loader', name='Visual', template='ExtVec3f')
        clothVisual.createObject('IdentityMapping', input='@..', output='@Visual')

        # rootNode/SquareGravity/Edge Mesh
        Edge_Mesh = SquareGravity.createChild('Edge Mesh')
        self.Edge_Mesh = Edge_Mesh
        Edge_Mesh.createObject('EdgeSetTopologyContainer', name='Container')
        Edge_Mesh.createObject('EdgeSetTopologyModifier', name='Modifier')
        Edge_Mesh.createObject('EdgeSetGeometryAlgorithms', drawEdges='1', name='GeomAlgo', template='Vec3d')
        Edge_Mesh.createObject('EdgeSetTopologyAlgorithms', name='TopoAlgo', template='Vec3d')
        Edge_Mesh.createObject('Triangle2EdgeTopologicalMapping', input='@../topo', name='Mapping', output='@Container')
        Edge_Mesh.createObject('MeshSpringForceField', name='MeshSpringForceField', template='Vec3d', stiffness='100', damping='1.0')

        # rootNode/SquareGravity/Edge Mesh/Visu
        Visu = Edge_Mesh.createChild('Visu')
        self.Visu = Visu
        #SquareGravity.createObject('PlaneForceField', normal='0 1 0', d='-5', stiffness='0.5', damping='0.3')

        # rootNode/Input
        Input = rootNode.createChild('Input')
        self.Input = Input
        #Input.createObject('PythonScriptController', classname='clothMesh', filename='keyboardControl.py')
        Input.createObject('MechanicalObject', position='0 50 -80  0  0  0  1.0', name='DOFs', template='Rigid')

        # rootNode/Input/VisuAvatar
        VisuAvatar = Input.createChild('VisuAvatar')
        self.VisuAvatar = VisuAvatar
        VisuAvatar.activated = 'false'
        VisuAvatar.createObject('OglModel', color='blue', scale='2', name='Visual', fileMesh='mesh/sphere.obj')
        VisuAvatar.createObject('RigidMapping', input='@..', output='@Visual')

        # rootNode/Input/RefModel
        RefModel = Input.createChild('RefModel')
        self.RefModel = RefModel
        RefModel.createObject('MeshObjLoader', name='loader', filename='mesh/sphere.obj')
        RefModel.createObject('Mesh', src='@loader')
        RefModel.createObject('MechanicalObject', src='@loader', name='instrumentCollisionState')
        RefModel.createObject('RigidMapping')

        Input_2 = rootNode.createChild('Input_2')
        self.Input_2 = Input_2
        #Input.createObject('PythonScriptController', classname='clothMesh', filename='keyboardControl.py')
        Input_2.createObject('MechanicalObject', position='100 50 -80  0  0  0  1.0', name='DOFs', template='Rigid')

        # rootNode/Input/VisuAvatar
        VisuAvatar = Input_2.createChild('VisuAvatar')
        self.VisuAvatar = VisuAvatar
        VisuAvatar.activated = 'false'
        VisuAvatar.createObject('OglModel', color='blue', scale='2', name='Visual', fileMesh='mesh/sphere.obj')
        VisuAvatar.createObject('RigidMapping', input='@..', output='@Visual')

        # rootNode/Input/RefModel
        RefModel = Input_2.createChild('RefModel')
        self.RefModel = RefModel
        RefModel.createObject('MeshObjLoader', name='loader', filename='mesh/sphere.obj')
        RefModel.createObject('Mesh', src='@loader')
        RefModel.createObject('MechanicalObject', src='@loader', name='instrumentCollisionState')
        RefModel.createObject('RigidMapping')

        # rootNode/ball
        ball = rootNode.createChild('ball')
        self.ball = ball
        ball.createObject('EulerImplicitSolver', rayleighStiffness='0.05', name='ODE solver', rayleighMass='0.0')
        ball.createObject('CGLinearSolver', threshold='10e-100', tolerance='1e-10', name='linear solver', iterations='10')
        ball.createObject('MechanicalObject', translation='0 50 -80', name='ballState', template='Rigid3d')
        ball.createObject('UniformMass', name='mass', totalmass='0.0')
        ball.createObject('LCPForceFeedback', activate='true', forceCoef='0.001')
        ball.createObject('UncoupledConstraintCorrection', compliance='100')

        # rootNode/ball/ball_surf
        ball_surf = ball.createChild('ball_surf')
        self.ball_surf = ball_surf
        ball_surf.createObject('MeshObjLoader', name='meshLoader', filename='mesh/sphere.obj')
        ball_surf.createObject('Mesh', src='@meshLoader')
        ball_surf.createObject('MechanicalObject', src='@meshLoader', scale='2', name='ballCollisionState', template='Vec3d')
        ball_surf.createObject('UniformMass', totalMass='0.0')
        ball_surf.createObject('UncoupledConstraintCorrection', compliance='1')
        ball_surf.createObject('RigidMapping', input='@..', output='@ballCollisionState')

        # rootNode/ball/ball_surf/VisualModel
        VisualModel = ball_surf.createChild('VisualModel')
        self.VisualModel = VisualModel
        VisualModel.createObject('OglModel', color='1.0 0.2 0.2 1.0', src='@../meshLoader', scale='2', name='ballVisualModel')
        VisualModel.createObject('IdentityMapping', input='@..', output='@ballVisualModel')
        ball.createObject('Sphere')
        ball.createObject('VectorSpringForceField', object1='@Input/RefModel/instrumentCollisionState', object2='@ball/ball_surf/ballCollisionState', viscosity='0', stiffness='100000')
        rootNode.createObject('AttachConstraint', indices1='0', name='attachConstraint', indices2='3', velocityFactor='1.0', responseFactor='1.0', object1='@ball/ball_surf', object2='@SquareGravity', positionFactor='1.0', constraintFactor='1', twoWay='true')
        #rootNode.createObject('AttachConstraint', indices1='0', name='attachConstraint', indices2='3', radius=2.0, object1='@ball/ball_surf', object2='@SquareGravity', constraintFactor='1', twoWay='false')

        # rootNode/ball
        ball_2 = rootNode.createChild('ball_2')
        self.ball_2 = ball_2
        ball_2.createObject('EulerImplicitSolver', rayleighStiffness='0.05', name='ODE solver', rayleighMass='0.0')
        ball_2.createObject('CGLinearSolver', threshold='10e-100', tolerance='1e-10', name='linear solver', iterations='10')
        ball_2.createObject('MechanicalObject', translation='100 50 -80', name='ballState', template='Rigid3d')
        ball_2.createObject('UniformMass', name='mass', totalmass='0.0')
        ball_2.createObject('LCPForceFeedback', activate='true', forceCoef='0.001')
        ball_2.createObject('UncoupledConstraintCorrection', compliance='100')

        # rootNode/ball/ball_2_surf
        ball_2_surf = ball_2.createChild('ball_2_surf')
        self.ball_2_surf = ball_2_surf
        ball_2_surf.createObject('MeshObjLoader', name='meshLoader', filename='mesh/sphere.obj')
        ball_2_surf.createObject('Mesh', src='@meshLoader')
        ball_2_surf.createObject('MechanicalObject', src='@meshLoader', scale='2', name='ball2CollisionState', template='Vec3d')
        ball_2_surf.createObject('UniformMass', totalMass='0.0')
        ball_2_surf.createObject('UncoupledConstraintCorrection', compliance='1')
        ball_2_surf.createObject('RigidMapping', input='@..', output='@ball2CollisionState')

        # rootNode/ball/ball_2_surf/VisualModel
        VisualModel = ball_2_surf.createChild('VisualModel')
        self.VisualModel = VisualModel
        VisualModel.createObject('OglModel', color='1.0 0.2 0.2 1.0', src='@../meshLoader', scale='2', name='ball2VisualModel')
        VisualModel.createObject('IdentityMapping', input='@..', output='@ball2VisualModel')
        ball_2.createObject('Sphere')
        ball_2.createObject('VectorSpringForceField', object1='@Input_2/RefModel/instrumentCollisionState', object2='@ball_2/ball_2_surf/ball2CollisionState', viscosity='0', stiffness='100000')
        rootNode.createObject('AttachConstraint', indices1='0', name='attachConstraint2', indices2='2', velocityFactor='1.0', responseFactor='1.0', object1='@ball_2/ball_2_surf', object2='@SquareGravity', positionFactor='1.0', constraintFactor='1', twoWay='true')
       
        # rootNode/goal1
        goal1 = rootNode.createChild('goal1')
        self.goal1 = goal1
        goal1.activated = 'true'
        goal1.createObject('MechanicalObject', name='goalState1', template='Rigid')

        # rootNode/goal1/VisualModel1
        VisualModel1 = goal1.createChild('VisualModel1')
        self.VisualModel1 = VisualModel1
        VisualModel1.createObject('OglModel', color='0.0 1.0 0.0 1.0', scale='2', name='goalVisualModel1', fileMesh='mesh/sphere.obj')
        VisualModel1.createObject('RigidMapping', input='@goalState1', name='MM->VM mapping', output='@goalVisualModel1')

        # rootNode/goal2
        goal2 = rootNode.createChild('goal2')
        self.goal2 = goal2
        goal2.activated = 'true'
        goal2.createObject('MechanicalObject', name='goalState2', template='Rigid')

        # rootNode/goal2/VisualModel2
        VisualModel2 = goal2.createChild('VisualModel2')
        self.VisualModel2 = VisualModel2
        VisualModel2.createObject('OglModel', color='0.0 1.0 0.0 1.0', scale='2', name='goalVisualModel2', fileMesh='mesh/sphere.obj')
        VisualModel2.createObject('RigidMapping', input='@goalState2', name='MM->VM mapping', output='@goalVisualModel2')

        # rootNode/Floor
        Floor = rootNode.createChild('Floor')
        self.Floor = Floor
        Floor.createObject('MeshObjLoader', filename='mesh/floorFlat.obj', translation='50 -10 50', scale='5', triangulate='true', name='loader')
        Floor.createObject('MeshTopology', src='@loader')
        Floor.createObject('MechanicalObject', src='@loader', name='DOFs', template='Vec3d')
        Floor.createObject('Triangle', moving='0', contactFriction='100', simulated='0')
        Floor.createObject('Line', moving='0', contactFriction='100', simulated='0')
        Floor.createObject('Point', moving='0', contactFriction='100', simulated='0')
        Floor.createObject('OglModel', name='FloorV')


        
        return 0;


    def onScriptEvent(self,senderNode, eventName, data):
        ## Please feel free to add an example for a simple usage in /home/rjangir/softwareInstalled/sofa/src/applications/plugins/SofaPython/scn2python.py
        self.SquareGravity.getObject('cloth').applyTranslation(random.randint(-self.random_start, self.random_start), 0, random.randint(-self.random_start, self.random_start))
        return 0;

    def onEndAnimationStep(self, deltaTime):
        ## Please feel free to add an example for a simple usage in /home/rjangir/softwareInstalled/sofa/src/applications/plugins/SofaPython/scn2python.py
        return 0;

    def reset(self):
        ## Please feel free to add an example for a simple usage in /home/rjangir/softwareInstalled/sofa/src/applications/plugins/SofaPython/scn2python.py
        #print "Reset being called in the workspace Python file", self.timeStep                
        return 0;

    def onBeginAnimationStep(self, deltaTime):
        ## Please feel free to add an example for a simple usage in /home/rjangir/softwareInstalled/sofa/src/applications/plugins/SofaPython/scn2python.py
        return 0;


def createScene(rootNode):
    rootNode.findData('dt').value = '0.05'
    try : 
        sys.argv[0]
    except :
        commandLineArguments = []
    else :
        commandLineArguments = sys.argv
    myworkspace = workspace(rootNode,commandLineArguments)
    return 0;