UR3运动学正解和逆解的C++实现

//正解

void forward(const double* q, double* T) {

    double s1 = sin(*q), c1 = cos(*q); q++;

    double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;

    double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;

    q234 += *q; q++;

    double s5 = sin(*q), c5 = cos(*q); q++;

    double s6 = sin(*q), c6 = cos(*q);

    double s234 = sin(q234), c234 = cos(q234);

    *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;

    *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) -

          (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;

    *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 -

          s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;

    *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 -

          d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 -

          a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;

    *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;

    *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) +

          s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;

    *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) -

          s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;

    *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 +

          (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 -

          a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;

    *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;

    *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;

    *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;

    *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 -

         (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;

    *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;

  }

 

//逆解

 

  int inverse(const double* T, double* q_sols, double q6_des) {

    int num_sols = 0;

    double T02 = -*T; T++; double T00 =  *T; T++; double T01 =  *T; T++; double T03 = -*T; T++;

    double T12 = -*T; T++; double T10 =  *T; T++; double T11 =  *T; T++; double T13 = -*T; T++;

    double T22 =  *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 =  *T;

 

    ////////////////////////////// shoulder rotate joint (q1) //////////////////////////////

    double q1[2];

    {

      double A = d6*T12 - T13;

      double B = d6*T02 - T03;

      double R = A*A + B*B;

      if(fabs(A) < ZERO_THRESH) {

        double div;

        if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)

          div = -SIGN(d4)*SIGN(B);

        else

          div = -d4/B;

        double arcsin = asin(div);

        if(fabs(arcsin) < ZERO_THRESH)

          arcsin = 0.0;

        if(arcsin < 0.0)

          q1[0] = arcsin + 2.0*PI;

        else

          q1[0] = arcsin;

        q1[1] = PI - arcsin;

      }

      else if(fabs(B) < ZERO_THRESH) {

        double div;

        if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)

          div = SIGN(d4)*SIGN(A);

        else

          div = d4/A;

        double arccos = acos(div);

        q1[0] = arccos;

        q1[1] = 2.0*PI - arccos;

      }

      else if(d4*d4 > R) {

        return num_sols;

      }

      else {

        double arccos = acos(d4 / sqrt(R)) ;

        double arctan = atan2(-B, A);

        double pos = arccos + arctan;

        double neg = -arccos + arctan;

        if(fabs(pos) < ZERO_THRESH)

          pos = 0.0;

        if(fabs(neg) < ZERO_THRESH)

          neg = 0.0;

        if(pos >= 0.0)

          q1[0] = pos;

        else

          q1[0] = 2.0*PI + pos;

        if(neg >= 0.0)

          q1[1] = neg;

        else

          q1[1] = 2.0*PI + neg;

      }

    }

    ////////////////////////////////////////////////////////////////////////////////

 

    ////////////////////////////// wrist 2 joint (q5) //////////////////////////////

    double q5[2][2];

    {

      for(int i=0;i<2;i++) {

        double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);

        double div;

        if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)

          div = SIGN(numer) * SIGN(d6);

        else

          div = numer / d6;

        double arccos = acos(div);

        q5[i][0] = arccos;

        q5[i][1] = 2.0*PI - arccos;

      }

    }

    ////////////////////////////////////////////////////////////////////////////////

 

    {

      for(int i=0;i<2;i++) {

        for(int j=0;j<2;j++) {

          double c1 = cos(q1[i]), s1 = sin(q1[i]);

          double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);

          double q6;

          ////////////////////////////// wrist 3 joint (q6) //////////////////////////////

          if(fabs(s5) < ZERO_THRESH)

            q6 = q6_des;

          else {

            q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1),

                       SIGN(s5)*(T00*s1 - T10*c1));

            if(fabs(q6) < ZERO_THRESH)

              q6 = 0.0;

            if(q6 < 0.0)

              q6 += 2.0*PI;

          }

          ////////////////////////////////////////////////////////////////////////////////

 

          double q2[2], q3[2], q4[2];

          ///////////////////////////// RRR joints (q2,q3,q4) ////////////////////////////

          double c6 = cos(q6), s6 = sin(q6);

          double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));

          double x04y = c5*(T20*c6 - T21*s6) - T22*s5;

          double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) +

                        T03*c1 + T13*s1;

          double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);

 

          double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);

          if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)

            c3 = SIGN(c3);

          else if(fabs(c3) > 1.0) {

            // TODO NO SOLUTION

            continue;

          }

          double arccos = acos(c3);

          q3[0] = arccos;

          q3[1] = 2.0*PI - arccos;

          double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;

          double s3 = sin(arccos);

          double A = (a2 + a3*c3), B = a3*s3;

          q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);

          q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);

          double c23_0 = cos(q2[0]+q3[0]);

          double s23_0 = sin(q2[0]+q3[0]);

          double c23_1 = cos(q2[1]+q3[1]);

          double s23_1 = sin(q2[1]+q3[1]);

          q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);

          q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);

          ////////////////////////////////////////////////////////////////////////////////

          for(int k=0;k<2;k++) {

            if(fabs(q2[k]) < ZERO_THRESH)

              q2[k] = 0.0;

            else if(q2[k] < 0.0) q2[k] += 2.0*PI;

            if(fabs(q4[k]) < ZERO_THRESH)

              q4[k] = 0.0;

            else if(q4[k] < 0.0) q4[k] += 2.0*PI;

            q_sols[num_sols*6+0] = q1[i];    q_sols[num_sols*6+1] = q2[k];

            q_sols[num_sols*6+2] = q3[k];    q_sols[num_sols*6+3] = q4[k];

            q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;

            num_sols++;

          }

 

        }

      }

    }

    return num_sols;

  }

};