Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

WorldModel2.cc

Go to the documentation of this file.

/*
 * Tekkotsu framework class for the Second Generation World Model.
 * This class is the glue that connects all of the representations in
 * the 2GWM with the rest of Tekkotsu. It implements EventListener so it
 * can receive motion and video events; it maintains a queue of actions
 * that the AIBO can elect to perform to improve the certainty of the
 * world representations; and it also has access functions that allow
 * other code to get at the contents of the world model.
 *
 * This code checks for the definition of WANT_GLOBAL_MAP. If WANT_GLOBAL_MAP
 * is enabled, then global mapping functionality (FastSLAM, the global height
 * map) is compiled in. If not, these parts are omitted.
 */

// include me
#include "WorldModel2.h"

// include STL, Tekkotsu, and other basics
#include <cmath>
#include <vector>
#include <deque>
#include "Events/EventRouter.h"
#include "Events/VisionEvent.h"
#include "Events/LocomotionEvent.h"
#include "Shared/WorldState.h"
#include "Vision/Vision.h"
#include "Wireless/Wireless.h"
#include "Shared/Config.h"

// include WorldModel2 particulars
#include "FastSLAM/afsMain.h"
#include "Maps/almMain.h"
#include "Maps/agmMain.h"
#include "Maps/almStructures.h"
// needed for AFS_NUM_LANDMARKS, AFS_NUM_PARTICLES
#include "FastSLAM/Configuration.h"
// needed for afsParticle
#include "FastSLAM/afsParticle.h"
// needed for pinhole camera model parameters
#include "Maps/Configuration.h"
// Standing robot parameters. TODO--replace with motion model
#include "Poses.h"

// Timing to time our moves
#include "Shared/get_time.h"

// use perl;
#define UNLESS(item)  if(!( (item) ))

// debugging?
//#define DEBUG_WM2

// Timer SIDs
#define TIMER_SID_GPA 0 // sid for ground plane assumption timer
#define TIMER_SID_SRL 1 // sid for serializer

using namespace std;


  // Start everything out with WorldMap2 resource lock flag (see
  // WorldModel2.h) unlocked
bool WorldModel2::locked = false;

  // Constructor. The constructor must initialize all of the world model
  // representations
WorldModel2::WorldModel2()
      // We start with no kludges enabled. This is known to all my "homies"
      // as "keeping it real".
  : kludges(0),
      // NOTE: moving is true because we don't know our state on initialization
    moving(true),
      // Set all enabled sensing mechanisms to false
    enabledIR(false), enabledGPA(false),
      // Set current velocities and travel time start to 0.
      // NOTE: 0 is a special value for movingSince--see WorldModel2.h
    dx(0.0), dy(0.0), da(0.0), movingSince(0),
      // Initialize sockets for outbound communication. Note strikingly
      // unintelligent outbound buffer sizes--this could lead to
      // problems if we ever get great big maps.
      // We're also assuming that the space used by cells in the maps is equal
      // to or greater than the amount of room you need to transmit cell data
      // over the ether.
    sockDM(wireless->socket(SocketNS::SOCK_STREAM, 1024,
          DM_CELL_COUNT*sizeof(dm_cell))),
    sockHM(wireless->socket(SocketNS::SOCK_STREAM, 1024,
          HM_CELL_COUNT*sizeof(hm_cell)))
#ifdef WANT_GLOBAL_MAP
    ,
    sockGM(wireless->socket(SocketNS::SOCK_STREAM, 1024,
          GM_CELL_COUNT*sizeof(hm_cell))),
    sockFS(wireless->socket(SocketNS::SOCK_STREAM, 1024,
    AFS_NUM_PARTICLES*sizeof(afsPose) + sizeof(afsParticle))),
    enabledMarkers(false),
    fs_updates()
#endif
{
    // Make sure the world model resources aren't owned by someone else.
  if(locked) return; // TODO: throw an exception?
    // OK, claim the resources for ourselves
  locked = true;

#ifdef WANT_GLOBAL_MAP
    // Clear sensor processing checklist flags (see WorldModel2.h)
  for(int i=0; i<AFS_NUM_LANDMARKS; ++i) haveSeenMarker[i] = false;
#endif

    // Start listening on output ports
  wireless->listen(sockDM->sock, config->worldmodel2.dm_port);
  wireless->listen(sockHM->sock, config->worldmodel2.hm_port);
#ifdef WANT_GLOBAL_MAP
  wireless->listen(sockGM->sock, config->worldmodel2.gm_port);
  wireless->listen(sockFS->sock, config->worldmodel2.fs_port);
#endif

    // We just set the world model to the start state.
  resetWorldModel();

    // Subscribe to locomotion events
  erouter->addListener(this,EventBase::locomotionEGID);

    // Turn on serialization timer
  erouter->addTimer(this, TIMER_SID_SRL, SRLdelay, true);
}

  // Destructor
WorldModel2::~WorldModel2()
{
    // Cancel subscriptions to various events--also turns off timers
  disableIR();
  disableGPA();
#ifdef WANT_GLOBAL_MAP
  disableMarkers();
#endif
  erouter->forgetListener(this);

    // Close sockets
  wireless->close(sockDM);
  wireless->close(sockHM);
#ifdef WANT_GLOBAL_MAP
  wireless->close(sockGM);
  wireless->close(sockFS);
#endif

    // Free the resources up for someone else
  locked = false;
}



  // Enable or disable certain sensing mechanisms.
  // infrared
void WorldModel2::enableIR()
{
    // Subscribe to sensor updates so that we can know what the IR sees
  erouter->addListener(this, EventBase::sensorEGID, SensorSourceID::UpdatedSID);
  enabledIR = true;
}
void WorldModel2::disableIR()
{
    // Cancel subscription to sensor updates--we don't need them for IR anymore
  erouter->removeListener(this, EventBase::sensorEGID,
        SensorSourceID::UpdatedSID);
  enabledIR = false;
}

  // Ground plane assumption. The GPA uses timer events to tell it to check
  // periodically for the head to be in the right position to do GPA.
  // Thus, after we've enabled GPA, we need to see if we're not moving
  // and if so, turn on the GPA timer
void WorldModel2::enableGPA()
{
    // turn on GPA timer if we're not moving
    // note: checks for indicator value of 0 of movingSince
    // Timer SID 0 is for
  if(movingSince && !moving)
    erouter->addTimer(this, TIMER_SID_GPA, GPAdelay, true);

  enabledGPA = true;
}
void WorldModel2::disableGPA()
{
    // turn off the GPA timer if it's running
  erouter->removeTimer(this, TIMER_SID_GPA);

  enabledGPA = false;
}


// Enable a kludge. See the definition of the WM2Kludge namespace
// to find out what they are
void WorldModel2::enableKludge(unsigned int kludge)
{
  kludges |= kludge;
}

// Disable a kludge
void WorldModel2::disableKludge(unsigned int kludge)
{
  kludges &= ~kludge;
}


#ifdef WANT_GLOBAL_MAP
  // Marker events--for FastSLAM
void WorldModel2::enableMarkers()
{
  // We even listen to them if we're not moving
  // note: also checks for indicator value of 0 of movingSince
  if(movingSince && !moving)
    erouter->addListener(this,EventBase::visionEGID, VisionEventNS::MarkersSID);
  enabledMarkers = true;
}
void WorldModel2::disableMarkers()
{
    // Marker vision events go off
  vision->disableEvents(VisionEventNS::MarkersSID);
  erouter->removeListener(this, EventBase::visionEGID,
        VisionEventNS::MarkersSID);
  enabledMarkers = false;
}
#endif



  // Resets the world model to the start state
void WorldModel2::resetWorldModel()
{
    // Reset egocentric maps
  ALM::init();
#ifdef WANT_GLOBAL_MAP
    // Reset global maps
  AGM::init();
    // Reset FastSLAM global localizer
  afsInit();
#endif
}

#ifdef WANT_GLOBAL_MAP
  // Specify the position of a landmark (for FastSLAM)
  // just a wrapper function, really.
void WorldModel2::setLandmark(int landmark,
            double x, double y, double covariance)
{
  afsSetLandmark(landmark, x, y, covariance);
}

  // Distribute FastSLAM particles randomly (uniformly) throughout
  // a bounding box.
  // just a wrapper function, really.
  // lx = left x, ty = top y, rx = right x, bottom y = by
void WorldModel2::fsDistribute(double lx, double ty, double rx, double by)
{
  afsDistribute(lx, ty, rx, by);
}
#endif

  // Process vision and motion events
void WorldModel2::processEvent(const EventBase& event)
{
    // Find out what sort of event we have here and act accordingly
  switch(event.getGeneratorID()) {
    // process sensor data, specifically IR
  case EventBase::sensorEGID: processSensors(); break;
#ifdef WANT_GLOBAL_MAP
    // process vision data, specifically markers
  case EventBase::visionEGID: processVision(event); break;
#endif
    // process locomotion data
  case EventBase::locomotionEGID: processLocomotion(event); break;
    // either it's time to try and process ground data or it's time to
    // serialize our data
  case EventBase::timerEGID:
   if(event.getSourceID() == TIMER_SID_GPA) processGround();
    else if(event.getSourceID() == TIMER_SID_SRL) serialize();
    break;
    // dunno what this is, then
  default:
    cerr << "warning: WorldModel2 receved unrequested event" << endl;
  }
}

  // Get suggestions about what to look at next
void WorldModel2::getRequests(MRvector &requests)
{
  ALM::genRequests(requests);
#ifdef WANT_GLOBAL_MAP
  AGM::genRequests(requests);
#endif
}


  // Access methods for the spherical depth map. See WorldModel2.h for
  // extensive documentation.
void WorldModel2::pickDMData(dmPicker &p, float *dest)
{
  // Get DM data
  dm_cell *DMp = ALM::getDM();
  for(int i=0; i<DM_CELL_COUNT; ++i) dest[i] = p(DMp[i]);
}
dm_cell *WorldModel2::invadeDMData(void) { return ALM::getDM(); }

  // Access methods for the horizontal height map. See WorldModel2.h for
  // extensive documentation.
void WorldModel2::pickHMData(hmPicker &p, float *dest)
{
  // Get HM data
  hm_cell *HMp = ALM::getHM();
  for(int i=0; i<HM_CELL_COUNT; ++i) dest[i] = p(HMp[i]);
}
hm_cell *WorldModel2::invadeHMData(void) { return ALM::getHM(); }

#ifdef WANT_GLOBAL_MAP
  // Access methods for the horizontal height map. See WorldModel2.h for
  // extensive documentation.
void WorldModel2::pickGMData(hmPicker &p, float *dest)
{
  // Get HM data
  /*
  hm_cell *GMp = ALM::getGM();
  for(int i=0; i<GM_CELL_COUNT; ++i) dest[i] = p(GMp[i]);
  */
}
hm_cell *WorldModel2::invadeGMData(void) { return NULL; } //return ALM::getGM(); }
#endif


  // process sensors, specifically IRs
void WorldModel2::processSensors()
{
  // We should never be called if enabledIR is off, but here's a test anyway
  UNLESS(enabledIR) {
    cerr << "warning: WorldModel2 handling IR event w/o authorization" << endl;
    return;
  }
  // just abort if the AIBO isn't standing erect--we can't do math on the
  // IR stuff then. In the future, we'll have inverse kinematics that
  // take care of this for us.
//UNCOMMENT THIS IN REAL LIFE
  //if(!aiboIsErect()) return;

  // OK, AIBO is in the right position. Go ahead and integrate its sensor data
  double depth    = state->sensors[IRDistOffset];
  double azimuth  = state->outputs[HeadOffset+PanOffset];
  double altitude = state->outputs[HeadOffset+TiltOffset];

  ALM::registerDepth(depth, altitude, azimuth, kludges);
}

#ifdef WANT_GLOBAL_MAP
  // process vision events
void WorldModel2::processVision(const EventBase& event)
{
  // These variables are part of the same mechanism that tracks image size
  // (and hence depth of the "film plane" in our pinhole camera model)
  // used in Maps/almMain.cc
  static int curr_img_width = 0;
  static float cam_depth_wpix = 0;
  static float ciw_2 = 0; // curr_img_width / 2

  // We should never be called if enabledMarkers is off, but here's a test
  UNLESS(enabledMarkers) {
    cerr << "warning: WorldModel2 handling markers w/o authorization" << endl;
    return;
  }
  // just abort if the AIBO isn't standing erect--we can't do math on the
  // vision stuff then. In the future, we'll have inverse kinematics that
  // take care of this for us.
//UNCOMMENT THIS IN REAL LIFE
  //UNLESS(aiboIsErect()) return;

  // Likewise if the AIBO's head is tilted or rolled. We'll improve this
  // later.
//UNCOMMENT THIS IN REAL LIFE
  //UNLESS(aiboIsLevelHeaded()) return;

  // Make sure this is a marker event
  UNLESS((event.getTypeID() == EventBase::statusETID) &&
   (event.getSourceID() == VisionEventNS::MarkersSID)) return;

  // Glean the data
  float x = static_cast<const VisionEvent*>(&event)->getCenterX();
  int whichMarker = static_cast<const VisionEvent*>(&event)->getProperty();

  // just in case things go crazy
  if((whichMarker < 0) || (whichMarker >= AFS_NUM_LANDMARKS)) return;

  // find out the degree offset of the landmark. As in the ground plane
  // assumption code, I'm assuming a pinhole camera model. The constants
  // used in the following code are drawn from Maps/Configuration.h

  //if(haveSeenMarker[whichMarker]) return; // seen this marker before? quit.

  // The first thing we have to do is see whether the image size has
  // changed and alter our own camera model measurements accordingly
  if(vision->width != curr_img_width) {
    curr_img_width = vision->width;
    ciw_2 = ((float)curr_img_width) / 2.0;

    // See almMain.cc method ALM::registerGround for details on this
    // computation.
    cam_depth_wpix = ((double) ciw_2)*tan(AIBO_CAM_H_FOV/2);
  }

  // Now we can find the degree offset of the landmark within the image
  float theta = atan2(ciw_2-x, cam_depth_wpix);

  // Let's ignore camera measurements that aren't relatively near to our
  // center of vision. Constant is in WorldModel2.h
  if(theta*theta > maxMarkerAngle*maxMarkerAngle) return;

  // We add that to our head pan angle to get the actual relative angle
  theta += state->outputs[HeadOffset+PanOffset];

  // Now we pass on the creamy data freshness to FastSLAM
  afsMeasurement(whichMarker, theta);

#ifdef DEBUG_WM2
  // Print out observation details.
  cout << "  OBS: " << whichMarker << " at " << theta
       << "\t (" << theta*180/M_PI << ")" << endl;
#endif

  // Now we've officially seen this marker.
  haveSeenMarker[whichMarker] = true;
}
#endif

  // process locomotion events--movements
void WorldModel2::processLocomotion(const EventBase& event)
{
  long currTime = get_time(); // get the current time

  // See if we know where we've been last. Update our representations' ideas
  // of our position given our past trajectory accordingly
  if(movingSince) {
    long timeDiff = currTime - movingSince;
    // Only translate maps, FastSLAM particles if we've been moving
    if(moving) {
      ALM::move(dx, dy, da, timeDiff);
#ifdef WANT_GLOBAL_MAP
      // Add this motion to the list of things for FastSLAM to process. If
      // the LazyFastSLAM kludge is on, it won't happen until the AIBO has
      // come to a complete stop.
      //afsMotion(dx, dy, da, timeDiff);
      FastSLAM_update fsu;
      fsu.type = FastSLAM_update::MOTION;
      fsu.dx = dx;
      fsu.dy = dy;
      fsu.da = da;
      fsu.time = timeDiff;
      fs_updates.push_back(fsu);
#endif
    }
  }

  // Update start time for new trajectory
  movingSince = currTime;

  // Update dx,dy,da with new trajectory info
  dx = static_cast<const LocomotionEvent*>(&event)->x;
  dy = static_cast<const LocomotionEvent*>(&event)->y;
  da = static_cast<const LocomotionEvent*>(&event)->a;

  // See if we're moving
  // Note use of movingSince test.
  if(movingSince && (dx == 0.0) && (dy == 0.0) && (da == 0.0)) moving = false;
  else moving = true;

    // Things to do if we've started moving
  if(moving) {
      // Turn off the GPA timer so that we don't try to do GPA on the move
      // If we did do GPA successfully, this will already have happened.
    if(enabledGPA) erouter->removeTimer(this);
#ifdef WANT_GLOBAL_MAP
      // Turn off marker events for FastSLAM
      // We don't seem to be able to request only markers
    if(enabledMarkers) erouter->removeListener(this, EventBase::visionEGID);//,
                 //VisionEventNS::MarkersSID);
#endif
      // Turn off IR ranging
    if(enabledIR) erouter->removeListener(this, EventBase::sensorEGID,
            SensorSourceID::UpdatedSID);
  }
    // Things to do if we've stopped
  else {
      // Turn on GPA timer to trigger GPA if the pose is right
    if(enabledGPA) erouter->addTimer(this, 0, GPAdelay, true);
#ifdef WANT_GLOBAL_MAP
      // Turn on marker events for FastSLAM
      // We don't seem to be able to request only markers
    if(enabledMarkers) erouter->addListener(this, EventBase::visionEGID);//,
              //VisionEventNS::MarkersSID);
#endif
    if(enabledIR) erouter->addListener(this, EventBase::sensorEGID,
               SensorSourceID::UpdatedSID);
  }

#ifdef WANT_GLOBAL_MAP
  // Clear sensor processing checklist flags, and see if we've seen any
  // landmarks. If we've seen more than a small number, then let's do
  // a FastSLAM resample.
  int numMarkersSeen = 0;
  for(int i=0; i<AFS_NUM_LANDMARKS; ++i)
    if(haveSeenMarker[i]) {
      ++numMarkersSeen;
      haveSeenMarker[i] = false;
    }
  //if(numMarkersSeen >= minMarkersForFastSLAM) afsResample();
  if(numMarkersSeen >= minMarkersForFastSLAM) {
    FastSLAM_update fsu;
    fsu.type = FastSLAM_update::LANDMARK;
    fsu.dx = fsu.dy = fsu.da = 0;
    fsu.time = 0;
    fs_updates.push_back(fsu);
  }


  // If we're moving and we have the LazyFastSLAM kludge enabled, then we
  // stop here.
  if(moving && (kludges & WM2Kludge::LazyFastSLAM)) return;
  // Either we're stopped or LazyFastSLAM is off. Process away.
  while(!fs_updates.empty()) {
    FastSLAM_update fsu = fs_updates.front();

    // Is this a resampling update or a motion update? Act accordingly.
    if(fsu.type == FastSLAM_update::MOTION) {
#ifdef DEBUG_WM2
      cout << "MOTION RESAMPLE <" << fsu.dx << ',' << fsu.dy << ',' << fsu.da
     << ' ' << fsu.time << ">... " << flush;
#endif
      afsMotion(fsu.dx, fsu.dy, fsu.da, fsu.time);
#ifdef DEBUG_WM2
      cout << "done." << endl;
      afsWhatsUp(NULL);
#endif
    }
    else if(fsu.type == FastSLAM_update::LANDMARK) {
#ifdef DEBUG_WM2
      cout << "LANDMARK RESAMPLE... " << flush;
#endif
      afsResample();
#ifdef DEBUG_WM2
      cout << "done." << endl;
      afsWhatsUp(NULL);
#endif
    }

    // Next!
    fs_updates.pop_front();
  }
#endif
}

  // Try to do ground plane assumption
void WorldModel2::processGround()
{
  // We should never be called if enabledGPA is off, but here's a test anyway
  UNLESS(enabledGPA) {
    cerr << "warning: WorldModel2 doing GPA w/o authorization" << endl;
    return;
  }
  // Make sure AIBO is standing erect AND facing dead ahead
  UNLESS(aiboIsErect() && aiboStaresDeadAhead()) return;

  // Delete the ground plane assumption timer
  erouter->removeTimer(this);

  // Go ahead and do the ground plane assumption
  ALM::registerGround();
}

#ifdef WANT_GLOBAL_MAP
  // Gets the current best FastSLAM map and places its contents in p
void WorldModel2::getFastSLAMMap(afsParticle &p)
{
  afsParticle *best = afsWhatsUp(NULL);

  p = *best;
}
#endif

  // Perform serialization -- send data down the line, if needed.
void WorldModel2::serialize()
{
  char *buf;

  // Encode depth map data
  buf = (char *)sockDM->getWriteBuffer(DM_CELL_COUNT*sizeof(dm_cell));
  if(buf) {
    dm_cell *DMp = ALM::getDM();

    for(int i=0; i<DM_CELL_COUNT; ++i) {
      dm_cell *cel = &DMp[i];
      encode(&buf, cel->depth);
      encode(&buf, cel->confidence);
      encode(&buf, cel->color);
    }
    sockDM->write(DM_CELL_COUNT*(sizeof(float)*2 + sizeof(colortype)));
  }

  // Encode height map data
  buf = (char *)sockHM->getWriteBuffer(HM_CELL_COUNT*sizeof(hm_cell));
  if(buf) {
    hm_cell *HMp = ALM::getHM();

    for(int i=0; i<HM_CELL_COUNT; ++i) {
      hm_cell *cel = &HMp[i];
      encode(&buf, cel->height);
      encode(&buf, cel->trav);
      encode(&buf, cel->confidence);
      encode(&buf, cel->color);
    }
    // colortype is defined in Maps/almStructures.h
    sockHM->write(HM_CELL_COUNT*(sizeof(float)*3 + sizeof(colortype)));
  }

#ifdef WANT_GLOBAL_MAP
  // Encode global map data
  buf = (char *)sockGM->getWriteBuffer(GM_CELL_COUNT*sizeof(hm_cell));
  if(buf) {
    hm_cell *GMp = AGM::getGM();

    for(int i=0; i<GM_CELL_COUNT; ++i) {
      hm_cell *cel = &GMp[i];
      encode(&buf, cel->height);
      encode(&buf, cel->trav);
      encode(&buf, cel->confidence);
      encode(&buf, cel->color);
    }
    // colortype is defined in Maps/almStructures.h
    sockGM->write(GM_CELL_COUNT*(sizeof(float)*3 + sizeof(colortype)));
  }

  // Encode FastSLAM map and location data
  buf = (char *)sockFS->getWriteBuffer(AFS_NUM_PARTICLES*sizeof(afsPose) +
               sizeof(afsParticle));
  if(buf) {
    // Insert the locations of all the particles
    afsParticle *Particles = afsInvadeFSData(); // Routine not for general use!
    for(int i=0; i<AFS_NUM_PARTICLES; ++i) {
      encode(&buf, (float) Particles[i].pose.x);
      encode(&buf, (float) Particles[i].pose.y);
      encode(&buf, (float) Particles[i].pose.theta);
    }

    // Send information about the best particle
    afsParticle *p = afsWhatsUp(NULL);

    encode(&buf, (float) p->pose.x);
    encode(&buf, (float) p->pose.y);
    encode(&buf, (float) p->pose.theta);
    for(int i=0; i<AFS_NUM_LANDMARKS; ++i) {
      afsLandmarkLoc *l = &p->landmarks[i];

      // Just use HUGE_VAL to indicate uninitialized landmarks. The code
      // at the other end will detect these unreasonable values and act
      // accordingly
      if(l->state == PRIMING) {
  encode(&buf, (float) HUGE_VAL);
  encode(&buf, (float) HUGE_VAL);
  encode(&buf, (float) HUGE_VAL);
  encode(&buf, (float) HUGE_VAL);
  encode(&buf, (float) HUGE_VAL);
      }
      else {  // state == PRIMED
  encode(&buf, (float) l->mean.x);
  encode(&buf, (float) l->mean.y);
  encode(&buf, (float) l->variance.x);
  encode(&buf, (float) l->variance.xy);
  encode(&buf, (float) l->variance.y);
      }
    }
    sockFS->write(AFS_NUM_PARTICLES*(3*sizeof(float)) +
      (3*sizeof(float)) + AFS_NUM_LANDMARKS*(5*sizeof(float)));
  }
#endif
}

  // Determine whether Aibo is in an erect stature, allowing us to do
  // measurements. TODO: replace with a real motion model
bool aiboIsErect()
{
  double diff;

  diff = SP_LFR_JOINT - state->outputs[LFrLegOffset+RotatorOffset];
  if(diff*diff > SP_TOLERANCE) return false;
  diff = SP_LFR_SHLDR - state->outputs[LFrLegOffset+ElevatorOffset];
  if(diff*diff > SP_TOLERANCE) return false;
  diff = SP_LFR_KNEE  - state->outputs[LFrLegOffset+KneeOffset];
  if(diff*diff > SP_TOLERANCE) return false;

  diff = SP_RFR_JOINT - state->outputs[RFrLegOffset+RotatorOffset];
  if(diff*diff > SP_TOLERANCE) return false;
  diff = SP_RFR_SHLDR - state->outputs[RFrLegOffset+ElevatorOffset];
  if(diff*diff > SP_TOLERANCE) return false;
  diff = SP_RFR_KNEE  - state->outputs[RFrLegOffset+KneeOffset];
  if(diff*diff > SP_TOLERANCE) return false;

  diff = SP_LFR_JOINT - state->outputs[LBkLegOffset+RotatorOffset];
  if(diff*diff > SP_TOLERANCE) return false;
  diff = SP_LFR_SHLDR - state->outputs[LBkLegOffset+ElevatorOffset];
  if(diff*diff > SP_TOLERANCE) return false;
  diff = SP_LFR_KNEE  - state->outputs[LBkLegOffset+KneeOffset];
  if(diff*diff > SP_TOLERANCE) return false;

  diff = SP_RFR_JOINT - state->outputs[RBkLegOffset+RotatorOffset];
  if(diff*diff > SP_TOLERANCE) return false;
  diff = SP_RFR_SHLDR - state->outputs[RBkLegOffset+ElevatorOffset];
  if(diff*diff > SP_TOLERANCE) return false;
  diff = SP_RFR_KNEE  - state->outputs[RBkLegOffset+KneeOffset];
  if(diff*diff > SP_TOLERANCE) return false;

  return true;
}

  // Determine whether Aibo is looking dead ahead. Needed for ground plane
  // assumption, for the moment.
bool aiboStaresDeadAhead()
{
  double diff;

  diff = DA_TILT - state->outputs[HeadOffset+TiltOffset];
  if(diff*diff > DA_TOLERANCE) return false;
  diff = DA_PAN  - state->outputs[HeadOffset+PanOffset];
  if(diff*diff > DA_TOLERANCE) return false;
  diff = DA_ROLL - state->outputs[HeadOffset+RollOffset];
  if(diff*diff > DA_TOLERANCE) return false;

  return true;
}

  // Determine whether Aibo is keeping its head level (i.e. no tilt or roll).
  // Needed for FastSLAM at the moment
bool aiboIsLevelHeaded()
{
  double diff;

  diff = DA_TILT - state->outputs[HeadOffset+TiltOffset];
  if(diff*diff > DA_TOLERANCE) return false;
  diff = DA_ROLL - state->outputs[HeadOffset+RollOffset];
  if(diff*diff > DA_TOLERANCE) return false;

  return true;
}

Tekkotsu v1.5

Generated Fri Oct 10 15:52:00 2003 by Doxygen 1.3.4