added WalkModule for von Mises Heading

2017-03-03 12:09:19 +01:00
parent b6be58eebc
commit f7d0d88448
1 changed files with 123 additions and 0 deletions
--- a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h
+++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h
@@ -0,0 +1,123 @@
+#ifndef WALKMODULEHEADINGVONMISES_H
+#define WALKMODULEHEADINGVONMISES_H
+
+#include "WalkModule.h"
+#include "WalkStateHeading.h"
+
+#include "../../../../geo/Heading.h"
+#include "../../../../math/Distributions.h"
+
+
+/** keep the state's heading */
+template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingVonMises : public WalkModule<Node, WalkState> {
+
+private:
+
+    /** van-Mises distribution */
+    Distribution::VonMises<float> dist;
+
+    /** random noise */
+    Distribution::Normal<float> distNoise;
+
+    const Control* ctrl;
+
+public:
+
+    /** ctor 3.0 should be OK! */
+    WalkModuleHeadingVonMises(const Control* ctrl, const float sensorNoiseDegreesSigma) :
+        dist(Distribution::VonMises<float>(0.0f, 2.0f)),
+        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;
+
+        //set kappa of mises
+        float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad));
+        dist.setKappa(kappa);
+
+    }
+
+    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);
+
+        // 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);
+
+        // determine probability
+        return dist.getProbability(angularDiff);
+
+    }
+
+
+};
+
+#endif // WALKMODULEHEADING_H