static void Main(string[] args)
{
List <IJoint> joints = Robot6DOF.Get6DOFRobot(2, 2, 2, 2, 2, 2);
Transform3D base_axis = new Transform3D(new Vector3F(0, 0, 0), new Vector3F(0, 0, 0));
Transform3D tf3d = base_axis;
tf3d.RotationMat = Matrix3F.GetUnitMatrix();
joints[0].Transform = new Vector3F(0, 0, Math.PI * 4 / 6);
joints[1].Transform = new Vector3F(0, Math.PI * 5 / 6, 0);
joints[2].Transform = new Vector3F(0, Math.PI / 6, 0);
joints[3].Transform = new Vector3F(0, 0, Math.PI / 6);
joints[4].Transform = new Vector3F(0, Math.PI / 3, 0);
joints[5].Transform = new Vector3F(0, 0, Math.PI / 4);
foreach (IJoint jo in joints)
{
tf3d = jo.GetEndPointState(tf3d);
}
Console.WriteLine("6DOF");
Vector3F2 transform = Robot6DOF.Inverse6DOF(ref joints, tf3d);
Console.WriteLine("The endpoint position is {0}, euler rotation is {1}", tf3d.Position - base_axis.Position, tf3d.Rotation);
Console.WriteLine("Robot Joint Transform: {0}", transform.ToString());
Console.ReadLine();
}
public static Vector3F2 Inverse6DOF(ref List <IJoint> robot, Transform3D transform)
{
double l1, l2, l3, l4;
l1 = robot[0].Length;
l2 = robot[1].Length;
l3 = robot[2].Length + robot[3].Length;
l4 = robot[4].Length + robot[5].Length;
Vector3F vec6z = transform.RotationMat.Col[2];
Vector3F posJ4end = transform.Position - l4 * vec6z;
Vector3F2 result = new Vector3F2();
{
Vector3F tvec = posJ4end;
double y = tvec * new Vector3F(0, 1, 0);
double x = tvec * new Vector3F(1, 0, 0);
result[0] = Math.Atan2(y, x);
result[0] = result[0] + Vector3F.CompareEPS < 0 ? result[0] + Math.PI * 2 : result[0];
robot[0].Transform = new Vector3F(0, 0, result[0]);
}
{
Vector3F tvec = posJ4end - l1 * new Vector3F(0, 0, 1);
double ll2 = l2 * l2 + l3 * l3;
double csc = (tvec.Length * tvec.Length - ll2) / (2 * l2 * l3);
csc = Math.Round(csc, 12);
result[2] = Math.Acos(csc);
robot[2].Transform = new Vector3F(0, result[2], 0);
}
{
double A = Math.Sqrt(Math.Pow(l3 * Math.Sin(result[2]), 2) + Math.Pow(l3 * Math.Cos(result[2]) + l2, 2));
double asc = (posJ4end.Z - l1) / A;
asc = Math.Round(asc, 12);
double t0 = Math.Asin(asc);
double t1 = Math.Acos((l2 * l2 + A * A - l3 * l3) / (2 * l2 * A));
result[1] = Math.PI / 2 - t0 - t1;
robot[1].Transform = new Vector3F(0, result[1], 0);
}
Transform3D base_axis = new Transform3D(new Vector3F(0, 0, 0), new Vector3F(0, 0, 0));
Transform3D tf3d = base_axis;
for (int i = 0; i < 3; i++)
{
tf3d = robot[i].GetEndPointState(tf3d);
}
Vector3F vec3z = tf3d.RotationMat.Col[2];
Vector3F vec3y = tf3d.RotationMat.Col[1];
Vector3F vec3x = tf3d.RotationMat.Col[0];
Vector3F endUnit = vec6z;
result[4] = Math.Acos(vec6z * vec3z);
robot[4].Transform = new Vector3F(0, result[4], 0);
result[3] = Math.Atan2(vec6z * vec3y, vec6z * vec3x);
result[3] = result[3] + Vector3F.CompareEPS < 0 ? result[3] + Math.PI * 2 : result[3];
robot[3].Transform = new Vector3F(0, 0, result[3]);
for (int i = 3; i < 5; i++)
{
tf3d = robot[i].GetEndPointState(tf3d);
}
Vector3F vec5y = tf3d.RotationMat.Col[1];
Vector3F vec5x = tf3d.RotationMat.Col[0];
Vector3F vec6x = transform.RotationMat.Col[0];
result[5] = Math.Atan2(vec6x * vec5y, vec6x * vec5x);
result[5] = result[5] + Vector3F.CompareEPS < 0 ? result[5] + Math.PI * 2 : result[5];
robot[5].Transform = new Vector3F(0, 0, result[5]);
return(result);
}
