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