finite_differences.cpp
Go to the documentation of this file.
00001 #include <mpc/finite_differences.h>
00002 
00003 //#include <ros/ros.h>
00004 
00005 #include <cmath>
00006 
00007 namespace finite_differences {
00008 
00009         static Dynamics dyn_;
00010         static CostFunction cost_;
00011         static float delta = 1e-4;
00012 
00013 
00014         void setBaseFunctions(Dynamics d, CostFunction c) {
00015                 dyn_ = d;
00016                 cost_ = c;
00017         }
00018 
00019         void dynamics(const VectorT &x, const VectorT &u, VectorT &xNext) {
00020                 dyn_(x, u, xNext);
00021         }
00022 
00023         
00024         void fu_(const VectorT &x, const VectorT &u, MatrixT &fu) {
00025                 int n = x.size();
00026                 int m = u.size();
00027 
00028                 VectorT base_val = VectorT(x.size()); 
00029                 VectorT utmp = u;
00030 
00031                 // calculate finite differences
00032                 dyn_(x, u, base_val);
00033                 for (int i = 0; i < m; ++i) {
00034                         utmp(i) = u(i) + delta;
00035                         VectorT val;
00036                         dyn_(x, utmp, val);
00037                         fu.col(i) =  (val - base_val) / delta;
00038                         utmp(i) = u(i);
00039                 }
00040         }
00041 
00042         void fx_(const VectorT &x, const VectorT &u, MatrixT &fx) {
00043                 int n = x.size();
00044                 int m = u.size();
00045 
00046                 VectorT base_val = VectorT(x.size()); 
00047                 VectorT xtmp = x;
00048 
00049                 // calculate finite differences
00050                 dyn_(x, u, base_val);
00051                 for (int i = 0; i < n; ++i) {
00052                         xtmp(i) = x(i) + delta;
00053                         VectorT val;
00054                         dyn_(xtmp, u, val);
00055                         fx.col(i) =  (val - base_val) / delta;
00056                         xtmp(i) = x(i);
00057                 }
00058         }
00059 
00060         /*
00061         void fxx_(const Vector &x, const VectorT &u, std::vector<MatrixT> &fxx) {
00062                 MatrixT base_val;
00063                 VectorT xtmp = x;
00064                 
00065                 // calc finite differences
00066                 fx_(x, u, base_val);
00067                 for (int i = 0; i < n; ++i) {
00068                         xtmp(i) = x(i) + delta;
00069                         MatrixT val;
00070                         fx_(xtmp, u, val);
00071                         fxx_[i] =  (base_val - val) / delta;
00072                         xtmp(i) = x(i);
00073                 }
00074 1        }
00075 
00076         void fxu_(const Vector &x, const VectorT &u, std::vector<MatrixT> &fxx) {
00077                 MatrixT base_val;
00078                 VectorT utmp = u;
00079                 
00080                 // calc finite differences
00081                 fx_(x, u, base_val);
00082                 for (int i = 0; i < n; ++i) {
00083                         utmp(i) = u(i) + delta;
00084                         MatrixT val;
00085                         fx_(xtmp, u, val);
00086                         fxx_[i] =  (base_val - val) / delta;
00087                         xtmp(i) = x(i);
00088                 }
00089         }
00090         */
00091 
00092         void dynamicsD(const VectorT &x, const VectorT &u, Deriv &d) {
00093                 int n = x.size();
00094                 int m = u.size();
00095         
00096                 VectorT base_val; 
00097                 dyn_(x, u, base_val);
00098 
00099                 d.fx.setZero(n, n);
00100                 fx_(x, u, d.fx);
00101         
00102                 d.fu.setZero(n, m);
00103                 fu_(x, u, d.fu);
00104         
00105                 // TODO
00106                 d.fxx.resize(n);
00107                 d.fxu.resize(n);
00108                 d.fuu.resize(n);
00109                 for (int i = 0; i < n; ++i) {
00110                         d.fxx[i].setZero(n,n);
00111                         d.fxu[i].setZero(n,m);
00112                         d.fuu[i].setZero(m,m);
00113                 }
00114                 
00115         }
00116 
00117         float cost(const VectorT &x, const VectorT &u) {
00118                 return cost_(x,u);
00119         }
00120 
00121         void costD(const VectorT &x, const VectorT &u, Deriv &d) { 
00122                 //ros::Time start = ros::Time::now();
00123                 int n = x.size();
00124                 int m = u.size();
00125 
00126                 float base_val = cost(x, u);
00127 
00128                 VectorT utmp = u;
00129                 d.cu.setZero(m);
00130                 for (int i = 0; i < m; ++i) {
00131                         utmp(i) = u(i) + delta;
00132                         float val = cost(x, utmp);
00133                         d.cu(i) = (val - base_val) / delta;
00134                         utmp(i) = u(i);
00135                 }
00136 
00137                 VectorT xtmp = x;
00138                 d.cx.setZero(n);
00139                 for (int i = 0; i < n; ++i) {
00140                         xtmp(i) = x(i) + delta;
00141                         float val = cost(xtmp, u);
00142                         d.cx(i) = (val - base_val) / delta;
00143                         xtmp(i) = x(i);
00144                 }
00145 
00146                 // TODO
00147                 d.cuu.setIdentity(m, m);
00148                 d.cxx.setIdentity(n, n);
00149                 d.cxu.setZero(n, m);
00150                 //ROS_INFO_STREAM("time in costD: " << (ros::Time::now() - start).toSec());
00151         }
00152 
00153 }
 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