scara.rar_4轴逆解_scara逆算法_scara逆解过程_situationlxc_正逆解

#include <math.h> #include "kinematics.h" #include "Define.h" //#include "..\MCCL\MCCL_Fun.h" #include "MCCL_Fun.h" ///////////////////////////////////////////////////////////////////// // Global objects DWORD dwPosture = 0; // mechanical parameters (from GR985 data sheet) double DH_a1 = 100; double DH_a2 = 450; double DH_d1 = 50; double DH_d4 = 50; ///////////////////////////////////////////////////////////////////// // Helper functions // The efficacy of transformation matrix and orientation are the same. // They all determine the orientation of a space. // // This function transfers transformation matrix to orientation. int sign(double x) { if(x > 0) return 1; else return -1; } void tr2orient(const TRANSFORMATION_RULES& tr, ORIENTATION* orient) { orient->rx = atan2(tr.V3, tr.W3); orient->ry = -asin(tr.U3); orient->rz = atan2(tr.U2, tr.U1); } // This function transfers orientation to transformation matrix. void orient2tr(const ORIENTATION& orient, TRANSFORMATION_RULES* tr) { double s_rx = sin(orient.rx); double c_rx = cos(orient.rx); double s_ry = sin(orient.ry); double c_ry = cos(orient.ry); double s_rz = sin(orient.rz); double c_rz = cos(orient.rz); tr->U1 = c_rz * c_ry; tr->U2 = s_rz * c_ry; tr->U3 = -s_ry; tr->V1 = -s_rz * c_rx + c_rz * s_ry * s_rx; tr->V2 = c_rz * c_rx + s_rz * s_ry * s_rx; tr->V3 = s_rx * c_ry; tr->W1 = s_rz * s_rx + c_rz * s_ry * c_rx; tr->W2 = -c_rz * s_rx + s_rz * s_ry * c_rx; tr->W3 = c_ry * c_rx; } /* There are several solutions in inverse kinematics. The posture parameters can give more detail conditions thus the right solution would be gotten. all of the parameters are listed below: POST_ARM_LEFTY POST_ELBOW_BELOW POST_NONFLIP POST_J4DOUBLE POST_J6DOUBLE */ DWORD DeterminePosture(BOOL bArmLefty, int nJointNum, const double* pdfJointAngle) { DWORD posture = (bArmLefty) ? POST_ARM_LEFTY: 0; // Elbow & Flip //if( bArmLefty ) //{ // if( pdfJointAngle[4] >= 0 ) // posture |= POST_NONFLIP; // if( pdfJointAngle[2] <= DH_elbow_post_threshold ) // posture |= POST_ELBOW_BELOW; //} //else //{ // if( pdfJointAngle[4] < 0 ) // posture |= POST_NONFLIP; // if( pdfJointAngle[2] >= DH_elbow_post_threshold ) // posture |= POST_ELBOW_BELOW; //} // theta 4 (single/double) if( pdfJointAngle[3] <= -PI || pdfJointAngle[3] > PI ) posture |= POST_J4DOUBLE; //// theta 6 (single/double) //if( pdfJointAngle[5] <= -PI || pdfJointAngle[5] > PI ) // posture |= POST_J6DOUBLE; return posture; } BOOL GetEquivalentOrientation(int nIndex, double rx, double ry, double rz, double* equiv_rx, double* equiv_ry, double* equiv_rz, int nGroup) { *equiv_rx = rx = fmod(rx, 2.0 * PI); *equiv_ry = ry = fmod(ry, 2.0 * PI); *equiv_rz = rz = fmod(rz, 2.0 * PI); switch( nIndex ) { case 0: *equiv_rx += ((rx < 0) ? 2.0 * PI: -2.0 * PI); break; case 1: //*equiv_rz += ((rz < 0) ? 2.0 * PI: -2.0 * PI); break; case 2: *equiv_rx += ((rx < 0) ? 2.0 * PI: -2.0 * PI); //*equiv_rz += ((rz < 0) ? 2.0 * PI: -2.0 * PI); break; default: return FALSE; } return TRUE; } BOOL RBV6_fwk(const JOINT_POINT& jnt_point, CARTESIAN_POINT* crt_point) { TRANSFORMATION_RULES tr; double c1,s1,c2,s2,c4,s4,c12,s12,c124,s124; double d3; s1 = sin(jnt_point.q1); c1 = cos(jnt_point.q1); s2 = sin(jnt_point.q2); c2 = cos(jnt_point.q2); s4 = sin(jnt_point.q4); c4 = cos(jnt_point.q4); c12 = cos(jnt_point.q1+jnt_point.q2); s12 = sin(jnt_point.q1+jnt_point.q2); c124 = cos(jnt_point.q1+jnt_point.q2+jnt_point.q4); s124 = sin(jnt_point.q1+jnt_point.q2+jnt_point.q4); d3 = -jnt_point.q3; // Forward Kine.Rules : // Those rules are derived by Ben, Foxconn(reference book:"Robot Analysis",Lung-Wen Tsai). // and help to get transformation matrix tr.U1 = c124; tr.U2 = s124; tr.U3 = 0; tr.V1 = s124; tr.V2 = -c124; tr.V3 = 0; tr.W1 = 0; tr.W2 = 0; tr.W3 = -1; tr.D1 = DH_a1*c1 + DH_a2*c12; tr.D2 = DH_a1*s1 + DH_a2*s12; tr.D3 = DH_d1 - d3 - DH_d4; ORIENTATION orient; tr2orient(tr, &orient); crt_point->x = tr.D1; crt_point->y = tr.D2; crt_point->z = tr.D3; crt_point->rx = orient.rx; crt_point->ry = orient.ry; crt_point->rz = orient.rz; return (jnt_point.q2 < 0); } BOOL RBV6_ink(const CARTESIAN_POINT& crt_point, JOINT_POINT* jnt_point) { // retrieve transformation matrix from orientation. see orient2tr() ORIENTATION orient; TRANSFORMATION_RULES tr; // valid orientation range: (-180) ~ [180] orient.rx = valid_angle(crt_point.rx, -PI, PI); orient.ry = valid_angle(crt_point.ry, -PI, PI); orient.rz = valid_angle(crt_point.rz, -2*PI, 2*PI); // retrieve transformation matrix orient2tr(orient, &tr); double dx = crt_point.x; double dy = crt_point.y; double dz = crt_point.z; // see Spong: P.95: p is wrist center double px = dx; double py = dy; double pz = dz; // check if the wrister center is out of range double xx = pow(px, 2); double yy = pow(py, 2); double zz = pow(pz, 2); // max. length from J2 to wrist center double dfMaxWorkSpace = DH_a1 + DH_a2; if(( sqrt(xx + yy) - dfMaxWorkSpace ) > EPSLION) return FALSE; double theta1,theta2,d3,theta4; double c1,s1,c2,s2; double u1,v1,u2,v2; double k1,k2; // J2 (-pi ~ pi) k1 = px*px + py*py - DH_a1*DH_a1 - DH_a2*DH_a2; k2 = 2*DH_a1*DH_a2; if(k1/k2 > 1.0) k1 = k2; else if(k1/k2 < -1.0) k1 = -k2; if( crt_point.FIG & POST_ARM_LEFTY ) theta2 = -acos(k1/k2); else theta2 = acos(k1/k2); // J1 (-pi ~ pi) c2 = cos(theta2); s2 = sin(theta2); u1 = DH_a1 + DH_a2*c2; v1 = -DH_a2*s2; u2 = DH_a2*s2; v2 = DH_a1 + DH_a2*c2; s1 = (u2*px - u1*py) / (u2*v1 - u1*v2); c1 = (v2*px - v1*py) / (v2*u1 - v1*u2); theta1 = atan2(s1, c1); // d3 d3 = DH_d1 - DH_d4 - pz; // J4 (0 ~ pi) theta4 = -1*(theta1 + theta2 - crt_point.rz); //single range: (-180) ~ [ 180] //double range: (-360) ~ [-180] + (180) ~ [360] //if( crt_point.FIG & POST_J4DOUBLE ) //{ // if( theta4 > 0 && theta4 <= PI ) // theta4 -= (2.0 * PI); // else if( theta4 <= 0 && theta4 > -PI ) // theta4 += (2.0 * PI); //} //else //{ // if ( theta4 > PI && theta4 <= (2 * PI) ) // theta4 -= (2.0 * PI); // else if( theta4 <= -PI && theta4 > (-2.0 * PI) ) // theta4 += (2.0 * PI); //} // constrain J2 and J3 whithin -180 ~ 180 degrees theta1 = valid_angle(theta1, -PI, PI); theta2 = valid_angle(theta2, -PI, PI); // to fill up data jnt_point->q1 = theta1; jnt_point->q2 = theta2; jnt_point->q3 = -d3; jnt_point->q4 = theta4; jnt_point->q5 = 0; jnt_point->q6 = 0; return TRUE; } ///////////////////////////////////////////////////////////////////// // Exported functions void ROBOT_V6_CALL Robot_InitExtension(void* pvExtension) { if( pvExtension ) { EXTENSION *ext = static_cast<EXTENSION*>( pvExtension ); ext->last_posture = 0; } } void ROBOT_V6_CALL Scara_PreProcessMotionCommand(int nGroup, int nSynAxisNum, const double* pdfCurCPos, const double* pdfTgtCPos, double* pdfOffset, void* pvExtension) { if( nGroup < 0 || nSynAxisNum < 4 ) return; int idx = 0; double min_orient_dist = 1e+100; double rx, ry, rz, min_rx, min_ry, min_rz; rx = min_rx = pdfCurCPos[3]; ry = min_ry = pdfCurCPos[4]; rz = min_rz = pdfCurCPos[5]; do { double orient_dist = sqrt( ((pdfTgtCPos[3] - rx) * (pdfTgtCPos[3] - rx)) + ((pdfTgtCPos[4] - ry) * (pdfTgtCPos[4] - ry)) + ((pdfTgtCPos[5] - rz) * (pdfTgtCPos[5] - rz)) ); if( orient_dist < min_orient_dist ) {