def setLegsXYZG(self, legXYZ, num=2):
"move legs to their relative XYZ coordinates"
assert len(legXYZ) == 6, legXYZ
abc = (0.0525, 0.0802, 0.1283)
servoDirs = (1, -1, 1, 1, -1, 1, 1, -1, 1, -1, 1, -1, -1, 1, -1, -1, 1, -1, 1, 1, 1, 1, 1)
angles = []
for xyz in legXYZ:
angles.extend(pos2angles10thDeg(xyz, abc=abc))
angles += [0, 0, 0, 0, 0] # Head & Pincers
cmd = [angle * servoDir + offset for angle, servoDir, offset in zip(angles, servoDirs, self.servoOffset)]
if STOP_SERVO in self.lastCmd:
prev = cmd[:]
else:
prev = self.lastCmd
# undo offset and direction
prev = [(angle - offset) * servoDir for angle, servoDir, offset in zip(prev, servoDirs, self.servoOffset)]
prevLegXYZ = []
for angles in zip(prev[0:-5:3], prev[1:-5:3], prev[2:-5:3]):
prevLegXYZ.append(angles10thDeg2pos(angles, abc=abc))
for i in xrange(num):
angles = []
for xyz1, xyz2 in zip(prevLegXYZ, legXYZ):
xyz = [((i + 1) * new + (num - 1 - i) * old) / float(num) for old, new in zip(xyz1, xyz2)]
angles.extend(pos2angles10thDeg(xyz, abc=abc))
angles += [0, 0, 0, 0, 0] # Head & Pincers
cmd = [angle * servoDir + offset for angle, servoDir, offset in zip(angles, servoDirs, self.servoOffset)]
yield cmd
def setLegsXYZG( self, legXYZ, num=20 ):
"move legs to their relative XYZ coordinates"
assert len(legXYZ) == 6, legXYZ
abc=(0.0525, 0.0802, 0.1283)
servoDirs = (1,-1,1, 1,-1,1, 1,-1,1, -1,1,-1, -1,1,-1, -1,1,-1, 1,1,1,1,1 )
angles = []
for xyz in legXYZ:
angles.extend( pos2angles10thDeg( xyz, abc=abc ) )
angles += self.headAnglesCmd # Head & Pincers
cmd = [angle*servoDir+offset for angle, servoDir, offset in zip(angles, servoDirs, self.servoOffset)]
if STOP_SERVO in self.lastCmd:
prev = cmd[:]
else:
prev = self.lastCmd
# undo offset and direction
prev = [(angle-offset)*servoDir for angle, servoDir, offset in zip(prev, servoDirs, self.servoOffset)]
prevLegXYZ = []
for angles in zip(prev[0:-5:3], prev[1:-5:3], prev[2:-5:3]):
prevLegXYZ.append( angles10thDeg2pos( angles, abc=abc ) )
triggerZ = [None]*6
for i in xrange(num):
forces = [f*d for f,d in zip(self.servoForceRaw, servoDirs)]
angles = []
for xyz1, xyz2,trig in zip(prevLegXYZ, legXYZ,enumerate(triggerZ)):
xyz = [((i+1)*new+(num-1-i)*old)/float(num) for old, new in zip(xyz1,xyz2)]
if trig[1] != None:
xyz[2] = trig[1]
elif self.forceLimit != None and forces[1+trig[0]*3] > self.forceLimit and xyz1[2]>xyz2[2]:
# i.e. trigger enabled and leg is going down
triggerZ[trig[0]] = xyz[2]
print triggerZ
angles.extend( pos2angles10thDeg( xyz, abc=abc ) )
angles += [0,0,0,0,0] # Head & Pincers
cmd = [angle*servoDir+offset for angle, servoDir, offset in zip(angles, servoDirs, self.servoOffset)]
yield cmd
