hekateros::HekKinJoint Class Reference

Hekateros powered joint dynamics and kinematics control class. More...

#include <hekKinJoint.h>

Inheritance diagram for hekateros::HekKinJoint:

Public Types
enum	MoveState {   MoveStateIdle,   MoveStateNewMove,   MoveStateMoving,   MoveStateStopping }
	Kinedynamics joint position and velocity control states. More...

Public Member Functions
	HekKinJoint ()
	Default constructor.
	HekKinJoint (HekRobotJoint *pJoint, DynaServo *pServo, const HekTunes &tunes)
	Initialization constructor. More...
virtual	~HekKinJoint ()
	Copy constructor. More...
virtual void	coupleJoint (HekKinJoint *)
	Couple joint. More...
HekKinJoint &	operator= (const HekKinJoint &rhs)
	Assignment operator. More...
virtual void	reload (const HekTunes &tunes)
	Reload configuration tuning parameters. More...
MoveState	getMoveState ()
	Get move control state for this joint. More...
bool	isMovingToGoal ()
	Test if actively moving joint to goal. More...
bool	isStopping ()
	Test if actively stopping joint. More...
bool	isMoving ()
	Test if actively moving joint. More...
virtual bool	isStopped ()
	Test if joint is stopped (i.e. control is not active). More...
bool	canStopNow ()
	Test if can stop now without slowing down. More...
void	getPidKParams (double &fKp, double &fKi, double &fKd) const
	Get the joint's position and velocity PID K parameters. More...
void	setPidKParams (const double fKp, const double fKi, const double fKd)
	Set the joint's position and velocity PID K parameters. More...
double	getPidMaxDeltaVParam () const
	Get joint PID maximum delta v (radians/second) parameter. More...
void	setPidMaxDeltaVParam (const double fMaxDeltaV)
	Set joint PID maximum delta v (radians/second) parameter. More...
void	getToleranceParams (double &fTolPos, double &fTolVel) const
	Get the position and velocity tolerance parameters. More...
void	setToleranceParams (const double fTolPos, const double fTolVel)
	Set the position and velocity tolerance parameters. More...
void	getJointCurPosVel (double &fCurPos, double &fCurVel) const
	Get the instantaneous current joint position and velocity. More...
void	getFilteredJointCurPosVel (double &fCurPos, double &fCurVel) const
	Get the smoothed (filtered) current joint position and velocity. More...
void	getServoCurPosSpeed (int &nServoCurPos, int &nServoCurSpeed) const
	Get the instantaneous current servo position and speed. More...
void	getTgtVelSpeed (double &fTgtVel, int &nTgtSpeed) const
	Get the PID output target joint velocity and the corresponding servo speed estemate. More...
virtual bool	hasOverTorqueCondition ()
	Test if joint is in an over torque condition. More...
virtual int	jointPosToServoPos (const double fPos)
	Convert joint position to the equivalent servo position. More...
virtual double	servoPosToJointPos (const int nOdPos)
	Convert servo position to the equivalent joint position. More...
virtual int	resetServoOdometer ()
	Reset servo odometer zero point to the current encoder position. More...
int	specifyMove (const double fGoalPos, const double fGoalVel)
	Specify a new joint goal position and velocity move. More...
virtual int	senseDynamics (bool bIsControlling)
	Sense (read) the current servo dynamics and transform to useful state. More...
virtual int	react (bool bIsControlling, SyncMoveMsgs &msgs)
	React to recently read sensor data. More...
virtual MoveState	planMotion (bool bIsControlling, SyncMoveMsgs &msgs)
	Plan motion. More...
virtual int	act ()
	Act (write) to effect the servo dynamics for torque and motion control. More...
virtual int	applyTorqueControl ()
	Apply torque limit control. More...
virtual MoveState	move ()
	Move joint using joint PID controller. More...
virtual int	stop ()
	Stop movement and move control of the joint. More...

Static Public Attributes
static const size_t	POS_WIN_SIZE = 6
	Position sliding window size for low-pass band filtering of input positions.
static const size_t	DT_WIN_SIZE = 10
	Delta t sliding window size for low-pass band filtering of input delta times.
static const size_t	VEL_WIN_SIZE = 6
	Velocity sliding window size for low-pass band filtering of input Velocities.
static const double	SLOW_DERATE_DELTA_V = 1.0
	Stop delta v as a fraction of max_delta_v tune parameter.
static const size_t	TORQUE_WIN_SIZE = 4
	Torque sliding window size for low-pass band filtering of input torques.
static const int	TORQUE_CTL_BACKOFF_SPEED = 100
	Torque control backoff speed (raw unitless)
static const int	TORQUE_CTL_BACKOFF_POS = 5
	Torque control backoff positon (odometer ticks)

Protected Member Functions
void	lock ()
	Lock the joint kinematics. More...
void	unlock ()
	Unlock the joint kinematics. More...
double	delta_t ()
	Calculate the delta time between calls to this. More...
double	averageDt (double fDtAvg, double fDt)
	Apply a running average for the delta times (seconds). More...
int	dejitterServoPos (int nServoCurPos, int nServoPrevPos)
	De-jitter servo odometer position. More...
double	filterPositions (double fPosAvg, double fJointPos)
	Apply a low-pass band filter on the joint positions (radians). More...
double	filterVelocities (double fVelAvg, double fJointVel)
	Apply a low-pass band filter on the joint velocities (radians/second). More...
double	filterTorques (double fTorqueAvg, int nServoLoad)
	Apply a low-pass band filter on the sensed torques (loads). More...
int	getGoalDir ()
	Get the current goal rotation direction. More...
int	getPlannedDir (int nServoTgtSpeed)
	Get the planned target rotation direction. More...
bool	canMoveInGoalDir ()
	Test if joint can move in direction of goal position. More...
bool	canMoveInPlannedDir (int nServoTgtSpeed)
	Test if joint can move in planned direction. More...
virtual double	jointVelocity (double fPos1, double fPos0, double fDt)
	Calculate the joint velocity from delta servo positions. More...
virtual void	updateAssoc (double fJointVel, int nServoSpeed)
	Update joint velocity - servo speed association. More...
virtual int	estimateServoTgtSpeed (double fJointTgtVel)
	Estimate of target servo speed from target joint velocity. More...
virtual int	estimateServoTgtSpeedSimple (double fJointTgtVel)
	Simple estimate of target servo speed from target joint velocity. More...
void	moveServo (int nServoGoalSpeed, int nServoGoalPos)
	Move servo. More...
void	addServoMsgEntries (SyncMoveMsgs &msg, int nServoGoalSpeed, int nServoGoalPos)
	Add servo to synchronous move message(s). More...
virtual int	nlspecifyMove (const double fGoalPos, const double fGoalVel)
	Specify a new joint goal position and velocity move (no lock version). More...
virtual int	nlapplyTorqueControl ()
	Apply torque limit control (no lock version). More...
virtual MoveState	nlmove ()
	Move joint using joint PID controller (no lock version). More...
virtual int	nlstop ()
	Stop movement and move control of the joint (no lock version). More...
virtual int	nlslowToStop (SyncMoveMsgs &msg)
	Slow to stop (no lock version). More...

Protected Attributes
MoveState	m_eState
	joint control state
pthread_mutex_t	m_mutexSync
	synchonization mutex
HekRobotJoint *	m_pJoint
	joint description
DynaServo *	m_pServo
	joint master servo control
double	m_fTolPos
	joint position ± tolerance (rads)
double	m_fTolVel
	joint velocity ± tolerance (rads/s)
double	m_fStopVelTh
	joint velocity threshold to safely stop move
HekPid	m_pid
	joint position and velocity PID
double	m_fJointRadPerTick
	joint radians / odometer tick
double	m_fJointGoalPos
	joint goal position (radians)
int	m_nServoGoalPos
	servo goal position (odometer ticks)
double	m_fJointCurPos
	joint current position (radians)
int	m_nServoCurPos
	servo current position (odometer ticks)
double	m_fJointPrevPos
	joint current position (radians)
int	m_nServoPrevPos
	servo previous pos (odometer ticks)
double	m_fJointPosOut
	low-pass filtered joint cur position
std::deque< double >	m_histPosIn
	recent history of joint positions
double	m_fDt
	delta time between positions reads
struct timespec	m_tsPrev
	previous time mark
double	m_fDtAvg
	average delta t
std::deque< double >	m_histDtIn
	sliding window of delta t's
double	m_fJointGoalVel
	joint goal velocity (radians/second)
double	m_fJointCurVel
	joint current velocity (radians/second)
int	m_nServoCurSpeed
	current motor speed (raw unitless)
double	m_fVelDerate
	joint velocity derate (normalized)
VelSpeedLookupTbl	m_tblVelSpeed
	dynamic velocity/speed lookup table
double	m_fJointVelOut
	low-pass filtered joint cur velocity
std::deque< double >	m_histVelIn
	sliding window of current velocities
double	m_fJointTgtVel
	joint target velocity (radians/second)
int	m_nServoTgtSpeed
	target motor speed (raw unitless)
bool	m_bOverTorqueCond
	is [not] in torque overload condition
double	m_fOverTorqueTh
	set over torque condition threshold
double	m_fClearTorqueTh
	clear over torque condition threshold
int	m_nServoCurLoad
	servo current load (raw unitless)
double	m_fTorqueOut
	low-pass filtered torque
std::deque< double >	m_histTorqueIn
	recent history of joint torques

Detailed Description

Hekateros powered joint dynamics and kinematics control class.

One or more motors can power a joint. Two or more joints may be coupled (e.g. wrist pitch and rotation). Motors can be actuators with built in controllers, DC motors, synthetic muscle, or any other physical entity that can provide usefull force and be controlled.

Currently, only Dynamixels actuators (servos) are supported.

Definition at line 387 of file hekKinJoint.h.

Member Enumeration Documentation

enum hekateros::HekKinJoint::MoveState

Kinedynamics joint position and velocity control states.

Note

Actively apply torque control in an over torque condition is somewhat independent of the control states.

Enumerator
MoveStateIdle	no active goal or stopping control
MoveStateNewMove	start move to new goal
MoveStateMoving	actively moving to goal
MoveStateStopping	actively stopping joint

Definition at line 435 of file hekKinJoint.h.

    {
      MoveStateIdle,      ///< no active goal or stopping control 
      MoveStateNewMove,   ///< start move to new goal
      MoveStateMoving,    ///< actively moving to goal
      MoveStateStopping,  ///< actively stopping joint
    };

Constructor & Destructor Documentation

HekKinJoint::HekKinJoint	(	HekRobotJoint *	pJoint,
		DynaServo *	pServo,
		const HekTunes &	tunes
	)

Initialization constructor.

Parameters

pJoint	Pointer to joint description.
pServo	Pointer to master servo controller.
tunes	Hekateros tuning parameters.

Definition at line 441 of file hekKinJoint.cxx.

References hekateros::VelSpeedLookupTbl::create(), hekateros::degToRad(), hekateros::HekTunes::getPidKParams(), hekateros::HekTunes::getPidMaxDeltaV(), hekateros::HekTunes::getToleranceParams(), hekateros::HekTunes::getTorqueParams(), hekateros::HekTunes::getVelocityDerate(), m_bOverTorqueCond, m_eState, m_fClearTorqueTh, m_fDt, m_fDtAvg, m_fJointCurPos, m_fJointCurVel, m_fJointGoalPos, m_fJointGoalVel, m_fJointPosOut, m_fJointPrevPos, m_fJointRadPerTick, m_fJointTgtVel, m_fJointVelOut, hekateros::HekRobotJoint::m_fMaxJointRadsPerSec, m_fOverTorqueTh, m_fStopVelTh, hekateros::HekRobotJoint::m_fTicksPerJointRad, m_fTolPos, m_fTolVel, m_fTorqueOut, m_fVelDerate, m_mutexSync, m_nServoCurPos, m_nServoCurSpeed, m_nServoGoalPos, m_nServoPrevPos, m_nServoTgtSpeed, m_pJoint, hekateros::HekRobotJoint::m_strName, m_tblVelSpeed, m_tsPrev, MoveStateIdle, setPidKParams(), setPidMaxDeltaVParam(), and SLOW_DERATE_DELTA_V.

                                                :
    m_pJoint(pJoint), m_pServo(pServo),
    m_histPosIn(POS_WIN_SIZE, 0.0),
    m_histDtIn(DT_WIN_SIZE, 0.0),
    m_histVelIn(VEL_WIN_SIZE, 0.0),
    m_histTorqueIn(TORQUE_WIN_SIZE, 0.0)
{
  double    fKp, fKi, fKd;
  double    fMaxDeltaV;
  double    fRawMaxTorque;

  m_eState    = MoveStateIdle;

  pthread_mutex_init(&m_mutexSync, NULL);

  // joint tolerance tuning parameters (normalized)
  tunes.getToleranceParams(pJoint->m_strName, m_fTolPos, m_fTolVel);

  // joint position and velocity PID tuning parameters
  tunes.getPidKParams(pJoint->m_strName, fKp, fKi, fKd);

  // pid constants
  setPidKParams(fKp, fKi, fKd);

  // joint pid maximum delta velocity tuning paramter
  fMaxDeltaV = tunes.getPidMaxDeltaV(m_pJoint->m_strName);

  // maximum PID output delta v (radians/second)
  setPidMaxDeltaVParam(fMaxDeltaV);

  // stop velocity maximum velocity
  m_fStopVelTh = SLOW_DERATE_DELTA_V * fMaxDeltaV;

  // position state
  m_fJointRadPerTick  = 1.0 / m_pJoint->m_fTicksPerJointRad;
  m_fJointGoalPos     = 0.0;
  m_nServoGoalPos     = 0;
  m_fJointCurPos      = 0.0;
  m_nServoCurPos      = 0;
  m_fJointPrevPos     = 0.0;
  m_nServoPrevPos     = 0;
  m_fJointPosOut      = 0.0;

  // clock
  m_fDt     = 0.0;
  clock_gettime(CLOCK_REALTIME, &m_tsPrev);
  m_fDtAvg  = 0.0;

  // velocity state
  m_fJointGoalVel   = 0.0;
  m_fJointCurVel    = 0.0;
  m_nServoCurSpeed  = 0;
  m_fVelDerate      = tunes.getVelocityDerate();  // tuning parameter (norm)
  m_fJointVelOut    = 0.0;

  m_fJointTgtVel    = 0.0;
  m_nServoTgtSpeed  = 0;

  // To debug any joint(s).
  //if( m_pJoint->m_strName == "base_rot")
  //{
  //  m_tblVelSpeed.enableDebugging(m_pJoint->m_strName);
  //}

  m_tblVelSpeed.create(m_pJoint->m_fMaxJointRadsPerSec, degToRad(5.0));

  // normalized joint torque tuning parameters
  tunes.getTorqueParams(pJoint->m_strName, m_fOverTorqueTh, m_fClearTorqueTh);

  // convert to raw theshold values 
  m_bOverTorqueCond = false;
  fRawMaxTorque     = (double)pServo->GetSpecification().m_uRawTorqueMax;
  m_fOverTorqueTh  *= fRawMaxTorque;
  m_fClearTorqueTh *= fRawMaxTorque;
  m_fTorqueOut      = 0.0;
}

virtual hekateros::HekKinJoint::~HekKinJoint ( )

inlinevirtual

Copy constructor.

Parameters

src	Source object.

Destructor.

Definition at line 469 of file hekKinJoint.h.

    {
      nlstop();
      pthread_mutex_destroy(&m_mutexSync);
    }

Member Function Documentation

int HekKinJoint::act ( )

virtual

Act (write) to effect the servo dynamics for torque and motion control.

Returns

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

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 938 of file hekKinJoint.cxx.

References canMoveInGoalDir(), hekateros::HEK_OK, isMovingToGoal(), lock(), m_bOverTorqueCond, nlapplyTorqueControl(), nlmove(), and unlock().

Referenced by hekateros::HekKinematics::exec().

{
  lock();

  //
  // Actively moving joint and there is no over torque condition or the goal
  // direction will naturally ease the torque.
  //
  if( isMovingToGoal() && canMoveInGoalDir() )
  {
    nlmove();
  }

  //
  // In over torque condition, apply torque control.
  //
  else if( m_bOverTorqueCond )
  {
    nlapplyTorqueControl();
  }

  unlock();

  return HEK_OK;
}

void HekKinJoint::addServoMsgEntries ( SyncMoveMsgs &  msg, int  nServoGoalSpeed, int  nServoGoalPos  )

protected

Add servo to synchronous move message(s).

Parameters

msgs	Synchronous move messages container object.
nServoGoalSpeed	Servo goal speed (raw unitless).
nServoGoalPos	Servo goal position (odometer ticks).

Definition at line 1118 of file hekKinJoint.cxx.

References hekateros::SyncMoveMsgs::addOdPos(), hekateros::SyncMoveMsgs::addSpeed(), hekateros::cap(), hekateros::iabs(), m_eState, m_nServoTgtSpeed, m_pServo, MoveStateMoving, and MoveStateNewMove.

Referenced by nlslowToStop(), hekateros::HekKinJointWristRot::nlslowToStop(), planMotion(), and hekateros::HekKinJointWristRot::planMotion().

{
  // 
  // See discussion in moveServo() for Dynamixel nuances.
  //
  if( m_pServo->GetServoMode() == DYNA_MODE_CONTINUOUS )
  {
    nServoTgtSpeed = cap(nServoTgtSpeed, -DYNA_SPEED_MAX_RAW,
                                          DYNA_SPEED_MAX_RAW);
    msgs.addSpeed(m_pServo->GetServoId(), nServoTgtSpeed);
  }
  else
  {
    m_nServoTgtSpeed = cap(iabs(nServoTgtSpeed), DYNA_SPEED_MIN_CTL,
                                                 DYNA_SPEED_MAX_RAW);
    msgs.addSpeed(m_pServo->GetServoId(), nServoTgtSpeed);
    if( m_eState == MoveStateNewMove )
    {
      msgs.addOdPos(m_pServo->GetServoId(), nServoGoalPos);
      m_eState = MoveStateMoving;
    }
  }
}

int HekKinJoint::applyTorqueControl ( )

virtual

Apply torque limit control.

Returns

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

Definition at line 964 of file hekKinJoint.cxx.

References hekateros::HEK_OK, lock(), m_pServo, nlapplyTorqueControl(), and unlock().

{
  int   rc;

  if( m_pServo == NULL )
  {
    return HEK_OK;
  }

  lock();

  rc = nlapplyTorqueControl();

  unlock();

  return rc;
}

double HekKinJoint::averageDt ( double  fDtAvg, double  fDt  )

protected

Apply a running average for the delta times (seconds).

A simple moving average (SMa) of delta times with a window size of DT_WIN_SIZE is used.

Parameters

fDtAvg	Current delta time average (seconds).
fDt	New delta time (seconds).

Returns

New average delta time (seconds).

Definition at line 1305 of file hekKinJoint.cxx.

References DT_WIN_SIZE, and m_histDtIn.

{
  double  fDt0;
  double  fDtK;

  fDt0 = fDt / (double)DT_WIN_SIZE;
  fDtK = m_histDtIn.back();
  m_histDtIn.pop_back();
  m_histDtIn.push_front(fDt0);

  return fDtAvg - fDtK + fDt0;
}

bool HekKinJoint::canMoveInGoalDir ( )

protected

Test if joint can move in direction of goal position.

The joint must not be in an over torque condition or the move will ease the torque, not exacerbate the condition.

Returns

Returns true or false.

Definition at line 749 of file hekKinJoint.cxx.

References getGoalDir(), m_bOverTorqueCond, and m_nServoCurLoad.

Referenced by act(), and hekateros::HekKinJointWristRot::act().

{
  return !m_bOverTorqueCond || (getGoalDir() == DYNA_GET_DIR(m_nServoCurLoad));
}

bool HekKinJoint::canMoveInPlannedDir ( int  nServoTgtSpeed )

protected

Test if joint can move in planned direction.

The joint must not be in an over torque condition or the move will ease the torque, not worsen the condition.

Parameters

nServoTgtSpeed

Planned servo target speed (raw unitless).

Returns

Returns true or false.

Definition at line 754 of file hekKinJoint.cxx.

References getPlannedDir(), m_bOverTorqueCond, and m_nServoCurLoad.

Referenced by planMotion(), and hekateros::HekKinJointWristRot::planMotion().

{
  return !m_bOverTorqueCond ||
          (getPlannedDir(nServoTgtSpeed) == DYNA_GET_DIR(m_nServoCurLoad));
}

bool hekateros::HekKinJoint::canStopNow ( )

inline

Test if can stop now without slowing down.

Returns

Returns true or false.

Definition at line 561 of file hekKinJoint.h.

References hekateros::HekOpStateCalibrated, and hekateros::HekTunePidDeltaVNoMax.

Referenced by nlstop(), react(), and hekateros::HekKinJointWristRot::react().

    {
      return (m_pJoint->m_eOpState != HekOpStateCalibrated) ||
             (m_fStopVelTh == HekTunePidDeltaVNoMax) || 
             (fabs(m_fJointCurVel) <= m_fStopVelTh);
    }

virtual void hekateros::HekKinJoint::coupleJoint ( HekKinJoint *  )

inlinevirtual

Couple joint.

Parameters

pKinJointCoupled

Base class joint has no coupled joint.

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 480 of file hekKinJoint.h.

    {
      // N/A
    }

int HekKinJoint::dejitterServoPos ( int  nServoCurPos, int  nServoPrevPos  )

protected

De-jitter servo odometer position.

The servo can report encoder position of 1.

Parameters

nServoCurPos	Servo current odometer position.
nServoPrevPos	Servo previous odometer position.

Returns

Return de-jittered current servo position.

Definition at line 1318 of file hekKinJoint.cxx.

References hekateros::iabs().

Referenced by senseDynamics().

{
  // Same position over two sense cycles - accept new position.
  if( nServoCurPos == nServoPrevPos )
  {
    return nServoCurPos;
  }

  // Sufficiently large jump in position, probably not jitter.
  else if( hekateros::iabs(nServoCurPos-nServoPrevPos) > 1 )
  {
    return nServoCurPos;
  }

  // Small change is position, treat as sampling and/or random jitter.
  else
  {
    return nServoPrevPos;
  }
}

double HekKinJoint::delta_t ( )

protected

Calculate the delta time between calls to this.

Returns

Delta time secs.frac (seconds).

Definition at line 1288 of file hekKinJoint.cxx.

References hekateros::dt(), and m_tsPrev.

Referenced by senseDynamics().

{
  struct timespec tsNow;
  double          deltaTime;

  // now
  clock_gettime(CLOCK_REALTIME, &tsNow);

  // delta time in seconds
  deltaTime = hekateros::dt(tsNow, m_tsPrev);

  // new previous
  m_tsPrev = tsNow;

  return deltaTime;
}

int HekKinJoint::estimateServoTgtSpeed ( double  fJointTgtVel )

protectedvirtual

Estimate of target servo speed from target joint velocity.

The Dynamixel speed value is a raw, unitless (and probably non-linear) value. The estimate is made using linear interpolation from a dynamically maintained velocity,speed lookup table.

Parameters

fJointTgtVel

Target joint velocity (radians/seconds).

Returns

Servo speed estimate matching target velocity.

Definition at line 1264 of file hekKinJoint.cxx.

References hekateros::VelSpeedLookupTbl::estimate(), m_pServo, m_tblVelSpeed, and hekateros::signof().

Referenced by nlmove(), hekateros::HekKinJointWristRot::nlmove(), nlslowToStop(), hekateros::HekKinJointWristRot::nlslowToStop(), planMotion(), and hekateros::HekKinJointWristRot::planMotion().

{
  int nSign = (int)signof(fJointTgtVel);

  if( m_pServo->IsOdometerReversed() )
  {
    nSign = -nSign;
  }

  return nSign * m_tblVelSpeed.estimate(fJointTgtVel);
}

int HekKinJoint::estimateServoTgtSpeedSimple ( double  fJointTgtVel )

protectedvirtual

Simple estimate of target servo speed from target joint velocity.

The Dynamixel speed value is a raw, unitless (and probably non-linear) value. The estimate is made by interpolating the current speed between 0 and the maximum unloaded joint velocity.

Parameters

fJointTgtVel

Target joint velocity (radians/seconds).

Returns

Servo speed estimate matching target velocity.

Definition at line 1276 of file hekKinJoint.cxx.

References hekateros::cap(), getGoalDir(), hekateros::HekRobotJoint::m_fMaxJointRadsPerSec, and m_pJoint.

{
  double  r;
  int     nSpeedEst;

  r         = fJointTgtVel / m_pJoint->m_fMaxJointRadsPerSec;
  nSpeedEst = (int)(DYNA_SPEED_MAX_RAW * r);
  nSpeedEst = cap(nSpeedEst, 0, DYNA_SPEED_MAX_RAW) * getGoalDir();

  return nSpeedEst;
}

double HekKinJoint::filterPositions ( double  fPosAvg, double  fJointPos  )

protected

Apply a low-pass band filter on the joint positions (radians).

A simple moving average (SMA) of positions with a window size of POS_WIN_SIZE is used as the filter.

Parameters

fPosAvg	Current joint position average (radians).
fJointPos	New joint position (radians).

Returns

New filtered joint position (radians).

Definition at line 1339 of file hekKinJoint.cxx.

References m_histPosIn, and POS_WIN_SIZE.

Referenced by senseDynamics().

{
  double  fPos0;
  double  fPosK;

  fPos0 = fJointPos / (double)POS_WIN_SIZE;
  fPosK = m_histPosIn.back();
  m_histPosIn.pop_back();
  m_histPosIn.push_front(fPos0);

  return fPosAvg - fPosK + fPos0;
}

double HekKinJoint::filterTorques ( double  fTorqueAvg, int  nServoLoad  )

protected

Apply a low-pass band filter on the sensed torques (loads).

The torques are interpreted from the read servo loads which are unitless values (and probably non-linear).

A simple moving average (SMA) of load values with a window size of TORQUE_WIN_SIZE is used as the filter.

Parameters

fTorqueAvg	Current torque (load) average value.
nServoLoad	New servo load value (raw unitless).

Returns

New torque average (raw unitless).

Definition at line 1365 of file hekKinJoint.cxx.

References m_histTorqueIn, and TORQUE_WIN_SIZE.

Referenced by nlapplyTorqueControl(), and senseDynamics().

{
  double  fTorque0;
  double  fTorqueK;

  fTorque0 = (double)nServoLoad / (double)TORQUE_WIN_SIZE;
  fTorqueK = m_histTorqueIn.back();
  m_histTorqueIn.pop_back();
  m_histTorqueIn.push_front(fTorque0);

  return fTorqueAvg - fTorqueK + fTorque0;
}

double HekKinJoint::filterVelocities ( double  fVelAvg, double  fJointVel  )

protected

Apply a low-pass band filter on the joint velocities (radians/second).

A simple moving average (SMA) of velocities with a window size of VEL_WIN_SIZE is used as the filter.

Parameters

fVelAvg	Current joint velocitie average (radians/second).
fJointVel	New joint velocity (radians/second).

Returns

New filtered joint velocity (radians/second).

Definition at line 1352 of file hekKinJoint.cxx.

References m_histVelIn, and VEL_WIN_SIZE.

Referenced by senseDynamics().

{
  double  fVel0;
  double  fVelK;

  fVel0 = fJointVel / (double)VEL_WIN_SIZE;
  fVelK = m_histVelIn.back();
  m_histVelIn.pop_back();
  m_histVelIn.push_front(fVel0);

  return fVelAvg - fVelK + fVel0;
}

void hekateros::HekKinJoint::getFilteredJointCurPosVel ( double &  fCurPos, double &  fCurVel  ) const

inline

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

Parameters

[out]	fCurPos	Current joint position (radians).
[out]	fCurVel	Current joint velocity (radians/second).

Definition at line 662 of file hekKinJoint.h.

    {
      fCurPos = m_fJointPosOut;
      fCurVel = m_fJointVelOut;
    }

int HekKinJoint::getGoalDir ( )

protected

Get the current goal rotation direction.

The direction is deterimed by the difference of current odometer value and the goal position.

Normally counter-clockwise is positive, clockwise is negative, but the sence can be reversed in the odometer configuration.

Returns

1 or -1.

Definition at line 737 of file hekKinJoint.cxx.

References m_nServoGoalPos, and m_pServo.

Referenced by canMoveInGoalDir(), and estimateServoTgtSpeedSimple().

{
  int nSign = m_pServo->IsOdometerReversed()? -1: 1;
  
  return (m_nServoGoalPos-m_pServo->GetOdometer()) >= 0? nSign: -1 * nSign;
}

void hekateros::HekKinJoint::getJointCurPosVel ( double &  fCurPos, double &  fCurVel  ) const

inline

Get the instantaneous current joint position and velocity.

Parameters

[out]	fCurPos	Current joint position (radians).
[out]	fCurVel	Current joint velocity (radians/second).

Definition at line 650 of file hekKinJoint.h.

    {
      fCurPos = m_fJointCurPos;
      fCurVel = m_fJointCurVel;
    }

MoveState hekateros::HekKinJoint::getMoveState ( )

inline

Get move control state for this joint.

Returns

MoveState

Definition at line 511 of file hekKinJoint.h.

Referenced by hekateros::HekKinJointWristRot::coupleJoint().

    {
      return m_eState;
    }

void hekateros::HekKinJoint::getPidKParams ( double &  fKp, double &  fKi, double &  fKd  ) const

inline

Get the joint's position and velocity PID K parameters.

Parameters

[out]	fKp	Proportional gain parameter.
[out]	fKi	Integral gain parameter.
[out]	fKd	Derivative gain parameter.

Definition at line 575 of file hekKinJoint.h.

    {
      m_pid.getKParams(fKp, fKi, fKd);
    }

double hekateros::HekKinJoint::getPidMaxDeltaVParam ( ) const

inline

Get joint PID maximum delta v (radians/second) parameter.

Returns

Maximum output delta velocity (radians/second).

Definition at line 599 of file hekKinJoint.h.

    {
      return m_pid.getMaxDeltaVParam();
    }

int HekKinJoint::getPlannedDir ( int  nServoTgtSpeed )

protected

Get the planned target rotation direction.

The direction is deterimed sign of the servo speed.

Normally counter-clockwise is positive, clockwise is negative, but the sence can be reversed in the odometer configuration.

Parameters

nServoTgtSpeed

Planned servo target speed (raw unitless). Odometer configuration is already added!

Returns

1 or -1.

Definition at line 744 of file hekKinJoint.cxx.

Referenced by canMoveInPlannedDir().

{
  return nServoTgtSpeed >= 0? 1: -1;
}

void hekateros::HekKinJoint::getServoCurPosSpeed ( int &  nServoCurPos, int &  nServoCurSpeed  ) const

inline

Get the instantaneous current servo position and speed.

Parameters

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

Definition at line 674 of file hekKinJoint.h.

    {
      nServoCurPos   = m_nServoCurPos;
      nServoCurSpeed = m_nServoCurSpeed;
    }

void hekateros::HekKinJoint::getTgtVelSpeed ( double &  fTgtVel, int &  nTgtSpeed  ) const

inline

Get the PID output target joint velocity and the corresponding servo speed estemate.

Parameters

[out]	fTgtVel	Target joint velocity (radians/second).
[out]	nTgtSpeed	Target servo speed (raw unitless).

Definition at line 687 of file hekKinJoint.h.

    {
      fTgtVel   = m_fJointTgtVel;
      nTgtSpeed = m_nServoTgtSpeed;
    }

void hekateros::HekKinJoint::getToleranceParams ( double &  fTolPos, double &  fTolVel  ) const

inline

Get the position and velocity tolerance parameters.

The joint PID will try to control to the goal position ± position tolerance and to the goal velocity ± velocity tolerance.

Parameters

[out]	fTolPos	Joint position tolerance (radians).
[out]	fTolVel	Joint velocity tolerance (radians/second).

Definition at line 623 of file hekKinJoint.h.

    {
      fTolPos = m_fTolPos;
      fTolVel = m_fTolVel;
    }

virtual bool hekateros::HekKinJoint::hasOverTorqueCondition ( )

inlinevirtual

Test if joint is in an over torque condition.

Returns

Returns true if in condition, false otherwise.

Definition at line 698 of file hekKinJoint.h.

    {
      return m_bOverTorqueCond;
    }

bool hekateros::HekKinJoint::isMoving ( )

inline

Test if actively moving joint.

Returns

Returns true or false.

Definition at line 541 of file hekKinJoint.h.

    {
      return isMovingToGoal() || isStopping();
    }

bool hekateros::HekKinJoint::isMovingToGoal ( )

inline

Test if actively moving joint to goal.

Returns

Returns true or false.

Definition at line 521 of file hekKinJoint.h.

Referenced by act(), hekateros::HekKinJointWristRot::act(), nlmove(), hekateros::HekKinJointWristRot::nlmove(), planMotion(), hekateros::HekKinJointWristRot::planMotion(), react(), and hekateros::HekKinJointWristRot::react().

    {
      return m_eState == MoveStateNewMove || m_eState == MoveStateMoving;
    }

virtual bool hekateros::HekKinJoint::isStopped ( )

inlinevirtual

Test if joint is stopped (i.e. control is not active).

Returns

Returns true if stopped, false otherwise.

Definition at line 551 of file hekKinJoint.h.

Referenced by hekateros::HekKinJointWristRot::act(), and hekateros::HekKinJointWristRot::react().

    {
      return m_eState == MoveStateIdle;
    }

bool hekateros::HekKinJoint::isStopping ( )

inline

Test if actively stopping joint.

Returns

Returns true or false.

Definition at line 531 of file hekKinJoint.h.

Referenced by react(), and hekateros::HekKinJointWristRot::react().

    {
      return m_eState == MoveStateStopping;
    }

int HekKinJoint::jointPosToServoPos ( const double  fPos )

virtual

Convert joint position to the equivalent servo position.

Parameters

fPos	Joint position (radians).

Returns

Equivalent servo position (odometer ticks).

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 1244 of file hekKinJoint.cxx.

References hekateros::HekRobotJoint::m_fTicksPerJointRad, and m_pJoint.

Referenced by hekateros::HekKinJointWristRot::nlmove(), nlspecifyMove(), and hekateros::HekKinJointWristRot::planMotion().

{
  return (int)(fPos * m_pJoint->m_fTicksPerJointRad);
}

double HekKinJoint::jointVelocity ( double  fPos1, double  fPos0, double  fDt  )

protectedvirtual

Calculate the joint velocity from delta servo positions.

Parameters

fPos1	Later joint position (radians/second).
fPos0	Earlier joint position (radians/second).
fDt	Delta time in seconds.

Returns

Calculated joint velocity (radians).

Definition at line 1254 of file hekKinJoint.cxx.

Referenced by senseDynamics().

{
  return fDt > 0.0? (fPos1 - fPos0) / fDt: 0.0;
}

void hekateros::HekKinJoint::lock ( )

inlineprotected

Lock the joint kinematics.

The calling thread will block while waiting for the mutex to become available. Once locked, the Hekateros kinematics thread will block when accessing this joint.

The lock()/unlock() primitives provide a safe mechanism to modify the joint data, such as the next goals.

Context:

Any.

Definition at line 874 of file hekKinJoint.h.

Referenced by act(), hekateros::HekKinJointWristRot::act(), applyTorqueControl(), move(), planMotion(), hekateros::HekKinJointWristRot::planMotion(), react(), hekateros::HekKinJointWristRot::react(), reload(), resetServoOdometer(), senseDynamics(), specifyMove(), and stop().

    {
      pthread_mutex_lock(&m_mutexSync);
    }

HekKinJoint::MoveState HekKinJoint::move ( )

virtual

Move joint using joint PID controller.

Once the move has reached the goal joint position, within tolerence, the move will automatically stop.

Returns

Move control state after call.

Definition at line 1028 of file hekKinJoint.cxx.

References lock(), m_pServo, MoveStateIdle, nlmove(), and unlock().

{
  MoveState eState;

  if( m_pServo == NULL )
  {
    return MoveStateIdle;
  }

  lock();

  eState = nlmove();

  unlock();

  return eState;
}

void HekKinJoint::moveServo ( int  nServoGoalSpeed, int  nServoGoalPos  )

protected

Move servo.

Parameters

nServoGoalSpeed	Servo goal speed (raw unitless).
nServoGoalPos	Servo goal position (odometer ticks).
bNewMove	This is [not] a new move.

Definition at line 1080 of file hekKinJoint.cxx.

References hekateros::cap(), hekateros::iabs(), m_eState, m_pServo, MoveStateMoving, and MoveStateNewMove.

Referenced by nlmove(), and hekateros::HekKinJointWristRot::nlmove().

{
  //
  // Annoying Dynamixel subtleties are outlined here.
  //
  // In continuous mode:
  //   When writing to the goal speed, the servo will immediately begin
  //   movement. Servo goal position has no effect. A value of 0 will
  //   stop the servo. The speeds are signed values.
  //
  // In servo mode:
  //    When writing the goal position, if different from the current
  //    position, the servo will immediately move at the goal speed
  //    loaded in its control table. So always write goal speed first,
  //    then goal position. The value of 0 cause the servo to go ape shit,
  //    moving at its maximum speed with no servo control. Avoid this value
  //    like the Ebola. To stop the servo, write the current position as
  //    its goal position. One final nit, no negative speeds.
  //
  if( m_pServo->GetServoMode() == DYNA_MODE_CONTINUOUS )
  {
    nServoGoalSpeed = cap(nServoGoalSpeed, -DYNA_SPEED_MAX_RAW,
                                            DYNA_SPEED_MAX_RAW);
    m_pServo->WriteGoalSpeed(nServoGoalSpeed);
  }
  else
  {
    nServoGoalSpeed = cap(iabs(nServoGoalSpeed), DYNA_SPEED_MIN_CTL,
                                                 DYNA_SPEED_MAX_RAW);
    m_pServo->WriteGoalSpeed(nServoGoalSpeed);
    if( m_eState == MoveStateNewMove )
    {
      m_pServo->WriteGoalPos(nServoGoalPos);
      m_eState = MoveStateMoving;
    }
  }
}

int HekKinJoint::nlapplyTorqueControl ( )

protectedvirtual

Apply torque limit control (no lock version).

Returns

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

Definition at line 982 of file hekKinJoint.cxx.

References filterTorques(), hekateros::HEK_OK, m_fOverTorqueTh, m_fTorqueOut, m_nServoCurLoad, m_pServo, TORQUE_CTL_BACKOFF_POS, and TORQUE_CTL_BACKOFF_SPEED.

Referenced by act(), hekateros::HekKinJointWristRot::act(), applyTorqueControl(), react(), and hekateros::HekKinJointWristRot::react().

{
  static const int        TuneMaxIters  = 5;    // max iterations to ease torque
  static const useconds_t TuneWait      = 100;  // wait between iterations

  int nSign;
  int nOffSpeed;
  int nOffPos;

  if( fabs(m_fTorqueOut) > m_fOverTorqueTh )
  {
    LOGDIAG4("Executing torque control for servo %d.", m_pServo->GetServoId());

    nSign     = DYNA_GET_DIR(m_nServoCurLoad);
    nOffSpeed = TORQUE_CTL_BACKOFF_SPEED;

    switch( m_pServo->GetServoMode() )
    {
      case DYNA_MODE_CONTINUOUS:
        nOffSpeed *= nSign;
        m_pServo->WriteGoalSpeed(nOffSpeed);
        for(int i = 0; i < TuneMaxIters; ++i)
        {
          usleep(TuneWait);
          m_pServo->ReadCurLoad(&m_nServoCurLoad);
          m_fTorqueOut = filterTorques(m_fTorqueOut, m_nServoCurLoad);
          if( fabs(m_fTorqueOut) <= m_fOverTorqueTh )
          {
            break;
          }
        }
        m_pServo->Stop();
        break;

      case DYNA_MODE_SERVO:
      default:
        nOffPos = nSign * TORQUE_CTL_BACKOFF_POS;
        m_pServo->WriteGoalSpeed(nOffSpeed);
        m_pServo->WriteGoalPos(nOffPos);
        break;
    }
  }

  return HEK_OK;
}

HekKinJoint::MoveState HekKinJoint::nlmove ( )

protectedvirtual

Move joint using joint PID controller (no lock version).

Once the move has reached the goal joint position, within tolerence, the move will automatically stop.

Returns

Move control state after move call.

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 1046 of file hekKinJoint.cxx.

References hekateros::HekPid::control(), estimateServoTgtSpeed(), isMovingToGoal(), m_eState, m_fDt, m_fJointCurPos, m_fJointCurVel, m_fJointGoalPos, m_fJointTgtVel, m_fTolPos, m_fTolVel, m_nServoGoalPos, m_nServoTgtSpeed, m_pid, moveServo(), MoveStateNewMove, and nlstop().

Referenced by act(), hekateros::HekKinJointWristRot::act(), and move().

{
  if( isMovingToGoal() )
  {
    // still away from goal position
    if( fabs(m_fJointGoalPos-m_fJointCurPos) > m_fTolPos )
    {
      // PID target velocity and corresponding target servo speed estimate
      m_fJointTgtVel    = m_pid.control(m_fJointCurPos, m_fJointCurVel, m_fDt);
      m_nServoTgtSpeed  = estimateServoTgtSpeed(m_fJointTgtVel);

      // new move - make it happen
      if( m_eState == MoveStateNewMove )
      {
        moveServo(m_nServoTgtSpeed, m_nServoGoalPos);
      }

      // subsequent changes of servo speed - only if sufficently different
      else if( fabs(m_fJointTgtVel-m_fJointCurVel) > m_fTolVel )
      {
        moveServo(m_nServoTgtSpeed, m_nServoGoalPos);
      }
    }

    // at goal position
    else
    {
      nlstop();
    }
  }

  return m_eState;
}

int HekKinJoint::nlslowToStop ( SyncMoveMsgs &  msg )

protectedvirtual

Slow to stop (no lock version).

Parameters

msgs	Synchronous move messages container object.

Returns

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

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 1210 of file hekKinJoint.cxx.

References addServoMsgEntries(), estimateServoTgtSpeed(), hekateros::HEK_OK, m_fJointCurVel, m_fStopVelTh, m_nServoGoalPos, and nlstop().

Referenced by react(), and hekateros::HekKinJointWristRot::react().

{
  double fJointStopVel;
  int    nServoStopSpeed;

  if( (fabs(m_fJointCurVel) <= m_fStopVelTh) )
  {
    //fprintf(stderr, "DBG: %s() SLOWED ENOUGH: curvel=%lf\n",
    //  LOGFUNCNAME, radToDeg(m_fJointCurVel));

    return nlstop();
  }
  else
  {
    if( m_fJointCurVel > 0.0 )
    {
      fJointStopVel = m_fJointCurVel - m_fStopVelTh;
    }
    else
    {
      fJointStopVel = m_fJointCurVel + m_fStopVelTh;
    }

    nServoStopSpeed  = estimateServoTgtSpeed(fJointStopVel);

    addServoMsgEntries(msgs, nServoStopSpeed, m_nServoGoalPos);

    //fprintf(stderr, "DBG: %s() SLOWING TO: curvel=%lf, stopvel=%lf\n",
    //  LOGFUNCNAME, radToDeg(m_fJointCurVel), radToDeg(fJointStopVel));

    return HEK_OK;
  }
}

int HekKinJoint::nlspecifyMove ( const double  fGoalPos, const double  fGoalVel  )

protectedvirtual

Specify a new joint goal position and velocity move (no lock version).

The joint PID will begin to control to the new goals, smoothly transitioning (I hope) from the current goals.

Parameters

fGoalPos	Joint goal position (radians).
fGoalVel	Joint goal velocity (radians/second).

Returns

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

Definition at line 675 of file hekKinJoint.cxx.

References hekateros::HEK_OK, hekateros::HekJointTypeContinuous, hekateros::HekOpStateCalibrated, jointPosToServoPos(), hekateros::HekRobotJoint::m_eJointType, hekateros::HekRobotJoint::m_eOpState, m_eState, m_fJointCurPos, m_fJointGoalPos, m_fJointGoalVel, hekateros::HekRobotJoint::m_fMaxSoftLimitRads, hekateros::HekRobotJoint::m_fMinSoftLimitRads, m_fVelDerate, hekateros::HekRobotJoint::m_nMasterServoId, m_nServoGoalPos, m_pid, m_pJoint, hekateros::HekRobotJoint::m_strName, MoveStateNewMove, hekateros::radToDeg(), and hekateros::HekPid::specifySetPoint().

Referenced by hekateros::HekKinJointWristRot::act(), hekateros::HekKinJointWristRot::react(), and specifyMove().

{
  // new goal position
  m_fJointGoalPos = fGoalPos;

  //
  // Check for goal position out of range condition and cap as necessary.
  //
  // Note:  Only check if the joint has been calibrated. If calibrated, assume
  //        a calibration routine is calling the function and "it knows what
  //        it is doing".
  //
  if( m_pJoint->m_eOpState == HekOpStateCalibrated )
  {
    if( m_pJoint->m_eJointType != HekJointTypeContinuous )
    {
      if( m_fJointGoalPos < m_pJoint->m_fMinSoftLimitRads )
      {
        LOGDIAG3("Joint %s (servo=%d): "
              "Goal position %.02lf < %.02lf degree limit - capping.",
          m_pJoint->m_strName.c_str(), m_pJoint->m_nMasterServoId,
          radToDeg(m_fJointGoalPos), radToDeg(m_pJoint->m_fMinSoftLimitRads));

        m_fJointGoalPos = m_pJoint->m_fMinSoftLimitRads;
      }
      else if( m_fJointGoalPos > m_pJoint->m_fMaxSoftLimitRads )
      {
        LOGDIAG3("Joint %s (servo=%d): "
              "Goal position %.02lf > %.02lf degree limit - capping.",
          m_pJoint->m_strName.c_str(), m_pJoint->m_nMasterServoId,
          radToDeg(m_fJointGoalPos), radToDeg(m_pJoint->m_fMaxSoftLimitRads));

        m_fJointGoalPos = m_pJoint->m_fMaxSoftLimitRads;
      }
    }
  }

  // joint position to odometer position
  m_nServoGoalPos = jointPosToServoPos(m_fJointGoalPos);

  //
  // Joint current and goal positions dictate sign of velocity. 
  //
  m_fJointGoalVel = fabs(fGoalVel);
  if( m_fJointCurPos > m_fJointGoalPos )
  {
    m_fJointGoalVel = -m_fJointGoalVel;
  }

  // derate
  m_fJointGoalVel *= m_fVelDerate;

  // new move
  //if( fabs(m_fJointGoalPos-m_fJointCurPos) > m_fTolPos )
  //{
    m_pid.specifySetPoint(m_fJointGoalPos, m_fJointGoalVel);
    m_eState = MoveStateNewMove;
  //}

  return HEK_OK;
}

int HekKinJoint::nlstop ( )

protectedvirtual

Stop movement and move control of the joint (no lock version).

Returns

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

Definition at line 1162 of file hekKinJoint.cxx.

References canStopNow(), hekateros::HEK_OK, m_eState, m_fJointCurPos, m_fJointCurVel, m_fJointGoalPos, m_fJointGoalVel, m_fJointPrevPos, m_fJointTgtVel, m_nServoCurPos, m_nServoCurSpeed, m_nServoGoalPos, m_nServoPrevPos, m_nServoTgtSpeed, m_pServo, MoveStateIdle, MoveStateStopping, and servoPosToJointPos().

Referenced by nlmove(), hekateros::HekKinJointWristRot::nlmove(), nlslowToStop(), hekateros::HekKinJointWristRot::nlslowToStop(), react(), hekateros::HekKinJointWristRot::react(), resetServoOdometer(), and stop().

{
#ifdef HEK_KIN_EXEC_ALG_SYNC
  //
  // Test if moving too fast. If true, use a fast ramp down to slow to a
  // stopping speed. The ramp is defined by the maximum delta velocity tune
  // parameter.
  //
  // Note:  During calibration stop means stop.
  //
  if( !canStopNow() )
  {
    //fprintf(stderr, "DBG: %s() MOVING TO FAST TO STOP: "
    //                "curvel=%lf, stopvelth=%lf\n",
    //  LOGFUNCNAME, radToDeg(m_fJointCurVel), radToDeg(m_fStopVelTh));

    m_eState = MoveStateStopping;
    return HEK_OK;
  }
#endif // HEK_KIN_EXEC_ALG_SYNC

  m_pServo->Stop();

  if( m_pServo->ReadCurPos(&m_nServoCurPos) == DYNA_OK )
  {
    m_fJointCurPos = servoPosToJointPos(m_nServoCurPos);
  }

  m_fJointPrevPos   = m_fJointCurPos;
  m_nServoPrevPos   = m_nServoCurPos;

  m_fJointCurVel    = 0.0;
  m_nServoCurSpeed  = 0;

  m_fJointGoalPos   = m_fJointCurPos;
  m_nServoGoalPos   = m_nServoCurPos;
  m_fJointGoalVel   = 0.0;

  m_fJointTgtVel    = 0.0;
  m_nServoTgtSpeed  = 0;

  m_eState = MoveStateIdle;

  //fprintf(stderr, "DBG: STOP: %s\n", m_pJoint->m_strName.c_str());

  return HEK_OK;
}

HekKinJoint & HekKinJoint::operator=

(

const HekKinJoint &

rhs

)

Assignment operator.

Parameters

rhs	Right hand side object.

Returns

*this

Definition at line 520 of file hekKinJoint.cxx.

References m_bOverTorqueCond, m_eState, m_fClearTorqueTh, m_fDt, m_fDtAvg, m_fJointCurPos, m_fJointCurVel, m_fJointGoalPos, m_fJointGoalVel, m_fJointPosOut, m_fJointPrevPos, m_fJointRadPerTick, m_fJointTgtVel, m_fJointVelOut, m_fOverTorqueTh, m_fStopVelTh, m_fTolPos, m_fTolVel, m_fTorqueOut, m_histDtIn, m_histPosIn, m_histTorqueIn, m_histVelIn, m_nServoCurPos, m_nServoCurSpeed, m_nServoGoalPos, m_nServoPrevPos, m_nServoTgtSpeed, m_pid, m_pJoint, m_pServo, m_tblVelSpeed, and m_tsPrev.

{
  m_eState    = rhs.m_eState;
  m_pJoint    = rhs.m_pJoint;
  m_pServo    = rhs.m_pServo;

  m_fTolPos     = rhs.m_fTolPos;
  m_fTolVel     = rhs.m_fTolVel;
  m_fStopVelTh  = rhs.m_fStopVelTh;
  m_pid         = rhs.m_pid;

  m_fJointRadPerTick  = rhs.m_fJointRadPerTick;
  m_fJointGoalPos     = rhs.m_fJointGoalPos;
  m_nServoGoalPos     = rhs.m_nServoGoalPos;
  m_fJointCurPos      = rhs.m_fJointCurPos;
  m_nServoCurPos      = rhs.m_nServoCurPos;
  m_fJointPrevPos     = rhs.m_fJointPrevPos;
  m_nServoPrevPos     = rhs.m_nServoPrevPos;
  m_fJointPosOut      = rhs.m_fJointPosOut;
  m_histPosIn         = rhs.m_histPosIn;

  m_fDt       = rhs.m_fDt;
  m_tsPrev    = rhs.m_tsPrev;
  m_fDtAvg    = rhs.m_fDtAvg;
  m_histDtIn  = rhs.m_histDtIn;

  m_fJointGoalVel   = rhs.m_fJointGoalVel;
  m_fJointCurVel    = rhs.m_fJointCurVel;
  m_nServoCurSpeed  = rhs.m_nServoCurSpeed;
  m_tblVelSpeed     = rhs.m_tblVelSpeed;
  m_fJointVelOut    = rhs.m_fJointVelOut;
  m_histVelIn       = rhs.m_histVelIn;

  m_fJointTgtVel    = rhs.m_fJointTgtVel;
  m_nServoTgtSpeed  = rhs.m_nServoTgtSpeed;

  m_bOverTorqueCond = rhs.m_bOverTorqueCond;
  m_fOverTorqueTh   = rhs.m_fOverTorqueTh;
  m_fClearTorqueTh  = rhs.m_fClearTorqueTh;
  m_fTorqueOut      = rhs.m_fTorqueOut;
  m_histTorqueIn    = rhs.m_histTorqueIn;

  return *this;
}

HekKinJoint::MoveState HekKinJoint::planMotion ( bool  bIsControlling, SyncMoveMsgs &  msgs  )

virtual

Plan motion.

Parameters

bIsControlling	Joint is [not] being actively controlled. However, planning may still be desired.
msgs	Synchronous move messages container object.

Returns

Move control state after call.

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 895 of file hekKinJoint.cxx.

References addServoMsgEntries(), canMoveInPlannedDir(), hekateros::HekPid::control(), estimateServoTgtSpeed(), isMovingToGoal(), lock(), m_eState, m_fDt, m_fJointCurPos, m_fJointCurVel, m_fJointGoalPos, m_fJointTgtVel, m_fTolPos, m_fTolVel, m_nServoGoalPos, m_nServoTgtSpeed, m_pid, m_pServo, MoveStateIdle, MoveStateNewMove, and unlock().

{
  if( m_pServo == NULL )
  {
    return MoveStateIdle;
  }
  else if( !bIsControlling )
  {
    return m_eState;
  }

  lock();

  if( isMovingToGoal() )
  {
    // PID target velocity and corresponding target servo speed estimate
    m_fJointTgtVel    = m_pid.control(m_fJointCurPos, m_fJointCurVel, m_fDt);
    m_nServoTgtSpeed  = estimateServoTgtSpeed(m_fJointTgtVel);

    // still away from goal position
    if( (fabs(m_fJointGoalPos-m_fJointCurPos) > m_fTolPos) &&
        canMoveInPlannedDir(m_nServoTgtSpeed) )
    {
      // new move - make it happen
      if( m_eState == MoveStateNewMove )
      {
        addServoMsgEntries(msgs, m_nServoTgtSpeed, m_nServoGoalPos);
      }

      // subsequent changes of servo speed - only if sufficently different
      else if( fabs(m_fJointTgtVel-m_fJointCurVel) > m_fTolVel )
      {
        addServoMsgEntries(msgs, m_nServoTgtSpeed, m_nServoGoalPos);
      }
    }
  }

  unlock();

  return m_eState;
}

int HekKinJoint::react ( bool  bIsControlling, SyncMoveMsgs &  msgs  )

virtual

React to recently read sensor data.

Parameters

bIsControlling	Joint is [not] being actively controlled. However, reaction may still be desired.
msgs	Synchronous move messages container object.

Returns

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

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 854 of file hekKinJoint.cxx.

References canStopNow(), hekateros::HEK_OK, isMovingToGoal(), isStopping(), lock(), m_bOverTorqueCond, m_fJointCurPos, m_fJointGoalPos, m_fTolPos, nlapplyTorqueControl(), nlslowToStop(), nlstop(), and unlock().

{
  if( !bIsControlling )
  {
    return HEK_OK;
  }

  lock();

  //
  // Joint is slowing to a stop.
  //
  if( isStopping() )
  {
    nlslowToStop(msgs);
  }

  //
  // Joint has moved close enough to goal position, and is slow enough to stop.
  // Otherwise let the PID do its thing.
  //
  else if( isMovingToGoal() &&
          (fabs(m_fJointGoalPos-m_fJointCurPos) <= m_fTolPos) &&
          canStopNow() )
  {
    nlstop();
  }

  //
  // In over torque condition, apply torque control.
  //
  if( m_bOverTorqueCond )
  {
    nlapplyTorqueControl();
  }

  unlock();

  return HEK_OK;
}

void HekKinJoint::reload ( const HekTunes &  tunes )

virtual

Reload configuration tuning parameters.

Parameters

tunes

Hekateros tuning parameters.

Definition at line 565 of file hekKinJoint.cxx.

References hekateros::HekTunes::getPidKParams(), hekateros::HekTunes::getPidMaxDeltaV(), hekateros::HekTunes::getToleranceParams(), hekateros::HekTunes::getTorqueParams(), hekateros::HekTunes::getVelocityDerate(), lock(), m_fClearTorqueTh, m_fOverTorqueTh, m_fStopVelTh, m_fTolPos, m_fTolVel, m_fVelDerate, m_pJoint, m_pServo, hekateros::HekRobotJoint::m_strName, setPidKParams(), setPidMaxDeltaVParam(), SLOW_DERATE_DELTA_V, and unlock().

{
  double    fKp, fKi, fKd;
  double    fMaxDeltaV;
  double    fRawMaxTorque;

  lock();

  // joint tolerance tuning parameters (normalized)
  tunes.getToleranceParams(m_pJoint->m_strName, m_fTolPos, m_fTolVel);

  // joint position and velocity PID tuning parameters
  tunes.getPidKParams(m_pJoint->m_strName, fKp, fKi, fKd);

  // set classic pid parameters
  setPidKParams(fKp, fKi, fKd);

  // joint pid maximum delta velocity tuning paramter
  fMaxDeltaV = tunes.getPidMaxDeltaV(m_pJoint->m_strName);

  // maximum PID output delta v (radians/second)
  setPidMaxDeltaVParam(fMaxDeltaV);

  // stop velocity maximum velocity
  m_fStopVelTh = SLOW_DERATE_DELTA_V * fMaxDeltaV;

  // velocity derate
  m_fVelDerate      = tunes.getVelocityDerate();

  // normalized joint torque tuning parameters
  tunes.getTorqueParams(m_pJoint->m_strName, m_fOverTorqueTh, m_fClearTorqueTh);

  // convert to raw theshold values 
  fRawMaxTorque     = (double)m_pServo->GetSpecification().m_uRawTorqueMax;
  m_fOverTorqueTh  *= fRawMaxTorque;
  m_fClearTorqueTh *= fRawMaxTorque;

  unlock();
}

int HekKinJoint::resetServoOdometer ( )

virtual

Reset servo odometer zero point to the current encoder position.

Returns

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

Definition at line 605 of file hekKinJoint.cxx.

References hekateros::boolstr(), hekateros::HEK_ECODE_DYNA, hekateros::HEK_OK, lock(), m_fJointCurPos, m_fJointCurVel, m_fJointGoalPos, m_fJointGoalVel, m_fJointPrevPos, m_fJointTgtVel, hekateros::HekRobotJoint::m_nMasterServoDir, hekateros::HekRobotJoint::m_nMasterServoId, m_nServoCurPos, m_nServoCurSpeed, m_nServoGoalPos, m_nServoPrevPos, m_nServoTgtSpeed, m_pJoint, m_pServo, hekateros::HekRobotJoint::m_strName, nlstop(), and unlock().

{
  uint_t  uEncZeroPt;   // encoder position mapping to new odometer zero pt
  bool    bIsReverse;   // odometer is [not] reverse to that of encoder
  int     rc;           // return code

  if( m_pServo == NULL )
  {
    return HEK_OK;
  }

  lock();

  // must be stopped
  nlstop();

  // read encoder position (note: should add libDynamixel function)
  if( (rc = m_pServo->Read(DYNA_ADDR_CUR_POS_LSB, &uEncZeroPt)) != DYNA_OK )
  {
    LOGERROR("Joint %s (servo=%d): Cannot read current encoder position.",
          m_pJoint->m_strName.c_str(), m_pJoint->m_nMasterServoId);
    rc = -HEK_ECODE_DYNA;
  }

  else
  {
    bIsReverse = m_pJoint->m_nMasterServoDir == DYNA_DIR_CCW? false: true;

    // reset soft odometer
    m_pServo->ResetOdometer((int)uEncZeroPt, bIsReverse);

    LOGDIAG3("Joint %s (servo=%d): Odometer reset at encoder=%u, reverse=%s.",
          m_pJoint->m_strName.c_str(), m_pJoint->m_nMasterServoId,
          uEncZeroPt, boolstr(bIsReverse));

    m_fJointGoalPos = 0.0;
    m_nServoGoalPos = 0;
    m_fJointCurPos  = 0.0;
    m_nServoCurPos  = 0;
    m_fJointPrevPos = 0.0;
    m_nServoPrevPos = 0;

    m_fJointGoalVel   = 0.0;
    m_fJointCurVel    = 0.0;
    m_nServoCurSpeed  = 0;

    m_fJointTgtVel    = 0.0;
    m_nServoTgtSpeed  = 0;

    rc = HEK_OK;
  }

  unlock();

  return rc;
}

int HekKinJoint::senseDynamics ( bool  bIsControlling )

virtual

Sense (read) the current servo dynamics and transform to useful state.

Parameters

bIsControlling

Joint is [not] being actively controlled.

Returns

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

Definition at line 760 of file hekKinJoint.cxx.

References dejitterServoPos(), delta_t(), filterPositions(), filterTorques(), filterVelocities(), hekateros::HEK_ECODE_DYNA, hekateros::HEK_OK, hekateros::HekOpStateCalibrated, hekateros::iabs(), jointVelocity(), lock(), m_bOverTorqueCond, hekateros::HekRobotJoint::m_eOpState, m_fClearTorqueTh, m_fDt, m_fJointCurPos, m_fJointCurVel, m_fJointPosOut, m_fJointPrevPos, m_fJointVelOut, m_fOverTorqueTh, hekateros::HekRobotJoint::m_fTicksPerServoRad, m_fTorqueOut, m_nServoCurLoad, m_nServoCurPos, m_nServoCurSpeed, m_nServoPrevPos, m_pJoint, m_pServo, hekateros::HekRobotJoint::m_strName, servoPosToJointPos(), unlock(), and updateAssoc().

Referenced by hekateros::HekKinematics::exec().

{
  int   nServoDejitPos;
  int   rc;

  if( m_pServo == NULL )
  {
    return HEK_OK;
  }

  lock();

  // save current servo position as the new previous position
  m_nServoPrevPos = m_nServoCurPos;

  // Read the servo dynamics.
  rc = m_pServo->ReadDynamics(&m_nServoCurPos,
                              &m_nServoCurSpeed,
                              &m_nServoCurLoad);

  // sanity check
  if( (m_pJoint->m_eOpState == HekOpStateCalibrated) &&
    (iabs(m_nServoCurPos-m_nServoPrevPos) >
                                        m_pJoint->m_fTicksPerServoRad * M_PI) )
  {
    LOGWARN("Large jump in servo position: prev_odpos=%d, cur_odpos=%d.",
            m_nServoPrevPos, m_nServoCurPos);
  }

  //
  // Transform.
  //
  if( rc == DYNA_OK )
  {
    // delta time
    m_fDt = delta_t();

    // running average of delta times (not used for now)
    //m_fDtAvg = averageDt(m_fDtAvg, m_fDt);

    // de-jitter servo odometer (encoder) position
    nServoDejitPos = dejitterServoPos(m_nServoCurPos, m_nServoPrevPos);

    // current joint position (radians)
    m_fJointPrevPos = m_fJointCurPos;
    m_fJointCurPos  = servoPosToJointPos(nServoDejitPos);

    // low-pass band filter current positions
    m_fJointPosOut = filterPositions(m_fJointPosOut, m_fJointCurPos);

    // current joint velocity (radians/second)
    m_fJointCurVel = jointVelocity(m_fJointCurPos, m_fJointPrevPos, m_fDt);
    
    // update joint instantaneous velocity-servo speed lookup table
    if( bIsControlling )
    {
      updateAssoc(m_fJointCurVel, m_nServoCurSpeed);
    }

    // low-pass band filter current velocities
    m_fJointVelOut = filterVelocities(m_fJointVelOut, m_fJointCurVel);

    // low-pass band filter torque (raw unitless load)
    m_fTorqueOut = filterTorques(m_fTorqueOut, m_nServoCurLoad);
    
    // set over torque limit condition
    if( fabs(m_fTorqueOut) > m_fOverTorqueTh )
    {
      if( !m_bOverTorqueCond && bIsControlling )
      {
        LOGWARN("Joint %s (servo=%d): Entered over-torque condition. "
                "Load=%lf.",
            m_pJoint->m_strName.c_str(), m_pServo->GetServoId(), m_fTorqueOut);
      }
      m_bOverTorqueCond = true;
    }

    // clear over torque limit condition
    else if( fabs(m_fTorqueOut) < m_fClearTorqueTh )
    {
      if( m_bOverTorqueCond && bIsControlling )
      {
        LOGDIAG2("Joint %s (servo=%d): Cleared over-torque condition.",
            m_pJoint->m_strName.c_str(), m_pServo->GetServoId());
      }
      m_bOverTorqueCond = false;
    }
  }

  unlock();

  return rc == DYNA_OK? HEK_OK: -HEK_ECODE_DYNA;
}

double HekKinJoint::servoPosToJointPos ( const int  nOdPos )

virtual

Convert servo position to the equivalent joint position.

Parameters

nOdPos

Servo position (odometer ticks).

Returns

Equivalent joint position (radian).

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 1249 of file hekKinJoint.cxx.

References hekateros::HekRobotJoint::m_fTicksPerJointRad, and m_pJoint.

Referenced by nlstop(), and senseDynamics().

{ 
  return (double)nOdPos / m_pJoint->m_fTicksPerJointRad;
}

void hekateros::HekKinJoint::setPidKParams ( const double  fKp, const double  fKi, const double  fKd  )

inline

Set the joint's position and velocity PID K parameters.

Parameters

[in]	fKp	Proportional gain parameter.
[in]	fKi	Integral gain parameter.
[in]	fKd	Derivative gain parameter.

Returns

Definition at line 589 of file hekKinJoint.h.

Referenced by HekKinJoint(), and reload().

    {
      m_pid.setKParams(fKp, fKi, fKd);
    }

void hekateros::HekKinJoint::setPidMaxDeltaVParam ( const double  fMaxDeltaV )

inline

Set joint PID maximum delta v (radians/second) parameter.

Parameters

fMaxDeltaV

Maximum output delta velocity (radians/second).

Definition at line 609 of file hekKinJoint.h.

Referenced by HekKinJoint(), and reload().

    {
      m_pid.setMaxDeltaVParam(fMaxDeltaV);
    }

void hekateros::HekKinJoint::setToleranceParams ( const double  fTolPos, const double  fTolVel  )

inline

Set the position and velocity tolerance parameters.

The joint PID will try to control to the goal position ± position tolerance and to the goal velocity ± velocity tolerance.

Parameters

[in]	fTolPos	Joint position tolerance (radians).
[in]	fTolVel	Joint velocity tolerance (radians/second).

Definition at line 638 of file hekKinJoint.h.

    {
      m_fTolPos = fTolPos;
      m_fTolVel = fTolVel;
    }

int HekKinJoint::specifyMove	(	const double	fGoalPos,
		const double	fGoalVel
	)

Specify a new joint goal position and velocity move.

The joint PID will begin to control to the new goals, smoothly transitioning (I hope) from the current goals.

Parameters

fGoalPos	Joint goal position (radians).
fGoalVel	Joint goal velocity (radians/second).

Returns

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

Definition at line 662 of file hekKinJoint.cxx.

References lock(), nlspecifyMove(), and unlock().

{
  int   rc;

  lock();

  rc = nlspecifyMove(fGoalPos, fGoalVel);

  unlock();

  return rc;
}

int HekKinJoint::stop ( )

virtual

Stop movement and move control of the joint.

Returns

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

Definition at line 1144 of file hekKinJoint.cxx.

References hekateros::HEK_ECODE_DYNA, lock(), m_pServo, nlstop(), and unlock().

{
  int   rc;

  if( m_pServo == NULL )
  {
    return -HEK_ECODE_DYNA;
  }

  lock();

  rc = nlstop();

  unlock();

  return rc;
}

void hekateros::HekKinJoint::unlock ( )

inlineprotected

Unlock the joint kinematics.

The Hekateros kinematics thread will be available to run when accessing this joint.

Context:

Any.

Definition at line 888 of file hekKinJoint.h.

Referenced by act(), hekateros::HekKinJointWristRot::act(), applyTorqueControl(), move(), planMotion(), hekateros::HekKinJointWristRot::planMotion(), react(), hekateros::HekKinJointWristRot::react(), reload(), resetServoOdometer(), senseDynamics(), specifyMove(), and stop().

    {
      pthread_mutex_unlock(&m_mutexSync);
    }

void HekKinJoint::updateAssoc ( double  fJointVel, int  nServoSpeed  )

protectedvirtual

Update joint velocity - servo speed association.

Parameters

fJointVel	Joint velocity (radians/second).
nServoSpeed	Servo speed (raw unitless).

Reimplemented in hekateros::HekKinJointWristRot.

Definition at line 1259 of file hekKinJoint.cxx.

References m_tblVelSpeed, and hekateros::VelSpeedLookupTbl::update().

Referenced by senseDynamics().

{
  m_tblVelSpeed.update(fJointVel, nServoSpeed);
}

---

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

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