euclidean.cpp

Go to the documentation of this file.

#include <mpc/examples/euclidean.h>

#include <cmath>
#include <algorithm>

namespace euclidean {
        
        static VectorT goal;
        static EuclideanObstacles obstacles;

        static float h = 0.5f;
        static float c_b   = 0.0007f;
        static float c_u   = 0.1f;
        static float sigma = 0.01f;
        static float c_xT = 0.01f;

        void setGoal(const VectorT &g) {
                goal = g;
        }

        void setObstacles(const EuclideanObstacles &obs) {
                obstacles = obs;
        }

        EuclideanObstacles &getObstacles() {
                return obstacles;
        }

        void dynamics(const VectorT &x, const VectorT &u, VectorT &xNext) {
                //int n = x.size();
                xNext = x + u;
        }

        void dynamicsD(const VectorT &x, const VectorT &u, Deriv &d) {
                int n = x.size();
                int m = u.size();
        
                d.fx.resize(n, n);
                d.fx.setIdentity();
        
                d.fu.resize(n, n);
                d.fu.setIdentity();
        
                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) {

                float goalCost = c_xT * (x - goal).squaredNorm(); 
                float obstacleSum = 0.;
                for (int i = 0; i < obstacles.size(); ++i) {
                        obstacleSum += std::exp((-1.f / sigma) * (x - obstacles[i]).squaredNorm());
                }
                // if (obstacleSum != 0.)
                //         std::cout <<  "obstacleSum " << obstacleSum << std::endl;
                float totalCost = goalCost + std::pow(c_u, h) * u.squaredNorm() + c_b * obstacleSum;
                return totalCost;
        }

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

                d.cu = 2.f * std::pow(c_u, h) * u;

                d.cuu.setIdentity(m, m);
                d.cuu *= 2.f * std::pow(c_u, h);

                d.cx = 2.f * c_xT * (x - goal);
                d.cxx.setIdentity(n, n);
                d.cxx *= 2.f * c_xT;

                VectorT cx_obs;
                MatrixT cxx_obs;
                cx_obs.setZero(x.size());
                cxx_obs.setZero(n, n);
                for (int i = 0; i < obstacles.size(); ++i) {
                        VectorT tmp_diff = (x - obstacles[i]);
                        MatrixT tmp_diff_outer = tmp_diff * tmp_diff.transpose();
                        cx_obs += (-2.f/sigma) * tmp_diff * std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
                        //cxx_obs.array() +=  ((4. * tmp_diff_outer.array() - 2. * sigma) / (sigma * sigma)) *  std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
                        cxx_obs.array() +=  (4. * tmp_diff_outer.array());
                        cxx_obs.diagonal().array() -=  2. * sigma;
                        cxx_obs.array() = (cxx_obs.array() / (sigma * sigma)) *  std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
                }
                d.cx += c_b * cx_obs;
                d.cxx += c_b * cxx_obs;

                d.cxu.setZero(n, m);
        }
}