Draft: Action planning improvements

components/falcons/action_aimed_kick/interface/Common.proto

@@ -5,7 +5,8 @@ package MRA.FalconsActionAimedKick;
 enum ShootingPhase
 {
     SHOOT_PHASE_INVALID = 0;
-    SHOOT_PHASE_PREPARE = 1; // moving the height actuator may take a while
-    SHOOT_PHASE_DISCHARGE = 2; // one moment actually, not really a phase
-    SHOOT_PHASE_COOLDOWN = 3; // give ball some time to leave before signaling SUCCESS, perhaps evaluate shot result
+    SHOOT_PHASE_AIM_COARSE = 1;
+    SHOOT_PHASE_AIM_FINE = 2;
+    SHOOT_PHASE_DISCHARGE = 3; // one moment actually, not really a phase
+    SHOOT_PHASE_COOLDOWN = 4; // give ball some time to leave before signaling SUCCESS, perhaps evaluate shot result
 }

components/falcons/action_aimed_kick/interface/DefaultParams.json

@@ -1,5 +1,13 @@
 {
     "angleAccuracyThreshold": 0.01,
     "maxCooldownDuration": 1.5,
-    "ballTargetProximity": 2.0
+    "ballTargetProximity": 2.0,
+    "timeout": {
+        "angleAccuracyThreshold": 0.5,
+        "duration": 1.0
+    },
+    "precision": {
+        "angleAccuracyThreshold": 0.001,
+        "duration": 2.0
+    }
 }

components/falcons/action_aimed_kick/interface/Diagnostics.proto

@@ -6,5 +6,5 @@ message Diagnostics
 {
     double remainingRotationAngle = 1;
     double aimError = 2; // radians, evaluation after shot, fill in when PASSED
-    string verdict = 3;
+    string failureReason = 3;
 }

components/falcons/action_aimed_kick/interface/Input.proto

@@ -5,8 +5,16 @@ package MRA.FalconsActionAimedKick;
 import "datatypes/WorldState.proto";
 import "datatypes/PosVel.proto";
 
+message BallHandlers
+{
+    double left = 1;
+    double right = 2;
+}
+
 message Input
 {
     MRA.Datatypes.WorldState worldstate = 1;
     MRA.Datatypes.PosVel target = 2; // typically only using pos.x and pos.y
+    BallHandlers arms = 3; // can be used for aim correction
+    bool precisionMode = 4; // set this flag for setpieces when robot has plenty of time
 }

components/falcons/action_aimed_kick/interface/Params.proto

@@ -2,9 +2,24 @@ syntax = "proto3";
 
 package MRA.FalconsActionAimedKick;
 
+
+message TimeoutParams // associated to AIM_FINE phase
+{
+    double angleAccuracyThreshold = 1; // start the timer when delta angle is smaller than this threshold
+    double duration = 2; // time in seconds before releasing anyway
+}
+
+message PrecisionParams // only used when precision mode is requested
+{
+    double angleAccuracyThreshold = 1; // overrule with a smaller value
+    double duration = 2; // overrule with a larger value
+}
+
 message Params
 {
     double angleAccuracyThreshold = 1;
     double maxCooldownDuration = 2;
     double ballTargetProximity = 3;
+    TimeoutParams timeout = 4;
+    PrecisionParams precision = 5;
 }

components/falcons/action_aimed_kick/interface/State.proto

@@ -9,4 +9,6 @@ message State
 {
     ShootingPhase phase = 1;
     google.protobuf.Timestamp dischargeTimestamp = 2;
+    google.protobuf.Timestamp timeoutStartTimestamp = 3; // a timer starts when advancing from AIM_COARSE to AIM_FINE
+    bool aimTimerExpired = 4;
 }

components/falcons/action_aimed_kick/tick.cpp

@@ -32,10 +32,12 @@ class ActionAimedKick
     MRA::Geometry::Position _deltaCurrentBallToRobot;
     float _remainingRotationAngle;
     void precalculate();
-    void phasePrepare();
+    void phaseAimCoarse();
+    void phaseAimFine();
     void phaseDischarge();
     float calculateAimError();
     void phaseCooldown();
+    bool checkParams(FalconsActionAimedKick::ParamsType const &params, std::string &failureReason);
 };
 
 int FalconsActionAimedKick::FalconsActionAimedKick::tick
@@ -69,12 +71,20 @@ ActionAimedKick::ActionAimedKick(google::protobuf::Timestamp const &timestamp, F
 int ActionAimedKick::run()
 {
     MRA_TRACE_FUNCTION();
+    // check params
+    std::string failureReason;
+    if (!checkParams(_params, failureReason))
+    {
+        _output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
+        _diagnostics.set_failurereason(failureReason);
+        return 0;
+    }
     // fail when there is no ball
     if (!_input.worldstate().has_ball())
     {
         _output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
         _output.set_phase(MRA::FalconsActionAimedKick::SHOOT_PHASE_INVALID);
-        _diagnostics.set_verdict("robot lost track of the ball");
+        _diagnostics.set_failurereason("robot lost track of the ball");
         return 0;
     }
     // calculate angles and such
@@ -86,11 +96,16 @@ int ActionAimedKick::run()
     if (_state.phase() == FalconsActionAimedKick::SHOOT_PHASE_INVALID)
     {
         // first tick: advance to PREPARE
-        _state.set_phase(FalconsActionAimedKick::SHOOT_PHASE_PREPARE);
+        _state.set_phase(FalconsActionAimedKick::SHOOT_PHASE_AIM_COARSE);
     }
-    if (_state.phase() == FalconsActionAimedKick::SHOOT_PHASE_PREPARE)
+    if (_state.phase() == FalconsActionAimedKick::SHOOT_PHASE_AIM_COARSE)
     {
-        phasePrepare();
+        phaseAimCoarse();
+    }
+    // allow to fall through coarse phase
+    if (_state.phase() == FalconsActionAimedKick::SHOOT_PHASE_AIM_FINE)
+    {
+        phaseAimFine();
     }
     else if (_state.phase() == FalconsActionAimedKick::SHOOT_PHASE_DISCHARGE)
     {
@@ -120,19 +135,50 @@ void ActionAimedKick::precalculate()
     MRA_TRACE_FUNCTION_OUTPUTS(_remainingRotationAngle);
 }
 
-void ActionAimedKick::phasePrepare()
+void ActionAimedKick::phaseAimCoarse()
 {
     MRA_TRACE_FUNCTION();
+    double angleAccuracyThreshold = _params.timeout().angleaccuracythreshold();
     if (!_input.worldstate().robot().hasball())
     {
-        // TODO: robustness: use state - it can happen that the robot kicked the ball away, but it takes a tick or more for the ball to actually leave?
         // in that case, functionally the shot was a success, so we should not produce FAILED
         _output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-        _diagnostics.set_verdict("robot lost possession of the ball");
+        _diagnostics.set_failurereason("robot lost possession of the ball");
         return;
     }
+    // prepare, aim, running
+    *_output.mutable_motiontarget()->mutable_position() = _input.worldstate().robot().position();
+    _output.mutable_motiontarget()->mutable_position()->set_rz(_ballTargetPos.rz);
+    *_output.mutable_balltarget() = _input.target().position();
+    _output.set_actionresult(MRA::Datatypes::ActionResult::RUNNING);
+    _diagnostics.set_remainingrotationangle(_remainingRotationAngle);
+    // advance to next phase?
+    if (abs(_remainingRotationAngle) < angleAccuracyThreshold)
+    {
+        *_state.mutable_timeoutstarttimestamp() = _timestamp; // start the timer
+        _output.set_actionresult(MRA::Datatypes::ActionResult::RUNNING);
+        _state.set_phase(MRA::FalconsActionAimedKick::SHOOT_PHASE_AIM_FINE);
+    }
+}
+
+void ActionAimedKick::phaseAimFine()
+{
+    MRA_TRACE_FUNCTION();
+    double angleAccuracyThreshold = _params.angleaccuracythreshold();
+    double timeoutDuration = _params.timeout().duration();
+    if (!_input.worldstate().robot().hasball())
+    {
+        // in that case, functionally the shot was a success, so we should not produce FAILED
+        _output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
+        _diagnostics.set_failurereason("robot lost possession of the ball");
+        return;
+    }
+    // check the timer
+    auto elapsedDuration = _timestamp - _state.timeoutstarttimestamp();
+    float elapsedSeconds = 1e-9 * google::protobuf::util::TimeUtil::DurationToNanoseconds(elapsedDuration);
+    bool timerExpired = (elapsedSeconds > timeoutDuration);
     // check if phase ends
-    if (abs(_remainingRotationAngle) > _params.angleaccuracythreshold())
+    if (abs(_remainingRotationAngle) > angleAccuracyThreshold && !timerExpired)
     {
         // prepare, aim, running
         *_output.mutable_motiontarget()->mutable_position() = _input.worldstate().robot().position();
@@ -145,10 +191,14 @@ void ActionAimedKick::phasePrepare()
     {
         // shoot, advance to next phase
         // TODO: calculate timeToKick, tune it?
+        _output.Clear();
+        _output.set_bhenabled(true);
         _output.set_dokick(true);
+        *_output.mutable_balltarget() = _input.target().position();
         *_state.mutable_dischargetimestamp() = _timestamp;
         _output.set_actionresult(MRA::Datatypes::ActionResult::RUNNING);
         _state.set_phase(MRA::FalconsActionAimedKick::SHOOT_PHASE_DISCHARGE);
+        _state.set_aimtimerexpired(timerExpired); // store this for diagnostics, but in state so it does not get reset next tick
     }
 }
 
@@ -178,7 +228,7 @@ void ActionAimedKick::phaseCooldown()
     // use a little timeout to make sure this action is not "stuck" for too long
     auto elapsedDuration = _timestamp - _state.dischargetimestamp();
     float elapsedSeconds = 1e-9 * google::protobuf::util::TimeUtil::DurationToNanoseconds(elapsedDuration);
-    // float ballTargetDistance = _deltaBallTargetToCurrentBall.size();
+    float ballTargetDistance = _deltaBallTargetToCurrentBall.size();
     _output.set_actionresult(MRA::Datatypes::ActionResult::RUNNING);
     if (elapsedSeconds > _params.maxcooldownduration())
     {
@@ -188,7 +238,6 @@ void ActionAimedKick::phaseCooldown()
     {
         // determine success
         // if ball ends up close to target, then PASSED
-        float ballTargetDistance = _deltaBallTargetToCurrentBall.size();
         if (ballTargetDistance < _params.balltargetproximity())
         {
             _diagnostics.set_aimerror(calculateAimError());
@@ -197,3 +246,34 @@ void ActionAimedKick::phaseCooldown()
     }
     MRA_TRACE_FUNCTION_OUTPUTS(elapsedSeconds, ballTargetDistance);
 }
+
+bool ActionAimedKick::checkParams(FalconsActionAimedKick::ParamsType const &params, std::string &failureReason)
+{
+    MRA_TRACE_FUNCTION();
+    if (params.angleaccuracythreshold() <= 0.0)
+    {
+        failureReason = "invalid configuration parameter angleAccuracyThreshold: should be larger than zero";
+        return false;
+    }
+    if (params.timeout().angleaccuracythreshold() <= 0.0)
+    {
+        failureReason = "invalid configuration parameter timeout.angleAccuracyThreshold: should be larger than zero";
+        return false;
+    }
+    if (params.timeout().duration() <= 0.0)
+    {
+        failureReason = "invalid configuration parameter timeout.duration: should be larger than zero";
+        return false;
+    }
+    if (params.precision().angleaccuracythreshold() <= 0.0)
+    {
+        failureReason = "invalid configuration parameter precision.angleAccuracyThreshold: should be larger than zero";
+        return false;
+    }
+    if (params.precision().duration() <= 0.0)
+    {
+        failureReason = "invalid configuration parameter precision.duration: should be larger than zero";
+        return false;
+    }
+    return true;
+}

components/falcons/action_catch_ball/interface/Diagnostics.proto

@@ -8,5 +8,5 @@ message Diagnostics
     bool ballMovingFastEnough = 2;
     bool ballMovingWithinCaptureRange = 3;
     double timeToCatch = 4;
-    string verdict = 5;
+    string failureReason = 5;
 }

components/falcons/action_catch_ball/tick.cpp

@@ -10,7 +10,7 @@ using namespace MRA;
 #include "geometry.hpp"
 
 
-bool checkParams(FalconsActionCatchBall::ParamsType const &params, std::string &verdict);
+bool checkParams(FalconsActionCatchBall::ParamsType const &params, std::string &failureReason);
 int calc_intercept_strafe(FalconsActionCatchBall::InputType const &input, FalconsActionCatchBall::ParamsType const &params, FalconsActionCatchBall::OutputType &output, FalconsActionCatchBall::DiagnosticsType &diagnostics);
 
 int FalconsActionCatchBall::FalconsActionCatchBall::tick
@@ -34,11 +34,11 @@ int FalconsActionCatchBall::FalconsActionCatchBall::tick
         diagnostics.Clear();
 
         // check params
-        std::string verdict;
-        if (!checkParams(params, verdict))
+        std::string failureReason;
+        if (!checkParams(params, failureReason))
         {
             output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-            diagnostics.set_verdict(verdict);
+            diagnostics.set_failurereason(failureReason);
             return 0;
         }
 
@@ -56,15 +56,15 @@ int FalconsActionCatchBall::FalconsActionCatchBall::tick
         if (!ws.robot().active())
         {
             output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-            diagnostics.set_verdict("robot is inactive");
+            diagnostics.set_failurereason("robot is inactive");
             return error_value;
         }
 
         // fail when there is no ball
         if (!ws.has_ball())
         {
             output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-            diagnostics.set_verdict("robot lost track of the ball");
+            diagnostics.set_failurereason("robot lost track of the ball");
             return error_value;
         }
 
@@ -74,7 +74,7 @@ int FalconsActionCatchBall::FalconsActionCatchBall::tick
             if (teammember.hasball())
             {
                 output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-                diagnostics.set_verdict("teammate got the ball");
+                diagnostics.set_failurereason("teammate got the ball");
                 return error_value;
             }
         }
@@ -90,7 +90,7 @@ int FalconsActionCatchBall::FalconsActionCatchBall::tick
             if (state.ballwasmovingfastenough())
             {
                 output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-                diagnostics.set_verdict("ball not moving fast enough anymore");
+                diagnostics.set_failurereason("ball not moving fast enough anymore");
             }
             else
             {
@@ -109,7 +109,7 @@ int FalconsActionCatchBall::FalconsActionCatchBall::tick
         if (!ballmovingtowardsrobot)
         {
             output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-            diagnostics.set_verdict("ball not moving towards robot");
+            diagnostics.set_failurereason("ball not moving towards robot");
             return error_value;
         }
 
@@ -118,7 +118,7 @@ int FalconsActionCatchBall::FalconsActionCatchBall::tick
         {
             //return calc_intercept_proactive(input, params, output, diagnostics);
             output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-            diagnostics.set_verdict("proactive intercept not yet implemented");
+            diagnostics.set_failurereason("proactive intercept not yet implemented");
         }
         else
         {
@@ -140,16 +140,16 @@ int FalconsActionCatchBall::FalconsActionCatchBall::tick
     return error_value;
 }
 
-bool checkParams(FalconsActionCatchBall::ParamsType const &params, std::string &verdict)
+bool checkParams(FalconsActionCatchBall::ParamsType const &params, std::string &failureReason)
 {
     if (params.ballspeedthreshold() == 0)
     {
-        verdict = "invalid configuration parameter ballspeedthreshold: should be larger than zero";
+        failureReason = "invalid configuration parameter ballspeedthreshold: should be larger than zero";
         return false;
     }
     if (params.captureradius() == 0)
     {
-        verdict = "invalid configuration parameter captureradius: should be larger than zero";
+        failureReason = "invalid configuration parameter captureradius: should be larger than zero";
         return false;
     }
     return true;
@@ -198,7 +198,7 @@ int calc_intercept_strafe(
     if (!ballmovingwithincapturerange)
     {
         output.set_actionresult(MRA::Datatypes::ActionResult::FAILED);
-        diagnostics.set_verdict("ball trajectory too far away");
+        diagnostics.set_failurereason("ball trajectory too far away");
         return 0;
     }
 
@@ -214,4 +214,4 @@ int calc_intercept_strafe(
     output.mutable_motiontarget()->mutable_position()->set_rz(robot_target_position.rz);
     output.set_actionresult(MRA::Datatypes::ActionResult::RUNNING);
     return 0;
-}
+}

components/falcons/action_fetch_ball/interface/Diagnostics.proto

@@ -4,5 +4,5 @@ package MRA.FalconsActionFetchBall;
 
 message Diagnostics
 {
-    string verdict = 1;
+    string failureReason = 1; // optional, set only if action failed
 }