IKCalliope.cc

Go to the documentation of this file.

#if defined(TGT_IS_CALLIOPE) && defined(TGT_HAS_ARMS)

#include "IKCalliope.h"
#include "Kinematics.h"

const float IKCalliope::EPSILON = std::numeric_limits<float>::min();
const std::string IKCalliope::autoRegisterIKCalliope = IKSolver::getRegistry().registerType<IKCalliope>("IKCalliope");

#  if defined(TGT_IS_CALLIOPE5)

IKCalliope::IKCalliope() : L1(), L2(), jointLimits() {
  const KinematicJoint* j = kine->getKinematicJoint(ArmWristOffset);
  if ( j == NULL )
    throw std::runtime_error("IKCalliope can't find GripperFrameOffset or ARM:wristrot");
  
  L1 = j->getParent()->r;
  L2 = j->r;
  jointLimits[0][0] = j->getParent()->getParent()->qmin;
  jointLimits[0][1] = j->getParent()->getParent()->qmax;
  jointLimits[1][0] = j->getParent()->qmin; 
  jointLimits[1][1] = j->getParent()->qmax;
  jointLimits[2][0] = j->qmin;
  jointLimits[2][1] = j->qmax;
}

bool IKCalliope::solve(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float posPri, const Orientation& oriTgt, float oriPri) const {
  // translate target point from BaseFrame to the frame of the ARM:base.
  const Point& pTgtPoint = dynamic_cast<const Point&>(pTgt);
  Point pTgtBase(kine->getKinematicJoint(ArmBaseOffset)->getTo().inverse() * pTgtPoint);
  
  // use std::atan2 to solve for base link
  float qBase = std::atan2(pTgtBase.y, pTgtBase.x);
  float qOffset = kine->getKinematicJoint(ArmBaseOffset)->qOffset;
  fmat::SubVector<2> gripperToBase(kine->linkToLink(GripperFrameOffset, ArmBaseOffset).translation());
  fmat::SubVector<2> wristToBase(kine->linkToLink(ArmWristOffset, ArmBaseOffset).translation());
  float qGripper = fmat::atan(gripperToBase) - fmat::atan(wristToBase);
  j.getParent()->getParent()->getParent()->getParent()->getParent()->setQ(qBase-qOffset-qGripper);
  
  // convert target point to shoulder reference frame
  fmat::Column<2> pTgtV;
  if (std::abs(std::cos(qBase)) < EPSILON)
    pTgtV[0] = pTgtBase.y;
  else
    pTgtV[0] = pTgtBase.x / std::cos(qBase);
  pTgtV[1] = pTgtBase.z;
  // shift from ARM:base -> ARM:shoulder for new origin for IK
  float baseToShoulder = kine->linkToLink(ArmShoulderOffset, ArmBaseOffset).translation()[0];
  pTgtV[0] -= baseToShoulder;
  
  const Rotation& oriTgtV = dynamic_cast<const Rotation&>(oriTgt);
  
  // perform IK with rotation around the Y axis for orientation
  IKCalliope::Solutions sols = closestThreeLinkIK(pTgtV, pEff, oriTgtV.axisComponent(fmat::UNIT3_Y));
  
  // set wristRot angle on the basis of how the x-y plane is rotated about the z axis
  j.getParent()->setQ(oriTgtV.axisComponent(fmat::UNIT3_Z));
  
  // set the rest based on the three link IK solution
  j.getParent()->getParent()->tryQ(sols.angles(0,2));
  j.getParent()->getParent()->getParent()->tryQ(sols.angles(0,1));
  j.getParent()->getParent()->getParent()->getParent()->tryQ(sols.angles(0,0));
  
  return sols.valid;
}

IKCalliope::Solutions IKCalliope::closestThreeLinkIK(const fmat::Column<2>& t, const fmat::Column<3>& pEff, float prefPhi) const {
  fmat::Column<2> target = updatePosition(t, pEff, prefPhi);
  IKCalliope::Solutions solutions = threeLinkIK(target, prefPhi); 
  if( solutions.valid )
    return solutions;
  
  /* If solution cannot be found with specified phi,
   try ±{1,2,3,...,180} degrees until achievable angles are found */
  else {
    float pstep = M_PI/180;
    for(float deltaPhi = pstep; deltaPhi <= M_PI; deltaPhi += pstep) {
      target = updatePosition(t, pEff, AngSignPi(prefPhi + deltaPhi));
      solutions = threeLinkIK(target, AngSignPi(prefPhi + deltaPhi));
      if( solutions.valid )
        return solutions;
      
      target = updatePosition(t, pEff, AngSignPi(prefPhi - deltaPhi));
      solutions = threeLinkIK(target, AngSignPi(prefPhi - deltaPhi));
      if( solutions.valid )
        return solutions;
    }
  }
  return punt(t);
}

fmat::Column<2> IKCalliope::updatePosition(const fmat::Column<2>& t, const fmat::Column<3>& pEff, float oriThreeLink) const {
  // shift from the gripper to the end of the wrist for three-link IK
  fmat::Column<2> gripperToWrist(kine->linkToLink(GripperFrameOffset, ArmWristOffset).translation());
  gripperToWrist = fmat::rotation2D(oriThreeLink) * gripperToWrist;
  fmat::Column<2> target = t - gripperToWrist;
  
  // factor in the effector point pEff
  return target + fmat::rotation2D(oriThreeLink) * fmat::Column<2>(pEff);
}

IKCalliope::Solutions IKCalliope::threeLinkIK(fmat::Column<2> t, float phi) const {
  IKCalliope::Solutions solutions;
  fmat::Matrix<2,2> sols_2link;
  fmat::Column<2> q3;
  float c2 = (t.sumSq() - L1*L1 - L2*L2) / (2*L1*L2);
  if(c2*c2 > 1) {
    solutions.noSols = 0;
    solutions.valid = false;
    return solutions;
  }
  else {
    sols_2link = twoLinkIK(t);
    q3[0] = phi - sols_2link(0,0) - sols_2link(0,1);
    q3[1] = phi - sols_2link(1,0) - sols_2link(1,1);
    solutions.angles(0,0) = sols_2link(0,0); solutions.angles(0,1) = sols_2link(0,1); solutions.angles(0,2) = q3[0];
    solutions.angles(1,0) = sols_2link(1,0); solutions.angles(1,1) = sols_2link(1,1); solutions.angles(1,2) = q3[1];
    solutions.noSols = 2;
    solutions = validAngles(solutions);
    return solutions;
  }
}

IKCalliope::Solutions IKCalliope::punt(const fmat::Column<2>& target) const {
  IKCalliope::Solutions solutions;
  float q1 = fmat::atan(target), q2 = 0.0f, q3 = 0.0f;
  q1 = std::min(std::max(q1,jointLimits[0][0]),jointLimits[0][1]);
  solutions.angles(0,0) = q1; solutions.angles(0,1) = q2; solutions.angles(0,2) = q3;
  solutions.noSols = 1;
  solutions.valid = false;
  return solutions;
}

fmat::Matrix<2,2> IKCalliope::twoLinkIK(fmat::Column<2> t) const {
  fmat::Matrix<2,2> ret;
  float c2 = (t.sumSq() - L1*L1 - L2*L2) / (2*L1*L2);
  float s2 = std::sqrt(1 - c2*c2);
  float q2 = std::atan2(s2, c2);
  float kQ = std::atan2(L2*s2,L1 + L2*c2);
  float q1plus = fmat::atan(t) - kQ;
  float q1minus = fmat::atan(t) + kQ;
  ret(0,0) = q1plus; ret(0,1) = q2;
  ret(1,0) = q1minus; ret(1,1) = -q2;
  return ret;
}

IKCalliope::Solutions IKCalliope::validAngles(IKCalliope::Solutions solutions) const {
  int valid_solutions[2] = {0, 0}; // assume both solutions are invalid
  for (int sol_no = 0; sol_no < solutions.noSols; sol_no++) {
    // Check for joint limits, any solution with a joint outside the limits is an invalid solution
    int val_angles = 0;
    for (int joint_no = 0; joint_no < 3; joint_no++) {
      if (solutions.angles(sol_no,joint_no) > jointLimits[joint_no][0] && solutions.angles(sol_no,joint_no) < jointLimits[joint_no][1])
        val_angles++;
    }
    if (val_angles == 3)
      valid_solutions[sol_no] = 1;
  } 
  if (valid_solutions[0]+valid_solutions[1] == 2) {      // both solutions were valid
    solutions.noSols = 2;
    solutions.valid = true;
    return solutions;
  }
  else if (valid_solutions[0]+valid_solutions[1] == 0) { // both solutions were invalid
    solutions.noSols = 0;
    solutions.valid = false;
    return solutions;
  }
  else {                                                  // one solution was invalid
    if (valid_solutions[0] == 0)
      solutions.angles.row(0) = solutions.angles.row(1);
    solutions.noSols = 1;
    solutions.valid = true;
    return solutions;
  }
}

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 {
  // find the closest target point
  fmat::Transform Tj = j.getFullT();
  fmat::Column<3> pEffBase(Tj*pEff);
  fmat::Quaternion qj = j.getWorldQuaternion();
  Rotation oEffBase = qj*oriEff;
  Point de;
  pTgt.computeErrorGradient(pEffBase,oEffBase,de);
  StepResult_t res = SUCCESS;
  if(de.norm() > pDist) {
    de *= pDist/de.norm();
    res = PROGRESS;
  }
  Point pEffBaseP(pEffBase[1],pEffBase[2],pEffBase[3]);
  Point pTgtP = pEffBaseP+de;
  if(solve(pEff,oriEff,j,pTgtP,posPri,oriTgt,oriPri))
    return res;
  return LIMITS;
}

#  elif defined(TGT_IS_CALLIOPE2) // TGT_IS_CALLIOPE5

IKCalliope::IKCalliope() : baseToArm(), qOffset(), forearmLength() {
  const KinematicJoint* j = kine->getKinematicJoint(GripperFrameOffset);
  
  forearmLength = (j->getWorldPosition() - j->getParent()->getParent()->getWorldPosition()).norm();
  qOffset = kine->getKinematicJoint(ArmBaseOffset)->qOffset;
  baseToArm = kine->getKinematicJoint(ArmBaseOffset)->getTo().inverse();
}

bool IKCalliope::solve(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float posPri, const Orientation& oriTgt, float oriPri) const {
  // translate target point from BaseFrame to the frame of the ARM:base.
  const Point& pTgtPoint = dynamic_cast<const Point&>(pTgt);
  Point pTgtBase(baseToArm * pTgtPoint);
  
  // use std::atan2 to solve for base link
  /* note that in reality, the GripperFrame is shifted 1.283cm to the left of the
   axis pointed to by the arm, and that this solver does not take into account
   that shift due to its insignificance. */
  float qBase = std::atan2(pTgtBase.y, pTgtBase.x);
  j.getParent()->getParent()->getParent()->tryQ(qBase-qOffset);
  
  /* we want the Z component of the specified target point to line up with the Gripper
   so that perhaps from there, the robot can move forward to touch the point */
  float opposite = pTgtBase[2];
  float qElbow;
  if (std::abs(opposite) < forearmLength)
    qElbow = std::asin(opposite / forearmLength);
  else
    qElbow = opposite > 0 ? M_PI_2 : -M_PI_2;
  j.getParent()->getParent()->tryQ(qElbow);
  
  if ( (j.getWorldPosition() - pTgtBase).norm() < 5.0f )
    return true;
  
  return false;
}

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 {
  // find the closest target point
  fmat::Transform Tj = j.getFullT();
  fmat::Column<3> pEffBase(Tj*pEff);
  fmat::Quaternion qj = j.getWorldQuaternion();
  Rotation oEffBase = qj*oriEff;
  Point de;
  pTgt.computeErrorGradient(pEffBase,oEffBase,de);
  StepResult_t res=SUCCESS;
  if(de.norm()>pDist) {
    de*=pDist/de.norm();
    res = PROGRESS;
  }
  Point pEffBaseP(pEffBase[1],pEffBase[2],pEffBase[3]);
  Point pTgtP = pEffBaseP+de;
  if(solve(pEff,oriEff,j,pTgtP,posPri,oriTgt,oriPri))
    return res;
  return LIMITS;
}

#  endif

#endif // TGT_HAS_ARMS