00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00030
00031
00033
00034
00035 #ifndef __FORMATION_CONTROL_H
00036 #define __FORMATION_CONTROL_H
00037
00038 #ifdef MIP_HOST_APPLE
00039 #include <applePatch.h>
00040 #endif
00041
00042 #include <map>
00043
00044 #include <baselib.h>
00045
00046 #include "FormControlMsgs.h"
00047 #include "FormCtrlShape.h"
00048
00049 #include <socketUDP.h>
00050
00051 using namespace MipBaselib;
00052
00056 enum FormationControlTypes{
00057 FORM_CTRL_TYPE_SPRDAM,
00058 FORM_CTRL_TYPE_ARTPOT,
00059 FORM_CTRL_TYPE_BOTTUP,
00060 FORM_CTRL_TYPES_NUM
00061 };
00062
00065 static const char* FormationControlTypesNames[FORM_CTRL_TYPES_NUM] = {
00066 "Spring-Damper Form. Control",
00067 "Artificial-Potential Form. Control"
00068 "Form Ctrl Bottom up"
00069 };
00070
00071
00075 class FormCtrlOptions : public Options {
00076 public:
00077
00078 StringOption *algorithm;
00079 StringOption *baseIp;
00080 IntOption *basePort;
00081 Position3DOption *centerPos;
00082
00083 DecimalOption *apAttrMaxDist;
00084 DecimalOption *apAttrGain;
00085 DecimalOption *apRepulMaxDist;
00086 DecimalOption *apRepulMinDist;
00087 DecimalOption *apRepulGain;
00088 BoolOption *apUseShape;
00089 StringOption *apShape;
00090 IntOption *apShapeVertNum;
00091
00092 DecimalOption *sdSelfDamp;
00093 DecimalOption *sdNeighDist;
00094 DecimalOption *sdNeighStiff;
00095 DecimalOption *sdNeighDamp;
00096
00097 DecimalOption *sdMinGrad;
00098 DecimalOption *sdMaxGrad;
00099 DecimalOption *sdGradD1;
00100 DecimalOption *sdGradD2;
00101 DecimalOption *sdGradD3;
00102 DecimalOption *sdGradSlope;
00103 DecimalOption *sdObsGain;
00104 DecimalOption *sdObsMinDist;
00105 DecimalOption *sdObsNullDist;
00106 DecimalOption *minTankLev;
00107 DecimalOption *maxTankLev;
00108 DecimalOption *initTankLev;
00109
00110 FormCtrlOptions();
00111
00112 string getObjectName() const {
00113 return "FormCtrlOptions";
00114 }
00115 };
00116
00117
00119 class SpringDampFormCtrlIn {
00120 private:
00121 void _setAll(Time currTime_, Position3D &robPos_, Velocity3D &robVel_, vector<Position3D>& obsPos_, vector<int>& neighList_,bool useExtVel_, Velocity3D extVel_);
00122
00123
00124 public:
00125
00127 ~SpringDampFormCtrlIn();
00128
00130 SpringDampFormCtrlIn(Time currTime_, Position3D &robPos_, Velocity3D &robVel_, vector<Position3D>& obsPos_, vector<int>& neighList_, Velocity3D extVel_);
00131
00133 SpringDampFormCtrlIn(Time currTime_, Position3D &robPos_, Velocity3D &robVel_, vector<Position3D>& obsPos_, vector<int>& neighList_);
00134
00136 SpringDampFormCtrlIn(const SpringDampFormCtrlIn& p);
00137
00139 SpringDampFormCtrlIn& operator=(const SpringDampFormCtrlIn& p);
00140
00141 Time currTime;
00142 Position3D robPos;
00143 Velocity3D robVel;
00144 vector<Position3D> obsPos;
00145 vector<int> neighList;
00146 bool useExtVel;
00147 Velocity3D extVel;
00148 };
00149
00151 class ArtPotentialFormCtrlIn {
00152 private:
00153
00154 void _setAll(Time& cT, Position3D &rP, vector<Position3D>& oP,vector<int>& nL, Velocity3D& eV,bool uEV,Decimal sS,bool uSS);
00155 public:
00156
00158 ~ArtPotentialFormCtrlIn();
00159
00163 ArtPotentialFormCtrlIn(Time currTime, Position3D& robotPosi, vector<Position3D>& obsPos, vector<int>& robDetList);
00164
00166 ArtPotentialFormCtrlIn(Time currTime, Position3D& robotPosi, vector<Position3D>& obsPos, vector<int>& robDetList, Velocity3D& externalVel);
00167
00169 ArtPotentialFormCtrlIn(Time currTime, Position3D &robotPos, vector<Position3D>& obsPos,vector<int>& robDetList, Velocity3D& externalVel, Decimal shapeScale_);
00170
00172 ArtPotentialFormCtrlIn(const ArtPotentialFormCtrlIn& p);
00173
00175 ArtPotentialFormCtrlIn& operator=(const ArtPotentialFormCtrlIn& p);
00176
00177 Time currentTime;
00178 Position3D robotPosition;
00179 vector<Position3D> obsPositions;
00180 vector<int> neighList;
00181 Velocity3D extVelocity;
00182 bool useExtVel;
00183 Decimal shapeScale;
00184 bool useShapeScale;
00185 };
00186
00188 class FormationControlOut {
00189 private:
00190
00191 public:
00192
00193 FormationControlOut();
00194
00195 bool valid;
00196 Position3D commandedPos;
00197 Velocity3D commandedVel;
00198 Acceleration3D commandedAcc;
00199
00200 Position3D virtPoint;
00201 Velocity3D virtPointVel;
00202 Velocity3D obsGradient;
00203 };
00204
00207 class SpringDampFormCtrlPar : public MIPObject {
00208 private:
00209
00210 Decimal _selfDamping;
00211
00212 Decimal _neighNeutrDist;
00213
00214 Decimal _neighStiff;
00215 Decimal _neighDamp;
00216
00217 Time _neighDataLifeTime;
00218
00219 stringstream _ssfatal;
00220
00221 public:
00226 SpringDampFormCtrlPar(Decimal selfDamp=10.0,Decimal neighDist = 1.5,Decimal neighStiff = 5.0,Decimal neighDamping=5.0,Time neighDataLifeTime=Time(0.1));
00227
00229 SpringDampFormCtrlPar(const SpringDampFormCtrlPar& p);
00230
00232 SpringDampFormCtrlPar& operator=(const SpringDampFormCtrlPar& rhs);
00233
00236 Decimal selfDamping();
00237
00240 Decimal neighborNeutrDist();
00241
00244 Decimal neighborStiff();
00245
00248 Decimal neighborDamping();
00249
00252 Time neighDataLifeTime();
00253
00255 string getObjectName() const {
00256 return "SpringDampFormCtrlPar";
00257 }
00258 };
00259
00262 class ArtPotentialFormCtrlPar : public MIPObject {
00263 private:
00264
00265 Decimal _attractMaxDist;
00266 Decimal _attractGain;
00267 Decimal _repulMaxDist;
00268 Decimal _repulsMinDist;
00269 Decimal _repulsGain;
00270
00271 Time _virtPointLifeTime;
00272
00273 stringstream _ssfatal;
00274
00275 public:
00282 ArtPotentialFormCtrlPar(Decimal attrMaxDist=3.0,Decimal attractGain = 1.0,Decimal repulMaxDist = 2.0,Decimal repulMinDist = 0.3,Decimal repulGain=1.0,
00283 Time virtPointLifeTime=Time(0.1));
00284
00286 ArtPotentialFormCtrlPar(const ArtPotentialFormCtrlPar& p);
00287
00289 ArtPotentialFormCtrlPar& operator=(const ArtPotentialFormCtrlPar& rhs);
00290
00293 Decimal attrMaxDist();
00294
00297 Decimal attrGain();
00298
00301 Decimal repulMinDist();
00302
00305 Decimal repulMaxDist();
00306
00309 Decimal repulGain();
00310
00313 Time virtPointLifeTime();
00314
00316 string getObjectName() const {
00317 return "ArtPotentialFormCtrlPar";
00318 }
00319 };
00320
00322 class SpringDampFormCtrlVar {
00323 private:
00324
00325 public:
00326
00327 Position3D virtPointPos;
00328 Velocity3D virtPointVel;
00329 Acceleration3D virtPointAcc;
00330 Time virtPointTime;
00331
00332 Decimal tankLevel;
00333
00334 vector<int> lastNeighList;
00335
00336 map<int,Position3D> rcvdVirtPoints;
00337 map<int,Time> rcvdTimeStamps;
00338
00339 bool initialized;
00340
00341 SpringDampFormCtrlVar(Decimal initTankLev);
00342
00343 SpringDampFormCtrlVar(const SpringDampFormCtrlVar& p);
00344
00345 SpringDampFormCtrlVar& operator=(const SpringDampFormCtrlVar& p);
00346 };
00347
00348
00352 enum ApFcState{
00353 APFC_STATE_TK_OFF,
00354 APFC_STATE_INIT,
00355 APFC_STATE_RUN,
00356 APFC_STATE_LAND,
00357 APFC_STATE_NUM
00358 };
00359
00362 static const char* ApFcStateNames[APFC_STATE_NUM] = {
00363 "take Off",
00364 "Init",
00365 "run",
00366 "land",
00367 };
00368
00372 enum ApFcTOffState{
00373 APFC_TOFF_STATE_CHECK_INIT_DATA,
00374 APFC_TOFF_STATE_SEND_CURR_POS,
00375 APFC_TOFF_STATE_FIND_ORDER,
00376 APFC_TOFF_STATE_ARRIVED,
00377 APFC_TOFF_STATE_NUM
00378 };
00379
00382 static const char* ApFcTOffStateNames[APFC_TOFF_STATE_NUM] = {
00383 "check init data",
00384 "send curr pos",
00385 "find order",
00386 "arrived",
00387 };
00388
00389
00393 enum ApFcLandState{
00394 APFC_LAND_TODO,
00395 APFC_LAND_STATE_NUM
00396 };
00397
00400 static const char* ApFcLandStateNames[APFC_LAND_STATE_NUM] = {
00401 "APFC_LAND_TODO",
00402 };
00403
00404
00406 class ArtPotentialFormCtrlVar {
00407 private:
00408
00409 public:
00410
00411 bool initialized;
00412
00413 ApFcState state;
00414 ApFcTOffState tOffState;
00415 ApFcLandState landState;
00416
00417 Position3D virtPoint;
00418 Velocity3D virtPointVel;
00419 Time virtPointTime;
00420
00421 bool useShape;
00422 FormCtrlShape* shape;
00423
00424 Decimal otherVPPotential;
00425 Decimal obsPotential;
00426
00427 map<int,Position3D> rcvdVirtPoints;
00428 map<int,Position3D>::iterator itVP;
00429 map<int,Time> rcvdTimeStamps;
00430 map<int,Time>::iterator itT;
00431
00432 Decimal rcvdShapeScale;
00433
00434 ArtPotentialFormCtrlVar();
00435 };
00436
00439 class FormationControl : public MIPObject{
00440 private:
00441 FormationControlTypes _ctrlType;
00442
00443 SpringDampFormCtrlVar *_sdVar;
00444 SpringDampFormCtrlPar *_sdPar;
00445
00446 ArtPotentialFormCtrlVar *_apVar;
00447 ArtPotentialFormCtrlPar *_apPar;
00448
00449 FormCtrlOptions _options;
00450
00451 ListenerUDP* _listener;
00452 pthread_mutex_t _listMtx;
00453
00454 stringstream _ssfatal,_ssdebug;
00455 FILE* _logFile;
00456 bool _extLogFile;
00457
00458 Decimal _sdGetGradient(Decimal distance);
00459 void _sdGetNeighGradAndPot(Position3D posDiff, Acceleration3D& gradient, Decimal& potential);
00460 Acceleration3D _sdGetObsGrad(SpringDampFormCtrlIn* in);
00461
00462 void _discardOldData(map<int,Position3D>& data , map<int,Time>& timeStamps, Time lifeTime);
00463 void _discardOutOfListData(map<int,Position3D>& data, map<int,Time>& timeStamps, vector<int>& toBeKept);
00464
00465 void _apSetShapeValues();
00466 Decimal _apGetAttrMaxDist(int otherId);
00467 Velocity3D _apGetOtherVirtPointVel();
00468 Velocity3D _apGetObstacleVel(ArtPotentialFormCtrlIn *input);
00469
00470 void _apTakeOff(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
00471 void _apInit(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
00472 void _apRun(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
00473 void _apLand(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
00474
00475 void _pruneVirtPoints();
00476 void _sendCommPackets(vector<int>& recipients);
00477
00478 public:
00479
00482 FormationControl(SpringDampFormCtrlPar &par);
00483
00489 FormationControl(ArtPotentialFormCtrlPar &par);
00490
00492 FormationControl(int argc, const char** argv);
00493
00495 ~FormationControl();
00496
00500 void run(SpringDampFormCtrlIn *input,FormationControlOut *output);
00501
00505 void run(ArtPotentialFormCtrlIn *input,FormationControlOut *output);
00506
00508 void reset();
00509
00513 Position3D getInitPosition();
00514
00515 void setLogFile(FILE* logFile);
00516
00519 FormationControlTypes getType(){
00520 return _ctrlType;
00521 }
00522
00524 string getObjectName() const {
00525 return "FormationControl";
00526 }
00527 };
00528
00529
00530 #endif
00531
00532
00533
00534
00535
00536
00537