def setupSimulation(N):
global world, walls, space, geoms, objs
world = ode.World()
space = ode.Space()
world.setERP(0.8)
world.setCFM(1E-5)
objs = []
planeConstraint = ode.Plane2DJoint(world)
for i in range(N):
ball = ode.Body(world)
M = ode.Mass()
M.setSphere(2500.0, 0.05)
M.mass = 2
ball.setMass(M)
bx = (np.random.rand() * 2 - 1) * 0.7
by = (np.random.rand() * 2 - 1) * 0.7
ball.setPosition((bx, by, 0))
geom = ode.GeomSphere(space, radius=0.2)
geom.setBody(ball)
geoms.append(geom)
planeConstraint.attach(ball, ode.environment)
objs.append(ball)
walls = []
walls.append(ode.GeomPlane(space, (0, 1, 0), -1))
walls.append(ode.GeomPlane(space, (1, 0, 0), -1))
walls.append(ode.GeomPlane(space, (0, -1, 0), -1))
walls.append(ode.GeomPlane(space, (-1, 0, 0), -1))
def __init__(self, logfile, stepsize=0.005):
self.logfile = logfile
# Create a world object
self.world = ode.World()
self.world.setGravity((0, 0, 0))
self.world.setERP(0.5)
self.world.setCFM(1E-4)
# Create a space object
self.space = ode.Space()
# Create a plane geom which prevent the objects from falling forever
self.floor = ode.GeomPlane(self.space, (0, 1, 0), 0)
# A list of ODE Bodies
self.bodies = {}
# A list of geoms for the bodies
self.geoms = {}
# Set the stepsize for the instance.
self.stepsize = stepsize
self.positions = []
self.quaternions = []
self.curstep = 0
self.read_file()
def __init__(self,
dt=argv.get("dt", 0.02),
mu=5000,
floor_mu=None,
bounce=1.2,
has_floor=True,
has_gravity=True,
threshold=0.001,
ERP=0.8,
CFM=1e-5):
world = ode.World()
if has_gravity:
world.setGravity((0, -9.81, 0))
world.setERP(ERP)
world.setCFM(CFM)
#world.setCFM(1e-4) # more spongey
space = ode.Space()
if has_floor:
self.floor = ode.GeomPlane(space, (0, 1, 0), 0)
else:
self.floor = None
self.world = world
self.space = space
self.items = []
self.transient = []
self.dt = dt
self.mu = mu
self.threshold = threshold
if floor_mu is None:
floor_mu = mu
self.floor_mu = floor_mu
self.bounce = bounce
def falling_torus_torus(event=None):
display.EraseAll()
dyn_context = DynamicSimulationContext()
dyn_context.set_display(display, safe_yield)
dyn_context.enable_collision_detection()
dyn_context.enable_gravity()
# The torus
s1 = BRepPrimAPI_MakeTorus(15,5).Shape()
d = dyn_context.register_shape(s1, enable_collision_detection=True,use_trimesh=True)
# The box
s2 = BRepPrimAPI_MakeTorus(10,6).Shape()
transformed_shape_2 = translate_topods_from_vector(s2, gp_Vec(0,0,100))
d2 = dyn_context.register_shape(transformed_shape_2,enable_collision_detection=True,use_trimesh=True)
d2.setAngularVel([-3,3,3])
# The plane (note: this plane is not a dynamic shape, it's just displayed)
P1 = gp_Pnt(0,0,-100)
V1 = gp_Vec(0,0,1)
face = make_plane(P1,V1,-100., 100., -60., 60.)
# Then create a geom for this plane
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(dyn_context._space, (0,0,1), -100)
display.DisplayColoredShape(face,'RED')
display.FitAll()
#Starts the simulation
dyn_context.set_simulation_duration(20) #1s forthe simulation
dyn_context.set_time_step(0.01)
dyn_context.start_open_loop()
def falling_torus(event=None):
display.EraseAll()
dyn_context = DynamicSimulationContext()
dyn_context.set_display(display, safe_yield)
dyn_context.enable_collision_detection()
dyn_context.enable_gravity()
# test the post_register_callback feature
def f():
print 'Wooooooooooo'
dyn_context.register_post_step_callback(f)
# The torus
s1 = BRepPrimAPI_MakeTorus(15,5).Shape()
d = dyn_context.register_shape(s1,enable_collision_detection=True,use_trimesh=True)
d.setAngularVel([-1,-0.5,0.3]) # the box is rotating
# The plane (note: this plane is not a dynamic shape, it's just displayed)
P1 = gp_Pnt(0,0,-100)
V1 = gp_Vec(0,0,1)
face = make_plane(P1,V1, -100., 100., -60., 60.)
# Then create a geom for this plane
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(dyn_context._space, (0,0,1), -100)
display.DisplayColoredShape(face,'RED')
display.FitAll()
#Starts the simulation
dyn_context.set_simulation_duration(20) #1s forthe simulation
dyn_context.set_time_step(0.01)
dyn_context.start_open_loop()
def falling_pump(event=None):
def load_brep(filename):
# Load the shape from a file
aShape = TopoDS_Shape()
builder = BRep_Builder()
BRepTools_Read(aShape, str(filename), builder)
return aShape
# Just a copy/paste of the torus code
display.EraseAll()
dyn_context = DynamicSimulationContext()
dyn_context.set_display(display, safe_yield)
dyn_context.enable_collision_detection()
dyn_context.enable_gravity()
s1 = load_brep('../../data/BREP/40_pump_body.brep')
d = dyn_context.register_shape(s1,
enable_collision_detection=True,
use_trimesh=True)
# The plane (note: this plane is not a dynamic shape, it's just displayed)
P1 = gp_Pnt(0, 0, -1000)
V1 = gp_Vec(0, 0, 1)
face = make_plane(P1, V1, -1000., 1000., -600., 600.)
# Then create a geom for this plane
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(dyn_context._space, (0, 0, 1), -1000)
display.DisplayColoredShape(face, 'RED')
display.FitAll()
#Starts the simulation
dyn_context.set_simulation_duration(30) #1s forthe simulation
dyn_context.set_time_step(0.001)
dyn_context.start_open_loop()
del dyn_context
def __init__(self, robotfile, standalone=0,obstaclefile=None,regionfile=None,region_calib=None,startingpose=None,heightmap=None): """ Initialize the simulator. """ # Simulation world parameters. self._initOpenGL() self.world = ode.World() self.world.setGravity((0, -9.81, 0)) self.world.setERP(0.1) self.space = ode.Space() self.ground = ode.GeomPlane(space=self.space, normal=(0,1,0), dist=0) # CKBot module parameters. self.cubesize = 6.0 self.cubemass = 10.0 self.hingemaxangle = 1.6 self.hingeminangle = -1.6 self.hingemaxforce = 1000000 # Control parameters. self.gait = 0 self.gain = 1.5 self.counter = 0 # Setting standalone to 1 allows for manual key input. # Setting standalone to 0 (LTLMoP mode) causes the camera to automatically follow the spawned robot) if standalone==1: self.standalone = 1 else: self.standalone = 0 self.clock = pygame.time.Clock()
def __init__(self, max_simsecs, noise_sd, quick):
log.debug('Sim.__init__(max_simsecs=%s)', max_simsecs)
# create world, set default gravity, geoms, flat ground, spaces etc.
assert type(max_simsecs) is float or type(max_simsecs) is int
self.quick = quick
self.total_time = 0.0
self.relax_time = 0
self.max_simsecs = float(max_simsecs)
self.world = ode.World()
if SOFT_WORLD:
self.world.setCFM(1e-6) # (defaults to 1e-10)
self.world.setERP(0.1) # (defaults to 0.2)
self.world.setGravity((0, 0, -9.8))
self.space = ode.SimpleSpace()
self.geoms = []
self.ground = ode.GeomPlane(self.space, (0, 0, 1), 0)
self.geoms.append(self.ground)
self.score = 0.0
self.finished = 0
self.siglog = None
self.bpgs = []
self.contactGroup = ode.JointGroup()
self.joints = []
self.noise_sd = noise_sd
self.points = []
def __init__(self):
# Create and draw arena and associated geoms
self.floor = ode.GeomPlane(vpyode._bigSpace, (0,1,0), 0)
self.arenafloor = box(pos=(0,0,0), size=(0.01,8,8), color=color.orange, material = tex2, axis=(0,1,0))
self.wall1 = ode.GeomPlane(vpyode._bigSpace, (-1,0,0), -4)
self.wall2 = ode.GeomPlane(vpyode._bigSpace, (1,0,0), -4)
self.wall3 = ode.GeomPlane(vpyode._bigSpace, (0,0,-1), -4)
self.wall4 = ode.GeomPlane(vpyode._bigSpace, (0,0,1), -4)
self.wall1vis = box(pos=(-4,0,0), size=(0.01,1,8), color=color.orange)
self.wall2vis = box(pos=(4,0,0), size=(0.01,1,8), color=color.orange)
self.wall3vis = box(pos=(0,0,-4), size=(8,1,0.01), color=color.orange)
self.wall4vis = box(pos=(0,0,4), size=(8,1,0.01), color=color.orange)
#Make internal walls
self.topwall = create_box(world,10000,0.25,0.1,2,(0,0.05,-3),0,color.brown)
self.rightwall = create_box(world,10000,2,0.1,0.25,(3,0.05,0),0,color.brown)
self.bottomwall = create_box(world,10000,0.25,0.1,2,(0,0.05,3),0,color.brown)
self.leftwall = create_box(world,10000,2,0.1,0.25,(-3,0.05,0),0,color.brown)
def __init__(self, col_callback, stepsize=0.005,log_data=False,output_path="",gravity=-9.81,fluid_dynamics=0,run_num=0,erp=0.5,cfm=1E-4):
# Create a world object
self.world = ode.World()
self.world.setGravity((0,gravity,0) )
self.world.setERP(erp)
self.world.setCFM(cfm)
self.world.setContactMaxCorrectingVel(5.)
# Run Number
self.run_num = run_num
# Create a space object
self.space = ode.Space()
# Create a plane geom which prevent the objects from falling forever
self.floor = ode.GeomPlane(self.space, (0,1,0), 0)
# A joint group for the contact joints that are generated whenever
# two bodies collide
self.contactgroup = ode.JointGroup()
# A list of ODE Bodies
self.bodies = {}
# Maintain a data structure of the surfaces of each body if fluid dynamics are required.
self.fluid_dynamics=fluid_dynamics
if(fluid_dynamics):
self.surfaces = {}
# A list of geoms for the bodies
self.geoms = {}
# A dict of terrain geoms for the world.
self.terrain_geoms = {}
# A list of joints in the world.
self.joints = {}
# Register the collision callback
self.col_callback = col_callback
# Set the stepsize for the instance.
self.stepsize = stepsize
# Create a cushion for joint limits.
self.joint_cushion = 0.#5.
# Create a maximum velocity that a joint is able to move in one step.
self.max_joint_vel = 36000. * self.stepsize*ANG_TO_RAD
# Logging functionality
self.log_data = log_data
self.output_path = output_path
if self.log_data:
self.positions = []
self.quaternions = []
self.joint_angles = []
def createWorld(self):
#create world
self.world = ode.World()
self.world.setGravity(GRAVITY)
self.world.setERP(0.8)
self.world.setCFM(1E-5)
#create Space
self.space = ode.Space()
# Create a plane geom which prevent the objects from falling forever
self.floor = ode.GeomPlane(self.space, (0, 1, 0), 0)
def reset(self):
# create an ODE world object
global world
world = ode.World()
world.setGravity((0.0, -9.81, 0.0))
world.setERP(0.1)
world.setCFM(1E-4)
# create an ODE space object
global space
space = ode.Space()
# create a plane geom to simulate a floor
floor = ode.GeomPlane(space, (0, 1, 0), 0)
# create a list to store any ODE bodies which are not part of the ragdoll (this
# is needed to avoid Python garbage collecting these bodies)
global bodies
bodies = []
# create a joint group for the contact joints generated during collisions
# between two bodies collide
global contactgroup
contactgroup = ode.JointGroup()
# set the initial simulation loop parameters
global Paused, lasttime, numiter, fps, dt, SloMo, stepsPerFrame
fps = 60
dt = 1.0 / fps
stepsPerFrame = 1
SloMo = 1
Paused = False
lasttime = time.time()
numiter = 0
# reward.
global reward
reward = 0
# create the pendulum
global pendulum
pendulum = pd.Pendulum(world, space, 500, (0.5, 0.0, 0.3))
print "total mass is %.1f kg (%.1f lbs)" % (pendulum.totalMass,
pendulum.totalMass * 2.2)
global TARGET_ANGLE
TARGET_ANGLE = pendulum.hHinge.getAngle()
# set GLUT callbacks
#glutKeyboardFunc(self.onKey)
#glutDisplayFunc(onDraw)
#glutIdleFunc(onIdle)
# enter the GLUT event loop
self.SpinOnce()
def __init__(self):
self.world = vpyode.World()
self.world.setGravity((0, -9.81, 0)) # Gravity of Earth
self.world.setERP(0.2)
self.world.setCFM(1E-5)
self.world.setContactSurfaceLayer(1E-3)
self.world.setContactMaxCorrectingVel(0.1)
# Create a plane geom which prevent the objects from falling forever
self.floor = ode.GeomPlane(vpyode._bigSpace, (0, 1, 0), 0)
vpyode._Mu = 5000
def floor():
P1 = gp_Pnt(0, 0, -100)
V1 = gp_Vec(0, 0, 1)
PL = gp_Pln(P1, gp_Dir(V1))
aPlane = GC_MakePlane(PL).Value()
aSurface = Geom_RectangularTrimmedSurface(aPlane, -100., 100., -60.,
60., 1, 1)
face = make_face(aSurface.GetHandle(), 1e-6)
floor = ode.GeomPlane(dyn_context._space, (0, 0, 1), -100)
references.append(floor)
display.DisplayColoredShape(face, 'RED')
display.FitAll()
def __init__(self):
self.world = ode.World()
self.world.setGravity( Vector3(0,-9.81,0) )
self.space = ode.Space()
self.boxes = {}
self.contactgroup = ode.JointGroup()
self.redis = redis.StrictRedis()
self.redis_pubsub = redis.StrictRedis()
self.channel = self.redis_pubsub.pubsub()
self.channel.subscribe('phys')
self.redis_fd = self.channel.connection._sock.fileno()
self.floor = ode.GeomPlane(self.space, (0,1,0), 0)
def createGeoms(self, obj, space):
"""Create all the geoms for one world object including the children.
The generated geoms will be encapsulated in GeomTransforms
so that all of them are defined with respect to the pivot coordinate
system of \a obj.
\param obj (\c WorldObject) Top level world object
\param space (\c ode.Space) ODE Space object
\return List of ODE geom objects
"""
# Plane? This is a special case because ODE planes are not placeable
if isinstance(obj.geom, PlaneGeom):
if not obj.static:
raise RuntimeError, "Planes can only be used as static bodies"
L = obj.localTransform()
n = L * vec3(0, 0, 1) - L * vec3(0)
n = n.normalize()
d = n * obj.pos
odegeom = ode.GeomPlane(space, n, d)
return [odegeom]
res = []
# Create all ODE geoms in the subtree starting at obj
# Each geom will have a position/rotation that moves it to
# the local coordinate system of obj.
geoms = self._createLGeoms(obj, mat4(1))
# Apply the offset transform and encapsulate the geoms inside
# geom transforms...
P = obj.getOffsetTransform()
Pinv = P.inverse()
pos, rot, scale = Pinv.decompose()
if scale != vec3(1, 1, 1):
print 'WARNING: ODEDynamics: Scaled geometries are not supported'
res = []
for g in geoms:
M = mat4(1).translation(vec3(g.getPosition()))
M.setMat3(mat3(g.getRotation()))
M *= Pinv
p4 = M.getColumn(3)
g.setPosition((p4.x, p4.y, p4.z))
# row major or column major?
g.setRotation(M.getMat3().toList(rowmajor=True))
gt = ode.GeomTransform(space)
gt.setGeom(g)
res.append(gt)
return res
def pack(self):
if self._running:
return
self._message = Message("Packing Objects...",
lifetime=-1,
dismissable=False,
progress=100)
self._message.show()
Application.getInstance().getController().setToolsEnabled(False)
plane = ode.GeomPlane(self._physics_plugin.space, (1, 0, 0),
self._initial_plane_distance)
self._planes.append(plane)
plane = ode.GeomPlane(self._physics_plugin.space, (-1, 0, 0),
self._initial_plane_distance)
self._planes.append(plane)
plane = ode.GeomPlane(self._physics_plugin.space, (0, 0, 1),
self._initial_plane_distance)
self._planes.append(plane)
plane = ode.GeomPlane(self._physics_plugin.space, (0, 0, -1),
self._initial_plane_distance)
self._planes.append(plane)
for node in DepthFirstIterator(Application.getInstance().getController(
).getScene().getRoot()):
if node.getDecorator(RigidBodyDecorator.RigidBodyDecorator):
random_position = Vector(
random.randrange(self._initial_plane_distance,
-self._initial_plane_distance), 0,
random.randrange(self._initial_plane_distance,
-self._initial_plane_distance))
node.setPosition(random_position)
self._timer.start()
def __init__(self, num_dice, *args, **kwargs):
super(DiceWidget, self).__init__(*args, **kwargs)
self.num_dice = num_dice
# Create a world object
self.world = ode.World()
self.world.setGravity((0, -9.81, 0))
self.world.setERP(0.8)
self.world.setCFM(1E-5)
# Create a space object
self.space = ode.Space()
self.floor = ode.GeomPlane(self.space, (0, 1, 0), 0)
# self.roof = ode.GeomPlane(self.space, (0, -1, 0), -self.height())
# self.wall_left = ode.GeomPlane(self.space, (-1, 0, 0), self.width())
# self.wall_left = ode.GeomPlane(self.space, (-1, 0, 0), 0)
self.wall_top = ode.GeomPlane(self.space, (1, 1, 0), -self.width() / 2)
self.wall_right = ode.GeomPlane(self.space, (-1, -1, 0),
-self.width() / 2)
self.contactgroup = ode.JointGroup()
def floor():
Z = -100
P1 = gp_Pnt(0, 0, Z)
V1 = gp_Vec(0, 0, 1)
PL = gp_Pln(P1, gp_Dir(V1))
aPlane = GC_MakePlane(PL).Value()
aSurface = Geom_RectangularTrimmedSurface(aPlane, -10., 10., -6., 6.,
1, 1)
face = BRepBuilderAPI_MakeFace(aSurface.GetHandle())
face.Build()
floor = ode.GeomPlane(dyn_context._space, (0, 0, 1), Z)
references.append(floor)
display.DisplayColoredShape(face.Shape(), 'RED')
display.FitAll()
def __init__(self, dt=1. / 60, max_angular_speed=20):
self.ode_world = ode.World()
self.ode_world.setMaxAngularSpeed(max_angular_speed)
self.ode_space = ode.QuadTreeSpace((0, 0, 0), (100, 100, 20), 10)
self.ode_floor = ode.GeomPlane(self.ode_space, (0, 0, 1), 0)
self.ode_contactgroup = ode.JointGroup()
self.frame_no = 0
self.dt = dt
self.elasticity = 0.1
self.friction = 2000
self.gravity = 0, 0, -9.81
self.cfm = 1e-6
self.erp = 0.7
self._bodies = {}
self._joints = {}
def __init__(self, n_enemybots=12):
self.n_robots = 12
self.n_enemybots = n_enemybots
self.world = ode.World()
self.world.setGravity((0, 0, -GRAVITY))
self.space = ode.HashSpace()
self.space.enable()
self.ground = ode.GeomPlane(self.space, (0, 0, 1), 0)
self.contactgroup = ode.JointGroup()
self.obj = Box(self.space, self.world, (LENGTH, WIDTH, HEIGHT), MASS)
self.wall = [None for _ in range(nWall)]
self.robot = [
SphereRobot(self.space, self.world, ROBOT_RADIUS, MASS)
for _ in range(self.n_robots)
]
self.joint = [None for _ in range(self.n_robots)]
self.enemy_bot = [
SphereRobot(self.space,
self.world,
ROBOT_RADIUS,
MASS,
color=[0, 0, 1]) for _ in range(self.n_enemybots)
]
self.seed()
self.objv = deque(maxlen=3)
[self.objv.append(np.zeros(2)) for _ in range(3)]
self.result_force = np.zeros(2)
self.objacc = np.zeros(2)
self._is_static = True
self.result_torque = 0
self.count = 0
self.drift_count = 0
self.sim_time = 0
self.fricdir = np.zeros(2)
self.stage = 0
self.sum_err_f_mag_pid = 0
self.time_f_mag_pid = 0
def createWorld(self, config):
self.world = ode.World()
self.space = ode.Space()
self.contactgroup = ode.JointGroup()
if config == None:
config = ypc.WorldConfig()
self.timeStep = config.timeStep
self.world.setGravity(config.gravity)
if config.planeHeight != None:
self.plane = ode.GeomPlane(self.space, (0, 1, 0),
config.planeHeight)
self.world.setContactSurfaceLayer(config.ContactSurfaceLayer)
self.world.setContactMaxCorrectingVel(config.ContactMaxCorrectingVel)
self.world.setERP(config.ERP)
self.world.setCFM(config.CFM)
def _parseGeomPlane(self, attrs):
def start(name, attrs):
if (name == 'ext'):
pass
else:
raise errors.ChildError('plane', name)
def end(name):
if (name == 'plane'):
self._parser.pop()
a = float(attrs['a'])
b = float(attrs['b'])
c = float(attrs['c'])
d = float(attrs['d'])
self.setODEObject(ode.GeomPlane(self._space, (a, b, c), d))
self._parser.push(startElement=start, endElement=end)
def __init__(self, world, xDelta, zDelta):
# Create a body inside the world
self.geom = ode.GeomPlane(world.space, (0, 1, 0), 0)
self.geom.name = 'Ground'
self.geom1 = ode.GeomPlane(world.space, (1.0, 0.0, 0.0), -xDelta)
self.geom1.name = 'Bound'
self.geom2 = ode.GeomPlane(world.space, (-1.0, 0.0, 0.0), -xDelta)
self.geom2.name = 'Bound'
self.geom3 = ode.GeomPlane(world.space, (0.0, 0.0, 1.0), -zDelta)
self.geom3.name = 'Bound'
self.geom4 = ode.GeomPlane(world.space, (0.0, 0.0, -1.0), -zDelta)
self.geom4.name = 'Bound'
self.geom5 = ode.GeomPlane(world.space, (0, -1, 0), -40)
self.geom5.name = 'Sky'
def run_simulation(self):
if self.trace:
faulthandler.enable()
sys.settrace(module.trace_func)
# dynamics world
world = ode.World()
world.setGravity((0, 0, -10)) # ODE uses Z as height by default
# collision space
space = ode.SimpleSpace() # SimpleSpace is slow-ish
# contact group
contactgroup = ode.JointGroup()
# mass
M = ode.Mass()
M.setSphere(2500, 1)
M.mass = 1.0
# dynamic body
body = ode.Body(world)
body.setMass(M)
body.setPosition((0, 0, 10))
body.setMovedCallback(self.body_callback)
# collision geom
geom = ode.GeomSphere(space, 1)
geom.setBody(body)
# plane
plane = ode.GeomPlane(space)
# run simulation
total_time = 0.0
dt = 0.04
while total_time < 4.5:
print(total_time)
space.collide((world, contactgroup),
self.collision_callback) # collision detection
world.quickStep(dt) # dynamics step
contactgroup.empty()
total_time += dt
def __init__(self):
super().__init__()
JobQueue.getInstance().jobFinished.connect(self._onJobFinished)
self._world = ode.World()
self._world.setGravity((0, toODE(-98100), 0))
self._world.setERP(0.2)
self._world.setCFM(1e-5)
#self._world.setContactMaxCorrectingVel(toODE(1000))
self._world.setContactSurfaceLayer(0.001)
self._world.setLinearDamping(1)
#self._world.setAngularDamping(0)
self._world.setAutoDisableLinearThreshold(0.001)
#self._world.setAutoDisableTime(0.1)
self._space = ode.Space(space_type=1)
self._contact_group = ode.JointGroup()
self._build_plate = ode.GeomPlane(self._space, (0, 1, 0), 0)
self._contact_bounce = 0.0
#self._contact_friction = float("inf")
self._contact_friction = 0
self._placement_strategy = OutsideInPlacementStrategy(self)
self._update_interval = 20
self._steps_per_update = 10
self._timer = QTimer()
self._timer.setInterval(self._update_interval)
self._timer.timeout.connect(self._onTimer)
self._timer.start()
self._object_packer = ObjectPacker(self)
self.addMenuItem("Pack Objects", self.packObjects)
def two_boxes_sphere_plane_collision(event=None):
display.EraseAll()
dyn_context = DynamicSimulationContext()
dyn_context.set_display(display, safe_yield)
dyn_context.enable_collision_detection()
dyn_context.enable_gravity()
# The first box
s1 = BRepPrimAPI_MakeBox(10, 20, 30).Shape()
d = dyn_context.register_shape(s1,
enable_collision_detection=True,
use_boundingbox=True)
d.setAngularVel([-1, -0.5, 0.3]) # the box is rotating
# The second box
box2 = BRepPrimAPI_MakeBox(10, 20, 30).Shape()
box2 = translate_topods_from_vector(box2, gp_Vec(5, 5, 100))
d2 = dyn_context.register_shape(box2,
enable_collision_detection=True,
use_boundingbox=True)
# The sphere
sphere = BRepPrimAPI_MakeSphere(10).Shape()
sphere = translate_topods_from_vector(sphere, gp_Vec(0, 0, 250))
d3 = dyn_context.register_shape(sphere,
enable_collision_detection=True,
use_sphere=True)
# Draw a plane (note: this plane is not a dynamic shape, it's just displayed)
face = make_plane(gp_Pnt(0, 0, -100), gp_Vec(0, 0, 1), -100., 100., -100.,
100.)
display.DisplayColoredShape(face, 'RED')
display.FitAll()
# Then create a geom for this plane
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(dyn_context._space, (0, 0, 1), -100)
#Starts the simulation
#raw_input('Hit a key when ready to start simulation')
dyn_context.set_simulation_duration(10.)
dyn_context.set_animation_frame_rate(60)
dyn_context.start_open_loop()
def box_plane_collision(event=None):
display.EraseAll()
dyn_context = DynamicSimulationContext()
dyn_context.set_display(display, safe_yield)
dyn_context.enable_collision_detection()
dyn_context.enable_gravity()
# The box
s1 = BRepPrimAPI_MakeBox(10, 20, 30).Shape()
d = dyn_context.register_shape(s1,
enable_collision_detection=True,
use_boundingbox=True)
d.setAngularVel([-1, -0.5, 0.3]) # the box is rotating
# The plane (note: this plane is not a dynamic shape, it's just displayed)
P1 = gp_Pnt(0, 0, -100)
V1 = gp_Vec(0, 0, 1)
face = make_plane(P1, V1, -100., 100., -60., 60)
# Then create a geom for this plane
# Create a plane geom which prevent the objects from falling forever
floor = ode.GeomPlane(dyn_context._space, (0, 0, 1), -100)
display.DisplayColoredShape(face, 'RED')
display.FitAll()
#Starts the simulation
dyn_context.set_animation_frame_rate(60)
dyn_context.start_open_loop()
def __init__(self,num):
# Create a world object
self.world = ode.World()
self.world.setGravity( (0,-self.g,0) )
self.world.setERP(0.8)
self.world.setCFM(1.e-5)
self.space = ode.Space()
self.contactgroup = ode.JointGroup()
# create objects
self.anchor = Sphere(self.world,self.space,-2,1.e30,0.001,False)
self.pendulum = Sphere(self.world,self.space,-1,self.d,self.r,True)
self.joint = ode.BallJoint(self.world)
self.joint.attach(self.anchor.body,self.pendulum.body)
self.sat = [Sphere(self.world,self.space,s,self.ds,self.rs,False) for s in range(num)]
self.walls = (
ode.GeomPlane(self.space, ( 1, 0, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0, 1, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0, 0, 1), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, (-1, 0, 0), -(self.l+self.r)*1.1),
ode.GeomPlane(self.space, ( 0,-1, 0), -self.l*0.1),
ode.GeomPlane(self.space, ( 0, 0,-1), -(self.l+self.r)*1.1)
)
# remember bodies
self.geoms = {}
self.geoms[self.anchor.geom] = -2
self.geoms[self.pendulum.geom] = -1
for s in range(num):
self.geoms[self.sat[s].geom] = s
for s in range(len(self.walls)):
self.geoms[self.walls[s]] = 1000+s
self.reset_1()
def adhesion(event=None):
'''
hinge joint example adapted from pyode
http://pyode.sourceforge.net/tutorials/tutorial2.html
'''
from OCC.Utils.Common import center_boundingbox
display.EraseAll()
# Create the dynamic context
dyn_context = DynamicSimulationContext()
dyn_context.set_display(display, safe_yield)
dyn_context.enable_collision_detection()
dyn_context.enable_gravity()
# create the table
block1 = BRepPrimAPI_MakeBox(4, 1, 1).Shape()
N = 12
dyn_shapes = []
for i in range(1, N):
table_shape = translate_topods_from_vector(block1,
gp_Vec(0, 0, 2 * (i * 0.5)),
copy=True)
dyn_shape = dyn_context.register_shape(table_shape,
enable_collision_detection=True,
use_boundingbox=True)
dyn_shapes.append((dyn_shape, table_shape))
_dyn_shapes = iter(dyn_shapes)
prev, table_shape = _dyn_shapes.next()
# safe for future reference
first, cube = prev, table_shape
prev_center = center_boundingbox(table_shape)
slider_vec = gp_Dir(1, 1, 0)
for i, xxx in enumerate(_dyn_shapes):
dyn_shape, table_shape = xxx
bj = DynamicSliderJoint(dyn_context)
bj.set_axis(slider_vec)
center = center_boundingbox(table_shape)
if i == 0:
bj.attach_shapes(prev, ode.environment)
first_joint = bj
else:
bj.attach_shapes(prev, dyn_shape)
dyn_context.register_joint(bj)
prev = dyn_shape
prev_center = center
# get the 1st block, the one one the ground...
xmin, ymin, zmin, xmax, ymax, zmax = get_boundingbox(cube)
# set the middle block in motion
vec1 = (160, 66, 0)
#vec1 = (1.20, 0.66, 0)
middle, topods = dyn_shapes[N / 2]
middle.setLinearVel(vec1)
#middle.setForce(vec1)
# change the color of the bottom element a little...
ais = middle.get_ais_shape().GetObject()
from OCC.Graphic3d import Graphic3d_NOM_SILVER
ais.SetMaterial(Graphic3d_NOM_SILVER)
bj.attach_shapes(dyn_shape, ode.environment)
first_joint.attach_shapes(first, ode.environment)
floor = ode.GeomPlane(dyn_context._space, (0, 0, 1), zmin)
dyn_context.mu = 0 # icey!!!
dyn_context.setERP(0.1) # 0.2 default
dyn_context.setCFM(1e-12) # 1e-10 default
dyn_context.set_simulation_duration(1200)
dyn_context.set_animation_frame_rate(40)
dyn_context.set_time_step(0.01)
dyn_context.start_open_loop(True)
