Здрасти Николай,
Да ти давам кода е безсмислено тъй като представлява 10-15 реда изчисления, които както ги гледам в момента са ми тъмна Индия. Но все пак за да видиш резултата от смятанията ето:
public void TransformCoord(Leg leg,out double XTrans,out double YTrans,out double ZTrans)
{
double SinX = Math.Sin(m_currentPitchAngle);
double SinY = Math.Sin(m_currentTiltAngle);
double SinZ = Math.Sin(leg.Rotate?m_currentRotateAngle:0);
double SinJ = Math.Sin(leg.MAngleFromBodyCenter);
double CosX = Math.Cos(m_currentPitchAngle);
double CosY = Math.Cos(m_currentTiltAngle);
double CosZ = Math.Cos(leg.Rotate?m_currentRotateAngle:0);
double CosJ = Math.Cos(leg.MAngleFromBodyCenter);
int offx = leg.Offset ? m_currentXoffset.Value : 0;
int offy = leg.Offset ? m_currentYoffset.Value : 0;
int offz = m_currentZoffset.Value;
double a = CosY * SinJ * SinX - CosJ * SinY;
double a1 = SinJ * (CosX * CosZ + CosZ * SinX * SinY) + CosJ * CosY * SinZ;
double a2 = CosJ * CosY * CosZ + SinJ * (CosZ * SinX * SinY - CosX * SinZ);
double b = CosJ * CosY * SinX + SinJ * SinY;
double b1 = CosJ * (CosX * CosZ + CosZ * SinX * SinY) - CosY * SinJ * SinZ;
double b2 = -CosY * CosZ * SinJ + CosJ * (CosZ * SinX * SinY - CosX * SinZ);
double c = offx + leg.MDecX1 * CosY * CosZ + leg.MDecY1 * (CosZ * SinX * SinY - CosX * SinZ);
double c1 = -offz - leg.MDecY1 * CosY * SinX + leg.MDecX1 * SinY;
double c2 = offy + leg.MDecY1 * (CosX * CosZ + CosZ * SinX * SinY) + leg.MDecX1 * CosY * SinZ;
double c3 = -CosZ * SinX + CosX * CosZ * SinY;
double c4 = CosX * CosZ * SinY + SinX * SinZ;
XTrans = a * (leg.CurrentZ + c1) + a1 * (leg.CurrentY - c2) + a2 * (leg.CurrentX - c);
YTrans = b * (leg.CurrentZ + c1) + b1 * (leg.CurrentY - c2) + b2 * (leg.CurrentX - c);
ZTrans = CosX * CosY * (leg.CurrentZ + c1) + c3 * (leg.CurrentY - c2) + c4 * (leg.CurrentX - c);
}
public void CalculateAngles()
{
double X;
double Y;
double Z;
double K;
//transform body coordinates to leg coordinates
BodyMovement.Instance().TransformCoord(mCurrentLeg,out X,out Y,out Z);
//get coxa angle
Leg.CoxaAngle = Atan2(Y, X);
K = Leg.MCoxaLenght - X * Cos(Leg.CoxaAngle) - Y * Sin(Leg.CoxaAngle);
//get tibia angle
double CosTibia = (Math.Pow(K,2) +Math.Pow(Z,2) - Math.Pow(Leg.MFemurLenght,2) -Math.Pow(Leg.MTibiaLenght,2))/(2*Leg.MFemurLenght*Leg.MTibiaLenght);
Leg.TibiaAngle = Atan2(Sqrt(1 - Math.Pow(CosTibia, 2)), CosTibia);
//get femur angle
double f1 = Leg.MFemurLenght + Leg.MTibiaLenght * Cos(Leg.TibiaAngle);
double f2 = -Leg.MTibiaLenght * Sin(Leg.TibiaAngle);
Leg.FemurAngle = Atan2(f1,f2) - Atan2(Sqrt(Math.Pow(f1, 2) + Math.Pow(f2, 2) - Math.Pow(Z, 2)),Z);
}
Това са двете най- важни функции за изчисленията: Първата трансформира координатите на върха на крака зададени първоначално с точка от координатната система на тялото(с начална точка центъра на тялото) в точка от координатната система на дадения крак(с начална точка лежаща на оста на кокса сервото). Втората функция спрямо тази точка изчислява ъглите на крака.
Наистина е трудно да се намери добро обяснение за изчисляването на трансформациите и от там ъглите. За мен най- полезни бяха следните неща:
http://www.youtube.com/watch?v=0yD3uBshJB0 - това е лекция от станфордския университет, гледах ги до към part 6 - по нататък почва да се говори за ускорения и тн., което мен не ме интересуваше в случая. Също така на сайта им има лекциите и упражненията от този курс.
http://www.intechopen.com/source/pdfs/379/InTech-Robot_kinematics_forward_and_inverse_kinematics.pdf - след като изгледах по няколко пъти лекциите това най- много ми помогна тъй като има няколко реални примера.
Като стъпки какво трябва да се направи, за да изразиш инверсната кинематика на робота си:
1) описваш кракът на робота с помощта на D-H(Denavit-Hartenberg) параметри
2) с тяхна помощ намираш трансформиращите матрици
3) извършваш различни трансформации, за да можеш да изразиш ъглите на връзките(сервотата).
Ако си по- наясно с матриците и трансформациите мисля, че ще ти по- лесно. Аз никога не съм си обяснявал за какво по дяволите са ми тези матрици въпреки бакалавърската ми степен, но ето че се наложи и наистина става много елегантно
Почети и гледай лекциите; ако ти е необходима помощ пиши тук и ще се опитам да помогна.
ПС. Другият вариант(първоначалната ми имплементация) е чисто геометрично решение, но това решение ще ти създаде много проблеми в бъдеще заради грешки в изчисленията и тригонометрични граници.
