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

75
grid/walk/v2/modules/WalkModuleFavorZ.h
View File

@@ -7,15 +7,46 @@
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"
/** state-parameter needed for WalkModuleFavorZ */
struct WalkStateFavorZ {
/** nested struct to prevent name clashes */
struct {
/**
* 0 = up / down / stay is legal
* > 0 force states to walk upwards
* < 0 force states to walk downwards
*
* shifted towards 0 after every taken edge
* so: we force states to walk into the same z-direction for some time
*/
int zTendence = 0;
} favorZ;
};
/** favor z-transitions */
template <typename Node, typename WalkState> class WalkModuleFavorZ : public WalkModule<Node, WalkState> {
private:
// force states to walk into the same z-direction for 30 edges
const int keepForXEdges = 12;
public:
/** ctor */
WalkModuleFavorZ() {
;
// ensure the template WalkState inherits from 'WalkStateFavorZ'
StaticAssert::AinheritsB<WalkState, WalkStateFavorZ>();
}
virtual void updateBefore(WalkState& state) override {
@@ -30,9 +61,27 @@ public:
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) state;
(void) curNode;
(void) nextNode;
// currently no walk-tendence configured
if (state.favorZ.zTendence == 0) {
// does the taken edge indicate a z-change?
const int diff = nextNode.z_cm - curNode.z_cm;
// if so, keep this z-direction for the next few edges to come!
if (diff != 0) {
state.favorZ.zTendence = (diff > 0) ? (+keepForXEdges) : (-keepForXEdges);
}
// currently there IS a walk-tendence configured
} else {
// update the tendence (shift towards 0)
if (state.favorZ.zTendence < 0) {++state.favorZ.zTendence;}
else if (state.favorZ.zTendence > 0) {--state.favorZ.zTendence;}
}
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
@@ -40,11 +89,19 @@ public:
(void) state;
(void) startNode;
if (curNode.z_cm != potentialNode.z_cm) {
return 40;
} else {
return 1;
}
const int tendence = state.favorZ.zTendence;
const int diff = potentialNode.z_cm - curNode.z_cm;
// tendence available + tendence match? -> high score!
if (tendence > 0 && diff > 0) {return 0.95;}
if (tendence < 0 && diff < 0) {return 0.95;}
// tendence available + tendence mismatch? -> very low score!
if (tendence > 0 && diff < 0) {return 0.05;}
if (tendence < 0 && diff > 0) {return 0.05;}
// no tendence available -> just favor z-transitions over non-z-transitions
return (diff != 0) ? (0.7) : (0.3);
}

47
grid/walk/v2/modules/WalkModuleHeading.h
View File

@@ -4,10 +4,52 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../geo/Heading.h"
/**
* base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work
*/
struct WalkStateHeading {
/** used for better naming: heading.error instead of headingError */
struct _Heading {
/**
* the direction [0:2pi] the walk should move to
* e.g. indiciated by:
* compass
* integration over gyroscope values
*/
Heading direction;
/**
* (cumulative) error between walked edges and requested direction (above).
* is used to ensure that (even though the grid contains only 45° edges) we
* approximately walk into the requested direction.
*/
float error = 0;
/** ctor */
_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}
} heading;
/** ctor */
explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}
};
/** keep the state's heading */
template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {
@@ -23,7 +65,10 @@ public:
/** ctor */
WalkModuleHeading() : dist(Distribution::VonMises<double>(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) {
;
// ensure the template WalkState inherits from 'WalkStateHeading'!
StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
}
virtual void updateBefore(WalkState& state) override {

7
grid/walk/v2/modules/WalkModuleHeadingControl.h
View File

@@ -58,6 +58,9 @@ public:
(void) state;
// for elevator edges [same (x,y) but different z] do not adjust anything
if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}
// get the heading denoted by the way from curNode to nextNode
const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);
@@ -80,6 +83,10 @@ public:
(void) startNode;
// for elevator edges [same (x,y) but different z] just return 1
if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}
// get the heading between curNode and potentialNode
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);

2
grid/walk/v2/modules/WalkModuleNodeImportance.h
View File

@@ -4,6 +4,7 @@
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../Assertions.h"
/**
* favor edges based on the importance-factor of the next node.
@@ -44,7 +45,6 @@ public:
(void) curNode;
const double prob = potentialNode.getNavImportance();
//return std::pow(prob, 10);
return prob;
}

77
grid/walk/v2/modules/WalkModulePreventVisited.h Normal file
View File

@@ -0,0 +1,77 @@
#ifndef WALKMODULEPREVENTVISITED_H
#define WALKMODULEPREVENTVISITED_H
#include "WalkModule.h"
#include "WalkStateHeading.h"
#include "../../../../data/RingBuffer.h"
#include "../../../../Assertions.h"
struct WalkStatePreventVisited {
struct PV {
RingBuffer<int> history;
PV(const int size) : history(size) {;}
} preventVisited;
/** ctor */
explicit WalkStatePreventVisited(const int historySize) : preventVisited(historySize) {;}
};
/**
* prevent a state from visiting nodes he has already visited
* within a certain timeframe (ringbuffer)
*
* this should avoid deadlocks in some situations where the transition
* just switched back and forth between two nodes
*
*/
template <typename Node, typename WalkState> class WalkModulePreventVisited : public WalkModule<Node, WalkState> {
private:
public:
/** ctor */
WalkModulePreventVisited() {
// ensure the templated WalkState inherits from 'WalkStatePreventVisited'
StaticAssert::AinheritsB<WalkState, WalkStatePreventVisited>();
}
virtual void updateBefore(WalkState& state) override {
(void) state;
}
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
(void) state;
(void) startNode;
(void) endNode;
}
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
(void) curNode;
state.preventVisited.history.add(nextNode.getIdx());
}
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
(void) startNode;
(void) curNode;
return (state.preventVisited.history.contains(potentialNode.getIdx())) ? 0.001 : 0.999;
}
};
#endif // WALKMODULEPREVENTVISITED_H

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
}

36
grid/walk/v2/modules/WalkStateHeading.h
View File

@@ -1,43 +1,7 @@
#ifndef WALKSTATEHEADING_H
#define WALKSTATEHEADING_H
#include "../../../../geo/Heading.h"
/**
* base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work
*/
struct WalkStateHeading {
/** used for better naming: heading.error instead of headingError */
struct _Heading {
/**
* the direction [0:2pi] the walk should move to
* e.g. indiciated by:
* compass
* integration over gyroscope values
*/
Heading direction;
/**
* (cumulative) error between walked edges and requested direction (above).
* is used to ensure that (even though the grid contains only 45° edges) we
* approximately walk into the requested direction.
*/
float error = 0;
/** ctor */
_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}
} heading;
/** ctor */
explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}
};
#endif // WALKSTATEHEADING_H