Special wrist rotation joint kinematics and dynamics class. More...

#include <hekKinJoint.h>

Inheritance diagram for hekateros::HekKinJointWristRot:

Public Member Functions
	HekKinJointWristRot ()
	Default constructor.

	HekKinJointWristRot (HekRobotJoint pJoint, DynaServo pServo, const HekTunes &tunes)
	Initialization constructor. More...

virtual	~HekKinJointWristRot ()
	Destructor.

HekKinJointWristRot &	operator= (const HekKinJointWristRot &rhs)
	Assignment operator. More...

virtual void	coupleJoint (HekKinJoint *pKinJointWristPitch)
	Couple joint. 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	jointPosToServoPos (double fPos)
	Convert joint position to the equivalent servo position. More...

virtual double	servoPosToJointPos (int nOdPos)
	Convert servo position to the equivalent joint position. More...

Public Member Functions inherited from hekateros::HekKinJoint
	HekKinJoint ()
	Default constructor.

	HekKinJoint (HekRobotJoint pJoint, DynaServo pServo, const HekTunes &tunes)
	Initialization constructor. More...

virtual	~HekKinJoint ()
	Copy constructor. 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	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	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...

Protected Member Functions
virtual void	updateAssoc (double fJointVel, int nServoSpeed)
	Update joint velocity - servo speed association. More...

MoveState	getCoupledJointState ()
	Get coupled joint's move state. More...

virtual bool	isCoupledJointStopped ()
	Test if coupled joint is stopped. More...

virtual bool	isCoupledJointMoving ()
	Test if coupled joint is moving. More...

virtual MoveState	nlmove ()
	Move joint using joint PID controller (no lock version). More...

virtual int	nlslowToStop (SyncMoveMsgs &msg)
	Slow to stop (no lock version). More...

Protected Member Functions inherited from hekateros::HekKinJoint
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 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 int	nlstop ()
	Stop movement and move control of the joint (no lock version). More...

Protected Attributes
HekKinJoint *	m_pKinJointWristPitch
	wrist pitch joint description

MoveState	m_eStateWristPitch
	joint control state

Protected Attributes inherited from hekateros::HekKinJoint
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

Additional Inherited Members
Public Types inherited from hekateros::HekKinJoint
enum	MoveState { MoveStateIdle, MoveStateNewMove, MoveStateMoving, MoveStateStopping }
	Kinedynamics joint position and velocity control states. More...

Static Public Attributes inherited from hekateros::HekKinJoint
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)

Detailed Description

Special wrist rotation joint kinematics and dynamics class.

The Hekateros wrist joints are coupled in that wrist pitch effect wrist rotation. This class "decouples" these joints in software.

Definition at line 1143 of file hekKinJoint.h.

Constructor & Destructor Documentation

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

inline

Initialization constructor.

Parameters

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

Definition at line 1162 of file hekKinJoint.h.

                                                 :
       HekKinJoint(pJoint, pServo, tunes)
     {
       m_pKinJointWristPitch = NULL;
       m_eStateWristPitch    = MoveStateIdle;
     }

Member Function Documentation

int HekKinJointWristRot::act ( )

virtual

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

Since the wrist rotation is coupled to wrist pitch, if the pitch is moving, so must the rotation.

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

Reimplemented from hekateros::HekKinJoint.

Definition at line 1519 of file hekKinJoint.cxx.

 {
   lock();
 
   HekKinJoint::MoveState eStatePrev = m_eStateWristPitch;
 
   m_eStateWristPitch = getCoupledJointState();
 
   if( isStopped() && isCoupledJointStopped() &&
       (eStatePrev != MoveStateIdle) )
   {
     nlspecifyMove(m_fJointGoalPos, m_fJointGoalVel);
   }
 
   //
   // Actively moving wrist rotation and/or wrist pitch and there is no over
   // torque condition or the goal direction will naturally ease the torque.
   //
   if( isMovingToGoal() || isCoupledJointMoving() && canMoveInGoalDir() )
   {
     nlmove();
   }
 
   //
   // In over torque condition, apply torque control.
   //
   else if( m_bOverTorqueCond )
   {
     nlapplyTorqueControl();
   }
 
   unlock();
 
   return HEK_OK;
 }

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

inlinevirtual

Couple joint.

Parameters

pKinJointWristPitch Wrist pitch joint kinematics.

Reimplemented from hekateros::HekKinJoint.

Definition at line 1198 of file hekKinJoint.h.

References hekateros::HekKinJoint::getMoveState().

     {
       m_pKinJointWristPitch = pKinJointWristPitch;
       if( m_pKinJointWristPitch != NULL )
       {
         m_eStateWristPitch = m_pKinJointWristPitch->getMoveState();
       }
     }

HekKinJoint::MoveState HekKinJointWristRot::getCoupledJointState ( )

protected

Get coupled joint's move state.

Returns: State.

Definition at line 1712 of file hekKinJoint.cxx.

References hekateros::HekKinJoint::MoveStateIdle.

 {
   if( m_pKinJointWristPitch != NULL )
   {
     return m_pKinJointWristPitch->getMoveState();
   }
   else
   {
     return MoveStateIdle;
   }
 }

virtual bool hekateros::HekKinJointWristRot::isCoupledJointMoving ( )

inlineprotectedvirtual

Test if coupled joint is moving.

Returns: Returns true if moving, false otherwise.

Definition at line 1298 of file hekKinJoint.h.

     {
       return !isCoupledJointStopped();
     }

virtual bool hekateros::HekKinJointWristRot::isCoupledJointStopped ( )

inlineprotectedvirtual

Test if coupled joint is stopped.

Returns: Returns true if stopped, false otherwise.

Definition at line 1288 of file hekKinJoint.h.

     {
       return getCoupledJointState() == MoveStateIdle;
     }

int HekKinJointWristRot::jointPosToServoPos ( double fPos )

virtual

Convert joint position to the equivalent servo position.

Adjust for wrist pitch.

Parameters

fPos	Joint position (radians).

Returns: Equivalent servo position (odometer ticks).

Reimplemented from hekateros::HekKinJoint.

Definition at line 1666 of file hekKinJoint.cxx.

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

 {
   int     nAdjustPos = 0, nAdjustSpeed = 0;
   int     nServoPos;
 
   if( m_pKinJointWristPitch != NULL )
   {
     m_pKinJointWristPitch->getServoCurPosSpeed(nAdjustPos, nAdjustSpeed);
   }
 
   nServoPos = (int)(fPos * m_pJoint->m_fTicksPerJointRad);
 
   return nServoPos + nAdjustPos;
 }

HekKinJoint::MoveState HekKinJointWristRot::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.

Since the wrist rotation is coupled to wrist pitch, if the pitch is moving, so must the rotation.

Returns: Move control state after call.

Reimplemented from hekateros::HekKinJoint.

Definition at line 1555 of file hekKinJoint.cxx.

 {
   double  fAdjustVel;
   int     fAdjustSpeed;
 
   if( m_eStateWristPitch != MoveStateIdle )
   {
     m_pKinJointWristPitch->getTgtVelSpeed(fAdjustVel, fAdjustSpeed);
   }
   else
   {
     fAdjustVel    = 0.0;
     fAdjustSpeed  = 0;
   }
 
   if( isMovingToGoal() || isCoupledJointMoving() )
   {
     // still away from goal position
     if( (fabs(m_fJointGoalPos-m_fJointCurPos) > m_fTolPos) ||
         (m_eStateWristPitch != MoveStateIdle) )
     {
       // servo goal od position can change with coupled joints - recalculate
       m_nServoGoalPos = jointPosToServoPos(m_fJointGoalPos);
 
       if( isMovingToGoal() )
       {
         // PID target velocity 
         m_fJointTgtVel = m_pid.control(m_fJointCurPos, m_fJointCurVel, m_fDt);
 
         // estimate corresponding target servo speed
         m_nServoTgtSpeed  = estimateServoTgtSpeed(m_fJointTgtVel);
       }
       else
       {
         m_fJointTgtVel    = 0.0;
         m_nServoTgtSpeed  = 0;
       }
 
       m_nServoTgtSpeed  -= fAdjustSpeed;
 
       // 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 HekKinJointWristRot::nlslowToStop ( SyncMoveMsgs & msg )

protectedvirtual

Slow to stop (no lock version).

Since the wrist rotation is coupled to wrist pitch, if the pitch is moving, so must the rotation. Slowing down must take this coupled action into account.

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 from hekateros::HekKinJoint.

Definition at line 1618 of file hekKinJoint.cxx.

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

 {
   double  fAdjustVel;
   int     fAdjustSpeed;
   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);
 
     if( isCoupledJointMoving() )
     {
       m_pKinJointWristPitch->getTgtVelSpeed(fAdjustVel, fAdjustSpeed);
     }
     else
     {
       fAdjustVel    = 0.0;
       fAdjustSpeed  = 0;
     }
 
     nServoStopSpeed -= fAdjustSpeed;
 
     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;
   }
 }

HekKinJointWristRot& hekateros::HekKinJointWristRot::operator= ( const HekKinJointWristRot & rhs )

inline

Assignment operator.

Parameters

rhs	Right hand side object.

Returns: *this

Definition at line 1185 of file hekKinJoint.h.

References m_eStateWristPitch, and m_pKinJointWristPitch.

     {
       m_pKinJointWristPitch = rhs.m_pKinJointWristPitch;
       m_eStateWristPitch    = rhs.m_eStateWristPitch;
 
       return *this;
     }

HekKinJoint::MoveState HekKinJointWristRot::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 from hekateros::HekKinJoint.

Definition at line 1444 of file hekKinJoint.cxx.

 {
   double  fAdjustVel;
   int     fAdjustSpeed;
 
   if( m_pServo == NULL )
   {
     return MoveStateIdle;
   }
   else if( !bIsControlling )
   {
     return m_eState;
   }
 
   lock();
 
   m_eStateWristPitch = getCoupledJointState();
 
   if( isCoupledJointMoving() )
   {
     m_pKinJointWristPitch->getTgtVelSpeed(fAdjustVel, fAdjustSpeed);
   }
   else
   {
     fAdjustVel    = 0.0;
     fAdjustSpeed  = 0;
   }
 
   if( isMovingToGoal() || isCoupledJointMoving() )
   {
     // servo goal od position can change with coupled joints - recalculate
     m_nServoGoalPos = jointPosToServoPos(m_fJointGoalPos);
 
     if( isMovingToGoal() )
     {
       // PID target velocity 
       m_fJointTgtVel = m_pid.control(m_fJointCurPos, m_fJointCurVel, m_fDt);
 
       // estimate corresponding target servo speed
       m_nServoTgtSpeed  = estimateServoTgtSpeed(m_fJointTgtVel);
     }
     else
     {
       m_fJointTgtVel    = 0.0;
       m_nServoTgtSpeed  = 0;
     }
 
     m_nServoTgtSpeed  -= fAdjustSpeed;
 
     // still away from goal position
     if( ( (fabs(m_fJointGoalPos-m_fJointCurPos) > m_fTolPos) ||
           isCoupledJointMoving() ) &&
         canMoveInPlannedDir(m_nServoTgtSpeed) )
     {
       // new move - make it happen
       if( (m_eState == MoveStateNewMove) ||
           (m_eStateWristPitch == 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 HekKinJointWristRot::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 from hekateros::HekKinJoint.

Definition at line 1383 of file hekKinJoint.cxx.

 {
   if( !bIsControlling )
   {
     return HEK_OK;
   }
 
   lock();
 
   HekKinJoint::MoveState eStatePrev = m_eStateWristPitch;
 
   m_eStateWristPitch = getCoupledJointState();
 
   //
   // Wrist rotation is not moving and wrist pitch has transitioned from moving
   // to stop. Respecify wrist rotation goal move to avoid minor wrist rotation
   // drift.
   //
   if( isStopped() )
   {
     if( isCoupledJointStopped() && (eStatePrev != MoveStateIdle) )
     {
       //fprintf(stderr, "DBG: %s() isStopped - respecify move\n", LOGFUNCNAME);
       nlspecifyMove(m_fJointGoalPos, m_fJointGoalVel);
     }
   }
 
   //
   // Joint is slowing to a stop.
   //
   else if( isStopping() )
   {
     //fprintf(stderr, "DBG: %s() isStopping\n", LOGFUNCNAME);
     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();
     //fprintf(stderr, "DBG: %s() stop\n", LOGFUNCNAME);
   }
 
   //
   // In over torque condition, apply torque control.
   //
   if( m_bOverTorqueCond )
   {
     nlapplyTorqueControl();
   }
 
   unlock();
 
   return HEK_OK;
 }

double HekKinJointWristRot::servoPosToJointPos ( int nOdPos )

virtual

Convert servo position to the equivalent joint position.

Adjust for wrist pitch.

Parameters

nOdPos Servo position (odometer ticks).

Returns: Equivalent joint position (radian).

Reimplemented from hekateros::HekKinJoint.

Definition at line 1681 of file hekKinJoint.cxx.

References hekateros::HekKinJoint::m_fJointRadPerTick.

 {
   double  fAdjustPos = 0, fAdjustVel = 0;
   double  fJointPos;
 
   if( m_pKinJointWristPitch != NULL )
   {
     m_pKinJointWristPitch->getJointCurPosVel(fAdjustPos, fAdjustVel);
   }
 
   fJointPos = (double)nOdPos * m_fJointRadPerTick;
 
   return fJointPos - fAdjustPos;
 }

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

protectedvirtual

Update joint velocity - servo speed association.

Adjustment is made for wrist pitch current velocity.

Parameters

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

Reimplemented from hekateros::HekKinJoint.

Definition at line 1696 of file hekKinJoint.cxx.

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

 {
   double  fAdjustPos = 0.0, fAdjustVel = 0.0;
 
   if( m_pKinJointWristPitch != NULL )
   {
     m_pKinJointWristPitch->getJointCurPosVel(fAdjustPos, fAdjustVel);
   }
 
   // pure wrist rotation move only
   if( (fAdjustVel == 0.0) && (getCoupledJointState() == MoveStateIdle) )
   {
     m_tblVelSpeed.update(fJointVel, nServoSpeed);
   }
 }

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

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

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation