laelaps::LaePlatform Class Reference

Robot platform control and state data class. More...

#include <laePlatform.h>

Public Types
enum	Idx {   IdxPrev = 0,   IdxCur = 1,   HistSize = 2 }
	Working indices and sizes. More...

Public Member Functions
	LaePlatform ()
	Default constructor.
	LaePlatform (const LaePlatform &src)
	Copy constructor.
	~LaePlatform ()
	Destructor.
LaePlatform	operator= (const LaePlatform &rhs)
	Assignment operator. More...
void	clear ()
	Clear data sans name.
std::string	getPlatformName () const
	Get robot platform name. More...
virtual int	configure (const LaeDescBase &desc)
	Configure robot platform from product description. More...
virtual int	configure (const LaeTunes &tunes)
	Configure platform. More...
virtual int	reload (const LaeTunes &tunes)
	Reload tuning parameters and re-configure. More...
virtual int	resetOdometer ()
	Reset odometry to zero. More...
virtual int	updateStateDynamics (const LaeMapPowertrain &mapPowertrains)
	Update robot platform state dynamics. More...
virtual int	updateCtlrHealth (int nCtlr, double fVolts, double fTemp, uint_t uStatus)
	Update robot platform motor controller health state. More...
virtual int	updateHealth (const LaeMapPowertrain &mapPowertrains)
	Update robot platform health state. More...
virtual void	clearPoses ()
	Clear pose history.
virtual void	pushNewPose (const LaePose &pose)
	Push a new pose on the pose history. More...

Public Attributes
std::string	m_strName
	robot platform name
Dim	m_dimRobot
	total dimensions of robot (TODO)
Dim	m_dimBody
	dimensions of robot body
double	m_fWheelbase
	wheelbase (m)
double	m_fWheeltrack
	wheeltrack (m)
double	m_fPosLast [LaeMotorsNumOf]
	last sensed wheel positions (radians)
LaePose	m_pose [HistSize]
	robot pose (meters, meters, radians)
double	m_fOdometer
	robot odometer (meters)
double	m_fVelocity
	robot velocity (meters/second)
double	m_fAmpsMotors
	total draw from all motors (A)
double	m_fVoltsAvg
	average sensed battery voltage (V)
double	m_fTempAvg
	average sensed interior tempature (C)
LaeMotorCtlrState	m_ctlr [LaeNumMotorCtlrs]
	motor controllers' state

Detailed Description

Robot platform control and state data class.

Definition at line 127 of file laePlatform.h.

Member Enumeration Documentation

enum laelaps::LaePlatform::Idx

Working indices and sizes.

Enumerator
IdxPrev	index of previous history
IdxCur	index of current state
HistSize	size of history

Definition at line 133 of file laePlatform.h.

    {
      IdxPrev   = 0,    ///< index of previous history
      IdxCur    = 1,    ///< index of current state
      HistSize  = 2     ///< size of history
    };

Member Function Documentation

int LaePlatform::configure ( const LaeDescBase &  desc )

virtual

Configure robot platform from product description.

Parameters

desc	Laelaps robot base product description data.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 218 of file laePlatform.cxx.

References laelaps::LaeDescBase::m_dimBody, m_dimBody, laelaps::LaeDescBase::m_dimRobot, m_dimRobot, laelaps::LaeDescBase::m_fWheelbase, m_fWheelbase, laelaps::LaeDescBase::m_fWheeltrack, and m_fWheeltrack.

Referenced by reload().

{
  // base kinematics
  m_dimBody     = desc.m_dimBody;
  m_dimRobot    = desc.m_dimRobot;
  m_fWheelbase  = desc.m_fWheelbase;
  m_fWheeltrack = desc.m_fWheeltrack;
}

int LaePlatform::configure ( const LaeTunes &  tunes )

virtual

Configure platform.

Parameters

tunes

Laelaps tuning parameters.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 227 of file laePlatform.cxx.

References laelaps::LaeDescBase::calcDimensions(), laelaps::LaeTunes::getTireDimParams(), laelaps::LAE_OK, laelaps::LaeKeyLeftFront, laelaps::LaeDescBase::m_dimBody, m_dimBody, laelaps::LaeDescBase::m_dimRobot, m_dimRobot, laelaps::LaeDescBase::m_fWheelbase, m_fWheelbase, laelaps::LaeDescBase::m_fWheeltrack, and m_fWheeltrack.

{
  LaeDescBase desc;
  double      fTuneTireRadius, fTuneTireWidth;      // tire dimensions

  // assumes all tires are the same dimensions
  tunes.getTireDimParams(LaeKeyLeftFront, fTuneTireRadius, fTuneTireWidth);

  desc.calcDimensions(fTuneTireRadius, fTuneTireWidth);

  m_dimBody     = desc.m_dimBody;
  m_dimRobot    = desc.m_dimRobot;
  m_fWheelbase  = desc.m_fWheelbase;
  m_fWheeltrack = desc.m_fWheeltrack;

  return LAE_OK;
}

std::string laelaps::LaePlatform::getPlatformName ( ) const

inline

Get robot platform name.

Returns

String.

Definition at line 199 of file laePlatform.h.

References laelaps::LaeMotorCtlrState::m_strName.

    {
      return m_strName;
    }

LaePlatform LaePlatform::operator=

(

const LaePlatform &

rhs

)

Assignment operator.

Parameters

rhs	Right hand side object.

Returns

Returns copy of this.

Definition at line 149 of file laePlatform.cxx.

References HistSize, laelaps::LaeMotorsNumOf, laelaps::LaeNumMotorCtlrs, m_ctlr, m_dimBody, m_dimRobot, m_fAmpsMotors, m_fOdometer, m_fPosLast, m_fTempAvg, m_fVelocity, m_fVoltsAvg, m_fWheelbase, m_fWheeltrack, m_pose, and m_strName.

{
  int   nCtlr;
  int   i;

  m_strName       = rhs.m_strName;
  
  m_dimRobot      = rhs.m_dimRobot;
  m_dimBody       = rhs.m_dimBody;

  m_fWheelbase    = rhs.m_fWheelbase;
  m_fWheeltrack   = rhs.m_fWheeltrack;

  for(i = 0; i < LaeMotorsNumOf; ++i)
  {
    m_fPosLast[i] = rhs.m_fPosLast[i];
  }

  for(i = 0; i < HistSize; ++i)
  {
    m_pose[i] = rhs.m_pose[i];
  }

  m_fOdometer     = rhs.m_fOdometer;
  m_fVelocity     = rhs.m_fVelocity;

  m_fAmpsMotors   = rhs.m_fAmpsMotors;
  m_fVoltsAvg     = rhs.m_fVoltsAvg;
  m_fTempAvg      = rhs.m_fTempAvg;

  for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
  {
    m_ctlr[nCtlr] = rhs.m_ctlr[nCtlr];
  }

  return *this;
}

void LaePlatform::pushNewPose ( const LaePose &  pose )

virtual

Push a new pose on the pose history.

The history is aged first.

Parameters

pose

New pose.

Definition at line 471 of file laePlatform.cxx.

References IdxCur, IdxPrev, and m_pose.

Referenced by updateStateDynamics().

{
  // save pose
  m_pose[IdxPrev] = m_pose[IdxCur];

  // clear new pose
  m_pose[IdxCur] = pose;
}

int LaePlatform::reload ( const LaeTunes &  tunes )

virtual

Reload tuning parameters and re-configure.

Parameters

tunes

Laelaps tuning parameters.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 245 of file laePlatform.cxx.

References configure().

{
  return configure(tunes);
}

int LaePlatform::resetOdometer ( )

virtual

Reset odometry to zero.

All related positioning data are reset to 0.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 250 of file laePlatform.cxx.

References clearPoses(), laelaps::LaeMotorsNumOf, m_fOdometer, and m_fPosLast.

{
  int   i;

  clearPoses();

  for(i = 0; i < LaeMotorsNumOf; ++i)
  {
    m_fPosLast[i] = 0.0;
  }

  m_fOdometer = 0.0;
}

int LaePlatform::updateCtlrHealth ( int  nCtlr, double  fVolts, double  fTemp, uint_t  uStatus  )

virtual

Update robot platform motor controller health state.

Parameters

nCtlr	Motor controller id.
fVolts	Motor input voltage (V).
fTemp	Motor controller board voltage (V).
uCtlrStatus	Motor controller status bits.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 414 of file laePlatform.cxx.

References laelaps::LAE_ECODE_BAD_VAL, laelaps::LAE_OK, laelaps::LaeNumMotorCtlrs, m_ctlr, laelaps::LaeMotorCtlrState::m_fBoardTemp, laelaps::LaeMotorCtlrState::m_fMainVolts, and laelaps::LaeMotorCtlrState::m_uStatus.

{
  if( (nCtlr < 0) || (nCtlr >= LaeNumMotorCtlrs) )
  {
    LOGERROR("Motor controller id %d: Out-of-range.", nCtlr);
    return -LAE_ECODE_BAD_VAL;
  }

  m_ctlr[nCtlr].m_fMainVolts = fVolts;
  m_ctlr[nCtlr].m_fBoardTemp = fTemp;
  m_ctlr[nCtlr].m_uStatus    = uStatus;

  return LAE_OK;
}

int LaePlatform::updateHealth ( const LaeMapPowertrain &  mapPowertrains )

virtual

Update robot platform health state.

Parameters

mapPowertrains

Powertrain kinodynamics.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 432 of file laePlatform.cxx.

References laelaps::LAE_OK, laelaps::LaePowertrainState::m_fAmps, m_fAmpsMotors, laelaps::LaePowertrainState::m_fTemp, laelaps::LaeDbRobotStatus::m_fTempAvg, m_fTempAvg, laelaps::LaePowertrainState::m_fVolts, m_fVoltsAvg, laelaps::LaeDb::m_robotstatus, laelaps::LaePowertrain::m_state, and laelaps::RtDb.

{
  LaeMapPowertrain::const_iterator iter;

  int   n;

  m_fAmpsMotors = 0.0;
  m_fVoltsAvg   = 0.0;
  m_fTempAvg    = 0.0;

  n = 0;

  for(iter = mapPowertrains.begin(); iter != mapPowertrains.end(); ++iter)
  {
    const LaePowertrain &train = iter->second;

    m_fAmpsMotors += train.m_state.m_fAmps;
    m_fVoltsAvg   += train.m_state.m_fVolts;
    m_fTempAvg    += train.m_state.m_fTemp;

    ++n;
  }

  m_fVoltsAvg /= (double)n;
  m_fTempAvg  /= (double)n;

  RtDb.m_robotstatus.m_fTempAvg = m_fTempAvg;

  return LAE_OK;
}

int LaePlatform::updateStateDynamics ( const LaeMapPowertrain &  mapPowertrains )

virtual

Update robot platform state dynamics.

Parameters

mapPowertrains

Powertrain kinodynamics.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 264 of file laePlatform.cxx.

References IdxCur, IdxPrev, laelaps::LAE_OK, laelaps::LaeMotorIdLF, laelaps::LaeMotorIdLR, laelaps::LaeMotorIdRF, laelaps::LaeMotorIdRR, laelaps::LaeMotorsNumOf, laelaps::LaePowertrain::m_attr, laelaps::LaePowertrainAttr::m_fMetersPerRadian, m_fOdometer, laelaps::LaeDbKinRobot::m_fOdometer, laelaps::LaePowertrainState::m_fPosition, m_fPosLast, m_fVelocity, laelaps::LaePowertrainState::m_fVelocity, laelaps::LaeDbKinRobot::m_fVelocity, m_fWheelbase, laelaps::LaeDb::m_kin, laelaps::LaePowertrainAttr::m_nMotorId, m_pose, laelaps::LaeDbKin::m_robot, laelaps::LaePowertrain::m_state, M_TAU, laelaps::LaePose::m_theta, laelaps::LaeDbKinRobot::m_theta, laelaps::LaePose::m_x, laelaps::LaeDbKinRobot::m_x, laelaps::LaePose::m_y, laelaps::LaeDbKinRobot::m_y, pushNewPose(), and laelaps::RtDb.

{
  static double ReallyZero = 1e-4;

  LaeMapPowertrain::const_iterator iter;

  int     nMotorId;                   // motor id
  double  fDeltaPos;                  // delta position last position (radians)
  double  fDeltaDist[LaeMotorsNumOf]; // delta wheel distances (meters)
  double  fDeltaOd;                   // delta odometer (meters)
  double  fDistL, fDistR;             // left and right average delta distance
  double  s;                          // delta arc length
  double  r;                          // radius of curvature
  double  c;                          // central angle
  double  dx, dy, da;                 // delta pose (m, m, radians)
  LaePose pose;                       // new pose

  // zero current velocity, delta odometer
  m_fVelocity = 0.0;
  fDeltaOd    = 0.0;

  //
  // Process powertrain kinodynamics to arrive at platform dynamics.
  //
  for(iter = mapPowertrains.begin(); iter != mapPowertrains.end(); ++iter)
  {
    const LaePowertrain &train = iter->second;

    nMotorId = train.m_attr.m_nMotorId;

    // delta position (radians) and delta distance (meters)
    fDeltaPos  = train.m_state.m_fPosition - m_fPosLast[nMotorId];
    m_fPosLast[nMotorId] = train.m_state.m_fPosition;
    fDeltaDist[nMotorId] = fDeltaPos * train.m_attr.m_fMetersPerRadian;

    // sum delta odometer distances
    fDeltaOd += fDeltaDist[nMotorId];

    // sum wheel velocity (radians/s) to linear velocity (meters/s)
    m_fVelocity += train.m_state.m_fVelocity * train.m_attr.m_fMetersPerRadian;
  }

  //
  // Average the odometer delta sum value and accumulate to attain meters 
  // traveled at the platform center of rotation. Note that this odometer is
  // the absolute distance traveled.
  //
  fDeltaOd = fabs(fDeltaOd / (double)LaeMotorsNumOf);
  m_fOdometer += fDeltaOd;

  //
  // Average the velocity sum. The velocity is calculated from the platform
  // center of rotation. Note that this velocity is the absolute value.
  //
  m_fVelocity = fabs(m_fVelocity / (double)LaeMotorsNumOf);

  //
  // Average the left and right side delta distances traveled.
  //
  fDistL = (fDeltaDist[LaeMotorIdLF] + fDeltaDist[LaeMotorIdLR]) / 2.0;
  fDistR = (fDeltaDist[LaeMotorIdRF] + fDeltaDist[LaeMotorIdRR]) / 2.0;

  //
  // Calculate the new absolute orientation.
  //
  // The new theta can be calculated directly from current odometry.
  //
  // Eq. s = r * c, where
  //
  // s = arc length (meters)
  // r = radius of curvature (meters) == wheel base
  // c = central angle (radians)
  //
  s = (fDistR + fDistL) / 2.0;
  c = (fDistR - fDistL) / m_fWheelbase;

  //
  // No linear component (i.e. spin or stopped).
  //
  if( fabs(s) <= ReallyZero )
  {
    dx = 0.0;
    dy = 0.0;
    da = c;
  }

  //
  // Degenerate curve: straight path.
  //
  // The arc length is a straigt line.
  //
  else if( fabs(c) <= ReallyZero )
  {
    dx = s;
    dy = 0.0;
    da = 0.0;
  }

  //
  // Circular arc path.
  //
  // Note: The following comments are nice, but the calculations simplify when
  // delta distances are used.
  //
  // The center of rotation is a pi/2 rotation from the robot frame of
  // reference. Rotating 90 degrees and translating the center to 0,0, we
  // can calculate the delta position. From the deltas, the current absolute
  // position is determined.
  //
  // Using trig identities:
  //   sin(-u) = -sin(u)
  //   cos(-u) =  cos(u)
  //   sin(u) = cos(theta)
  //   cos(u) = sin(theta)
  //
  // Let  u = pi/2 - theta
  //     -u = theta - pi/2 
  // Then:
  //   dx = r * (m.cos(theta[cur]-m.pi/2) - m.cos(theta[prev]-m.pi/2))
  //   dy = r * (m.sin(theta[cur]-m.pi/2) - m.sin(theta[prev]-m.pi/2))
  //
  else
  {
    r  = s / c;
    dx = r * sin(c);
    dy = r * -cos(c);
    da = c;
  }

  //
  // New current pose.
  //
  pose.m_x      = m_pose[IdxPrev].m_x + dx;
  pose.m_y      = m_pose[IdxPrev].m_y + dy;
  pose.m_theta  = fmod(m_pose[IdxPrev].m_theta + da, M_TAU);

  pushNewPose(pose);

  //
  // Save to real-time DB.
  //
  RtDb.m_kin.m_robot.m_x          = m_pose[IdxCur].m_x;
  RtDb.m_kin.m_robot.m_y          = m_pose[IdxCur].m_y;
  RtDb.m_kin.m_robot.m_theta      = m_pose[IdxCur].m_theta;
  RtDb.m_kin.m_robot.m_fOdometer  = m_fOdometer;
  RtDb.m_kin.m_robot.m_fVelocity  = m_fVelocity;

  return LAE_OK;
}

---

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

• include/Laelaps/laePlatform.h
• sw/liblaelaps/laePlatform.cxx