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