class FlexCubeRenderInterface(object):
def __init__(self, ip="127.0.0.1", port="21560"):
self.target = array([80.0, 0.0, 0.0])
#self.dataLock = threading.Lock()
self.centerOfGrav = array([0.0, -2.0, 0.0])
self.points = ones((8, 3), float)
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
@threaded()
def updateData(self, pos, cog):
self.updateDone = False
if not self.updateLock.acquire(False): return
self.points = pos.copy()
self.centerOfGrav = cog.copy()
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send([self.points, self.centerOfGrav, self.target])
sleep(0.02)
self.updateLock.release()
self.updateDone = True
def __init__(self,
render=True,
realtime=True,
ip="127.0.0.1",
port="21560"):
# initialize base class
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.actLen = 12
self.mySensors = sensors.Sensors(["EdgesReal"])
self.dists = array([20.0, sqrt(2.0) * 20, sqrt(3.0) * 20])
self.gravVect = array([0.0, -100.0, 0.0])
self.centerOfGrav = zeros((1, 3), float)
self.pos = ones((8, 3), float)
self.vel = zeros((8, 3), float)
self.SpringM = ones((8, 8), float)
self.d = 60.0
self.dt = 0.02
self.startHight = 10.0
self.dumping = 0.4
self.fraktMin = 0.7
self.fraktMax = 1.3
self.minAkt = self.dists[0] * self.fraktMin
self.maxAkt = self.dists[0] * self.fraktMax
self.reset()
self.count = 0
self.setEdges()
self.act(array([20.0] * 12))
self.euler()
self.realtime = realtime
self.step = 0
class FlexCubeRenderInterface(object):
def __init__(self, ip="127.0.0.1", port="21560"):
self.target=array([80.0,0.0,0.0])
#self.dataLock = threading.Lock()
self.centerOfGrav=array([0.0,-2.0,0.0])
self.points=ones((8,3),float)
self.updateDone=True
self.updateLock=threading.Lock()
self.server=UDPServer(ip, port)
@threaded()
def updateData(self, pos, cog):
self.updateDone=False
if not self.updateLock.acquire(False): return
self.points=pos.copy()
self.centerOfGrav=cog.copy()
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send([self.points, self.centerOfGrav, self.target])
sleep(0.02)
self.updateLock.release()
self.updateDone=True
def __init__(self, renderer=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
GraphicalEnvironment.__init__(self)
if renderer:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port, buf)
self.render=renderer
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 __init__(self, ip="127.0.0.1", port="21560"):
self.target = array([80.0, 0.0, 0.0])
#self.dataLock = threading.Lock()
self.centerOfGrav = array([0.0, -2.0, 0.0])
self.points = ones((8, 3), float)
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
def __init__(self, render=True, ip="127.0.0.1", port="21580", numdir=1):
# initialize the environment (randomly)
self.action = [0.0, 0.0]
self.delay = False
self.numdir = numdir # number of directions in which ship starts
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.reset()
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 __init__(self, render=True, realtime=True, ip="127.0.0.1", port="21560"):
# initialize base class
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.actLen = 12
self.mySensors = sensors.Sensors(["EdgesReal"])
self.dists = array([20.0, sqrt(2.0) * 20, sqrt(3.0) * 20])
self.gravVect = array([0.0, -100.0, 0.0])
self.centerOfGrav = zeros((1, 3), float)
self.pos = ones((8, 3), float)
self.vel = zeros((8, 3), float)
self.SpringM = ones((8, 8), float)
self.d = 60.0
self.dt = 0.02
self.startHight = 10.0
self.dumping = 0.4
self.fraktMin = 0.7
self.fraktMax = 1.3
self.minAkt = self.dists[0] * self.fraktMin
self.maxAkt = self.dists[0] * self.fraktMax
self.reset()
self.count = 0
self.setEdges()
self.act(array([20.0] * 12))
self.euler()
self.realtime = realtime
self.step = 0
def __init__(self, ip="127.0.0.1", port="21560"):
self.target=array([80.0,0.0,0.0])
#self.dataLock = threading.Lock()
self.centerOfGrav=array([0.0,-2.0,0.0])
self.points=ones((8,3),float)
self.updateDone=True
self.updateLock=threading.Lock()
self.server=UDPServer(ip, port)
def __init__(self, render=True, ip="127.0.0.1", port="21580", numdir=1):
# initialize the environment (randomly)
self.action = [0.0, 0.0]
self.delay = False
self.numdir = numdir # number of directions in which ship starts
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.reset()
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 closeSocket(self):
self.server.UDPInSock.close()
time.sleep(10)
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 FlexCubeEnvironment(Environment):
def __init__(self, render=True, realtime=True, ip="127.0.0.1", port="21560"):
# initialize base class
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.actLen = 12
self.mySensors = sensors.Sensors(["EdgesReal"])
self.dists = array([20.0, sqrt(2.0) * 20, sqrt(3.0) * 20])
self.gravVect = array([0.0, -100.0, 0.0])
self.centerOfGrav = zeros((1, 3), float)
self.pos = ones((8, 3), float)
self.vel = zeros((8, 3), float)
self.SpringM = ones((8, 8), float)
self.d = 60.0
self.dt = 0.02
self.startHight = 10.0
self.dumping = 0.4
self.fraktMin = 0.7
self.fraktMax = 1.3
self.minAkt = self.dists[0] * self.fraktMin
self.maxAkt = self.dists[0] * self.fraktMax
self.reset()
self.count = 0
self.setEdges()
self.act(array([20.0] * 12))
self.euler()
self.realtime = realtime
self.step = 0
def closeSocket(self):
self.server.UDPInSock.close()
sleep(10)
def setEdges(self):
self.edges = zeros((12, 2), int)
count = 0
c1 = 0
for i in range(2):
for j in range(2):
for k in range(2):
c2 = 0
for i2 in range(2):
for j2 in range(2):
for k2 in range(2):
sum = abs(i - i2) + abs(j - j2) + abs(k - k2)
if sum == 1 and i <= i2 and j <= j2 and k <= k2:
self.edges[count] = [c1, c2]
count += 1
c2 += 1
c1 += 1
def reset(self):
self.action = ones((1, 12), float) * self.dists[0]
for i in range(2):
for j in range(2):
for k in range(2):
self.pos[i * 4 + j * 2 + k] = [i * self.dists[0] - self.dists[0] / 2.0, j * self.dists[0] - self.dists[0] / 2.0 + self.startHight, k * self.dists[0] - self.dists[0] / 2.0]
self.vel = zeros((8, 3), float)
idx0 = arange(8).repeat(8)
idx1 = array(range(8) * 8)
self.difM = self.pos[idx0, :] - self.pos[idx1, :] #vectors from all points to all other points
self.springM = sqrt((self.difM ** 2).sum(axis=1)).reshape(64, 1)
self.distM = self.springM.copy() #distance matrix
self.step = 0
self.mySensors.updateSensor(self.pos, self.vel, self.distM, self.centerOfGrav, self.step, self.action)
if self.render:
if self.server.clients > 0:
# If there are clients send them reset signal
self.server.send(["r", "r"])
def performAction(self, action):
action = self.normAct(action)
self.action = action.copy()
self.act(action)
self.euler()
self.step += 1
if self.render:
if self.updateDone:
self.updateRenderer()
if self.server.clients > 0 and self.realtime:
sleep(0.02)
def getSensors(self):
self.mySensors.updateSensor(self.pos, self.vel, self.distM, self.centerOfGrav, self.step, self.action)
return self.mySensors.getSensor()[:]
def normAct(self, s):
return clip(s, self.minAkt, self.maxAkt)
def act(self, a):
count = 0
for i in self.edges:
self.springM[i[0] * 8 + i[1]] = a[count]
self.springM[i[1] * 8 + i[0]] = a[count]
count += 1
def euler(self):
self.count += 1
#Inner Forces
distM = self.distM.copy()
disM = self.springM - distM #difference between wanted spring lengths and current ones
disM = disM.reshape(64, 1)
distM = distM + 0.0000000001 #hack to prevent divs by 0
#Forces to Velos
#spring vectors normalized to 1 times the actual force from deformation
vel = self.difM / distM
vel *= disM * self.d * self.dt
idx2 = arange(8)
#TODO: arggggg!!!!!
for i in range(8):
self.vel[i] += vel[idx2 + i * 8, :].sum(axis=0)
#Gravity
self.vel += self.gravVect * self.dt
#Dumping
self.vel -= self.vel * self.dumping * self.dt
#velos to positions
self.pos += self.dt * self.vel
#Collisions and friction
for i in range(8):
if self.pos[i][1] < 0.0:
self.pos[i][1] = 0.0
self.vel[i] = self.vel[i] * [0.0, -1.0, 0.0]
self.centerOfGrav = self.pos.sum(axis=0) / 8.0
#Distances of new state
idx0 = arange(8).repeat(8)
idx1 = array(range(8) * 8)
self.difM = self.pos[idx0, :] - self.pos[idx1, :] #vectors from all points to all other points
self.distM = sqrt((self.difM ** 2).sum(axis=1)).reshape(64, 1) #distance matrix
@threaded()
def updateRenderer(self):
self.updateDone = False
if not self.updateLock.acquire(False): return
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send(repr([self.pos, self.centerOfGrav]))
sleep(0.02)
self.updateLock.release()
self.updateDone = True
class FlexCubeEnvironment(Environment):
def __init__(self,
render=True,
realtime=True,
ip="127.0.0.1",
port="21560"):
# initialize base class
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.actLen = 12
self.mySensors = sensors.Sensors(["EdgesReal"])
self.dists = array([20.0, sqrt(2.0) * 20, sqrt(3.0) * 20])
self.gravVect = array([0.0, -100.0, 0.0])
self.centerOfGrav = zeros((1, 3), float)
self.pos = ones((8, 3), float)
self.vel = zeros((8, 3), float)
self.SpringM = ones((8, 8), float)
self.d = 60.0
self.dt = 0.02
self.startHight = 10.0
self.dumping = 0.4
self.fraktMin = 0.7
self.fraktMax = 1.3
self.minAkt = self.dists[0] * self.fraktMin
self.maxAkt = self.dists[0] * self.fraktMax
self.reset()
self.count = 0
self.setEdges()
self.act(array([20.0] * 12))
self.euler()
self.realtime = realtime
self.step = 0
def closeSocket(self):
self.server.UDPInSock.close()
sleep(10)
def setEdges(self):
self.edges = zeros((12, 2), int)
count = 0
c1 = 0
for i in range(2):
for j in range(2):
for k in range(2):
c2 = 0
for i2 in range(2):
for j2 in range(2):
for k2 in range(2):
sum = abs(i - i2) + abs(j - j2) + abs(k - k2)
if sum == 1 and i <= i2 and j <= j2 and k <= k2:
self.edges[count] = [c1, c2]
count += 1
c2 += 1
c1 += 1
def reset(self):
self.action = ones((1, 12), float) * self.dists[0]
for i in range(2):
for j in range(2):
for k in range(2):
self.pos[i * 4 + j * 2 + k] = [
i * self.dists[0] - self.dists[0] / 2.0,
j * self.dists[0] - self.dists[0] / 2.0 +
self.startHight,
k * self.dists[0] - self.dists[0] / 2.0
]
self.vel = zeros((8, 3), float)
idx0 = arange(8).repeat(8)
idx1 = array(range(8) * 8)
self.difM = self.pos[idx0, :] - self.pos[
idx1, :] #vectors from all points to all other points
self.springM = sqrt((self.difM**2).sum(axis=1)).reshape(64, 1)
self.distM = self.springM.copy() #distance matrix
self.step = 0
self.mySensors.updateSensor(self.pos, self.vel, self.distM,
self.centerOfGrav, self.step, self.action)
if self.render:
if self.server.clients > 0:
# If there are clients send them reset signal
self.server.send(["r", "r"])
def performAction(self, action):
action = self.normAct(action)
self.action = action.copy()
self.act(action)
self.euler()
self.step += 1
if self.render:
if self.updateDone:
self.updateRenderer()
if self.server.clients > 0 and self.realtime:
sleep(0.02)
def getSensors(self):
self.mySensors.updateSensor(self.pos, self.vel, self.distM,
self.centerOfGrav, self.step, self.action)
return self.mySensors.getSensor()[:]
def normAct(self, s):
return clip(s, self.minAkt, self.maxAkt)
def act(self, a):
count = 0
for i in self.edges:
self.springM[i[0] * 8 + i[1]] = a[count]
self.springM[i[1] * 8 + i[0]] = a[count]
count += 1
def euler(self):
self.count += 1
#Inner Forces
distM = self.distM.copy()
disM = self.springM - distM #difference between wanted spring lengths and current ones
disM = disM.reshape(64, 1)
distM = distM + 0.0000000001 #hack to prevent divs by 0
#Forces to Velos
#spring vectors normalized to 1 times the actual force from deformation
vel = self.difM / distM
vel *= disM * self.d * self.dt
idx2 = arange(8)
#TODO: arggggg!!!!!
for i in range(8):
self.vel[i] += vel[idx2 + i * 8, :].sum(axis=0)
#Gravity
self.vel += self.gravVect * self.dt
#Dumping
self.vel -= self.vel * self.dumping * self.dt
#velos to positions
self.pos += self.dt * self.vel
#Collisions and friction
for i in range(8):
if self.pos[i][1] < 0.0:
self.pos[i][1] = 0.0
self.vel[i] = self.vel[i] * [0.0, -1.0, 0.0]
self.centerOfGrav = self.pos.sum(axis=0) / 8.0
#Distances of new state
idx0 = arange(8).repeat(8)
idx1 = array(range(8) * 8)
self.difM = self.pos[idx0, :] - self.pos[
idx1, :] #vectors from all points to all other points
self.distM = sqrt(
(self.difM**2).sum(axis=1)).reshape(64, 1) #distance matrix
@threaded()
def updateRenderer(self):
self.updateDone = False
if not self.updateLock.acquire(False): return
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send(repr([self.pos, self.centerOfGrav]))
sleep(0.02)
self.updateLock.release()
self.updateDone = True
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
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
class ShipSteeringEnvironment(GraphicalEnvironment):
"""
Simulates an ocean going ship with substantial inertia in both forward
motion and rotation, plus noise.
State space (continuous):
h heading of ship in degrees (North=0)
hdot angular velocity of heading in degrees/minute
v velocity of ship in knots
Action space (continuous):
rudder angle of rudder
thrust propulsion of ship forward
"""
# some (more or less) physical constants
dt = 4. # simulated time (in seconds) per step
mass = 1000. # mass of ship in unclear units
I = 1000. # rotational inertia of ship in unclear units
def __init__(self, render=True, ip="127.0.0.1", port="21580", numdir=1):
GraphicalEnvironment.__init__(self)
# initialize the environment (randomly)
self.action = [0.0, 0.0]
self.delay = False
self.numdir = numdir # number of directions in which ship starts
self.render=render
if self.render:
self.updateDone=True
self.updateLock=threading.Lock()
self.server=UDPServer(ip, port)
self.reset()
def step(self):
""" integrate state using simple rectangle rule """
thrust = float(self.action[0])
rudder = float(self.action[1])
h, hdot, v = self.sensors
rnd = random.normal(0,1.0, size=3)
thrust = min(max(thrust,-1),+2)
rudder = min(max(rudder,-90),+90)
drag = 5*h + (rudder**2 + rnd[0])
force = 30.0*thrust - 2.0*v - 0.02*v*drag + rnd[1]*3.0
v = v + self.dt*force/self.mass
v = min(max(v,-10),+40)
torque = -v*(rudder + h + 1.0*hdot + rnd[2]*10.)
last_hdot = hdot
hdot += torque / self.I
hdot = min(max(hdot,-180),180)
h += (hdot + last_hdot) / 2.0
if h>180.:
h -= 360.
elif h<-180.:
h += 360.
self.sensors = (h,hdot,v)
def reset(self):
""" re-initializes the environment, setting the ship to rest at a random orientation.
"""
# [h, hdot, v]
self.sensors = [random.uniform(-30., 30.), 0.0, 0.0]
if self.render:
if self.server.clients > 0:
# If there are clients send them reset signal
self.server.send(["r","r","r"])
def getHeading(self):
""" auxiliary access to just the heading, to be used by GoNorthwardTask """
return self.sensors[0]
def getSpeed(self):
""" auxiliary access to just the speed, to be used by GoNorthwardTask """
return self.sensors[2]
def getSensors(self):
""" returns the state one step (dt) ahead in the future. stores the state in
self.sensors because it is needed for the next calculation.
"""
return self.sensors
def performAction(self, action):
""" stores the desired action for the next time step.
"""
self.action = action
self.step()
if self.render:
if self.updateDone:
self.updateRenderer()
if self.server.clients > 0:
sleep(0.2)
@threaded()
def updateRenderer(self):
self.updateDone=False
if not self.updateLock.acquire(False): return
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send(self.sensors)
sleep(0.02)
self.updateLock.release()
self.updateDone=True
@property
def indim(self):
return len(self.action)
@property
def outdim(self):
return len(self.sensors)
class ShipSteeringEnvironment(Environment):
"""
Simulates an ocean going ship with substantial inertia in both forward
motion and rotation, plus noise.
State space (continuous):
h heading of ship in degrees (North=0)
hdot angular velocity of heading in degrees/minute
v velocity of ship in knots
Action space (continuous):
rudder angle of rudder
thrust propulsion of ship forward
"""
# some (more or less) physical constants
dt = 4. # simulated time (in seconds) per step
mass = 1000. # mass of ship in unclear units
I = 1000. # rotational inertia of ship in unclear units
def __init__(self, render=True, ip="127.0.0.1", port="21580", numdir=1):
# initialize the environment (randomly)
self.action = [0.0, 0.0]
self.delay = False
self.numdir = numdir # number of directions in which ship starts
self.render = render
if self.render:
self.updateDone = True
self.updateLock = threading.Lock()
self.server = UDPServer(ip, port)
self.reset()
def step(self):
""" integrate state using simple rectangle rule """
thrust = float(self.action[0])
rudder = float(self.action[1])
h, hdot, v = self.sensors
rnd = random.normal(0, 1.0, size=3)
thrust = min(max(thrust, -1), +2)
rudder = min(max(rudder, -90), +90)
drag = 5 * h + (rudder**2 + rnd[0])
force = 30.0 * thrust - 2.0 * v - 0.02 * v * drag + rnd[1] * 3.0
v = v + self.dt * force / self.mass
v = min(max(v, -10), +40)
torque = -v * (rudder + h + 1.0 * hdot + rnd[2] * 10.)
last_hdot = hdot
hdot += torque / self.I
hdot = min(max(hdot, -180), 180)
h += (hdot + last_hdot) / 2.0
if h > 180.:
h -= 360.
elif h < -180.:
h += 360.
self.sensors = (h, hdot, v)
def closeSocket(self):
self.server.UDPInSock.close()
sleep(10)
def reset(self):
""" re-initializes the environment, setting the ship to rest at a random orientation.
"""
# [h, hdot, v]
self.sensors = [random.uniform(-30., 30.), 0.0, 0.0]
if self.render:
if self.server.clients > 0:
# If there are clients send them reset signal
self.server.send(["r", "r", "r"])
def getHeading(self):
""" auxiliary access to just the heading, to be used by GoNorthwardTask """
return self.sensors[0]
def getSpeed(self):
""" auxiliary access to just the speed, to be used by GoNorthwardTask """
return self.sensors[2]
def getSensors(self):
""" returns the state one step (dt) ahead in the future. stores the state in
self.sensors because it is needed for the next calculation.
"""
return self.sensors
def performAction(self, action):
""" stores the desired action for the next time step.
"""
self.action = action
self.step()
if self.render:
if self.updateDone:
self.updateRenderer()
if self.server.clients > 0:
sleep(0.2)
@threaded()
def updateRenderer(self):
self.updateDone = False
if not self.updateLock.acquire(False): return
# Listen for clients
self.server.listen()
if self.server.clients > 0:
# If there are clients send them the new data
self.server.send(self.sensors)
sleep(0.02)
self.updateLock.release()
self.updateDone = True
@property
def indim(self):
return len(self.action)
@property
def outdim(self):
return len(self.sensors)
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
