hekateros::VelSpeedLookupTbl Class Reference

Joint velocity/ servo speed lookup table class. More...

#include <hekKinJoint.h>

Public Types
typedef std::vector< VelSpeedTuple >	VelSpeedTbl
	Velocity-speed table type.

Public Member Functions
	VelSpeedLookupTbl ()
	Default constructor.
	~VelSpeedLookupTbl ()
	Destructor.
VelSpeedLookupTbl	operator= (const VelSpeedLookupTbl &rhs)
	Assignment operator. More...
void	create (double fJointMaxVel, double fBucketSize)
	Create lookup table. More...
int	hash (double fJointVel)
	Hash joint velocity to table index. More...
void	update (double fJointVel, int nServoSpeed)
	Update lookup table entry. More...
int	estimate (double fJointGoalVel)
	Estimate servo goal speed. More...
int	getLookupTableSize ()
	Get the lookup table fixed size. More...
int	getNumLookupTableEntries ()
	Get the number of populated entries in lookup table. More...
void	enableDebugging (const std::string &strId)
	Enable table debugging. More...
void	disableDebugging ()
	Disable table debugging.
void	dumpTable (const std::string &strId)
	Dump lookup table entries. More...

Static Public Attributes
static const int	NO_ENTRY = -1
	No table entry value.
static const int	MIN_NON_ZERO_SPEED = 10
	Minimum servo speed for non-zero velocities.

Protected Attributes
double	m_fBucketSize
	table entry size (degrees/second)
int	m_nMaxIndex
	maximum index into table
int	m_nNumEntries
	number of populated table entries.
VelSpeedTbl	m_tbl
	dynamic interpolation table
std::string	m_strDbgId
	debugging id
bool	m_bDbg
	enable [disable] debugging

Detailed Description

Joint velocity/ servo speed lookup table class.

A table of associated joint velocities to servo speed is dynamically built and updated to provide:

• Good estimates of servo speed given the goal velocity through linear interpolation.
• Adaptive kinodynamics.

Definition at line 249 of file hekKinJoint.h.

Member Function Documentation

void VelSpeedLookupTbl::create	(	double	fJointMaxVel,
		double	fBucketSize
	)

Create lookup table.

Parameters

fJointMaxVel	Joint maximum velocity (radians/second).
fBucketSize	Table bucket size (radians/second).

Definition at line 197 of file hekKinJoint.cxx.

Referenced by hekateros::HekKinJoint::HekKinJoint().

{
  size_t        nElems;
  VelSpeedTuple noentry(0.0, NO_ENTRY);

  fJointMaxVel  = fabs(fJointMaxVel);
  m_fBucketSize = fabs(fBucketSize);

  nElems = (int)(fJointMaxVel / m_fBucketSize);
  if( (double)nElems * m_fBucketSize < fJointMaxVel )
  {
    ++nElems; // round up
  }
  ++nElems; // include zero entry

  m_tbl.insert(m_tbl.begin(), nElems, noentry);

  m_nMaxIndex = (int)m_tbl.size() - 1;

  m_tbl[0]            = VelSpeedTuple(0.0, 0);
  m_tbl[m_nMaxIndex]  = VelSpeedTuple(fJointMaxVel, DYNA_SPEED_MAX_RAW);

  m_nNumEntries = 2;

  if( m_bDbg )
  {
    fprintf(stderr, "DBG: %s(): %s: tbl_size=%zu, num_entries=%d\n",
      LOGFUNCNAME,
      m_strDbgId.c_str(), m_tbl.size(), m_nNumEntries);
  }
}

void VelSpeedLookupTbl::dumpTable

(

const std::string &

strId

)

Dump lookup table entries.

Debugging is printed to stderr.

Parameters

strId

Debug identification string.

Definition at line 374 of file hekKinJoint.cxx.

References hekateros::radToDeg().

{
  fprintf(stderr, "DBG: %s: bucket_size=%.2lf deg, lookup table[%zu] =\n{\n",
      strId.c_str(), radToDeg(m_fBucketSize), m_tbl.size());

  for(int i=0; i<m_tbl.size(); ++i)
  {
    if( m_tbl[i].m_nServoSpeed != NO_ENTRY )
    {
      fprintf(stderr, "  [%3d]: vel=%7.3lf (%6.2lf deg), speed=%4d\n",
        i, m_tbl[i].m_fJointVel, radToDeg(m_tbl[i].m_fJointVel),
        m_tbl[i].m_nServoSpeed);
    }
  }
  fprintf(stderr, "}\n");
}

void VelSpeedLookupTbl::enableDebugging

(

const std::string &

strId

)

Enable table debugging.

Debugging is printed to stderr.

Parameters

strId

Debug identification string.

Definition at line 363 of file hekKinJoint.cxx.

{
  m_strDbgId = strId;
  m_bDbg = true;
}

int VelSpeedLookupTbl::estimate

(

double

fJointGoalVel

)

Estimate servo goal speed.

Linear interpolation is performed between the two nearest neighbors in the lookup table.

Parameters

fJointGoalVel

Joint goal velocity (radians/second).

Returns

Servo speed estimate.

Definition at line 266 of file hekKinJoint.cxx.

Referenced by hekateros::HekKinJoint::estimateServoTgtSpeed().

{
  double  fJointAbsVel = fabs(fJointGoalVel);

  // zero is 0
  if( fJointAbsVel == 0.0 )
  {
    return m_tbl[0].m_nServoSpeed;
  }
  // maximum velocity
  else if( fJointAbsVel >= m_tbl[m_nMaxIndex].m_fJointVel )
  {
    return m_tbl[m_nMaxIndex].m_nServoSpeed;
  }

  double  fMinVel;
  int     nMinSpeed;
  double  fMaxVel;
  int     nMaxSpeed;

  int     index = hash(fJointAbsVel);

  // no entry at hashed location
  if( m_tbl[index].m_nServoSpeed == NO_ENTRY )
  {
    nMinSpeed = NO_ENTRY;
    nMaxSpeed = NO_ENTRY;
  }
  // go lucky - no interpolation needed
  else if( m_tbl[index].m_fJointVel == fJointAbsVel )
  {
    return m_tbl[index].m_nServoSpeed;
  }
  // hashed location is minimum neighbor
  else if( m_tbl[index].m_fJointVel < fJointAbsVel )
  {
    fMinVel   = m_tbl[index].m_fJointVel;
    nMinSpeed = m_tbl[index].m_nServoSpeed;
    nMaxSpeed = NO_ENTRY;
  }
  // hashed location is maximum neighbor
  else
  {
    nMinSpeed = NO_ENTRY;
    fMaxVel   = m_tbl[index].m_fJointVel;
    nMaxSpeed = m_tbl[index].m_nServoSpeed;
  }

  // no minimum neighbor - search downward
  if( nMinSpeed == NO_ENTRY )
  {
    for(int i = index-1; i >= 0; --i)
    {
      if( m_tbl[i].m_nServoSpeed == NO_ENTRY )
      {
        continue;
      }
      else if( m_tbl[i].m_fJointVel < fJointAbsVel )
      {
        fMinVel   = m_tbl[i].m_fJointVel;
        nMinSpeed = m_tbl[i].m_nServoSpeed;
        break;
      }
    }
  }
  if( nMinSpeed == NO_ENTRY )
  {
    return m_tbl[0].m_nServoSpeed;
  }

  // no maximum neighbor - search upward
  if( nMaxSpeed == NO_ENTRY )
  {
    for(int i = index+1; i <= m_nMaxIndex; ++i)
    {
      if( m_tbl[i].m_nServoSpeed == NO_ENTRY )
      {
        continue;
      }
      else if( m_tbl[i].m_fJointVel > fJointAbsVel )
      {
        fMaxVel   = m_tbl[i].m_fJointVel;
        nMaxSpeed = m_tbl[i].m_nServoSpeed;
        break;
      }
    }
  }
  if( nMaxSpeed == NO_ENTRY )
  {
    return m_tbl[m_nMaxIndex].m_nServoSpeed;
  }

  // interpolate
  return (int)((double)nMinSpeed + fabs(nMaxSpeed - nMinSpeed) * 
                      (fJointAbsVel - fMinVel) / (fMaxVel - fMinVel));
}

int hekateros::VelSpeedLookupTbl::getLookupTableSize ( )

inline

Get the lookup table fixed size.

Returns

int

Definition at line 324 of file hekKinJoint.h.

    {
      return (int)m_tbl.size();
    }

int hekateros::VelSpeedLookupTbl::getNumLookupTableEntries ( )

inline

Get the number of populated entries in lookup table.

Returns

int

Definition at line 334 of file hekKinJoint.h.

    {
      return m_nNumEntries;
    }

int VelSpeedLookupTbl::hash

(

double

fJointVel

)

Hash joint velocity to table index.

Parameters

fJointVel

Joint velocity (radians/second).

Returns

Table index.

Definition at line 229 of file hekKinJoint.cxx.

References hekateros::cap().

{
  int index = (int)(fabs(fJointVel)/m_fBucketSize);
  return cap(index, 0, m_nMaxIndex);
}

VelSpeedLookupTbl VelSpeedLookupTbl::operator=

(

const VelSpeedLookupTbl &

rhs

)

Assignment operator.

Parameters

rhs	Right hand side object.

Returns

Returns copy of this.

Definition at line 187 of file hekKinJoint.cxx.

References m_bDbg, m_fBucketSize, m_nMaxIndex, m_nNumEntries, m_strDbgId, and m_tbl.

{
  m_fBucketSize = rhs.m_fBucketSize;
  m_nMaxIndex   = rhs.m_nMaxIndex;
  m_nNumEntries = rhs.m_nNumEntries;
  m_tbl         = rhs.m_tbl;
  m_strDbgId    = rhs.m_strDbgId;
  m_bDbg        = rhs.m_bDbg;
}

void VelSpeedLookupTbl::update	(	double	fJointVel,
		int	nServoSpeed
	)

Update lookup table entry.

Parameters

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

Definition at line 235 of file hekKinJoint.cxx.

References hekateros::iabs(), and hekateros::radToDeg().

Referenced by hekateros::HekKinJoint::updateAssoc(), and hekateros::HekKinJointWristRot::updateAssoc().

{
  int index = hash(fJointVel);

  if( (index > 0) && (index < m_nMaxIndex) )
  {
    int nOldCnt = m_nNumEntries;

    if( m_tbl[index].m_nServoSpeed == NO_ENTRY )
    {
      ++m_nNumEntries;
    }

    m_tbl[index].m_fJointVel    = fabs(fJointVel);
    m_tbl[index].m_nServoSpeed  = iabs(nServoSpeed);
    if( m_tbl[index].m_nServoSpeed < MIN_NON_ZERO_SPEED )
    {
      m_tbl[index].m_nServoSpeed = MIN_NON_ZERO_SPEED;
    }

    if( m_bDbg && (m_nNumEntries != nOldCnt) )
    {
      fprintf(stderr, "DBG: %s(): %s: "
                  "num_entries=%d, index=%d, vel=%lf, speed=%d\n",
        LOGFUNCNAME,
        m_strDbgId.c_str(), m_nNumEntries, index,
        radToDeg(m_tbl[index].m_fJointVel), m_tbl[index].m_nServoSpeed);
    }
  }
}

---

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

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