private Matrix[] CalculateTransformation()
{
var F0 = new ArmOrientation(Vector3.Right, Vector3.Up, Vector3.Backward, Vector3.Zero);
var F01 = Matrix.CreateFromAxisAngle(F0.AlphaY, Angle1);
Matrix F1 = F01 * F0.ToMatrix();
//var F12 = Matrix.CreateTranslation(Vector3.Up * Length1) * Matrix.CreateFromAxisAngle(F1.Forward, Angle2); //here
//Matrix F2 = F1 * F12;
Matrix F2 = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) *
F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1);
//var F23 = Matrix.CreateTranslation(F2.Up * Length2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3);
//Matrix F3 = F2 * F23;
Matrix F3 = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3) *
F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1) * Matrix.CreateTranslation(F2.Up * Length2); //ok
//var F34 = Matrix.CreateTranslation(Vector3.Up * -Length3) * Matrix.CreateFromAxisAngle(F3.Up, Angle4);
//Matrix F4 = F3 * F34;
Matrix F4 = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3) *
Matrix.CreateFromAxisAngle(F3.Forward, (float)(2 * Math.PI - Math.PI / 2)) * Matrix.CreateFromAxisAngle(F3.Up, -Angle4) *
F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1) * Matrix.CreateTranslation(F2.Up * Length2) * Matrix.CreateTranslation(F3.Up * Length3);
//var F45 = Matrix.CreateTranslation(Vector3.Right * Length4) * Matrix.CreateFromAxisAngle(F4.Right, Angle5);
//Matrix F5 = F4 * F45;
Matrix F5 = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3) *
Matrix.CreateFromAxisAngle(F3.Forward, (float)(2 * Math.PI - Math.PI / 2)) * Matrix.CreateFromAxisAngle(F3.Up, -Angle4) * Matrix.CreateFromAxisAngle(F4.Up, Angle5) *
F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1) * Matrix.CreateTranslation(F2.Up * Length2) * Matrix.CreateTranslation(F3.Up * Length3) * Matrix.CreateTranslation(F4.Up * Length4);
Matrix EndPoint = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3) *
Matrix.CreateFromAxisAngle(F3.Forward, (float)(2 * Math.PI - Math.PI / 2)) * Matrix.CreateFromAxisAngle(F3.Up, -Angle4) * Matrix.CreateFromAxisAngle(F4.Up, Angle5) *
F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1) * Matrix.CreateTranslation(F2.Up * Length2) * Matrix.CreateTranslation(F3.Up * Length3) * Matrix.CreateTranslation(F4.Up * Length4) * Matrix.CreateTranslation(F5.Up * Length5);
Matrix[] result = { F1, F2, F3, F4, F5, EndPoint };
return(result);
}
private Configuration CalculateAnglesFromEndPosition(ArmOrientation endOrientation)
{
//check for Asin = NaN (3 places)
//take best option from angle4
//chaneck ranges
//flipping z axis to match puma cooedinate system -> TODO: move it to QuaternionRotation in Cylinder
endOrientation.Position.Z *= -1;
Configuration angles = new Configuration();
/******* angle 1 & 4 **********/
angles.Angle1 = (float)Math.Atan2(endOrientation.Position.Z - Length4 * endOrientation.AlphaX.Z, endOrientation.Position.X - Length4 * endOrientation.AlphaX.X);
angles.Angle4 = (float)Math.Asin(Math.Cos(angles.Angle1) * endOrientation.AlphaX.Z - Math.Sin(angles.Angle1) * endOrientation.AlphaX.X); //1 or 2 sol for given a1
//sec sol
float angle4a = ((Angle4 > 0) ? (float)Math.PI - Angle4 : -(float)Math.PI + Angle4);
/********* angle 5 ***********/
var aCos = (Math.Cos(angles.Angle1) * endOrientation.AlphaZ.Z - Math.Sin(angles.Angle1) * endOrientation.AlphaZ.X) / (Math.Cos(angles.Angle4));
if (aCos > 1)
{
aCos = 1;
}
if (aCos < -1)
{
aCos = -1;
}
var Angle5a = (float)Math.Acos(aCos); //uniq
float angle5a2 = -Angle5a;
var Angle5b = (float)Math.Asin((Math.Sin(angles.Angle1) * endOrientation.AlphaY.X - Math.Cos(angles.Angle1) * endOrientation.AlphaY.Z) / (Math.Cos(angles.Angle4)));
float angle5b2 = ((Angle5b > 0) ? (float)Math.PI - Angle5b : -(float)Math.PI - Angle5b);
if (Angle5b > 0)
{
if (Angle5a > 0)
{
angles.Angle5 = Angle5a;
}
else
{
angles.Angle5 = angle5a2;
}
}
else
{
if (Angle5a < 0)
{
angles.Angle5 = Angle5a;
}
else
{
angles.Angle5 = angle5a2;
}
}
/********* angle 2 and length ***********/
angles.Angle2 = (float)Math.Atan2(-(Math.Cos(angles.Angle1) * Math.Cos(angles.Angle4) * (endOrientation.Position.Y - Length4 * endOrientation.AlphaX.Y - Length1) + Length3 * (endOrientation.AlphaX.X + Math.Sin(angles.Angle1) * Math.Sin(angles.Angle4))),
Math.Cos(angles.Angle4) * (endOrientation.Position.X - Length4 * endOrientation.AlphaX.X) - Math.Cos(angles.Angle1) * Length3 * endOrientation.AlphaX.Y);
angles.Length2 = (float)((Math.Cos(angles.Angle4) * (endOrientation.Position.X - Length4 * endOrientation.AlphaX.X) - Math.Cos(angles.Angle1) * Length3 * endOrientation.AlphaX.Y)
/ (Math.Cos(angles.Angle1) * Math.Cos(angles.Angle2) * Math.Cos(angles.Angle4)));
/********* anglee 3 *******************/
float angle23;
aCos = (endOrientation.AlphaX.X + (Math.Sin(angles.Angle1) * Math.Sin(angles.Angle4))) / (Math.Cos(angles.Angle1) * Math.Cos(angles.Angle4));
if (aCos > 1)
{
aCos = 1;
}
if (aCos < -1)
{
aCos = -1;
}
var angle23Cos = (float)Math.Acos(aCos);
var angle23Cos2 = -angle23Cos;
var angle23Sin = (float)Math.Asin(-(endOrientation.AlphaX.Y) / Math.Cos(angles.Angle4));
var angle23Sin2 = (angle23Sin > 0) ? (float)Math.PI - angle23Sin : -(float)Math.PI - angle23Sin;
if (angle23Sin > 0)
{
if (angle23Cos > 0)
{
angle23 = angle23Cos;
}
else
{
angle23 = angle23Cos2;
}
}
else
{
if (angle23Cos < 0)
{
angle23 = angle23Cos;
}
else
{
angle23 = angle23Cos2;
}
}
angles.Angle3 = angle23 - angles.Angle2;
/*var Angle3a = (float)Math.Acos((endOrientation.AlphaX.X + Math.Sin(angles.Angle1) * Math.Sin(angles.Angle4)) / (Math.Cos(angles.Angle1) * Math.Cos(angles.Angle4)))
* - angles.Angle2; //uniq
* //float angle3a2 = ((Angle3a < 0) ? Angle3a + (float)Math.PI : Angle3a - (float)Math.PI);
* angles.Angle3 = (float)Math.Asin(-endOrientation.AlphaX.Y / Math.Cos(angles.Angle4)) - angles.Angle2;
*
*/
//+ start position
angles.Angle2 += (float)Math.PI / 2;
angles.Angle3 += (float)Math.PI / 2;
//range check
if (angles.Angle1 <= -(float)Math.PI)
{
angles.Angle2 += 2 * (float)Math.PI;
}
if (angles.Angle2 <= -(float)Math.PI)
{
angles.Angle2 += 2 * (float)Math.PI;
}
if (angles.Angle3 <= -(float)Math.PI)
{
angles.Angle2 += 2 * (float)Math.PI;
}
if (angles.Angle4 <= -(float)Math.PI)
{
angles.Angle2 += 2 * (float)Math.PI;
}
if (angles.Angle5 <= -(float)Math.PI)
{
angles.Angle2 += 2 * (float)Math.PI;
}
if (angles.Angle1 > (float)Math.PI)
{
angles.Angle2 -= 2 * (float)Math.PI;
}
if (angles.Angle2 > (float)Math.PI)
{
angles.Angle2 -= 2 * (float)Math.PI;
}
if (angles.Angle3 > (float)Math.PI)
{
angles.Angle2 -= 2 * (float)Math.PI;
}
if (angles.Angle4 > (float)Math.PI)
{
angles.Angle2 -= 2 * (float)Math.PI;
}
if (angles.Angle5 > (float)Math.PI)
{
angles.Angle2 -= 2 * (float)Math.PI;
}
return(angles);
}