FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added new sanity checks and compile-time assertions to prevent errors

Browse Source 
fixed stair-building issue
new test-cases
added elevator support
fixed/improved some walker modules
This commit is contained in:
FrankE
2016-09-10 15:12:39 +02:00
parent 7baeecb3f9
commit 82f8828a04
26 changed files with 996 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
grid/walk/v2/modules/WalkModuleSpread.h
View File

@@ -4,6 +4,21 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
/** state-parameter needed for WalkModuleSpread */
struct WalkStateSpread {
/** nested struct to prevent name-clashes */
struct {
/** keep something like a moving-average-position we want to strictly depart from */
GridPoint departFrom;
} spread;
};
/**
* simply try to move away from the starting node as much as possible
@@ -12,19 +27,26 @@ template <typename Node, typename WalkState> class WalkModuleSpread : public Wal
private:
Point3 avg;
/**
* how fast to adjust the average-position to depart from
* values between 3% and 10% seem fine
*/
const float kappa = 0.10;
public:
/** ctor */
WalkModuleSpread() {
;
/** ensure the templated WalkState inherits from WalkStateSpread */
StaticAssert::AinheritsB<WalkState, WalkStateSpread>();
}
virtual void updateBefore(WalkState& state) override {
(void) state;
avg = avg * 0.999 + state.position.inMeter() * 0.001;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
@@ -34,24 +56,27 @@ public:
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
state.spread.departFrom = state.spread.departFrom * (1.0f-kappa) + nextNode * (kappa);
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) state;
(void) startNode;
(void) curNode;
const float dOld = avg.getDistance(curNode.inMeter());
const float dNew = avg.getDistance(potentialNode.inMeter());
// current distance from the depart-from position
const float dOld = state.spread.departFrom.getDistanceInCM(curNode);
if (dNew > dOld) {return 0.8;}
if (curNode.z_cm != potentialNode.z_cm) {return 0.8;}
if (dNew == dOld) {return 0.2;}
return 0;
// potential distance from the depart-from position
const float dNew = state.spread.departFrom.getDistanceInCM(potentialNode);
// now, favor edges that depart even further from the depart-from position!
if (dNew > dOld) {return 0.90;} // departing
if (dNew == dOld) {return 0.09;} // distance does not change
{return 0.01;} // NOT departing.. unlikely
}