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); }