Example #1
0
        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')
Example #2
0
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")
Example #3
0
File: API.py Project: Ngautier/sofa
    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))
Example #4
0
File: sml.py Project: Sreevis/sofa
    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')
Example #5
0
File: API.py Project: Ngautier/sofa
    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))
Example #6
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')

        # 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))
Example #7
0
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 = "no stabilization"
    ode.warm_start = False

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

    node.gravity = '0 0 0'
    node.dt = 1e-2

    node.createObject('RequiredPlugin', pluginName="SofaLoader")

    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,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 = 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 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
Example #13
0
def createScene(node):
    node.createObject('RequiredPlugin', pluginName="SofaLoader")

    # 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=50,
                            precision=0)
    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)

    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')
Example #15
0
        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):
    scene = Tools.scene(node)

    ode = node.getObject('ode')

    ode.stabilization = "pre-stabilization"
    ode.warm_start = False
    ode.constraint_forces = "propagate"

    # ode.debug = True

    node.gravity = '0 -1 0'

    node.createObject('RequiredPlugin', pluginName="SofaLoader")

    num = node.createObject('SequentialSolver',
                            name='num',
                            iterations=200,
                            precision=0,
                            iterateOnBilaterals=True)

    resp = node.createObject('DiagonalResponse')

    # NB: either iterateOnBilaterals or use a non-diagonal Response

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

    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 = StructuralAPI.RigidBody(scene, 'rigid')
    rigid.setManually([0, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1])
    rigid.addCollisionMesh(path + '/examples/mesh/ground.obj')

    ground = StructuralAPI.RigidBody(scene, 'ground')
    ground.setManually([0, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1])
    ground.node.createObject('FixedConstraint')

    # blocked joint between ground/rigid
    joint = StructuralAPI.FixedRigidJoint("joint", ground.node, rigid.node)

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

    return node
Example #17
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
Example #18
0
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
Example #19
0
    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))
Example #20
0
    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 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)
Example #23
0
    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 )
import Sofa

import math

from Compliant import Rigid, Tools

dir = Tools.path(__file__)


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
Example #25
0
    def insert(self, parent, **kwargs):
        
        node = parent.createChild(self.name)

        self.offset_node = []

        geometric = kwargs.get('geometric', geometric_stiffness)
        
        # build input data for multimapping
        input = []
        for b, o in zip(self.body, self.offset):
            if o is None:
                input.append( '@' + Tools.node_path_rel(node, b) + '/dofs' )
            else:
                joint = b.createChild( self.name + '-offset' )
                self.offset_node.append( joint )
                
                joint.createObject('MechanicalObject', 
                                   template = 'Rigid', 
                                   name = 'dofs' )
                
                joint.createObject('AssembledRigidRigidMapping', 
                                   template = "Rigid,Rigid",
                                   source = '0 ' + concat( o ),
                                   geometricStiffness = geometric)
                
                input.append( '@' + Tools.node_path_rel(node, b) + '/' + joint.name + '/dofs' )
                             
        assert len(input) > 0
   
        dofs = node.createObject('MechanicalObject', 
                                 template = 'Vec6d', 
                                 name = 'dofs', 
                                 position = '0 0 0 0 0 0',
                                 velocity = '0 0 0 0 0 0')

        mapping = node.createObject('RigidJointMultiMapping',
                                name = 'mapping', 
                                template = 'Rigid,Vec6d', 
                                input = concat(input),
                                output = '@dofs',
                                pairs = "0 0",
                                geometricStiffness = geometric
        )
                
        sub = node.createChild("constraints")

        dim = sum(np.array(self.dofs) != 0)
        
        sub.createObject('MechanicalObject', 
                         template = 'Vec1d', 
                         name = 'dofs',
                         position = concat( np.zeros(dim) ),
                         velocity = concat( np.zeros(dim) ),
        )
	
        mask = [ (1 - d) for d in self.dofs ]
        mapping = sub.createObject('MaskMapping', 
                               name = 'mapping',
                               template = 'Vec6d,Vec1d',
                               input = '@../',
                               output = '@dofs',
                               dofs = concat(mask) )
		
        ff = sub.createObject('UniformCompliance',
                              name = 'compliance',
                              template = 'Vec1d',
                              compliance = self.compliance)

        if self.stabilize:
            stab = sub.createObject('Stabilization')
        
        self.ff = ff
        self.node = node
        
        return node
Example #26
0
def createScene(node):
    # controller
    node.createObject('PythonScriptController',
                      filename=__file__,
                      classname='Controller')

    node.dt = 0.005

    # friction coefficient
    shared.mu = 0.5

    scene = Tools.scene(node)

    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"

    bench = node.createObject('Benchmark')

    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.1

    # ground
    ground = Rigid.Body('ground')
    ground.node = ground.insert(scene)

    # 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)

    ground.node.createObject('FixedConstraint', indices='0')

    # ground-plane joint
    frame = Rigid.Frame.Frame()
    frame.translation = [8, 0, 0]

    joint = Rigid.RevoluteJoint(2)
    joint.absolute(frame, ground.node, plane.node)
    joint.upper_limit = 0

    joint.node = joint.insert(scene)

    # 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.node = box.insert(scene)

    shared.plane = plane.node.getObject('dofs')
    shared.box = box.node.getObject('dofs')
    shared.joint = joint.node.getObject('dofs')

    # pid
    shared.pid = Control.PID(shared.joint)
    shared.pid.pos = -math.atan(shared.mu)  # target should trigger slide
    shared.pid.basis = [0, 0, 0, 0, 0, 1]

    # shared.pid.dofs.externalForce = '-1e7'

    scale = 2e5

    shared.pid.kp = -5 * scale
    shared.pid.kd = -100 * scale
    shared.pid.ki = -1e-1 * scale
Example #27
0
def createScene(node):
    scene = Tools.scene( node )

    ode = node.getObject('ode')

    ode.stabilization = "pre-stabilization"
    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,
                            iterateOnBilaterals = True)

    resp = node.createObject('DiagonalResponse')

    # NB: either iterateOnBilaterals or use a non-diagonal Response


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


    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 = StructuralAPI.RigidBody( scene, 'rigid' )
    rigid.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] )
    rigid.addCollisionMesh( path + '/examples/mesh/ground.obj' )
    
    
    ground = StructuralAPI.RigidBody( scene, 'ground' )
    ground.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] )
    ground.node.createObject('FixedConstraint')
    
    
    
    # blocked joint between ground/rigid
    joint = StructuralAPI.FixedRigidJoint( "joint", ground.node, rigid.node )
    
   
    
    script = node.createObject('PythonScriptController',
                               filename = __file__,
                               classname = 'Controller')
    
    return node
Example #28
0
import Sofa

from Compliant import Rigid, Tools

mesh_path = Tools.path( __file__ )

scale = 1

# parts of the mechanism
parts = [ 
    ["Corps","Corps.msh","1.36 0 0.0268 0 0 0 1","0 0 0 0 0 0 1","22.8 751 737", "2.1e+11","0.28","7.8e+3",1291.453/scale,"TetrahedronFEMForceField","Rigid","Vec3d","TLineModel","TPointModel","ExtVec3f","0.obj","Actor_Sensor_NA",],
    ["Roue","Roue.msh","0 -0.00604 0.354 0 0 0 1","0 0 -0.148 0 0 0 1","105 106 205", "2.1e+11","0.28","7.8e+3",780.336/scale,"TetrahedronFEMForceField","Rigid","Vec3d","TLineModel","TPointModel","ExtVec3f","3.obj","Actor_Sensor_NA"],
    ["Came","Came.msh","0 0 -0.00768 0 0 0 1","1.085 -0.072 0.33 0 0 0 1","40.5 40.6 0.331", "2.1e+11","0.28","7.8e+3",161.416/scale,"TetrahedronFEMForceField","Rigid","Vec3d","TLineModel","TPointModel","ExtVec3f","2.obj","Actor_Sensor_NA"],
    ["Piston","Piston.msh","0 0 0.424 0 0 0 1","2.05 0 0.33 0 0 0 1","0.356 14.6 14.7", "2.1e+11","0.28","7.8e+3",132.759/scale,"TetrahedronFEMForceField","Rigid","Vec3d","TLineModel","TPointModel","ExtVec3f","1.obj","Actor_Sensor_NA"]
]


# joint offsets
offset = [
    [0, Rigid.Frame().read('0 0 0 0 0 0 1')],
    [1, Rigid.Frame().read('0 0 0.148 0 0 0 1')],

    [1, Rigid.Frame().read('0.24 -0.145 0.478 0 0 0 1')],
    [2, Rigid.Frame().read('-0.845 -0.073 0 0 0 0 1')],

    [2, Rigid.Frame().read('0.852 0.072 0 0 0 0 1')],
    [3, Rigid.Frame().read('-0.113 0 0 0 0 0 1')],

    [3, Rigid.Frame().read('0.15 0 0 0 0 0 1')],
    [0, Rigid.Frame().read('2.2 0 0.33 0 0 0 1')]    
]
    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)
Example #30
0
def createScene(node):
    scene = Tools.scene( node )

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

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

    manager.responseParams="stiffness=1e5"

    node.dt = 2.5e-3
    # node.createObject('CompliantAttachButton')
                            
    ode = node.getObject('ode')
    node.removeObject(ode)
    ode = node.createObject("EulerImplicitSolver", rayleighStiffness = 0, rayleighMass = 0)
    
    num = node.createObject('CGLinearSolver',
                            name = 'num',
                            iterations = 100,
                            tolerance = 1e-7, threshold = 0)
    num.printLog = 1
    # num = node.createObject('CgSolver', name = 'num', iterations = 100, precision = 1e-7 )
    
    proximity = node.getObject('proximity')

    proximity.alarmDistance = 0.1
    proximity.contactDistance = 0
    proximity.useLineLine = True

  
    # planes
    for i in xrange(4):
        mesh = dir + '/../mesh/ground.obj'
        plane = StructuralAPI.RigidBody( node, "plane-{}".format(i) )

        g = Rigid3()

        n = np.zeros(3)
        n[:] = [0, 1, 0]

        r = Quaternion.exp( math.pi / 8 * np.array([1.0, 0.0, 0.0]))
        
        q = Quaternion.exp(i * math.pi / 2 * np.array([0.0, 1.0, 0.0]))

        g.orient = q * r
        
        plane.setManually( g, 1, [1,1,1] )
        
        #body3.setFromMesh( mesh, 1 )
        cm = plane.addCollisionMesh( mesh, [10, 1, 10] )
        cm.triangles.group = "0"
        
        cm.addVisualModel()
        plane.node.createObject('FixedConstraint', indices = '0')
        cm.triangles.contactFriction = 0.5 # per object friction coefficient
        
    # box

    particles = node.createChild('particles')
    n = 400
    dofs = particles.createObject('MechanicalObject', template = "Vec3d")

    pos = np.zeros( (n, 3) )
    vel = np.zeros( (n, 3) )    

    for i in xrange(n):
        pos[i, 1] = 2 + i / 1.5
        vel[i, 1] = -1        

    dofs.position = pos.tolist()
    dofs.velocity = vel.tolist()
    
    mass = particles.createObject('UniformMass', template = 'Vec3d')
    model = particles.createObject('SphereModel', template = 'Vec3d',
                                   selfCollision = True,
                                   radius = 0.1)
Example #31
0
import Sofa

from Compliant import Rigid, Tools, Frame

mesh_path = Tools.path(__file__)

scale = 1

# parts of the mechanism
parts = [[
    "Corps",
    "Corps.msh",
    "1.36 0 0.0268 0 0 0 1",
    "0 0 0 0 0 0 1",
    "22.8 751 737",
    "2.1e+11",
    "0.28",
    "7.8e+3",
    1291.453 / scale,
    "TetrahedronFEMForceField",
    "Rigid",
    "Vec3d",
    "TLineModel",
    "TPointModel",
    "ExtVec3f",
    "0.obj",
    "Actor_Sensor_NA",
],
         [
             "Roue", "Roue.msh", "0 -0.00604 0.354 0 0 0 1",
             "0 0 -0.148 0 0 0 1", "105 106 205", "2.1e+11", "0.28", "7.8e+3",
Example #32
0
def run():

    ok = True

    info = SofaPython.mass.RigidMassInfo()

    # testing axis-aligned known geometric shapes
    for m in xrange(len(meshes)):
        mesh = meshes[m]
        mesh_path = path + meshes[m]

        for s in xrange(len(scales)):
            scale = scales[s]

            if mesh == "cylinder.obj" and scale[0] != scale[1]:
                continue

            for d in xrange(len(densities)):
                density = densities[d]

                info.setFromMesh(mesh_path, density, scale)

                error = " (" + meshes[m] + ", s=" + Tools.cat(
                    scale) + " d=" + str(density) + ")"

                ok &= EXPECT_TRUE(
                    almostEqualReal(info.mass, masses[m][s][d]), "mass" +
                    error + " " + str(info.mass) + "!=" + str(masses[m][s][d]))
                ok &= EXPECT_TRUE(
                    almostEqualLists(info.com, [x * 0.5 for x in scale]),
                    "com" + error + " " + Tools.cat(info.com) + "!=" +
                    Tools.cat([x * 0.5 for x in scale]))
                ok &= EXPECT_TRUE(
                    almostEqualLists(info.diagonal_inertia, inertia[m][s][d]),
                    "inertia" + error + " " + str(info.diagonal_inertia) +
                    "!=" + str(inertia[m][s][d]))

# testing diagonal inertia extraction from a rotated cuboid
    mesh = "cube.obj"
    mesh_path = path + mesh
    scale = scales[3]
    density = 1
    theory = sorted(inertia[0][3][0])
    for r in rotations:
        info.setFromMesh(mesh_path, density, scale, r)
        local = sorted(info.diagonal_inertia)
        ok &= EXPECT_TRUE(
            almostEqualLists(local, theory), "inertia " + str(local) + "!=" +
            str(theory) + " (rotation=" + str(r) + ")")

# testing extracted inertia rotation
    mesh = "rotated_cuboid_12_35_-27.obj"
    mesh_path = path + mesh
    density = 1
    info.setFromMesh(mesh_path, density)

    # theoretical results
    scale = [2, 3, 1]
    mass = density * scale[0] * scale[1] * scale[2]
    inertiat = numpy.empty(3)
    inertiat[0] = 1.0 / 12.0 * mass * (
        scale[1] * scale[1] + scale[2] * scale[2])  # x
    inertiat[1] = 1.0 / 12.0 * mass * (
        scale[0] * scale[0] + scale[2] * scale[2])  # y
    inertiat[2] = 1.0 / 12.0 * mass * (
        scale[0] * scale[0] + scale[1] * scale[1])  # z

    # used quaternion in mesh

    q = Quaternion.normalized(
        Quaternion.from_euler([
            12 * math.pi / 180.0, 35 * math.pi / 180.0, -27 * math.pi / 180.0
        ]))

    # corresponding rotation matrices (ie frame defined by columns)
    mt = Quaternion.to_matrix(q)
    m = Quaternion.to_matrix(info.inertia_rotation)

    # matching inertia
    idxt = numpy.argsort(inertiat)
    idx = numpy.argsort(info.diagonal_inertia)

    # checking if each axis/column are parallel (same or opposite for unitary vectors)
    for i in xrange(3):
        ok &= EXPECT_TRUE(
            almostEqualLists(mt[:, idxt[i]].tolist(), m[:, idx[i]].tolist(),
                             1e-5)
            or almostEqualLists(mt[:, idxt[i]].tolist(),
                                (-m[:, idx[i]]).tolist(), 1e-5),
            "wrong inertia rotation")

#    print mt[:,idxt]
#    print m [:,idx ]

    return ok
    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)
Example #34
0
import Sofa
import os, numpy, sys
from Compliant import StructuralAPI
from Compliant import Tools as CompliantTools
from SofaPython import Quaternion, Tools

currentdir = CompliantTools.path(__file__)

N = 10

bodies = []


def createScene(root):

    root.createObject('PythonScriptController',
                      name="moveController",
                      filename=__file__,
                      classname="MoveController")

    root.createObject('RequiredPlugin', name='Compliant')
    root.createObject('RequiredPlugin', name='ContactMapping')

    root.createObject(
        'VisualStyle',
        name='VisualStyle',
        displayFlags="hideMechanicalMappings showCollisionModels")

    root.createObject('CompliantImplicitSolver')
    #root.createObject('CompliantPseudoStaticSolver',iterations=1,velocityFactor=0)
    #root.createObject('CgSolver',iterations="25", precision="1e-6")
Example #35
0
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.7 # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)
    
    manager.responseParams = 'mu={0}&compliance={1}&horizontalConeProjection=1'.format(globalMu,
                                                                                       1e-8)  
                            

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


    # (un)comment these to see anisotropy issues with sequential solver
    solver = 'ModulusSolver'
    solver = 'SequentialSolver'
    
    num = node.createObject(solver,
                            name = 'num',
                            iterations = 100,
                            precision = 1e-14,
                            anderson = 4)
    num.printLog = True
    
    proximity = node.getObject('proximity')

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

  
    # plane
    plane = Rigid.Body('plane')
    plane.dofs.translation = [0, 5, -15]

    alpha = math.pi / 5
    mu = math.tan( alpha )

    print "plane mu:", mu, 
    if mu < globalMu: print '(should stick)'
    else: print '(should slide)'

    q = Quaternion.exp( [alpha, 0.0, 0.0] )

    plane.dofs.rotation = q
    s = 6
    plane.scale = [s, s, s]
    
    plane.visual = path + '/../mesh/ground.obj'
    plane.collision = plane.visual
    plane.mass_from_mesh( plane.visual, 10 )
    plane.mu = 0.5 # per object friction coefficient
    plane.node = plane.insert( scene )
    plane.node.createObject('FixedConstraint', indices = '0')

    # box
    box = Rigid.Body('box')
    box.visual = path + '/../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.dofs.rotation = q


    box.node = box.insert( scene )
Example #36
0
    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')
Example #37
0
    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
                )  # 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))
Example #38
0
def createScene(node):
    scene = Tools.scene(node)

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

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

    manager.responseParams = "stiffness=1e5"

    node.dt = 2.5e-3
    # node.createObject('CompliantAttachButton')

    ode = node.getObject('ode')
    node.removeObject(ode)
    ode = node.createObject("EulerImplicitSolver",
                            rayleighStiffness=0,
                            rayleighMass=0)

    num = node.createObject('CGLinearSolver',
                            name='num',
                            iterations=100,
                            tolerance=1e-7,
                            threshold=0)
    num.printLog = 1
    # num = node.createObject('CgSolver', name = 'num', iterations = 100, precision = 1e-7 )

    proximity = node.getObject('proximity')

    proximity.alarmDistance = 0.1
    proximity.contactDistance = 0
    proximity.useLineLine = True

    # planes
    for i in xrange(4):
        mesh = dir + '/../mesh/ground.obj'
        plane = StructuralAPI.RigidBody(node, "plane-{}".format(i))

        g = Rigid3()

        n = np.zeros(3)
        n[:] = [0, 1, 0]

        r = Quaternion.exp(math.pi / 8 * np.array([1.0, 0.0, 0.0]))

        q = Quaternion.exp(i * math.pi / 2 * np.array([0.0, 1.0, 0.0]))

        g.orient = q * r

        plane.setManually(g, 1, [1, 1, 1])

        #body3.setFromMesh( mesh, 1 )
        cm = plane.addCollisionMesh(mesh, [10, 1, 10])
        cm.triangles.group = "0"

        cm.addVisualModel()
        plane.node.createObject('FixedConstraint', indices='0')
        cm.triangles.contactFriction = 0.5  # per object friction coefficient

    # box

    particles = node.createChild('particles')
    n = 400

    for i in xrange(n):
        p = particles.createChild('particle-{0}'.format(i))
        dofs = p.createObject('MechanicalObject', template="Vec3d")

        pos = np.zeros(3)
        vel = np.zeros(3)

        pos[1] = 2 + i / 1.5
        vel[1] = -1

        dofs.position = pos.tolist()
        dofs.velocity = vel.tolist()

        mass = p.createObject('UniformMass', template='Vec3d')
        model = p.createObject('SphereModel',
                               template='Vec3d',
                               selfCollision=True,
                               radius=0.1)
Example #39
0
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.7  # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)

    manager.responseParams = 'mu={0}&compliance={1}&horizontalConeProjection=1'.format(
        globalMu, 1e-8)

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

    # (un)comment these to see anisotropy issues with sequential solver
    solver = 'ModulusSolver'
    solver = 'SequentialSolver'

    num = node.createObject(solver,
                            name='num',
                            iterations=100,
                            precision=1e-14,
                            anderson=4)
    num.printLog = True

    proximity = node.getObject('proximity')

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

    # plane
    plane = Rigid.Body('plane')
    plane.dofs.translation = [0, 5, -15]

    alpha = math.pi / 5
    mu = math.tan(alpha)

    print "plane mu:", mu,
    if mu < globalMu: print '(should stick)'
    else: print '(should slide)'

    q = Quaternion.exp([alpha, 0.0, 0.0])

    plane.dofs.rotation = q
    s = 6
    plane.scale = [s, s, s]

    plane.visual = path + '/../mesh/ground.obj'
    plane.collision = plane.visual
    plane.mass_from_mesh(plane.visual, 10)
    plane.mu = 0.5  # per object friction coefficient
    plane.node = plane.insert(scene)
    plane.node.createObject('FixedConstraint', indices='0')

    # box
    box = Rigid.Body('box')
    box.visual = path + '/../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.dofs.rotation = q

    box.node = box.insert(scene)
Example #40
0
def createScene(node):
    # controller
    node.createObject('PythonScriptController', 
                      filename = __file__,
                      classname = 'Controller' )

    node.dt = 0.005

    # friction coefficient
    shared.mu = 0.5

    scene = Tools.scene( node )

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

    manager = node.getObject('manager')
    manager.response = 'FrictionCompliantContact'
    manager.responseParams = 'mu=' + str(shared.mu)

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

    proximity.alarmDistance = 0.5
    proximity.contactDistance = 0.1

    # ground
    ground = Rigid.Body('ground');
    ground.node = ground.insert( scene );

    # 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 )
    
    ground.node.createObject('FixedConstraint', 
                             indices = '0')

    # ground-plane joint
    frame = Rigid.Frame()
    frame.translation = [5, 0, 0]

    joint = Rigid.RevoluteJoint(2)
    joint.absolute(frame, ground.node, plane.node)

    joint.node = joint.insert( scene )

    # joint limit 
    limit = joint.node.createChild("limit")
    limit.createObject('MechanicalObject', template = 'Vec1d', position = '0')
    projection = limit.createObject('ProjectionMapping', template = 'Vec6d, Vec1d' )
    projection.set = '0   0 0 0 0 0 -1'
    limit.createObject('UniformCompliance', template = 'Vec1d', compliance = '0' )
    limit.createObject('UnilateralConstraint');
    limit.createObject('Stabilization');
    
    # 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.node = box.insert( scene )

    shared.plane = plane.node.getObject('dofs')
    shared.box = box.node.getObject('dofs')
    shared.joint = joint.node.getObject('dofs')

    # pid
    shared.pid = Control.PID(shared.joint)
    shared.pid.ref_pos = -math.atan( shared.mu ) # target should trigger slide

    shared.pid.basis = [0, 0, 0, 0, 0, 1]

    scale = 1e6

    shared.pid.kp = - 1 * scale
    shared.pid.kd =  - 5 * scale
    shared.pid.ki = - 0.05 * scale
Example #41
0
import Sofa

import math

from Compliant import StructuralAPI, Tools
from Compliant.types import Quaternion, Rigid3
import numpy as np

dir = Tools.path( __file__ )

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

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

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

    manager.responseParams="stiffness=1e5"

    node.dt = 2.5e-3
    # node.createObject('CompliantAttachButton')
                            
    ode = node.getObject('ode')
    node.removeObject(ode)
    ode = node.createObject("EulerImplicitSolver", rayleighStiffness = 0, rayleighMass = 0)
    
    num = node.createObject('CGLinearSolver',
Example #42
0
File: gear.py Project: bcarrez/sofa
def createScene(root):
  
    ##### global parameters
    root.createObject('VisualStyle', displayFlags="showBehavior showCollisionModels" )
    root.dt = 0.01
    root.gravity = [0, -10, 0]
    
    root.createObject('RequiredPlugin', pluginName = 'Compliant')
    root.createObject('CompliantAttachButton')
    
    
    root.createObject('DefaultPipeline', name='DefaultCollisionPipeline', depth="6")
    root.createObject('BruteForceBroadPhase', name='N2')
    root.createObject('BVHNarrowPhase')
    root.createObject('DiscreteIntersection')
    root.createObject('DefaultContactManager', name="Response", response="CompliantContact", responseParams="compliance=0&restitution=0" )
    
    
    
    
    
    ##### SOLVER
    root.createObject('CompliantImplicitSolver', stabilization=1, neglecting_compliance_forces_in_geometric_stiffness=1)
    root.createObject('SequentialSolver', iterations=100, precision=0)
    #root.createObject('LUResponse')
    root.createObject('LDLTResponse')
    
    
    
    
    
    ##### GEAR
    
    gearNode = root.createChild( "GEAR" )
    
    r0 = 0.33
    r1 = 0.66
    
    body0 = StructuralAPI.RigidBody( gearNode, "body_0" )
    body0.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] )
    body0.dofs.showObject = True
    body0.dofs.showObjectScale = r0*1.1
    body0.dofs.velocity="0 0 0 0 1 0"
    body0.node.createObject('Sphere', radius=r0)
        
    body1 = StructuralAPI.RigidBody( gearNode, "body_1" )
    body1.setManually( [1,0,0,0,0,0,1], 1, [1,1,1] )
    body1.dofs.showObject = True
    body1.dofs.showObjectScale = r1*1.1
    body1.node.createObject('Sphere', radius=r1)
    
   
    body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    
    
    d = body0.node.createChild( "d" )
    d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
    input = [] # @internal
    input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
    input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
    d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = -r0/r1 )
    body1.node.addChild( d )
    d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
    
    
    
    
    #####  driving belt / chain
    
    
    beltNode = root.createChild( "BELT" )
    
    r0 = 0.7
    r1 = 0.3
    
    body0 = StructuralAPI.RigidBody( beltNode, "body_0" )
    body0.setManually( [0,-2,0,0,0,0,1], 1, [1,1,1] )
    body0.dofs.showObject = True
    body0.dofs.showObjectScale = r0*1.1
    body0.dofs.velocity="0 0 0 0 1 0"
    body0.node.createObject('Sphere', radius=r0)
        
    body1 = StructuralAPI.RigidBody( beltNode, "body_1" )
    body1.setManually( [1.5,-2,0,0,0,0,1], 1, [1,1,1] )
    body1.dofs.showObject = True
    body1.dofs.showObjectScale = r1*1.1
    body1.node.createObject('Sphere', radius=r1)
    
   
    body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    
    
    d = body0.node.createChild( "d" )
    d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
    input = [] # @internal
    input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
    input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
    d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = r0/r1 )
    body1.node.addChild( d )
    d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
    
    
    
    
    
    
    #####  angle transmission
    
    
    angleNode = root.createChild( "ANGLE" )
    
    r0 = 0.49
    r1 = 0.49
    
    body0 = StructuralAPI.RigidBody( angleNode, "body_0" )
    body0.setManually( [0,-4,0,0,0,0,1], 1, [1,1,1] )
    body0.dofs.showObject = True
    body0.dofs.showObjectScale = r0*1.1
    body0.dofs.velocity="0 0 0 1 0 0"
    body0.node.createObject('Sphere', radius=r0)
        
    body1 = StructuralAPI.RigidBody( angleNode, "body_1" )
    body1.setManually( [1,-4,0,0,0,0,1], 1, [1,1,1] )
    body1.dofs.showObject = True
    body1.dofs.showObjectScale = r1*1.1
    body1.node.createObject('Sphere', radius=r1)
    
   
    body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 0 1 1")
    body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    
    
    d = body0.node.createChild( "d" )
    d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
    input = [] # @internal
    input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
    input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
    d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 3 0 4", ratio = r0/r1 )
    body1.node.addChild( d )
    d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
    
    
    
    
    #####  rack
    
    
    rackNode = root.createChild( "RACK" )
    
    
    body0 = StructuralAPI.RigidBody( rackNode, "body_0" )
    body0.setManually( [0,-6,0,0,0,0,1], 1, [1,1,1] )
    body0.dofs.showObject = True
    body0.dofs.showObjectScale = 0.55
    body0.dofs.velocity="0 0 0 0 0 1"
    body0.node.createObject('Sphere', radius=0.5)
        
    body1 = StructuralAPI.RigidBody( rackNode, "body_1" )
    body1.setManually( [-2,-6.71,0, 0,0,0.7071067811865476,0.7071067811865476], 1, [1,1,1] )
    body1.dofs.showObject = True
    body1.dofs.showObjectScale = 0.3
    body1.node.createObject('Capsule', radii="0.2", heights="5")
    
   
    body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 1 0")
    body1.node.createObject('PartialFixedConstraint', fixedDirections="0 1 1 1 1 1")
    
    
    d = body0.node.createChild( "d" )
    d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
    input = [] # @internal
    input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
    input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
    d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 5 0 0", ratio = 1 )
    body1.node.addChild( d )
    d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
    
    
Example #43
0
def createScene(root):
  
    ##### global parameters
    root.createObject('VisualStyle', displayFlags="showBehavior showCollisionModels" )
    root.dt = 0.01
    root.gravity = [0, -10, 0]
    
    root.createObject('RequiredPlugin', pluginName = 'Compliant')
    root.createObject('CompliantAttachButton')
    
    
    root.createObject('DefaultPipeline', name='DefaultCollisionPipeline', depth="6")
    root.createObject('BruteForceDetection')
    root.createObject('DiscreteIntersection')
    root.createObject('DefaultContactManager', name="Response", response="CompliantContact", responseParams="compliance=0&restitution=0" )
    
    
    
    
    
    ##### SOLVER
    root.createObject('CompliantImplicitSolver', stabilization=1, neglecting_compliance_forces_in_geometric_stiffness=1)
    root.createObject('SequentialSolver', iterations=100, precision=0)
    #root.createObject('LUResponse')
    root.createObject('LDLTResponse')
    
    
    
    
    
    ##### GEAR
    
    gearNode = root.createChild( "GEAR" )
    
    r0 = 0.33
    r1 = 0.66
    
    body0 = StructuralAPI.RigidBody( gearNode, "body_0" )
    body0.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] )
    body0.dofs.showObject = True
    body0.dofs.showObjectScale = r0*1.1
    body0.dofs.velocity="0 0 0 0 1 0"
    body0.node.createObject('Sphere', radius=r0)
        
    body1 = StructuralAPI.RigidBody( gearNode, "body_1" )
    body1.setManually( [1,0,0,0,0,0,1], 1, [1,1,1] )
    body1.dofs.showObject = True
    body1.dofs.showObjectScale = r1*1.1
    body1.node.createObject('Sphere', radius=r1)
    
   
    body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    
    
    d = body0.node.createChild( "d" )
    d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
    input = [] # @internal
    input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
    input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
    d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = -r0/r1 )
    body1.node.addChild( d )
    d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
    
    
    
    
    #####  driving belt / chain
    
    
    beltNode = root.createChild( "BELT" )
    
    r0 = 0.7
    r1 = 0.3
    
    body0 = StructuralAPI.RigidBody( beltNode, "body_0" )
    body0.setManually( [0,-2,0,0,0,0,1], 1, [1,1,1] )
    body0.dofs.showObject = True
    body0.dofs.showObjectScale = r0*1.1
    body0.dofs.velocity="0 0 0 0 1 0"
    body0.node.createObject('Sphere', radius=r0)
        
    body1 = StructuralAPI.RigidBody( beltNode, "body_1" )
    body1.setManually( [1.5,-2,0,0,0,0,1], 1, [1,1,1] )
    body1.dofs.showObject = True
    body1.dofs.showObjectScale = r1*1.1
    body1.node.createObject('Sphere', radius=r1)
    
   
    body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    
    
    d = body0.node.createChild( "d" )
    d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
    input = [] # @internal
    input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
    input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
    d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = r0/r1 )
    body1.node.addChild( d )
    d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
    
    
    
    
    
    
    #####  angle transmission
    
    
    angleNode = root.createChild( "ANGLE" )
    
    r0 = 0.49
    r1 = 0.49
    
    body0 = StructuralAPI.RigidBody( angleNode, "body_0" )
    body0.setManually( [0,-4,0,0,0,0,1], 1, [1,1,1] )
    body0.dofs.showObject = True
    body0.dofs.showObjectScale = r0*1.1
    body0.dofs.velocity="0 0 0 1 0 0"
    body0.node.createObject('Sphere', radius=r0)
        
    body1 = StructuralAPI.RigidBody( angleNode, "body_1" )
    body1.setManually( [1,-4,0,0,0,0,1], 1, [1,1,1] )
    body1.dofs.showObject = True
    body1.dofs.showObjectScale = r1*1.1
    body1.node.createObject('Sphere', radius=r1)
    
   
    body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 0 1 1")
    body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
    
    
    d = body0.node.createChild( "d" )
    d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
    input = [] # @internal
    input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
    input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
    d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 3 0 4", ratio = r0/r1 )
    body1.node.addChild( d )
    d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
    
    
    
    
    #####  rack
    
    
    rackNode = root.createChild( "RACK" )
    
    
    body0 = StructuralAPI.RigidBody( rackNode, "body_0" )
    body0.setManually( [0,-6,0,0,0,0,1], 1, [1,1,1] )
    body0.dofs.showObject = True
    body0.dofs.showObjectScale = 0.55
    body0.dofs.velocity="0 0 0 0 0 1"
    body0.node.createObject('Sphere', radius=0.5)
        
    body1 = StructuralAPI.RigidBody( rackNode, "body_1" )
    body1.setManually( [-2,-6.71,0, 0,0,0.7071067811865476,0.7071067811865476], 1, [1,1,1] )
    body1.dofs.showObject = True
    body1.dofs.showObjectScale = 0.3
    body1.node.createObject('Capsule', radii="0.2", heights="5")
    
   
    body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 1 0")
    body1.node.createObject('PartialFixedConstraint', fixedDirections="0 1 1 1 1 1")
    
    
    d = body0.node.createChild( "d" )
    d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
    input = [] # @internal
    input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
    input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
    d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 5 0 0", ratio = 1 )
    body1.node.addChild( d )
    d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
    
    
    def createGraph(self, root):

        # Variable
        self.E_t = 0
        self.E_t_dt = 0
        self.root_node = root
        self.root_node.findData('gravity').value = '0 0 0'

        # Sofa parameters
        self.root_node.createObject('BackgroundSetting', color='1 1 1')
        self.root_node.createObject(
            'VisualStyle',
            displayFlags=
            'showVisual hideWireframe showBehaviorModels hideForceFields showInteractionForceFields'
        )

        self.root_node.createObject('EulerImplicit',
                                    rayleighStiffness='0.01',
                                    rayleighMass='0.01')
        self.root_node.createObject('CGLinearSolver',
                                    iterations=200,
                                    tolerance=1E-6,
                                    threshold=1E-6)

        # Object to transfer creation
        node = self.root_node.createChild('main')
        node.createObject('MeshObjLoader',
                          name='source',
                          filename='./mesh/source.obj',
                          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=0.01,
                          padSize=2,
                          rotateImage='false')
        node.createObject('ImageContainer',
                          template='ImageUC',
                          name='image',
                          src='@rasterizer',
                          drawBB='false')
        node.createObject('MeshTopology',
                          name='frame_topo',
                          position=listListToStr(dofs_position))

        #================================ Target model ===================================
        targetNode = node.createChild('target')
        targetNode.createObject('MeshObjLoader',
                                name='loader',
                                filename='./mesh/target.obj',
                                rotation='0 0 0',
                                translation='0 0 0',
                                scale3d='1 1 1',
                                showObject=0)
        targetNode.createObject('MechanicalObject',
                                template='Vec3d',
                                name='DOFs',
                                src='@loader',
                                showObject=0)
        targetNode.createObject('NormalsFromPoints',
                                name='normalsFromPoints',
                                template='Vec3d',
                                position='@DOFs.position',
                                triangles='@loader.triangles',
                                invertNormals=0)
        targetNode.createObject('FixedConstraint', fixAll='1', drawSize=0.001)
        targetVisuNode = targetNode.createChild('visu')
        targetVisuNode.createObject('OglModel',
                                    template='ExtVec3f',
                                    name='visual',
                                    src='@../loader',
                                    color='0.5 0.5 0.5 0.85')

        #================================ Rigid frame ====================================
        rigidNode = node.createChild('rigid')
        rigidNode.createObject('MechanicalObject',
                               template='Rigid',
                               name='DOFs',
                               src='@../frame_topo',
                               showObject=1,
                               showObjectScale='0.1')
        rigidNode.createObject('RigidMass',
                               mass="1 1 ",
                               inertia="1 1 1  1 1 1")
        #rigidNode.createObject('FixedConstraint', name='fixed', indices='0')

        #=================================== Scale =======================================
        scaleNode = node.createChild('scale')
        scaleNode.createObject('MechanicalObject',
                               template='Vec3d',
                               name='DOFs',
                               position='1 1 1 1 1 1',
                               showObject=0,
                               showObjectScale='0.1')

        #=========================== Alignement constraint ===============================
        offsetNode = rigidNode.createChild('offset')
        offsetNode.createObject('MechanicalObject',
                                template='Rigid',
                                name='DOFs',
                                position='0 0 0 0 0 0 1  0 0 0 0 0 0 1',
                                showObject=1,
                                showObjectScale='0.1')
        offsetNode.createObject('AssembledRigidRigidMapping',
                                template='Rigid,Rigid',
                                source='0 ' + listToStr(offset_position[0]) +
                                listToStr(joint_orientation) + ' 1 ' +
                                listToStr(offset_position[1]) +
                                listToStr(joint_orientation))
        # --- old things even if they don't work well are often more stable, just for creating the prototype ...
        offsetNode.createObject(
            'JointSpringForceField',
            template='Rigid',
            name='joint',
            object1='@.',
            object2='@.',
            spring=
            ' BEGIN_SPRING 0 1 FREE_AXIS 0 1 0 0 0 0 KS_T 0 1E10 KS_R 0 1e10 KS_B 3E3 KD 0.1 R_LIM_X 0 0 R_LIM_Y 0 0 R_LIM_Z 0 0 REST_T 0 1 0 END_SPRING'
        )

        #============================= Registration model ================================
        objMainNode = rigidNode.createChild('deformable')
        scaleNode.addChild(objMainNode)
        # scene creation
        loader = objMainNode.createObject('MeshObjLoader',
                                          name='source',
                                          filename='./mesh/source.obj',
                                          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=0.01,
                                 padSize=0,
                                 rotateImage='false')
        objMainNode.createObject('ImageContainer',
                                 template='ImageUC',
                                 name='image',
                                 src='@rasterizer',
                                 drawBB='false')
        objMainNode.createObject('MechanicalObject',
                                 template='Affine',
                                 name='parent',
                                 showObject=1,
                                 src='@../../frame_topo',
                                 showObjectScale='0.1')
        objMainNode.createObject('RigidScaleToAffineMultiMapping',
                                 template='Rigid,Vec3d,Affine',
                                 input1='@../../rigid/DOFs',
                                 input2='@../../scale/DOFs',
                                 output='@.',
                                 autoInit='1',
                                 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)
        # Behavior Node
        objBehaviorNode = objMainNode.createChild('behavior')
        objBehaviorNode.createObject('ImageGaussPointSampler',
                                     name='sampler',
                                     indices='@../SF.indices',
                                     weights='@../SF.weights',
                                     transform='@../SF.transform',
                                     method=2,
                                     order=1,
                                     targetNumber=40)
        objBehaviorNode.createObject('MechanicalObject', template='F331')
        objBehaviorNode.createObject('LinearMapping', template='Affine,F331')
        objBehaviorNode.createObject('ProjectiveForceField',
                                     template='F331',
                                     youngModulus=1E6,
                                     poissonRatio=0,
                                     viscosity=0,
                                     isCompliance=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.8')
        objVisuNode.createObject('IdentityMapping', template='Vec3d,ExtVec3f')
        # Registration : first attraction field to well positioning the source on the target
        objRegistrationNode = objMainNode.createChild('force')
        objRegistrationNode.createObject('MeshTopology',
                                         name='topo',
                                         src='@../source')
        objRegistrationNode.createObject('MechanicalObject',
                                         template='Vec3d',
                                         name='DOFs')
        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)
        # Registration : surface to surface registration forces
        surfaceRegistrationNode = objMainNode.createChild('reg_force')
        surfaceRegistrationNode.createObject('MeshTopology',
                                             name='topo',
                                             src='@../source')
        surfaceRegistrationNode.createObject('MechanicalObject', name='DOFs')
        surfaceRegistrationNode.createObject('Triangle')
        surfaceRegistrationNode.createObject('LinearMapping',
                                             template='Affine,Vec3d',
                                             assemble=0)
        surfaceRegistrationNode.createObject('NormalsFromPoints',
                                             name='normalsFromPoints',
                                             template='Vec3d',
                                             position='@DOFs.position',
                                             triangles='@topo.triangles',
                                             invertNormals=0)
        surfaceRegistrationNode.createObject(
            'ClosestPointRegistrationForceField',
            name='ICP',
            template='Vec3d',
            sourceTriangles='@topo.triangles',
            sourceNormals='@normalsFromPoints.normals',
            position='@../../../target/loader.position',
            triangles='@../../../target/loader.triangles',
            normals='@../../../target/normalsFromPoints.normals',
            cacheSize=4,
            blendingFactor=1,
            stiffness=1E3,
            damping=1E-3,
            outlierThreshold=0,
            normalThreshold=0,
            rejectOutsideBbox=0,
            drawColorMap='0')
Example #45
0
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 )
Example #46
0
## testing Rigid.generate_rigid that computes com, mass and inertia from a mesh

# TODO compute and test principal axes basis by rotating meshes


from SofaTest.Macro import *
from Compliant import Tools
from SofaPython import Quaternion
import SofaPython.mass
import numpy

path = Tools.path( __file__ ) + "/geometric_primitives/"


# cube.obj is unit 1x1x1 cube
# sphere.obj radius=1
# cylinder.obj, height=1 in z, radius=1
# all meshes are centered in (0.5,0.5,0.5)

meshes = ['cube.obj', 'sphere.obj', 'cylinder.obj']
scales = [[1,1,1],[1,1,10],[3.3,3.3,10],[10,5,2]]
densities = [1, 1000, 7.7]
rotations = [[90,0,0],[22.456,0,0],[0,90,0],[0,23.546,0],[0,0,90],[0,0,-63.2],[90,90,0],[90,12.152,0],[25.645,12.36,0],[90,90,90],[-12.356,124.33,-56.1]]

# prepare multi-dimensional structure
masses = []
for x in range(len(meshes)):
    row = []
    for y in xrange(len(scales)):
        col = []
        for z in xrange(len(densities)):
Example #47
0
## testing Rigid.generate_rigid that computes com, mass and inertia from a mesh

# TODO compute and test principal axes basis by rotating meshes

from SofaTest.Macro import *
from Compliant import Tools
from SofaPython import Quaternion
import SofaPython.mass
import numpy

path = Tools.path(__file__) + "/geometric_primitives/"

# cube.obj is unit 1x1x1 cube
# sphere.obj radius=1
# cylinder.obj, height=1 in z, radius=1
# all meshes are centered in (0.5,0.5,0.5)

meshes = ['cube.obj', 'sphere.obj', 'cylinder.obj']
scales = [[1, 1, 1], [1, 1, 10], [3.3, 3.3, 10], [10, 5, 2]]
densities = [1, 1000, 7.7]
rotations = [[90, 0, 0], [22.456, 0, 0], [0, 90, 0], [0, 23.546, 0],
             [0, 0, 90], [0, 0, -63.2], [90, 90, 0], [90, 12.152, 0],
             [25.645, 12.36, 0], [90, 90, 90], [-12.356, 124.33, -56.1]]

# prepare multi-dimensional structure
masses = []
for x in range(len(meshes)):
    row = []
    for y in xrange(len(scales)):
        col = []
        for z in xrange(len(densities)):
    def createGraph(self, root):

        # Variable
        self.E_t = 0
        self.E_t_dt = 0
        self.root_node = root
        self.root_node.findData('gravity').value = '0 0 0'
        
        # Sofa parameters
        self.root_node.createObject('BackgroundSetting',color='1 1 1')
        self.root_node.createObject('VisualStyle', displayFlags='showVisual hideWireframe showBehaviorModels hideForceFields showInteractionForceFields')

        self.root_node.createObject('EulerImplicit',rayleighStiffness='0.01',rayleighMass='0.01')
        self.root_node.createObject('CGLinearSolver', iterations=200, tolerance=1E-6, threshold=1E-6)
        
        # Object to transfer creation
        node = self.root_node.createChild('main')
        node.createObject('MeshObjLoader',name='source', filename='./mesh/source.obj', 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=0.01, padSize=2, rotateImage='false')
        node.createObject('ImageContainer', template='ImageUC', name='image', src='@rasterizer', drawBB='false')
        node.createObject('MeshTopology', name='frame_topo', position=listListToStr(dofs_position))

        #================================ Target model ===================================
        targetNode = node.createChild('target')
        targetNode.createObject('MeshObjLoader', name='loader', filename='./mesh/target.obj', rotation='0 0 0', translation='0 0 0', scale3d='1 1 1', showObject=0)
        targetNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', src='@loader', showObject=0)
        targetNode.createObject('NormalsFromPoints', name='normalsFromPoints', template='Vec3d', position='@DOFs.position', triangles='@loader.triangles', invertNormals=0)
        targetNode.createObject('FixedConstraint', fixAll='1', drawSize=0.001)
        targetVisuNode = targetNode.createChild('visu')
        targetVisuNode.createObject('OglModel', template='ExtVec3f', name='visual', src='@../loader', color='0.5 0.5 0.5 0.85')

        #================================ Rigid frame ====================================
        rigidNode = node.createChild('rigid')           
        rigidNode.createObject('MechanicalObject', template='Rigid', name='DOFs', src='@../frame_topo', showObject=1, showObjectScale='0.1')
        rigidNode.createObject('RigidMass', mass="1 1 ", inertia="1 1 1  1 1 1")
        #rigidNode.createObject('FixedConstraint', name='fixed', indices='0')

        #=================================== Scale =======================================
        scaleNode = node.createChild('scale')
        scaleNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', position='1 1 1 1 1 1', showObject=0, showObjectScale='0.1')

        #=========================== Alignement constraint ===============================
        offsetNode = rigidNode.createChild('offset')
        offsetNode.createObject('MechanicalObject', template='Rigid', name='DOFs', position='0 0 0 0 0 0 1  0 0 0 0 0 0 1', showObject=1, showObjectScale='0.1')
        offsetNode.createObject('AssembledRigidRigidMapping', template='Rigid,Rigid', source='0 '+listToStr(offset_position[0]) + listToStr(joint_orientation) + ' 1 '+listToStr(offset_position[1]) + listToStr(joint_orientation))
        # --- old things even if they don't work well are often more stable, just for creating the prototype ...
        offsetNode.createObject('JointSpringForceField', template='Rigid', name='joint', object1='@.', object2='@.', spring=' BEGIN_SPRING 0 1 FREE_AXIS 0 1 0 0 0 0 KS_T 0 1E10 KS_R 0 1e10 KS_B 3E3 KD 0.1 R_LIM_X 0 0 R_LIM_Y 0 0 R_LIM_Z 0 0 REST_T 0 1 0 END_SPRING')

        #============================= Registration model ================================
        objMainNode = rigidNode.createChild('deformable')
        scaleNode.addChild(objMainNode)
        # scene creation
        loader = objMainNode.createObject('MeshObjLoader',name='source', filename='./mesh/source.obj', 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=0.01, padSize=0, rotateImage='false')
        objMainNode.createObject('ImageContainer', template='ImageUC', name='image', src='@rasterizer', drawBB='false') 
        objMainNode.createObject('MechanicalObject', template='Affine', name='parent', showObject=1, src='@../../frame_topo', showObjectScale='0.1')
        objMainNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3d,Affine', input1='@../../rigid/DOFs', input2='@../../scale/DOFs', output='@.', autoInit='1', 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)
        # Behavior Node
        objBehaviorNode = objMainNode.createChild('behavior')
        objBehaviorNode.createObject('ImageGaussPointSampler', name='sampler', indices='@../SF.indices', weights='@../SF.weights', transform='@../SF.transform', method=2, order=1, targetNumber=40)
        objBehaviorNode.createObject('MechanicalObject', template='F331')  
        objBehaviorNode.createObject('LinearMapping', template='Affine,F331')    
        objBehaviorNode.createObject('ProjectiveForceField', template='F331', youngModulus=1E6, poissonRatio=0, viscosity=0, isCompliance=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.8')
        objVisuNode.createObject('IdentityMapping', template='Vec3d,ExtVec3f')
        # Registration : first attraction field to well positioning the source on the target
        objRegistrationNode = objMainNode.createChild('force')
        objRegistrationNode.createObject('MeshTopology', name='topo', src='@../source')
        objRegistrationNode.createObject('MechanicalObject', template='Vec3d', name='DOFs')
        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)
        # Registration : surface to surface registration forces
        surfaceRegistrationNode = objMainNode.createChild('reg_force')
        surfaceRegistrationNode.createObject('MeshTopology', name='topo', src='@../source')
        surfaceRegistrationNode.createObject('MechanicalObject', name='DOFs')
        surfaceRegistrationNode.createObject('Triangle')
        surfaceRegistrationNode.createObject('LinearMapping', template='Affine,Vec3d', assemble=0)
        surfaceRegistrationNode.createObject('NormalsFromPoints', name='normalsFromPoints', template='Vec3d', position='@DOFs.position', triangles='@topo.triangles', invertNormals=0)
        surfaceRegistrationNode.createObject('ClosestPointRegistrationForceField', name='ICP', template='Vec3d'
                                                                                             , sourceTriangles='@topo.triangles', sourceNormals='@normalsFromPoints.normals'
                                                                                             , position='@../../../target/loader.position' , triangles='@../../../target/loader.triangles', normals='@../../../target/normalsFromPoints.normals'
                                                                                             , cacheSize=4, blendingFactor=1, stiffness=1E3, damping=1E-3
                                                                                             , outlierThreshold=0, normalThreshold=0, rejectOutsideBbox=0, drawColorMap='0')
Example #49
0
def run():

    ok = True

    info = SofaPython.mass.RigidMassInfo()

# testing axis-aligned known geometric shapes
    for m in xrange(len(meshes)):
        mesh = meshes[m]
        mesh_path = path + meshes[m]

        for s in xrange(len(scales)):
            scale = scales[s]

            if mesh=="cylinder.obj" and scale[0]!=scale[1]:
                continue
        
            for d in xrange(len(densities)):
                density=densities[d]

                info.setFromMesh( mesh_path, density, scale )

                error = " ("+meshes[m]+", s="+Tools.cat(scale)+" d="+str(density)+")"

                ok &= EXPECT_TRUE( almostEqualReal(info.mass, masses[m][s][d]), "mass"+error+" "+str(info.mass)+"!="+str(masses[m][s][d]) )
                ok &= EXPECT_TRUE( almostEqualLists(info.com,[x*0.5 for x in scale]), "com"+error+" "+Tools.cat(info.com)+"!="+Tools.cat([x*0.5 for x in scale]) )
                ok &= EXPECT_TRUE( almostEqualLists(info.diagonal_inertia,inertia[m][s][d]), "inertia"+error+" "+str(info.diagonal_inertia)+"!="+str(inertia[m][s][d]) )

# testing diagonal inertia extraction from a rotated cuboid
    mesh = "cube.obj"
    mesh_path = path + mesh
    scale = scales[3]
    density = 1
    theory = sorted(inertia[0][3][0])
    for r in rotations:
        info.setFromMesh( mesh_path, density, scale, r )
        local = sorted(info.diagonal_inertia)
        ok &= EXPECT_TRUE( almostEqualLists(local,theory), "inertia "+str(local)+"!="+str(theory)+" (rotation="+str(r)+")" )

# testing extracted inertia rotation
    mesh = "rotated_cuboid_12_35_-27.obj"
    mesh_path = path + mesh
    density = 1
    info.setFromMesh( mesh_path, density )

    # theoretical results
    scale = [2,3,1]
    mass = density * scale[0]*scale[1]*scale[2]
    inertiat = numpy.empty(3)
    inertiat[0] = 1.0/12.0 * mass * (scale[1]*scale[1]+scale[2]*scale[2]) # x
    inertiat[1] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[2]*scale[2]) # y
    inertiat[2] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[1]*scale[1]) # z

    # used quaternion in mesh

    q = Quaternion.normalized( Quaternion.from_euler( [12*math.pi/180.0, 35*math.pi/180.0, -27*math.pi/180.0] ) )

    # corresponding rotation matrices (ie frame defined by columns)
    mt = Quaternion.to_matrix( q )
    m  = Quaternion.to_matrix( info.inertia_rotation )

    # matching inertia
    idxt = numpy.argsort(inertiat)
    idx  = numpy.argsort(info.diagonal_inertia)

    # checking if each axis/column are parallel (same or opposite for unitary vectors)
    for i in xrange(3):
        ok &= EXPECT_TRUE( almostEqualLists(mt[:,idxt[i]].tolist(),m[:,idx[i]].tolist(),1e-5) or almostEqualLists(mt[:,idxt[i]].tolist(),(-m[:,idx[i]]).tolist(),1e-5), "wrong inertia rotation" )


#    print mt[:,idxt]
#    print m [:,idx ]


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

    node.dt = 0.005

    # friction coefficient
    shared.mu = 0.5

    scene = Tools.scene( node )

    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')
    node.removeObject(ode)

    node.createObject('RequiredPlugin',
                      pluginName = 'pouf')

    ode = node.createObject('pouf_solver')

    num = node.createObject('pouf.pgs',
                            name = 'num',
                            iterations = 100,
                            homogenize =  True,
                            precision = 1e-14)
    
    proximity = node.getObject('proximity')

    proximity.alarmDistance = 0.5
    proximity.contactDistance = 0.1

    # ground
    ground = Rigid.Body('ground');
    ground.node = ground.insert( scene );

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

    # ground-plane joint
    frame = Rigid.Frame()
    frame.translation = [8, 0, 0]

    joint = Rigid.RevoluteJoint(2)
    joint.absolute(frame, ground.node, plane.node)
    joint.upper_limit = 0

    joint.node = joint.insert( scene )
    
    # box
    box = Rigid.Body('box')
    box.visual = path + '/share/mesh/cube.obj'
    box.collision = box.visual
    box.dofs.translation = [0, 3, 0]
    box.mass_from_mesh( box.visual, 50 )
    box.node = box.insert( scene )

    shared.plane = plane.node.getObject('dofs')
    shared.box = box.node.getObject('dofs')
    shared.joint = joint.node.getObject('dofs')

    # pid
    shared.pid = Control.PID(shared.joint)
    shared.pid.pos = -math.atan( shared.mu ) # target should trigger slide
    shared.pid.basis = [0, 0, 0, 0, 0, 1]

    # shared.pid.dofs.externalForce = '-1e7'

    scale = 2e5

    shared.pid.kp = - 5 * scale
    shared.pid.kd =  - 100 * scale
    shared.pid.ki = - 1e-1 * scale