private void RefreshLabels()
{
tuple p = r.GetPosition();
lX.Text = round((p.t1), 4).ToString();
lY.Text = round(p.t2, 4).ToString();
lZ.Text = round(p.t3, 4).ToString();
lA.Text = round(p.t4 / dToR, 4).ToString();
lB.Text = round(p.t5 / dToR, 4).ToString();
lC.Text = round(p.t6 / dToR, 4).ToString();
// MessageBox.Show("CLR MessageBox", round((p.t1), 4).ToString(), MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
}
//Given position and orientation, calculate the angles of the motors and draw robot consequently
public tuple MoveToPositions(double x, double y, double z, double a, double b, double c)
{
tuple r = inverse(x, y, z, a, b, c);
draw(r.t1, r.t2, r.t3, r.t4, r.t5, r.t6);
_t1 = r.t1;
_t2 = r.t2;
_t3 = r.t3;
_t4 = r.t4;
_t5 = r.t5;
_t6 = r.t6;
return(r);
}
//Return the actual position and orientation of the top of the arm6
public tuple GetPosition()
{
double nx;
double ny;
double nz;
double sx;
double sy;
double sz;
double ax;
double ay;
double az;
double par = 30 * pi / 180;
double s1;
double c1;
double s2;
double c2;
double s3;
double c3;
double s23;
double c23;
double s4;
double c4;
double s5;
double c5;
double s6;
double c6;
s1 = Math.Sin(_t1);
s2 = Math.Sin(_t2);
s3 = Math.Sin(_t3);
s23 = Math.Sin(_t2 + _t3);
s4 = Math.Sin(_t4);
s5 = Math.Sin(_t5);
s6 = Math.Sin(_t6);
c1 = Math.Cos(_t1);
c2 = Math.Cos(_t2);
c3 = Math.Cos(_t3);
c23 = Math.Cos(_t2 + _t3);
c4 = Math.Cos(_t4);
c5 = Math.Cos(_t5);
c6 = Math.Cos(_t6);
//nx = ((s1*s4 - c1*s23*c4)*c5 - c1*c23*s5)*s6 + (-c1*s23*s4 - s1*c4)*c6;
//ny = ((-c1*s4 - s1*s23*c4)*c5 - s1*c23*s5)*s6 + (c1*c4 - s1*s23*s4)*c6;
nz = (c23 * c4 * c5 - s23 * s5) * s6 + c23 * s4 * c6;
//sx = ((s1*s4 - c1*s23*c4)*c5 - c1*c23*s5)*c6 - (-c1*s23*s4 - s1*c4)*s6;
//sy = ((-c1*s4 - s1*s23*c4)*c5 - s1*c23*s5)*c6 - (c1*c4 - s1*s23*s4)*s6;
sz = (c23 * c4 * c5 - s23 * s5) * c6 - c23 * s4 * s6;
ax = (s1 * s4 - c1 * s23 * c4) * s5 + c1 * c23 * c5;
ay = (-c1 * s4 - s1 * s23 * c4) * s5 + s1 * c23 * c5;
az = c23 * c4 * s5 + s23 * c5;
//nx = round2(nx, 6);
//ny = round2(ny, 6);
nz = round2(nz, 6);
//sy = round2(sy, 6);
sz = round2(sz, 6);
az = round2(az, 6);
ay = round2(ay, 6);
ax = round2(ax, 6);
Point3D p = Arm6(_t1, _t2, _t3, _t4, _t5, _t6, 0, 0, d6);
tuple t = new tuple();
t.t1 = p.x;
t.t2 = p.y;
t.t3 = p.z;
//I calculate this value with brute force, see below
t.t5 = Math.Atan2(-az, Math.Sqrt(ax * ax + ay * ay)); // it works also with atan2(-az, sqrt(sz*sz + nz*nz));
t.t4 = Math.Atan2(ay / Math.Cos(t.t5), ax / Math.Cos(t.t5));
t.t6 = Math.Atan2(sz / Math.Cos(t.t5), nz / Math.Cos(t.t5)) - pi / 2;
//This quoted procedure needs to find, brute force, the right configuration of parameters... there is to go crazy
//Remember to initiate the class MyForm in Robot::Robot
//double m[9];
//System::Text::StringBuilder^ st = gcnew System::Text::StringBuilder("");
//m[0] = round2(nx,6);
//m[1] = round2(ny, 6);
//m[2] = round2(nz, 6);
//m[3] = round2(sx, 6);
//m[4] = round2(sy, 6);
//m[5] = round2(sz, 6);
//m[6] = round2(ax, 6);
//m[7] = round2(ay, 6);
//m[8] = round2(az, 6);
//for (int i = 0; i < 9;i++)
//{
// for (int j = 0; j < 9; j++)
// {
// /*for (int k = 0; k < 9; k++)
// {
// */ st->Append(i);
// st->Append(j);
// // st->Append(k);
// st->Append("=");
// st->Append(atan2(m[i] / cos(t->t5), m[j] / cos(t->t5)) / dToR);
// // st->Append(atan2(m[i],sqrt(m[j]*m[j]+m[k]*m[k]))/dToR);
// st->Append("\r\n");
// /*}*/
// }
//}
//f->textBox1->Text = st->ToString();
return(t);
}
//Given position and orientation, calculate the angles of the motors
private tuple inverse(double x, double y, double z, double a, double b, double c)
{
tuple p;
p = new tuple();
double pWx;
double pWy;
double pWz;
double px;
double py;
double pz;
double s3;
double c3;
double s2;
double c2;
double xi;
double fi;
int sgn;
pWx = x - Math.Cos(a) * Math.Cos(b) * (d6 + a5);
pWy = y - Math.Sin(a) * Math.Cos(b) * (d6 + a5);
pWz = z + Math.Sin(b) * (d6 + a5);
double u;
int sg = 1;
p.t1 = Math.Atan2(pWy, pWx);
///////Dunno what it does
//if ((int)(pWx * pWx + pWy * pWy - a1 * a1 < 0) != 0)
//{
// sg = -1;
//}
//distance of the wirst from the Motor 2
px = pWx - a1 * Math.Cos(p.t1);
py = pWy - a1 * Math.Sin(p.t1);
pz = pWz - d1 - h;
c3 = (px * px + py * py + pz * pz - Math.Pow(a3 + d4, 2) - a2 * a2) / (2 * (a3 + d4) * a2);
s3 = -Math.Sqrt(1 - c3 * c3);
p.t3 = Math.Atan2(s3, c3);
//the follow code show in the MyForm f, some parameters. MyForm get shown in the creator
//f->textBox1->Text = System::String::Concat("c3num =", (px*px + py*py + pz*pz - pow((a3 + d4), 2) ), "\r\n"
// , "mod =", px*px + py*py + pz*pz, "\r\n"
// , "pWx =", pWx, "\r\n"
// , "pWy =", pWy, "\r\n"
// , "pWz =", pWz, "\r\n"
// , "px =", px, "\r\n"
// , "py =", py, "\r\n"
// , "pz =", pz, "\r\n"
// , "c3 =", c3, "\r\n"
// , "s3 =", s3, "\r\n"
// , "t3 =", p->t3, "\r\n"
// );
// http://www.rob.uni-luebeck.de/Lehre/2008w/Robotik/Vorlesung/Robotik1VL5_1_vers1.pdf pag. 50
xi = Math.Atan2(pz, sg * Math.Sqrt(px * px + py * py)); // sg is my addition
fi = Math.Atan2((a3 + d4) * s3, (a2 + (a3 + d4) * c3));
p.t2 = xi - fi;
double nx;
double ny;
double nz;
double sx;
double ax;
double sa;
double sb;
double sc;
double s1;
double ca;
double cb;
double cc;
double c1;
double c23;
double s23;
sa = Math.Sin(a);
sb = Math.Sin(b);
sc = Math.Sin(c);
s1 = Math.Sin(p.t1);
s23 = Math.Sin(p.t3 + p.t2);
ca = Math.Cos(a);
cb = Math.Cos(b);
cc = Math.Cos(c);
c1 = Math.Cos(p.t1);
c23 = Math.Cos(p.t3 + p.t2);
nx = round2(-sb * s23 + cb * c23 * Math.Cos(a - p.t1), 6);
ny = round2(-cb * s23 * Math.Cos(a - p.t1) - sb * c23, 6);
nz = round2(cb * Math.Sin(p.t1 - a), 6);
sx = round2(cb * sc * s23 + (sa * sb * sc + ca * cc) * s1 * c23 + (ca * sb * sc - sa * cc) * c1 * c23, 6);
ax = round2(cb * cc * s23 + (sa * sb * cc - ca * sc) * s1 * c23 + (sa * sc + ca * sb * cc) * c1 * c23, 6);
p.t5 = Math.Atan2(Math.Sqrt(nz * nz + ny * ny), (nx));
if (p.t5 < 0)
{
p.t4 = Math.Atan2((-nz), (-ny));
p.t5 = Math.Atan2(-Math.Sqrt(nz * nz + ny * ny), (nx));
}
else
{
//p->t4 = atan2((nz), (ny));
//p->t5 = atan2(sqrt(nz *nz + ny *ny), (nx));
p.t4 = Math.Atan2((nz), (ny));
}
p.t6 = Math.Atan2(-sx, -ax);
if (sg < 0)
{
p.t6 = Math.Atan2(sx, ax);
if (p.t6 < 0)
{
p.t6 = Math.Atan2(-sx, -ax);
}
else
{
p.t6 = Math.Atan2(sx, ax);
}
}
return(p);
}