Exemple #1
0
    def onBeginAnimationStep(self, dt):

        # pid update
        shared.pid.pre_step( dt )

        # info display
        relative = Rigid.Frame( shared.plane.position[0] ).inv() * Rigid.Frame( shared.box.position[0] )
        tangent = [relative.translation[0], relative.translation[2]]

        relative_speed = Vec.diff(shared.box.velocity[0][:3], shared.plane.velocity[0][:3])

        local = Quaternion.rotate(Quaternion.conj( Rigid.Frame( shared.plane.position[0] ).rotation ),
                                  relative_speed)
        
        tangent_speed = [local[0], local[2]]

        print 'plane/box relative speed (norm): ', Vec.norm(tangent_speed)
        
        alpha = math.fabs( shared.joint.position[0][-1] )
        mu = math.tan( alpha )
        print 'plane/ground angle:', alpha , 'mu = ', mu
        
        if mu > shared.mu:
            print '(should be moving)'
 
        print
        
        return 0
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
Exemple #3
0
def createScene(root):
    root.createObject('RequiredPlugin', pluginName = 'Compliant')
    root.createObject('VisualStyle', displayFlags="showBehavior" )
    
    root.dt = 0.001
    root.gravity = [0, -9.8, 0]
    
    ode = root.createObject('AssembledSolver')
    ode.stabilization = True
    
    num = root.createObject('MinresSolver')
    num.iterations = 500
    
    scene = root.createChild('scene')
    
    base = Rigid.Body('base')
    moving = Rigid.Body('moving')

    moving.inertia_forces = True

    moving.dofs.translation = [0, 2, 0]

    base_node = base.insert( scene );
    base_node.createObject('FixedConstraint', indices = '0')

    moving_node = moving.insert( scene );

    base_offset = Rigid.Frame()
    base_offset.translation = [0, 1, 0]
    
    moving_offset = Rigid.Frame()
    moving_offset.translation = [0, -1, 0]
    
    joint = Rigid.SphericalJoint()
    
    # only rotation dofs
    joint.append(base_node, base_offset)
    joint.append(moving_node, moving_offset)

    node = joint.insert(scene)
    
    node.createObject('UniformCompliance',
                      template = 'Vec6d',
                      compliance = 1e-3 )
Exemple #4
0
def createScene(root):

    # root node setup
    root.createObject('RequiredPlugin', pluginName='Compliant')
    root.createObject('VisualStyle', displayFlags="showBehavior")

    # simuation parameters
    root.dt = 1e-2
    root.gravity = [0, -9.8, 0]

    # ode solver
    ode = root.createObject('AssembledSolver')
    ode.stabilization = True

    # numerical solver
    num = root.createObject('MinresSolver')
    num.iterations = 500

    # scene node
    scene = root.createChild('scene')

    # script variables
    n = 10
    length = 2

    # objects creation
    obj = []

    for i in xrange(n):
        # rigid bodies
        body = Rigid.Body()
        body.name = 'link-' + str(i)
        body.dofs.translation = [0, length * i, 0]

        body.inertia_forces = 'true'

        obj.append(body)
        # insert the object into the scene node, saves the created
        # node in body_node
        body.node = body.insert(scene)

    # joints creation
    for i in xrange(n - 1):
        # the joint
        j = Rigid.SphericalJoint()

        # joint offset definitions
        up = Rigid.Frame()
        up.translation = [0, length / 2, 0]

        down = Rigid.Frame()
        down.translation = [0, -length / 2, 0]

        # append node/offset to the joint
        j.append(obj[i].node, up)  # parent
        j.append(obj[i + 1].node, down)  # child

        j.insert(scene)

    # attach first node
    obj[0].node.createObject('FixedConstraint', indices='0')
def createScene(node):
    node.createObject('RequiredPlugin', pluginName='Compliant')

    node.animate = 'true'

    node.createObject(
        'VisualStyle',
        displayFlags=
        'hideBehaviorModels hideCollisionModels hideMappings hideForceFields')
    node.dt = 0.005

    node.gravity = '0 -9.81 0'

    ode = node.createObject('AssembledSolver', name='odesolver')

    ode.stabilization = 'true'
    # ode.debug = 'true'

    num = node.createObject('MinresSolver',
                            name='numsolver',
                            iterations='250',
                            precision='1e-14')

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

    scene = node.createChild('scene')

    inertia_forces = 'true'

    # dofs
    base = Rigid.Body('base')
    base.node = base.insert(scene)
    base.visual = 'mesh/box.obj'
    base.node.createObject('FixedConstraint', indices='0')

    link1 = Rigid.Body('link1')
    link1.dofs.translation = [0, 0, 0]
    link1.visual = 'mesh/cylinder.obj'
    link1.inertia_forces = inertia_forces
    link1.mass_from_mesh(link1.visual)
    link1.node = link1.insert(scene)

    link2 = Rigid.Body('link2')
    link2.dofs.translation = [0, 10, 0]
    link2.visual = 'mesh/cylinder.obj'
    link2.mass_from_mesh(link2.visual)
    link2.inertia_forces = inertia_forces
    link2.node = link2.insert(scene)

    # joints
    joint1 = Rigid.RevoluteJoint(2)
    joint1.append(base.node, Rigid.Frame().read('0 0 0 0 0 0 1'))
    joint1.append(link1.node, Rigid.Frame().read('0 0 0 0 0 0 1'))
    joint1.node = joint1.insert(scene)

    joint2 = Rigid.RevoluteJoint(2)
    joint2.append(link1.node, Rigid.Frame().read('0 10 0 0 0 0 1'))
    joint2.append(link2.node, Rigid.Frame().read('0 0 0 0 0 0 1'))
    joint2.node = joint2.insert(scene)

    # control
    control.joint1 = ControlledJoint(joint1.node.getObject('dofs'))

    control.joint2 = ControlledJoint(joint2.node.getObject('dofs'))

    # pid controller reference pos for joint2
    control.joint2.ref_pos = [0, 0, 0, 0, 0, math.pi / 2]

    return node
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')]
]

# joints: parent offset, child offset, joint def
links = [ 
Exemple #7
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

    bench = node.createObject('Benchmark')
    
    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 = [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 = 1e6

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