Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Representations/Perception/CollisionPercept.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file CollisionPercept.cpp
    00003 *
    00004 * Implementation of class CollisionPercept.
    00005 */
    00006
    00007 #include "CollisionPercept.h"
    00008
    00009 CollisionPercept::CollisionPercept()
    00010 {
    00011   for (int i = 0; i < JointData::numOfJoint; i++)
    00012   {
    00013     state[i] = 0;
    00014   }
    00015 }
    00016
    00017 void CollisionPercept::reset()
    00018 {
    00019   for (int i = 0; i < JointData::numOfJoint; i++)
    00020   {
    00021     state[i] = 0;
    00022   }
    00023 }
    00024
    00025
    00026 CollisionPercept::~CollisionPercept()
    00027 { }
    00028
    00029
    00030 /** Did collision occur at front left leg */
    00031   bool CollisionPercept::getCollisionFrontLeft() const
    00032   {
    00033     return ((state[JointData::legFL1] >= 1000) ||
    00034             (state[JointData::legFL2] >= 1000) ||
    00035             (state[JointData::legFL3] >= 1000));
    00036   }
    00037
    00038   /** Did collision occur at front right leg */
    00039   bool CollisionPercept::getCollisionFrontRight() const
    00040   {
    00041     return ((state[JointData::legFR1] >= 1000) ||
    00042             (state[JointData::legFR2] >= 1000) ||
    00043             (state[JointData::legFR3] >= 1000));
    00044
    00045   }
    00046   /** Did collision occur at hind left leg */
    00047   bool CollisionPercept::getCollisionHindLeft() const
    00048   {
    00049     return ((state[JointData::legHL1] >= 1000) ||
    00050             (state[JointData::legHL2] >= 1000) ||
    00051             (state[JointData::legHL3] >= 1000));
    00052   }
    00053
    00054
    00055   /** Did collision occur at hind right leg */
    00056   bool CollisionPercept::getCollisionHindRight() const
    00057   {
    00058     return ((state[JointData::legHR1] >= 1000) ||
    00059             (state[JointData::legHR2] >= 1000) ||
    00060             (state[JointData::legHR3] >= 1000));
    00061
    00062   }
    00063
    00064
    00065   /** Did collision occur at head */
    00066   bool CollisionPercept::getCollisionHead() const
    00067   {
    00068     return ((state[JointData::neckTilt] >= 1000) ||
    00069             (state[JointData::headPan]  >= 1000) ||
    00070             (state[JointData::headTilt] >= 1000));
    00071   }
    00072
    00073
    00074   /** Did collision occur at any body part */
    00075   bool CollisionPercept::getCollisionAggregate() const
    00076   {
    00077     return (getCollisionFrontLeft()||
    00078             getCollisionFrontRight()||
    00079             getCollisionHindLeft()||
    00080             getCollisionHindRight());
    00081   }
    00082
    00083
    00084   /** Quantitative Value for occurred collision */
    00085   double CollisionPercept::getOdometryDisturbance() const
    00086   {
    00087     double max = 0;
    00088     if (state[JointData::legFL1] > max) {max = state[JointData::legFL1];}
    00089     if (state[JointData::legFL2] > max) {max = state[JointData::legFL2];}
    00090     if (state[JointData::legFL3] > max) {max = state[JointData::legFL3];}
    00091     if (state[JointData::legFR1] > max) {max = state[JointData::legFR1];}
    00092     if (state[JointData::legFR2] > max) {max = state[JointData::legFR2];}
    00093     if (state[JointData::legFR3] > max) {max = state[JointData::legFR3];}
    00094     if (state[JointData::legHL1] > max) {max = state[JointData::legHL1];}
    00095     if (state[JointData::legHL2] > max) {max = state[JointData::legHL2];}
    00096     if (state[JointData::legHL3] > max) {max = state[JointData::legHL3];}
    00097     if (state[JointData::legHR1] > max) {max = state[JointData::legHR1];}
    00098     if (state[JointData::legHR2] > max) {max = state[JointData::legHR2];}
    00099     if (state[JointData::legHR3] > max) {max = state[JointData::legHR3];}
    00100
    00101     max /= 500;
    00102
    00103     return max;
    00104   }
    00105
    00106
    00107 In& operator>>(In& stream, CollisionPercept& collisionPercept)
    00108 {
    00109   STREAM_REGISTER_BEGIN_EXT( collisionPercept);
    00110   STREAM_EXT( stream, collisionPercept.frameNumber);
    00111   STREAM_ARRAY_EXT( stream, collisionPercept.state);
    00112   STREAM_REGISTER_FINISH();
    00113   /*
    00114   stream >> collisionPercept.frameNumber;
    00115   stream.read(&collisionPercept, sizeof(collisionPercept));
    00116   */
    00117   return stream;
    00118 }
    00119
    00120 Out& operator<<(Out& stream, const CollisionPercept& collisionPercept)
    00121 {
    00122   STREAM_REGISTER_BEGIN_EXT( collisionPercept);
    00123   STREAM_EXT( stream, collisionPercept.frameNumber);
    00124   STREAM_ARRAY_EXT( stream, collisionPercept.state);
    00125   STREAM_REGISTER_FINISH();
    00126   /*
    00127   stream << collisionPercept.frameNumber;
    00128   stream.write(&collisionPercept, sizeof(collisionPercept));
    00129   */
    00130   return stream;
    00131 }
    00132