euclidean.cpp
Go to the documentation of this file.
00001 #include <mpc/examples/euclidean.h>
00002 
00003 #include <cmath>
00004 #include <algorithm>
00005 
00006 namespace euclidean {
00007         
00008         static VectorT goal;
00009         static EuclideanObstacles obstacles;
00010 
00011         static float h = 0.5f;
00012         static float c_b   = 0.0007f;
00013         static float c_u   = 0.1f;
00014         static float sigma = 0.01f;
00015         static float c_xT = 0.01f;
00016 
00017         void setGoal(const VectorT &g) {
00018                 goal = g;
00019         }
00020 
00021         void setObstacles(const EuclideanObstacles &obs) {
00022                 obstacles = obs;
00023         }
00024 
00025         EuclideanObstacles &getObstacles() {
00026                 return obstacles;
00027         }
00028 
00029         void dynamics(const VectorT &x, const VectorT &u, VectorT &xNext) {
00030                 //int n = x.size();
00031                 xNext = x + u;
00032         }
00033 
00034         void dynamicsD(const VectorT &x, const VectorT &u, Deriv &d) {
00035                 int n = x.size();
00036                 int m = u.size();
00037         
00038                 d.fx.resize(n, n);
00039                 d.fx.setIdentity();
00040         
00041                 d.fu.resize(n, n);
00042                 d.fu.setIdentity();
00043         
00044                 d.fxx.resize(n);
00045                 d.fxu.resize(n);
00046                 d.fuu.resize(n);
00047                 for (int i = 0; i < n; ++i) {
00048                         d.fxx[i].setZero(n,n);
00049                         d.fxu[i].setZero(n,m);
00050                         d.fuu[i].setZero(m,m);
00051                 }
00052         }
00053 
00054         float cost(const VectorT &x, const VectorT &u) {
00055 
00056                 float goalCost = c_xT * (x - goal).squaredNorm(); 
00057                 float obstacleSum = 0.;
00058                 for (int i = 0; i < obstacles.size(); ++i) {
00059                         obstacleSum += std::exp((-1.f / sigma) * (x - obstacles[i]).squaredNorm());
00060                 }
00061                 // if (obstacleSum != 0.)
00062                 //         std::cout <<  "obstacleSum " << obstacleSum << std::endl;
00063                 float totalCost = goalCost + std::pow(c_u, h) * u.squaredNorm() + c_b * obstacleSum;
00064                 return totalCost;
00065         }
00066 
00067         void costD(const VectorT &x, const VectorT &u, Deriv &d) { 
00068                 int n = x.size();
00069                 int m = u.size();
00070 
00071                 d.cu = 2.f * std::pow(c_u, h) * u;
00072 
00073                 d.cuu.setIdentity(m, m);
00074                 d.cuu *= 2.f * std::pow(c_u, h);
00075 
00076                 d.cx = 2.f * c_xT * (x - goal);
00077                 d.cxx.setIdentity(n, n);
00078                 d.cxx *= 2.f * c_xT;
00079 
00080                 VectorT cx_obs;
00081                 MatrixT cxx_obs;
00082                 cx_obs.setZero(x.size());
00083                 cxx_obs.setZero(n, n);
00084                 for (int i = 0; i < obstacles.size(); ++i) {
00085                         VectorT tmp_diff = (x - obstacles[i]);
00086                         MatrixT tmp_diff_outer = tmp_diff * tmp_diff.transpose();
00087                         cx_obs += (-2.f/sigma) * tmp_diff * std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
00088                         //cxx_obs.array() +=  ((4. * tmp_diff_outer.array() - 2. * sigma) / (sigma * sigma)) *  std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
00089                         cxx_obs.array() +=  (4. * tmp_diff_outer.array());
00090                         cxx_obs.diagonal().array() -=  2. * sigma;
00091                         cxx_obs.array() = (cxx_obs.array() / (sigma * sigma)) *  std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
00092                 }
00093                 d.cx += c_b * cx_obs;
00094                 d.cxx += c_b * cxx_obs;
00095 
00096                 d.cxu.setZero(n, m);
00097         }
00098 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


mpc
Author(s): Jost Tobias Springenberg
autogenerated on Wed Dec 26 2012 16:21:13