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(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(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('CompliantImplicitSolver')
ode.stabilization = "pre-stabilization"
# 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)
body.node.getObject("dofs").showObject = True
# joints creation
for i in xrange(n - 1):
# the joint
j = Rigid.SphericalJoint()
# joint offset definitions
up = Rigid.Frame.Frame()
up.translation = [0, length / 2, 0]
down = Rigid.Frame.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', name='Compliant')
node.createObject('CompliantAttachButtonSetting')
node.createObject(
'VisualStyle',
displayFlags=
'hideBehaviorModels hideCollisionModels hideMappings hideForceFields')
node.findData('dt').value = 0.01
node.findData('gravity').value = '0 -9.81 0'
node.createObject('CompliantImplicitSolver',
name='odesolver',
stabilization="true")
node.createObject('MinresSolver',
name='numsolver',
iterations='250',
precision='1e-14')
scene = node.createChild('scene')
rigid = []
joint = []
# create rigid bodies
for p in parts:
r = Rigid.Body()
r.name = p[0]
# r.collision = part_path + p[1]
r.dofs.read(p[3])
r.visual = mesh_path + '/' + p[15]
r.collision = r.visual
r.inertia_forces = True
density = float(p[7])
r.mass_from_mesh(r.visual, density)
r.insert(scene)
rigid.append(r)
# create joints
for i in links:
j = i[2]
j.compliance = 0
p = offset[i[0]][0]
off_p = offset[i[0]][1]
c = offset[i[1]][0]
off_c = offset[i[1]][1]
j.append(rigid[p].user, off_p)
j.append(rigid[c].user, off_c)
joint.append(j.insert(scene))
# fix first body
rigid[0].node.createObject('FixedConstraint', indices='0')
return node
# joint offsets
offset = [[0, Frame.Frame().read('0 0 0 0 0 0 1')],
[1, Frame.Frame().read('0 0 0.148 0 0 0 1')],
[1, Frame.Frame().read('0.24 -0.145 0.478 0 0 0 1')],
[2, Frame.Frame().read('-0.845 -0.073 0 0 0 0 1')],
[2, Frame.Frame().read('0.852 0.072 0 0 0 0 1')],
[3, Frame.Frame().read('-0.113 0 0 0 0 0 1')],
[3, Frame.Frame().read('0.15 0 0 0 0 0 1')],
[0, Frame.Frame().read('2.2 0 0.33 0 0 0 1')]]
# joints: parent offset, child offset, joint def
links = [
# revolute joint around z
[0, 1, Rigid.RevoluteJoint(2)], # corps-roue
[2, 3, Rigid.RevoluteJoint(2)], # roue-came
[4, 5, Rigid.RevoluteJoint(2)], # came-piston
# sliding joint around x
[6, 7, Rigid.PrismaticJoint(0)]
]
def createScene(node):
node.createObject('RequiredPlugin', name='Compliant')
node.createObject('CompliantAttachButtonSetting')
node.createObject(
'VisualStyle',
displayFlags=
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)
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
def update(self, dt):
tau, e = self.pid()
self.dofs.externalForce = Rigid.concat(tau)
self.integral = [i + dt * f for i, f in zip(self.integral, e)]
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 update(self, dt):
tau, e = self.pid()
self.dofs.externalForce = Rigid.concat(tau)
self.integral = [i + dt * f for i, f in zip(self.integral, e) ]
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
def run():
ok = True
# 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 = Rigid.generate_rigid(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.tolist(),
inertia[m][s][d]),
"inertia" + error + " " + str(info.diagonal_inertia) +
"!=" + str(inertia[m][s][d]) + " " + str(info.inertia))
# 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 = Rigid.generate_rigid(mesh_path, density, scale, r)
local = sorted(info.diagonal_inertia.tolist())
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 = Rigid.generate_rigid(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' )
# 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 )
