ball_collision_testing.scn

<?xml version="1.0" ?>
<!-- Testing ball collision problem from hugo -->
<Node name="root" dt="0.05" gravity="0 -9.810 0">
    <VisualStyle displayFlags="hideBehaviorModels hideCollisionModels hideMappings hideForceFields showVisualModels hideInteractionForceFields" />
    <RequiredPlugin name='SofaMiscCollision'/>
    <RequiredPlugin name="SofaPython" pluginName="SofaPython" />

    <FreeMotionAnimationLoop solveVelocityConstraintFirst="0" />
    <LCPConstraintSolver maxIt="1000" tolerance="1e-6" initial_guess="false" build_lcp="true" multi_grid="false" printLog="0" mu="0.000000000001"/>

    <CollisionPipeline depth="6" verbose="0" draw="0" />
    <BruteForceDetection name="N2" />
    <!-- <LocalMinDistance name="Proximity" alarmDistance="03" contactDistance="01" useLMDFilters="0" /> -->
    <MinProximityIntersection name="Proximitty" alarmDistance="3.0" contactDistance="1.0" />
    <CollisionResponse name="Response" response="FrictionContact" />

    <Node name="Input">
        <PythonScriptController filename="keyboardControl.py" classname="KeyboardControl"/>
        <!-- <PythonScriptController filename="sofaTest.py" classname="clothMesh"/> -->
        <MechanicalObject template="Rigid" name="DOFs" scale="3"/>
        <Node name="VisuAvatar" activated="true" >
            <OglModel name="Visual" fileMesh="mesh/sphere.obj" color="blue" scale="3" />
            <RigidMapping input="@.." output="@Visual"/>
        </Node>
        <Node name="RefModel">
            <MeshObjLoader filename="mesh/sphere.obj"  name="loader"/>
            <Mesh src="@loader"  />
            <MechanicalObject src="@loader" name="instrumentCollisionState"/>
            <RigidMapping />
        </Node>
        
    </Node>



    <Node name="ball">
        <EulerImplicitSolver name="ODE solver" rayleighStiffness="0.05" rayleighMass="1.0" />
        <CGLinearSolver name="linear solver" iterations="25" tolerance="1e-10" threshold="10e-100" /> 
        <MechanicalObject name="ballState" template="Rigid3d"/>
        <UniformMass name="mass" totalmass="0.1" />
        <LCPForceFeedback activate="true" forceCoef="0.001"/>
        <UncoupledConstraintCorrection compliance="100"/>
        <Node name="ball_surf" >
            <MeshObjLoader name="meshLoader" filename="mesh/sphere.obj" />
            <Mesh src="@meshLoader" />
            <MechanicalObject name="ballCollisionState" template="Vec3d" src="@meshLoader" scale="3" translation="0 0 0"/>
            <UniformMass totalMass="0.1" />
            <UncoupledConstraintCorrection compliance="1"/> 
            <RigidMapping input="@.." output="@ballCollisionState"/>
            <Node name="VisualModel" >
               <OglModel name="ballVisualModel" src="@../meshLoader" color="1.0 0.2 0.2 1.0" scale="3" translation="0 0 0"/>
               <IdentityMapping input="@.." output="@ballVisualModel"/>
            </Node>

        </Node>
        <Sphere />

        <VectorSpringForceField  object1="@Input/RefModel" object2="@ball/ball_surf/ballCollisionState" stiffness="100000" viscosity="0" />

    </Node>

    <Node name="Floor">
      <MeshObjLoader name="loader" filename="mesh/floorFlat.obj" triangulate="true" scale="10" translation="50 -10 50"  />
      <MeshTopology src="@loader" />
      <MechanicalObject src="@loader" name="DOFs" template="Vec3d"  />
      <Triangle simulated="0" moving="0" contactFriction="100"/>
      <Line simulated="0" moving="0" contactFriction="100"/>
      <Point simulated="0" moving="0" contactFriction="100"/>
      <OglModel name="FloorV" />
    </Node>

</Node>

keyboardControl.py

import Sofa
import numpy as np

''' To test ball collision prob from hugo'''

class KeyboardControl(Sofa.PythonScriptController):
    # called once graph is created, to init some stuff...
    def initGraph(self,node):
        print 'initGraph called (python side)'
        self.MechanicalState = node.getObject('DOFs')
        gravity = node.findData('gravity').value
        print gravity
        self.rootNode = node.getRoot()
        self.ballMesh = self.rootNode.getChild('ball')
        self.node = node
        
        return 0
     
     
    def onKeyPressed(self,k):
        
        free_position=self.MechanicalState.position


        # translation speed
        speed = 1 

       
        if ord(k)==19:
            self.rootNode.reset()
        # DOWN key : rear
        if ord(k)==21:
            self.rootNode.cleanup()
        # LEFT key : left
        if ord(k)==23:
            free_position[0][1]-=speed
            self.MechanicalState.findData('position').value=free_position
            self.MechanicalState.free_position=free_position
        # RIGHT key : right
        if ord(k)==22:
            free_position[0][1]+=speed
            self.MechanicalState.findData('position').value=free_position
            self.MechanicalState.free_position=free_position
        # PAGEUP key : up
        if ord(k)==18:
            free_position[0][0]-=speed
            self.MechanicalState.findData('position').value=free_position
            self.MechanicalState.free_position=free_position
        # PAGEDN key : down
        if ord(k)==20:
            free_position[0][0]+=speed
            self.MechanicalState.findData('position').value=free_position
            self.MechanicalState.free_position=free_position
        
        return 0