134 lines
4.4 KiB
C++
134 lines
4.4 KiB
C++
#ifndef WALKMODULEHEADINGCONTROL_H
|
|
#define WALKMODULEHEADINGCONTROL_H
|
|
|
|
#include "WalkModule.h"
|
|
#include "WalkStateHeading.h"
|
|
|
|
#include "../../../../geo/Heading.h"
|
|
#include "../../../../math/distribution/Normal.h"
|
|
#include "../../../../math/distribution/LUT.h"
|
|
#include "../../../../math/distribution/VonMises.h"
|
|
|
|
/** keep the state's heading */
|
|
template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingControl : public WalkModule<Node, WalkState> {
|
|
|
|
private:
|
|
|
|
/** CURRENTLY NOT USED van-Mises distribution */
|
|
Distribution::LUT<float> dist;
|
|
|
|
/** random noise */
|
|
Distribution::Normal<float> distNoise;
|
|
|
|
const Control* ctrl;
|
|
|
|
public:
|
|
|
|
/** ctor 3.0 should be OK! */
|
|
WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
|
|
dist(Distribution::VonMises<float>(0.0f, 2.0f).getLUT()),
|
|
distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
|
|
ctrl(ctrl) {
|
|
|
|
// ensure the template WalkState inherits from 'WalkStateHeading'!
|
|
StaticAssert::AinheritsB<WalkState, WalkStateHeading>();
|
|
|
|
}
|
|
|
|
|
|
virtual void updateBefore(WalkState& state, const Node& startNode) override {
|
|
|
|
// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
|
|
// reset this one after every transition!
|
|
Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");
|
|
|
|
// sensor noise
|
|
float var = distNoise.draw();
|
|
|
|
// stair? -> increase variance
|
|
if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}
|
|
|
|
// adjust the state's heading using the control-data
|
|
state.heading.direction += ctrl->turnSinceLastTransition_rad + var;
|
|
|
|
}
|
|
|
|
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) state;
|
|
|
|
// ignore for stairs?
|
|
//if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}
|
|
|
|
// for elevator edges [same (x,y) but different z] do not adjust anything
|
|
if (nextNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
|
|
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return;}
|
|
//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);
|
|
|
|
// get the heading requested by the state
|
|
const Heading stateHead = state.heading.direction;
|
|
|
|
// get the error (signed difference) between both
|
|
// const float angularDiff = stateHead.getSignedDiff(head);
|
|
const float angularDiff = Heading::getSignedDiff(head, stateHead);
|
|
|
|
// adjust the error.
|
|
// note: the error may get > +/- 2PI but this is not an issue!
|
|
// when the error is added to the current heading within getProbability(),
|
|
// it is ensured their sum is within [0:2pi]
|
|
state.heading.error += angularDiff;
|
|
|
|
|
|
}
|
|
|
|
double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {
|
|
|
|
(void) startNode;
|
|
|
|
// ignore for stairs?
|
|
//if (potentialNode.getType() == GridNode::TYPE_STAIR) {return 1.0;}
|
|
|
|
// for elevator edges [same (x,y) but different z] just return 1
|
|
if (potentialNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
|
|
if (curNode.getType() == GridNode::TYPE_ELEVATOR) {return 1.0;}
|
|
//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);
|
|
|
|
// compare the heading against the state's heading - the last error
|
|
const Heading stateHead = state.heading.direction + state.heading.error;
|
|
|
|
// get the difference
|
|
const float angularDiff = head.getDiffHalfRAD(stateHead);
|
|
|
|
|
|
if (angularDiff > Angle::degToRad(135)) {return 0.01;}
|
|
if (angularDiff > Angle::degToRad(90)) {return 0.02;}
|
|
if (angularDiff > Angle::degToRad(45)) {return 0.07;}
|
|
{return 0.90;}
|
|
|
|
|
|
// add error to allow stronger deviation with respect to the "BIG GLOBAL SCOPE"
|
|
|
|
// determine probability
|
|
// const float prob = dist.getProbability(angularDiff);
|
|
// return prob;
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
#endif // WALKMODULEHEADINGCONTROL_H
|