Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

IKCalliope.cc

Go to the documentation of this file.
00001 #if (defined(TGT_IS_CALLIOPE) || defined(TGT_IS_CALLIOPE3)) && defined(TGT_HAS_ARMS)
00002 
00003 #include "IKCalliope.h"
00004 #include "IKGradientSolver.h"
00005 #include "Kinematics.h"
00006 
00007 const float IKCalliope::EPSILON = 1e-3;
00008 const std::string IKCalliope::autoRegisterIKCalliope = IKSolver::getRegistry().registerType<IKCalliope>("IKCalliope");
00009 
00010 #if defined(TGT_IS_CALLIOPE3)
00011 
00012 IKCalliope::IKCalliope() {}
00013 
00014 bool IKCalliope::solve(const Point& pEff, const Rotation& oriEff, KinematicJoint& j,
00015                        const Position& pTgt, float posPri, 
00016                        const Orientation& oriTgt, float oriPri) const {
00017   if ( j.outputOffset != GripperFrameOffset ) {
00018     throw std::runtime_error("IKCalliope::solve only accepts arm gripper joints.");
00019     return false;
00020   }
00021   KinematicJoint *shoulder = j.getParent();
00022   KinematicJoint *base = shoulder->getParent();
00023   shoulder->tryQ(0.0);   // *** hack for now
00024   base->tryQ(0.5); // *** hack for now
00025   return true;
00026 }
00027 
00028 IKSolver::StepResult_t IKCalliope::step(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float pDist, float posPri, const Orientation& oriTgt, float oriDist, float oriPri) const {
00029   // find the closest target point
00030   fmat::Transform Tj = j.getFullT();
00031   fmat::Column<3> pEffBase(Tj*pEff);
00032   fmat::Quaternion qj = j.getWorldQuaternion();
00033   Rotation oEffBase = qj*oriEff;
00034   Point de;
00035   pTgt.computeErrorGradient(pEffBase,oEffBase,de);
00036   StepResult_t res=SUCCESS;
00037   if(de.norm()>pDist) {
00038     de*=pDist/de.norm();
00039     res = PROGRESS;
00040   }
00041   Point pEffBaseP(pEffBase[1],pEffBase[2],pEffBase[3]);
00042   Point pTgtP = pEffBaseP+de;
00043   if(solve(pEff,oriEff,j,pTgtP,posPri,oriTgt,oriPri))
00044     return res;
00045   return LIMITS;
00046 }
00047 
00048 #elif defined(TGT_IS_CALLIOPE5)
00049 
00050 //================ Calliope5 version ================
00051 
00052 IKCalliope::IKCalliope() : L1(), L2(), jointLimits(), qGripper(0), qWristRot(0), qOffset(0),
00053          baseToArm(), baseToArmRot(), shoulderOffset(0) {
00054   const KinematicJoint* j = kine->getKinematicJoint(ArmWristOffset);
00055   if ( j == NULL )
00056     throw std::runtime_error("IKCalliope can't find the ARM:wrist joint!");
00057   
00058   L1 = j->getParent()->r;
00059   L2 = j->r;
00060   jointLimits[0][0] = j->getParent()->getParent()->qmin;
00061   jointLimits[0][1] = j->getParent()->getParent()->qmax;
00062   jointLimits[1][0] = j->getParent()->qmin; 
00063   jointLimits[1][1] = j->getParent()->qmax;
00064   jointLimits[2][0] = j->qmin;
00065   jointLimits[2][1] = j->qmax;
00066   
00067   fmat::SubVector<2> gripperToBase(kine->linkToLink(GripperFrameOffset, ArmBaseOffset).translation());
00068   fmat::SubVector<2> wristToBase(kine->linkToLink(ArmWristOffset, ArmBaseOffset).translation());
00069   fmat::SubVector<2> wristRotToBase(kine->linkToLink(WristRotateOffset, ArmBaseOffset).translation());
00070   qGripper = fmat::atan(gripperToBase) - fmat::atan(wristToBase);
00071   qWristRot = fmat::atan(wristRotToBase) - fmat::atan(wristToBase);
00072   qOffset = kine->getKinematicJoint(ArmBaseOffset)->qOffset;
00073   baseToArm = kine->getKinematicJoint(ArmBaseOffset)->getTo().inverse();
00074   baseToArmRot = fmat::Quaternion::aboutZ(-qOffset) * fmat::Quaternion::fromMatrix(baseToArm.rotation());
00075   shoulderOffset = kine->getKinematicJoint(ArmShoulderOffset)->getTo().translation()[0];
00076 }
00077 
00078 /*****************
00079 
00080 This code doesn't set the wrist or wristrot angle properly if the
00081 effector is GripperFrame.  Should check if oriTgt is a Parallel
00082 constraint, and if so, if y=1 set wristrot to pi/2; if z=1 then
00083 solve 2-link IK to put the wrist above the target and then set phi
00084 to point the fingers down.
00085 
00086 */
00087 
00088 bool IKCalliope::solve(const Point& pEff, const Rotation& oriEff, KinematicJoint& j,
00089            const Position& pTgt, float posPri,
00090            const Orientation& oriTgt, float oriPri) const {
00091   // we can only work on arm joints
00092   if ( j.outputOffset != GripperFrameOffset &&
00093        (j.outputOffset < ArmBaseOffset || j.outputOffset > WristRotateOffset) ) {
00094     throw std::runtime_error("IKCalliope::solve only accepts arm joints.");
00095     return false;
00096   }
00097   
00098   if ( ! IKCalliope::analyticallySolvable(oriEff, pTgt, oriTgt) )
00099     return IKCalliope::gradientSolve(pEff, oriEff, j, pTgt, posPri, oriTgt, oriPri);
00100 
00101   // translate target point from BaseFrame to the frame of the ARM:base.
00102   const Point& pTgtPoint = dynamic_cast<const Point&>(pTgt);
00103   Point pTgtBase(baseToArm * pTgtPoint);
00104   
00105   // get a rotation value for the target
00106   Rotation oriTgt2;
00107   if ( dynamic_cast<const Rotation*>(&oriTgt) != NULL )
00108     oriTgt2 = dynamic_cast<const Rotation&>(oriTgt);
00109   else { // must be Parallel
00110     const Parallel &par = dynamic_cast<const Parallel&>(oriTgt);
00111     oriTgt2 = fmat::Quaternion::fromAxis(fmat::pack(par.x, par.y, par.z));
00112   }
00113   //const Rotation& oriTgt2 = dynamic_cast<const Rotation&>(oriTgt);
00114   
00115   // target orientation in the target link's frame
00116   Rotation oriTgtV;
00117   
00118   // matrix representation of oriTgtV
00119   fmat::Matrix<3,3> ori;
00120   
00121   // angle of the base joint, offset included
00122   float qBase;
00123   
00124   // phi, for 3-link IK
00125   float y, z;
00126   
00127   // select whether we focus on position, or not
00128   bool position = posPri >= oriPri ? true : false;
00129   
00130   if (j.outputOffset == ArmShoulderOffset || j.outputOffset == ArmElbowOffset || j.outputOffset == ArmWristOffset) {
00131     // rotate -90° about the x axis
00132     oriTgtV = fmat::Quaternion::aboutX(-M_PI_2) * baseToArmRot * oriTgt2;
00133     ori = oriTgtV.toMatrix();
00134     qBase = std::atan2(pTgtBase.y, pTgtBase.x) - qOffset;
00135     
00136     // qBase = std::acos(oriTgtV.toMatrix()(2,2));
00137   }
00138   else if (j.outputOffset == WristRotateOffset) {
00139     // rotate 90° about the y axis
00140     oriTgtV = fmat::Quaternion::aboutY(M_PI_2) * baseToArmRot * oriTgt2;
00141     ori = oriTgtV.toMatrix();
00142     
00143     // determine phi
00144     if (std::abs(ori(0,2)) - 1.f <= EPSILON) {
00145       z = (std::acos(ori(1,1)) + std::asin(ori(1,0))) / 2.f;
00146       y = std::asin(ori(0,2));
00147     }
00148     else {
00149       float cosZ = -ori(0,0) / std::cos(std::asin(ori(0,2)));
00150       z = std::acos(cosZ);
00151       y = std::atan2(ori(0,2), ori(0,0) / cosZ);
00152     }
00153     qBase = std::atan2(pTgtBase.y, pTgtBase.x) - qOffset - qWristRot;
00154     
00155     // qBase = std::asin(oriTgtV.toMatrix()(1,2) / std::cos(y)) - qWristRot;
00156   }
00157   else if (j.outputOffset == GripperFrameOffset) {
00158     // rotate 90° about the y axis
00159     oriTgtV = fmat::Quaternion::aboutY(M_PI_2) * baseToArmRot * oriTgt2;
00160     ori = oriTgtV.toMatrix();
00161     
00162     // determine phi
00163     if (std::abs(ori(0,2)) - 1.f <= EPSILON) {
00164       z = (std::acos(ori(1,1)) + std::asin(ori(1,0))) / 2.f;
00165       y = std::asin(ori(0,2));
00166     }
00167     else {
00168       float cosZ = -ori(0,0) / std::cos(std::asin(ori(0,2)));
00169       z = std::acos(cosZ);
00170       y = std::atan2(ori(0,2), ori(0,0) / cosZ);
00171     }
00172     qBase = std::atan2(pTgtBase.y, pTgtBase.x) - qOffset - qGripper;
00173     
00174     // qBase = std::asin(oriTgtV.toMatrix()(1,2) / std::cos(y)) - qGripper;
00175   }
00176   else if (j.outputOffset == ArmBaseOffset) {
00177     // no rotation other than to ArmBase
00178     oriTgtV = baseToArmRot * oriTgt2;
00179     ori = oriTgtV.toMatrix();
00180     
00181     // take yaw angle
00182     qBase = oriTgtV.ypr()[0];
00183   }
00184   
00185   // if we're just solving for the base or shoulder, we're done.
00186   if (j.outputOffset == ArmBaseOffset) {
00187     return j.tryQ(qBase);
00188   }
00189   else if (j.outputOffset == ArmShoulderOffset) {
00190     j.getParent()->tryQ(qBase);
00191     j.tryQ(std::acos(ori(1,1)));
00192     return (j.getWorldPosition() - pTgtPoint).sumSq() < 25.0f;
00193   }
00194   
00195   // If we get here, we're solving for the elbow, wrist, or gripper.
00196   // Convert target point to shoulder reference frame.
00197   fmat::Column<2> pTgtV;
00198   float realQ = qBase + qOffset;
00199   if (j.outputOffset == WristRotateOffset) realQ += qWristRot;
00200   if (j.outputOffset == GripperFrameOffset) realQ += qGripper;
00201   if (std::abs(std::cos(realQ)) < EPSILON)
00202     pTgtV[0] = pTgtBase.y;
00203   else
00204     pTgtV[0] = pTgtBase.x / std::cos(realQ);
00205   pTgtV[1] = pTgtBase.z;
00206   // shift from ARM:base -> ARM:shoulder for new origin for IK
00207   pTgtV[0] -= shoulderOffset;
00208   
00209   if (j.outputOffset == ArmElbowOffset) {
00210     j.getParent()->getParent()->tryQ(qBase);
00211     
00212     float qShoulder = fmat::atan(pTgtV);
00213     j.getParent()->tryQ(qShoulder);
00214     
00215     // if oriTgtV, solve for shoulder joint
00216     if (!position)
00217       j.tryQ(std::acos(ori(1,1)) - qShoulder);
00218     
00219     return (j.getWorldPosition() - pTgtPoint).sumSq() < 25.0f;
00220   }
00221   else if (j.outputOffset == ArmWristOffset) {
00222     j.getParent()->getParent()->getParent()->tryQ(qBase);
00223     
00224     // 3-link IK
00225     Solutions s = closestThreeLinkIK(pTgtV, position, std::acos(ori(1,1)), fmat::ZERO2);
00226     
00227     int c = closestSolution(j, s);
00228     j.getParent()->getParent()->tryQ(s.angles(c,0));
00229     j.getParent()->tryQ(s.angles(c,1));
00230     j.tryQ(s.angles(c,2));
00231     
00232     return s.valid;
00233   }
00234   else if (j.outputOffset == WristRotateOffset) {
00235     j.getParent()->getParent()->getParent()->getParent()->tryQ(qBase);
00236     
00237     // 3-link IK
00238     Solutions s = closestThreeLinkIK(pTgtV, position, y, fmat::pack(0, j.r));
00239     
00240     int c = closestSolution(j, s);
00241     j.getParent()->getParent()->getParent()->tryQ(s.angles(c,0));
00242     j.getParent()->getParent()->tryQ(s.angles(c,1));
00243     j.getParent()->tryQ(s.angles(c,2));
00244     j.tryQ(z + qBase);
00245     
00246     return s.valid;
00247   }
00248   else if (j.outputOffset == GripperFrameOffset) {
00249     j.getParent()->getParent()->getParent()->getParent()->getParent()->tryQ(qBase);
00250     
00251     // 3-link IK
00252     Solutions s = closestThreeLinkIK(pTgtV, position, y, fmat::pack(-j.d, j.getParent()->r));
00253     
00254     int c = closestSolution(j, s);
00255     j.getParent()->getParent()->getParent()->getParent()->tryQ(s.angles(c,0));
00256     j.getParent()->getParent()->getParent()->tryQ(s.angles(c,1));
00257     j.getParent()->getParent()->tryQ(s.angles(c,2));
00258     j.getParent()->tryQ(z + qBase);
00259     
00260     return s.valid;
00261   }
00262   else
00263     // should never get here
00264     return false;
00265 }
00266 
00267 IKCalliope::Solutions IKCalliope::closestThreeLinkIK(const fmat::Column<2>& t, bool posPri, float prefPhi, const fmat::Column<2>& delta) const {
00268   fmat::Column<2> target = t + fmat::rotation2D(prefPhi) * delta;
00269   IKCalliope::Solutions solutions = threeLinkIK(target, prefPhi); 
00270   if (solutions.valid)
00271     return solutions;
00272   
00273   else {
00274     /* If solution cannot be found with specified phi,
00275      try ±{1,2,3,...,180} degrees until achievable angles are found */
00276     float pstep = M_PI/180;
00277     for (float deltaPhi = pstep; deltaPhi <= M_PI; deltaPhi += pstep) {
00278       AngSignPi angleUp(prefPhi + deltaPhi);
00279       target = t + fmat::rotation2D(angleUp) * delta;
00280       solutions = threeLinkIK(target, angleUp);
00281       if (solutions.valid) {
00282         if (!posPri) {
00283           assureOrientation(solutions, prefPhi);
00284           solutions.valid = false;
00285         }
00286         return solutions;
00287       }
00288       
00289       AngSignPi angleDown(prefPhi - deltaPhi);
00290       target = t + fmat::rotation2D(angleDown) * delta;
00291       solutions = threeLinkIK(target, angleDown);
00292       if (solutions.valid) {
00293         if (!posPri) {
00294           assureOrientation(solutions, prefPhi);
00295           solutions.valid = false;
00296         }
00297         return solutions;
00298       }
00299     }
00300   }
00301   
00302   return pointToward(t, posPri, prefPhi);
00303 }
00304 
00305 IKCalliope::Solutions IKCalliope::threeLinkIK(fmat::Column<2> t, float phi) const {
00306   IKCalliope::Solutions solutions;
00307   fmat::Matrix<2,2> sols_2link;
00308   fmat::Column<2> q3;
00309   float c2 = (t.sumSq() - L1*L1 - L2*L2) / (2*L1*L2);
00310   if(c2*c2 > 1) {
00311     solutions.noSols = 0;
00312     solutions.valid = false;
00313     return solutions;
00314   }
00315   else {
00316     sols_2link = twoLinkIK(t);
00317     q3[0] = phi - sols_2link(0,0) - sols_2link(0,1);
00318     q3[1] = phi - sols_2link(1,0) - sols_2link(1,1);
00319     solutions.angles(0,0) = sols_2link(0,0); solutions.angles(0,1) = sols_2link(0,1); solutions.angles(0,2) = q3[0];
00320     solutions.angles(1,0) = sols_2link(1,0); solutions.angles(1,1) = sols_2link(1,1); solutions.angles(1,2) = q3[1];
00321     solutions.noSols = 2;
00322     solutions = validAngles(solutions);
00323     return solutions;
00324   }
00325 }
00326 
00327 IKCalliope::Solutions IKCalliope::pointToward(const fmat::Column<2>& target, bool posPri, float prefPhi) const {
00328   IKCalliope::Solutions solutions;
00329   float q1 = fmat::atan(target), q2 = 0.0f, q3 = 0.0f;
00330   q1 = std::min(std::max(q1,jointLimits[0][0]),jointLimits[0][1]);
00331   solutions.angles(0,0) = q1; solutions.angles(0,1) = q2;
00332   if (posPri)
00333     solutions.angles(0,2) = q3;
00334   else
00335     solutions.angles(0,2) = prefPhi - q2 - q1;
00336   solutions.noSols = 1;
00337   solutions.valid = false;
00338   return solutions;
00339 }
00340 
00341 fmat::Matrix<2,2> IKCalliope::twoLinkIK(fmat::Column<2> t) const {
00342   fmat::Matrix<2,2> ret;
00343   float c2 = (t.sumSq() - L1*L1 - L2*L2) / (2*L1*L2);
00344   float s2 = std::sqrt(1 - c2*c2);
00345   float q2 = std::atan2(s2, c2);
00346   float kQ = std::atan2(L2*s2,L1 + L2*c2);
00347   float q1plus = fmat::atan(t) + kQ;
00348   float q1minus = fmat::atan(t) - kQ;
00349   ret(0,0) = q1plus; ret(0,1) = -q2;
00350   ret(1,0) = q1minus; ret(1,1) = +q2;
00351   return ret;
00352 }
00353 
00354 int IKCalliope::closestSolution(KinematicJoint& j, IKCalliope::Solutions s) const {
00355   if (!(s.valid && s.noSols == 2))
00356     return 0;
00357   
00358   KinematicJoint* jP = &j;
00359   
00360   while (jP->outputOffset != ArmElbowOffset)
00361     jP = jP->getParent();
00362   
00363   float jVal = jP->getQ();
00364   
00365   // if we're sticking straight up, we want to go for the elbow up solution.
00366   if (jVal < EPSILON)
00367     return (s.angles(1,1) > s.angles(0,1) ? 0 : 1);
00368   
00369   float a0 = jVal - s.angles(0,1);
00370   float a1 = jVal - s.angles(1,1);
00371   float d0 = a0 * a0;
00372   float d1 = a1 * a1;
00373   
00374   return (d0 < d1 ? 0 : 1);
00375 }
00376 
00377 void IKCalliope::assureOrientation(IKCalliope::Solutions &s, float phi) const {
00378   for (int i = 0; i < 2; i++) {
00379     float q2 = phi - s.angles(i,1) - s.angles(i,0);
00380     float q2Diff = 0;
00381     if (q2 > jointLimits[2][1]) {
00382       q2Diff = jointLimits[2][1] - q2;
00383       s.angles(i,2) = jointLimits[2][1];
00384     }
00385     else if (q2 < jointLimits[2][0]) {
00386       q2Diff = jointLimits[2][0] - q2;
00387       s.angles(i,2) = jointLimits[2][0];
00388     }
00389     else {
00390       s.angles(i,2) = q2;
00391       continue;
00392     }
00393     
00394     // if the wrist angle can't get to where we want it, modify the elbow
00395     float q1 = s.angles(i,1) - q2Diff;
00396     float q1Diff = 0;
00397     if (q1 > jointLimits[1][1]) {
00398       q1Diff = jointLimits[1][1] - q1;
00399       s.angles(i,1) = jointLimits[1][1];
00400     }
00401     else if (q1 < jointLimits[1][0]) {
00402       q1Diff = jointLimits[1][0] - q1;
00403       s.angles(i,1) = jointLimits[1][0];
00404     }
00405     else {
00406       s.angles(i,1) = q1;
00407       continue;
00408     }
00409     
00410     // if the elbow angle can't get to where we want it, modify the shoulder
00411     s.angles(i,0) = s.angles(i,0) - q1Diff;
00412     continue;
00413   }
00414   
00415   return;
00416 }
00417 
00418 IKCalliope::Solutions IKCalliope::validAngles(IKCalliope::Solutions solutions) const {
00419   int valid_solutions[2] = {0, 0}; // assume both solutions are invalid
00420   for (int sol_no = 0; sol_no < solutions.noSols; sol_no++) {
00421     // Check for joint limits, any solution with a joint outside the limits is an invalid solution
00422     int val_angles = 0;
00423     for (int joint_no = 0; joint_no < 3; joint_no++)
00424       if (solutions.angles(sol_no,joint_no) > jointLimits[joint_no][0] && solutions.angles(sol_no,joint_no) < jointLimits[joint_no][1])
00425         val_angles++;
00426       
00427     if (val_angles == 3)
00428       valid_solutions[sol_no] = 1;
00429   } 
00430   if (valid_solutions[0]+valid_solutions[1] == 2) {      // both solutions were valid
00431     solutions.noSols = 2;
00432     solutions.valid = true;
00433     return solutions;
00434   }
00435   else if (valid_solutions[0]+valid_solutions[1] == 0) { // both solutions were invalid
00436     solutions.noSols = 0;
00437     solutions.valid = false;
00438     return solutions;
00439   }
00440   else {                                                  // one solution was invalid
00441     if (valid_solutions[0] == 0)
00442       solutions.angles.row(0) = solutions.angles.row(1);
00443     solutions.noSols = 1;
00444     solutions.valid = true;
00445     return solutions;
00446   }
00447 }
00448 
00449 IKSolver::StepResult_t IKCalliope::step(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float pDist, float posPri, const Orientation& oriTgt, float oriDist, float oriPri) const {
00450   // find the closest target point
00451   fmat::Transform Tj = j.getFullT();
00452   fmat::Column<3> pEffBase(Tj*pEff);
00453   fmat::Quaternion qj = j.getWorldQuaternion();
00454   Rotation oEffBase = qj*oriEff;
00455   Point de;
00456   pTgt.computeErrorGradient(pEffBase,oEffBase,de);
00457   StepResult_t res = SUCCESS;
00458   if(de.norm() > pDist) {
00459     de *= pDist/de.norm();
00460     res = PROGRESS;
00461   }
00462   Point pEffBaseP(pEffBase[1],pEffBase[2],pEffBase[3]);
00463   Point pTgtP = pEffBaseP+de;
00464   if(solve(pEff,oriEff,j,pTgtP,posPri,oriTgt,oriPri))
00465     return res;
00466   return LIMITS;
00467 }
00468 
00469 bool IKCalliope::isSideGrasp(const Rotation &oriEff, const Orientation &oriTgt) {
00470   const Parallel *par = dynamic_cast<const Parallel*>(&oriTgt);
00471   return (par != NULL && *par == Parallel(0,0,1) &&
00472           oriEff == Rotation(fmat::Quaternion::aboutX(-M_PI/2)));
00473 }
00474 
00475 bool IKCalliope::isOverheadGrasp(const Rotation &oriEff, const Orientation &oriTgt) {
00476   const Parallel *par = dynamic_cast<const Parallel*>(&oriTgt);
00477   return (par != NULL && *par == Parallel(0,0,1) && oriEff == Rotation());
00478 }
00479 
00480 
00481 bool IKCalliope::analyticallySolvable(const Rotation &oriEff,
00482                                       const Position &pTgt, const Orientation &oriTgt) {
00483   // Can handle position = Point but not Line or Plane
00484   if ( dynamic_cast<const Point*>(&pTgt) == NULL )
00485     return false;
00486   // Can always handle orientation = Rotation
00487   if ( dynamic_cast<const Rotation*>(&oriTgt) != NULL )
00488     return true;
00489   return false;  //**** force use of gradient solver for now
00490   // Can handle two special orientation = Parallel cases
00491   if ( isSideGrasp(oriEff,oriTgt) || isOverheadGrasp(oriEff,oriTgt) )
00492     return true;
00493   // If we reach here, we have an unusual Parallel constraint: punt
00494   return false;
00495 }
00496 
00497 bool IKCalliope::gradientSolve(const Point& pEff, const Rotation& oriEff,
00498                                KinematicJoint& j, 
00499                                const Position& pTgt, float posPri, 
00500                                const Orientation& oriTgt, float oriPri) {
00501   static IKGradientSolver solver(750, 0.5f, 0.001f, 0.2/5000);
00502   j.getParent()->tryQ(M_PI/2);
00503   j.getParent()->getParent()->tryQ(1.38);
00504   j.getParent()->getParent()->getParent()->tryQ(-2.28);
00505   j.getParent()->getParent()->getParent()->getParent()->tryQ(1.01);
00506   j.getParent()->getParent()->getParent()->getParent()->getParent()->tryQ(0.);
00507   std::cout << "Calling gradientSolve" << std::endl;
00508   return solver.solve(pEff, oriEff, j, pTgt, posPri, oriTgt, oriPri);
00509 }
00510 
00511 #  elif defined(TGT_IS_CALLIOPE2)
00512 
00513 //================ Calliope2 Version ================
00514 
00515 IKCalliope::IKCalliope() : baseToArm(), qOffset(), gripperOffset(0), forearmLength() {
00516   const KinematicJoint* j = kine->getKinematicJoint(GripperFrameOffset);
00517   
00518   forearmLength = (j->getWorldPosition() - j->getParent()->getWorldPosition()).norm();
00519   qOffset = kine->getKinematicJoint(ArmBaseOffset)->qOffset;
00520   gripperOffset = kine->getKinematicJoint(GripperFrameOffset)->qOffset;
00521   baseToArm = kine->getKinematicJoint(ArmBaseOffset)->getTo().inverse();
00522 }
00523 
00524 bool IKCalliope::solve(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float posPri, const Orientation& oriTgt, float oriPri) const {
00525   // translate target point from BaseFrame to the frame of the ARM:base.
00526   const Point& pTgtPoint = dynamic_cast<const Point&>(pTgt);
00527   Point pTgtBase(baseToArm * pTgtPoint);
00528   Point pTgtShoulder(kine->getKinematicJoint(ArmShoulderOffset)->getTo().inverse() * pTgtPoint);
00529   
00530   // use std::atan2 to solve for base link
00531   /* note that in reality, the GripperFrame is shifted 1.283cm to the left of the
00532    axis pointed to by the arm, and that this solver does not take into account
00533    that shift due to its insignificance. */
00534   float qBase = std::atan2(pTgtBase.y, pTgtBase.x);
00535   j.getParent()->getParent()->tryQ(qBase-qOffset);
00536   
00537   /* we want the Z component of the specified target point to line up with the Gripper
00538    so that perhaps from there, the robot can move forward to touch the point. */
00539   float shoulderHeight = kine->getPosition(ArmShoulderOffset)[2];
00540   float opposite = pTgtShoulder[1] - shoulderHeight - gripperOffset;
00541   // std::cout << "pTgtBase=" << pTgtBase << "   pTgtShoulder=" << pTgtShoulder
00542   //    << "  gripperOffset=" << gripperOffset << std::endl;
00543   float qShoulder;
00544   if (std::abs(opposite) < forearmLength)
00545     qShoulder = std::asin(opposite / forearmLength);
00546   else
00547     qShoulder = opposite > 0 ? M_PI_2 : -M_PI_2;
00548   j.getParent()->tryQ(qShoulder);
00549   
00550   return (j.getWorldPosition() - pTgtPoint).norm() < 5.0f;
00551 }
00552 
00553 IKSolver::StepResult_t IKCalliope::step(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float pDist, float posPri, const Orientation& oriTgt, float oriDist, float oriPri) const {
00554   // find the closest target point
00555   fmat::Transform Tj = j.getFullT();
00556   fmat::Column<3> pEffBase(Tj*pEff);
00557   fmat::Quaternion qj = j.getWorldQuaternion();
00558   Rotation oEffBase = qj*oriEff;
00559   Point de;
00560   pTgt.computeErrorGradient(pEffBase,oEffBase,de);
00561   StepResult_t res=SUCCESS;
00562   if(de.norm()>pDist) {
00563     de*=pDist/de.norm();
00564     res = PROGRESS;
00565   }
00566   Point pEffBaseP(pEffBase[1],pEffBase[2],pEffBase[3]);
00567   Point pTgtP = pEffBaseP+de;
00568   if(solve(pEff,oriEff,j,pTgtP,posPri,oriTgt,oriPri))
00569     return res;
00570   return LIMITS;
00571 }
00572 
00573 #  endif  // Calliope2
00574 
00575 
00576 #endif // TGT_IS_CALLIOPE && TGT_HAS_ARMS

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:41 2016 by Doxygen 1.6.3