Hekateros robotic manipulator plus accesories class. More...

#include <hekRobot.h>

Public Types
enum	AsyncTaskId { AsyncTaskIdNone, AsyncTaskIdCalibrate }
	Asynchronous task id. More...

typedef map< std::string, int >	IMapRobotJoints
	Indirect map of robot joints. More...

typedef std::map< std::string, double >	MapDouble
	Map of doubles.

Public Member Functions
	HekRobot (bool bNoExec=false)
	Default initialization constructor. More...

virtual	~HekRobot ()
	Destructor.

int	connect (const std::string &strDevDynabus=HekDevDynabus, int nBaudRateDynabus=HekBaudRateDynabus, const std::string &strDevArduino=HekDevArduino, int nBaudRateArduino=HekBaudRateArduino)
	Connect to *Hekateros*. More...

int	disconnect ()
	Disconnect from *Hekateros*. More...

int	calibrate (bool bForceRecalib=true)
	Calibrate *Hekateros*'s odometers and limit switch positions. More...

int	calibrateAsync (bool bForceRecalib=true)
	Asynchronously calibrate *Hekateros*'s odometers and limit switch positions. More...

void	reload ()
	Reload *Hekateros*'s reloadable configuration and reset operational parameters. More...

int	gotoBalancedPos ()
	Move robotic arm to its pre-defined balanced position. More...

int	gotoParkedPos ()
	Move robotic arm to its pre-defined parked position. More...

int	gotoZeroPtPos ()
	Move robotic arm to its pre-defined calibrated zero point position. More...

int	openGripper ()
	Open gripper to its maximum opened position. More...

int	closeGripper ()
	Close gripper to its minimum closed position. More...

int	estop ()
	Emergency stop. More...

int	freeze ()
	Freeze arm and accessories at current position. More...

int	release ()
	Release arm and accessories. More...

int	stop (const std::vector< std::string > &vecNames)
	Stops specified joints at current position. More...

int	clearAlarms ()
	Attempt to clear all servo alarms in all kinematic chains. More...

void	resetEStop ()
	Reset (clears) emergency stop condition. More...

int	moveArm (HekJointTrajectoryPoint &trajectoryPoint)
	Move arm through trajectory point. More...

int	getRobotState (HekRobotState &robotState)
	Get the robot current state. More...

int	getJointState (HekJointStatePoint &jointStatePoint)
	Get the joint states of a kinematic chain. More...

int	getTrajectoryState (HekJointTrajectoryFeedback &trajectoryFeedback)
	Get trajectory feedback. More...

void	setRobotMode (HekRobotMode eRobotMode)
	Set robot's operational mode. More...

bool	isInMotion ()
	Test if any joint in any of the kinematic chains is moving. More...

void	cancelAsyncTask ()
	Cancel any asynchronous task. More...

HekAsyncTaskState	getAsyncState ()
	Get the current asynchronous task state. More...

int	getAsyncRc ()
	Get the last asynchronous task return code. More...

bool	isConnected ()
	Test if connected to *Hekateros* hardware. More...

void	getVersion (int &nVerMajor, int &nVerMinor, int &nRevision)
	Get the *Hekateros* robotic arm hardware version numbers. More...

uint_t	getVersionNum ()
	Get the *Hekateros* robotic arm hardware version compact number. More...

std::string	getVersion ()
	Get the *Hekateros* robotic arm hardware version string. More...

HekDesc *	getHekDesc ()
	Get the *Hekateros* product description. More...

int	getProdId ()
	Convenience function to get this *Hekateros* description's base product id. More...

std::string	getProdName ()
	Convenience function to get this *Hekateros* description's base product name. More...

std::string	getFullProdBrief ()
	Get the *Hekateros* full brief descirption. More...

int	isDescribed ()
	Test if robot is fully described via configuration XML. More...

int	isCalibrated ()
	Test if robot is calibrated. More...

int	isEStopped ()
	Test if robot is current emergency stopped. More...

int	areServosPowered ()
	Test if robot servos are currently being driven (powered). More...

int	isAlarmed ()
	Test if robot is alarmed. More...

int	isAtBalancedPos ()
	Test if robot is as pre-defined balanced position. More...

int	isAtParkedPos ()
	Test if robot is as pre-defined parked position. More...

DynaChain *	getDynaChain ()
	Get the dynamixel chain object. More...

void	getTrajectoryParams (HekNorm &eNorm, double &fEpsilon)
	Get trajectory parameters. More...

HekRobotJoint *	getArmJointAt (int index)
	Get robotic joint in arm+ee kinematic chain at the given index. More...

HekRobotJoint *	getArmJoint (const std::string &strName)
	Get robotic joint in arm+ee kinematic chain. More...

HekRobotJoint *	getArmJoint (int nServoId)
	Get robotic joint in arm+ee kinematic chain. More...

DynaServo *	getMasterServo (HekRobotJoint &joint)
	Get robotic joint's master servo. More...

double	pctOfMaxJointVelocity (HekRobotJoint &joint, double fVelPct)
	Convert percent of maximum to joint velocity. More...

Protected Member Functions
int	scanHw ()
	Scan *Hekateros* hardware and match against description. More...

int	scanDynaBus (int nMaxTries)
	Scan *Hekateros* dynamixel bus hardware and match against description. More...

int	convertSpecs ()
	Convert specification(s) to operational parameters. More...

int	addRobotJoint (HekSpecJoint_T pSpecJoint, HekSpecServo_T pSpecServo, MapRobotJoints &kin, IMapRobotJoints &imap)
	Add a joint to robot's kinematic chain. More...

void	adjustTuningFromSpecs ()
	Adjust tuning parameters for values in compiled product specifications.

void	fauxcalibrate ()
	Faux calibrate *Hekateros*. More...

int	configServos ()
	Configure *Hekateros* servos. More...

int	configEEPROMForAllServos (MapRobotJoints &kin)
	Configure the EEPROM for all *Hekateros* servos in kinematic chain. More...

int	configEEPROMForServo (int nServoId, HekRobotJoint &joint)
	Configure one *Hekateros* servo EEPROM. More...

int	configRAMForAllServos (MapRobotJoints &kin)
	Configure the RAM for all *Hekateros* servos in kinematic chain. More...

int	configRAMForServo (int nServoId, HekRobotJoint &joint)
	Configure one *Hekateros* servo RAM. More...

void	resetCalibStateForAllJoints (bool bForceRecalib)
	Mark all relevant joints for recalibration. More...

HekOpState	determineRobotOpState ()
	Determine robot operational state from collective joint operational states. More...

int	moveWait (DynaServo *pServo, int nOdGoalPos, int nSpeed)
	Move servo to position. More...

void	stopWait (const std::vector< std::string > &vecNames, int nMaxTries=10)
	Wait for the specified joints to stop moving. More...

void	moveWristRot ()
	Move wrist rotation for any necessary final position adjustments. More...

void	setAllJointGoalsToNull ()
	Set all joint goals to a null trajectory. More...

void	setJointGoalsToNull (const std::vector< std::string > &vecNames)
	Set a set of joint goals to a null trajectory. More...

void	setJointGoalToNull (const std::string &strName)
	Set joint goal to null a trajectory. More...

int	createAsyncThread ()
	Create the asynchronous thread. More...

void	lock ()
	Lock the robot mutex. More...

void	unlock ()
	Unlock the robot mutex. More...

bool	trylock ()
	Try to lock the robot mutex. More...

Static Protected Member Functions
static void	cbWristRot (void *pUserArg)
	Callback to control wrist rotation. More...

static void *	asyncThread (void *pArg)
	Asynchronous operation thread. More...

Protected Attributes
bool	m_bNoExec
	do [not] execute physical movements

HekDesc	m_descHek
	*Hekateros* description

HekRobotMode	m_eRobotMode
	robot operating mode

HekOpState	m_eOpState
	arm operational state

bool	m_bIsEStopped
	arm is [not] emergency stopped

bool	m_bAlarmState
	robot is [not] alarmed

bool	m_bAreServosPowered
	arm servos are [not] driven

bool	m_bAtBalancedPos
	arm is [not] at balanced position

bool	m_bAtParkedPos
	arm is [not] at parked position

HekTunes	m_tunes
	tune parameters

DynaComm *	m_pDynaComm
	dynamixel communication

DynaChain *	m_pDynaChain
	dynamixel chain

DynaBgThread *	m_pDynaBgThread
	dynamixel background thread

MapRobotJoints	m_jointsArm
	robot arm + end effector joints

MapRobotJoints	m_jointsEquipDeck
	robot equipement deck kin. joints

MapRobotJoints	m_jointsAux
	robot auxiliary kinematic joints

IMapRobotJoints	m_imapJoints
	joints indirect map

HekKinematics *	m_pKin
	dynamics and kinematics

HekJointTrajectoryPoint	m_lastTrajArm
	last trajectory point for arm

HekMonitor	m_monitor
	power, health, and safety monitor

HekAsyncTaskState	m_eAsyncTaskState
	asynchronous task state

int	m_rcAsyncTask
	last async task return code

AsyncTaskId	m_eAsyncTaskId
	asynchronous task id

void *	m_pAsyncTaskArg
	asynchronous argument

pthread_t	m_threadAsync
	async pthread identifier

pthread_mutex_t	m_mutex
	synchronization mutex

Friends
class	HekCalib

class	HekCalibStretch

Detailed Description

Hekateros robotic manipulator plus accesories class.

Includes up to 3 kinematic chains.

Definition at line 88 of file hekRobot.h.

Member Typedef Documentation

typedef map<std::string, int> hekateros::HekRobot::IMapRobotJoints

Indirect map of robot joints.

key:	joint name
mapped type:	master servo id

Definition at line 99 of file hekRobot.h.

Member Enumeration Documentation

enum hekateros::HekRobot::AsyncTaskId

Asynchronous task id.

Enumerator
AsyncTaskIdNone	no task
AsyncTaskIdCalibrate	calibrate hekateros arm task id

Definition at line 104 of file hekRobot.h.

     {
       AsyncTaskIdNone,        ///< no task
       AsyncTaskIdCalibrate    ///< calibrate hekateros arm task id
 
       // add others here, as needed
     };

Constructor & Destructor Documentation

HekRobot::HekRobot ( bool bNoExec = false )

Default initialization constructor.

Parameters

bNoExec Do [not] execute arm physical movements. All commands and responses are supported but the lower level arm movement commands will not be issued.

Definition at line 194 of file hekRobot.cxx.

 {
   // state
   m_bNoExec           = bNoExec;
   m_eRobotMode        = HekRobotModeAuto;
   m_eOpState          = HekOpStateUncalibrated;
   m_bIsEStopped       = false;
   m_bAlarmState       = false;
   m_bAreServosPowered = false;
   m_bAtBalancedPos    = false;
   m_bAtParkedPos      = false;
 
   // dynamixel i/f
   m_pDynaComm         = NULL;
   m_pDynaChain        = NULL;
   m_pDynaBgThread     = NULL;
 
   // asynchronous task control and synchronization
   m_eAsyncTaskState   = HekAsyncTaskStateIdle;
   m_rcAsyncTask       = HEK_OK;
   m_eAsyncTaskId      = AsyncTaskIdNone;
   m_pAsyncTaskArg     = NULL;
 
   pthread_mutex_init(&m_mutex, NULL);
 }

Member Function Documentation

int HekRobot::addRobotJoint	(	HekSpecJoint_T *	pSpecJoint,
		HekSpecServo_T *	pSpecServo,
		MapRobotJoints &	kin,
		IMapRobotJoints &	imap
	)

protected

Add a joint to robot's kinematic chain.

Parameters

[in]	pSpecJoint	Pointer to joint spcecification.
[in]	pSpecServo	Pointer to master servo spcecification.
[out]	kin	Modified kinematics chain of joints.
[out]	imap	Indirect map of kinematic chain.

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

Definition at line 1271 of file hekRobot.cxx.

 {
   int               nMasterServoId;     // master servo id
   DynaServo        *pServo;             // servo
   uint_t            uTicks;             // ticks
   double            fAngleMin;          // min angle(deg) in servo mode
   double            fAngleMax;          // max angle(deg) in servo mode
   double            fMaxRpm;            // max unloaded rpm
   double            fDegrees;           // working degrees
   double            fRadians;           // working radians
   HekRobotJoint     joint;              // robotic joint
   int               i;                  // working index
 
   // master servo id associated with joint
   nMasterServoId = pSpecJoint->m_nMasterServoId;
 
   // dynamixel servo object in dynamixel chain
   if( (pServo = m_pDynaChain->GetServo(nMasterServoId)) == NULL )
   {
     LOGERROR("Servo %d: Cannot find servo in dynamixel chain.", nMasterServoId);
     return -HEK_ECODE_NO_EXEC;
   }
 
   //
   // Dynamixel servo specification loaded during scan.
   //
   uTicks      = pServo->GetSpecification().m_uRawPosModulo;
   fAngleMin   = pServo->GetSpecification().m_fAngleMin;
   fAngleMax   = pServo->GetSpecification().m_fAngleMax;
   fMaxRpm     = pServo->GetSpecification().m_fMaxSpeed;
 
   //
   // Servo rotation range.
   //
   fDegrees = pSpecServo->m_bIsContinuous? 360.0: fAngleMax - fAngleMin;
   fRadians = degToRad(fDegrees);
 
   //
   // Populate joint data.
   //
   // Some data, such as joint rotation limits may be refined during
   // calibration.
   //
   joint.m_strName             = pSpecJoint->m_strName;
   joint.m_nMasterServoId      = nMasterServoId;
   joint.m_nSlaveServoId       = pSpecJoint->m_nSlaveServoId;
   joint.m_bIsServoContinuous  = pSpecServo->m_bIsContinuous;
   joint.m_nMasterServoDir     = pSpecServo->m_nDir;
   joint.m_eJointType          = pSpecJoint->m_eJointType;
   joint.m_fGearRatio          = pSpecJoint->m_fGearRatio;
   joint.m_fTicksPerServoRad   = (double)uTicks / fRadians; 
   joint.m_fTicksPerJointRad   = joint.m_fTicksPerServoRad *
                                   pSpecJoint->m_fGearRatio;
   joint.m_fMaxServoRadsPerSec = fMaxRpm * M_TAU / 60.0;
   joint.m_fMaxJointRadsPerSec = joint.m_fMaxServoRadsPerSec /
                                   joint.m_fGearRatio;
   joint.m_fMinPhyLimitRads    = degToRad(pSpecJoint->m_fMinPhyLimit);
   joint.m_fMaxPhyLimitRads    = degToRad(pSpecJoint->m_fMaxPhyLimit);
   joint.m_nMinPhyLimitOd      = (int)(joint.m_fTicksPerJointRad *
                                       joint.m_fMinPhyLimitRads);
   joint.m_nMaxPhyLimitOd      = (int)(joint.m_fTicksPerJointRad *
                                       joint.m_fMaxPhyLimitRads );
   joint.m_fMinSoftLimitRads   = joint.m_fMinPhyLimitRads;
   joint.m_fMaxSoftLimitRads   = joint.m_fMaxPhyLimitRads;
   joint.m_nMinSoftLimitOd     = joint.m_nMinPhyLimitOd;
   joint.m_nMaxSoftLimitOd     = joint.m_nMaxPhyLimitOd;
   joint.m_fCalibPosRads       = degToRad(pSpecJoint->m_fCalibPos);
   joint.m_fBalPosRads         = degToRad(pSpecJoint->m_fBalancedPos);
   joint.m_fParkPosRads        = degToRad(pSpecJoint->m_fParkedPos);
   joint.m_eLimitTypes         = pSpecJoint->m_eLimitTypes;
 
   joint.m_fOverTorqueThDft    = fcap(pSpecServo->m_fTorqueLimitPct,
                                     HekTuneOverTorqueThMin,
                                     HekTuneOverTorqueThMax) / 100.0;
 
   joint.m_eOpState            = HekOpStateUncalibrated;
   joint.m_bStopAtOptLimits    = false;
 
   for(i=0; i<HekOptLimitMaxPerJoint; ++i)
   {
     joint.m_byOptLimitMask[i] = pSpecJoint->m_limit[i].m_uBit;
   }
 
   //
   // Sanity checks.
   //
   joint.m_fCalibPosRads = fcap(joint.m_fCalibPosRads,
                                 joint.m_fMinPhyLimitRads,
                                 joint.m_fMaxPhyLimitRads);
   joint.m_fBalPosRads   = fcap(joint.m_fBalPosRads,
                                 joint.m_fMinPhyLimitRads,
                                 joint.m_fMaxPhyLimitRads);
   joint.m_fParkPosRads  = fcap(joint.m_fParkPosRads,
                                 joint.m_fMinPhyLimitRads,
                                 joint.m_fMaxPhyLimitRads);
 
   //
   // Add to kinematic chain.
   //
   kin[nMasterServoId]   = joint;            // kinematic chain
   imap[joint.m_strName] = nMasterServoId;   // indirect map by joint name
 
   return HEK_OK;
 }

int hekateros::HekRobot::areServosPowered ( )

inline

Test if robot servos are currently being driven (powered).

Returns: Returns true or false.

Definition at line 528 of file hekRobot.h.

References m_bAreServosPowered.

     {
       return m_bAreServosPowered;
     }

void * HekRobot::asyncThread ( void * pArg )

staticprotected

Asynchronous operation thread.

Parameters

pArg	Thread argument (point to HekRobot object).

Returns: Returns NULL on thread exit.

Definition at line 1709 of file hekRobot.cxx.

References calibrate(), m_eAsyncTaskId, m_eAsyncTaskState, m_pAsyncTaskArg, and m_rcAsyncTask.

 {
   HekRobot *pThis = (HekRobot *)pArg;
   int       oldstate;
   int       rc;
 
   pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, &oldstate);
   pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldstate);
   //pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, &oldstate);
 
   LOGDIAG3("Async robot task thread created.");
 
   //
   // Execute asychronous task.
   //
   // For now, only calibrate task is supported asynchronously.
   //
   switch( pThis->m_eAsyncTaskId )
   {
     // Calibrate the robot. 
     case AsyncTaskIdCalibrate:
       {
         bool bForceRecalib = (bool)pThis->m_pAsyncTaskArg;
         rc = pThis->calibrate(bForceRecalib);
       }
       break;
 
     // Unknown task id.
     default:
       LOGERROR("Unknown async task id = %d.", (int)pThis->m_eAsyncTaskId);
       rc = -HEK_ECODE_BAD_VAL;
       break;
   }
 
   pThis->m_eAsyncTaskId     = AsyncTaskIdNone;
   pThis->m_pAsyncTaskArg    = NULL;
   pThis->m_rcAsyncTask      = rc;
   pThis->m_eAsyncTaskState  = HekAsyncTaskStateIdle;
 
   LOGDIAG3("Async robot task thread exited.");
 
   return NULL;
 }

int HekRobot::calibrate ( bool bForceRecalib = true )

Calibrate Hekateros's odometers and limit switch positions.

Parameters

bForceRecalib If true, recalibrate all joints. Otherwise calibrate only the uncalibrated.

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

Definition at line 436 of file hekRobot.cxx.

References hekateros::HekCalibStretch::calibrate(), HEK_TRY_CONN, and HEK_TRY_NO_EXEC.

Referenced by asyncThread().

 {
   HekCalibStretch calib(*this);
   int             rc;
 
   HEK_TRY_NO_EXEC();
   HEK_TRY_CONN();
 
   // lock the arm
   freeze();
 
   // robot is now uncalibrated
   m_eOpState = HekOpStateUncalibrated;
 
   // mark relevant joints for recalibration
   resetCalibStateForAllJoints(bForceRecalib);
 
   // robot calibration started 
   m_eOpState = HekOpStateCalibrating;
 
   // now physically calibrate the arm
   if( (rc = calib.calibrate()) < 0 )
   {
     LOGERROR("Failed to calibrate Hekateros.");
     m_eOpState = HekOpStateUncalibrated;
     return rc;
   }
 
   //
   // Should be all calibrated at this point.
   //
   
   // synchronize robot operation state to collective joint operational states
   if( (m_eOpState = determineRobotOpState()) != HekOpStateCalibrated )
   {
     LOGERROR("BUG: Failed to calibrate Hekateros.");
     return -HEK_ECODE_INTERNAL;
   }
 
   // arm is calibrated
   gotoZeroPtPos();
 
   LOGDIAG1("Hekateros calibrated.");
 
   return HEK_OK;
 }

int HekRobot::calibrateAsync ( bool bForceRecalib = true )

Asynchronously calibrate Hekateros's odometers and limit switch positions.

Call cancelAsync() to cancel operation.

Parameters

bForceRecalib If true, recalibrate all joints. Otherwise calibrate only the uncalibrated.

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

Definition at line 483 of file hekRobot.cxx.

 {
   if( m_eAsyncTaskState != HekAsyncTaskStateIdle )
   {
     LOGERROR("Already executing asynchronous task.");
     return -HEK_ECODE_NO_RSRC;
   }
   else
   {
     m_eAsyncTaskId  = AsyncTaskIdCalibrate;
     m_pAsyncTaskArg = (void *)bForceRecalib;
 
     return createAsyncThread();
   }
 }

void HekRobot::cancelAsyncTask ( )

Cancel any asynchronous task.

Note: There may be a little delay between canceling an async task and the task actually stopping.

Definition at line 1662 of file hekRobot.cxx.

Referenced by setRobotMode().

 {
   MapRobotJoints::iterator  iter;
 
   if( m_eAsyncTaskState != HekAsyncTaskStateIdle )
   {
     // cancel thread
     pthread_cancel(m_threadAsync);
     pthread_join(m_threadAsync, NULL);
 
     // cleanup
     switch( m_eAsyncTaskId )
     {
       case AsyncTaskIdCalibrate:
         freeze();
         for(iter = m_jointsArm.begin(); iter != m_jointsArm.end(); ++iter)
         {
           if( iter->second.m_eOpState != HekOpStateCalibrated )
           {
             iter->second.m_eOpState = HekOpStateUncalibrated;
             m_eOpState              = HekOpStateUncalibrated;
           }
         }
         break;
       default:
         break;
     }
 
     // clear state
     m_eAsyncTaskId    = AsyncTaskIdNone;
     m_pAsyncTaskArg   = NULL;
     m_rcAsyncTask     = -HEK_ECODE_INTR;
     m_eAsyncTaskState = HekAsyncTaskStateIdle;
     LOGDIAG3("Async task canceled.");
   }
 }

static void hekateros::HekRobot::cbWristRot ( void * pUserArg )

staticprotected

Callback to control wrist rotation.

The wrist rotation is coupled to wrist pitch.

Parameters

pUserArg Point to this.

int HekRobot::clearAlarms ( )

Attempt to clear all servo alarms in all kinematic chains.

Note: Not all alarms are clearable (e.g. temperature).

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

Definition at line 847 of file hekRobot.cxx.

References HEK_TRY_CONN.

 {
   int             iter;       // servo iterator
   int             nServoId;   // servo id
   DynaServo      *pServo;     // servo
 
   HEK_TRY_CONN();
 
   release();
 
   //
   // Try to clear alarms for all servos.
   //
   for(nServoId = m_pDynaChain->IterStart(&iter);
       nServoId != DYNA_ID_NONE;
       nServoId = m_pDynaChain->IterNext(&iter))
   {
     pServo = m_pDynaChain->GetServo(nServoId);
 
     // over torque alarm zero's torque limit - reload from EEPROM
     pServo->ReloadMaxTorqueLimit();
   }
 
   //
   // Clear hekateros alarm led to provide visual feedback to the user. The
   // background check may relight the alarm led if alarms still persist.
   //
   m_bAlarmState = false;
   m_monitor.markAlarmCond(m_bAlarmState);
 
   return HEK_OK;
 }

int HekRobot::closeGripper ( )

Close gripper to its minimum closed position.

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

Definition at line 739 of file hekRobot.cxx.

References hekateros::HekJointTrajectoryPoint::append(), hekateros::degToRad(), HEK_TRY_CALIB, HEK_TRY_CONN, HEK_TRY_NOT_ESTOP, hekateros::HekRobotJoint::m_fMinSoftLimitRads, and hekateros::HekRobotJoint::m_strName.

 {
   HekJointTrajectoryPoint   trajPoint;
 
   HekRobotJoint  *pJoint;
   int             rc;
 
   HEK_TRY_CONN();
   HEK_TRY_CALIB();
   HEK_TRY_NOT_ESTOP();
 
   if( (pJoint = getArmJoint(HekServoIdGraboid)) == NULL )
   {
     LOGERROR("No gripper end effector found.");
     return -HEK_ECODE_NO_RSRC;
   }
 
   trajPoint.append(pJoint->m_strName, pJoint->m_fMinSoftLimitRads,
             degToRad(60.0));
 
   if( (rc = moveArm(trajPoint)) < 0 )
   {
     LOGERROR("Move to pre-defined gripper closed position failed.");
   }
   else
   {
     LOGDIAG2("Hekateros gripper closed.");
   }
 
   return rc;
 }

int HekRobot::configEEPROMForAllServos ( MapRobotJoints & kin )

protected

Configure the EEPROM for all Hekateros servos in kinematic chain.

For each servo, ensure that the proper servo EEPROM values are set.

Parameters

kin	Kinematic chain of joints.

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

Definition at line 1430 of file hekRobot.cxx.

 {
   MapRobotJoints::iterator  iter;
   
   int     nServoId;
   int     rc;
 
   for(iter = kin.begin(); iter != kin.end(); ++iter)
   {
     nServoId = iter->second.m_nMasterServoId;
 
     if( (rc = configEEPROMForServo(nServoId, iter->second)) < 0 )
     {
       LOGERROR("Servo %d: Failed to configure servo EEPROM memory map.",
         nServoId);
       return rc;
     }
 
     nServoId = iter->second.m_nSlaveServoId;
 
     if( nServoId != DYNA_ID_NONE )
     {
       if( (rc = configEEPROMForServo(nServoId, iter->second)) < 0 )
       {
         LOGERROR("Servo %d: Failed to configure servo EEPROM memory map.",
           nServoId);
         return rc;
       }
     }
   }
 
   return HEK_OK;
 }

int HekRobot::configEEPROMForServo	(	int	nServoId,
		HekRobotJoint &	joint
	)

protected

Configure one Hekateros servo EEPROM.

A read, then write if necessary is performed to limit EEPROM wear.

Note: System reads, writes, and sleeps are pthread cancelation points.

Parameters

nServoId	Servo id.
joint	Associated robotic joint.

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

Definition at line 1464 of file hekRobot.cxx.

References HEK_TRY_GET_SERVO, and hekateros::HekRobotJoint::m_bIsServoContinuous.

 {
   DynaServo  *pServo;
   uint_t      uPosMin;
   uint_t      uPosMax;
   uint_t      uTorqueMax;
   uint_t      uVal;
   uint_t      uMask;
   int         rc;
 
   // dynamixel servo object in dynamixel chain
   HEK_TRY_GET_SERVO(nServoId, pServo);
 
   uPosMin     = pServo->GetSpecification().m_uRawPosMin;
   uPosMax     = pServo->GetSpecification().m_uRawPosMax;
   uTorqueMax  = pServo->GetSpecification().m_uRawTorqueMax;
 
   // --
   // Ensure servo is in the correct servo mode.
   // --
   
   // get servo must be in continuous mode
   if( joint.m_bIsServoContinuous &&
       (pServo->GetServoMode() != DYNA_MODE_CONTINUOUS)) 
   {
     if( (rc = pServo->CfgWriteServoModeContinuous()) < 0 )
     {
       LOGERROR("Servo %d: Cannot configure EEPROM for continuous mode.",
           nServoId);
       return -HEK_ECODE_DYNA;
     }
   }
 
   // servo must be in servo mode
   else if( !joint.m_bIsServoContinuous && 
            (pServo->GetServoMode() == DYNA_MODE_CONTINUOUS) )
   {
     if( (rc = pServo->CfgWriteServoMode(uPosMin, uPosMax)) < 0 )
     {
       LOGERROR("Servo %d: Cannot configure EEPROM for servo mode.", nServoId);
       return -HEK_ECODE_DYNA;
     }
   }
 
   // --
   // Ensure maximum torque limit reload value is at the maximum.
   // --
   
   // always set maximum torque limit
   if( (rc = pServo->CfgReadMaxTorqueLimit(&uVal)) < 0 )
   {
     LOGWARN("Servo %d: Cannot read EEPROM maximum torque limit.", nServoId);
   }
   else if( uVal < uTorqueMax )
   {
     if( (rc = pServo->CfgWriteMaxTorqueLimit(uTorqueMax)) < 0 )
     {
       LOGWARN("Servo %d: Cannot configure EEPROM maximum torque limit.",
           nServoId);
     }
   }
 
   // --
   // Ensure alarm shutdown mask as the correct values.
   // --
 
   // for servos with no torque limit, disable over-load alarms
   uMask = DYNA_ALARM_VOLTAGE | DYNA_ALARM_ANGLE | DYNA_ALARM_TEMP |
             DYNA_ALARM_LOAD;
 
   if( (rc = pServo->CfgReadAlarmShutdownMask(&uVal)) < 0 )
   {
     LOGWARN("Servo %d: Cannot read EEPROM alarm shutdown mask.", nServoId);
   }
   else if( uVal != uMask )
   {
     if( (rc = pServo->CfgWriteAlarmShutdownMask(uMask)) < 0 )
     {
       LOGWARN("Servo %d: Cannot configure EEPROM alarm shutdown mask.",
           nServoId);
     }
   }
 
   return HEK_OK;
 }

int HekRobot::configRAMForAllServos ( MapRobotJoints & kin )

protected

Configure the RAM for all Hekateros servos in kinematic chain.

For each servo, ensure that the proper servo RAM values are set.

Parameters

kin	Kinematic chain of joints.

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

Definition at line 1550 of file hekRobot.cxx.

 {
   MapRobotJoints::iterator  iter;
   int                       nServoId;
   int                       rc;
 
   for(iter = kin.begin(); iter != kin.end(); ++iter)
   {
     nServoId = iter->second.m_nMasterServoId;
 
     if( (rc = configRAMForServo(nServoId, iter->second)) < 0 )
     {
       LOGERROR("Servo %d: Failed to configure servo RAM memory map.",
         nServoId);
       return rc;
     }
 
 
     // no slave configuration
   }
 
   return HEK_OK;
 }

int HekRobot::configRAMForServo	(	int	nServoId,
		HekRobotJoint &	joint
	)

protected

Configure one Hekateros servo RAM.

Note: System reads, writes, and sleeps are pthread cancelation points.

Parameters

nServoId	Servo id.
joint	Associated robotic joint.

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

Definition at line 1574 of file hekRobot.cxx.

References HEK_TRY_GET_SERVO.

 {
   DynaServo  *pServo;
   uint_t      uTorqueMax;
   int         rc;
 
   // get dynamixel servo object in dynamixel chain
   HEK_TRY_GET_SERVO(nServoId, pServo);
 
   uTorqueMax  = pServo->GetSpecification().m_uRawTorqueMax;
 
   // --
   // Ensure running torque limit is at the maximum. 
   // --
  
   if( (rc = pServo->WriteMaxTorqueLimit(uTorqueMax)) < 0 )
   {
     LOGWARN("Servo %d: Cannot write RAM alarm shutdown mask.", nServoId);
   }
 
   // --
   // Disable servo drive on start up.
   // --
 
   if( (rc = pServo->WriteTorqueEnable(false)) < 0 )
   {
     LOGERROR("Servo %d: Cannot enable servo drive.", nServoId);
     return -HEK_ECODE_DYNA;
   }
 
   return HEK_OK;
 }

int HekRobot::configServos ( )

protected

Configure Hekateros servos.

Each servo's EEPROM and RAM control tables must match the joint descriptions.

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

Definition at line 1411 of file hekRobot.cxx.

 {
   int   rc;
 
   if( (rc = configEEPROMForAllServos(m_jointsArm)) < 0 )
   {
     LOGERROR("Failed to configure servo EEPROM for all servos.");
     return rc;
   }
 
   if( (rc = configRAMForAllServos(m_jointsArm)) < 0 )
   {
     LOGERROR("Failed to configure servo RAM for all servos.");
     return rc;
   }
 
   return HEK_OK;
 }

int HekRobot::connect	(	const std::string &	strDevDynabus = `HekDevDynabus`,
		int	nBaudRateDynabus = `HekBaudRateDynabus`,
		const std::string &	strDevArduino = `HekDevArduino`,
		int	nBaudRateArduino = `HekBaudRateArduino`
	)

Connect to Hekateros.

Parameters

strDevDynabus	Dynabus serial device name.
nBaudRateDynabus	Dynabus baud rate.
strDevArduino	Arduino serial device name.
nBaudRateArduino	Arduino baud rate.

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

Definition at line 226 of file hekRobot.cxx.

References hekateros::getRealDeviceName(), and hekateros::HekXmlTune::load().

 {
   string  strDevName; // real device name
   int     rc;         // return code
 
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
   // Pre-connect requirements.
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
   //
   // Need a robot description before preceeding. The controlling application, 
   // typically a ROS node, provides the description. The desription is normally
   // the parsed data found in /etc/hekaeros/hekateros.conf.
   //
   if( !m_descHek.isDescribed() )
   {
     LOGERROR("Undefined Hekateros description - "
              "don't know how to initialized properly.");
     return -HEK_ECODE_BAD_OP;
   }
   
 
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
   // Dynabus communication and configuration.
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
   // get the real device name, not a symbolic link
   strDevName = getRealDeviceName(strDevDynabus);
 
   // open (proxied) serial device to dynamixel bus
   m_pDynaComm = DynaComm::New(strDevName.c_str(), nBaudRateDynabus);
 
   if( m_pDynaComm == NULL )
   {
     LOGERROR("Failed to create dynamixel interface on '%s'@%d.",
         strDevName.c_str(), nBaudRateDynabus);
     return -HEK_ECODE_DYNA;
   }
   LOGDIAG2("Created dynamixel interface on '%s'@%d.",
         strDevName.c_str(), nBaudRateDynabus);
 
   // create dynamixel bus chain
   m_pDynaChain = new DynaChain(*m_pDynaComm);
 
   // scan Dynabus for all attached servos
   if( (rc = scanDynaBus(3)) < 0 )
   {
     LOGERROR("Hekateros dynamixel bus scan failed.");
     disconnect();
     return rc;
   }
   LOGDIAG2("Scanned for servos.");
 
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
   // Build specifications, set tuning parameters, and configure.
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
   //
   // Convert fixed specifications to joint operation descriptions. The
   // specifications are compliled-in, product-specific link, joint, servo data.
   //
   // Required: product descriptions, scanned servos, and monitor object.
   //
   if( (rc = convertSpecs()) < 0 )
   {
     LOGERROR("Failed to convert product specifications to "
              "operations parameters.");
     disconnect();
     return rc;
   }
   LOGDIAG2("Converted product specifications to operations parameters.");
 
   //
   // Adjust tuning parameters from any defaults specified in the converted
   // joint descriptions.
   //
   // Required: joint descriptions and tuning paramaters.
   //
   adjustTuningFromSpecs();
   
   //
   // Override any tuning parameters from the optional, user-specified tuning
   // XML file.
   //
   HekXmlTune  xml;
 
   // parse tune XML file and set tuning parameter overrides
   xml.load(m_tunes, HekSysCfgPath, HekEtcTune);
 
   //
   // Configure servos EEPROM and RAM control tables for operation.
   //
   // Required: joint descriptions and scanned servos.
   //
   configServos();
 
   //
   // Mark all joints for calibration.
   //
   resetCalibStateForAllJoints(true);
 
 
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
   // Kinodynmics thread.
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  
   // create active dynamics and kinematics chain
   m_pKin = new HekKinematics(*m_pDynaChain, m_jointsArm, m_tunes);
 
   // run kinematics thread at the given Hertz
   m_pKin->runThread(m_tunes.getKinematicsHz());
 
   LOGDIAG2("Kinematics thread started.");
 
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
   // Health and safety monitoring thread.
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
   // get the real device name, not a symbolic link
   strDevName = getRealDeviceName(strDevArduino);
 
   // open all interfaces to monitoring hardware and create monitoring thread
   if( (rc = m_monitor.open(getVersionNum(), strDevName, nBaudRateArduino)) < 0 )
   {
     LOGERROR("Hekateros monitor interface failed to open on '%s'@%d.",
         strDevName.c_str(), nBaudRateArduino);
     disconnect();
     return rc;
   }
   LOGDIAG2("Hekateros monitor interface open on '%s'@%d.",
         strDevName.c_str(), nBaudRateArduino);
 
   m_bAlarmState = false;
   m_monitor.markAlarmCond(m_bAlarmState);
 
   // add chain to monitor 
   m_monitor.addServoChainToMonitor(m_pDynaChain);
 
   // add kinematics to monitor 
   m_monitor.addKinematicsToMonitor(m_pKin);
 
   // start robot monitoring health and safety thread
   m_monitor.start();
 
   LOGDIAG2("Monitor thread started.");
 
 
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
   // Final touches
   // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
   
   LOGDIAG1("Connected to Hekateros.");
 
   //
   // DBG DANGER, WILL ROBINSON
   //
   // Uncomment the next line to circumvent calibration step. Make sure arm
   // is at the zero point.
   // 
   //fauxcalibrate();
   //
   // DBG DANGER, WILL ROBINSON
   //
 
   return HEK_OK;
 }

int HekRobot::convertSpecs ( )

protected

Convert specification(s) to operational parameters.

The specifications must be defined and the connecton to the Hekateros hardware must be open.

There are up to 3 kinematic chains, given the specifications and the hardware:

arm + end effector
equipment deck effector
auxiliary effector

Note: A kinematic chain differs from the dynamixel chain. Hekateros supports one dynamixel bus in which all servos hang. A dynamixel chain defines this communication bus. Servos on the bus can be associated with any of the above three kinematic chains.

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

Definition at line 1175 of file hekRobot.cxx.

References hekateros::HekSpec::getJointSpecAt(), hekateros::HekSpec::getServoSpec(), hekateros::HekSpec::m_nDoF, hekateros::HekSpecJoint_T::m_nMasterServoId, hekateros::HekDescEE::m_spec, and hekateros::HekDescArm::m_spec.

 {
   HekDescArm       *pDescArm;
   HekDescEE        *pDescEE;
   HekSpecJoint_T   *pSpecJoint;
   HekSpecServo_T   *pSpecServo;
   int               nServoId;
   HekRobotJoint    *pJoint;
   int               i;
   int               rc;
 
   m_jointsArm.clear();
   m_jointsEquipDeck.clear();
   m_jointsAux.clear();
   m_imapJoints.clear();
 
   pDescArm = m_descHek.getArmDesc();
   pDescEE  = m_descHek.getEEDesc();
 
   //
   // Build up arm kinematic chain.
   //
   for(i = 0; i < pDescArm->m_spec.m_nDoF; ++i)
   {
     // joint specification
     pSpecJoint = pDescArm->m_spec.getJointSpecAt(i);
 
     // master servo id associated with joint
     nServoId  = pSpecJoint->m_nMasterServoId;
     pJoint    = &m_jointsArm[nServoId];
 
     // servo specification
     if( (pSpecServo = pDescArm->m_spec.getServoSpec(nServoId)) == NULL )
     {
       LOGERROR("Servo %d: Cannot find servo specification.", nServoId);
       return -HEK_ECODE_NO_EXEC;
     }
 
     // add rebotic joint
     rc = addRobotJoint(pSpecJoint, pSpecServo, m_jointsArm, m_imapJoints);
     if( rc < 0 )
     {
       LOGERROR("Servo %d: Cannot add to kinematic chain.", nServoId);
       return rc;
     }
 
     // add any optical limit switches
     if( (rc = m_monitor.addJointLimitsToMonitor(pSpecJoint, pJoint)) < 0 )
     {
       LOGERROR("Servo %d: Cannot add to optical limit(s).", nServoId);
       return rc;
     }
   }
 
   //
   // Add end effector joints to arm kinematic chain.
   //
   for(i = 0; i < pDescEE->m_spec.m_nDoF; ++i)
   {
     // joint specification
     pSpecJoint = pDescEE->m_spec.getJointSpecAt(i);
 
     // master servo id associated with joint
     nServoId  = pSpecJoint->m_nMasterServoId;
     pJoint    = &m_jointsArm[nServoId];
 
     // servo specification
     if( (pSpecServo = pDescEE->m_spec.getServoSpec(nServoId)) == NULL )
     {
       LOGERROR("Servo %d: Cannot find servo specification.", nServoId);
       return -HEK_ECODE_NO_EXEC;
     }
 
     // add rebotic joint
     rc = addRobotJoint(pSpecJoint, pSpecServo, m_jointsArm, m_imapJoints);
     if( rc < 0 )
     {
       LOGERROR("Servo %d: Cannot add to kinematic chain.", nServoId);
       return rc;
     }
 
     // add any optical limit switches
     if( (rc = m_monitor.addJointLimitsToMonitor(pSpecJoint, pJoint)) < 0 )
     {
       LOGERROR("Servo %d: Cannot add to optical limit(s).", nServoId);
       return rc;
     }
   }
 
   //
   // TODO: Add equipment deck and auxiliary effector chains here.
   //
  
   return HEK_OK;
 }

int HekRobot::createAsyncThread ( )

protected

Create the asynchronous thread.

Context:: Calling thread.

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

Definition at line 1635 of file hekRobot.cxx.

 {
   int   rc;
 
   m_eAsyncTaskState = HekAsyncTaskStateWorking;
   m_rcAsyncTask     = HEK_OK;
 
   rc = pthread_create(&m_threadAsync, NULL, HekRobot::asyncThread, (void*)this);
  
   if( rc == 0 )
   {
     rc = HEK_OK;
   }
 
   else
   {
     m_eAsyncTaskState = HekAsyncTaskStateIdle;
     LOGSYSERROR("pthread_create()");
     m_rcAsyncTask   = -HEK_ECODE_SYS;
     m_eAsyncTaskId  = AsyncTaskIdNone;
     m_pAsyncTaskArg = NULL;
     rc = m_rcAsyncTask;
   }
 
   return rc;
 }

HekOpState HekRobot::determineRobotOpState ( )

protected

Determine robot operational state from collective joint operational states.

Returns: Determined robot operational state synchronized with joint states.

Definition at line 1620 of file hekRobot.cxx.

 {
   MapRobotJoints::iterator  iter;
 
   for(iter = m_jointsArm.begin(); iter != m_jointsArm.end(); ++iter)
   {
     if( iter->second.m_eOpState != HekOpStateCalibrated )
     {
       return HekOpStateUncalibrated;
     }
   }
 
   return HekOpStateCalibrated;
 }

int HekRobot::disconnect ( )

Disconnect from Hekateros.

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

Definition at line 394 of file hekRobot.cxx.

 {
   // close monitor 
   m_monitor.close();
   
   if( m_pKin != NULL )
   {
     delete m_pKin;
     m_pKin = NULL;
   }
 
   // delete dynamixel chain
   if( m_pDynaChain != NULL )
   {
     m_pDynaChain->EStop();  // == force release of arm motors
     delete m_pDynaChain;
     m_pDynaChain = NULL;
   }
 
   // close connection and delete dynamixel communication object
   if( m_pDynaComm != NULL )
   {
     if( m_pDynaComm->IsOpen() )
     {
       m_pDynaComm->Close();
     }
     delete m_pDynaComm;
     m_pDynaComm = NULL;
   }
 
   // reset robot state
   m_eRobotMode        = HekRobotModeAuto;
   m_eOpState          = HekOpStateUncalibrated;
   m_bIsEStopped       = false;
   m_bAlarmState       = false;
   m_bAreServosPowered = false;
   m_bAtBalancedPos    = false;
   m_bAtParkedPos      = false;
 
   LOGDIAG1("Disconnected from Hekateros.");
 }

int HekRobot::estop ( )

Emergency stop.

All servos will stop driving, so arm or accessories may fall. Emergency stop requires recalibration.

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

Definition at line 771 of file hekRobot.cxx.

References HEK_TRY_CONN, and HEK_TRY_NO_EXEC.

 {
   HEK_TRY_NO_EXEC();
   HEK_TRY_CONN();
 
   m_pKin->estop();
 
   m_bIsEStopped       = true;
   m_bAreServosPowered = false;
 
   m_monitor.markEStopCond(m_bIsEStopped);
   m_monitor.markPoweredCond(m_bAreServosPowered);
  
   m_lastTrajArm.clear();
 
   m_bAlarmState = true;
   m_monitor.markAlarmCond(m_bAlarmState);
 
   LOGDIAG1("Hekateros emergency stopped.");
 
   return HEK_OK;
 }

void HekRobot::fauxcalibrate ( )

protected

Faux calibrate Hekateros.

DANGER:: This function should in place of calibrate() for debugging. Make suere the arm is placed at the zero point prior to starting.

Definition at line 1397 of file hekRobot.cxx.

 {
   MapRobotJoints::iterator  iter;
 
   freeze();
 
   for(iter = m_jointsArm.begin(); iter != m_jointsArm.end(); ++iter)
   {
     iter->second.m_eOpState = HekOpStateCalibrated;
   }
   m_pKin->resetServoOdometersForAllJoints();
   m_eOpState = HekOpStateCalibrated;
 }

int HekRobot::freeze ( )

Freeze arm and accessories at current position.

Servos are still being driven.

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

Definition at line 794 of file hekRobot.cxx.

References HEK_TRY_CONN, and HEK_TRY_NO_EXEC.

 {
   HEK_TRY_NO_EXEC();
   HEK_TRY_CONN();
 
   m_bAreServosPowered = true;
 
   m_monitor.markPoweredCond(m_bAreServosPowered);
 
   m_lastTrajArm.clear();
 
   m_pKin->freeze();
 
   LOGDIAG2("Hekateros frozen at current position.");
 
   return HEK_OK;
 }

HekRobotJoint* hekateros::HekRobot::getArmJoint ( const std::string & strName )

inline

Get robotic joint in arm+ee kinematic chain.

Parameters

strName Joint name (key).

Returns: If found, returns pointer to joint. Otherwise returns NULL.

Definition at line 610 of file hekRobot.h.

References m_imapJoints, and m_jointsArm.

     {
       IMapRobotJoints::iterator  iter;
 
       if( (iter = m_imapJoints.find(strName)) != m_imapJoints.end() )
       {
         return &(m_jointsArm[iter->second]);
       }
       else
       {
         return NULL;
       }
     }

HekRobotJoint* hekateros::HekRobot::getArmJoint ( int nServoId )

inline

Get robotic joint in arm+ee kinematic chain.

Parameters

nServoId Master servo id (primary key).

Returns: If found, returns pointer to joint. Otherwise returns NULL.

Definition at line 631 of file hekRobot.h.

References m_jointsArm.

     {
       MapRobotJoints::iterator  iter;
 
       if( (iter = m_jointsArm.find(nServoId)) != m_jointsArm.end() )
       {
         return &(iter->second);
       }
       else
       {
         return NULL;
       }
     }

HekRobotJoint* hekateros::HekRobot::getArmJointAt ( int index )

inline

Get robotic joint in arm+ee kinematic chain at the given index.

Parameters

index Joint index.

Returns: If found, returns pointer to joint. Otherwise returns NULL.

Definition at line 598 of file hekRobot.h.

References m_jointsArm.

     {
       return index < m_jointsArm.size()? &m_jointsArm.at(index): NULL;
     }

int HekRobot::getAsyncRc ( )

Get the last asynchronous task return code.

Returns: Returns HEK_OK when task terminated successfully. Otherwise, returns < 0 Hekateros Error Codes.

Definition at line 1704 of file hekRobot.cxx.

Referenced by setRobotMode().

 {
   return m_rcAsyncTask;
 }

HekAsyncTaskState HekRobot::getAsyncState ( )

Get the current asynchronous task state.

Returns: HekAsyncTaskState enum value.

Definition at line 1699 of file hekRobot.cxx.

Referenced by setRobotMode().

 {
   return m_eAsyncTaskState;
 }

DynaChain* hekateros::HekRobot::getDynaChain ( )

inline

Get the dynamixel chain object.

Returns: Returns point to DynaChain object.

Definition at line 570 of file hekRobot.h.

References m_pDynaChain.

     {
       return m_pDynaChain;
     }

std::string hekateros::HekRobot::getFullProdBrief ( )

inline

Get the Hekateros full brief descirption.

Returns: Returns product brief description.

Definition at line 488 of file hekRobot.h.

References hekateros::HekDesc::getFullProdBrief(), and m_descHek.

     {
       return m_descHek.getFullProdBrief();
     }

HekDesc* hekateros::HekRobot::getHekDesc ( )

inline

Get the Hekateros product description.

Returns: Returns pointer to description.

Definition at line 456 of file hekRobot.h.

References m_descHek.

     {
       return &m_descHek;
     }

int HekRobot::getJointState ( HekJointStatePoint & jointStatePoint )

Get the joint states of a kinematic chain.

Parameters

[in,out] jointStatePoint Joint state point.

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

Definition at line 979 of file hekRobot.cxx.

Referenced by resetEStop().

 {
   MapRobotJoints::iterator  iter;   // kinematic chain iterator
   int             nMasterServoId;   // master servo id
   HekRobotJoint  *pJoint;           // robotic joint
   DynaServo      *pServo;           // servo
   HekJointState   jointState;       // working joint state
   byte_t          uMask;            // working bit mask
   int             i;                // working index
 
   HEK_TRY_CONN();
 
   jointStatePoint.clear();
 
   jointStatePoint.setKinematicChainName("arm");
 
   //
   // Build joint state point.
   //
   for(iter = m_jointsArm.begin(); iter != m_jointsArm.end(); ++iter)
   {
     nMasterServoId  = iter->first;
     pJoint          = &(iter->second);
 
     HEK_TRY_GET_SERVO(nMasterServoId, pServo);
 
     // identifiers
     jointState.m_strName        = pJoint->m_strName;
     jointState.m_eOpState       = pJoint->m_eOpState;
     jointState.m_nMasterServoId = nMasterServoId;
     jointState.m_nSlaveServoId  = pJoint->m_nSlaveServoId;
 
     // joint dynamics
     m_pKin->getFilteredJointCurPosVel(jointState.m_strName,
                                       jointState.m_fPosition,
                                       jointState.m_fVelocity);
 
     // servo dynamics
     jointState.m_nOdPos   = pServo->GetOdometer();
     jointState.m_nEncPos  = pServo->OdometerToEncoder(jointState.m_nOdPos);
     jointState.m_nSpeed   = pServo->GetCurSpeed();
     jointState.m_fEffort  = (double)pServo->GetCurLoad();
 
     // optical limit switch states
     for(int i=0; i<HekOptLimitMaxPerJoint; ++i)
     {
       jointState.m_eOptSwitch[i] = m_monitor.getJointLimitTriState(pJoint, i);
     }
 
     // add state
     jointStatePoint.append(jointState);
   }
 
   return HEK_OK;
 }

DynaServo* hekateros::HekRobot::getMasterServo ( HekRobotJoint & joint )

inline

Get robotic joint's master servo.

Parameters

joint Robotic joint.

Returns: If found, returns pointer to servo. Otherwise returns NULL.

Definition at line 652 of file hekRobot.h.

References hekateros::HekRobotJoint::m_nMasterServoId, and m_pDynaChain.

     {
       return m_pDynaChain!=NULL? m_pDynaChain->GetServo(joint.m_nMasterServoId):
                                  NULL;
     }

int hekateros::HekRobot::getProdId ( )

inline

Convenience function to get this Hekateros description's base product id.

Returns: Returns product id. See HekProdId.

Definition at line 467 of file hekRobot.h.

References hekateros::HekDesc::getArmDesc(), hekateros::HekDescArm::getProdId(), and m_descHek.

     {
       return m_descHek.getArmDesc()->getProdId();
     }

std::string hekateros::HekRobot::getProdName ( )

inline

Convenience function to get this Hekateros description's base product name.

Returns: Returns product name. See HekProdName.

Definition at line 478 of file hekRobot.h.

References hekateros::HekDesc::getArmDesc(), hekateros::HekDescArm::getProdName(), and m_descHek.

     {
       return m_descHek.getArmDesc()->getProdName();
     }

int HekRobot::getRobotState ( HekRobotState & robotState )

Get the robot current state.

Parameters

[in,out] robotState Robot state.

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

< stopped speed

Definition at line 904 of file hekRobot.cxx.

Referenced by resetEStop().

 {
   static int      TuneStoppedSpeed = 9;   ///< stopped speed
 
   int             iter;           // servo iterator
   int             nServoId;       // servo id
   DynaServo      *pServo;         // servo
   bool            bIsMoving;      // robot is [not] moving
   HekServoHealth  health;         // servo health
   bool            bOldAlarmState; // old alarm state
 
   robotState.clear();
 
   bOldAlarmState  = m_bAlarmState;
   m_bAlarmState   = m_monitor.getAlarmCond();
 
   robotState.m_eRobotMode         = m_eRobotMode;
   robotState.m_eIsEStopped        = m_bIsEStopped?
                                             HekTriStateTrue: HekTriStateFalse;
   robotState.m_eIsCalibrated      = m_eOpState == HekOpStateCalibrated?
                                             HekTriStateTrue: HekTriStateFalse;
   robotState.m_eAreDrivesPowered  = m_bAreServosPowered? 
                                             HekTriStateTrue: HekTriStateFalse;
   robotState.m_eIsMotionPossible  = (m_eOpState == HekOpStateCalibrated) &&
                                       !m_bAlarmState && !m_bNoExec? 
                                             HekTriStateTrue: HekTriStateFalse;
 
   robotState.m_eIsInError = HekTriStateFalse;
   robotState.m_nErrorCode = HEK_OK;
   bIsMoving               = false;
 
   for(nServoId = m_pDynaChain->IterStart(&iter);
       nServoId != DYNA_ID_NONE;
       nServoId = m_pDynaChain->IterNext(&iter))
   {
     pServo = m_pDynaChain->GetServo(nServoId);
 
     health.m_nServoId     = nServoId;
     health.m_fTemperature = pServo->CvtRawTempToC(pServo->GetCurTemp());
     health.m_fVoltage     = pServo->CvtRawVoltToVolts(pServo->GetCurVolt());
     health.m_uAlarms      = pServo->GetAlarms();
 
     if( health.m_uAlarms != DYNA_ALARM_NONE )
     {
       robotState.m_eIsInError = HekTriStateTrue;
       robotState.m_nErrorCode = -HEK_ECODE_ALARMED;
     }
 
     if( iabs(pServo->GetCurSpeed()) > TuneStoppedSpeed )
     {
       bIsMoving = true;
     }
 
     robotState.m_vecServoHealth.push_back(health);
   }
 
   // estop overrides other alarms
   if( m_bIsEStopped )
   {
     robotState.m_eIsInError = HekTriStateTrue;
     robotState.m_nErrorCode = -HEK_ECODE_ESTOP;
   }
 
   // new alarm state other than estop. 
   else if( m_bAlarmState && (m_bAlarmState != bOldAlarmState) )
   {
     robotState.m_eIsInError = HekTriStateTrue;
     robotState.m_nErrorCode = -HEK_ECODE_ALARMED;
   }
 
   robotState.m_eIsInMotion = bIsMoving? HekTriStateTrue: HekTriStateFalse;
 
   return HEK_OK;
 }

void hekateros::HekRobot::getTrajectoryParams	(	HekNorm &	eNorm,
		double &	fEpsilon
	)

inline

Get trajectory parameters.

Parameters

[out]	eNorm	Distance norm.
[out]	fEpsilon	Waypoint precision (radians)

Definition at line 581 of file hekRobot.h.

References hekateros::HekTunes::getTrajectoryParams(), and m_tunes.

     {
       m_tunes.getTrajectoryParams(eNorm, fEpsilon);
     }

int HekRobot::getTrajectoryState ( HekJointTrajectoryFeedback & trajectoryFeedback )

Get trajectory feedback.

Parameters

[in,out] trajectoryFeedback Joint state point.

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

Definition at line 1035 of file hekRobot.cxx.

References hekateros::HekJointTrajectoryFeedback::clear(), and hekateros::HekJointStatePoint::getNumPoints().

Referenced by resetEStop().

 {
   HekJointStatePoint  jointStatePoint;
   int                 iActual;
   int                 i;
   int                 rc;
 
   trajectoryFeedback.clear();
 
   // last issued trajectory is the desired trajectory
   trajectoryFeedback[HekJointTrajectoryFeedback::TRAJ_DESIRED] = m_lastTrajArm;
 
   // retrieve joint state
   if( (rc = getJointState(jointStatePoint)) < 0 )
   {
     LOGERROR("Cannot retrieve joint state data.");
     return rc;
   }
 
   // copy joint state to actual trajectory
   iActual = HekJointTrajectoryFeedback::TRAJ_ACTUAL;
  
   for(i=0; i<jointStatePoint.getNumPoints(); ++i)
   {
     trajectoryFeedback[iActual].append(jointStatePoint[i].m_strName,
                                        jointStatePoint[i].m_fPosition,
                                        jointStatePoint[i].m_fVelocity);
   }
 
   return HEK_OK;
 }

void hekateros::HekRobot::getVersion	(	int &	nVerMajor,
		int &	nVerMinor,
		int &	nRevision
	)

inline

Get the Hekateros robotic arm hardware version numbers.

Parameters

[out]	nVerMajor	Major version number.
[out]	nVerMinor	Minor version number.
[out]	nVerRevision	Revision version number.

Definition at line 419 of file hekRobot.h.

References hekateros::HekDesc::getArmDesc(), hekateros::HekDescArm::getProdHwVer(), HEK_VER_MAJOR, HEK_VER_MINOR, HEK_VER_REV, m_descHek, and hekateros::strToVersion().

     {
       uint_t  uHwVer;
 
       uHwVer    = strToVersion(m_descHek.getArmDesc()->getProdHwVer());
       nVerMajor = HEK_VER_MAJOR(uHwVer);
       nVerMinor = HEK_VER_MINOR(uHwVer);
       nRevision = HEK_VER_REV(uHwVer);
     }

std::string hekateros::HekRobot::getVersion ( )

inline

Get the Hekateros robotic arm hardware version string.

Version number strings are of the dotted form maj.min.rev.

Returns: Version string.

Definition at line 446 of file hekRobot.h.

References hekateros::HekDesc::getArmDesc(), hekateros::HekDescArm::getProdHwVer(), and m_descHek.

     {
       return m_descHek.getArmDesc()->getProdHwVer();
     }

uint_t hekateros::HekRobot::getVersionNum ( )

inline

Get the Hekateros robotic arm hardware version compact number.

Returns: Return byte-packed comparable verion number.

Definition at line 434 of file hekRobot.h.

References hekateros::HekDesc::getArmDesc(), hekateros::HekDescArm::getProdHwVer(), m_descHek, and hekateros::strToVersion().

     {
       return strToVersion(m_descHek.getArmDesc()->getProdHwVer());
     }

int HekRobot::gotoBalancedPos ( )

Move robotic arm to its pre-defined balanced position.

Some Considerations:

This move assumes an unrestricted workspace save for an horizontal plane defined by the platform on which the Hekateros is mounted to. Strategy:

The shoulder movement is the most critical as it will lift away the horizontal plane, and hopefully away from any obstructions.
The elbow joint may swing out, possibly causing a collision before the shoulder has sufficiently moved to a safer position. Therefore the elbow joint's velocity is slower than the shoulder's velocity.
The rotating base, if present, swings left or right, possibly dragging the arm across the horizontal plane. Therefore, this joint also moves slower than the shoulder.
The other joints are of no consequence.

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

Definition at line 513 of file hekRobot.cxx.

References hekateros::HekJointTrajectoryPoint::append(), hekateros::degToRad(), HEK_TRY_CALIB, HEK_TRY_CONN, HEK_TRY_NOT_ESTOP, hekateros::HekRobotJoint::m_fBalPosRads, and hekateros::HekRobotJoint::m_strName.

 {
   HekJointTrajectoryPoint   trajPoint;
   MapRobotJoints::iterator  iter;
 
   int             nMasterServoId;
   HekRobotJoint  *pJoint;
   int             rc;
 
   HEK_TRY_CONN();
   HEK_TRY_CALIB();
   HEK_TRY_NOT_ESTOP();
 
   //
   // Build trajoectory point.
   //
   for(iter = m_jointsArm.begin(); iter != m_jointsArm.end(); ++iter)
   {
     nMasterServoId  = iter->first;
     pJoint          = &(iter->second);
 
     switch( nMasterServoId )
     {
       case HekServoIdBase:
         trajPoint.append(pJoint->m_strName, pJoint->m_fBalPosRads,
             degToRad(60.0));
         break;
       case HekServoIdShoulderL:
         trajPoint.append(pJoint->m_strName, pJoint->m_fBalPosRads,
             degToRad(40.0));
         break;
       case HekServoIdElbow:
         trajPoint.append(pJoint->m_strName, pJoint->m_fBalPosRads,
             degToRad(35.0));
         break;
       case HekServoIdWristPitch:
         trajPoint.append(pJoint->m_strName, pJoint->m_fBalPosRads,
             degToRad(50.0));
         break;
       case HekServoIdWristRot:
         trajPoint.append(pJoint->m_strName, pJoint->m_fBalPosRads,
             degToRad(60.0));
         break;
       case HekServoIdGraboid:
         trajPoint.append(pJoint->m_strName, pJoint->m_fBalPosRads,
             degToRad(20.0));
         break;
       default:
         break;
     }
   }
 
   if( (rc = moveArm(trajPoint)) < 0 )
   {
     LOGERROR("Move to pre-defined balanced position failed.");
   }
   else
   {
     m_bAtBalancedPos = true;
     LOGDIAG2("Hekateros at balanced position.");
   }
 
   return rc;
 }

int HekRobot::gotoParkedPos ( )

Move robotic arm to its pre-defined parked position.

See discussion at gotoBalancedPos about pre-defined movements.

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

Definition at line 578 of file hekRobot.cxx.

References hekateros::HekJointTrajectoryPoint::append(), hekateros::degToRad(), HEK_TRY_CALIB, HEK_TRY_CONN, HEK_TRY_NOT_ESTOP, hekateros::HekRobotJoint::m_fParkPosRads, and hekateros::HekRobotJoint::m_strName.

 {
   HekJointTrajectoryPoint   trajPoint;
   MapRobotJoints::iterator  iter;
 
   int             nMasterServoId;
   HekRobotJoint  *pJoint;
   int             rc;
 
   HEK_TRY_CONN();
   HEK_TRY_CALIB();
   HEK_TRY_NOT_ESTOP();
 
   //
   // Build trajoectory point.
   //
   for(iter = m_jointsArm.begin(); iter != m_jointsArm.end(); ++iter)
   {
     nMasterServoId  = iter->first;
     pJoint          = &(iter->second);
 
     switch( nMasterServoId )
     {
       case HekServoIdBase:
         trajPoint.append(pJoint->m_strName, pJoint->m_fParkPosRads,
             degToRad(60.0));
         break;
       case HekServoIdShoulderL:
         trajPoint.append(pJoint->m_strName, pJoint->m_fParkPosRads,
             degToRad(40.0));
         break;
       case HekServoIdElbow:
         trajPoint.append(pJoint->m_strName, pJoint->m_fParkPosRads,
             degToRad(35.0));
         break;
       case HekServoIdWristPitch:
         trajPoint.append(pJoint->m_strName, pJoint->m_fParkPosRads,
             degToRad(50.0));
         break;
       case HekServoIdWristRot:
         trajPoint.append(pJoint->m_strName, pJoint->m_fParkPosRads,
             degToRad(60.0));
         break;
       case HekServoIdGraboid:
         trajPoint.append(pJoint->m_strName, pJoint->m_fParkPosRads,
             degToRad(20.0));
         break;
       default:
         break;
     }
   }
 
   if( (rc = moveArm(trajPoint)) < 0 )
   {
     LOGERROR("Move to pre-defined parked position failed.");
   }
   else
   {
     m_bAtParkedPos = true;
     LOGDIAG2("Hekateros at parked position.");
   }
 
   return rc;
 }

int HekRobot::gotoZeroPtPos ( )

Move robotic arm to its pre-defined calibrated zero point position.

See discussion at gotoBalancedPos about pre-defined movements.

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

Definition at line 643 of file hekRobot.cxx.

References hekateros::HekJointTrajectoryPoint::append(), hekateros::degToRad(), HEK_TRY_CALIB, HEK_TRY_CONN, HEK_TRY_NOT_ESTOP, hekateros::HekRobotJoint::m_fCalibPosRads, and hekateros::HekRobotJoint::m_strName.

 {
   HekJointTrajectoryPoint   trajPoint;
   MapRobotJoints::iterator  iter;
 
   int             nMasterServoId;
   HekRobotJoint  *pJoint;
   int             rc;
 
   HEK_TRY_CONN();
   HEK_TRY_CALIB();
   HEK_TRY_NOT_ESTOP();
 
   //
   // Build trajoectory point.
   //
   for(iter = m_jointsArm.begin(); iter != m_jointsArm.end(); ++iter)
   {
     nMasterServoId  = iter->first;
     pJoint          = &(iter->second);
 
     switch( nMasterServoId )
     {
       case HekServoIdBase:
         trajPoint.append(pJoint->m_strName, pJoint->m_fCalibPosRads,
             degToRad(60.0));
         break;
       case HekServoIdShoulderL:
         trajPoint.append(pJoint->m_strName, pJoint->m_fCalibPosRads,
             degToRad(30.0));
         break;
       case HekServoIdElbow:
         trajPoint.append(pJoint->m_strName, pJoint->m_fCalibPosRads,
             degToRad(25.0));
         break;
       case HekServoIdWristPitch:
         trajPoint.append(pJoint->m_strName, pJoint->m_fCalibPosRads,
             degToRad(50.0));
         break;
       case HekServoIdWristRot:
         trajPoint.append(pJoint->m_strName, pJoint->m_fCalibPosRads,
             degToRad(60.0));
         break;
       case HekServoIdGraboid:
         trajPoint.append(pJoint->m_strName, pJoint->m_fCalibPosRads,
             degToRad(20.0));
         break;
       default:
         break;
     }
   }
 
   if( (rc = moveArm(trajPoint)) < 0 )
   {
     LOGERROR("Move to pre-defined calibrated zero point position failed.");
   }
   else
   {
     LOGDIAG2("Hekateros at zero point position.");
   }
 
   return rc;
 }

int hekateros::HekRobot::isAlarmed ( )

inline

Test if robot is alarmed.

Returns: Returns true or false.

Definition at line 538 of file hekRobot.h.

References hekateros::HekMonitor::getAlarmCond(), m_bAlarmState, and m_monitor.

     {
       // monitor always has the latest alarm conditions. Robot simple shadows.
       m_bAlarmState = m_monitor.getAlarmCond();
       return m_bAlarmState;
     }

int hekateros::HekRobot::isAtBalancedPos ( )

inline

Test if robot is as pre-defined balanced position.

Returns: Returns true or false.

Definition at line 550 of file hekRobot.h.

References m_bAtBalancedPos.

     {
       return m_bAtBalancedPos;
     }

int hekateros::HekRobot::isAtParkedPos ( )

inline

Test if robot is as pre-defined parked position.

Returns: Returns true or false.

Definition at line 560 of file hekRobot.h.

References m_bAtParkedPos.

     {
       return m_bAtParkedPos;
     }

int hekateros::HekRobot::isCalibrated ( )

inline

Test if robot is calibrated.

Returns: Returns true or false.

Definition at line 508 of file hekRobot.h.

References hekateros::HekOpStateCalibrated, and m_eOpState.

     {
       return m_eOpState == HekOpStateCalibrated;
     }

bool hekateros::HekRobot::isConnected ( )

inline

Test if connected to Hekateros hardware.

Returns: Returns true or false.

Definition at line 407 of file hekRobot.h.

References m_pDynaComm.

     {
       return (m_pDynaComm != NULL) && m_pDynaComm->IsOpen()? true: false;
     }

int hekateros::HekRobot::isDescribed ( )

inline

Test if robot is fully described via configuration XML.

Returns: Returns true or false.

Definition at line 498 of file hekRobot.h.

References hekateros::HekDesc::isDescribed(), and m_descHek.

     {
       return m_descHek.isDescribed();
     }

int hekateros::HekRobot::isEStopped ( )

inline

Test if robot is current emergency stopped.

Returns: Returns true or false.

Definition at line 518 of file hekRobot.h.

References m_bIsEStopped.

     {
       return m_bIsEStopped;
     }

bool HekRobot::isInMotion ( )

Test if any joint in any of the kinematic chains is moving.

Returns: Returns true or false.

< stopped speed

Definition at line 1067 of file hekRobot.cxx.

References hekateros::iabs().

Referenced by setRobotMode().

 {
   static int      TuneStoppedSpeed = 9;   ///< stopped speed
 
   int             iter;       // servo iterator
   int             nServoId;   // servo id
   DynaServo      *pServo;     // servo
 
   for(nServoId = m_pDynaChain->IterStart(&iter);
       nServoId != DYNA_ID_NONE;
       nServoId = m_pDynaChain->IterNext(&iter))
   {
     pServo = m_pDynaChain->GetServo(nServoId);
 
     if( iabs(pServo->GetCurSpeed()) > TuneStoppedSpeed )
     {
       return true;
     }
   }
 
   return false;
 }

void hekateros::HekRobot::lock ( )

inlineprotected

Lock the robot mutex.

The lock()/unlock() primitives provide thread-safe access.

Context:: Any.

Definition at line 980 of file hekRobot.h.

     {
       pthread_mutex_lock(&m_mutex);
     }

int HekRobot::moveArm ( HekJointTrajectoryPoint & trajectoryPoint )

Move arm through trajectory point.

Parameters

trajectoryPoint Trajectory end point.

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

Definition at line 880 of file hekRobot.cxx.

References HEK_TRY_CALIB, HEK_TRY_CONN, and HEK_TRY_NOT_ESTOP.

Referenced by resetEStop().

 {
   int   rc;
 
   HEK_TRY_CONN();
   HEK_TRY_CALIB();
   HEK_TRY_NOT_ESTOP();
 
   lock();
 
   if( !m_bAreServosPowered )
   {
     m_bAreServosPowered = true;
 
     m_monitor.markPoweredCond(m_bAreServosPowered);
   }
 
   rc = m_pKin->move(trajectoryPoint);
 
   unlock();
 
   return rc >= 0? HEK_OK: rc;
 }

int hekateros::HekRobot::moveWait	(	DynaServo *	pServo,
		int	nOdGoalPos,
		int	nSpeed
	)

protected

Move servo to position.

This call blocks until move is complete.

Parameters

pServo	Pointer to servo.
nOdGoalPos	Goal position in raw odometer units.
nSpeed	Speed in raw units.

Returns: New odometer position.

void hekateros::HekRobot::moveWristRot ( )

protected

Move wrist rotation for any necessary final position adjustments.

The wrist coupled pitch and rotation joints can have their respective goal trajectories change by various actions.

int HekRobot::openGripper ( )

Open gripper to its maximum opened position.

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

Definition at line 707 of file hekRobot.cxx.

References hekateros::HekJointTrajectoryPoint::append(), hekateros::degToRad(), HEK_TRY_CALIB, HEK_TRY_CONN, HEK_TRY_NOT_ESTOP, hekateros::HekRobotJoint::m_fMaxSoftLimitRads, and hekateros::HekRobotJoint::m_strName.

 {
   HekJointTrajectoryPoint   trajPoint;
 
   HekRobotJoint  *pJoint;
   int             rc;
 
   HEK_TRY_CONN();
   HEK_TRY_CALIB();
   HEK_TRY_NOT_ESTOP();
 
   if( (pJoint = getArmJoint(HekServoIdGraboid)) == NULL )
   {
     LOGERROR("No gripper end effector found.");
     return -HEK_ECODE_NO_RSRC;
   }
 
   trajPoint.append(pJoint->m_strName, pJoint->m_fMaxSoftLimitRads,
             degToRad(60.0));
 
   if( (rc = moveArm(trajPoint)) < 0 )
   {
     LOGERROR("Move to pre-defined gripper open position failed.");
   }
   else
   {
     LOGDIAG2("Hekateros gripper opened.");
   }
 
   return rc;
 }

double hekateros::HekRobot::pctOfMaxJointVelocity	(	HekRobotJoint &	joint,
		double	fVelPct
	)

inline

Convert percent of maximum to joint velocity.

The maximum joint velocity is the theoretical maximum joint velocity based on joint servo specs of unloaded maximum RPMs and the Hekateros external gear ratios.

Parameters

joint	Joint description.
fValPct	Joint velocity (%).

Returns: Joint velocity (radians/second).

Definition at line 670 of file hekRobot.h.

References hekateros::HekRobotJoint::m_fMaxJointRadsPerSec.

     {
       return fVelPct < 100.0? fVelPct/100.0 * joint.m_fMaxJointRadsPerSec:
                               joint.m_fMaxJointRadsPerSec;
     }

int HekRobot::release ( )

Release arm and accessories.

Servos will stop driving, so the arm may fall. This call is assumed to be under control, so recalibration is not required. Typically, the arm is released during manual repositioning or teaching.

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

Definition at line 812 of file hekRobot.cxx.

References HEK_TRY_CONN, and HEK_TRY_NO_EXEC.

 {
   HEK_TRY_NO_EXEC();
   HEK_TRY_CONN();
 
   m_bAreServosPowered = false;
 
   m_monitor.markPoweredCond(m_bAreServosPowered);
 
   m_lastTrajArm.clear();
 
   m_pKin->release();
 
   LOGDIAG2("Hekateros servo drives released.");
 
   return HEK_OK;
 }

void HekRobot::reload ( )

Reload Hekateros's reloadable configuration and reset operational parameters.

The robot connection and calibration states are uneffected.

Definition at line 499 of file hekRobot.cxx.

References hekateros::HekXmlTune::load().

 {
   HekXmlTune  xml;
 
   // parse user tune parameter overrides
   if( xml.load(m_tunes,HekSysCfgPath, HekEtcTune) == HEK_OK )
   {
     LOGDIAG2("User specified tuning parameters reloaded from XML file %s.",
             HekEtcTune);
 
     m_pKin->reload(m_tunes);
   }
 }

void HekRobot::resetCalibStateForAllJoints ( bool bForceRecalib )

protected

Mark all relevant joints for recalibration.

Parameters

bForceRecalib If true, force recalibration all joints. Otherwise calibrate only the uncalibrated.

Definition at line 1607 of file hekRobot.cxx.

 {
   MapRobotJoints::iterator  iter;
 
   for(iter = m_jointsArm.begin(); iter != m_jointsArm.end(); ++iter)
   {
     if( bForceRecalib || (iter->second.m_eOpState != HekOpStateCalibrated) )
     {
       iter->second.m_eOpState = HekOpStateUncalibrated;
     }
   }
 }

void hekateros::HekRobot::resetEStop ( )

inline

Reset (clears) emergency stop condition.

Note: Servos are not re-powered until an move or freeze action is called.

Definition at line 309 of file hekRobot.h.

References getJointState(), getRobotState(), getTrajectoryState(), m_bIsEStopped, m_monitor, hekateros::HekMonitor::markEStopCond(), and moveArm().

     {
       m_bIsEStopped = false;
       m_monitor.markEStopCond(m_bIsEStopped);
     }

int HekRobot::scanDynaBus ( int nMaxTries )

protected

Scan Hekateros dynamixel bus hardware and match against description.

Parameters

nMaxTries Maximums number of scan attempts before failing.

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

Definition at line 1090 of file hekRobot.cxx.

 {
   static uint_t usecTry = 500000;
 
   int   nNumServosExpected;   // number of expected and required servos
   int   nNumServosScanned;    // number of scanned servos
   int   nNumServosMatched;    // number of scanned and matched servos
   int   nTries;               // scan attempts
   int   nServoId;             // servo id
   int   iter;                 // servo iterator
   int   rc;                   // return code
 
   nNumServosExpected = m_descHek.getNumServos();
 
   //
   // Scan hardware.
   //
   for(nTries=0; nTries<nMaxTries; ++nTries)
   {
     LOGDIAG3("Scanning Hekateros hardware - attempt %d of %d.",
         nTries+1, nMaxTries);
 
     // scan for servos
     nNumServosScanned = m_pDynaChain->AddNewServosByScan();
 
     //
     // Scanned at least the required number.
     //
     if( nNumServosScanned >= nNumServosExpected )
     {
       // check scanned ids against required
       for(nServoId = m_pDynaChain->IterStart(&iter), nNumServosMatched=0;
           nServoId != DYNA_ID_NONE;
           nServoId = m_pDynaChain->IterNext(&iter))
       {
         // matched
         if( m_descHek.hasServo(nServoId) )
         {
           ++nNumServosMatched;
         }
 
         // unmathced
         else
         {
           LOGWARN("Servo %d unexpected - uncontrolled.", nServoId);
         }
       }
 
       // servos matched
       if( nNumServosMatched == nNumServosExpected )
       {
         rc = HEK_OK;
         break;
       }
 
       // some required servos are missing
       else
       {
         LOGERROR("Match %d scanned servo ids, expected %d required matches.",
           nNumServosMatched, nNumServosExpected);
         rc = -HEK_ECODE_FORMAT;
       }
     }
 
     //
     // Scan failed to detect the minimum number of servos.
     //
     else
     {
       LOGERROR("Scanned %d servos, expected %d.",
           nNumServosScanned, nNumServosExpected);
       rc = -HEK_ECODE_NO_RSRC;
     }
 
     usleep(usecTry);
   }
 
   if( rc == HEK_OK )
   {
     LOGDIAG3("Hekateros dynamixel bus hardware scanned successfully.");
   }
 
   return rc;
 }

int hekateros::HekRobot::scanHw ( )

protected

Scan Hekateros hardware and match against description.

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

void hekateros::HekRobot::setAllJointGoalsToNull ( )

protected

Set all joint goals to a null trajectory.

A null trajectory for a joint is the joint's current position and zero velocity.

void hekateros::HekRobot::setJointGoalsToNull ( const std::vector< std::string > & vecNames )

protected

Set a set of joint goals to a null trajectory.

A null trajectory for a joint is the joint's current position and zero velocity.

Parameters

vecNames Vector of joint names.

void hekateros::HekRobot::setJointGoalToNull ( const std::string & strName )

protected

Set joint goal to null a trajectory.

A null trajectory for a joint is the joint's current position and zero velocity.

Parameters

strName Joint name.

void hekateros::HekRobot::setRobotMode ( HekRobotMode eRobotMode )

inline

Set robot's operational mode.

Returns: eRobotMode Robot operation mode. See HekRobotMode.

Definition at line 361 of file hekRobot.h.

References cancelAsyncTask(), getAsyncRc(), getAsyncState(), isInMotion(), and m_eRobotMode.

     {
       m_eRobotMode = eRobotMode;
     }

int HekRobot::stop ( const std::vector< std::string > & vecNames )

Stops specified joints at current position.

Servos are still being driven.

Parameters

vecNames Vector of joint names.

the number of joints stopped. the number of joints stopped.

Definition at line 830 of file hekRobot.cxx.

References HEK_TRY_CONN, HEK_TRY_NO_EXEC, and HEK_TRY_NOT_ESTOP.

 {
   int   n;
 
   HEK_TRY_NO_EXEC();
   HEK_TRY_CONN();
   HEK_TRY_NOT_ESTOP();
 
   m_lastTrajArm.clear();
 
   n = m_pKin->stop(vecNames);
 
   LOGDIAG3("%d joints stopped.", n);
 
   return n;
 }

void hekateros::HekRobot::stopWait	(	const std::vector< std::string > &	vecNames,
		int	nMaxTries = `10`
	)

protected

Wait for the specified joints to stop moving.

A maximum number of wait-test tries is performed until either all of the joints haved stopped or the maximum number of tries has been reached. The between-test wait is 10 milliseconds.

Parameters

vecNames	Vector of joint names.
nMaxTries	Maximum number of test-wait tries.

bool hekateros::HekRobot::trylock ( )

inlineprotected

Try to lock the robot mutex.

Context:: Any.

Returns: Returns true if lock acquired, else returns false.

Definition at line 1004 of file hekRobot.h.

     {
       return pthread_mutex_trylock(&m_mutex) == 0? true: false;
     }

void hekateros::HekRobot::unlock ( )

inlineprotected

Unlock the robot mutex.

Context:: Any.

Definition at line 991 of file hekRobot.h.

     {
       pthread_mutex_unlock(&m_mutex);
     }

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

include/Hekateros/hekRobot.h
sw/libhekateros/hekRobot.cxx

Public Types

Public Member Functions

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Friends

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation