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
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 )
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 = [
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