Representations/Perception/CollisionPercept.cpp

Go to the documentation of this file.

/**
* @file CollisionPercept.cpp
*
* Implementation of class CollisionPercept.
*/

#include "CollisionPercept.h"

CollisionPercept::CollisionPercept()
{
  for (int i = 0; i < JointData::numOfJoint; i++)
  {
    state[i] = 0;
  }
}

void CollisionPercept::reset()
{
  for (int i = 0; i < JointData::numOfJoint; i++)
  {
    state[i] = 0;
  }
}


CollisionPercept::~CollisionPercept()
{ }


/** Did collision occur at front left leg */
  bool CollisionPercept::getCollisionFrontLeft() const
  {
    return ((state[JointData::legFL1] >= 1000) ||
            (state[JointData::legFL2] >= 1000) ||
            (state[JointData::legFL3] >= 1000));
  }

  /** Did collision occur at front right leg */
  bool CollisionPercept::getCollisionFrontRight() const
  {
    return ((state[JointData::legFR1] >= 1000) ||
            (state[JointData::legFR2] >= 1000) ||
            (state[JointData::legFR3] >= 1000));
  
  }
  /** Did collision occur at hind left leg */
  bool CollisionPercept::getCollisionHindLeft() const
  {
    return ((state[JointData::legHL1] >= 1000) ||
            (state[JointData::legHL2] >= 1000) ||
            (state[JointData::legHL3] >= 1000));
  }


  /** Did collision occur at hind right leg */
  bool CollisionPercept::getCollisionHindRight() const
  {
    return ((state[JointData::legHR1] >= 1000) ||
            (state[JointData::legHR2] >= 1000) ||
            (state[JointData::legHR3] >= 1000));

  }


  /** Did collision occur at head */
  bool CollisionPercept::getCollisionHead() const
  {
    return ((state[JointData::neckTilt] >= 1000) ||
            (state[JointData::headPan]  >= 1000) ||
            (state[JointData::headTilt] >= 1000));
  }


  /** Did collision occur at any body part */
  bool CollisionPercept::getCollisionAggregate() const
  {
    return (getCollisionFrontLeft()||
            getCollisionFrontRight()||
            getCollisionHindLeft()||
            getCollisionHindRight());
  }


  /** Quantitative Value for occurred collision */
  double CollisionPercept::getOdometryDisturbance() const
  { 
    double max = 0;
    if (state[JointData::legFL1] > max) {max = state[JointData::legFL1];}
    if (state[JointData::legFL2] > max) {max = state[JointData::legFL2];}
    if (state[JointData::legFL3] > max) {max = state[JointData::legFL3];}
    if (state[JointData::legFR1] > max) {max = state[JointData::legFR1];}
    if (state[JointData::legFR2] > max) {max = state[JointData::legFR2];}
    if (state[JointData::legFR3] > max) {max = state[JointData::legFR3];}
    if (state[JointData::legHL1] > max) {max = state[JointData::legHL1];}
    if (state[JointData::legHL2] > max) {max = state[JointData::legHL2];}
    if (state[JointData::legHL3] > max) {max = state[JointData::legHL3];}
    if (state[JointData::legHR1] > max) {max = state[JointData::legHR1];}
    if (state[JointData::legHR2] > max) {max = state[JointData::legHR2];}
    if (state[JointData::legHR3] > max) {max = state[JointData::legHR3];}
    
    max /= 500;
    
    return max;
  }


In& operator>>(In& stream, CollisionPercept& collisionPercept)
{
  STREAM_REGISTER_BEGIN_EXT( collisionPercept);
  STREAM_EXT( stream, collisionPercept.frameNumber);
  STREAM_ARRAY_EXT( stream, collisionPercept.state);
  STREAM_REGISTER_FINISH();
  /*
  stream >> collisionPercept.frameNumber;
  stream.read(&collisionPercept, sizeof(collisionPercept));
  */
  return stream;
}

Out& operator<<(Out& stream, const CollisionPercept& collisionPercept)
{
  STREAM_REGISTER_BEGIN_EXT( collisionPercept);
  STREAM_EXT( stream, collisionPercept.frameNumber);
  STREAM_ARRAY_EXT( stream, collisionPercept.state);
  STREAM_REGISTER_FINISH();
  /*
  stream << collisionPercept.frameNumber;
  stream.write(&collisionPercept, sizeof(collisionPercept));
  */
  return stream;
}

---