finite_differences.cpp

Go to the documentation of this file.

#include <mpc/finite_differences.h>

//#include <ros/ros.h>

#include <cmath>

namespace finite_differences {

        static Dynamics dyn_;
        static CostFunction cost_;
        static float delta = 1e-4;


        void setBaseFunctions(Dynamics d, CostFunction c) {
                dyn_ = d;
                cost_ = c;
        }

        void dynamics(const VectorT &x, const VectorT &u, VectorT &xNext) {
                dyn_(x, u, xNext);
        }

        
        void fu_(const VectorT &x, const VectorT &u, MatrixT &fu) {
                int n = x.size();
                int m = u.size();

                VectorT base_val = VectorT(x.size()); 
                VectorT utmp = u;

                // calculate finite differences
                dyn_(x, u, base_val);
                for (int i = 0; i < m; ++i) {
                        utmp(i) = u(i) + delta;
                        VectorT val;
                        dyn_(x, utmp, val);
                        fu.col(i) =  (val - base_val) / delta;
                        utmp(i) = u(i);
                }
        }

        void fx_(const VectorT &x, const VectorT &u, MatrixT &fx) {
                int n = x.size();
                int m = u.size();

                VectorT base_val = VectorT(x.size()); 
                VectorT xtmp = x;

                // calculate finite differences
                dyn_(x, u, base_val);
                for (int i = 0; i < n; ++i) {
                        xtmp(i) = x(i) + delta;
                        VectorT val;
                        dyn_(xtmp, u, val);
                        fx.col(i) =  (val - base_val) / delta;
                        xtmp(i) = x(i);
                }
        }

        /*
        void fxx_(const Vector &x, const VectorT &u, std::vector<MatrixT> &fxx) {
                MatrixT base_val;
                VectorT xtmp = x;
                
                // calc finite differences
                fx_(x, u, base_val);
                for (int i = 0; i < n; ++i) {
                        xtmp(i) = x(i) + delta;
                        MatrixT val;
                        fx_(xtmp, u, val);
                        fxx_[i] =  (base_val - val) / delta;
                        xtmp(i) = x(i);
                }
1        }

        void fxu_(const Vector &x, const VectorT &u, std::vector<MatrixT> &fxx) {
                MatrixT base_val;
                VectorT utmp = u;
                
                // calc finite differences
                fx_(x, u, base_val);
                for (int i = 0; i < n; ++i) {
                        utmp(i) = u(i) + delta;
                        MatrixT val;
                        fx_(xtmp, u, val);
                        fxx_[i] =  (base_val - val) / delta;
                        xtmp(i) = x(i);
                }
        }
        */

        void dynamicsD(const VectorT &x, const VectorT &u, Deriv &d) {
                int n = x.size();
                int m = u.size();
        
                VectorT base_val; 
                dyn_(x, u, base_val);

                d.fx.setZero(n, n);
                fx_(x, u, d.fx);
        
                d.fu.setZero(n, m);
                fu_(x, u, d.fu);
        
                // TODO
                d.fxx.resize(n);
                d.fxu.resize(n);
                d.fuu.resize(n);
                for (int i = 0; i < n; ++i) {
                        d.fxx[i].setZero(n,n);
                        d.fxu[i].setZero(n,m);
                        d.fuu[i].setZero(m,m);
                }
                
        }

        float cost(const VectorT &x, const VectorT &u) {
                return cost_(x,u);
        }

        void costD(const VectorT &x, const VectorT &u, Deriv &d) { 
                //ros::Time start = ros::Time::now();
                int n = x.size();
                int m = u.size();

                float base_val = cost(x, u);

                VectorT utmp = u;
                d.cu.setZero(m);
                for (int i = 0; i < m; ++i) {
                        utmp(i) = u(i) + delta;
                        float val = cost(x, utmp);
                        d.cu(i) = (val - base_val) / delta;
                        utmp(i) = u(i);
                }

                VectorT xtmp = x;
                d.cx.setZero(n);
                for (int i = 0; i < n; ++i) {
                        xtmp(i) = x(i) + delta;
                        float val = cost(xtmp, u);
                        d.cx(i) = (val - base_val) / delta;
                        xtmp(i) = x(i);
                }

                // TODO
                d.cuu.setIdentity(m, m);
                d.cxx.setIdentity(n, n);
                d.cxu.setZero(n, m);
                //ROS_INFO_STREAM("time in costD: " << (ros::Time::now() - start).toSec());
        }

}