hekateros::HekKinematics Class Reference

#include <hekKin.h>

Public Types
enum	ThreadState {   ThreadStateUninit,   ThreadStateReady,   ThreadStateRunning,   ThreadStateExit }
	Kinematics thread states. More...
typedef map< std::string, HekKinJoint * >	KinChain
	Associative Map of kinematic joints.
typedef vector< std::string >	TaskList
	Order list of tasks.

Public Member Functions
	HekKinematics (DynaChain &dynaChain, MapRobotJoints &kinJointDesc, const HekTunes &tunes)
	Default initializer constructor. More...
virtual	~HekKinematics ()
	Desctructor.
virtual void	buildTaskList ()
	Build ordered list of tasks to execute. More...
virtual void	reload (const HekTunes &tunes)
	Reload configuration tuning parameters for all joints. More...
virtual void	getJointCurPosVel (const std::string &strJointName, double &fJointCurPos, double &fJointCurVel)
	Get the current instantaneous joint position and velocity. More...
virtual void	getFilteredJointCurPosVel (const std::string &strJointName, double &fJointCurPos, double &fJointCurVel)
	Get the smoothed (filtered) current joint position and velocity. More...
void	getServoCurPosSpeed (const std::string &strJointName, int &nServoCurPos, int &nServoCurSpeed)
	Get the current servo position and speed. More...
int	jointPosToServoPos (const std::string &strJointName, const double fPos)
	Convert joint position to the equivalent servo position. More...
double	servoPosToJointPos (const std::string &strJointName, const int nOdPos)
	Convert servo position to the equivalent joint position. More...
virtual bool	isStopped (const std::string &strJointName)
	Test if joint is moving. More...
virtual bool	hasOverTorqueCondition (const std::string &strJointName)
	Test if joint is in an over torque condition. More...
virtual int	resetServoOdometersForAllJoints ()
	Reset all joints' master servos odometers to the current respective encoder positions. More...
virtual int	resetServoOdometer (const std::string &strJointName)
	Reset joint's master servo odometer to the current encoder position. More...
virtual void	estop ()
	Emergency stop the kinematics chain. More...
virtual void	freeze ()
	Freeze kinematics chain at the current position. More...
virtual void	release ()
	Release kinematics chain. More...
virtual int	stop ()
	Stop kinematics chain at the current position. More...
virtual int	stop (const std::vector< std::string > &vecJointNames)
	Stop the set of joints at the current position. More...
virtual int	stop (const std::string &strJointName)
	Stop one joint at the current position. More...
virtual int	waitForAllStop (double fSeconds)
	Wait for all joints to stop. More...
virtual int	move (HekJointTrajectoryPoint &trajectoryPoint)
	Move kinematic chain through a trajectory point. More...
virtual int	move (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel)
	Move single joint. More...
virtual void	setHz (const double fHz)
	Set thread run rate and ancillary data. More...
int	runThread (const double fHz=HekTuneKinHzDft)
	Run the kinematics thread. More...
void	waitOneCycle ()
	Wait one full cycle. More...

Protected Member Functions
void	lock ()
	Lock the kinematics thread. More...
void	unlock ()
	Unlock the kinematics thread. More...
virtual void	sense ()
	Sense the state of all joints. More...
virtual void	react ()
	React to any necessary time-critical joint events. More...
virtual void	sense_react ()
	Sense-react the state of all joints. More...
virtual void	plan ()
	Plan motions for all joints. More...
virtual int	act ()
	Move all joints. More...
virtual void	monitorHealth ()
	Monitor servo health thread task. More...
virtual void	exec ()
	Execute kinematics task(s). More...
void	createThread ()
	Create the kinematics thread. More...
void	terminateThread ()
	Terminate the kinematics thread. More...
void	changeState (ThreadState eNewState)
	Change the kinematics thread state. More...
void	readyWait ()
	Wait indefinitely while in the ready state. More...
void	timedWait (const struct timespec &tsTimeout)
	Timed wait until state change or time out. More...
void	schedWait ()
	Block kinematics thread until the next subcycle task is to be run. More...

Static Protected Member Functions
static void *	thread (void *pArg)
	The kinematics thread. More...

Protected Attributes
ThreadState	m_eState
	thread state
pthread_mutex_t	m_mutexSync
	synchonization mutex
pthread_cond_t	m_condSync
	synchonization condition
pthread_t	m_thread
	pthread identifier
DynaChain &	m_dynaChain
	dynamixel chain
KinChain	m_kinChain
	kinematic chain
int	m_nNumJoints
	number of joints to control
int	m_nNumServos
	number of servos to monitor
double	m_fHz
	thread run rate (Hertz)
double	m_fTExec
	task execution period (seconds)
struct timespec	m_tsExecPeriod
	task execution period (converted)
struct timespec	m_tsSched
	working scheduler time stamp
bool	m_bIsControlling
	[not] actively controlling joints
TaskList	m_taskList
	list of tasks to exec per cycle
TaskList::iterator	m_iterTask
	task iterator
std::string	m_strTaskOneCycle
	task name one cycle away
int	m_iterHealth
	servo health iterator
int	m_nHealthServoId
	health monitoring servo id

Detailed Description

Hekateros kinematics class.

The HekKinematics class supports background control and monitoring of registered joints and servos.

Definition at line 137 of file hekKin.h.

Member Enumeration Documentation

enum hekateros::HekKinematics::ThreadState

Kinematics thread states.

Enumerator
ThreadStateUninit	thread created but unitialized
ThreadStateReady	thread created and ready to run
ThreadStateRunning	thread running
ThreadStateExit	thread exiting/exited

Definition at line 143 of file hekKin.h.

    {
      ThreadStateUninit,    ///< thread created but unitialized
      ThreadStateReady,     ///< thread created and ready to run
      ThreadStateRunning,   ///< thread running
      ThreadStateExit       ///< thread exiting/exited
    };

Constructor & Destructor Documentation

HekKinematics::HekKinematics	(	DynaChain &	dynaChain,
		MapRobotJoints &	kinJointDesc,
		const HekTunes &	tunes
	)

Default initializer constructor.

Parameters

dynaChain	Bound Dynamixel chain of all motors (servos).
kinJointDesc	Joint descriptions in kinematic chain to control.
tunes	Hekateros tuning parameters.

Definition at line 87 of file hekKin.cxx.

References buildTaskList(), createThread(), hekateros::HekRobotJoint::getJointName(), hekateros::HekRobotJoint::getMasterServoId(), hekateros::HekTuneKinHzMin, m_bIsControlling, m_condSync, m_dynaChain, m_eState, m_fHz, m_iterHealth, m_iterTask, m_kinChain, m_mutexSync, m_nHealthServoId, m_nNumJoints, m_nNumServos, m_taskList, and ThreadStateUninit.

                                                    :
    m_dynaChain(dynaChain)
{
  MapRobotJoints::iterator  iter;
  string                    strJointName;
  HekRobotJoint            *pJoint;
  int                       nServoId;
  DynaServo                *pServo;

  m_eState      = ThreadStateUninit;
  m_nNumJoints  = (int)kinJointDesc.size();
  m_nNumServos  = m_dynaChain.GetNumberInChain();
  m_fHz         = HekTuneKinHzMin;

  for(iter=kinJointDesc.begin(); iter!=kinJointDesc.end(); ++iter)
  {
    pJoint        = &iter->second;
    strJointName  = pJoint->getJointName();
    nServoId      = pJoint->getMasterServoId();
    pServo        = m_dynaChain.GetServo(nServoId);

    if( strJointName == "wrist_rot" )
    {
      m_kinChain[strJointName] = new HekKinJointWristRot(pJoint, pServo, tunes);
    }
    else
    {
      m_kinChain[strJointName] = new HekKinJoint(pJoint, pServo, tunes);
    }
  }

  //
  // Coupled wrist joints.
  //
  if( (m_kinChain.find("wrist_rot")   != m_kinChain.end()) &&
      (m_kinChain.find("wrist_pitch") != m_kinChain.end()) )
  {
    m_kinChain["wrist_rot"]->coupleJoint(m_kinChain["wrist_pitch"]);
  }

  buildTaskList();

#ifdef HEK_KIN_EXEC_ALG_INDIV
  m_iterTask        = m_taskList.begin();
#endif // HEK_KIN_EXEC_ALG_INDIV

  m_nHealthServoId  = m_dynaChain.IterStart(&m_iterHealth);

  pthread_mutex_init(&m_mutexSync, NULL);
  pthread_cond_init(&m_condSync, NULL);

  // create kinematics thread
  createThread();

  m_bIsControlling  = false;

#ifdef HEK_KIN_EXEC_ALG_SYNC
  m_bWaitOneCycle   = false;
#endif // HEK_KIN_EXEC_ALG_SYNC

  LOGDIAG3("Registered kinematic chain of %d joints and %d servos.",
      m_nNumJoints, m_nNumServos);
}

Member Function Documentation

int HekKinematics::act ( )

protectedvirtual

Move all joints.

Context:

Kinematics thread.

Definition at line 608 of file hekKin.cxx.

References hekateros::HEK_ECODE_DYNA, hekateros::HEK_OK, m_bIsControlling, and m_dynaChain.

Referenced by monitorHealth(), and unlock().

{
  uint_t  uCount;
  int     rc;

  if( !m_bIsControlling )
  {
    return HEK_OK;
  }

  if( (uCount = m_msgs.getSpeedTupleCount()) > 0 )
  {
    rc = m_dynaChain.SyncWriteGoalSpeed(m_msgs.getSpeedTuples(), uCount);
  }

  if( (rc == DYNA_OK) && ((uCount = m_msgs.getOdPosTupleCount()) > 0) )
  {
    rc = m_dynaChain.SyncWriteGoalPos(m_msgs.getOdPosTuples(), uCount);
  }

  rc = rc < 0? -HEK_ECODE_DYNA: HEK_OK;

  return rc;
}

void HekKinematics::buildTaskList ( )

virtual

Build ordered list of tasks to execute.

Todo:

TODO: Currently this list is hard coded to the Hekateros family of arms and end effectors. How to generalize?

Definition at line 174 of file hekKin.cxx.

References m_kinChain, and m_taskList.

Referenced by HekKinematics().

{
  m_taskList.empty();

  // 4DOF arms have no rotating base
  if( m_kinChain.find("base_rot") != m_kinChain.end() )
  {
    m_taskList.push_back("base_rot");
  }

  m_taskList.push_back("shoulder");
  m_taskList.push_back("elbow");
  m_taskList.push_back("wrist_pitch");

  // wrist rot coupled with pitch, keep close
  m_taskList.push_back("wrist_rot");

  // default gripper
  if( m_kinChain.find("grip") != m_kinChain.end() )
  {
    m_taskList.push_back("grip");
  }

#ifdef HEK_KIN_EXEC_ALG_INDIV
  // servo health task
  m_taskList.push_back("health");

  if( m_taskList.size() != m_kinChain.size()+1 )
  {
    LOGERROR("WTF: Task list not matching kinematic chain.");
  }
#endif // HEK_KIN_EXEC_ALG_INDIV
}

void hekateros::HekKinematics::changeState ( ThreadState  eNewState )

inlineprotected

Change the kinematics thread state.

The thread, if blocked, will be immediately woken up.

Parameters

eNewState

New state.

Context:

Calling thread or kinematics thread.

Definition at line 598 of file hekKin.h.

References readyWait(), schedWait(), and timedWait().

Referenced by runThread(), and terminateThread().

    {
      m_eState = eNewState;
      pthread_cond_signal(&m_condSync);
    }

void HekKinematics::createThread ( )

protected

Create the kinematics thread.

The thread remains blocked in the ready state until runThread() is called.

Context:

Calling thread.

Definition at line 809 of file hekKin.cxx.

References lock(), m_condSync, m_eState, m_mutexSync, m_thread, thread(), ThreadStateExit, and unlock().

Referenced by HekKinematics(), and unlock().

{
  pthread_attr_t      attr;
  struct sched_param  parm;
  int                 rc;

  // Initialize thread attributes.
  pthread_attr_init(&attr);

  //
  // Set thread scheduling policy.
  // SCHED_FIFO:
  //  + preemptive priority scheduling
  //  + highest priority thread runs until it choses to block/sleep
  //  + when it unblock/wakes it goes to the end of the queue for its priority
  //
  pthread_attr_setschedpolicy(&attr, SCHED_FIFO);

  //
  // Get the system's maximum priority and set this thread to the
  // next-to-the-highest priority.
  parm.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;
  pthread_attr_setschedparam(&attr, &parm);

  // Create kinematics thread.
  rc = pthread_create(&m_thread, &attr, HekKinematics::thread, (void *)this);
 
  // wait for thread to initialize
  if( rc == 0 )
  {
    lock();
    pthread_cond_wait(&m_condSync, &m_mutexSync);
    unlock();
  }
  else
  {
    LOGSYSERROR("pthread_create()");
    m_eState = ThreadStateExit;
  }
}

void HekKinematics::estop ( )

virtual

Emergency stop the kinematics chain.

All servos will stop driving, so chain may fall.

Context:

Calling thread.

Definition at line 356 of file hekKin.cxx.

References lock(), m_bIsControlling, m_dynaChain, stop(), unlock(), and waitForAllStop().

{
  if( m_bIsControlling )
  {
    stop();
    waitForAllStop(1.0);
  }

  lock();

  m_bIsControlling = false;

  m_dynaChain.EStop();

  unlock();
}

void HekKinematics::exec ( )

protectedvirtual

Execute kinematics task(s).

Context:

Kinematic thread.

Definition at line 687 of file hekKin.cxx.

References hekateros::HekKinJoint::act(), lock(), m_bIsControlling, m_condSync, m_iterTask, m_kinChain, m_strTaskOneCycle, m_taskList, monitorHealth(), hekateros::HekKinJoint::senseDynamics(), and unlock().

Referenced by monitorHealth(), thread(), and unlock().

{
  lock();

  string strTask = *m_iterTask++;

  if( m_iterTask == m_taskList.end() )
  {
    m_iterTask = m_taskList.begin();
  }

#ifdef DBG
  if( !m_strTaskOneCycle.empty() )
  {
    fprintf(stderr, "DBG: %s(): task=%s\n", LOGFUNCNAME, strTask.c_str());
  }
#endif // DBG

  if( strTask == m_strTaskOneCycle )
  {
    m_strTaskOneCycle.clear();
    pthread_cond_signal(&m_condSync);
  }


  // health monitor task
  if( strTask == "health" )
  {
    monitorHealth();
  }

  // joint sense and control tasks
  else
  {
    HekKinJoint *pKinJoint = m_kinChain[strTask];

    pKinJoint->senseDynamics(m_bIsControlling);

    if( m_bIsControlling )
    {
      pKinJoint->act();
    }
  }

  unlock();
}

void HekKinematics::freeze ( )

virtual

Freeze kinematics chain at the current position.

The joint servos are still being driven. However, all active joint motion control will cease.

Context:

Calling thread.

Definition at line 373 of file hekKin.cxx.

References m_bIsControlling, stop(), and waitForAllStop().

{
  m_bIsControlling = true;

  stop();

  waitForAllStop(5.0);
}

void HekKinematics::getFilteredJointCurPosVel ( const std::string &  strJointName, double &  fJointCurPos, double &  fJointCurVel  )

virtual

Get the smoothed (filtered) current joint position and velocity.

Parameters

[in]	strJointName	Joint name.
[out]	fJointCurPos	Current joint position (radians).
[out]	fJointCurVel	Current joint velocity (radians/second).

Definition at line 254 of file hekKin.cxx.

References m_kinChain.

{
  KinChain::iterator  pos;

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    pos->second->getFilteredJointCurPosVel(fJointCurPos, fJointCurVel);
  }
}

void HekKinematics::getJointCurPosVel ( const std::string &  strJointName, double &  fJointCurPos, double &  fJointCurVel  )

virtual

Get the current instantaneous joint position and velocity.

Parameters

[in]	strJointName	Joint name.
[out]	fJointCurPos	Current joint position (radians).
[out]	fJointCurVel	Current joint velocity (radians/second).

Definition at line 242 of file hekKin.cxx.

References m_kinChain.

{
  KinChain::iterator  pos;

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    pos->second->getJointCurPosVel(fJointCurPos, fJointCurVel);
  }
}

void HekKinematics::getServoCurPosSpeed	(	const std::string &	strJointName,
		int &	nServoCurPos,
		int &	nServoCurSpeed
	)

Get the current servo position and speed.

Parameters

[in]	strJointName	Joint name.
[out]	nServoCurPos	Current servo position (odometer ticks).
[out]	nServoCurSpeed	Current servo velocity (raw unitless).

Definition at line 266 of file hekKin.cxx.

References m_kinChain.

{
  KinChain::iterator  pos;

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    pos->second->getServoCurPosSpeed(nServoCurPos, nServoCurSpeed);
  }
}

bool HekKinematics::hasOverTorqueCondition ( const std::string &  strJointName )

virtual

Test if joint is in an over torque condition.

Parameters

strJointName

Joint name.

Returns

Returns true if in condition, false otherwise.

Definition at line 322 of file hekKin.cxx.

References m_kinChain.

{
  KinChain::iterator  pos;

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    return pos->second->hasOverTorqueCondition();
  }
  else // non-existent joints have no torque issues
  {
    return false;
  }
}

bool HekKinematics::isStopped ( const std::string &  strJointName )

virtual

Test if joint is moving.

Parameters

strJointName

Joint name.

Returns

Returns true if stopped, false otherwise.

Definition at line 308 of file hekKin.cxx.

References m_kinChain.

{
  KinChain::iterator  pos;

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    return pos->second->isStopped();
  }
  else // non-existent joints don't move
  {
    return true;
  }
}

int HekKinematics::jointPosToServoPos	(	const std::string &	strJointName,
		const double	fPos
	)

Convert joint position to the equivalent servo position.

Parameters

strJointName	Joint name.
fPos	Joint position (radians).

Returns

Equivalent servo position (odometer ticks).

Definition at line 278 of file hekKin.cxx.

References m_kinChain.

{
  KinChain::iterator  pos;

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    return pos->second->jointPosToServoPos(fPos);
  }
  else
  {
    return 0;
  }
}

void hekateros::HekKinematics::lock ( )

inlineprotected

Lock the kinematics thread.

The calling thread will block while waiting for the mutex to become available. Once locked, the kinematics thread will block.

The lock()/unlock() primitives provide a safe mechanism to modify the registered vServo data.

Context:

Any.

Definition at line 483 of file hekKin.h.

Referenced by createThread(), estop(), exec(), monitorHealth(), move(), readyWait(), release(), resetServoOdometersForAllJoints(), terminateThread(), timedWait(), and waitOneCycle().

    {
      pthread_mutex_lock(&m_mutexSync);
    }

void HekKinematics::monitorHealth ( )

protectedvirtual

Monitor servo health thread task.

Context:

Kinematics thread.

Definition at line 633 of file hekKin.cxx.

References act(), exec(), lock(), m_condSync, m_dynaChain, m_iterHealth, m_nHealthServoId, plan(), sense_react(), and unlock().

Referenced by exec(), and unlock().

{
  DynaServo  *pServo;
  uint_t      uAlarms;
  int         nCurLoad;
  uint_t      uCurVolt;
  uint_t      uCurTemp;

  pServo = m_dynaChain.GetServo(m_nHealthServoId);

  if( (m_nHealthServoId = m_dynaChain.IterNext(&m_iterHealth)) == DYNA_ID_NONE )
  {
    m_nHealthServoId = m_dynaChain.IterStart(&m_iterHealth);
  }
  
  LOGDIAG4("Kinematics Thread: Monitoring health for servo %d.",
      pServo->GetServoId());

  pServo->ReadHealth(&uAlarms, &nCurLoad, &uCurVolt, &uCurTemp);
}

int HekKinematics::move ( HekJointTrajectoryPoint &  trajectoryPoint )

virtual

Move kinematic chain through a trajectory point.

Context:

Calling thread.

Parameters

trajectoryPoint

Trajectory end point.

Returns

Number of joints with new move initiated.

Definition at line 500 of file hekKin.cxx.

References hekateros::HekJointTrajectoryPoint::getNumPoints(), lock(), m_bIsControlling, m_kinChain, and unlock().

{
  int                 i;                  // trajectory point iterator
  int                 nNumPts;            // number of actual points
  string              strJointName;       // joint name
  double              fJointGoalPos;      // goal joint position (radians)
  double              fJointGoalVel;      // goal joint velocity (rads/sec)
  double              fJointGoalAcc;      // goal joint acceleration (N/A)
  KinChain::iterator  pos;                // kinematics chain position

  lock();

  for(i=0, nNumPts=0; i<trajectoryPoint.getNumPoints(); ++i)
  {
    //
    // Joint point data: joint name, goal position, velocity, and acceleration.
    //
    trajectoryPoint[i].get(strJointName, fJointGoalPos,
                          fJointGoalVel, fJointGoalAcc);

    if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
    {
      pos->second->specifyMove(fJointGoalPos, fJointGoalVel);
      ++nNumPts;
    }
    else
    {
      LOGWARN("Joint %s: Not found in kinematic chain.", strJointName.c_str());
      continue;
    }
  }

  if( nNumPts > 0 )
  {
    m_bIsControlling = true;
  }

  unlock();

  return nNumPts;
}

int HekKinematics::move ( const std::string &  strJointName, const double  fJointGoalPos, const double  fJointGoalVel  )

virtual

Move single joint.

Context:

Calling thread.

Parameters

strJointName	Joint name.
fJointGoalPos	Joint goal position (radians).
fJointGoalVel	Joint goal velocity (radians/second).

Definition at line 542 of file hekKin.cxx.

References hekateros::HEK_ECODE_BAD_VAL, lock(), m_bIsControlling, m_kinChain, and unlock().

{
  KinChain::iterator  pos;    // kinematics chain position
  int                 rc;     // return code

  lock();

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    rc = pos->second->specifyMove(fJointGoalPos, fJointGoalVel);
    m_bIsControlling = true;
  }
  else
  {
    LOGWARN("Joint %s: Not found in kinematic chain.", strJointName.c_str());
    rc = -HEK_ECODE_BAD_VAL;
  }

  unlock();

  return rc;
}

void HekKinematics::plan ( )

protectedvirtual

Plan motions for all joints.

Context:

Kinematics thread.

Definition at line 598 of file hekKin.cxx.

References m_bIsControlling, m_kinChain, and m_taskList.

Referenced by monitorHealth(), and unlock().

{
  TaskList::iterator  iter;

  for(iter = m_taskList.begin(); iter != m_taskList.end(); ++iter)
  {
    m_kinChain[*iter]->planMotion(m_bIsControlling, m_msgs);
  }
}

void HekKinematics::react ( )

protectedvirtual

React to any necessary time-critical joint events.

Context:

Kinematics thread.

Definition at line 577 of file hekKin.cxx.

References m_bIsControlling, m_kinChain, and m_taskList.

Referenced by unlock().

{
  TaskList::iterator  iter;

  for(iter = m_taskList.begin(); iter != m_taskList.end(); ++iter)
  {
    m_kinChain[*iter]->react(m_bIsControlling, m_msgs);
  }
}

void HekKinematics::readyWait ( )

protected

Wait indefinitely while in the ready state.

Context:

Calling thread or kinematics thread.

Definition at line 941 of file hekKin.cxx.

References lock(), m_condSync, m_eState, m_mutexSync, ThreadStateReady, and unlock().

Referenced by changeState(), and thread().

{
  lock();

  // signal calling thread that created kinematics thread.
  pthread_cond_signal(&m_condSync);

  while( m_eState == ThreadStateReady )
  {
    pthread_cond_wait(&m_condSync, &m_mutexSync);
  }

  unlock();
}

void HekKinematics::release ( )

virtual

Release kinematics chain.

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

All active joint motion control will cease.

Context:

Calling thread.

Definition at line 382 of file hekKin.cxx.

References lock(), m_bIsControlling, m_dynaChain, stop(), unlock(), and waitForAllStop().

{
  if( m_bIsControlling )
  {
    stop();
    waitForAllStop(5.0);
  }

  lock();

  m_bIsControlling = false;

  m_dynaChain.Release();

  unlock();
}

void HekKinematics::reload ( const HekTunes &  tunes )

virtual

Reload configuration tuning parameters for all joints.

Parameters

tunes

Hekateros tuning parameters.

Definition at line 208 of file hekKin.cxx.

References hekateros::HekTunes::m_fKinematicsHz, m_kinChain, and setHz().

{
  KinChain::iterator  iter;   // kinematics chain iterator

  for(iter = m_kinChain.begin(); iter != m_kinChain.end(); ++iter)
  {
    iter->second->reload(tunes);
  }

  setHz(tunes.m_fKinematicsHz);
}

int HekKinematics::resetServoOdometer ( const std::string &  strJointName )

virtual

Reset joint's master servo odometer to the current encoder position.

Context:

Calling thread.

Parameters

strJointName

Name of joint with master servo to reset.

Returns

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

Definition at line 336 of file hekKin.cxx.

References hekateros::HEK_ECODE_BAD_VAL, m_kinChain, and waitOneCycle().

{
  KinChain::iterator  pos;    // kinematics chain position
  int                 rc;     // return code

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    rc = pos->second->resetServoOdometer();
  }
  else
  {
    LOGWARN("Joint %s: Not found in kinematic chain.", strJointName.c_str());
    rc = -HEK_ECODE_BAD_VAL;
  }

  waitOneCycle();

  return rc;
}

int HekKinematics::resetServoOdometersForAllJoints ( )

virtual

Reset all joints' master servos odometers to the current respective encoder positions.

Context:

Calling thread.

Returns

Number of joint servos reset.

Definition at line 220 of file hekKin.cxx.

References hekateros::HEK_OK, lock(), m_kinChain, unlock(), and waitOneCycle().

{
  KinChain::iterator  iter;   // kinematics chain iterator
  int                 n;      // number of actual joints reset

  lock();

  for(iter = m_kinChain.begin(), n = 0; iter != m_kinChain.end(); ++iter)
  {
    if( iter->second->resetServoOdometer() == HEK_OK )
    {
      ++n;
    }
  }

  unlock();

  waitOneCycle();

  return n;
}

int HekKinematics::runThread

(

const double

fHz = HekTuneKinHzDft

)

Run the kinematics thread.

Valid Current State:

ThreadStateReady

New State:

ThreadStateRunning

Context:

Calling thread.

Parameters

fHZ	Thread run rate (Hertz).

Definition at line 871 of file hekKin.cxx.

References changeState(), hekateros::HEK_ECODE_GEN, hekateros::HEK_OK, m_eState, setHz(), ThreadStateExit, ThreadStateReady, and ThreadStateRunning.

{
  int       rc;       // return code

  setHz(fHz);

  switch( m_eState )
  {
    case ThreadStateReady:
      changeState(ThreadStateRunning);
      rc = HEK_OK;
      break;

    case ThreadStateRunning:
    case ThreadStateExit:
    default:
      rc = -HEK_ECODE_GEN;
      LOGERROR("Kinematics thread in invalid state %d to run.", m_eState);
      break;
  } 

  return rc;
}

void HekKinematics::schedWait ( )

protected

Block kinematics thread until the next subcycle task is to be run.

Context:

Kinematics thread.

Definition at line 965 of file hekKin.cxx.

References m_iterTask, m_tsExecPeriod, m_tsSched, and timedWait().

Referenced by changeState(), and thread().

{
  static bool     bLogWarnings = false; // true to debug scheduling overloading
  struct timespec tsNow;                // now
  struct timespec tsSlip;               // any slippage

  // now
  clock_gettime(CLOCK_REALTIME, &tsNow);

  if( m_tsSched < tsNow )
  { 
    tsSlip = tsNow - m_tsSched;
  }
  else
  {
    tsSlip.tv_sec = tsSlip.tv_nsec = 0;
  }

  // block wait for next execute cycle
  if( tsNow < m_tsSched )
  {
    timedWait(m_tsSched);
    clock_gettime(CLOCK_REALTIME, &tsNow);
    m_tsSched = tsNow + m_tsExecPeriod;
  }

  // slipped by less than two execution cycles - try to make up the time
  else if( tsSlip < m_tsExecPeriod )
  {
    m_tsSched = tsNow + m_tsExecPeriod - tsSlip;
  }

  // slipped a bunch
  else
  {
    m_tsSched = tsNow + m_tsExecPeriod;

    // debug logging
    if( bLogWarnings )
    {
#ifdef HEK_KIN_EXEC_ALG_SYNC
      LOGWARN("Kinematics thread: "
          "Execution slipped by %ld.%09ld seconds.",
          tsSlip.tv_sec, tsSlip.tv_nsec);
#else // HEK_KIN_EXEC_ALG_INDIV
      LOGWARN("Kinematics thread: "
          "Scheduled task %s slipped by %ld.%09ld seconds.",
          m_iterTask->c_str(), tsSlip.tv_sec, tsSlip.tv_nsec);
#endif // HEK_KIN_EXEC_ALG_SYNC
    }
  }
}

void HekKinematics::sense ( )

protectedvirtual

Sense the state of all joints.

Context:

Kinematics thread.

Definition at line 567 of file hekKin.cxx.

References m_bIsControlling, m_kinChain, and m_taskList.

Referenced by unlock().

{
  TaskList::iterator  iter;

  for(iter = m_taskList.begin(); iter != m_taskList.end(); ++iter)
  {
    m_kinChain[*iter]->senseDynamics(m_bIsControlling);
  }
}

void HekKinematics::sense_react ( )

protectedvirtual

Sense-react the state of all joints.

Context:

Kinematics thread.

Definition at line 587 of file hekKin.cxx.

References m_bIsControlling, m_kinChain, and m_taskList.

Referenced by monitorHealth(), and unlock().

{
  TaskList::iterator  iter;

  for(iter = m_taskList.begin(); iter != m_taskList.end(); ++iter)
  {
    m_kinChain[*iter]->senseDynamics(m_bIsControlling);
    m_kinChain[*iter]->react(m_bIsControlling, m_msgs);
  }
}

double HekKinematics::servoPosToJointPos	(	const std::string &	strJointName,
		const int	nOdPos
	)

Convert servo position to the equivalent joint position.

Parameters

strJointName	Joint name.
nOdPos	Servo position (odometer ticks).

Returns

Equivalent joint position (radians).

Definition at line 293 of file hekKin.cxx.

References m_kinChain.

{
  KinChain::iterator  pos;

  if( (pos = m_kinChain.find(strJointName)) != m_kinChain.end() )
  {
    return pos->second->servoPosToJointPos(nOdPos);
  }
  else
  {
    return 0.0;
  }
}

void HekKinematics::setHz ( const double  fHz )

virtual

Set thread run rate and ancillary data.

Parameters

fHZ	Thread run rate (Hertz).

Definition at line 850 of file hekKin.cxx.

References hekateros::fcap(), hekateros::HekTuneKinHzMin, m_fHz, m_fTExec, m_taskList, and m_tsExecPeriod.

Referenced by reload(), and runThread().

{
  m_fHz = fHz;

  if( m_fHz < HekTuneKinHzMin )
  {
    m_fHz = HekTuneKinHzMin;
  }

#ifdef HEK_KIN_EXEC_ALG_SYNC
  m_fTExec = 1.0 / m_fHz;
#else // !HEK_KIN_EXEC_ALG_INDIV
  m_fTExec = 1.0 / (m_fHz * (double)m_taskList.size());
#endif // HEK_KIN_EXEC_ALG_SYNC

  m_tsExecPeriod.tv_sec  = (long)floor(m_fTExec); 
  m_tsExecPeriod.tv_nsec = (long)fcap(
                                (m_fTExec-floor(m_fTExec)) * (double)BILLION,
                                0.0, (double)(BILLION-1) );
}

int HekKinematics::stop ( )

virtual

Stop kinematics chain at the current position.

The joint servos are still being driven. However, all active joint motion control will cease.

Context:

Calling thread.

Returns

Number of joints stopped.

Definition at line 399 of file hekKin.cxx.

References hekateros::HEK_ECODE_BAD_OP, hekateros::HEK_ECODE_BAD_VAL, hekateros::HEK_OK, m_bIsControlling, and m_kinChain.

Referenced by estop(), freeze(), and release().

{
  KinChain::iterator  iter;

  if( !m_bIsControlling )
  {
    return -HEK_ECODE_BAD_OP;
  }

  for(iter=m_kinChain.begin(); iter!=m_kinChain.end(); ++iter)
  {
    iter->second->stop();
  }

  return HEK_OK;
}

virtual int hekateros::HekKinematics::stop ( const std::vector< std::string > &  vecJointNames )

virtual

Stop the set of joints at the current position.

The joint servos are still being driven. However, all active joint motion control will cease.

Context:

Calling thread.

Parameters

vecJointName

Vector list of joint names to stop.

Returns

Number of joints stopped.

virtual int hekateros::HekKinematics::stop ( const std::string &  strJointName )

virtual

Stop one joint at the current position.

The joint servos are still being driven. However, all active joint motion control will cease.

Context:

Calling thread.

Parameters

strJointName

Name of joint.

Returns

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

void HekKinematics::terminateThread ( )

protected

Terminate the kinematics thread.

This function does not return until the thread actually terminates.

Context:

Calling thread.

Definition at line 895 of file hekKin.cxx.

References changeState(), lock(), m_condSync, m_mutexSync, m_thread, ThreadStateExit, unlock(), and waitOneCycle().

Referenced by unlock(), and ~HekKinematics().

{
  changeState(ThreadStateExit);
  pthread_join(m_thread, NULL);
}

void * HekKinematics::thread ( void *  pArg )

staticprotected

The kinematics thread.

Parameters

pArg	Thread argument (point to HekKinematics object).

Returns

Returns NULL on thread exit.

Definition at line 736 of file hekKin.cxx.

References exec(), m_eState, m_fHz, m_fTExec, m_tsSched, readyWait(), schedWait(), ThreadStateExit, ThreadStateReady, and ThreadStateRunning.

Referenced by createThread(), and unlock().

{
  HekKinematics *pThis = (HekKinematics *)pArg;
  int           oldstate;
  int           rc;

  LOGDIAG3("Kinematics thread created - ready to run.");

  pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, &oldstate);
  pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldstate);

  pThis->m_eState = ThreadStateReady;

  //
  // Loop forever until exit
  //
  while( (pThis->m_eState != ThreadStateExit) )
  {
    switch( pThis->m_eState )
    {
      //
      // Blocked on Ready "queue"
      //
      case ThreadStateReady:
        pThis->readyWait();

        // ready -> running 
        if( pThis->m_eState == ThreadStateRunning )
        {
#ifdef HEK_KIN_EXEC_ALG_SYNC
          LOGDIAG3("Kinematics thread started - running at %.1lf Hz.",
                pThis->m_fHz);
#else // HEK_KIN_EXEC_ALG_INDIV
          LOGDIAG3("Kinematics thread started - "
                   "running at %.1lf Hz, %lf seconds/task.",
                pThis->m_fHz, pThis->m_fTExec);
#endif // HEK_KIN_EXEC_ALG_SYNC
 
          // force immediately execution of first task
          clock_gettime(CLOCK_REALTIME, &pThis->m_tsSched);
        }
        break;

      //
      // Run wait,execute subcycle
      //
      case ThreadStateRunning:
        pThis->schedWait();
        if( pThis->m_eState == ThreadStateRunning )
        {
          pThis->exec();
        }
        break;

      //
      // Exit
      //
      case ThreadStateExit:
        break;

      default:
        LOGERROR("%d: Unexpected dynamixel background thread state.",
            pThis->m_eState);
        pThis->m_eState = ThreadStateExit;
        break;
    }
  }

  LOGDIAG3("Kinematics thread exited.");

  return NULL;
}

void HekKinematics::timedWait ( const struct timespec &  tsTimeout )

protected

Timed wait until state change or time out.

Parameters

lMicroSec

Maximum wait duration (microseconds).

Context:

Calling thread or kinematics thread.

Definition at line 956 of file hekKin.cxx.

References lock(), m_condSync, m_mutexSync, and unlock().

Referenced by changeState(), and schedWait().

{
  lock();

  pthread_cond_timedwait(&m_condSync, &m_mutexSync, &tsTimeout);

  unlock();
}

void hekateros::HekKinematics::unlock ( )

inlineprotected

Unlock the kinematics thread.

The kinematics thread will be available to run.

Context:

Any.

Definition at line 497 of file hekKin.h.

References act(), createThread(), exec(), monitorHealth(), plan(), react(), sense(), sense_react(), terminateThread(), and thread().

Referenced by createThread(), estop(), exec(), monitorHealth(), move(), readyWait(), release(), resetServoOdometersForAllJoints(), terminateThread(), timedWait(), and waitOneCycle().

    {
      pthread_mutex_unlock(&m_mutexSync);
    }

int HekKinematics::waitForAllStop ( double  fSeconds )

virtual

Wait for all joints to stop.

Context:

Calling thread.

Parameters

fSeconds

Maximum number of seconds to wait.

Returns

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

Definition at line 464 of file hekKin.cxx.

References hekateros::HEK_ECODE_TIMEDOUT, hekateros::HEK_OK, and m_kinChain.

Referenced by estop(), freeze(), and release().

{
  static const useconds_t WaitDt  = 100;  // usec wait between tests

  KinChain::iterator  iter;

  int nMaxTries = (int)ceil(fSeconds * MILLION / (double)WaitDt);
  int nMoveCnt;

  for(int i = 0; i < nMaxTries; ++i)
  {
    nMoveCnt = 0;

    for(iter = m_kinChain.begin(); iter != m_kinChain.end(); ++iter)
    {
      if( !iter->second->isStopped() )
      {
        ++nMoveCnt;
        break;
      }
    }

    if( nMoveCnt == 0 )
    {
      return HEK_OK;
    }

    usleep(WaitDt);
  }

  LOGERROR("Timed out waiting for %.2lf seconds for all joints to stop.",
      fSeconds);

  return -HEK_ECODE_TIMEDOUT;
}

void HekKinematics::waitOneCycle

(

)

Wait one full cycle.

Context:

Calling thread.

Definition at line 915 of file hekKin.cxx.

References lock(), m_condSync, m_iterTask, m_mutexSync, m_strTaskOneCycle, m_taskList, and unlock().

Referenced by resetServoOdometer(), resetServoOdometersForAllJoints(), and terminateThread().

{
  lock();

  if( m_iterTask == m_taskList.begin() )
  {
    m_strTaskOneCycle = m_taskList.back();
  }
  else
  {
    TaskList::iterator  prev = m_iterTask - 1;
    m_strTaskOneCycle = *prev;
  }

#ifdef DBG
    fprintf(stderr, "DBG: %s(): taskonecycle=%s\n", LOGFUNCNAME,
          strTaskOneCycle.c_str());
#endif // DBG

  pthread_cond_wait(&m_condSync, &m_mutexSync);

  unlock();
}

---

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

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