Exemplo n.º 1
0
    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
Exemplo n.º 2
0
  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