Python Compliant.Tools类代码示例

    def setManually(self, filepath=None, offset=[[0,0,0,0,0,0,1]], voxelSize=0.01, density=1000, generatedDir=None):
        if len(offset) == 0:
            Sofa.msg_error("RigidScale.API","ShearlessAffineBody should have at least 1 ShearLessAffine")
            return
        self.framecom = Frame.Frame()
        self.bodyOffset = Frame.Frame([0,0,0,0,0,0,1])
        path_affine_rigid = '@'+ Tools.node_path_rel(self.affineNode, self.rigidNode)
        path_affine_scale = '@'+ Tools.node_path_rel(self.affineNode, self.scaleNode)
        if len(offset) == 1: self.frame = [Frame.Frame(offset[0])]
        str_position = ""
        for p in offset:
            str_position = str_position + concat(p) + " "

        ### scene creation
        # rigid dof
        self.rigidDofs = self.rigidNode.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs', position=str_position, rest_position=str_position)

        # scale dofs
        self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position=concat([1,1,1]*len(offset)))
        positiveNode = self.scaleNode.createChild('positive')
        positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
        positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3'+template_suffix+',Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
        positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
        positiveNode.createObject('UnilateralConstraint')
        positiveNode.createObject('Stabilization', name='Stabilization')

        # affine dofs
        self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='parent', showObject=0)
        self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')
        if filepath:
            self.image = SofaImage.API.Image(self.affineNode, name="image_" + self.name, imageType="ImageUC")
            self.shapeFunction = Flexible.API.ShapeFunction(self.affineNode)
            if generatedDir is None:
                self.image.addMeshLoader(filepath, value=1, insideValue=1, scale=scale3d)  # TODO support multiple meshes closingValue=1,
                self.image.addMeshToImage(voxelSize)
                self.shapeFunction.addVoronoi(self.image, position='@dofs.rest_position')
                # mass
                self.affineMassNode = self.affineNode.createChild('mass')
                self.affineMassNode.createObject('TransferFunction', name='density', template='ImageUC,ImageD', inputImage='@../image.image', param='0 0 1 '+str(density))
                self.affineMassNode.createObject('MechanicalObject', template='Vec3'+template_suffix)
                self.affineMassNode.createObject('LinearMapping', template='Affine,Vec3'+template_suffix)
                self.affineMassNode.createObject('MassFromDensity',  name='MassFromDensity', template='Affine,ImageD', image='@density.outputImage', transform='@../image.transform', lumping='0')
                self.mass = self.affineNode.createObject('AffineMass', massMatrix='@mass/MassFromDensity.massMatrix')
            else:
                self.image.addContainer(filename=self.node.name + "_rasterization.raw", directory=generatedDir)
                self.shapeFunction.shapeFunction = serialization.importImageShapeFunction(self.affineNode, generatedDir+self.node.name+"_SF_indices.raw", generatedDir+self.node.name+"_SF_weights.raw", 'dofs')
                self.mass = serialization.importAffineMass(self.affineNode, generatedDir+self.node.name+"_affinemass.json")

            # computation of the object mass center
            massInfo = SofaPython.mass.RigidMassInfo()
            massInfo.setFromMesh(filepath, density, [1,1,1])
            # get the object mass center
            self.framecom.rotation = massInfo.inertia_rotation
            self.framecom.translation = massInfo.com
        else:
            print "You need a mesh to create an articulated system"
        self.frame = []
        for o in offset:
            self.frame.append(Frame.Frame(o))

    def __insert_registration_node(self):
        """ Insert the registration node in the graph, under dofRigid node. """
        registration_node = self.createChild(self.nodes["dofRigid"], "RegistrationNode")

        param = self.param  # shortcut

        # merge topologies
        repartition = ""
        sources_component_name = []

        collision_node = [elem.collision for elem in self.rigids.values()]

        for i, elem in enumerate(collision_node):
            sources_component_name.append(Tools.node_path_rel(registration_node, elem.node) + "/topology")

            repartition += "{0} ".format(i) * len(elem.topology.position)

        registration_node.createObject('MergeMeshes', name='source_topology', nbMeshes=len(sources_component_name),
                                       **dict({'position' + str(i + 1): '@' + item + '.position' for i, item in
                                               enumerate(sources_component_name)},
                                              **{'triangles' + str(i + 1): '@' + item + '.triangles' for i, item in
                                                 enumerate(sources_component_name)}))

        registration_node.createObject('MeshTopology', name='topo', src='@./source_topology')
        registration_node.createObject('MechanicalObject', name='DOFs')
        registration_node.createObject('Triangle')

        registration_node.createObject('RigidMapping', template='Rigid3d,Vec3d',
                                       input="@" + Tools.node_path_rel(registration_node, self.nodes["dofRigid"]),
                                       output="@DOFs", rigidIndexPerPoint=repartition)

        registration_node.createObject('NormalsFromPoints', name='NormalsFromPoints',
                                       template='Vec3d', position='@DOFs.position',
                                       triangles='@topo.triangles',
                                       invertNormals=self.param.invertSourceNormals)

        # Force Field
        target_path = Tools.node_path_rel(registration_node, self.nodes["Target"])
        registration_node.createObject('ClosestPointRegistrationForceField', name='ICP',
                                       template='Vec3d',
                                       # source
                                       sourceTriangles='@topo.triangles',
                                       sourceNormals='@NormalsFromPoints.normals',
                                       # target
                                       position='@{0}/loader.position'.format(target_path),
                                       triangles='@{0}/loader.triangles'.format(target_path),
                                       normals='@{0}/NormalsFromPoints.normals'.format(target_path),
                                       # Param
                                       cacheSize='4', blendingFactor=param.blendingFactor,
                                       stiffness=param.stiffness,
                                       damping=param.damping,
                                       outlierThreshold=param.outlierThreshold,
                                       normalThreshold=param.normalThreshold,
                                       rejectOutsideBbox=param.rejectOutsideBbox,
                                       drawColorMap='0')

    def setFromMesh(self, filepath, density=1000, offset=[0,0,0,0,0,0,1], scale3d=[1,1,1], voxelSize=0.01, numberOfPoints=1, generatedDir=None):
        # variables
        self.bodyOffset = Frame.Frame(offset)
        path_affine_rigid = '@' + Tools.node_path_rel(self.affineNode, self.rigidNode)
        path_affine_scale = '@' + Tools.node_path_rel(self.affineNode, self.scaleNode)
        massInfo = SofaPython.mass.RigidMassInfo()
        massInfo.setFromMesh(filepath, density, scale3d)

        self.image = SofaImage.API.Image(self.node, name="image_" + self.name, imageType="ImageUC")
        self.image.node.addChild(self.affineNode)  # for initialization
        self.image.node.addChild(self.rigidNode)  # for initialization
        self.shapeFunction = Flexible.API.ShapeFunction(self.rigidNode)

        if generatedDir is None:
            self.image.addMeshLoader(filepath, value=1, insideValue=1, offset=offset, scale=scale3d) # TODO support multiple meshes closingValue=1,
            self.image.addMeshToImage(voxelSize)
            # rigid dofs
            self.sampler = SofaImage.API.Sampler(self.rigidNode)
            self.sampler.addImageSampler(self.image, numberOfPoints)
            self.rigidDofs = self.sampler.addMechanicalObject('Rigid3'+template_suffix)
        else:
            self.image.addContainer(filename=self.node.name+"_rasterization.raw", directory=generatedDir)
            self.rigidDofs = serialization.importRigidDofs(self.rigidNode, generatedDir+"/"+self.node.name+'_dofs.json')

        # scale dofs
        self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position=concat([1,1,1]*numberOfPoints))
        positiveNode = self.scaleNode.createChild('positive')
        positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
        positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3d,Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
        positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
        positiveNode.createObject('UnilateralConstraint')
        positiveNode.createObject('Stabilization', name='Stabilization')

        # affine dofs
        self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='dofs')
        self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3d,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')

        # shapefunction and mass
        if generatedDir is None:
            self.shapeFunction.addVoronoi(self.image, position='@dofs.rest_position')
            # mass
            densityImage = self.image.createTransferFunction(self.affineNode, "density", param='0 0 1 '+str(density))
            affineMass = Flexible.API.AffineMass(self.affineNode)
            affineMass.massFromDensityImage(self.affineNode, densityImage=densityImage)
            self.mass = affineMass.mass
        else:
            self.shapeFunction.shapeFunction = serialization.importImageShapeFunction(self.affineNode, generatedDir+self.node.name+"_SF_indices.raw", generatedDir+self.node.name+"_SF_weights.raw", 'dofs')
            self.mass = serialization.importAffineMass(self.affineNode, generatedDir+self.node.name+"_affinemass.json")

        # hack to get the frame position
        self.node.init()
        for p in self.rigidDofs.position:
            p.extend([0,0,0,1])
            self.frame.append(Frame.Frame(p))

def createScene(root):
  
    ##### global parameters
    root.createObject('VisualStyle', displayFlags="showBehavior showWireframe showCollisionModels" )
    root.dt = 0.01
    root.gravity = [0, -10, 0]
    
    root.createObject('RequiredPlugin', pluginName = 'Compliant')
    root.createObject('CompliantAttachButtonSetting')
    
    ##### SOLVER
    root.createObject('CompliantImplicitSolver', stabilization=0, neglecting_compliance_forces_in_geometric_stiffness=0)
    root.createObject('SequentialSolver', iterations=100, precision=0)
    #root.createObject('LUResponse')
    root.createObject('LDLTResponse')
    
    
    bodies = []
    points = []
    
    N = 10
   
    for i in xrange(N):
        body = StructuralAPI.RigidBody( root, "body_"+str(i) )
        body.setManually( [i,0,0,0,0,0,1], 1, [1,1,1] )
        body.dofs.showObject = True
        body.dofs.showObjectScale = .5
        bodies.append( body )
        
    bodies[0].node.createObject('FixedConstraint')
    bodies[N-1].mass.mass = 10
    bodies[N-1].mass.inertia = "10 10 10"
    
        
    for i in xrange(N-1):
        p0 = bodies[i].addMappedPoint( "right", [0.5, 0, 0] )
        p0.dofs.showObject = True
        p0.dofs.showObjectScale = .1
        p0.dofs.drawMode=1
        p1 = bodies[i+1].addMappedPoint( "left", [-0.5, 0, 0] )
        p1.dofs.showObject = True
        p1.dofs.showObjectScale = .1
        p1.dofs.drawMode=2
        d = p0.node.createChild( "d"+str(i) )
        d.createObject('MechanicalObject', template = 'Vec3'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
        input = [] # @internal
        input.append( '@' + Tools.node_path_rel(root,p0.node) + '/dofs' )
        input.append( '@' + Tools.node_path_rel(root,p1.node) + '/dofs' )
        d.createObject('DifferenceMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 0" )
        p1.node.addChild( d )
        d.createObject('UniformCompliance', name = 'compliance', compliance="0" )

def createScene(node):
    scene = Tools.scene( node )

    ode = node.getObject('ode')

    ode.stabilization = False
    ode.warm_start = False

    # ode.debug = True
    # ode.propagate_lambdas = True

    node.gravity = '0 0 0'
    node.dt = 1e-2
    
    num = node.createObject('MinresSolver',
                            name = 'num',
                            iterations = 100,
                            precision = 0)

    # resp = node.createObject('DiagonalResponse')

    script = node.createObject('PythonScriptController',
                               filename = __file__,
                               classname = 'Controller')

    style = node.getObject('style')

    style.displayFlags = 'showCollisionModels'

    # parameters
    mass = 1.0
    stiff = 1e3 
    damping = 0.0

    # dofs
    p = insert_point(scene, 'p', [-1, 0, 0], mass) 
    

    sub = p.createChild('sub')
    sub.createObject('MechanicalObject',
                     name = 'dofs',
                     position = '0 0 0')
    
    sub.createObject('IdentityMapping',
                     template = 'Vec3d')

    compliance = 1/stiff

    sub.createObject('UniformCompliance',
                     template = 'Vec3d',
                     compliance = compliance,
                     damping = damping)
    
    shared.dofs = p.getObject('dofs')

    shared.mass = mass
    shared.stiff = stiff
    shared.damping = damping
    
    return node

def createScene(node):
    scene = Tools.scene( node )

    ode = node.getObject('ode')

    ode.stabilization = "no stabilization"
    # ode.warm_start = False
    # ode.debug = True


    node.gravity = '0 0 0'
    node.dt = 1e-2
    
    num = node.createObject('MinresSolver',
                            name = 'num',
                            iterations = 10,
                            precision = 0)

    style = node.getObject('style')
    style.displayFlags = 'showCollisionModels showBehaviorModels'

    script = node.createObject('PythonScriptController',
                               filename = __file__,
                               classname = 'Controller')

    mass = 1
    
    p1 = insert_point(scene, 'p1', [-1, 1, 0], mass)
    p2 = insert_point(scene, 'p2', [3, 1, 0], mass)

    out = scene.createChild('out')
    out.createObject('MechanicalObject', 
                     name = 'dofs',
                     template = 'Vec1d', 
                     position = '0 0 0')

    alpha = 0.2
    shared.alpha = alpha

    matrix = vec([1 - alpha, 0, 0, alpha, 0, 0,
                  0, 1 - alpha, 0, 0, alpha, 0,
                  0, 0, 1 - alpha, 0, 0, alpha] )
    
    value = vec([0, 0,  0])
    
    out.createObject('AffineMultiMapping',
                     template = 'Vec3d,Vec1d',
                     input = '@../p1/dofs @../p2/dofs',
                     output = '@dofs',
                     matrix = str(matrix),
                     value = str(value))
    
    out.createObject('UniformCompliance',
                     template = 'Vec1d',
                     compliance = 1e-5)

    shared.p1 = p1.getObject('dofs')
    shared.p2 = p2.getObject('dofs')

def createScene(node):
    scene = Tools.scene( node )

    ode = node.getObject('ode')

    ode.stabilization = True
    ode.warm_start = False
    ode.propagate_lambdas = True
    
    # ode.debug = True

    node.gravity = '0 -1 0'
    
    num = node.createObject('SequentialSolver',
                            name = 'num',
                            iterations = 200,
                            precision = 0)

    resp = node.createObject('DiagonalResponse')

    manager = node.getObject('manager')
    manager.response = 'CompliantContact'

    script = node.createObject('PythonScriptController',
                               filename = __file__,
                               classname = 'Controller')

    style = node.getObject('style')

    style.displayFlags = 'showBehaviorModels showCollisionModels'
    
    proximity = node.getObject('proximity')
    proximity.contactDistance = 0.01
    
    # dofs
    p1 = insert_point(scene, 'p1', [-1, 1, 0]) 
    p2 = insert_point(scene, 'p2', [1, 1, 0]) 

    rigid = Rigid.Body('rigid')
    rigid.collision = path + '/examples/mesh/ground.obj'

    rigid.node = rigid.insert( scene )

    ground = Rigid.Body('ground')
    ground.node = ground.insert( scene )
    ground.node.createObject('FixedConstraint', indices = '0')
    
    # blocked joint between ground/rigid
    joint = Rigid.Joint('joint')
    joint.absolute(Rigid.Frame(), ground.node, rigid.node)
    
    joint.node = joint.insert( scene )
    
    shared.joint = joint
    shared.body = rigid
    
    return node

        def __init__(self, rigidNode, scaleNode, name, offset, arg=-1):
            # node creation
            self.node = rigidNode.createChild(name)
            scaleNode.addChild(self.node)
            # variables
            self.frame = Frame.Frame(offset)
            path_offset_rigid = '@' + Tools.node_path_rel(self.node, rigidNode)
            path_offset_scale = '@' + Tools.node_path_rel(self.node, scaleNode)
            # scene creation
            self.dofs = self.frame.insert(self.node, template='Rigid3'+template_suffix, name='dofs')

            if arg==-1:
                self.mapping = self.node.createObject('RigidScaleToRigidMultiMapping', template='Rigid3'+template_suffix+',Vec3'+template_suffix+',Rigid3'+template_suffix
                                                                                     , input1=path_offset_rigid, input2=path_offset_scale, output='@.'
                                                                                     , useGeometricStiffness=geometric_stiffness, printLog='0')
            else:
                self.mapping = self.node.createObject('RigidScaleToRigidMultiMapping', template='Rigid3'+template_suffix+',Vec3'+template_suffix+',Rigid3'+template_suffix
                                                                                     , input1=path_offset_rigid, input2=path_offset_scale, output='@.'
                                                                                     , index='0 '+ str(arg) + ' ' + str(arg), useGeometricStiffness=geometric_stiffness, printLog='0')

def createScene(node):
    # controller
    node.createObject('PythonScriptController', filename = __file__, classname = 'Controller' )


    # friction coefficient
    shared.mu = float( random.randint(0,10) ) / 10.0 # a random mu in [0,1] with 0.1 step

    scene = Tools.scene( node )
    node.dt = 0.005

    style = node.getObject('style')
    style.findData('displayFlags').showMappings = True

    manager = node.getObject('manager')
    manager.response = 'FrictionCompliantContact'
    manager.responseParams = 'mu=' + str(shared.mu) +"&horizontalConeProjection=1"

    ode = node.getObject('ode')
    ode.stabilization = "pre-stabilization"
    
    num = node.createObject('SequentialSolver',
                            name = 'num',
                            iterations = 1000,
                            precision = 1e-20)
    node.createObject('LDLTResponse')
    
    proximity = node.getObject('proximity')

    proximity.alarmDistance = 0.5
    proximity.contactDistance = 0.1


    # plane
    plane = StructuralAPI.RigidBody( node, 'plane' )
    plane.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] )
    plane.node.createObject('FixedConstraint')
    cm = plane.addCollisionMesh( "mesh/cube.obj", [10,1,10] )
    cm.addVisualModel()
    
    # box
    box = StructuralAPI.RigidBody( node, 'box' )
    box.setFromMesh( 'mesh/cube.obj', 50, [0,2.5,0,0,0,0,1] )
    #box.setManually( [0,2.5,0,0,0,0,1], 1, [1,1,1] )
    box.dofs.showObject=True
    box.dofs.showObjectScale=5
    cm = box.addCollisionMesh( "mesh/cube.obj" )
    cm.addVisualModel()

    # keep an eye on dofs
    shared.plane = plane.dofs
    shared.box = box.dofs

def createScene(node):
    scene = Tools.scene( node )

    style = node.getObject('style')
    style.findData('displayFlags').showMappings = True

    manager = node.getObject('manager')
    manager.response = 'FrictionCompliantContact'
    
    node.createObject('CompliantAttachButton')
    
    
    globalMu = 0 # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)
    manager.responseParams = 'mu='+str(globalMu)+"&horizontalConeProjection=1"  # perfom an horizontal Coulomb cone projection
                            

    ode = node.getObject('ode')
    ode.stabilization = "pre-stabilization"
    ode.debug = False

    num = node.createObject('SequentialSolver',
                            name = 'num',
                            iterations = 100,
                            precision = 1e-14)
    node.createObject( "LDLTResponse" )
    
    proximity = node.getObject('proximity')

    proximity.alarmDistance = 0.5
    proximity.contactDistance = 0.2
    proximity.useLineLine = True

  
    # plane
    mesh = dir + '/../mesh/ground.obj'
    plane = StructuralAPI.RigidBody( node, "plane" )
    plane.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] )
    #body3.setFromMesh( mesh, 1 )
    cm = plane.addCollisionMesh( mesh )
    cm.addVisualModel()
    plane.node.createObject('FixedConstraint', indices = '0')
    cm.triangles.contactFriction = 0.5 # per object friction coefficient
    
    
    # box
    mesh = dir + '/../mesh/cube.obj'
    box = StructuralAPI.RigidBody( node, "box" )
    box.setFromMesh( mesh, 50, [0, 3, 0, 0,0,0,1] )
    cm = box.addCollisionMesh( mesh )
    cm.addVisualModel()
    cm.triangles.contactFriction = 1 # per object friction coefficient

    def setMeshLess(self, offset=[[0,0,0,0,0,0,1]], mass=1, rayleigh=0.1, generatedDir=None):
        if len(offset) == 0:
            Sofa.msg_error("RigidScale.API","ShearlessAffineBody should have at least 1 ShearLessAffine")
            return
        self.framecom = Frame.Frame()
        self.bodyOffset = Frame.Frame([0,0,0,0,0,0,1])
        path_affine_rigid = '@' + Tools.node_path_rel(self.affineNode, self.rigidNode)
        path_affine_scale = '@' + Tools.node_path_rel(self.affineNode, self.scaleNode)
        if len(offset) == 1: self.frame = [Frame.Frame(offset[0])]
        str_position = ""
        for p in offset:
            str_position = str_position + concat(p) + " "

        ### scene creation
        # rigid dof
        self.rigidDofs = self.rigidNode.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs', position=str_position, rest_position=str_position)
        self.rigidNode.createObject('UniformMass', totalMass=mass, rayleighStiffness=rayleigh);

        # scale dofs

        self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position= concat([1,1,1]*len(offset)))
        self.scaleNode.createObject('UniformMass', totalMass=mass, rayleighStiffness=rayleigh);
        #positiveNode = self.scaleNode.createChild('positive')
        #positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
        #target_scale = [0.5,0.5,0.5]
        #positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3d,Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
        #positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
        #positiveNode.createObject('UnilateralConstraint')
        #positiveNode.createObject('Stabilization', name='Stabilization')

        # affine dofs
        self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='parent')
        self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')
        
        self.frame = []
        for o in offset:
            self.frame.append(Frame.Frame(o))

def createScene(node):
    scene = Tools.scene( node )

    style = node.getObject('style')
    style.findData('displayFlags').showMappings = True

    manager = node.getObject('manager')
    manager.response = 'FrictionCompliantContact'
    manager.responseParams = 'mu=0' # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)

    ode = node.getObject('ode')
    ode.stabilization = True
    ode.debug = False

    num = node.createObject('SequentialSolver',
                            name = 'num',
                            iterations = 100,
                            precision = 1e-14)
    
    proximity = node.getObject('proximity')

    proximity.alarmDistance = 0.5
    proximity.contactDistance = 0.2

  
    # plane
    plane = Rigid.Body('plane')
    plane.visual = dir + '/../mesh/ground.obj'
    plane.collision = plane.visual
    plane.mass_from_mesh( plane.visual, 10 )
    plane.mu = 0.8 # per object friction coefficient
    plane.node = plane.insert( scene )
    plane.node.createObject('FixedConstraint', indices = '0')
    
    # box
    box = Rigid.Body('box')
    box.visual = dir + '/../mesh/cube.obj'
    box.collision = box.visual
    box.dofs.translation = [0, 3, 0]
    box.mass_from_mesh( box.visual, 50 )
    box.mu = 1 # per object friction coefficient
    box.node = box.insert( scene )

    def __init__(self, parent, name, dofs, dim):

        self.node = parent.createChild(name)

        self.compliance = 0
        self.damping = 0
        
        input = []
        dofs_dim = 0
        
        for n in dofs:
            input.append( Tools.node_path_rel(self.node, n) )
            dofs_dim += len( n.velocity ) * len(n.velocity[0] )

        self.matrix = np.zeros( (dim, dofs_dim) )
        self.value = np.zeros( dim )


        
        self.dofs = self.node.createObject('MechanicalObject',
                                           name = 'dofs',
                                           template = 'Vec1d',
                                           position = concat( [0] * dofs_dim ) )

        template = dofs[0].template
        self.map = self.node.createObject('AffineMultiMapping',
                                          name = 'map',
                                          template = '{0}, Vec1d'.format( template ),
                                          input = concat( input ),
                                          output = '@dofs',
                                          matrix = concat( self.matrix.reshape( self.matrix.size ).tolist() ),
                                          value = concat( -self.value ) )

        self.ff = self.node.createObject('UniformCompliance',
                                         name = 'ff',
                                         template = 'Vec1d',
                                         compliance = self.compliance,
                                         damping = self.damping )

    def createGraph(self, root):
        # Variable
        self.E_t = 0
        self.E_t_dt = 0
        self.root_node = root
        
        # Sofa parameters
        self.root_node.createObject('BackgroundSetting',color='1 1 1')
        self.root_node.createObject('VisualStyle', displayFlags='showVisual hideWireframe showBehaviorModels hideForceFields hideInteractionForceFields')
        self.root_node.createObject('StaticSolver')
        self.root_node.createObject('CGLinearSolver', iterations=500, tolerance=1E-10, threshold=1E-10)
        self.root_node.findData('gravity').value = '0 0 0'

        # Object to transfer creation
        node = self.root_node.createChild('left_humerus')
        node.createObject('MeshObjLoader',name='source', filename=source, triangulate=1, translation='0 0 0', rotation='0 0 0', scale3d='1 1 1')
        node.createObject('MeshToImageEngine', template='ImageUC', name='rasterizer', src='@source', insideValue='1', voxelSize=voxel_size, padSize=2, rotateImage='false')
        node.createObject('ImageContainer', template='ImageUC', name='image', src='@rasterizer', drawBB='false')
        node.createObject('ImageSampler', template='ImageUC', name='sampler', src='@image', method=1, param='1 0', clearData=0)
        node.createObject('MeshTopology', name='frame_topo', position='@sampler.position') 
                
        #================================ Target model ===================================
        targetNode = node.createChild('target') 
        targetNode.createObject('MeshObjLoader', name='target', filename=target, triangulate=1, translation='0 0 0', rotation='0 0 0', scale3d='3 3 3', showObject=0)        
        targetNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', src='@target', showObject=0)
        targetNode.createObject('FixedConstraint', fixAll='1' )          
        targetVisuNode = targetNode.createChild('visu')
        targetVisuNode.createObject('OglModel', template='ExtVec3f', name='visual', src='@../target', color='0.5 0.5 0.5 0.75') 
        
        #================================ Rigid frame ====================================
        rigidNode = node.createChild('rigid')
        rigidNode.createObject('MechanicalObject', template='Rigid3d', name='DOFs', src='@../frame_topo', showObject=0, showObjectScale='0.1')
                
        #=================================== Scale =======================================
        scaleNode = node.createChild('scale')
        scaleNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', position='1 1 1', showObject=0, showObjectScale='0.1')

        #============================= Registration model ================================
        objMainNode = rigidNode.createChild('main')
        scaleNode.addChild(objMainNode)

        # scene creation
        loader = objMainNode.createObject('MeshObjLoader',name='source', filename=source, triangulate=1, translation='0 0 0', rotation='0 0 0', scale3d='1 1 1')
        objMainNode.createObject('MeshToImageEngine', template='ImageUC', name='rasterizer', src='@source', value=1, insideValue=1, voxelSize=voxel_size, padSize=0, rotateImage='false')
        objMainNode.createObject('ImageContainer', template='ImageUC', name='image', src='@rasterizer', drawBB='false') 
        objMainNode.createObject('MechanicalObject', template='Affine', name='parent', src='@../../frame_topo', showObject=1, showObjectScale='0.1')
        objMainNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3d,Affine', input1='@../../rigid/DOFs', input2='@../../scale/DOFs', output='@.', index='0 0 0', printLog='0')
        objMainNode.createObject('VoronoiShapeFunction', template='ShapeFunctiond,ImageUC', name='SF', position='@parent.rest_position', image='@image.image', transform='@image.transform', nbRef=4, clearData=1, bias=0)  
        # Contact        
        objContactNode = objMainNode.createChild('registration')     
        objContactNode.createObject('MeshTopology', name='topo', src='@../source')
        objContactNode.createObject('MechanicalObject', name='DOFs')
        objContactNode.createObject('UniformMass', totalMass=1)
        objContactNode.createObject('TriangleModel')
        objContactNode.createObject('LinearMapping', template='Affine,Vec3d')
        # Visual model
        objVisuNode = objContactNode.createChild('visual')
        objVisuNode.createObject('OglModel', template='ExtVec3f', name='visual', src='@../topo', color='1 0.2 0.2 0.9')
        objVisuNode.createObject('IdentityMapping', template='Vec3d,ExtVec3f') 
        # Registration
        objRegistrationNode = objMainNode.createChild('force')
        objRegistrationNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', src='@../source')
        objRegistrationNode.createObject('LinearMapping', template='Affine,Vec3d')
        # registration force field
        springs = ""
        for i in range(len(loader.position)):
            springs += str(i)+' '+str(i)+' '
        distanceNode = objRegistrationNode.createChild('registration_constraint')
        targetNode.addChild(distanceNode)
        distanceNode.createObject('MechanicalObject', template='Vec3d', name='distanceDOFs')
        distanceNode.createObject('DifferenceMultiMapping', template='Vec3d,Vec3d', input='@'+Tools.node_path_rel(distanceNode, targetNode)+' @'+Tools.node_path_rel(distanceNode, objRegistrationNode), output='@.', pairs=springs, showObjectScale="0.005")
        distanceNode.createObject('UniformCompliance', name='constraint', isCompliance=0, compliance=1E-6, damping=0.1)

def createScene(node):

    # controller
    node.createObject('PythonScriptController', 
                      filename = __file__,
                      classname = 'Controller' )

    # time step
    node.dt = 0.005

    # scene node
    scene = Tools.scene( node )

    # display flags
    style = node.getObject('style')
    style.findData('displayFlags').showMappings = True

    # collision detection
    proximity = node.getObject('proximity')
    proximity.alarmDistance = 0.5
    proximity.contactDistance = 0.1

    # contat manager
    manager = node.getObject('manager')
    manager.response = 'CompliantContact'
    manager.responseParams = 'compliance=0'
    
    # integrator
    ode = node.getObject('ode',)
    ode.stabilization = True
    ode.stabilization_damping = 0

    # main solver
    num = node.createObject('BenchmarkSolver', name = 'num')

    response = node.createObject('LDLTResponse', name = 'response')
    
    iterations = 300
    precision = 1e-8

    
    # we need compliantdev for qpsolver
    node.createObject('RequiredPlugin', 
                      pluginName = 'CompliantDev')

    # benchmarks
    shared.pgs = node.createObject('Benchmark', name = 'bench-pgs')
    shared.qp = node.createObject('Benchmark', name = 'bench-qp')

    # solvers
    pgs = node.createObject('SequentialSolver',
                            name = 'pgs',
                            iterations = iterations,
                            precision = precision,
                            bench = '@./bench-pgs')

    qp = node.createObject('QPSolver',
                           name = 'qp',
                           iterations = iterations,
                           precision = precision,
                           bench = '@./bench-qp',
                           schur = True)


    # plane
    plane = Rigid.Body('plane')
    plane.visual = dir + '/mesh/ground.obj'
    plane.collision = plane.visual
    plane.mass_from_mesh( plane.visual, 10 )
    plane.node = plane.insert( scene )
    
    plane.node.createObject('FixedConstraint', 
                             indices = '0')

    # boxes
    n_boxes = 10

    for i in xrange(n_boxes):
        box = Rigid.Body('box-{0}'.format(i))
        box.visual = dir + '/mesh/cube.obj'
        box.collision = box.visual
        box.dofs.translation = [0, 2.5 * (i + 1), 0]
        box.mass_from_mesh( box.visual, 50 )
        box.node = box.insert( scene )