...And populating them
* Now with code to handle multiple objects, save and load objects to a human-readable file and rotate the scene with the cursor keys. This is as about as far as I am taking the Python code as it has served its purpose, to design and assemble the models. There is a bit of tidying up needed, a routine to attach the limbs in a proper chain using the joint angles to display the entire model... And another to export the parts as WaveFront object files, which is pretty easy. Then Cardware can be 3D-printed, milled and rendered in almost any rendering package commercial or otherwise, be simulated in many engines, and, represented by a networked Cardware engine and optionally attached to the sensors on a real Cardware model...
To understand the world, and indeed itself, a robot needs to metricate everything and represent it mathematically. It is very important for movement that the robot also understands its perimeters and not just the angles of the joints, so it has situational awareness. So, the first thing I had to do was measure everything and create mathematical models for it to represent itself with.
First the easy bits, simple to pull measurements directly off the plans in MM. I'm using that because it fits better into denary math and I can round it to the nearest MM without affecting the shape. The card itself has a 0.5MM width and the physical model can vary by that anyway.
Turns out the model can deviate quite a lot from the plans even when they are printed and folded to make the actual model, and accuracy has little to do with it in practice on some parts. More on that later...
The Thigh Module
Hip and Ankle Modules are simply half of the above part, easy to generate a mesh for during import. The legendary Hip Unit (Units are unpowered, Modules are powered) was already measured to calculate the inner radius from the outer, a 2mm difference in diameter.
The Hip Unit
The foot is more complicated. Filled curved shapes are a nightmare to compute edges for, so I've broken them into a mesh. This was done manually in Inkscape from the original drawing.
Overlaid with polygons and then measured, thats the foot done too.
The Foot Module
The lid was a little bit more complicated. While I can draw orthogonal plans to scale I'm not entirely sure thats accurate to the MM in all three dimensions. The original MorningStar was not calculated to be the shape it was, I discovered the fold almost accidentally and then figured out the mathematics for it, then used them to compute the lid dimensions as curves. Interpolations from that math was used to create the mesh visually, but is it correct? I dont know for sure.
It looks pretty close, but it isnt exact. I recreated the mesh in Python from the given dimensions so I know its accurate. Each corner is defined as 33 points in two concentric circles surrounding a centre point. Then the edges of the circles are joined with polygons.
That just defines the vertices and the edges joining them as a list of XYZ coordinates followed by a list of groups of four pointers into it. Once this information is obtained, its fairly easy to rotate them into any position.
This is still not the end of the story, the computer still doesnt understand 3 dimensions fully although it knows about them. It has to be instructed that the further away from the screen the coordinate is, the smaller it appears - this is perspective projection, and its important because when the robot sees with its camera, the world will be distorted by it and will need correcting for orthogonal projection inside the map.
It also needs to be told that objects can be in front of other objects and thus obscure them, so the system caters for depth by sorting everything it 'sees' into order and working from the back to the front. The computational equation at the end of the log produces this image from just the corner radius and length of the sides between the corners by calculation of all the angles of the polygons mathematically.
The Lid (After a bit of adjusting, to scale. 1px/mm...)
Is mine... Muahahah! Fly, my beauties, fly... XD
Everything the robot experiences will be treated this way. What the camera sees will be projected onto a spherical surface beyond the known meshes, and the meshes are rendered from camera data so the two merge inside the computer memory as a perspective image with embedded 3d information. To crack open intelligence requires creativity, an imagination, so I'm building the robot one that it can perceive the world with rather than just sense it, exactly the same way we do. It produces a render of what it interacts with that can be recreated at any time.
More than this, the robot can render itself and others into a 3d scene accurately because each knows what it looks like and can tell the others. Once experienced, another robot can remember the first down to the pixel and interact with it in imagination, not just real world. Comparing the two should reveal behaviours, which is the purpose of the level of detail I'm using here.
Here is an imaginary representation of a table with a laptop and mouse on it, and a cable across the back. The robot has walked on the first image and mapped the table top but doesnt understand whats under it or around it. The cable is logged as an obstruction, breaking up the surface. I've catered for surfaces by defining the world as a solid until the robot maps it. Anything walked on becomes a flat surface on a pillar, and anything walked under then becomes a floating shelf joined to the ground plane by pillars defined by where it cannot walk. Because the robot will only ever experience things it can walk on or under, and motion in between, it makes assumptions about what is in between and just projects what it can see with its camera on it so it looks like a table.
Understanding of a table then is limited to a surface that can be walked on, above another surface that can be walked on, both with obstructions. Obstructions are treated as pillars with possible walking surfaces on top, but extend to the ceiling unless they are walked on. The robot is equipped with an accelerometer and can estimate height when picked up from the floor and placed on a table.
from math import sin,cos,atan as atn,sqrt as sqr # make the maths comfy import pygame,Image # for realtime display from pygame.locals import * from time import sleep pygame.init() # set it up window = pygame.display.set_mode((1024,768)) pygame.display.set_caption("1024x768") screen = pygame.display.get_surface() pi=atn(1.0)*4.0 # pi to machine resolution d2r=pi/180.0 # convert between degrees and radians r2d=180.0/pi # and back ### subroutines ### def test(): # calculate overall size of entire object in mm global coords xs=; ys=; zs= # temporary lists of x y and z coords for c in coords: xs.append(c) # split the lists ys.append(c) zs.append(c) w=int(max(xs)-min(xs)) # find their bounding box dimensions d=int(max(ys)-min(ys)) h=int(max(zs)-min(zs)) pygame.image.save(screen,'lidmesh.png') # and dump a bitmap of the current viewport xs=; ys=; zs= return w,d,h ### def qspoly(plst): # quicksort z coords into order lstlen = len(plst) # number of polys if lstlen <= 1: # less than 2 must be in order already return plst # return it elif lstlen == 2: # if there's only 2 if plst < plst: # and their z coords are in order already return [plst,plst] # return them else: # otherwise return [plst,plst] # swap them else: # and return them pivpt = plst # take the first poly z as a pivot point plst1 =  # make 2 empty lists plst2 =  for lstval in range(1,lstlen): # go through all the z coords if plst[lstval] < pivpt: # if the pivotpoint z is greater plst1.append(plst[lstval]) # put the poly in one list else: # otherwise plst2.append(plst[lstval]) # put it in the other sort1 = qspoly(plst1) # recursively sort the biggers list sort2 = qspoly(plst2) # and the smallers list result = sort1 + [pivpt] + sort2 # then join them all together return result # and return the list ### def angle(sx,sy): # quadratic angle of coords from origin if sx<>0: a=float(atn(abs(float(sy))/abs(float(sx)))*r2d) # angle of hypotenuse: arctan(y/x) if sx>=0 and sy==0: a=0.0 # point lays on origin or to right; return 0 degrees if sx<0 and sy==0: a=180.0 # directly to the left; 180 degrees if sx==0 and sy>0: a=90.0 # directly above; 90 degrees if sx==0 and sy<0: a=270.0 # directly below; 270 degrees if sx>0 and sy>0: a=float(a) # its in quadrant 1,1; 0.n1-89.9n degrees if sx<0 and sy>0: a=float(180-a) # quadrant -1,1; 90.n1-179.9n degrees if sx>0 and sy<0: a=float(360-a) # quadrant 1,-1; 270.n1-359.9n degrees if sx<0 and sy<0: a=float(180+a) # quadrant -1,-1; 180.n1-269.9n degrees return a # return actual angle ### def radius(sx,sy): # get radius of coords from origin r=float(sqr((abs(float(sx))*abs(float(sx)))+(abs(float(sy))*abs(float(sy))))) return r # its pythagorus, sum of the root of the squares def rotate(x,y,z,rs): # rotate a point x,y,z around origin 0,0,0 for ra in rs.split(): # step through list of rotates if ra=='z': # z plane a=angle(x,y)+float(ra[1:]) # slice the xy plane, find angle and add rotate to it r=radius(x,y) # find the radius from 0,0,z x=cos(a*d2r)*r # calculate new x,y,n y=sin(a*d2r)*r if ra=='x': # x plane a=angle(z,y)+float(ra[1:]) # slice the yz plane, find angle and add rotate to it r=radius(z,y) # find the radius from x,0,0 z=cos(a*d2r)*r # calculate new n,x,y y=sin(a*d2r)*r if ra=='y': # y plane a=angle(x,z)+float(ra[1:]) # slice the xz plane, find angle and add rotate to it r=radius(x,z) # find the radius from 0,y,0 x=cos(a*d2r)*r # calculate new x,n,z z=sin(a*d2r)*r return (x,y,z) # return final x,y,z coordinate ### def isfacing(crd): # is polygon facing up (vertices ordered clockwise) for n in range(len(crd)-1): # remove duplicate coordinates if len(crd)>n: if crd.count(crd[n])>=2: crd.remove(crd[n]) if len(crd)>=3: # if there's at least 3 left x1=crd; y1=crd # pick 3 spaced equally round the perimeter x2=crd[len(crd)/3]; y2=crd[len(crd)/3] x3=crd[(len(crd)/3)*2]; y3=crd[(len(crd)/3)*2] area=x1*y2+x2*y3+x3*y1-x2*y1-x3*y2-x1*y3 # find poly area, if its negative its facing down return (area/2>=0) # return that, dont need the actual area else: return 0 # otherwise its not a polygon ### def buildlid(sw,sh,r): coords= # list of vertices polys= # list of polygons rot=['z113.5 x50 z45','z22.5 y-50 z45','z292.5 x-50 z45','z202.5 y50 z45'] # corner rotations minz=0 sws=[-((sw/2)+r),(sw/2)+r,(sw/2)+r,-((sw/2)+r)] # width of side between centres of rotation shs=[(sh/2)+r,(sh/2)+r,-((sh/2)+r),-((sh/2)+r)] # height of side between centres spr=360.0/16 # spread the points around a full circle for s in range(4): # make 4 corners for a in range(16): # with 16 segments each around the edge x=cos(a*spr*d2r)*r # make a circle round the origin y=sin(a*spr*d2r)*r # on the z axis z=0 # on ground plane so origin is at the top x,y,z=rotate(x,y,z,rot[s]) # rotate the points in 3d to match the corner angles coords.append([x+sws[s],y+shs[s],z]) # add coordinates to vertex list for a in range(16): # another 16 segments inside x=cos(a*spr*d2r)*(r/2) # make a concentric circle half the size y=sin(a*spr*d2r)*(r/2) z=-r/2.0 # above the first, again in the z axis x,y,z=rotate(x,y,z,rot[s]) # rotate the points in 3d coords.append([x+sws[s],y+shs[s],z]) # add coordinates to vertex list x=0 # make a single point at the centre y=0 z=r/8 # slightly above the rings x,y,z=rotate(x,y,z,rot[s]) # rotate it into place in 3d coords.append([x+sws[s],y+shs[s],z]) # add it to the coords list if z<minz: minz=z for n in range(16): # now polygons, these point to the actual coords a=n; b=n+1; c=n+17; d=n+16 # point at the vertices of ring 1 and 2 if b>15: b=b-16 # its a grid; wrap the edges around if c>31: c=c-16 if d>31: d=d-16 p=[a+(s*33),b+(s*33),c+(s*33),d+(s*33)] # point at correct corner vertices polys.append([p,n<16]) # and add the poly to the list for n in range(16): # second ring a=n+16; b=n+17; c=n+33; d=n+32 # points at vertices of ring 2 and centre if b>31: b=b-16 # wrap the edges around if c>31: c=32 if d>31: d=32 p=[a+(s*33),b+(s*33),c+(s*33),d+(s*33)] # point at correct corner vertices polys.append([p,n<16]) # add poly to the list polys.append([[40,7,106,73],True]) # add central square for n in range(6): # add surrounding polygons between the circles polys.append([[7+n,40-n,39-n,8+n],True]) # they are all clockwise polys.append([[40+n,73-n,72-n,41+n],True]) # so work out where they are from centre polys.append([[73+n,106-n,105-n,74+n],True]) polys.append([[106+n,7-n,6-n,107+n],True]) lc=len(coords) # make a note of the coords to point to for c in range(4): # four corner circles for n in range(5): # five points intersect with rim v=n # but they arent arranged in a decent order so if n>1: v=n+11 # bodge the order of the points in the sector ev=v+(c*33) # for each corner x,y,z=coords[ev] # find the actual coordinates coords.append([x,y,minz-50]) # copy the rim coordinates lower to make an edge rim=[[1,0,0,1],[0,15,4,0],[15,14,3,4],[14,13,2,3]] # bodge the new rim coordinates to the old ones for a,b,c,d in rim: # pick up a set of corners polys.append([[a,b,lc+c,lc+d],True]) # and make a polygon from them polys.append([[a+33,b+33,lc+c+5,lc+d+5],True]) # echo for the other corner pieces polys.append([[a+66,b+66,lc+c+10,lc+d+10],True]) polys.append([[a+99,b+99,lc+c+15,lc+d+15],True]) polys.append([[13,34,lc+6,lc+2],True]) # and fill in the gaps to make complete sides polys.append([[46,67,lc+11,lc+7],True]) polys.append([[79,100,lc+16,lc+12],True]) # bodge... polys.append([[112,1,lc+1,lc+17],True]) polys.append([[lc+12,lc+16,lc+15,lc+13],True]) # bridge across the front edge to close the base polys.append([[lc+13,lc+15,lc+19,lc+14],True]) polys.append([[lc+14,lc+19,lc+18,lc+10],True]) polys.append([[lc+10,lc+18,lc+17,lc+11],True]) polys.append([[lc+11,lc+17,lc+1,lc+7],True]) # fill in the middle polys.append([[lc+7,lc+1,lc+0,lc+8],True]) # and bridge across the back to close that too polys.append([[lc+8,lc+0,lc+4,lc+9],True]) polys.append([[lc+9,lc+4,lc+3,lc+5],True]) # bodge... polys.append([[lc+5,lc+3,lc+2,lc+6],True]) lc=len(coords) # make a note of the coords to point to c= # make containers to hold some measurements c.append([[78,74,0],[78,74,0],[39,85,0],[0,90,0],[-39,85,0],[-78,74,0],[-78,74,0]]) # behold my shiny c.append([[48,34,0],[30,41,10],[14,44,10],[0,45,10],[-14,44,10],[-30,41,10],[-48,34,0]]) # quadratic c.append([[42,18,0],[16,25,12],[9,25,12],[0,26,12],[-9,25,12],[-16,25,12],[-42,18,0]]) # metal c.append([[39,0,0],[7,0,14],[3,0,14],[0,0,14],[-3,0,14],[-7,0,14],[-39,0,0]]) # ass-plate bodge ;) for a in range(4): # iterate the containers for n in range(7): # iterate the vertices in the containers x,y,z=c[a][n] # make the top half coords.append([-x,y,minz-50-z]) # and write it into the coords pool for a in range(3,0,-1): # make the bottom half for n in range(7): x,y,z=c[a-1][n] coords.append([-x,-y,minz-50-z]) # write it to the pool for t in range(6): # 7 rows of vertices, 6 columns of polys for n in range(6): # 7 columns of vertices polys.append([[lc+n+(t*7),lc+n+(t*7)+1,lc+n+(t*7)+8,lc+n+(t*7)+7],True]) # write the polys return polys,coords # and return the complete orthogonal set ### def renderise(msh,verts,ctr,agl): # build a list of 2d polygons from 3d data global origins,scl # pick up the positions of the objects render= # make a blank scene for i in range(len(mesh)): # go through all the objects for m in msh[i]: # go through all the polys in the object az=0 # centroid position crd= # blank list of 2d coordinates for pygame.draw.poly() for c in range(4): # 4 corners for hybrid 'bitmesh' polygons x,y,z=verts[m[c]] # get the coordinates of the corners x=x+origins[i] y=y+origins[i] z=z+origins[i] x,y,z=rotate(x,y,z,agl) # rotate them into 3d space persp=(z+1000)/1000.0 # calculate perspective distortion x=x*persp*scl; y=y*persp*scl # and apply it to each vertex crd.append([x+ctr,y+ctr]) az=az+z az=az/4.0 # find the average z of the four (the centroid) if isfacing(crd): # if its facing towards camera (up or +z) render.append([az,crd]) # render it return render def savemesh(fil,i): # save the submesh as a modified wavefront format file global mesh,coords,origins,axes # pick up the entire set f=open(fil,'w') # make a file f.write('#cardware object\n\n') # make a header f.write('#parameters\n') # write out metadata for n in range(len(axes[i])): # angles of the legs are held in a chain from foot to foot f.write('a '+str(axes[i][n])+','+str(axes[i][n])+','+str(axes[i][n])+','+str(axes[i][n])+'\n') f.write('\no '+str(origins[i])+','+str(origins[i])+','+str(origins[i])+'\n') f.write('\n#vertices\n') # write out vertex data minp=mesh[i] maxp=mesh[i] # first polygon corner vertex in submesh for p in mesh[i]: # find the all others for m in p: if m<minp: minp=m # determine the range within the coords block if m>maxp: maxp=m print minp,maxp for c in range((maxp-minp)+1): # select just submesh vertices from the master vertex list x,y,z=coords[c+minp] f.write('v '+str(x)+', '+str(y)+', '+str(z)+'\n') # and write them to the file f.write('\n#faces\n') for p in mesh[i]: # now go through polygons pointing to those vertices a,b,c,d=p # pick up the corner vertices if p: # write out the modified vertex pointers f.write('f '+str(a-minp)+', '+str(b-minp)+', '+str(c-minp)+', '+str(d-minp)) # if its drawn write the polygon else: # otherwise f.write('#f '+str(a-minp)+', '+str(b-minp)+', '+str(c-minp)+', '+str(d-minp)) # just annotate it in the file f.write('\n') f.close() def loadmesh(fil,i): # load a mesh into the coord and polygon blocks global mesh,coords,axes,origins,cx,cy # pick up mesh structures imesh=; icoords=; iaxes=; iorigs= # make new blank ones lc=len(coords) # make a note of the current vertex list length f=open(fil,'r') # open the file lst=list(f) # load it into a list f.close() # close the file icx=cx; icy=cx; icz=0 # new instance defaults to screen center minx=0; miny=0; minz=0 maxx=0; maxy=0; maxz=0 if '#cardware object\n' in lst: # make sure its a cardware mesh err=False # check syntax centre=False # use quadratic cartesian coordinates for l in lst: # go through lines one by one if l.strip()!='': # if its not a blank line if '#' in l: l=l.strip().split('#').strip() # remove annotations if l>'': # if its still not blank if l=='v': # if its a vertex line l=l[1:].strip().split(',') # remove the preamble and snap it at commas x=float(l) # and read it as three floating point values y=float(l) z=float(l) if x<minx: minx=x if y<miny: miny=y if z<minz: minz=z if x>maxx: maxx=x if y>maxy: maxy=y if z>maxz: maxz=z icoords.append([x,y,z]) # store it temporarily elif l=='f': # otherwise, if its a face line l=l[1:].strip().split(',') # snap it at commas a=int(l)+lc # read the pointer and modify it to point past b=int(l)+lc # the existing coords c=int(l)+lc d=int(l)+lc imesh.append([[a,b,c,d],True]) # store it temporarily elif l=='o': # otherwise if its an origin; initial position in map l=l[1:].strip().split(',') # snap it up icx=float(l) # and make a note of it icy=float(l) icz=float(l) elif l=='a': # otherwise if its an articulation l=l[1:].strip().split(',') # snap it up a=int(l) # get the id of the part and articulation it attaches to x=float(l) # and the relative coords of the centre of rotation y=float(l) z=float(l) iaxes.append([a,[0,0,0],[x,y,z]]) # temporarily store the id, angles and position elif l=='c': # otherwise is a flag to say data is topleft or quadratic centre=True else: # otherwise its not supposed to be there print'** warning ** malformed file **' err=True # so give up on load without modifying memory else: print '** not a cardware file **' # otherwise, cant even see a proper format err=True if not err: # providing all went to plan while len(mesh)<=i: mesh.append() axes.append() origins.append() for c in icoords: x,y,z=c if centre: x=x-((maxx-minx)/2.0) y=y-((maxy-miny)/2.0) z=z+((maxz-minz)/2.0) coords.append([x,y,z]) # append the new coords to the end of the coord block for p in imesh: mesh[i].append(p) # add the new polygon pointers to the mesh block for a in iaxes: axes[i].append(a) # attach the new part to the parts chain origins[i]=[icx,icy,icz] # and give it an initial reference position ### main routine ### cx=512; cy=384 # centre coords of screen r=43; sw=25; sh=80 # morningstar template radius, side lengths render= coords= mesh= axes= origins= #msh,coords=buildlid(sw,sh,r) # build the lid #mesh.append(msh) #savemesh('body.msh',0) loadmesh('body.msh',0) loadmesh('thigh.msh',1) loadmesh('thigh.msh',2) loadmesh('hipmodule.msh',3) loadmesh('hipmodule.msh',4) loadmesh('hipunit.msh',5) loadmesh('hipunit.msh',6) loadmesh('hipunit.msh',7) loadmesh('hipunit.msh',8) loadmesh('foot.msh',9) loadmesh('foot.msh',10) origins=[-300,0,30] origins=[-100,0,0] origins=[-50,0,0] origins=[-100,100,-17] origins=[-50,100,-17] origins=[25,0,0] origins=[100,0,0] origins=[25,100,0] origins=[100,100,0] origins=[-100,-150,0] origins=[50,-150,0] chg=False # display changed flag xa=-90 # rotation of entire scene round origin ya=0 scl=2.0 rend=qspoly(renderise(mesh,coords,(cx,cy),'z'+str(ya)+' x&ap