FrankE
82f8828a04
fixed stair-building issue new test-cases added elevator support fixed/improved some walker modules
125 lines
2.9 KiB
C++
125 lines
2.9 KiB
C++
#ifndef WALKMODULEHEADING_H
|
|
#define WALKMODULEHEADING_H
|
|
|
|
#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> {
|
|
|
|
private:
|
|
|
|
/** van-Mises distribution */
|
|
Distribution::LUT<double> dist;
|
|
|
|
/** van-Mises draw list */
|
|
DrawList<double> draw;
|
|
|
|
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 {
|
|
|
|
// add noise
|
|
state.heading.direction += draw.get();
|
|
|
|
}
|
|
|
|
virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
|
|
|
|
(void) state;
|
|
(void) startNode;
|
|
(void) endNode;
|
|
// if (startNode.x_cm != endNode.x_cm || startNode.y_cm != endNode.y_cm) {
|
|
// Heading head(startNode.x_cm, startNode.y_cm, endNode.x_cm, endNode.y_cm);
|
|
// state.startHeading = head;
|
|
// }
|
|
|
|
}
|
|
|
|
/** one step (edge) is taken */
|
|
virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
|
|
|
|
// TODO
|
|
(void) state;
|
|
(void) curNode;
|
|
(void) nextNode;
|
|
|
|
}
|
|
|
|
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
|
|
|
(void) startNode;
|
|
|
|
// get the heading between curNode and potentialNode
|
|
const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);
|
|
|
|
// compare the heading against the state's heading
|
|
const Heading stateHead = state.heading.direction;
|
|
|
|
// get the difference
|
|
const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD();
|
|
|
|
// determine probability
|
|
return dist.getProbability(angularDiff);
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // WALKMODULEHEADING_H
|