#!/usr/bin/env python # -*- coding: iso-8859-15 -*- # robot.py, Robot Structure Module # Copyright (C) 2006 por Rafael Treviño Menéndez # Autor: Rafael Treviño Menéndez # This program is free software; you can redistribute it and/or # modify it under the terms of the GNU Library General Public # License as published by the Free Software Foundation; either # version 2 of the License, or (at your option) any later version. # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU # Library General Public License for more details. # You should have received a copy of the GNU General Public License # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. import ode, utils, types, time, sys import libs.drawstuff, serial, bashcolor from math import pi, cos, sin, tan SERVO_F = .4 # kg m WEIGHT = 0.02 # kg # Dimensions MOD_X = 0.36 MOD_Y = 0.52 MOD_Z = 0.52 # Servo gain vel GAIN = 5.0 # Render position of the joint JOINT_UP = 0.075 # Max error allowed by module MAX_ERROR_PER_MOD = 0.1 # Constant for conversions between degrees and radians constant = pi / 180.0 class robot: def __init__ (self, source, world, space, pos, dModule, wire, ser): """Initialize the robot reading a file of structure""" self.structure = [] self.bodies = [] self.geoms = [] self.joints = [] self.structureJoints = [] self.modules = 0 self.initPos = pos self.initCenter = None self.lastCenter = None self.dModule = dModule self.wire = wire if ser: self.serial = serial.servos (ser) self.change = True else: self.serial = self.change = None if type (source) == types.StringType: try: f = open (source) except: bashcolor.msg ([('VERDE', 'mrs:'), ('rojo', 'Error:'), ('cyan', "robot file doesn't exits.")]) sys.exit (-1) for i, line in enumerate ([l for l in f.readlines () if l [0] != '#']): self.structure.append ((i, line.split ())) for i, item in self.structure: item [0] = int (item [0]) item [-1] = int (item [-1]) f.close () else: self.structure = source self.world = world self.space = space self.dim = len (self.structure) # print self.structure def setServos (self, seq): # Global error value globalError = 0.0 # Servo joints try: desired = seq.gait [seq.seqPosition] except: desired = self.dim * [0.0] if self.serial and self.change: self.serial.setServos (desired) self.serial.write (' @ %s\n' % str (time.clock ())) self.change = False for way, servo, index in self.joints: error = servo.getAngle () - way * desired [index] * constant globalError += abs (error) servo.setParam (ode.ParamVel, -error * GAIN) # Structural joints for i, fijacion in enumerate (self.structureJoints): error = fijacion.getAngle () fijacion.setParam (ode.ParamVel, -error) # If no too much error, forward the sequence if seq and globalError < MAX_ERROR_PER_MOD * self.dim: seq.seqPosition = (seq.seqPosition + 1) % seq.nMoves self.change = True def drawRobot (self): # Draw all the bodies type = 0 # Set the texture color libs.drawstuff.dsSetColor (0.75, 0.75, 0.75) # Set the texture (DS_WOOD = 1) libs.drawstuff.dsSetTexture (1) # Total position tPos = zip (*[g.getPosition () for b, c, g in self.bodies]) tPos = [sum (l) for l in tPos] for b, c, g in self.bodies: pos = g.getPosition () R = g.getRotation () sides = g.getLengths () vertical = (c [0] == 'V') if self.dModule: libs.drawstuff.dsDrawY1 (pos, R, sides, type, vertical, self.wire) else: libs.drawstuff.dsDrawBox (pos, R, sides) type = 1 - type if self.initCenter: self.lastCenter = (tPos [0] / (self.dim * 2), tPos [1] / (self.dim * 2), tPos [2] / (self.dim * 2)) else: self.initCenter = (tPos [0] / (self.dim * 2), tPos [1] / (self.dim * 2), tPos [2] / (self.dim * 2)) def createBox(self, boxMass, lx, ly, lz): """Create a box body and its corresponding geom.""" # Create body M = ode.Mass() body = ode.Body (self.world) M.setBox (boxMass, lx, ly, lz) body.setMass (M) # Create a box geom for collision detection geom = ode.GeomBox(self.space, (lx, ly, lz)) geom.setBody (body) return body, geom # drop_y1 def createModule (self, pos1, pos3, angle, config, index): """Drop an y1-module into the scene.""" pos2 = ((pos1 [0] + pos3 [0]) / 2, (pos1 [1] + pos3 [1]) / 2, (pos1 [2] + pos3 [2]) / 2) m = utils.rotation (angle, (0, 0, 1)) body1, geom1 = self.createBox (WEIGHT, MOD_X, MOD_Y, MOD_Z) body1.setPosition (pos1) body1.setRotation (m) self.bodies.append ( (body1, config, geom1) ) self.geoms.append (geom1) body2, geom2 = self.createBox (WEIGHT, MOD_X, MOD_Y, MOD_Z) body2.setPosition (pos3) body2.setRotation (m) self.bodies.append ( (body2, config, geom2) ) self.geoms.append (geom2) if config [2] == 'N': way = -1.0 else: way = 1.0 j = ode.HingeJoint (self.world) j.attach (body1, body2) j.setAnchor ( (pos2 [0], pos2 [1], pos2 [2] + JOINT_UP)) if config [0] == 'V': j.setAxis ((cos (angle + pi / 2), sin (angle + pi / 2), 0)) else: j.setAxis ((0.0, 0.0, 1.0)) j.setParam (ode.ParamFMax, SERVO_F) j.setParam (ode.ParamVel, 0.0) j.setParam (ode.ParamLoStop, -pi / 2) j.setParam (ode.ParamHiStop, pi / 2) self.joints.append ((way, j, index)) return body1, body2 def calculePosition (self, actPos1, actPos3, actAngle, n, i, actBody, config, index): d1 = (0.27 / tan (pi / (n + 1))) + MOD_X / 2 # Distance between the center and the far module d3 = d1 + MOD_X # Center of the modules center = (actPos3 [0] + (actPos3 [0] - actPos1 [0]) / MOD_X * d1, actPos3 [1] + (actPos3 [1] - actPos1 [1]) / MOD_X * d1, actPos3 [2] + (actPos3 [2] - actPos1 [2]) / MOD_X * d1) # Correct angle rotation angle = 2 * pi * (i + 1) / (n + 1) - pi + actAngle # New pos1 and pos3 positions pos1 = (center [0] + cos (angle) * d1, center [1] + sin (angle) * d1, actPos1 [2]) pos3 = (center [0] + cos (angle) * d3, center [1] + sin (angle) * d3, actPos3 [2]) # Bodies of the module b1, b2 = self.createModule (pos1, pos3, angle, config, index) # Create the structural joint j = ode.HingeJoint (self.world) j.attach (actBody, b1) j.setAnchor (center) j.setAxis ((0, 0, 1)) j.setParam (ode.ParamFMax, 1000.0) j.setParam (ode.ParamVel, 0.0) j.setParam (ode.ParamLoStop, -pi / 2) j.setParam (ode.ParamHiStop, pi / 2) self.structureJoints.append (j) return pos1, pos3, angle, b2 def createRobot (self): heap = [] # Insert the first element of the heap # heap element: (node, module, pos1, pos3, angle, body) # First node (need to be a leaf) node = utils.findLeaf (self) self.modules = 2 * len (self.structure) # There's only one neighbour neighbours = utils.findNeighbours (self, node) pos1 = self.initPos pos3 = (pos1 [0] + MOD_X, pos1 [1] + 0.0, pos1 [2]) angle = 0 # Create the module for this node b1, b2 = self.createModule (pos1, pos3, angle, neighbours [0][1][1], neighbours [0][0]) # Push this module to the heap heap.append ((node, neighbours [0][1], pos1, pos3, angle, b2)) # Explore the robot graph deep-first while (len (heap) > 0): # print heap # Pop the first element (last inserted) item = heap [-1] heap = heap [:-1] # Find the neighbours of the active node if (item [1][0] == item [0]): node = item [1][2] else: node = item [1][0] neighbours = utils.findNeighbours (self, node) # Insert the neighbours in the heap n = len (neighbours) for i in range (n): pos1, pos3, angle, b = self.calculePosition (item [2], item [3], item [4], n, i, item [5], neighbours [i][1][1], neighbours [i][0]) heap.append ((node, neighbours [i][1], pos1, pos3, angle, b)) if __name__ == '__main__': # Create a world world = ode.World () # Set Gravity world.setGravity ( (0, 0, -9.81) ) # Set CFM parameter world.setCFM (1e-5) # Auto-disabled mode on world.setAutoDisableFlag (1) # Other parameters of the world simulated world.setContactMaxCorrectingVel (0.1) world.setContactSurfaceLayer (0.001) # Create a collision space space = ode.HashSpace () # Floor plane geometry floor = ode.GeomPlane (space, (0, 0, 1), 0) # Create a new model in this world and collision space print 'Autotesting...' model = robot ('../quad.rbt', world, space, (0.0, 0.0, 0.5), 1, 1, None) print 'OK'