def loadConfig(self, filename, reload=False):
# parameters are given in (our own brand of) config-file syntax
self.config = ConfigGrabber(filename,
sectionId="<!--odeenvironment parameters",
delim=("<", ">"))
# <passpairs>
self.passpairs = []
for passpairstring in self.config.getValue("passpairs")[:]:
self.passpairs.append(eval(passpairstring))
if self.verbosity > 0:
print "-------[pass tuples]--------"
print self.passpairs
print "----------------------------"
# <centerOn>
# set focus of camera to the first object specified in the section, if any
if self.render:
try:
self.centerOn(self.config.getValue("centerOn")[0])
except IndexError:
pass
# <affixToEnvironment>
for jointName in self.config.getValue("affixToEnvironment")[:]:
try:
# find first object with that name
obj = self.root.namedChild(jointName).getODEObject()
except IndexError:
print "ERROR: Could not affix object '" + jointName + "' to environment!"
sys.exit(1)
if isinstance(obj, ode.Joint):
# if it is a joint, use this joint to fix to environment
obj.attach(obj.getBody(0), ode.environment)
elif isinstance(obj, ode.Body):
# if it is a body, create new joint and fix body to environment
j = ode.FixedJoint(self.world)
j.attach(obj, ode.environment)
j.setFixed()
# <colors>
for coldefstring in self.config.getValue("colors")[:]:
# ('name', (0.3,0.4,0.5))
objname, coldef = eval(coldefstring)
for (body, _) in self.body_geom:
if hasattr(body, 'name'):
if objname == body.name:
body.color = coldef
break
if not reload:
# add the JointSensor as default
self.sensors = []
## self.addSensor(self._jointSensor)
# <sensors>
# expects a list of strings, each of which is the executable command to create a sensor object
# example: DistToPointSensor('legSensor', (0.0, 0.0, 5.0))
sens = self.config.getValue("sensors")[:]
for s in sens:
try:
self.addSensor(eval('sensors.' + s))
except AttributeError:
print dir(sensors)
warnings.warn("Sensor name with name " + s +
" not found. skipped.")
else:
for s in self.sensors:
s._connect(self)
for a in self.actuators:
a._connect(self)
class ODEEnvironment(Environment):
"""
Virtual simulation for rigid bodies. Uses ODE as a physics engine and OpenGL as graphics
interface to simulate and display arbitrary objects. Virtual worlds are defined through
an XML file in XODE format (see http://tanksoftware.com/xode/). This file can be loaded
into the world with the "loadXODE(...)" function. Use performAction(...) or step() to advance
the simulation by one step.
"""
# objects with names within one tuple can pass each other
passpairs = []
# define a verbosity level for selective debug output (0=none)
verbosity = 0
def __init__(self,
render=True,
realtime=True,
ip="127.0.0.1",
port="21590",
buf='16384'):
""" initializes the virtual world, variables, the frame rate and the callback functions."""
print "ODEEnvironment -- based on Open Dynamics Engine."
# initialize base class
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.realtime = realtime
# initialize attributes
self.resetAttributes()
# initialize the textures dictionary
self.textures = {}
# initialize sensor and exclude list
self.sensors = []
self.excludesensors = []
#initialize actuators list
self.actuators = []
# A joint group for the contact joints that are generated whenever two bodies collide
self.contactgroup = ode.JointGroup()
self.dt = 0.01
self.FricMu = 8.0
self.stepsPerAction = 1
self.stepCounter = 0
def resetAttributes(self):
"""resets the class attributes to their default values"""
# initialize root node
self.root = None
# A list with (body, geom) tuples
self.body_geom = []
def reset(self):
"""resets the model and all its parameters to their original values"""
self.loadXODE(self._currentXODEfile, reload=True)
self.stepCounter = 0
def setGravity(self, g):
"""set the world's gravity constant in negative y-direction"""
self.world.setGravity((0, -g, 0))
def _setWorldParameters(self):
""" sets parameters for ODE world object: gravity, error correction (ERP, default=0.2),
constraint force mixing (CFM, default=1e-5). """
self.world.setGravity((0, -9.81, 0))
# self.world.setERP(0.2)
# self.world.setCFM(1e-9)
def _create_box(self, space, density, lx, ly, lz):
"""Create a box body and its corresponding geom."""
# Create body and mass
body = ode.Body(self.world)
M = ode.Mass()
M.setBox(density, lx, ly, lz)
body.setMass(M)
body.name = None
# Create a box geom for collision detection
geom = ode.GeomBox(space, lengths=(lx, ly, lz))
geom.setBody(body)
geom.name = None
return (body, geom)
def _create_sphere(self, space, density, radius):
"""Create a sphere body and its corresponding geom."""
# Create body and mass
body = ode.Body(self.world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
body.name = None
# Create a sphere geom for collision detection
geom = ode.GeomSphere(space, radius)
geom.setBody(body)
geom.name = None
return (body, geom)
def drop_object(self):
"""Drops a random object (box, sphere) into the scene."""
# choose between boxes and spheres
if random.uniform() > 0.5:
(body, geom) = self._create_sphere(self.space, 10, 0.4)
else:
(body, geom) = self._create_box(self.space, 10, 0.5, 0.5, 0.5)
# randomize position slightly
body.setPosition((random.normal(-6.5,
0.5), 6.0, random.normal(-6.5, 0.5)))
# body.setPosition( (0.0, 3.0, 0.0) )
# randomize orientation slightly
#theta = random.uniform(0,2*pi)
#ct = cos (theta)
#st = sin (theta)
# rotate body and append to (body,geom) tuple list
# body.setRotation([ct, 0., -st, 0., 1., 0., st, 0., ct])
self.body_geom.append((body, geom))
# -- sensor and actuator functions
def addSensor(self, sensor):
""" adds a sensor object to the list of sensors """
if not isinstance(sensor, sensors.Sensor):
raise TypeError(
"the given sensor is not an instance of class 'Sensor'.")
# add sensor to sensors list
self.sensors.append(sensor)
# connect sensor and give it the virtual world object
sensor._connect(self)
def addActuator(self, actuator):
""" adds a sensor object to the list of sensors """
if not isinstance(actuator, actuators.Actuator):
raise TypeError(
"the given actuator is not an instance of class 'Actuator'.")
# add sensor to sensors list
self.actuators.append(actuator)
# connect actuator and give it the virtual world object
actuator._connect(self)
def addTexture(self, name, texture):
""" adds a texture to the given ODE object name """
self.textures[name] = texture
def centerOn(self, name):
return
""" if set, keeps camera to the given ODE object name. """
try:
self.getRenderer().setCenterObj(
self.root.namedChild(name).getODEObject())
except KeyError:
# name not found, unset centerObj
print "Warning: Cannot center on " + name
self.centerObj = None
def loadXODE(self, filename, reload=False):
""" loads an XODE file (xml format) and parses it. """
f = file(filename)
self._currentXODEfile = filename
p = xode.parser.Parser()
self.root = p.parseFile(f)
f.close()
try:
# filter all xode "world" objects from root, take only the first one
world = filter(lambda x: isinstance(x, xode.parser.World),
self.root.getChildren())[0]
except IndexError:
# malicious format, no world tag found
print "no <world> tag found in " + filename + ". quitting."
sys.exit()
self.world = world.getODEObject()
self._setWorldParameters()
try:
# filter all xode "space" objects from world, take only the first one
space = filter(lambda x: isinstance(x, xode.parser.Space),
world.getChildren())[0]
except IndexError:
# malicious format, no space tag found
print "no <space> tag found in " + filename + ". quitting."
sys.exit()
self.space = space.getODEObject()
# load bodies and geoms for painting
self.body_geom = []
self._parseBodies(self.root)
if self.verbosity > 0:
print "-------[body/mass list]-----"
for (body, _) in self.body_geom:
try:
print body.name, body.getMass()
except AttributeError:
print "<Nobody>"
# now parse the additional parameters at the end of the xode file
self.loadConfig(filename, reload)
def loadConfig(self, filename, reload=False):
# parameters are given in (our own brand of) config-file syntax
self.config = ConfigGrabber(filename,
sectionId="<!--odeenvironment parameters",
delim=("<", ">"))
# <passpairs>
self.passpairs = []
for passpairstring in self.config.getValue("passpairs")[:]:
self.passpairs.append(eval(passpairstring))
if self.verbosity > 0:
print "-------[pass tuples]--------"
print self.passpairs
print "----------------------------"
# <centerOn>
# set focus of camera to the first object specified in the section, if any
if self.render:
try:
self.centerOn(self.config.getValue("centerOn")[0])
except IndexError:
pass
# <affixToEnvironment>
for jointName in self.config.getValue("affixToEnvironment")[:]:
try:
# find first object with that name
obj = self.root.namedChild(jointName).getODEObject()
except IndexError:
print "ERROR: Could not affix object '" + jointName + "' to environment!"
sys.exit(1)
if isinstance(obj, ode.Joint):
# if it is a joint, use this joint to fix to environment
obj.attach(obj.getBody(0), ode.environment)
elif isinstance(obj, ode.Body):
# if it is a body, create new joint and fix body to environment
j = ode.FixedJoint(self.world)
j.attach(obj, ode.environment)
j.setFixed()
# <colors>
for coldefstring in self.config.getValue("colors")[:]:
# ('name', (0.3,0.4,0.5))
objname, coldef = eval(coldefstring)
for (body, _) in self.body_geom:
if hasattr(body, 'name'):
if objname == body.name:
body.color = coldef
break
if not reload:
# add the JointSensor as default
self.sensors = []
## self.addSensor(self._jointSensor)
# <sensors>
# expects a list of strings, each of which is the executable command to create a sensor object
# example: DistToPointSensor('legSensor', (0.0, 0.0, 5.0))
sens = self.config.getValue("sensors")[:]
for s in sens:
try:
self.addSensor(eval('sensors.' + s))
except AttributeError:
print dir(sensors)
warnings.warn("Sensor name with name " + s +
" not found. skipped.")
else:
for s in self.sensors:
s._connect(self)
for a in self.actuators:
a._connect(self)
def _parseBodies(self, node):
""" parses through the xode tree recursively and finds all bodies and geoms for drawing. """
# body (with nested geom)
if isinstance(node, xode.body.Body):
body = node.getODEObject()
body.name = node.getName()
try:
# filter all xode geom objects and take the first one
xgeom = filter(lambda x: isinstance(x, xode.geom.Geom),
node.getChildren())[0]
except IndexError:
return () # no geom object found, skip this node
# get the real ode object
geom = xgeom.getODEObject()
# if geom doesn't have own name, use the name of its body
geom.name = node.getName()
self.body_geom.append((body, geom))
# geom on its own without body
elif isinstance(node, xode.geom.Geom):
try:
node.getFirstAncestor(ode.Body)
except xode.node.AncestorNotFoundError:
body = None
geom = node.getODEObject()
geom.name = node.getName()
self.body_geom.append((body, geom))
# special cases for joints: universal, fixed, amotor
elif isinstance(node, xode.joint.Joint):
joint = node.getODEObject()
if type(joint) == ode.UniversalJoint:
# insert an additional AMotor joint to read the angles from and to add torques
# amotor = ode.AMotor(self.world)
# amotor.attach(joint.getBody(0), joint.getBody(1))
# amotor.setNumAxes(3)
# amotor.setAxis(0, 0, joint.getAxis2())
# amotor.setAxis(2, 0, joint.getAxis1())
# amotor.setMode(ode.AMotorEuler)
# xode_amotor = xode.joint.Joint(node.getName() + '[amotor]', node.getParent())
# xode_amotor.setODEObject(amotor)
# node.getParent().addChild(xode_amotor, None)
pass
if type(joint) == ode.AMotor:
# do the euler angle calculations automatically (ref. ode manual)
joint.setMode(ode.AMotorEuler)
if type(joint) == ode.FixedJoint:
# prevent fixed joints from bouncing to center of first body
joint.setFixed()
# recursive call for all child nodes
for c in node.getChildren():
self._parseBodies(c)
def excludeSensors(self, exclist):
self.excludesensors.extend(exclist)
def getSensors(self):
""" returns combined sensor data """
output = []
for s in self.sensors:
if not s.name in self.excludesensors:
output.extend(s.getValues())
return asarray(output)
def getSensorNames(self):
return [s.name for s in self.sensors]
def getActuatorNames(self):
return [a.name for a in self.actuators]
def getSensorByName(self, name):
try:
idx = self.getSensorNames().index(name)
except ValueError:
warnings.warn('sensor ' + name + ' is not in sensor list.')
return []
return self.sensors[idx].getValues()
@property
def indim(self):
num = 0
for a in self.actuators:
num += a.getNumValues()
return num
def getActionLength(self):
print "getActionLength() is deprecated. use property 'indim' instead."
return self.indim
@property
def outdim(self):
return len(self.getSensors())
def performAction(self, action):
""" sets the values for all actuators combined. """
pointer = 0
for a in self.actuators:
val = a.getNumValues()
a._update(action[pointer:pointer + val])
pointer += val
for _ in range(self.stepsPerAction):
self.step()
def getXODERoot(self):
return self.root
#--- callback functions ---#
def _near_callback(self, args, geom1, geom2):
"""Callback function for the collide() method.
This function checks if the given geoms do collide and
creates contact joints if they do."""
# only check parse list, if objects have name
if geom1.name != None and geom2.name != None:
# Preliminary checking, only collide with certain objects
for p in self.passpairs:
g1 = False
g2 = False
for x in p:
g1 = g1 or (geom1.name.find(x) != -1)
g2 = g2 or (geom2.name.find(x) != -1)
if g1 and g2:
return ()
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world, contactgroup = args
for c in contacts:
p = c.getContactGeomParams()
# parameters from Niko Wolf
c.setBounce(0.2)
c.setBounceVel(
0.05) #Set the minimum incoming velocity necessary for bounce
c.setSoftERP(0.6) #Set the contact normal "softness" parameter
c.setSoftCFM(0.00005) #Set the contact normal "softness" parameter
c.setSlip1(
0.02
) #Set the coefficient of force-dependent-slip (FDS) for friction direction 1
c.setSlip2(
0.02
) #Set the coefficient of force-dependent-slip (FDS) for friction direction 2
c.setMu(self.FricMu) #Set the Coulomb friction coefficient
j = ode.ContactJoint(world, contactgroup, c)
j.name = None
j.attach(geom1.getBody(), geom2.getBody())
def getCurrentStep(self):
return self.stepCounter
@threaded()
def updateClients(self):
self.updateDone = False
if not self.updateLock.acquire(False):
return
# build message to send
message = []
for (body, geom) in self.body_geom:
item = {}
# real bodies (boxes, spheres, ...)
if body != None:
# transform (rotate, translate) body accordingly
item['position'] = body.getPosition()
item['rotation'] = body.getRotation()
if hasattr(body, 'color'): item['color'] = body.color
# switch different geom objects
if type(geom) == ode.GeomBox:
# cube
item['type'] = 'GeomBox'
item['scale'] = geom.getLengths()
elif type(geom) == ode.GeomSphere:
# sphere
item['type'] = 'GeomSphere'
item['radius'] = geom.getRadius()
elif type(geom) == ode.GeomCCylinder:
# capped cylinder
item['type'] = 'GeomCCylinder'
item['radius'] = geom.getParams()[0]
item['length'] = geom.getParams()[1] - 2 * item['radius']
elif type(geom) == ode.GeomCylinder:
# solid cylinder
item['type'] = 'GeomCylinder'
item['radius'] = geom.getParams()[0]
item['length'] = geom.getParams()[1]
else:
# TODO: add other geoms here
pass
else:
# no body found, then it must be a plane (we only draw planes)
if type(geom) == ode.GeomPlane:
# plane
item['type'] = 'GeomPlane'
item['normal'] = geom.getParams()[
0] # the normal vector to the plane
item['distance'] = geom.getParams()[
1] # the distance to the origin
message.append(item)
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send(message)
time.sleep(0.02)
self.updateLock.release()
self.updateDone = True
def step(self):
""" Here the ode physics is calculated by one step. """
# call additional callback functions for all kinds of tasks (e.g. printing)
self._printfunc()
# Detect collisions and create contact joints
self.space.collide((self.world, self.contactgroup),
self._near_callback)
# Simulation step
self.world.step(float(self.dt))
# Remove all contact joints
self.contactgroup.empty()
# update all sensors
for s in self.sensors:
s._update()
# update clients
if self.render and self.updateDone:
self.updateClients()
if self.server.clients > 0 and self.realtime:
time.sleep(self.dt)
# increase step counter
self.stepCounter += 1
return self.stepCounter
def _printfunc(self):
pass
# print self.root.namedChild('palm').getODEObject().getPosition()
def specialkeyfunc(self, c, x, y):
"""Derived classes can implement extra functionality here"""
pass
#--- helper functions ---#
def _print_help(self):
""" prints out the keyboard shortcuts. """
print "v -> toggle view with mouse on/off"
print "s -> toggle screen capture on/off"
print "d -> drop an object"
print "f -> lift all objects"
print "m -> toggle mouse view (press button to zoom)"
print "r -> random torque at all joints"
print "a/z -> negative/positive torque to all joints"
print "g -> print current state"
print "n -> reset environment"
self.specialfunctionDoc()
print "x,q -> exit program"
def specialfunctionDoc(self):
"""Derived classes can implement extra functionality here"""
pass
class ODEEnvironment(Environment):
"""ODEEnvironment class
Virtual simulation for rigid bodies. Uses ODE as a physics engine to
simulate objects acting in a physical space. Virtual worlds are defined
through an XML file in XODE format (see http://tanksoftware.com/xode/).
This file can be loaded into the world with the 'loadXODE()' method.
Use performAction(...) or step() to advance the simulation by one step.
Methods:
"""
def __init__(
self, render=True, realtime=True, ip="127.0.0.1", port="21590",
buf='16384', verbose=False, gravity=-9.81):
"""Initialize
Initializes the ODE world, variables, the frame rate and the
callback functions.
Arguments:
render: Determines if object information should be sent over UDP
to the viewer client. (Boolean - Default: True)
realtime: Determines if the simulation should have real-time
constraints. (Boolean - Default: True)
ip: The IP address of the viewer client. This is only used if
render is set to True. (String - Default: '127.0.0.1')
port: The port of the viewer client. This is only used if
render is set to True. (String - Default: '21590')
buf: The size of the UDP buffer. This is only used if render is
set to True. (String - Default: '16384')
verbose: Determines if extra debug information should be
displayed. (Boolean - Default: False)
gravity: The gravity in meters per second to be excerted on objects
in the world. (Float - Default: -9.81)
"""
# Initialize base class
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.realtime = realtime
# Initialize attributes
self.resetAttributes()
# Determine if extra debug information will be displayed
self.verbose = verbose
# initialize the textures dictionary
self.textures = {}
# objects with names within one tuple can pass each other
self.passpairs = []
# initialize sensor and exclude list
self.sensors = []
self.excludesensors = []
#initialize actuators list
self.actuators = []
# A joint group for the contact joints that are generated whenever two bodies collide
self.contactgroup = ode.JointGroup()
self.dt = 0.01
self.FricMu = 8.0
self.stepsPerAction = 1
self.stepCounter = 0
# Determine world gravity based on the initialization argument
self.g = gravity
# Maintain a list of bodies that should not be sent to the OpenGL viewer
self.invisible_bodies = []
# Store all geometries/bodies sent to the OpenGL renderer to determine
# if they have been modified or not
self.prev_message = {}
if self.verbose:
print "ODEEnvironment -- based on Open Dynamics Engine."
return
def closeSocket(self):
self.server.UDPInSock.close()
time.sleep(10)
return
def resetAttributes(self):
"""resets the class attributes to their default values"""
# initialize root node
self.root = None
# A list with (body, geom) tuples
self.body_geom = []
return
def reset(self):
"""resets the model and all its parameters to their original values"""
self.loadXODE(self._currentXODEfile, reload=True)
self.stepCounter = 0
return
def stop(self):
"""Stop Simulation
Stops the simulation, destroys the world, and releases all ODE
specific resources.
"""
del self.world
ode.CloseODE()
return
def setGravity(self, g):
"""set the world's gravity constant in negative y-direction"""
self.world.setGravity((0, g, 0))
self.g = g
return
def _setWorldParameters(self):
""" sets parameters for ODE world object: gravity, error correction (ERP, default=0.2),
constraint force mixing (CFM, default=1e-5). """
self.world.setGravity((0, self.g, 0))
self.world.setContactSurfaceLayer(0.0001)
self.world.setContactMaxCorrectingVel(0.1)
#self.world.setERP(0.2)
#self.world.setCFM(1e-9)
return
def _create_box(self, space, density, lx, ly, lz):
"""Create a box body and its corresponding geom."""
# Create body and mass
body = ode.Body(self.world)
M = ode.Mass()
M.setBox(density, lx, ly, lz)
body.setMass(M)
body.name = None
# Create a box geom for collision detection
geom = ode.GeomBox(space, lengths=(lx, ly, lz))
geom.setBody(body)
geom.name = None
return (body, geom)
def _create_sphere(self, space, density, radius):
"""Create a sphere body and its corresponding geom."""
# Create body and mass
body = ode.Body(self.world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
body.name = None
# Create a sphere geom for collision detection
geom = ode.GeomSphere(space, radius)
geom.setBody(body)
geom.name = None
return (body, geom)
def drop_object(self):
"""Drops a random object (box, sphere) into the scene."""
# choose between boxes and spheres
if random.uniform() > 0.5:
(body, geom) = self._create_sphere(self.space, 10, 0.4)
else:
(body, geom) = self._create_box(self.space, 10, 0.5, 0.5, 0.5)
# randomize position slightly
body.setPosition((random.normal(-6.5, 0.5), 6.0, random.normal(-6.5, 0.5)))
# body.setPosition( (0.0, 3.0, 0.0) )
# randomize orientation slightly
#theta = random.uniform(0,2*pi)
#ct = cos (theta)
#st = sin (theta)
# rotate body and append to (body,geom) tuple list
# body.setRotation([ct, 0., -st, 0., 1., 0., st, 0., ct])
self.body_geom.append((body, geom))
return
def addSensor(self, sensor):
""" adds a sensor object to the list of sensors """
if not isinstance(sensor, sensors.Sensor):
raise TypeError("the given sensor is not an instance of class 'Sensor'.")
# add sensor to sensors list
self.sensors.append(sensor)
# connect sensor and give it the virtual world object
sensor._connect(self)
return
def addActuator(self, actuator):
""" adds a sensor object to the list of sensors """
if not isinstance(actuator, actuators.Actuator):
raise TypeError("the given actuator is not an instance of class 'Actuator'.")
# add sensor to sensors list
self.actuators.append(actuator)
# connect actuator and give it the virtual world object
actuator._connect(self)
return
def addTexture(self, name, texture):
""" adds a texture to the given ODE object name """
self.textures[name] = texture
return
def centerOn(self, name):
""" if set, keeps camera to the given ODE object name. """
try:
self.getRenderer().setCenterObj(self.root.namedChild(name).getODEObject())
except KeyError:
# name not found, unset centerObj
print "Warning: Cannot center on " + name
self.centerObj = None
return
def loadXODE(self, filename, reload=False):
""" loads an XODE file (xml format) and parses it. """
f = file(filename)
self._currentXODEfile = filename
p = xode.parser.Parser()
self.root = p.parseFile(f)
f.close()
try:
# filter all xode "world" objects from root, take only the first one
world = filter(lambda x: isinstance(x, xode.parser.World), self.root.getChildren())[0]
except IndexError:
# malicious format, no world tag found
print "no <world> tag found in " + filename + ". quitting."
sys.exit(1)
self.world = world.getODEObject()
self._setWorldParameters()
try:
# filter all xode "space" objects from world, take only the first one
space = filter(lambda x: isinstance(x, xode.parser.Space), world.getChildren())[0]
except IndexError:
# malicious format, no space tag found
print "no <space> tag found in " + filename + ". quitting."
sys.exit(1)
self.space = space.getODEObject()
# load bodies and geoms for painting
self.body_geom = []
self._parseBodies(self.root)
if self.verbose:
print "-------[body/mass list]-----"
for (body, _) in self.body_geom:
try:
print body.name, body.getMass()
except AttributeError:
print "<Nobody>"
# now parse the additional parameters at the end of the xode file
self.loadConfig(filename, reload)
return
def loadConfig(self, filename, reload=False):
# parameters are given in (our own brand of) config-file syntax
self.config = ConfigGrabber(filename, sectionId="<!--odeenvironment parameters", delim=("<", ">"))
# <passpairs>
self.passpairs = []
for passpairstring in self.config.getValue("passpairs")[:]:
self.passpairs.append(eval(passpairstring))
if self.verbose:
print "-------[pass tuples]--------"
print self.passpairs
print "----------------------------"
# <noGravity>
for obj_name in self.config.getValue('noGravity'):
try:
obj = self.root.namedChild(obj_name).getODEObject()
except IndexError:
print 'ERROR: Could not find object \'%s\'' % (obj_name)
sys.exit(1)
if isinstance(obj, ode.Body):
obj.setGravityMode(False)
else:
print ('ERROR: noGravity configuration is only valid for' +
'ODE Body class objects')
# <invisible>
for obj_name in self.config.getValue('invisible'):
try:
obj = self.root.namedChild(obj_name).getODEObject()
except IndexError:
print 'ERROR: Could not find object \'%s\'' % (obj_name)
sys.exit(1)
if isinstance(obj, ode.Body):
self.invisible_bodies.append(obj_name)
else:
print ('ERROR: invisible configuration is only valid for' +
'ODE Body class objects')
# <centerOn>
# set focus of camera to the first object specified in the section, if any
if self.render:
try:
self.centerOn(self.config.getValue("centerOn")[0])
except IndexError:
pass
# <affixToEnvironment>
for jointName in self.config.getValue("affixToEnvironment")[:]:
try:
# find first object with that name
obj = self.root.namedChild(jointName).getODEObject()
except IndexError:
print "ERROR: Could not affix object '" + jointName + "' to environment!"
sys.exit(1)
if isinstance(obj, ode.Joint):
# if it is a joint, use this joint to fix to environment
obj.attach(obj.getBody(0), ode.environment)
elif isinstance(obj, ode.Body):
# if it is a body, create new joint and fix body to environment
j = ode.FixedJoint(self.world)
j.attach(obj, ode.environment)
j.setFixed()
# <colors>
for coldefstring in self.config.getValue("colors")[:]:
# ('name', (0.3,0.4,0.5))
objname, coldef = eval(coldefstring)
for (body, _) in self.body_geom:
if hasattr(body, 'name'):
if objname == body.name:
body.color = coldef
break
if not reload:
# add the JointSensor as default
self.sensors = []
## self.addSensor(self._jointSensor)
# <sensors>
# expects a list of strings, each of which is the executable command to create a sensor object
# example: DistToPointSensor('legSensor', (0.0, 0.0, 5.0))
sens = self.config.getValue("sensors")[:]
for s in sens:
try:
self.addSensor(eval('sensors.' + s))
except AttributeError:
print dir(sensors)
warnings.warn("Sensor name with name " + s + " not found. skipped.")
else:
for s in self.sensors:
s._connect(self)
for a in self.actuators:
a._connect(self)
return
def _parseBodies(self, node):
""" parses through the xode tree recursively and finds all bodies and geoms for drawing. """
# body (with nested geom)
if isinstance(node, xode.body.Body):
body = node.getODEObject()
body.name = node.getName()
try:
# filter all xode geom objects and take the first one
xgeom = filter(lambda x: isinstance(x, xode.geom.Geom), node.getChildren())[0]
except IndexError:
return() # no geom object found, skip this node
# get the real ode object
geom = xgeom.getODEObject()
# if geom doesn't have own name, use the name of its body
geom.name = node.getName()
self.body_geom.append((body, geom))
# geom on its own without body
elif isinstance(node, xode.geom.Geom):
try:
node.getFirstAncestor(ode.Body)
except xode.node.AncestorNotFoundError:
body = None
geom = node.getODEObject()
geom.name = node.getName()
self.body_geom.append((body, geom))
# special cases for joints: universal, fixed, amotor
elif isinstance(node, xode.joint.Joint):
joint = node.getODEObject()
if type(joint) == ode.UniversalJoint:
# insert an additional AMotor joint to read the angles from and to add torques
# amotor = ode.AMotor(self.world)
# amotor.attach(joint.getBody(0), joint.getBody(1))
# amotor.setNumAxes(3)
# amotor.setAxis(0, 0, joint.getAxis2())
# amotor.setAxis(2, 0, joint.getAxis1())
# amotor.setMode(ode.AMotorEuler)
# xode_amotor = xode.joint.Joint(node.getName() + '[amotor]', node.getParent())
# xode_amotor.setODEObject(amotor)
# node.getParent().addChild(xode_amotor, None)
pass
if type(joint) == ode.AMotor:
# do the euler angle calculations automatically (ref. ode manual)
joint.setMode(ode.AMotorEuler)
if type(joint) == ode.FixedJoint:
# prevent fixed joints from bouncing to center of first body
joint.setFixed()
# recursive call for all child nodes
for c in node.getChildren():
self._parseBodies(c)
return
def excludeSensors(self, exclist):
self.excludesensors.extend(exclist)
return
def getSensors(self):
""" returns combined sensor data """
output = []
for s in self.sensors:
if not s.name in self.excludesensors:
output.extend(s.getValues())
return asarray(output)
def getSensorNames(self):
return [s.name for s in self.sensors]
def getActuatorNames(self):
return [a.name for a in self.actuators]
def getSensorByName(self, name):
try:
idx = self.getSensorNames().index(name)
except ValueError:
warnings.warn('sensor ' + name + ' is not in sensor list.')
return []
return self.sensors[idx].getValues()
@property
def indim(self):
num = 0
for a in self.actuators:
num += a.getNumValues()
return num
def getActionLength(self):
print "getActionLength() is deprecated. use property 'indim' instead."
return self.indim
@property
def outdim(self):
return len(self.getSensors())
def performAction(self, action, fast=False):
"""Perform Action
Applies the given 'action' list of actuations to each actuator defined
in the system for the next time step. The next time step is then
calculated.
Arguments:
action: A list of actuations for each actuator defined.
fast: If True, the fast step Open Dynamics Engine algorithm will
be used. This method is quicker and requires less memory but
is less accurate. Otherwise the standard step is used.
(Default: False)
"""
pointer = 0
for a in self.actuators:
val = a.getNumValues()
a._update(action[pointer:pointer + val])
pointer += val
for _ in range(self.stepsPerAction):
self.step(fast=fast)
return
def getXODERoot(self):
return self.root
#--- callback functions ---#
def _near_callback(self, args, geom1, geom2):
"""Near Callback
Callback function for the collide() method.
This function checks if the given geoms do collide and
creates contact joints if they do.
"""
# only check parse list, if objects have name
if geom1.name != None and geom2.name != None:
# Preliminary checking, only collide with certain objects
for p in self.passpairs:
g1 = False
g2 = False
for x in p:
g1 = g1 or (geom1.name.find(x) != -1)
g2 = g2 or (geom2.name.find(x) != -1)
if g1 and g2:
return()
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Unpack function arguments
world, contactgroup = args
# Create contact joints
for c in contacts:
p = c.getContactGeomParams()
# parameters from Niko Wolf
c.setBounce(0.2)
c.setBounceVel(0.05) #Set the minimum incoming velocity necessary for bounce
c.setSoftERP(0.6) #Set the contact normal "softness" parameter
c.setSoftCFM(0.00005) #Set the contact normal "softness" parameter
c.setSlip1(0.02) #Set the coefficient of force-dependent-slip (FDS) for friction direction 1
c.setSlip2(0.02) #Set the coefficient of force-dependent-slip (FDS) for friction direction 2
c.setMu(self.FricMu) #Set the Coulomb friction coefficient
j = ode.ContactJoint(world, contactgroup, c)
j.name = None
j.attach(geom1.getBody(), geom2.getBody())
return
def getCurrentStep(self):
return self.stepCounter
@threaded()
def updateClients(self):
self.updateDone = False
if not self.updateLock.acquire(False):
return
# build message to send
message = {}
for body, geom in self.body_geom:
item = {}
item_name = ''
if body != None:
item_name = body.name
if item_name in self.invisible_bodies:
continue
item_pos = body.getPosition()
item_rot = body.getRotation()
# Try to find this body from the previous message. See if
# the item exists and has changed. If it is modified, then
# the item must be sent. If not, it is implied that the body
# has not changed.
if item_name in self.prev_message:
# The item has been sent before
prev_item = self.prev_message[item_name]
# See if the item has been modified
if (prev_item['position'] == item_pos and
prev_item['rotation'] == item_rot):
# The item has not changed. Don't add it to the message
continue
# The item wasn't in the previous message or has been altered.
# Send the new info to the viewer
# transform (rotate, translate) body accordingly
item['position'] = item_pos
item['rotation'] = item_rot
if hasattr(body, 'color'):
item['color'] = body.color
# switch different geom objects
if type(geom) == ode.GeomBox:
# cube
item['type'] = 'box'
item['scale'] = geom.getLengths()
elif type(geom) == ode.GeomSphere:
# sphere
item['type'] = 'sphere'
item['radius'] = geom.getRadius()
elif type(geom) == ode.GeomCCylinder:
# capped cylinder
item['type'] = 'ccylinder'
item['radius'] = geom.getParams()[0]
item['length'] = geom.getParams()[1] - 2 * item['radius']
elif type(geom) == ode.GeomCylinder:
# solid cylinder
item['type'] = 'cylinder'
item['radius'] = geom.getParams()[0]
item['length'] = geom.getParams()[1]
else:
item_name = geom.name
# See if the item exists in the previous message. If so, the
# viewer is already aware of it.
if item_name in self.prev_message:
continue
# no body found, then it must be a plane (we only draw planes)
if type(geom) == ode.GeomPlane:
item['type'] = 'plane'
item['normal'] = geom.getParams()[0] # the normal vector to the plane
item['distance'] = geom.getParams()[1] # the distance to the origin
message[item_name] = item
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send(message)
# Store the message items in their current positions and rotations
self.prev_message = message
self.updateLock.release()
self.updateDone = True
return
def step(self, fast=False):
"""Step
Determine all collisions and step through the environment physics
using the Open Dynamic Engine.
Arguments:
fast: Determines if a quick ODE step should be used instead of the
standard ODE step function. A fast step is much faster but
less accurate. (Default: False)
Return:
Integer representing the number of total steps taken from the
start of the simulation.
"""
# call additional callback functions for all kinds of tasks (e.g. printing)
self._printfunc()
# Detect collisions and create contact joints
self.space.collide((self.world, self.contactgroup), self._near_callback)
# Simulation step
if fast:
# If we are using a quick step, accuracy will decrease but speed
# and memory use decreases drastically
self.world.quickStep(float(self.dt))
else:
# Step normally
self.world.step(float(self.dt))
# Remove all contact joints
self.contactgroup.empty()
# update all sensors
for s in self.sensors:
s._update()
# update clients
if self.render and self.updateDone:
self.updateClients()
if self.server.clients > 0 and self.realtime:
time.sleep(self.dt)
# increase step counter
self.stepCounter += 1
return self.stepCounter
def _printfunc (self):
pass
def specialkeyfunc(self, c, x, y):
"""Derived classes can implement extra functionality here"""
pass
#--- helper functions ---#
def _print_help(self):
""" prints out the keyboard shortcuts. """
print "v -> toggle view with mouse on/off"
print "s -> toggle screen capture on/off"
print "d -> drop an object"
print "f -> lift all objects"
print "m -> toggle mouse view (press button to zoom)"
print "r -> random torque at all joints"
print "a/z -> negative/positive torque to all joints"
print "g -> print current state"
print "n -> reset environment"
self.specialfunctionDoc()
print "x,q -> exit program"
def specialfunctionDoc(self):
"""Derived classes can implement extra functionality here"""
pass
def loadConfig(self, filename, reload=False):
# parameters are given in (our own brand of) config-file syntax
self.config = ConfigGrabber(filename, sectionId="<!--odeenvironment parameters", delim=("<", ">"))
# <passpairs>
self.passpairs = []
for passpairstring in self.config.getValue("passpairs")[:]:
self.passpairs.append(eval(passpairstring))
if self.verbose:
print "-------[pass tuples]--------"
print self.passpairs
print "----------------------------"
# <noGravity>
for obj_name in self.config.getValue('noGravity'):
try:
obj = self.root.namedChild(obj_name).getODEObject()
except IndexError:
print 'ERROR: Could not find object \'%s\'' % (obj_name)
sys.exit(1)
if isinstance(obj, ode.Body):
obj.setGravityMode(False)
else:
print ('ERROR: noGravity configuration is only valid for' +
'ODE Body class objects')
# <invisible>
for obj_name in self.config.getValue('invisible'):
try:
obj = self.root.namedChild(obj_name).getODEObject()
except IndexError:
print 'ERROR: Could not find object \'%s\'' % (obj_name)
sys.exit(1)
if isinstance(obj, ode.Body):
self.invisible_bodies.append(obj_name)
else:
print ('ERROR: invisible configuration is only valid for' +
'ODE Body class objects')
# <centerOn>
# set focus of camera to the first object specified in the section, if any
if self.render:
try:
self.centerOn(self.config.getValue("centerOn")[0])
except IndexError:
pass
# <affixToEnvironment>
for jointName in self.config.getValue("affixToEnvironment")[:]:
try:
# find first object with that name
obj = self.root.namedChild(jointName).getODEObject()
except IndexError:
print "ERROR: Could not affix object '" + jointName + "' to environment!"
sys.exit(1)
if isinstance(obj, ode.Joint):
# if it is a joint, use this joint to fix to environment
obj.attach(obj.getBody(0), ode.environment)
elif isinstance(obj, ode.Body):
# if it is a body, create new joint and fix body to environment
j = ode.FixedJoint(self.world)
j.attach(obj, ode.environment)
j.setFixed()
# <colors>
for coldefstring in self.config.getValue("colors")[:]:
# ('name', (0.3,0.4,0.5))
objname, coldef = eval(coldefstring)
for (body, _) in self.body_geom:
if hasattr(body, 'name'):
if objname == body.name:
body.color = coldef
break
if not reload:
# add the JointSensor as default
self.sensors = []
## self.addSensor(self._jointSensor)
# <sensors>
# expects a list of strings, each of which is the executable command to create a sensor object
# example: DistToPointSensor('legSensor', (0.0, 0.0, 5.0))
sens = self.config.getValue("sensors")[:]
for s in sens:
try:
self.addSensor(eval('sensors.' + s))
except AttributeError:
print dir(sensors)
warnings.warn("Sensor name with name " + s + " not found. skipped.")
else:
for s in self.sensors:
s._connect(self)
for a in self.actuators:
a._connect(self)
return
class ODEEnvironment(Environment):
"""
Virtual simulation for rigid bodies. Uses ODE as a physics engine and OpenGL as graphics
interface to simulate and display arbitrary objects. Virtual worlds are defined through
an XML file in XODE format (see http://tanksoftware.com/xode/). This file can be loaded
into the world with the "loadXODE(...)" function. Use performAction(...) or step() to advance
the simulation by one step.
"""
# objects with names within one tuple can pass each other
passpairs = []
# define a verbosity level for selective debug output (0=none)
verbosity = 0
def __init__(self, render=True, realtime=True, ip="127.0.0.1", port="21590", buf="16384"):
""" initializes the virtual world, variables, the frame rate and the callback functions."""
print "ODEEnvironment -- based on Open Dynamics Engine."
# initialize base class
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.realtime = realtime
# initialize attributes
self.resetAttributes()
# initialize the textures dictionary
self.textures = {}
# initialize sensor and exclude list
self.sensors = []
self.excludesensors = []
# initialize actuators list
self.actuators = []
# A joint group for the contact joints that are generated whenever two bodies collide
self.contactgroup = ode.JointGroup()
self.dt = 0.01
self.FricMu = 8.0
self.stepsPerAction = 1
self.stepCounter = 0
def resetAttributes(self):
"""resets the class attributes to their default values"""
# initialize root node
self.root = None
# A list with (body, geom) tuples
self.body_geom = []
def reset(self):
"""resets the model and all its parameters to their original values"""
self.loadXODE(self._currentXODEfile, reload=True)
self.stepCounter = 0
def setGravity(self, g):
"""set the world's gravity constant in negative y-direction"""
self.world.setGravity((0, -g, 0))
def _setWorldParameters(self):
""" sets parameters for ODE world object: gravity, error correction (ERP, default=0.2),
constraint force mixing (CFM, default=1e-5). """
self.world.setGravity((0, -9.81, 0))
# self.world.setERP(0.2)
# self.world.setCFM(1e-9)
def _create_box(self, space, density, lx, ly, lz):
"""Create a box body and its corresponding geom."""
# Create body and mass
body = ode.Body(self.world)
M = ode.Mass()
M.setBox(density, lx, ly, lz)
body.setMass(M)
body.name = None
# Create a box geom for collision detection
geom = ode.GeomBox(space, lengths=(lx, ly, lz))
geom.setBody(body)
geom.name = None
return (body, geom)
def _create_sphere(self, space, density, radius):
"""Create a sphere body and its corresponding geom."""
# Create body and mass
body = ode.Body(self.world)
M = ode.Mass()
M.setSphere(density, radius)
body.setMass(M)
body.name = None
# Create a sphere geom for collision detection
geom = ode.GeomSphere(space, radius)
geom.setBody(body)
geom.name = None
return (body, geom)
def drop_object(self):
"""Drops a random object (box, sphere) into the scene."""
# choose between boxes and spheres
if random.uniform() > 0.5:
(body, geom) = self._create_sphere(self.space, 10, 0.4)
else:
(body, geom) = self._create_box(self.space, 10, 0.5, 0.5, 0.5)
# randomize position slightly
body.setPosition((random.normal(-6.5, 0.5), 6.0, random.normal(-6.5, 0.5)))
# body.setPosition( (0.0, 3.0, 0.0) )
# randomize orientation slightly
# theta = random.uniform(0,2*pi)
# ct = cos (theta)
# st = sin (theta)
# rotate body and append to (body,geom) tuple list
# body.setRotation([ct, 0., -st, 0., 1., 0., st, 0., ct])
self.body_geom.append((body, geom))
# -- sensor and actuator functions
def addSensor(self, sensor):
""" adds a sensor object to the list of sensors """
if not isinstance(sensor, sensors.Sensor):
raise TypeError("the given sensor is not an instance of class 'Sensor'.")
# add sensor to sensors list
self.sensors.append(sensor)
# connect sensor and give it the virtual world object
sensor._connect(self)
def addActuator(self, actuator):
""" adds a sensor object to the list of sensors """
if not isinstance(actuator, actuators.Actuator):
raise TypeError("the given actuator is not an instance of class 'Actuator'.")
# add sensor to sensors list
self.actuators.append(actuator)
# connect actuator and give it the virtual world object
actuator._connect(self)
def addTexture(self, name, texture):
""" adds a texture to the given ODE object name """
self.textures[name] = texture
def centerOn(self, name):
return
""" if set, keeps camera to the given ODE object name. """
try:
self.getRenderer().setCenterObj(self.root.namedChild(name).getODEObject())
except KeyError:
# name not found, unset centerObj
print "Warning: Cannot center on " + name
self.centerObj = None
def loadXODE(self, filename, reload=False):
""" loads an XODE file (xml format) and parses it. """
f = file(filename)
self._currentXODEfile = filename
p = xode.parser.Parser()
self.root = p.parseFile(f)
f.close()
try:
# filter all xode "world" objects from root, take only the first one
world = filter(lambda x: isinstance(x, xode.parser.World), self.root.getChildren())[0]
except IndexError:
# malicious format, no world tag found
print "no <world> tag found in " + filename + ". quitting."
sys.exit()
self.world = world.getODEObject()
self._setWorldParameters()
try:
# filter all xode "space" objects from world, take only the first one
space = filter(lambda x: isinstance(x, xode.parser.Space), world.getChildren())[0]
except IndexError:
# malicious format, no space tag found
print "no <space> tag found in " + filename + ". quitting."
sys.exit()
self.space = space.getODEObject()
# load bodies and geoms for painting
self.body_geom = []
self._parseBodies(self.root)
if self.verbosity > 0:
print "-------[body/mass list]-----"
for (body, _) in self.body_geom:
try:
print body.name, body.getMass()
except AttributeError:
print "<Nobody>"
# now parse the additional parameters at the end of the xode file
self.loadConfig(filename, reload)
def loadConfig(self, filename, reload=False):
# parameters are given in (our own brand of) config-file syntax
self.config = ConfigGrabber(filename, sectionId="<!--odeenvironment parameters", delim=("<", ">"))
# <passpairs>
self.passpairs = []
for passpairstring in self.config.getValue("passpairs")[:]:
self.passpairs.append(eval(passpairstring))
if self.verbosity > 0:
print "-------[pass tuples]--------"
print self.passpairs
print "----------------------------"
# <centerOn>
# set focus of camera to the first object specified in the section, if any
if self.render:
try:
self.centerOn(self.config.getValue("centerOn")[0])
except IndexError:
pass
# <affixToEnvironment>
for jointName in self.config.getValue("affixToEnvironment")[:]:
try:
# find first object with that name
obj = self.root.namedChild(jointName).getODEObject()
except IndexError:
print "ERROR: Could not affix object '" + jointName + "' to environment!"
sys.exit(1)
if isinstance(obj, ode.Joint):
# if it is a joint, use this joint to fix to environment
obj.attach(obj.getBody(0), ode.environment)
elif isinstance(obj, ode.Body):
# if it is a body, create new joint and fix body to environment
j = ode.FixedJoint(self.world)
j.attach(obj, ode.environment)
j.setFixed()
# <colors>
for coldefstring in self.config.getValue("colors")[:]:
# ('name', (0.3,0.4,0.5))
objname, coldef = eval(coldefstring)
for (body, _) in self.body_geom:
if hasattr(body, "name"):
if objname == body.name:
body.color = coldef
break
if not reload:
# add the JointSensor as default
self.sensors = []
## self.addSensor(self._jointSensor)
# <sensors>
# expects a list of strings, each of which is the executable command to create a sensor object
# example: DistToPointSensor('legSensor', (0.0, 0.0, 5.0))
sens = self.config.getValue("sensors")[:]
for s in sens:
try:
self.addSensor(eval("sensors." + s))
except AttributeError:
print dir(sensors)
warnings.warn("Sensor name with name " + s + " not found. skipped.")
else:
for s in self.sensors:
s._connect(self)
for a in self.actuators:
a._connect(self)
def _parseBodies(self, node):
""" parses through the xode tree recursively and finds all bodies and geoms for drawing. """
# body (with nested geom)
if isinstance(node, xode.body.Body):
body = node.getODEObject()
body.name = node.getName()
try:
# filter all xode geom objects and take the first one
xgeom = filter(lambda x: isinstance(x, xode.geom.Geom), node.getChildren())[0]
except IndexError:
return () # no geom object found, skip this node
# get the real ode object
geom = xgeom.getODEObject()
# if geom doesn't have own name, use the name of its body
geom.name = node.getName()
self.body_geom.append((body, geom))
# geom on its own without body
elif isinstance(node, xode.geom.Geom):
try:
node.getFirstAncestor(ode.Body)
except xode.node.AncestorNotFoundError:
body = None
geom = node.getODEObject()
geom.name = node.getName()
self.body_geom.append((body, geom))
# special cases for joints: universal, fixed, amotor
elif isinstance(node, xode.joint.Joint):
joint = node.getODEObject()
if type(joint) == ode.UniversalJoint:
# insert an additional AMotor joint to read the angles from and to add torques
# amotor = ode.AMotor(self.world)
# amotor.attach(joint.getBody(0), joint.getBody(1))
# amotor.setNumAxes(3)
# amotor.setAxis(0, 0, joint.getAxis2())
# amotor.setAxis(2, 0, joint.getAxis1())
# amotor.setMode(ode.AMotorEuler)
# xode_amotor = xode.joint.Joint(node.getName() + '[amotor]', node.getParent())
# xode_amotor.setODEObject(amotor)
# node.getParent().addChild(xode_amotor, None)
pass
if type(joint) == ode.AMotor:
# do the euler angle calculations automatically (ref. ode manual)
joint.setMode(ode.AMotorEuler)
if type(joint) == ode.FixedJoint:
# prevent fixed joints from bouncing to center of first body
joint.setFixed()
# recursive call for all child nodes
for c in node.getChildren():
self._parseBodies(c)
def excludeSensors(self, exclist):
self.excludesensors.extend(exclist)
def getSensors(self):
""" returns combined sensor data """
output = []
for s in self.sensors:
if not s.name in self.excludesensors:
output.extend(s.getValues())
return asarray(output)
def getSensorNames(self):
return [s.name for s in self.sensors]
def getActuatorNames(self):
return [a.name for a in self.actuators]
def getSensorByName(self, name):
try:
idx = self.getSensorNames().index(name)
except ValueError:
warnings.warn("sensor " + name + " is not in sensor list.")
return []
return self.sensors[idx].getValues()
@property
def indim(self):
num = 0
for a in self.actuators:
num += a.getNumValues()
return num
def getActionLength(self):
print "getActionLength() is deprecated. use property 'indim' instead."
return self.indim
@property
def outdim(self):
return len(self.getSensors())
def performAction(self, action):
""" sets the values for all actuators combined. """
pointer = 0
for a in self.actuators:
val = a.getNumValues()
a._update(action[pointer : pointer + val])
pointer += val
for _ in range(self.stepsPerAction):
self.step()
def getXODERoot(self):
return self.root
# --- callback functions ---#
def _near_callback(self, args, geom1, geom2):
"""Callback function for the collide() method.
This function checks if the given geoms do collide and
creates contact joints if they do."""
# only check parse list, if objects have name
if geom1.name != None and geom2.name != None:
# Preliminary checking, only collide with certain objects
for p in self.passpairs:
g1 = False
g2 = False
for x in p:
g1 = g1 or (geom1.name.find(x) != -1)
g2 = g2 or (geom2.name.find(x) != -1)
if g1 and g2:
return ()
# Check if the objects do collide
contacts = ode.collide(geom1, geom2)
# Create contact joints
world, contactgroup = args
for c in contacts:
p = c.getContactGeomParams()
# parameters from Niko Wolf
c.setBounce(0.2)
c.setBounceVel(0.05) # Set the minimum incoming velocity necessary for bounce
c.setSoftERP(0.6) # Set the contact normal "softness" parameter
c.setSoftCFM(0.00005) # Set the contact normal "softness" parameter
c.setSlip1(0.02) # Set the coefficient of force-dependent-slip (FDS) for friction direction 1
c.setSlip2(0.02) # Set the coefficient of force-dependent-slip (FDS) for friction direction 2
c.setMu(self.FricMu) # Set the Coulomb friction coefficient
j = ode.ContactJoint(world, contactgroup, c)
j.name = None
j.attach(geom1.getBody(), geom2.getBody())
def getCurrentStep(self):
return self.stepCounter
@threaded()
def updateClients(self):
self.updateDone = False
if not self.updateLock.acquire(False):
return
# build message to send
message = []
for (body, geom) in self.body_geom:
item = {}
# real bodies (boxes, spheres, ...)
if body != None:
# transform (rotate, translate) body accordingly
item["position"] = body.getPosition()
item["rotation"] = body.getRotation()
if hasattr(body, "color"):
item["color"] = body.color
# switch different geom objects
if type(geom) == ode.GeomBox:
# cube
item["type"] = "GeomBox"
item["scale"] = geom.getLengths()
elif type(geom) == ode.GeomSphere:
# sphere
item["type"] = "GeomSphere"
item["radius"] = geom.getRadius()
elif type(geom) == ode.GeomCCylinder:
# capped cylinder
item["type"] = "GeomCCylinder"
item["radius"] = geom.getParams()[0]
item["length"] = geom.getParams()[1] - 2 * item["radius"]
elif type(geom) == ode.GeomCylinder:
# solid cylinder
item["type"] = "GeomCylinder"
item["radius"] = geom.getParams()[0]
item["length"] = geom.getParams()[1]
else:
# TODO: add other geoms here
pass
else:
# no body found, then it must be a plane (we only draw planes)
if type(geom) == ode.GeomPlane:
# plane
item["type"] = "GeomPlane"
item["normal"] = geom.getParams()[0] # the normal vector to the plane
item["distance"] = geom.getParams()[1] # the distance to the origin
message.append(item)
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send(message)
time.sleep(0.02)
self.updateLock.release()
self.updateDone = True
def step(self):
""" Here the ode physics is calculated by one step. """
# call additional callback functions for all kinds of tasks (e.g. printing)
self._printfunc()
# Detect collisions and create contact joints
self.space.collide((self.world, self.contactgroup), self._near_callback)
# Simulation step
self.world.step(float(self.dt))
# Remove all contact joints
self.contactgroup.empty()
# update all sensors
for s in self.sensors:
s._update()
# update clients
if self.render and self.updateDone:
self.updateClients()
if self.server.clients > 0 and self.realtime:
time.sleep(self.dt)
# increase step counter
self.stepCounter += 1
return self.stepCounter
def _printfunc(self):
pass
# print self.root.namedChild('palm').getODEObject().getPosition()
def specialkeyfunc(self, c, x, y):
"""Derived classes can implement extra functionality here"""
pass
# --- helper functions ---#
def _print_help(self):
""" prints out the keyboard shortcuts. """
print "v -> toggle view with mouse on/off"
print "s -> toggle screen capture on/off"
print "d -> drop an object"
print "f -> lift all objects"
print "m -> toggle mouse view (press button to zoom)"
print "r -> random torque at all joints"
print "a/z -> negative/positive torque to all joints"
print "g -> print current state"
print "n -> reset environment"
self.specialfunctionDoc()
print "x,q -> exit program"
def specialfunctionDoc(self):
"""Derived classes can implement extra functionality here"""
pass