|
GCOP
1.0
|
00001 // This file is part of libgcop, a library for Geometric Control, Optimization, and Planning (GCOP) 00002 // 00003 // Copyright (C) 2004-2014 Marin Kobilarov <marin(at)jhu.edu> 00004 // 00005 // This Source Code Form is subject to the terms of the Mozilla 00006 // Public License v. 2.0. If a copy of the MPL was not distributed 00007 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00008 00009 #ifndef GCOP_SYSTEM_H 00010 #define GCOP_SYSTEM_H 00011 00012 #include <Eigen/Dense> 00013 #include <vector> 00014 #include <assert.h> 00015 #include "manifold.h" 00016 #include <iostream> 00017 #include "rn.h" 00018 00019 //#include "sensor.h" 00020 00064 namespace gcop { 00065 00066 using namespace Eigen; 00067 00068 00080 template <typename T = VectorXd, 00081 int _nx = Dynamic, 00082 int _nu = Dynamic, 00083 int _np = Dynamic> class System { 00084 00085 public: 00086 00087 typedef Matrix<double, _nx, 1> Vectornd; 00088 typedef Matrix<double, _nu, 1> Vectorcd; 00089 typedef Matrix<double, _np, 1> Vectormd; 00090 00091 typedef Matrix<double, _nx, _nx> Matrixnd; 00092 typedef Matrix<double, _nx, _nu> Matrixncd; 00093 typedef Matrix<double, _nu, _nx> Matrixcnd; 00094 typedef Matrix<double, _nu, _nu> Matrixcd; 00095 00096 // typedef Matrix<double, Dynamic, 1> Vectormd; 00097 typedef Matrix<double, _np, _np> Matrixmd; 00098 typedef Matrix<double, _nx, _np> Matrixnmd; 00099 typedef Matrix<double, _np, _nx> Matrixmnd; 00100 00108 System(Manifold<T, _nx> &X, int nu = 0, int np = 0); 00109 00123 virtual double Step(T &xb, double t, const T &xa, 00124 const Vectorcd &u, double h, const Vectormd *p = 0, 00125 Matrixnd *A = 0, Matrixncd *B = 0, Matrixnmd *C = 0); 00126 00127 00147 virtual double Step(T &xb, double t, const T &xa, 00148 const Vectorcd &u, double h, 00149 const Vectornd &w, const Vectormd *p, 00150 Matrixnd *A = 0, Matrixncd *B = 0, Matrixnmd *C = 0, Matrixnd *D = 0); 00151 00164 virtual double Step(T &xb, const Vectorcd &u, 00165 double h, const Vectormd *p = 0, 00166 Matrixnd *A = 0, Matrixncd *B = 0, Matrixnmd *C = 0); 00167 00178 virtual double Step(const Vectorcd &u, 00179 double h, const Vectormd *p = 0, 00180 Matrixnd *A = 0, Matrixncd *B = 0, Matrixnmd *C = 0); 00181 00194 virtual double Step(T &xb, const Vectorcd &u, double h, 00195 const Vectornd &w, const Vectormd *p = 0, 00196 Matrixnd *A = 0, Matrixncd *B = 0, Matrixnmd *C = 0, Matrixnd *D = 0); 00197 00202 virtual bool Reset(const T &x, double t = 0); 00203 00211 virtual void Rec(T &x, double h) {}; 00212 00216 virtual void print(const T &x) const {}; 00217 00230 virtual bool Noise(Matrixnd &Q, double t, const T &x, const Vectorcd &u, 00231 double dt, const Vectormd *p = 0); 00232 00233 00245 virtual bool NoiseMatrix(Matrixnd &Q, double t, const T &x, const Vectorcd &u, 00246 double h, const Vectormd *p = 0); 00247 00248 00255 virtual void StateAndControlsToFlat(VectorXd &y, const T &x, const Vectorcd &u); 00256 00263 virtual void FlatToStateAndControls(T &x, Vectorcd &u, const std::vector<VectorXd> &y); 00264 00265 Manifold<T, _nx> &X; 00266 Rn<_nu> U; 00267 Rn<_np> P; 00268 int np; 00269 00270 bool internalState; 00271 00272 bool affineNoise; 00273 00274 T x; 00275 double t; 00276 00277 }; 00278 00279 00280 template <typename T, int _nx, int _nu, int _np> 00281 System<T, _nx, _nu, _np>::System(Manifold<T, _nx> &X, int nu, int np) : 00282 X(X), U(nu), P(np), np(np), internalState(false), affineNoise(true) { 00283 } 00284 00285 /* 00286 template <typename T, typename Tu, int _nx, int _nu> 00287 double System<T, Tu, _nx, _nu>::F(Vectornd &v, double t, const T& x, 00288 const Tu &u, double h, 00289 const VectorXd *p, 00290 Matrix<double, _nx, _nx> *A, Matrix<double, _nx, _nu> *B, 00291 Matrix<double, _nx, Dynamic> *C) { 00292 std::cout << "[W] System::F: unimplemented!" << std::endl; 00293 return 0; 00294 } 00295 */ 00296 00297 template <typename T, int _nx, int _nu, int _np> 00298 double System<T, _nx, _nu, _np>::Step(T& xb, double t, const T& xa, 00299 const Vectorcd &u, double h, const Vectormd *p, 00300 Matrixnd *A, Matrix<double, _nx, _nu> *B, 00301 Matrix<double, _nx, _np> *C) { 00302 return 0; 00303 } 00304 00305 template <typename T, int _nx, int _nu, int _np> 00306 double System<T, _nx, _nu, _np>::Step(T& xb, double t, const T& xa, 00307 const Vectorcd &u, double h, 00308 const Vectornd &w, const Vectormd *p, 00309 Matrixnd *A, Matrix<double, _nx, _nu> *B, 00310 Matrix<double, _nx, _np> *C, 00311 Matrix<double, _nx, _nx> *D) { 00312 if (affineNoise) { 00313 double result = this->Step(xb, t, xa, u, h, p, A, B, C); 00314 Matrixnd H; 00315 this->NoiseMatrix(H, this->t, this->x, u, h, p); 00316 this->X.Retract(xb, xb, (H*w)); 00317 if (D) 00318 *D = H; 00319 return result; 00320 } else { 00321 std::cout << "[W] System::Step: unimplemented for non-affine noise!" << std::endl; 00322 } 00323 } 00324 00325 template <typename T, int _nx, int _nu, int _np> 00326 double System<T, _nx, _nu, _np>::Step(T& xb, const Vectorcd &u, double h, const Vectormd *p, 00327 Matrixnd *A, Matrix<double, _nx, _nu> *B, 00328 Matrix<double, _nx, _np> *C) { 00329 double result = this->Step(xb, this->t, this->x, u, h, p, A, B, C); 00330 this->x = xb; 00331 this->t += h; 00332 return result; 00333 } 00334 00335 template <typename T, int _nx, int _nu, int _np> 00336 double System<T, _nx, _nu, _np>::Step(const Vectorcd &u, double h, const Vectormd *p, 00337 Matrixnd *A, Matrix<double, _nx, _nu> *B, 00338 Matrix<double, _nx, _np> *C) { 00339 T xb; 00340 double result = this->Step(xb, u, h, p, A, B, C); 00341 return result; 00342 } 00343 00344 template <typename T, int _nx, int _nu, int _np> 00345 double System<T, _nx, _nu, _np>::Step(T &xb, const Vectorcd &u, 00346 double h, const Vectornd &w, const Vectormd *p, 00347 Matrixnd *A, Matrixncd *B, Matrixnmd *C, Matrixnd *D) 00348 { 00349 if (affineNoise) { 00350 double result = this->Step(xb, u, h, p, A, B, C); 00351 Matrixnd H; 00352 this->NoiseMatrix(H, this->t, this->x, u, h, p); 00353 this->X.Retract(xb, xb, (H*w)); 00354 this->x = xb;//Update internal state 00355 this->t = (this->t + h); 00356 if (D) 00357 *D = H; 00358 return result; 00359 } else { 00360 std::cout << "[W] System::Step: unimplemented for non-affine noise!" << std::endl; 00361 } 00362 } 00363 00364 template <typename T, int _nx, int _nu, int _np> 00365 bool System<T, _nx, _nu, _np>::Reset(const T& x, double t) 00366 { 00367 this->x = x;//Copying state and time 00368 this->t = t; 00369 return true; 00370 } 00371 00372 00373 template <typename T, int _nx, int _nu, int _np> 00374 bool System<T, _nx, _nu, _np>::Noise(Matrixnd &Q, double t, const T &x, const Vectorcd &u, 00375 double dt, const Vectormd *p) { 00376 std::cout << "[W] System::Noise: unimplemented! Subclasses should override." << std::endl; 00377 return false; 00378 } 00379 00380 template <typename T, int _nx, int _nu, int _np> 00381 bool System<T, _nx, _nu, _np>::NoiseMatrix(Matrixnd &Q, 00382 double t, const T &x, const Vectorcd &u, 00383 double dt, const Vectormd *p) { 00384 Q.setIdentity(); 00385 return true; 00386 } 00387 00388 template <typename T, int _nx, int _nu, int _np> 00389 void System<T, _nx, _nu, _np>::StateAndControlsToFlat(VectorXd &y, const T &x, 00390 const Vectorcd &u) { 00391 y.resize(0); 00392 std::cout << "[W] System::StateAndControlsToFlat: unimplemented! Subclasses should override." << std::endl; 00393 } 00394 00395 template <typename T, int _nx, int _nu, int _np> 00396 void System<T, _nx, _nu, _np>::FlatToStateAndControls(T &x, Vectorcd &u, 00397 const std::vector<VectorXd> &y) { 00398 00399 std::cout << "[W] System::FlatToStateAndControls: unimplemented! Subclasses should override." << std::endl; 00400 } 00401 } 00402 00403 #endif 00404
1.7.6.1