dlib C++ Library - mpc.cpp

// Copyright (C) 2015 Davis E. King (davis@dlib.net)
// License: Boost Software License See LICENSE.txt for the full license.
#include <string>
#include <sstream>
#include <dlib/control.h>
#include <dlib/optimization.h>
#include "tester.h"
namespace 
{
 using namespace test;
 using namespace std;
 using namespace dlib;
 
 logger dlog("test.mpc");
 template <
 typename EXP1,
 typename EXP2,
 typename T, long NR, long NC, typename MM, typename L
 >
 unsigned long solve_qp_box_using_smo ( 
 const matrix_exp<EXP1>& _Q,
 const matrix_exp<EXP2>& _b,
 matrix<T,NR,NC,MM,L>& alpha,
 matrix<T,NR,NC,MM,L>& lower,
 matrix<T,NR,NC,MM,L>& upper,
 T eps,
 unsigned long max_iter
 )
 /*!
 ensures
 - solves: 0.5*trans(x)*Q*x + trans(b)*x where x is box constrained.
 !*/
 {
 const_temp_matrix<EXP1> Q(_Q);
 const_temp_matrix<EXP2> b(_b);
 //cout << "IN QP SOLVER" << endl;
 //cout << "max eig: " << max(real_eigenvalues(Q)) << endl;
 //cout << "min eig: " << min(real_eigenvalues(Q)) << endl;
 //cout << "Q: \n" << Q << endl;
 //cout << "b: \n" << b << endl;
 // make sure requires clause is not broken
 DLIB_ASSERT(Q.nr() == Q.nc() &&
 alpha.size() == lower.size() &&
 alpha.size() == upper.size() &&
 is_col_vector(b) &&
 is_col_vector(alpha) &&
 is_col_vector(lower) &&
 is_col_vector(upper) &&
 b.size() == alpha.size() &&
 b.size() == Q.nr() &&
 alpha.size() > 0 &&
 0 <= min(alpha-lower) &&
 0 <= max(upper-alpha) &&
 eps > 0 &&
 max_iter > 0,
 "\t unsigned long solve_qp_box_using_smo()"
 << "\n\t Invalid arguments were given to this function"
 << "\n\t Q.nr(): " << Q.nr()
 << "\n\t Q.nc(): " << Q.nc()
 << "\n\t is_col_vector(b): " << is_col_vector(b)
 << "\n\t is_col_vector(alpha): " << is_col_vector(alpha)
 << "\n\t is_col_vector(lower): " << is_col_vector(lower)
 << "\n\t is_col_vector(upper): " << is_col_vector(upper)
 << "\n\t b.size(): " << b.size() 
 << "\n\t alpha.size(): " << alpha.size() 
 << "\n\t lower.size(): " << lower.size() 
 << "\n\t upper.size(): " << upper.size() 
 << "\n\t Q.nr(): " << Q.nr() 
 << "\n\t min(alpha-lower): " << min(alpha-lower) 
 << "\n\t max(upper-alpha): " << max(upper-alpha) 
 << "\n\t eps: " << eps 
 << "\n\t max_iter: " << max_iter 
 );
 // Compute f'(alpha) (i.e. the gradient of f(alpha)) for the current alpha. 
 matrix<T,NR,NC,MM,L> df = Q*alpha + b;
 matrix<T,NR,NC,MM,L> QQ = reciprocal_max(diag(Q));
 unsigned long iter = 0;
 for (; iter < max_iter; ++iter)
 {
 T max_df = 0;
 long best_r =0;
 for (long r = 0; r < Q.nr(); ++r)
 {
 if (alpha(r) <= lower(r) && df(r) > 0)
 ;//alpha(r) = lower(r);
 else if (alpha(r) >= upper(r) && df(r) < 0)
 ;//alpha(r) = upper(r);
 else if (std::abs(df(r)) > max_df)
 {
 best_r = r;
 max_df = std::abs(df(r));
 }
 }
 //for (long r = 0; r < Q.nr(); ++r)
 long r = best_r;
 {
 const T old_alpha = alpha(r);
 alpha(r) = -(df(r)-Q(r,r)*alpha(r))*QQ(r);
 if (alpha(r) < lower(r))
 alpha(r) = lower(r);
 else if (alpha(r) > upper(r))
 alpha(r) = upper(r);
 const T delta = old_alpha-alpha(r);
 // Now update the gradient. We will perform the equivalent of: df = Q*alpha + b;
 for(long k = 0; k < df.nr(); ++k)
 df(k) -= Q(r,k)*delta;
 }
 if (max_df < eps)
 break;
 }
 //cout << "df: \n" << trans(df) << endl;
 //cout << "objective value: " << 0.5*trans(alpha)*Q*alpha + trans(b)*alpha << endl;
 return iter+1;
 }
// ----------------------------------------------------------------------------------------
 namespace impl_mpc
 {
 template <long N>
 void pack(
 matrix<double,0,1>& out,
 const std::vector<matrix<double,N,1> >& item
 )
 {
 DLIB_CASSERT(item.size() != 0,"");
 out.set_size(item.size()*item[0].size());
 long j = 0;
 for (unsigned long i = 0; i < item.size(); ++i)
 for (long r = 0; r < item[i].size(); ++r)
 out(j++) = item[i](r);
 }
 template <long N>
 void pack(
 matrix<double,0,1>& out,
 const matrix<double,N,1>& item,
 const long num
 )
 {
 out.set_size(item.size()*num);
 long j = 0;
 for (long r = 0; r < num; ++r)
 for (long i = 0; i < item.size(); ++i)
 out(j++) = item(i);
 }
 template <long N>
 void unpack(
 std::vector<matrix<double,N,1> >& out,
 const matrix<double,0,1>& item 
 )
 {
 DLIB_CASSERT(out.size() != 0,"");
 DLIB_CASSERT((long)out.size()*out[0].size() == item.size(),"");
 long j = 0;
 for (unsigned long i = 0; i < out.size(); ++i)
 for (long r = 0; r < out[i].size(); ++r)
 out[i](r) = item(j++);
 }
 }
 template <long S, long I>
 unsigned long solve_linear_mpc (
 const matrix<double,S,S>& A,
 const matrix<double,S,I>& B,
 const matrix<double,S,1>& C,
 const matrix<double,S,1>& Q,
 const matrix<double,I,1>& R,
 const matrix<double,I,1>& _lower,
 const matrix<double,I,1>& _upper,
 const std::vector<matrix<double,S,1> >& target, 
 const matrix<double,S,1>& initial_state,
 std::vector<matrix<double,I,1> >& controls // input and output
 )
 {
 using namespace impl_mpc;
 DLIB_CASSERT(target.size() == controls.size(),"");
 matrix<double> K(B.nr()*controls.size(), B.nc()*controls.size());
 matrix<double,0,1> M(B.nr()*controls.size());
 // compute powers of A: Apow[i] == A^i
 std::vector<matrix<double,S,S> > Apow(controls.size());
 Apow[0] = identity_matrix(A);
 for (unsigned long i = 1; i < Apow.size(); ++i)
 Apow[i] = A*Apow[i-1];
 // fill in K
 K = 0;
 for (unsigned long r = 0; r < controls.size(); ++r)
 for (unsigned long c = 0; c <= r; ++c)
 set_subm(K,r*B.nr(),c*B.nc(), B.nr(), B.nc()) = Apow[r-c]*B;
 // fill in M
 set_subm(M,0*A.nr(),0,A.nr(),1) = A*initial_state + C;
 for (unsigned long i = 1; i < controls.size(); ++i)
 set_subm(M,i*A.nr(),0,A.nr(),1) = A*subm(M,(i-1)*A.nr(),0,A.nr(),1) + C;
 //cout << "M: \n" << M << endl;
 //cout << "K: \n" << K << endl;
 matrix<double,0,1> t, v, lower, upper;
 pack(t, target);
 pack(v, controls);
 pack(lower, _lower, controls.size());
 pack(upper, _upper, controls.size());
 matrix<double> QQ(K.nr(),K.nr()), RR(K.nc(),K.nc());
 QQ = 0;
 RR = 0;
 for (unsigned long c = 0; c < controls.size(); ++c)
 {
 set_subm(QQ,c*Q.nr(),c*Q.nr(),Q.nr(),Q.nr()) = diagm(Q);
 set_subm(RR,c*R.nr(),c*R.nr(),R.nr(),R.nr()) = diagm(R);
 }
 matrix<double> m1 = trans(K)*QQ*K+RR;
 matrix<double> m2 = trans(K)*QQ*(M-t);
 // run the solver...
 unsigned long iter;
 iter = solve_qp_box_using_smo(
 m1,
 m2,
 v,
 lower,
 upper,
 0.00000001,
 100000);
 //cout << "iterations: " << iter << endl;
 unpack(controls, v);
 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
 {
 public:
 test_mpc (
 ) :
 tester ("test_mpc",
 "Runs tests on the mpc object.")
 {}
 void perform_test (
 )
 {
 // 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.02,0.1; // 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 = 1;
 lower = -0.2;
 upper = 0.2;
 std::vector<matrix<double,1,1> > controls(30);
 std::vector<matrix<double,2,1> > target(30);
 for (unsigned long i = 0; i < controls.size(); ++i)
 {
 controls[i] = 0;
 target[i] = 0;
 }
 mpc<2,1,30> solver(A,B,C,Q,R,lower,upper);
 solver.set_epsilon(0.00000001);
 solver.set_max_iterations(10000);
 matrix<double,2,1> initial_state;
 initial_state = 0;
 initial_state(0) = 5;
 for (int i = 0; i < 30; ++i)
 {
 print_spinner();
 matrix<double,1,1> control = solver(initial_state);
 for (unsigned long i = 1; i < controls.size(); ++i)
 controls[i-1] = controls[i];
 // Compute the correct control via SMO and make sure it matches.
 solve_linear_mpc(A,B,C,Q,R,lower,upper, target, initial_state, controls);
 dlog << LINFO << "ERROR: " << length(control-controls[0]);
 DLIB_TEST(length(control-controls[0]) < 1e-7);
 initial_state = A*initial_state + B*control + C;
 //cout << control(0) << "\t" << trans(initial_state);
 }
 {
 // also just generally test our QP solver.
 matrix<double,20,20> Q = gaussian_randm(20,20,5);
 Q = Q*trans(Q);
 matrix<double,20,1> b = randm(20,1)-0.5;
 matrix<double,20,1> alpha, lower, upper, alpha2;
 alpha = 0;
 alpha2 = 0;
 lower = -4;
 upper = 3;
 solve_qp_box_using_smo(Q,b,alpha,lower, upper, 1e-12, 500000);
 solve_qp_box_constrained(Q,b,alpha2,lower, upper, 1e-12, 50000);
 dlog << LINFO << trans(alpha);
 dlog << LINFO << trans(alpha2);
 dlog << LINFO << "objective value: " << 0.5*trans(alpha)*Q*alpha + trans(b)*alpha;
 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;
}