Added mpc option to say you only care about the first time we get to the target

dlib/control/mpc.h

@@ -184,6 +184,19 @@ namespace dlib
             return target[time];
         }
 
+        double get_target_error_threshold (
+        ) const 
+        {
+            return target_error_threshold;
+        }
+
+        void set_target_error_threshold (
+            const double thresh 
+        )
+        {
+            target_error_threshold = thresh;
+        }
+
         unsigned long get_max_iterations (
         ) const { return max_iterations; }
 
@@ -258,8 +271,23 @@ namespace dlib
             M[0] = A*initial_state + C;
             for (unsigned long i = 1; i < horizon; ++i)
                 M[i] = A*M[i-1] + C;
-            for (unsigned long i = 0; i < horizon; ++i)
+            double min_error_seen = std::numeric_limits<double>::infinity();
+            for (unsigned long i = 0; i < horizon; ++i) {
                 M[i] = diagm(Q)*(M[i]-target[i]);
+                if (target_error_threshold >= 0) {
+                    const double current_error = dot(M[i]-target[i], M[i]);
+                    min_error_seen = std::min(current_error, min_error_seen);
+                    // Once our trajectory gets us within target_error_threshold of the target at any time
+                    // then we essentially stop caring about what happens at times after that.  This
+                    // gives us a "just hit the target, I don't care what happens after the hit" model.
+                    if (min_error_seen < target_error_threshold) 
+                    {
+                        // Make it so all future errors now appear to be 0.  E.g. it is as if target[i]
+                        // was equal to the state the current control sequence generates at time i.
+                        M[i] = 0;
+                    }
+                }
+            }
             for (long i = (long)horizon-2; i >= 0; --i)
                 M[i] += trans(A)*M[i+1];
             for (unsigned long i = 0; i < horizon; ++i)
@@ -348,6 +376,7 @@ namespace dlib
 
         unsigned long max_iterations;
         double eps;
+        double target_error_threshold = -1;
 
         matrix<double,S,S> A;
         matrix<double,S,I> B;

dlib/control/mpc_abstract.h

@@ -75,6 +75,7 @@ namespace dlib
                 - The A,B,C,Q,R,lower, and upper parameter matrices are filled with zeros.
                   Therefore, to use this object you must initialize it via the constructor
                   that supplies these parameters.
+                - #get_target_error_threshold() == -1
         !*/
 
         mpc (
@@ -108,6 +109,7 @@ namespace dlib
                     - get_target(i).size() == A.nr()
                 - #get_max_iterations() == 10000 
                 - #get_epsilon() == 0.01
+                - #get_target_error_threshold() == -1
         !*/
 
         const matrix<double,S,S>& get_A (
@@ -205,6 +207,29 @@ namespace dlib
                 - performs: set_target(val, horizon-1)
         !*/
 
+        double get_target_error_threshold (
+        ) const;
+        /*!
+            ensures
+                - The target error terms in the objective function with values less than
+                  get_target_error_threshold() are ignored.  That is, the
+                  trans(x_i-target_i)*Q*(x_i-target_i) terms with values less than this are dropped
+                  from the objective function.  Therefore, setting get_target_error_threshold() to a
+                  value >= 0 allows you to encode a control law that says "find me the controls that
+                  make the target error less than or equal to this at some point, but I don't care
+                  what happens at times after that."
+        !*/
+
+        void set_target_error_threshold (
+            const double thresh 
+        );
+        /*!
+            ensures
+                - #target_error_threshold() == thresh
+        !*/
+
+
+
         unsigned long get_max_iterations (
         ) const; 
         /*!
@@ -256,7 +281,7 @@ namespace dlib
                 - A.nr() == current_state.size()
             ensures
                 - Solves the model predictive control problem defined by the arguments to
-                  this objects constructor, assuming that the starting state is given by
+                  this object's constructor, assuming that the starting state is given by
                   current_state.  Then we return the control that should be taken in the
                   current state that best optimizes the quadratic objective function
                   defined above.

dlib/test/mpc.cpp

@@ -253,6 +253,75 @@ namespace
         return iter;
     }
 
+    void test_with_positive_target_error_thresh() 
+    {
+        // a basic position + velocity model
+        matrix<double,2,2> A;
+        A = 1, 1,
+            0, 1;
+        matrix<double,2,1> B, C;
+        B = 0,
+            1;
+
+        C = 0.0,0.0; // no constant bias
+
+        matrix<double,2,1> Q;
+        Q = 2, 0; // only care about getting the position right
+        matrix<double,1,1> R, lower, upper;
+        R = 0.001;
+
+        // We set it up so that the controller that only cares about getting to the target but
+        // doesn't care about what happens after can just give large controls of -1.  It will fly
+        // past the target once it gets there, but it doesn't care about that.  The solver_normal
+        // however wants to come to rest at the target.  So it will have to give a much smaller
+        // control to get it moving towards the target and then slowly decelerate over time to end
+        // up stopping.  So it will take longer to get to the target, which we test below.
+        lower = -1.0;
+        upper =  0.05;
+
+        mpc<2,1,30> solver_normal(A,B,C,Q,R,lower,upper);
+        solver_normal.set_epsilon(0.00000001);
+        solver_normal.set_max_iterations(10000);
+
+        mpc<2,1,30> solver_target_thresh = solver_normal;
+        solver_target_thresh.set_target_error_threshold(1.0);
+
+        matrix<double,2,1> state_normal;
+        state_normal = 10, 0;
+        matrix<double,2,1> state_target_thresh = state_normal;
+
+        // Run the control law with and without set_target_error_threshold() called.  We should
+        // observe that the controller using set_target_error_threshold(1) drives us to the target
+        // state faster, however, only the normal solver results in a rest state at the target
+        // state.
+        int time_at_target_normal = -1;
+        int time_at_target_other  = -1;
+        for (int i = 0; i < 30; ++i)
+        {
+            print_spinner();
+            const double normal_control = solver_normal(state_normal);
+            state_normal = A*state_normal + B*normal_control + C;
+            
+            const double target_thresh_control = solver_target_thresh(state_target_thresh);
+            state_target_thresh = A*state_target_thresh + B*target_thresh_control + C;
+
+            const double normal_error = trans(state_normal)*diagm(Q)*state_normal;
+            const double target_error = trans(state_target_thresh)*diagm(Q)*state_target_thresh;
+            if (normal_error < 1.0 && time_at_target_normal == -1)
+                time_at_target_normal = i;
+            if (target_error < 1.0 && time_at_target_other == -1)
+                time_at_target_other = i;
+        }
+
+        // Check that the normal solver took longer to get to the target.
+        DLIB_TEST(time_at_target_normal == 13);
+        DLIB_TEST(time_at_target_other == 7);
+
+        // Check that the normal solver ends up right on the target.
+        DLIB_TEST(length(state_normal) < 0.01);
+        // But the one that got there faster blew past the target and is no way off in the distance.
+        DLIB_TEST(length(state_target_thresh) > 20);
+    }
 
 
     class test_mpc : public tester
@@ -336,6 +405,8 @@ namespace
                 dlog << LINFO << "objective value2: " << 0.5*trans(alpha2)*Q*alpha + trans(b)*alpha2;
                 DLIB_TEST_MSG(max(abs(alpha-alpha2)) < 1e-7, max(abs(alpha-alpha2)));
             }
+
+            test_with_positive_target_error_thresh();
         }
     } a;