hekateros::HekCalibStretch Class Reference

Hekateros robotic manipulator calibration by stretching class. More...

#include <hekCalibStretch.h>

Inheritance diagram for hekateros::HekCalibStretch:

Public Member Functions
	HekCalibStretch (HekRobot &robot)
virtual	~HekCalibStretch ()
virtual int	calibrate ()
	Calibrate the Hekateros's robotic arm. More...
Public Member Functions inherited from hekateros::HekCalib
	HekCalib (HekRobot &robot)
virtual	~HekCalib ()
virtual double	moveWait (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel)
	Move joint to position. More...
virtual double	moveToTorqueLimit (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel)
	Move joint until torque limit is reached. More...
virtual double	moveToLight (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel, byte_t byMask)
	Move joint until unoccluded optical limit position is detected. More...
virtual double	moveToDark (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel, byte_t byMask)
	Move joint until occluded optical limit position is detected. More...

Protected Member Functions
virtual int	calibrateJointTopDeadCenter (HekRobotJoint &joint)
	Calibrate a continuously rotating joint by top-dead-center electronic limits. More...
virtual int	calibrateJointByLimits (HekRobotJoint &joint)
	Calibrate a rovolute joint by electronic limits. More...
virtual int	calibByLimitsFinal (HekRobotJoint &joint, byte_t byOptMask, int nEdge)
	Perform final calibration by limits calculations and movements. More...
virtual int	calibrateJointByTorqueLimits (HekRobotJoint &joint)
	Calibrate joint by torque limits. More...
virtual int	calibrateJointByTrust (HekRobotJoint &joint)
	Calibrate joint by trust. More...
virtual int	fineTuneTDC (HekRobotJoint &joint, byte_t byOptMask, int nEdge, double &fPosition)
	Fine tune joint to top dead center of optical limit. More...
virtual int	fineTuneLimit (HekRobotJoint &joint, byte_t byOptMask, int nEdge, double &fDeltaPos)
	Fine tune joint to optical limit edge. More...
double	calcJointRatios (const std::string &strJointName1, const std::string &strJointName2)
	Calculate two joints' gear ratios. More...
virtual int	multiMoveToLight (const std::string &strJointName, int nDir, double fJointGoalPos, double fJointGoalVel, byte_t byMask)
	Move joint until unoccluded optical limit position is detected. More...
virtual int	multiMoveToDark (const std::string &strJointName, int nDir, double fJointGoalPos, double fJointGoalVel, byte_t byMask)
	Move joint until occluded optical limit position is detected. More...
virtual HekRobotJoint *	getHelpfulParentJoint (int nChildServoId)
	Get the parent joint (and link) that could aid in calibration. More...

Additional Inherited Members
Protected Attributes inherited from hekateros::HekCalib
HekRobot &	m_robot
	robot instance

Detailed Description

Hekateros robotic manipulator calibration by stretching class.

The hekateros will do a choreographed sequence of moves to find the limits or top-dead-centers for all joints.

Note

It is best if the user places the arm in the upright position and with not obstructions in the workspace prior to executing sequence.

Move Sequence:

• End effector open and close limits. (torque determines this range).
• Wrist rotation top dead center.
• Wrist pitch clockwise and counter-clockwise limits.
• Elbow clockwise and counter-clockwise limits.
• Shoulder clockwise and counter-clockwise limits.
• Base rotation top dead center.

Definition at line 83 of file hekCalibStretch.h.

Constructor & Destructor Documentation

hekateros::HekCalibStretch::HekCalibStretch ( HekRobot &  robot )

inline

Default initialization constructor.

Parameters

robot

Instance of Hekateros robot.

Definition at line 92 of file hekCalibStretch.h.

                                     : HekCalib(robot)
    {
    }

virtual hekateros::HekCalibStretch::~HekCalibStretch ( )

inlinevirtual

Destructor.

Definition at line 99 of file hekCalibStretch.h.

References calcJointRatios(), calibByLimitsFinal(), calibrate(), calibrateJointByLimits(), calibrateJointByTorqueLimits(), calibrateJointByTrust(), calibrateJointTopDeadCenter(), fineTuneLimit(), fineTuneTDC(), getHelpfulParentJoint(), multiMoveToDark(), and multiMoveToLight().

    {
    }

Member Function Documentation

double HekCalibStretch::calcJointRatios ( const std::string &  strJointName1, const std::string &  strJointName2  )

protected

Calculate two joints' gear ratios.

Parameters

strJointName1

First joint. strJointName2 Second joint.

Returns

joint1 gear ratio to joint 2 gear ratio.

Definition at line 1380 of file hekCalibStretch.cxx.

References hekateros::HekRobotJoint::m_fGearRatio.

Referenced by ~HekCalibStretch().

{
  HekRobotJoint  *pJoint;       // working joint description
  double          fGearRatios;  // gear ratio 1 to gear ratio 2

  if( (pJoint = m_robot.getArmJoint(strJointName1)) != NULL )
  {
    fGearRatios = pJoint->m_fGearRatio;
    if( (pJoint = m_robot.getArmJoint(strJointName2)) != NULL )
    {
      fGearRatios /= pJoint->m_fGearRatio;
    }
    else
    {
      fGearRatios = 0.0;
    }
  }
  else
  {
    fGearRatios = 0.0;
  }

  return fGearRatios;
}

int HekCalibStretch::calibByLimitsFinal ( HekRobotJoint &  joint, byte_t  byOptMask, int  nEdge  )

protectedvirtual

Perform final calibration by limits calculations and movements.

Parameters

joint	Robotic joint.
byOptMask	Joint optical limit bit mask;
nEdge	Which edge triggered limit switch. The minimum, unknown, or maximum leading edge is specified as < 0, 0, or > 0, respectively.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 674 of file hekCalibStretch.cxx.

References hekateros::degToRad(), hekateros::HekRobotJoint::m_bStopAtOptLimits, hekateros::HekOpticalLimit_T::m_fMaxEdgePos, hekateros::HekRobotJoint::m_fMaxPhyLimitRads, hekateros::HekRobotJoint::m_fMaxSoftLimitRads, hekateros::HekOpticalLimit_T::m_fMinEdgePos, hekateros::HekRobotJoint::m_fMinPhyLimitRads, hekateros::HekRobotJoint::m_fMinSoftLimitRads, hekateros::HekRobotJoint::m_fTicksPerJointRad, hekateros::HekRobotJoint::m_nMasterServoDir, hekateros::HekRobotJoint::m_nMasterServoId, hekateros::HekRobotJoint::m_nMaxPhyLimitOd, hekateros::HekRobotJoint::m_nMaxSoftLimitOd, hekateros::HekRobotJoint::m_nMinPhyLimitOd, hekateros::HekRobotJoint::m_nMinSoftLimitOd, hekateros::HekRobotJoint::m_strName, and hekateros::radToDeg().

Referenced by ~HekCalibStretch().

{
  // joint calibrating velocity (radians/second)
  static double TuneCalVel      = degToRad(20.0);

  // additional angle margin (radians)
  static double TuneMarginAngle = degToRad(10.0);

  string      strJointName;     // joint name
  int         nServoId;         // servo id
  bool        bIsReverse;       // odometer is [not] reversed

  HekOpticalLimit_T  *pLimit;       // limit info associated with mask

  double      fJointCurPos;     // user's starting/current position (radians)
  double      fJointCurVel;     // joint current velocity (radians/second) 
  double      fJointGoalPos;    // joint working goal position (radians)
  double      fJointCalibPos;   // joint calibrated position (radians)

  int         nServoCurPos;     // servo current position (odometer ticks)
  int         nServoCurSpeed;   // servo current speed (raw unitless)

  int         i;                // working index
  int         rc;               // return code

  strJointName  = joint.m_strName;
  nServoId      = joint.m_nMasterServoId;
  bIsReverse    = joint.m_nMasterServoDir == DYNA_DIR_CCW? false: true;
  pLimit        = m_robot.m_monitor.getJointLimitInfo(byOptMask);

  // fine tune edge
  if( (rc = fineTuneLimit(joint, byOptMask, nDir, fJointCalibPos)) < 0 )
  {
    LOGERROR("Joint %s (servo=%d): Fine tuning failed.",
          strJointName.c_str(), nServoId);
    return rc;
  }

  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  fJointGoalPos = fJointCurPos - fJointCalibPos;

  //
  // Special joint movements.
  //
  // Note: Slow down the elbow a tad to reduce hard stop at new zero point.
  //       Looks cleaner.
  //
  if( strJointName == "shoulder" )
  {
    string  strJointName2("elbow");
    double  fGearRatios = calcJointRatios(strJointName2, strJointName);
    m_robot.m_pKin->move(strJointName2, 0.0, 0.75 * fGearRatios * TuneCalVel);
  }

  LOGDIAG2("Joint %s (servo=%d): Move to the new zero point at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

  moveWait(strJointName, fJointGoalPos, TuneCalVel);

  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

  m_robot.m_pKin->resetServoOdometer(strJointName);

  LOGDIAG2("Joint %s (servo=%d): Odometer reset.",
        strJointName.c_str(), nServoId);

  //
  // Adjust limits and positions.
  //
  joint.m_fMinSoftLimitRads = pLimit->m_fMinEdgePos;
  joint.m_fMaxSoftLimitRads = pLimit->m_fMaxEdgePos;
  joint.m_nMinSoftLimitOd   = (int)(joint.m_fMinSoftLimitRads *
                                        joint.m_fTicksPerJointRad);
  joint.m_nMaxSoftLimitOd   = (int)(joint.m_fMaxSoftLimitRads *
                                        joint.m_fTicksPerJointRad);

  // Enable optical limit stopping for joint during normal operation.
  joint.m_bStopAtOptLimits = true;
 
  LOGDIAG2("Joint %s (servo=%d): Calibrated by optical limits:\n"
          "  Physical limits: [%.2lfdeg (od=%d), %.2lfdeg (od=%d)]\n"
          "      Soft limits: [%.2lfdeg (od=%d), %.2lfdeg (od=%d)].",
      strJointName.c_str(), nServoId,
      radToDeg(joint.m_fMinPhyLimitRads),  joint.m_nMinPhyLimitOd,
      radToDeg(joint.m_fMaxPhyLimitRads),  joint.m_nMaxPhyLimitOd,
      radToDeg(joint.m_fMinSoftLimitRads), joint.m_nMinSoftLimitOd,
      radToDeg(joint.m_fMaxSoftLimitRads), joint.m_nMaxSoftLimitOd);

  return HEK_OK;
}

int HekCalibStretch::calibrate ( )

virtual

Calibrate the Hekateros's robotic arm.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Implements hekateros::HekCalib.

Definition at line 112 of file hekCalibStretch.cxx.

References CalibOrder, hekateros::HekRobotJoint::m_eLimitTypes, and hekateros::HekRobotJoint::m_eOpState.

Referenced by hekateros::HekRobot::calibrate(), and ~HekCalibStretch().

{
  MapRobotJoints::iterator  pos;

  string          strJointName;
  HekRobotJoint  *pJoint;
  int             eLimitTypes;
  size_t          i;
  int             rc;

  // reset odometer to current arm pose
  m_robot.m_pKin->resetServoOdometersForAllJoints();

  //
  // Calibrate Hekateros joints in the specified calibration order.
  //
  for(i=0, rc=HEK_OK; (i<arraysize(CalibOrder)) && (rc >= 0); ++i)
  {
    strJointName = CalibOrder[i];

    // no joint in this product version with associated joint name
    if( (pJoint = m_robot.getArmJoint(strJointName)) == NULL )
    {
      continue;
    }

    // already calibrated
    if( pJoint->m_eOpState == HekOpStateCalibrated )
    {
      continue;
    }

    LOGDIAG2("\n----------- Calibrating Joint %s...", strJointName.c_str());

    eLimitTypes         = pJoint->m_eLimitTypes;
    pJoint->m_eOpState  = HekOpStateCalibrating;

    // one or more top dead center electronic switch limits
    if( eLimitTypes & HekLimitTypeElecTDC )
    {
      rc = calibrateJointTopDeadCenter(*pJoint);
    }

    // one or more electronic switch limiters
    else if( eLimitTypes & HekLimitTypeElec )
    {
      rc = calibrateJointByLimits(*pJoint);
    }

    // physical limits
    else if( eLimitTypes & HekLimitTypePhys )
    {
      rc = calibrateJointByTorqueLimits(*pJoint);
    }

    // no detectable limits - assume current position is the good position
    else if( eLimitTypes & HekLimitTypeNone )
    {
      rc = calibrateJointByTrust(*pJoint);
    }

    else if( eLimitTypes & HekLimitTypeAbsEnc )
    {
      LOGWARN("Joint %s: Absolute encoders are not supported yet.",
          strJointName.c_str());
    }

    // warning
    else
    {
      LOGWARN("Joint %s: Do not know how to calibrate this joint.",
          strJointName.c_str());
    }

    pJoint->m_eOpState =  rc == HEK_OK? HekOpStateCalibrated:
                                        HekOpStateUncalibrated;
  }

  return rc;
}

int HekCalibStretch::calibrateJointByLimits ( HekRobotJoint &  joint )

protectedvirtual

Calibrate a rovolute joint by electronic limits.

The joint has an electronic limit switch and a occlusion band that marks the minimum and maximum rotation limits.

The joint is rotated until the electronic limit are triggered to find these min,max limits.

When the joint calibration moves are finished, the joint is repositioned at its new home position.

Heuristic:

light = optical switch unoccluded (closed)
dark = optical switch occluded (open)

1. If starting in the dark.
   1. Get the park position from spec to determine best guess direction.
   2. Try to rotate X degrees to find the light, stopping either when first light is detected, torqued limit is reached, or have rotated X °.
   3. If in the light, goto 4.
   4. Else if over torqued (probably at physical limit).
      1. Reverse rotate X degrees to find the light, stopping either when first light is detected, torqued limit is reached, or have rotated X °.
      2. If in the light, goto 4.
   5. Else cannot get out of the dark, return with failure.
2. Else starting in the light.
   1. Rotate in minimum direction Y degrees to find the dark, stopping either when first dark is detected, torqued limit is reached, or have rotated Y °.
   2. If in the dark, goto 4.
   3. Else probably obstructed.
      1. While less than MAX tries.
         1. Rotate in maximum direction Y degrees to find the dark, stopping either when first dark is detected, torqued limit is reached, or have rotated Y °.
         2. If in the dark, goto 4.
         3. Else if have helpful parent joint (e.g. shoulder for elbow).
            1. Rotate parent joint position Z degrees.
            2. If no movement return with failure.
         4. Else break loop.
3. Return with failure.
4. Final Stage.
   1. Fine tune to find edge precisely.
   2. While less than MAX tries.
      1. Rotate to 0 °.
      2. If reached 0 °.
         1. Reset odometer.
         2. Return with success.
      3. Else if have helpful parent joint.
         1. Rotate parent joint position Z degrees.
         2. If no movement return with failure.
      4. Else break loop.
   3. Return with failure

Parameters

joint

Robotic joint to calibrate.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 433 of file hekCalibStretch.cxx.

References hekateros::degToRad(), hekateros::getDarkOpticalLimits(), hekateros::getLitOpticalLimits(), hekateros::HekRobotJoint::m_bStopAtOptLimits, hekateros::HekRobotJoint::m_byOptLimitMask, hekateros::HekOpticalLimit_T::m_fMaxBlackPos, hekateros::HekRobotJoint::m_fMaxPhyLimitRads, hekateros::HekOpticalLimit_T::m_fMinBlackPos, hekateros::HekRobotJoint::m_fMinPhyLimitRads, hekateros::HekRobotJoint::m_fParkPosRads, hekateros::HekRobotJoint::m_nMasterServoDir, hekateros::HekRobotJoint::m_nMasterServoId, hekateros::HekRobotJoint::m_strName, and hekateros::radToDeg().

Referenced by ~HekCalibStretch().

{
  // joint calibrating velocity (radians/second)
  static double TuneCalVel      = degToRad(20.0);

  // additional angle margin (radians)
  static double TuneMarginAngle = degToRad(50.0);

  string      strJointName;     // joint name
  int         nServoId;         // servo id
  bool        bIsReverse;       // odometer is [not] reversed

  double      fJointCurPos;     // user's starting/current position (radians)
  double      fJointCurVel;     // joint current velocity (radians/second) 
  double      fJointGoalPos;    // joint working goal position (radians)
  double      fJointCalibPos;   // joint calibrated position (radians)

  int         nServoCurPos;     // servo current position (odometer ticks)
  int         nServoCurSpeed;   // servo current speed (raw unitless)

  byte_t      byOptMask;        // optical limits mask
  byte_t      byOptBits;        // optical limits bits
  HekOpticalLimit_T *pLimit;    // limit info associated with mask

  int         nDir;             // working direction
  double      fAngle1;          // working angle 1
  double      fAngle2;          // working angle 2

  int         i;                // working index
  int         rc = HEK_OK;      // return code

  //
  // Initialize fixed working data.
  //
  // Note: Only work with one optical limit per joint for now. Can extend if
  // needed.
  //
  strJointName  = joint.m_strName;
  nServoId      = joint.m_nMasterServoId;
  bIsReverse    = joint.m_nMasterServoDir == DYNA_DIR_CCW? false: true;
  byOptMask     = joint.m_byOptLimitMask[0];
  pLimit        = m_robot.m_monitor.getJointLimitInfo(byOptMask);

  LOGDIAG2("Joint %s (servo=%d): Calibrating by optical limits.",
          strJointName.c_str(), joint.m_nMasterServoId);

  //
  // Sanity check.
  //
  if( byOptMask == 0x00 )
  {
    LOGERROR("Joint %s (servo=%d): No optical limit bits set, spec correct?",
          strJointName.c_str(), nServoId);
    return -HEK_ECODE_INTERNAL;
  }

  // Disable optical limit auto-stopping for the joint during calibration.
  joint.m_bStopAtOptLimits = false;

  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Starting at %.2lf uncalibrated degrees.",
      strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

  //
  // Get current optical limit values.
  //
  byOptBits = getDarkOpticalLimits(m_robot.m_monitor.getJointLimitBits(),
                                   byOptMask);

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Starting in the dark.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  if( byOptBits != 0x00 )
  {
    LOGDIAG2("Joint %s (servo=%d): "
           "Determining joint optical limit dark-to-light edge.",
      strJointName.c_str(), nServoId);

    // best guess direction based on configured parked position for this joint.
    nDir = joint.m_fParkPosRads < 0? 1: -1;

    //
    // Calculate the maximum of the minimum travel angles to reach an edge.
    //
    fAngle1 = pLimit->m_fMinBlackPos   - joint.m_fMinPhyLimitRads;
    fAngle2 = joint.m_fMaxPhyLimitRads - pLimit->m_fMaxBlackPos;

    if( fAngle2 > fAngle1 )
    {
      fAngle1 = fAngle2;
    }

    fAngle1 += TuneMarginAngle;

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    fJointGoalPos = fJointCurPos + (double)nDir * fAngle1;

    LOGDIAG2("Joint %s (servo=%d): Move to light for <= %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

    // Move to occlusion band edge.
    multiMoveToLight(strJointName, nDir, fJointGoalPos, TuneCalVel, byOptMask);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

    // optical bits
    byOptBits = m_robot.m_monitor.getJointLimitBits();

    //
    // Did't find the light, probably guessed wrong direction and joint is at
    // a physical limit. Reverse direction.
    //
    if( !getLitOpticalLimits(byOptBits, byOptMask) )
    {
      LOGDIAG2("Joint %s (servo=%d): "
              "Did not find the light. Reverse direction.",
        strJointName.c_str(), nServoId);

      nDir = -nDir;

      m_robot.m_pKin->getJointCurPosVel(strJointName,
                                        fJointCurPos, fJointCurVel);

      fJointGoalPos = fJointCurPos + (double)nDir * fAngle1;

      LOGDIAG2("Joint %s (servo=%d): Move to light <= %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

      multiMoveToLight(strJointName, nDir, fJointGoalPos, TuneCalVel,
                      byOptMask);

      m_robot.m_pKin->getJointCurPosVel(strJointName,
                                        fJointCurPos, fJointCurVel);

      LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));
    }

    // optical bits
    byOptBits = m_robot.m_monitor.getJointLimitBits();

    //
    // Got an edge, perform final calibration fine tuning.
    //
    if( getLitOpticalLimits(byOptBits, byOptMask) )
    {
      m_robot.m_pKin->getJointCurPosVel(strJointName,
                                        fJointCurPos, fJointCurVel);

      LOGDIAG2("Joint %s (servo=%d): Got a light edge at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

      return calibByLimitsFinal(joint, byOptMask, nDir);
    }

    //
    // Still no light. Give up.
    //
    else
    {
      LOGERROR("Joint %s (servo=%d): "
               "Could not find optical limit switch light.",
          strJointName.c_str(), nServoId);
      return -HEK_ECODE_NO_EXEC;
    }
  }


  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Starting in the light.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  else
  {
    // best guess direction
    nDir = nServoId == HekServoIdShoulderL? -1: 1;

    //
    // Calculate the maximum angle to reach an edge.
    //
    fAngle1  = pLimit->m_fMaxBlackPos - pLimit->m_fMinBlackPos;
    fAngle1 += TuneMarginAngle;

    LOGDIAG2("Joint %s (servo=%d): "
           "Determining joint optical limit light-to-dark minimum edge.",
      strJointName.c_str(), nServoId);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    fJointGoalPos = fJointCurPos + (double)nDir * fAngle1;

    LOGDIAG2("Joint %s (servo=%d): Move to dark for <= %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

    // Move to occlusion band edge.
    multiMoveToDark(strJointName, nDir, fJointGoalPos, TuneCalVel, byOptMask);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

    // optical bits
    byOptBits = m_robot.m_monitor.getJointLimitBits();

    //
    // Got an edge, perform final calibration fine tuning.
    //
    if( getDarkOpticalLimits(byOptBits, byOptMask) )
    {
      m_robot.m_pKin->getJointCurPosVel(strJointName,
                                        fJointCurPos, fJointCurVel);

      LOGDIAG2("Joint %s (servo=%d): Got a dark edge at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

      return calibByLimitsFinal(joint, byOptMask, nDir);
    }

    else
    {
      LOGERROR("Joint %s (servo=%d): "
             "Could not find optical limit occlusion edge.",
          strJointName.c_str(), nServoId);

      return -HEK_ECODE_NO_EXEC;
    }
  }

  LOGERROR("BUG: Joint %s (servo=%d): Should not be here.",
          strJointName.c_str(), nServoId);

  return -HEK_ECODE_INTERNAL;
}

int HekCalibStretch::calibrateJointByTorqueLimits ( HekRobotJoint &  joint )

protectedvirtual

Calibrate joint by torque limits.

The joint is rotated to its minimum and maximum physical end points. When the torque reaches a software defined limit, an endpoint is considered reached.

From the joint specification and detected limits, the home position at 0 ° is fine tuned.

When the joint calibration moves are finished, the joint is repositioned at its new home position.

Heuristic:

1. Rotate in minimum direction until torque limit reached.
2. Mark position.
3. Rotate in maximum direction until torque limit reached.
4. Mark position.
5. Calculate 0 ° based on expected min,max positions and detected min,max positions.
6. Rotate to 0 °.
7. Reset odomter.

Parameters

joint

Robotic joint to calibrate.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 771 of file hekCalibStretch.cxx.

References hekateros::degToRad(), HEK_TRY_GET_SERVO, hekateros::HekRobotJoint::m_bIsServoContinuous, hekateros::HekRobotJoint::m_fCalibPosRads, hekateros::HekRobotJoint::m_fMaxPhyLimitRads, hekateros::HekRobotJoint::m_fMaxSoftLimitRads, hekateros::HekRobotJoint::m_fMinPhyLimitRads, hekateros::HekRobotJoint::m_fMinSoftLimitRads, hekateros::HekRobotJoint::m_nMasterServoDir, hekateros::HekRobotJoint::m_nMasterServoId, hekateros::HekRobotJoint::m_nMaxPhyLimitOd, hekateros::HekRobotJoint::m_nMaxSoftLimitOd, hekateros::HekRobotJoint::m_nMinPhyLimitOd, hekateros::HekRobotJoint::m_nMinSoftLimitOd, hekateros::HekRobotJoint::m_strName, M_TAU, and hekateros::radToDeg().

Referenced by ~HekCalibStretch().

{
  // joint calibrating velocity (radians/second)
  static double TuneCalVel  = degToRad(20.0);

  // backoff angle margin (radians)
  static double TuneBackoff = degToRad(0.1);

  string      strJointName;     // joint name
  int         nServoId;         // servo id
  bool        bIsReverse;       // odometer is [not] reversed
  DynaServo  *pServo;           // servo control

  double      fMinLimit;        // joint minimum position
  double      fMaxLimit;        // joint maximum position
  double      fJointRange;      // joint range of motion

  double      fJointStartPos;   // user's starting position (radians)
  double      fJointCurPos;     // joint current position (radians)
  double      fJointCurVel;     // joint current velocity (radians/second) 
  double      fJointGoalPos;    // joint working goal position (radians)

  int         nServoPos;        // working servo position (odometer ticks);

  strJointName  = joint.m_strName;
  nServoId      = joint.m_nMasterServoId;
  bIsReverse    = joint.m_nMasterServoDir == DYNA_DIR_CCW? false: true;

  LOGDIAG2("Joint %s (servo=%d): Calibrating by torque limits.",
          strJointName.c_str(), joint.m_nMasterServoId);

  HEK_TRY_GET_SERVO(nServoId, pServo);

  // starting position
  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointStartPos, fJointCurVel);
 
  LOGDIAG2("Joint %s (servo=%d): Starting at %.2lf uncalibrated degrees.",
      strJointName.c_str(), nServoId, radToDeg(fJointStartPos));

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Find minimum joint limit.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  LOGDIAG2("Joint %s (servo=%d): "
           "Determining minimum joint angle by torque limit.",
      joint.m_strName.c_str(), nServoId);

  fJointCurPos = fJointStartPos;

  //
  // A continuous mode servo must have its joint minimum limit within 360
  // degreees of joint rotation from the current position. Go to this endpoint
  // until the torque limit is found.
  //
  if( joint.m_bIsServoContinuous )
  {
    fJointGoalPos = -M_TAU + -fabs(fJointCurPos);
  }

  //
  // A servo mode servo has encoder endpoints that are known and finite. Go to
  // the appropriate endpoint until the torque limit is found.
  //
  else
  {
    if( (nServoPos = pServo->CalcOdometerAtEncMin()) > 0 )
    {
      nServoPos = pServo->CalcOdometerAtEncMax();
    }
    fJointGoalPos = m_robot.m_pKin->servoPosToJointPos(strJointName, nServoPos);
  }

  LOGDIAG2("Joint %s (servo=%d): Move to minimum >= %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

  moveToTorqueLimit(strJointName, fJointGoalPos, TuneCalVel);

  // save minimum limit position
  m_robot.m_pKin->getJointCurPosVel(strJointName, fMinLimit, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Minimum reached at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fMinLimit));

  //
  // Off load servo by moving back to starting calibration position.
  //
  LOGDIAG2("Joint %s (servo=%d): Move back to start at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointStartPos));

  moveWait(strJointName, fJointStartPos, TuneCalVel);

  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Find maximum limit.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  LOGDIAG2("Joint %s (servo=%d): "
           "Determining maximum joint angle by torque limit.",
      joint.m_strName.c_str(), nServoId);

  // current position
  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
  
  //
  // A continuous mode servo must have its joint minimum limit within 360
  // degreees of joint rotation from the current position. Go to this endpoint
  // until the torque limit is found.
  //
  if( joint.m_bIsServoContinuous )
  {
    fJointGoalPos = M_TAU + fabs(fJointCurPos);
  }

  //
  // A servo mode servo has encoder endpoints that are known and finite. Go to
  // the appropriate endpoint until the torque limit is found.
  //
  else
  {
    if( (nServoPos = pServo->CalcOdometerAtEncMin()) < 0 )
    {
      nServoPos = pServo->CalcOdometerAtEncMax();
    }
    fJointGoalPos = m_robot.m_pKin->servoPosToJointPos(strJointName, nServoPos);
  }

  LOGDIAG2("Joint %s (servo=%d): Move to maximum <= %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

  moveToTorqueLimit(strJointName, fJointGoalPos, TuneCalVel);

  // save maximum limit position
  m_robot.m_pKin->getJointCurPosVel(strJointName, fMaxLimit, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Maximum reached at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fMaxLimit));

  //
  // Off load servo by moving back to starting calibration position
  //
  LOGDIAG2("Joint %s (servo=%d): Move back again to start at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointStartPos));

  moveWait(strJointName, fJointStartPos, TuneCalVel);

  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Set zero point and limits.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  //
  // Joint range of motion.
  //
  fJointRange = fMaxLimit - fMinLimit;

  //
  // Move to new zero point position.
  //
  fJointGoalPos = fMinLimit;

  LOGDIAG2("Joint %s (servo=%d): Move to the new zero point at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

  moveToTorqueLimit(strJointName, fJointGoalPos, TuneCalVel);

  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

  // 
  // Reset odometer to new zero degree position.
  //
  m_robot.m_pKin->resetServoOdometer(strJointName);

  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Odometer reset.",
        strJointName.c_str(), nServoId);

  //
  // Adjust limits and positions to adjusted zero degree point.
  //
  joint.m_fMinPhyLimitRads  = fJointCurPos;
  joint.m_fMaxPhyLimitRads  = fJointCurPos + fJointRange;

  joint.m_nMinPhyLimitOd    = m_robot.m_pKin->jointPosToServoPos(
                                                    strJointName,
                                                    joint.m_fMinPhyLimitRads);
  joint.m_nMaxPhyLimitOd    = m_robot.m_pKin->jointPosToServoPos(
                                                    strJointName,
                                                    joint.m_fMaxPhyLimitRads);

  joint.m_fMinSoftLimitRads   = joint.m_fMinPhyLimitRads + TuneBackoff;
  joint.m_fMaxSoftLimitRads   = joint.m_fMaxPhyLimitRads - TuneBackoff;

  // hack - should be in spec
  if( nServoId == HekServoIdGraboid )
  {
    joint.m_fMaxSoftLimitRads = joint.m_fMaxPhyLimitRads - degToRad(1.0);
  }


  joint.m_nMinSoftLimitOd   = m_robot.m_pKin->jointPosToServoPos(
                                                    strJointName,
                                                  joint.m_fMinSoftLimitRads);
  joint.m_nMaxSoftLimitOd   = m_robot.m_pKin->jointPosToServoPos(
                                                  strJointName,
                                                  joint.m_fMaxSoftLimitRads);
  
  
  //
  // Finally move to pre-defined calibration zero point position.
  //
  if( joint.m_fCalibPosRads != 0.0 )
  { 
    LOGDIAG2("Joint %s (servo=%d): Move to calibrated zero pt=%.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(joint.m_fCalibPosRads));

    moveWait(strJointName, joint.m_fCalibPosRads, TuneCalVel);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));
  }

  //
  // Calibrated.
  //
  LOGDIAG2("Joint %s (servo=%d): Calibrated by torque:\n"
          "  Physical limits: [%.2lfdeg (od=%d), %.2lfdeg (od=%d)]\n"
          "      Soft limits: [%.2lfdeg (od=%d), %.2lfdeg (od=%d)].",
      joint.m_strName.c_str(), nServoId,
      radToDeg(joint.m_fMinPhyLimitRads),  joint.m_nMinPhyLimitOd,
      radToDeg(joint.m_fMaxPhyLimitRads),  joint.m_nMaxPhyLimitOd,
      radToDeg(joint.m_fMinSoftLimitRads), joint.m_nMinSoftLimitOd,
      radToDeg(joint.m_fMaxSoftLimitRads), joint.m_nMaxSoftLimitOd);

  return HEK_OK;
}

int HekCalibStretch::calibrateJointByTrust ( HekRobotJoint &  joint )

protectedvirtual

Calibrate joint by trust.

The joint is assumed to be already position at zero degrees by the user.

Parameters

joint

Robotic joint to calibrate.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 1021 of file hekCalibStretch.cxx.

References hekateros::HekRobotJoint::m_eJointType, hekateros::HekRobotJoint::m_fMaxPhyLimitRads, hekateros::HekRobotJoint::m_fMinPhyLimitRads, hekateros::HekRobotJoint::m_nMasterServoDir, hekateros::HekRobotJoint::m_nMasterServoId, hekateros::HekRobotJoint::m_nMaxPhyLimitOd, hekateros::HekRobotJoint::m_nMinPhyLimitOd, hekateros::HekRobotJoint::m_strName, and hekateros::radToDeg().

Referenced by ~HekCalibStretch().

{
  string      strJointName;   // joint name
  int         nServoId;       // servo id
  bool        bIsReverse;     // odometer is [not] reversed

  double      fJointCurPos;   // joint current position (radians)
  double      fJointCurVel;   // joint current velocity (radians/second) 

  int         nServoCurPos;   // servo current position (odometer ticks)
  int         nServoCurSpeed; // servo current speed (raw unitless)

  strJointName  = joint.m_strName;
  nServoId      = joint.m_nMasterServoId;
  bIsReverse    = joint.m_nMasterServoDir == DYNA_DIR_CCW? false: true;

  LOGDIAG2("Joint %s (servo=%d): Calibrating by trust.",
          strJointName.c_str(), joint.m_nMasterServoId);

  // 
  // Reset odometer to the current position
  //
  m_robot.m_pKin->resetServoOdometer(strJointName);

  // starting position
  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
  m_robot.m_pKin->getServoCurPosSpeed(strJointName,
                                        nServoCurPos,
                                        nServoCurSpeed);

  if( joint.m_eJointType == HekJointTypeContinuous )
  {
    LOGDIAG2("Joint %s (servo=%d): Calibrated by trust:\n"
            "  Top dead center: %.2lf deg (od=%d).",
      strJointName.c_str(), nServoId, radToDeg(fJointCurPos), nServoCurPos);
  }
  else
  {
    LOGDIAG2("Joint %s (servo=%d): Calibrated by trust:\n"
         "  Rotation [min, max] limits: [%.2lfdeg (od=%d), %.2lfdeg (od=%d)].",
            strJointName.c_str(), nServoId,
            radToDeg(joint.m_fMinPhyLimitRads), joint.m_nMinPhyLimitOd,
            radToDeg(joint.m_fMaxPhyLimitRads), joint.m_nMaxPhyLimitOd);
  }

  return HEK_OK;
}

int HekCalibStretch::calibrateJointTopDeadCenter ( HekRobotJoint &  joint )

protectedvirtual

Calibrate a continuously rotating joint by top-dead-center electronic limits.

The joint has optical limit switch(es) at defined location, typically at 0 ° and/or, at 180 °. A small occlusion band triggers the (occludes) the switch(es).

The joint is rotated until the electronic limits are triggered to find top-dead-center.

When the joint calibration moves are finished, the joint is repositioned at its new home position.

Heuristic:

light = optical switch unoccluded (closed)
dark = optical switch occluded (open)

1. If starting in the light
   1. Rotate a small number of degrees in minimum direction.
   2. If light, rotate joint in the oposite direction 360 °.
   3. If light, return with failure.
2. Started in or moved to the dark.
   1. Now find an edge of occlusion band (dark to light).
   2. Move back to find opposite edge.
   3. Calculated center of band from both edge positions and rotate there.
   4. Rotate to 0 °.
   5. Reset odometer.
   6. Return with success.

Parameters

joint

Robotic joint to calibrate.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 193 of file hekCalibStretch.cxx.

References hekateros::degToRad(), hekateros::getDarkOpticalLimits(), hekateros::HekRobotJoint::m_bStopAtOptLimits, hekateros::HekRobotJoint::m_byOptLimitMask, hekateros::HekRobotJoint::m_eJointType, hekateros::HekRobotJoint::m_fCalibPosRads, hekateros::HekRobotJoint::m_fMaxSoftLimitRads, hekateros::HekRobotJoint::m_fMinSoftLimitRads, hekateros::HekRobotJoint::m_fTicksPerJointRad, hekateros::HekRobotJoint::m_nMasterServoDir, hekateros::HekRobotJoint::m_nMasterServoId, hekateros::HekRobotJoint::m_nMaxSoftLimitOd, hekateros::HekRobotJoint::m_nMinSoftLimitOd, hekateros::HekRobotJoint::m_strName, M_TAU, and hekateros::radToDeg().

Referenced by ~HekCalibStretch().

{
  // joint calibrating velocity (radians/second)
  static double TuneCalVel    = degToRad(20.0);

  // quick try in the minimum direction (radians)
  static double TuneMinAngle  = degToRad(20.0);

  string      strJointName;     // joint name
  int         nServoId;         // servo id
  bool        bIsReverse;       // odometer is [not] reversed

  double      fJointCurPos;     // user's starting/current position (radians)
  double      fJointCurVel;     // joint current velocity (radians/second) 
  double      fJointGoalPos;    // joint working goal position (radians)
  double      fJointCalibPos;   // joint calibrated position (radians)

  int         nServoCurPos;     // servo current position (odometer ticks)
  int         nServoCurSpeed;   // servo current speed (raw unitless)

  byte_t      byOptMask;        // optical limits mask
  byte_t      byOptBits;        // optical limits bits
  int         nEdge;            // which edge triggered

  int         i;                // working index
  int         rc = HEK_OK;      // return code

  strJointName  = joint.m_strName;
  nServoId      = joint.m_nMasterServoId;
  bIsReverse    = joint.m_nMasterServoDir == DYNA_DIR_CCW? false: true;

  LOGDIAG2("Joint %s (servo=%d): Calibrating by TDC optical limits.",
          strJointName.c_str(), joint.m_nMasterServoId);

  //
  // Only continuous joints support tdc limits
  //
  if( joint.m_eJointType != HekJointTypeContinuous )
  {
    LOGERROR("Joint %s (servo=%d): Not a continuous joint.",
          strJointName.c_str(), joint.m_nMasterServoId);
    return -HEK_ECODE_NO_EXEC;
  }

  //
  // Build optical limits bit mask.
  //
  for(i=0, byOptMask=0x00; i<HekOptLimitMaxPerJoint; ++i)
  {
    byOptMask |= joint.m_byOptLimitMask[i];
  }

  //
  // Sanity check
  //
  if( byOptMask == 0x00 )
  {
    LOGERROR("Joint %s (servo=%d): No optical limit bits set, spec correct?",
          strJointName.c_str(), nServoId);
    return -HEK_ECODE_INTERNAL;
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Find joint top-dead-center position.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
  LOGDIAG2("Joint %s (servo=%d): "
           "Determining joint top-dead-center by optical limit(s).",
      strJointName.c_str(), nServoId);

  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Starting at uncalibrated %.2lf degrees.",
      strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

  // Never auto-stop at TDC limits.
  joint.m_bStopAtOptLimits = false;

  byOptBits = getDarkOpticalLimits(m_robot.m_monitor.getJointLimitBits(),
                                   byOptMask);

  //
  // Starting in the light.  First try a small degree of rotation in the
  // minimum direction to find an optical limit dark edge.
  //
  if( byOptBits == 0x00 )
  {
    LOGDIAG2("Joint %s (servo=%d): "
           "Determining joint optical limit light-to-dark edge.",
      strJointName.c_str(), nServoId);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    fJointGoalPos = fJointCurPos - TuneMinAngle;
    nEdge         = 1;  // move will detect edge on maximum side

    LOGDIAG2("Joint %s (servo=%d): Move to the dark for <= %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

    moveToDark(strJointName, fJointGoalPos, TuneCalVel, byOptMask);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));
  }

  byOptBits = getDarkOpticalLimits(m_robot.m_monitor.getJointLimitBits(),
                                   byOptMask);

  //
  // Did not find the optical limit. Do a complete spin in the opposite
  // direction to find a limit.
  //
  if( byOptBits == 0x00 )
  {
    LOGDIAG2("Joint %s (servo=%d): Did not find the dark. Reverse direction.",
        strJointName.c_str(), nServoId);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    fJointGoalPos = fJointCurPos + M_TAU;
    nEdge         = -1;   // move will detect edge on minimum side

    LOGDIAG2("Joint %s (servo=%d): "
            "Move to the dark for <= %.2lf degrees in the maximum direction.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

    moveToDark(strJointName, fJointGoalPos, TuneCalVel, byOptMask);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));
  }

  byOptBits = getDarkOpticalLimits(m_robot.m_monitor.getJointLimitBits(),
                                   byOptMask);

  //
  // Make sure that an optical limits has been found.
  //
  if( byOptBits == 0x00 )
  {
    LOGERROR("Joint %s (servo=%d): Did not find any optical limits.",
          strJointName.c_str(), nServoId);
    return -HEK_ECODE_NO_EXEC;
  }

  //
  // Fine tune center of optical limit.
  //
  for(i=0, byOptMask=0x00; i<HekOptLimitMaxPerJoint; ++i)
  {
    if( byOptBits & joint.m_byOptLimitMask[i] )
    {
      rc = fineTuneTDC(joint, joint.m_byOptLimitMask[i], nEdge, fJointCalibPos);
      break;
    }
  }

  if( rc < 0 )
  {
    LOGERROR("Joint %s (servo=%d): Fine tuning failed.",
          strJointName.c_str(), nServoId);
    return rc;
  }

  //
  // Move to 0 degrees the shortest way.
  //
  if( fJointCalibPos != 0.0 )
  {
    if( fJointCalibPos < M_PI )
    {
      fJointGoalPos = fJointCalibPos;
    }
    else
    {
      fJointGoalPos = M_TAU - fJointCalibPos;
    }

    LOGDIAG2("Joint %s (servo=%d): "
            "Move to the new zero point at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

    moveWait(strJointName, fJointGoalPos, TuneCalVel);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));
  }

  // 
  // Reset odometer to new zero degree position.
  //
  m_robot.m_pKin->resetServoOdometer(strJointName);

  LOGDIAG2("Joint %s (servo=%d): Odometer reset.",
        strJointName.c_str(), nServoId);

  //
  // Adjust limits and positions to adjusted zero degree point.
  //
  joint.m_fMinSoftLimitRads = 0.0;
  joint.m_fMaxSoftLimitRads = M_TAU;
  joint.m_nMinSoftLimitOd   = 0;
  joint.m_nMaxSoftLimitOd   = (int)(joint.m_fMaxSoftLimitRads *
                                    joint.m_fTicksPerJointRad);
 
  //
  // Finally move to the pre-defined calibration zero point position.
  //
  if( joint.m_fCalibPosRads != 0.0 )
  { 
    LOGDIAG2("Joint %s (servo=%d): Move to calibrated zero pt=%.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(joint.m_fCalibPosRads));

    moveWait(strJointName, joint.m_fCalibPosRads, TuneCalVel);

   m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));
  }

  //
  // Calibrated.
  //
  m_robot.m_pKin->getServoCurPosSpeed(strJointName,
                                      nServoCurPos, nServoCurSpeed);

  LOGDIAG2("Joint %s (servo=%d): Calibrated by TDC optical limits:\n"
        "  Top dead center: %.02lfdeg (od=%d).",
      strJointName.c_str(), nServoId, 0.0, nServoCurPos);

  return HEK_OK;
}

int HekCalibStretch::fineTuneLimit ( HekRobotJoint &  joint, byte_t  byOptMask, int  nEdge, double &  fDeltaPos  )

protectedvirtual

Fine tune joint to optical limit edge.

Limit Fine Tuning:

*  fine tune step           occlusion band
*                       _____________________
*   0 (start)           |     *             |
*                       |     *             |
*   1 lit_edge      * <------ *             |
*                   *   |                   |
*   2 dark_edge     * ----> *               |
*                       |___________________|
*
* Or:
*
*  fine tune step           occlusion band
*                       _____________________
*   0 start         *   |                   |
*                   *   |                   |
*   2 dark_edge     * ----> *               |
*                       |___________________|
* 

Parameters

	joint	Robotic joint.
	byOptMask	Joint optical limit bit mask;
	nEdge	Which edge triggered limit switch. The minimum, unknown, or maximum leading edge is specified as < 0, 0, or > 0, respectively.
[out]	fDeltaPos	Delta position (radians) from current to zero point as determined by the optical limit.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 1254 of file hekCalibStretch.cxx.

References hekateros::degToRad(), hekateros::getDarkOpticalLimits(), hekateros::getLitOpticalLimits(), hekateros::HekOpticalLimit_T::m_fMaxEdgePos, hekateros::HekOpticalLimit_T::m_fMinEdgePos, hekateros::HekRobotJoint::m_nMasterServoId, hekateros::HekRobotJoint::m_strName, and hekateros::radToDeg().

Referenced by ~HekCalibStretch().

{
  // joint calibrating velocity (radians/second)
  static double TuneCalVel  = degToRad(5.0);

  // tuning slop (radians)
  static double TuneFudge  = degToRad(10.0);

  string      strJointName;     // joint name
  int         nServoId;         // servo id

  HekOpticalLimit_T  *pLimit;   // limit info associated with mask
  byte_t      byOptBits;        // optical limits bits

  double      fJointCurPos;     // joint current position (radians)
  double      fJointCurVel;     // joint current velocity (radians/second) 
  double      fJointGoalPos;    // joint working goal position (radians)
  double      fLightPos;        // occlusion band light position (radian)
  double      fDarkPos;         // occlusion band dark position (radian)
  double      fEdgePos;         // occlusion band edge position (radians)

  int         rc = HEK_OK;  // return code

  strJointName  = joint.m_strName;
  nServoId      = joint.m_nMasterServoId;
  pLimit        = m_robot.m_monitor.getJointLimitInfo(byOptMask);

  m_robot.m_pKin->stop(strJointName);

  // optical bits
  byOptBits = m_robot.m_monitor.getJointLimitBits();

  // need to start in the light
  if( !getLitOpticalLimits(byOptBits, byOptMask) )
  {
    nDir = -nDir;

    // current position
    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    fJointGoalPos = fJointCurPos + (double)nDir * TuneFudge;

    LOGDIAG2("Joint %s (servo=%d): "
      "Fine tune move for <= %.2lf degrees to find light edge.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

    moveToLight(strJointName, fJointGoalPos, TuneCalVel, byOptMask);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fLightPos, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Stopped at first light at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fLightPos));
  }

  // optical bits
  byOptBits = m_robot.m_monitor.getJointLimitBits();

  // sanity check
  if( getLitOpticalLimits(byOptBits, byOptMask) == 0x00 )
  {
    LOGERROR("Joint %s (servo=%d): Cannot find occlusion band edge.",
          strJointName.c_str(), nServoId);
    return -HEK_ECODE_NO_EXEC;
  }

  // move to dark
  nDir = -nDir;

  // current position
  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  fJointGoalPos = fJointCurPos + (double)nDir * TuneFudge;

  LOGDIAG2("Joint %s (servo=%d): "
      "Fine tune move for <= %.2lf degrees to find dark edge.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

  moveToDark(strJointName, fJointGoalPos, TuneCalVel, byOptMask);

  m_robot.m_pKin->getJointCurPosVel(strJointName, fDarkPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Stopped at first dark at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fDarkPos));

  // optical bits
  byOptBits = m_robot.m_monitor.getJointLimitBits();

  //
  // Didn't find the dark past the edge.
  //
  if( getDarkOpticalLimits(byOptBits, byOptMask) == 0x00 )
  {
    if( m_robot.m_pKin->hasOverTorqueCondition(strJointName) )
    {
      LOGERROR("Joint %s (servo=%d): Cannot calibrate: Joint is obstucted.",
          strJointName.c_str(), nServoId);
      rc = -HEK_ECODE_COLLISION;
    }
    else
    {
      LOGERROR("Joint %s (servo=%d): Cannot calibrate: "
               "Did not find the dark edge of occlusion band.",
          strJointName.c_str(), nServoId);
      rc = -HEK_ECODE_NO_EXEC;
    }
  }

  //
  // Set delta joint position as specified in spec. 
  //
  fEdgePos = (fLightPos + fDarkPos) / 2.0;
  if( nDir > 0 )
  {
    fDeltaPos = pLimit->m_fMaxEdgePos + fDarkPos - fEdgePos;
  }
  else
  {
    fDeltaPos = pLimit->m_fMinEdgePos + fEdgePos - fDarkPos;
  }

  return rc;
}

int HekCalibStretch::fineTuneTDC ( HekRobotJoint &  joint, byte_t  byOptMask, int  nEdge, double &  fPosition  )

protectedvirtual

Fine tune joint to top dead center of optical limit.

The position of the joint must be in the dark, occluded region of a band prior to calling this function.

TDC Fine Tuning:

*  fine tune step           occlusion band
*                       _____________________
*   0 (start)           |     *             |
*                       |     *             |
*   1 lit_edge_0    * <------ *             |
*                   *   |                   |
*   2 dark_edge_0   * ----> *               |
*                       |   *               |
*   3 lit_edge_1        |   * ----------------> *
*                       |                   |   *
*   4 center            |         * <---------- *
*                       |___________________|
*
* 

Parameters

	joint	Robotic joint.
	byOptMask	Joint optical limit bit mask;
	nEdge	Which edge triggered limit switch. The minimum, unknown, or maximum leading edge is specified as < 0, 0, or > 0, respectively. Not used
[out]	fPosition	Difference of determined uncalibrated joint position at TDC and the designed TDC (radians).

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 1069 of file hekCalibStretch.cxx.

References hekateros::degToRad(), hekateros::getLitOpticalLimits(), hekateros::HekOpticalLimit_T::m_fCenterPos, hekateros::HekOpticalLimit_T::m_fMaxEdgePos, hekateros::HekOpticalLimit_T::m_fMinEdgePos, hekateros::HekRobotJoint::m_nMasterServoId, hekateros::HekRobotJoint::m_strName, and hekateros::radToDeg().

Referenced by ~HekCalibStretch().

{
  // joint calibrating velocity (radians/second)
  static double TuneCalVel  = degToRad(10.0);

  // tuning slop (radians)
  static double TuneFudge  = degToRad(5.0);

  string      strJointName; // joint name
  int         nServoId;     // servo id

  byte_t      byOptBits;        // optical limits bits
  HekOpticalLimit_T  *pLimit;   // limit info associated with mask
  double      fBandWidth;       // occlusion band width (radians)

  double      fJointStartPos;   // user's starting position (radians)
  double      fJointCurPos;     // joint current position (radians)
  double      fJointCurVel;     // joint current velocity (radians/second) 
  double      fJointGoalPos;    // joint working goal position (radians)

  double      fLitEdgePos0;     // joint lit edge position 0 (radians)
  double      fLitEdgePos1;     // joint lit edge position 1 (radians)

  int         rc = HEK_OK;      // return code

  strJointName  = joint.m_strName;
  nServoId      = joint.m_nMasterServoId;
  pLimit        = m_robot.m_monitor.getJointLimitInfo(byOptMask);

  LOGDIAG3("Joint %s (servo=%d): opt_mask=0x%02x, edge=%d.",
        strJointName.c_str(), nServoId, byOptMask, nEdge);

  LOGDIAG2("Joint %s (servo=%d): "
           "Occlusion band: max=%lfdeg, center=%lfdeg, min=%lfdeg.",
        strJointName.c_str(), nServoId,
        radToDeg(pLimit->m_fMaxEdgePos),
        radToDeg(pLimit->m_fCenterPos), 
        radToDeg(pLimit->m_fMinEdgePos));

  // width of occlusion band + some
  fBandWidth = pLimit->m_fMaxEdgePos - pLimit->m_fMinEdgePos + TuneFudge;

  // starting position
  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointStartPos, fJointCurVel);

  //
  // Move to the light side of one of the occlusion band edges.
  // 
  fJointGoalPos = fJointStartPos + nEdge * fBandWidth;

  LOGDIAG2("Joint %s (servo=%d): "
      "Fine tune move for <= %.2lf degrees to find the first light edge.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

  moveToLight(strJointName, fJointGoalPos, TuneCalVel, byOptMask);

  // optical bits
  byOptBits = m_robot.m_monitor.getJointLimitBits();

  //
  // Found edge 0.
  //
  if( getLitOpticalLimits(byOptBits, byOptMask) != 0x00 )
  {
    // mark edge 0
    m_robot.m_pKin->getJointCurPosVel(strJointName, fLitEdgePos0, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Edge 0 at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fLitEdgePos0));
  }

  //
  // Didn't find the light.
  //
  else
  {
    if( m_robot.m_pKin->hasOverTorqueCondition(strJointName) )
    {
      LOGERROR("Joint %s (servo=%d): Cannot calibrate: Joint is obstucted.",
          joint.m_strName.c_str(), nServoId);
      rc = -HEK_ECODE_COLLISION;
    }
    else
    {
      LOGERROR("Joint %s (servo=%d): Cannot calibrate: "
               "Did not find edge 0 of the occlusion band.",
          joint.m_strName.c_str(), nServoId);
      rc = -HEK_ECODE_NO_EXEC;
    }
  }

  //
  // Move in opposite direction to the light side of the other occlusion band
  // edge.
  //
  // Note: Even on any failures above, try to move back to a probably more
  // reasonable position.
  // 

  // first back into the dark
  LOGDIAG2("Joint %s (servo=%d): "
      "Move back into the occlusion band at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointStartPos));

  moveToDark(strJointName, fJointStartPos, TuneCalVel, byOptMask);

  // current position in dark band
  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

  LOGDIAG2("Joint %s (servo=%d): Stopped at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointCurPos));

  //
  // Move to the opposite light side of one of the occlusion band edges.
  // 
  fJointGoalPos = fJointCurPos - nEdge * fBandWidth;

  LOGDIAG2("Joint %s (servo=%d): "
      "Fine tune move for %.2lf degrees to find the opposite light edge.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

  // then move to the opposite light
  moveToLight(strJointName, fJointGoalPos, TuneCalVel, byOptMask);

  // optical bits
  byOptBits = m_robot.m_monitor.getJointLimitBits();

  //
  // Found edge 1.
  //
  if( getLitOpticalLimits(byOptBits, byOptMask) != 0x00 )
  {
    // mark edge 1
    m_robot.m_pKin->getJointCurPosVel(strJointName, fLitEdgePos1, fJointCurVel);

    LOGDIAG2("Joint %s (servo=%d): Edge 1 at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fLitEdgePos1));
  }

  //
  // Didn't find the light edge.
  //
  else
  {
    if( m_robot.m_pKin->hasOverTorqueCondition(strJointName) )
    {
      LOGERROR("Joint %s (servo=%d): Cannot calibrate: Joint is obstucted.",
          joint.m_strName.c_str(), nServoId);
      rc = (rc == HEK_OK)? -HEK_ECODE_COLLISION: rc;
    }
    else
    {
      LOGERROR("Joint %s (servo=%d): Cannot calibrate: "
               "Did not find edge 1 of occlusion band.",
          joint.m_strName.c_str(), nServoId);
      rc = (rc == HEK_OK)? -HEK_ECODE_NO_EXEC: rc;
    }
  }

  //
  // Move to center of occlusion band.
  // 
  if( rc == HEK_OK )
  {
    fJointGoalPos = (fLitEdgePos1 + fLitEdgePos0) / 2.0;

    LOGDIAG2("Joint %s (servo=%d): Move to TDC at %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fJointGoalPos));

    moveWait(strJointName, fJointGoalPos, TuneCalVel);

    m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);

    fPosition = pLimit->m_fCenterPos; 

    LOGDIAG2("Joint %s (servo=%d): Fine tune TDC diff = %.2lf degrees.",
        strJointName.c_str(), nServoId, radToDeg(fPosition));
  }

  return rc;
}

HekRobotJoint * HekCalibStretch::getHelpfulParentJoint ( int  nChildServoId )

protectedvirtual

Get the parent joint (and link) that could aid in calibration.

Only a few joints serve this function.

Parameters

nChildServoId

Child joint's master servo id.

Returns

If parenet exists, returns pointer to parent joint. Otherwise returns NULL.

Definition at line 1486 of file hekCalibStretch.cxx.

Referenced by ~HekCalibStretch().

{
  switch( nChildServoId )
  {
    case HekServoIdWristPitch:
      return &(m_robot.m_jointsArm[HekServoIdElbow]);
    case HekServoIdElbow:
      return &(m_robot.m_jointsArm[HekServoIdShoulderL]);
    default:
      return NULL;
  }
}

int HekCalibStretch::multiMoveToDark ( const std::string &  strJointName, int  nDir, double  fJointGoalPos, double  fJointGoalVel, byte_t  byMask  )

protectedvirtual

Move joint until occluded optical limit position is detected.

Since moving a given joint during calibration may result in large trajectory arcs, other joints may be reposition to minimize this effects.

This call blocks until move is complete.

Parameters

strJointName	Name of joint.
nDir	Direction indicator.
fJointGoalPos	Joint goal position (radians).
fJointGoalVel	Joint goal velocity (radians/second).
byMask	Mask of limits to check.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 1446 of file hekCalibStretch.cxx.

Referenced by ~HekCalibStretch().

{
  string  strJointName2;    // helper joint name
  double  fGearRatios;      // joint 2 gear ratio to joint 1 gear ratio
  double  fJointGoalPos2;   // helper joint goal position (radians)
  double  fJointGoalVel2;   // helper joint goal velocity (radians/second)

  // start helper joint moving is good direction
  if( strJointName == "shoulder" )
  {
    strJointName2 = "elbow";
    fGearRatios   = calcJointRatios(strJointName2, strJointName);
    if( nDir > 0 )
    {
      fJointGoalPos2 = m_robot.getArmJoint(strJointName2)->m_fMinSoftLimitRads;
    }
    else
    {
      fJointGoalPos2 = m_robot.getArmJoint(strJointName2)->m_fMaxSoftLimitRads;
    }
    fJointGoalVel2 = fGearRatios * fJointGoalVel;
    m_robot.m_pKin->move(strJointName2, fJointGoalPos2, fJointGoalVel2);
  }

  // block while joint moves to light
  moveToDark(strJointName, fJointGoalPos, fJointGoalVel, byMask);

  // stop helper joint
  if( strJointName2.size() > 0 )
  {
    m_robot.m_pKin->stop(strJointName2);
  }

  return HEK_OK;
}

int HekCalibStretch::multiMoveToLight ( const std::string &  strJointName, int  nDir, double  fJointGoalPos, double  fJointGoalVel, byte_t  byMask  )

protectedvirtual

Move joint until unoccluded optical limit position is detected.

Since moving a given joint during calibration may result in large trajectory arcs, other joints may be reposition to minimize this effects.

This call blocks until move is complete.

Parameters

strJointName	Name of joint.
nDir	Direction indicator.
fJointGoalPos	Joint goal position (radians).
fJointGoalVel	Joint goal velocity (radians/second).
byMask	Mask of limits to check.

Returns

On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Definition at line 1406 of file hekCalibStretch.cxx.

Referenced by ~HekCalibStretch().

{
  string  strJointName2;    // helper joint name
  double  fGearRatios;      // joint 2 gear ratio to joint 1 gear ratio
  double  fJointGoalPos2;   // helper joint goal position (radians)
  double  fJointGoalVel2;   // helper joint goal velocity (radians/second)

  // start helper joint moving is good direction
  if( strJointName == "shoulder" )
  {
    strJointName2 = "elbow";
    fGearRatios   = calcJointRatios(strJointName2, strJointName);
    if( nDir > 0 )
    {
      fJointGoalPos2 = m_robot.getArmJoint(strJointName2)->m_fMinSoftLimitRads;
    }
    else
    {
      fJointGoalPos2 = m_robot.getArmJoint(strJointName2)->m_fMaxSoftLimitRads;
    }
    fJointGoalVel2 = fGearRatios * fJointGoalVel;
    m_robot.m_pKin->move(strJointName2, fJointGoalPos2, fJointGoalVel2);
  }

  // block while joint moves to light
  moveToLight(strJointName, fJointGoalPos, fJointGoalVel, byMask);

  // stop helper joint
  if( strJointName2.size() > 0 )
  {
    m_robot.m_pKin->stop(strJointName2);
  }

  return HEK_OK;
}

---

The documentation for this class was generated from the following files:

• include/Hekateros/hekCalibStretch.h
• sw/libhekateros/hekCalibStretch.cxx