Janee... 
Mit den Gleichungen hast du so gesehen Recht, aber die IK ist eben nicht sooo anspruchsvoll, weil es eben nicht mehrere Möglichkeiten gibt. Es gibt meistens 2 Möglichkeiten, in dem man das Knie nach hinten oder vorne knickt, aber es gibt keine Lösungsmengen.
Den Link hab ich oben ja schon gepostet: https://www.robotshop.com/community/...nematics/11759 ...aber da sieht man, wie man sukzessive die 3 Winkel der Servos ausrechnen kann. Und wie man auch sieht - es sind nur 3 Formeln und fertich!
Bei mir sieht das so aus:
Code:
def moveFeetAbsolute(self, id, p):
error = 0
self.tmp[0] = (p - self.anubis['position']) * self.Rt # Die Position der Base wird abgezogen und um die Lage von Anubis zurückgedreht.
p = self.tmp[0] - self.npLegBase[id] # Die LegBase wird ebenfalls abgezogen, damit das Bein im 0-Punkt startet
# In p ist nun der Punkt gespeichert, der vom 0-Punkt aus über die 3 Gelenke j1 - j3 erreicht werden soll.
j1 = math.atan2(p[1], -p[2]) * self.rad2deg
lengthYZ = math.sqrt(p[1] * p[1] + p[2] * p[2])
t = math.sqrt(p[0] * p[0] + lengthYZ * lengthYZ) / self.legLength
if t > 1.0:
t = 1.0
error |= 1
if t < -1.0:
t = -1.0
error |= 1
gamma = 2 * math.asin(t) # Winkel des Ellenbogens
alpha = (self.deg180_2rad - gamma) / 2
j2 = (alpha - math.atan2(p[0], lengthYZ)) * self.rad2deg
j3 = (gamma - self.deg180_2rad) * self.rad2deg
return self.defineServoGoalPosition(id, error, j1, j2, j3)
Grüße, Marcus
Lesezeichen