Merge branch 'master' of git.lcsr.jhu.edu:mkobila1/gcop

jhu-asco · Jul 6, 2015 · 493b2f4 · 493b2f4
2 parents 60f785d + ac63468
commit 493b2f4
Show file tree

Hide file tree

Showing 18 changed files with 258 additions and 68 deletions.
diff --git a/bin/BulletVehicleEstimation/VehicleDemo.h b/bin/BulletVehicleEstimation/VehicleDemo.h
@@ -26,7 +26,7 @@ class btCollisionShape;
 #include <stdlib.h>
 
 #include "point3dgps.h"
-#include "gndoepv2.h"
+#include "deterministicgndoep.h"
 #include "params.h"
 #include "lqsensorcost.h"
 #include "utils.h"
@@ -41,7 +41,7 @@ using namespace Eigen;
 class VehicleDemo : public GlutDemoApplication
 {
   typedef LqSensorCost<Vector4d, 4, 2, Dynamic, Dynamic, Vector3d, 3> RccarCost;
-  typedef GnDoep1<Vector4d, 4, 2, Dynamic, Dynamic, Vector3d, 3, Point3dState, 6> RccarDoep;
+  typedef GnDoep<Vector4d, 4, 2, Dynamic, Dynamic, Vector3d, 3, Point3dState, 6> RccarDoep;
 
   protected:
     int iters; //Number of iterations

diff --git a/bin/BulletZVehicleTest/VehicleDemo.h b/bin/BulletZVehicleTest/VehicleDemo.h
@@ -26,7 +26,7 @@ class btCollisionShape;
 #include <stdlib.h>
 
 #include "point3dgps.h"
-#include "gndoepv2.h"
+#include "deterministicgndoep.h"
 #include "params.h"
 #include "lqsensorcost.h"
 #include "utils.h"
@@ -41,7 +41,7 @@ using namespace Eigen;
 class VehicleDemo : public GlutDemoApplication
 {
   typedef LqSensorCost<Vector4d, 4, 2, Dynamic, 10, Vector3d, 3> RccarCost;
-  typedef GnDoep1<Vector4d, 4, 2, Dynamic, 10, Vector3d, 3, Point3dState, 6> RccarDoep;
+  typedef GnDoep<Vector4d, 4, 2, Dynamic, 10, Vector3d, 3, Point3dState, 6> RccarDoep;
 
   protected:
     int iters; //Number of iterations

diff --git a/bin/body3dforceest.cc b/bin/body3dforceest.cc
@@ -7,10 +7,17 @@
 #include "utils.h"
 #include "so3.h"
 #include "params.h"
-#include "gndoep.h"
 #include "sensor.h"
 #include "insmanifold.h"
 
+//#define USE_STOCHASTIC_DYNAMICS
+
+#ifdef USE_STOCHASTIC_DYNAMICS
+#include "gndoep.h"
+#else
+#include "deterministicgndoep.h"
+#endif
+
 using namespace std;
 using namespace Eigen;
 using namespace gcop;

diff --git a/lib/algos/CMakeLists.txt b/lib/algos/CMakeLists.txt
@@ -5,7 +5,7 @@ set(headers
       doep.h
       gndocp.h
       gndoep.h
-      gndoepv2.h
+      deterministicgndoep.h
       pddp.h
       ba.h
       body2dslam.h
@@ -41,4 +41,4 @@ set(headers
   ${headers}
   body3ddemcontroller.h
 )
-ENDIF (DSL_FOUND AND PQP_FOUND)
+ENDIF (DSL_FOUND AND PQP_FOUND)
diff --git a/lib/algos/gndoepv2.h → lib/algos/deterministicgndoep.h b/lib/algos/gndoepv2.h → lib/algos/deterministicgndoep.h
@@ -6,6 +6,9 @@
 
 #include <unsupported/Eigen/NonLinearOptimization>
 
+/** GnDoep with deterministic dynamics
+*/
+
 namespace gcop {
 
 
@@ -32,7 +35,7 @@ namespace gcop {
     int _nz = Dynamic,
     typename T1 = T,
     int _nx1 = _nx> 
-    class GnDoep1 : public Doep<T, _nx, _nu, _np, Tz, _nz, T1, _nx1> {
+    class GnDoep : public Doep<T, _nx, _nu, _np, Tz, _nz, T1, _nx1> {
 
     typedef Matrix<double, _ng, 1> Vectorgd;
     typedef Matrix<double, _ng, _nx> Matrixgxd;
@@ -90,12 +93,12 @@ namespace gcop {
      */
 
 
-    GnDoep1(System<T, _nx, _nu, _np> &sys, Sensor<T1, _nx1, _nu, _np, Tz, _nz> &sensor,
+    GnDoep(System<T, _nx, _nu, _np> &sys, Sensor<T1, _nx1, _nu, _np, Tz, _nz> &sensor,
            LqSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz> &cost, 
            vector<double> &ts, vector<T> &xs, vector<Vectorcd> &us, Vectormd &p,
            vector<double> &ts1, Func_type _project=NULL, bool update = true);
 
-    virtual ~GnDoep1();
+    virtual ~GnDoep();
 
     /**
      * Perform one DOCP iteration. Internally calls:
@@ -155,7 +158,7 @@ struct Functor
    struct GnCost : Functor<double> {  
    GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>(int inputs, int values) : Functor<double>(inputs, values) {};
 
-   GnDoep1<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1> *doep;
+   GnDoep<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1> *doep;
 
    typedef Matrix<double, _nx, 1> Vectornd;
    typedef Matrix<double, _np, 1> Vectormd;
@@ -212,9 +215,11 @@ struct Functor
      {
        std::cout<<"Input: "<<s.transpose()<<endl;
        std::cout<<"Fvec: "<<fvec.transpose()<<endl;
+       std::cout<<"Cost: "<<0.5*(fvec.transpose()*fvec)<<endl;
        //std::cout<<"Resp: "<<fvec.tail(np).transpose()<<"\t"<<doep->p<<endl;
      }
-       std::cout<<"Cost: "<<0.5*(fvec.transpose()*fvec)<<endl;
+     doep->J = 0.5*(fvec.squaredNorm());//#TODO Directly from  LevinBergMarquadt if possible
+     //getchar();
      return 0;
    }
   };
@@ -223,7 +228,7 @@ struct Functor
   using namespace Eigen;
 
   template <typename T, int _nx, int _nu, int _np, int _ng, typename Tz, int _nz, typename T1, int _nx1> 
-    GnDoep1<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::GnDoep1(System<T, _nx, _nu, _np> &sys, Sensor<T1, _nx1, _nu, _np, Tz, _nz> &sensor,
+    GnDoep<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::GnDoep(System<T, _nx, _nu, _np> &sys, Sensor<T1, _nx1, _nu, _np, Tz, _nz> &sensor,
                                                 LqSensorCost<T, _nx, _nu, _np, _ng, Tz, _nz> &cost,
                                                 vector<double> &ts, 
                                                 vector<T> &xs, 
@@ -245,15 +250,15 @@ struct Functor
     }
 
   template <typename T, int _nx, int _nu, int _np, int _ng, typename Tz, int _nz, typename T1, int _nx1> 
-    GnDoep1<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::~GnDoep1()
+    GnDoep<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::~GnDoep()
     {
       delete lm;
       delete numDiff;
       delete functor;
     }  
 
   template <typename T, int _nx, int _nu, int _np, int _ng, typename Tz, int _nz, typename T1, int _nx1> 
-    void GnDoep1<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::Iterate() {
+    void GnDoep<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>::Iterate() {
 
     if (!lm) {
       functor = new GnCost<T, _nx, _nu, _np, _ng, Tz, _nz, T1, _nx1>(inputs, values);
@@ -267,6 +272,10 @@ struct Functor
 
       const int &np = this->sys.P.n;
       s = this->p;//Set the system parameters to initial guess
+
+      lm->parameters.maxfev = 1e6;//Maximum nof evaluations is very high
+      info = lm->minimizeInit(s);
+      cout <<"info="<<info <<endl;
     }
 
     /*
@@ -277,7 +286,7 @@ struct Functor
     */
 
     //    lm.parameters.maxfev=10000;
-    info = lm->minimize(s);
+    info = lm->minimizeOneStep(s);
 
     cout <<"info="<<info <<endl;
     // check return values

diff --git a/lib/algos/gndocp.h b/lib/algos/gndocp.h
@@ -100,6 +100,8 @@ using ceres::Solve;
      * are updated. 
      */
     void Iterate();
+
+    void Reset();
     Tparam<T, _nx, _nu, _np, _ntp> &tparam;
 
     int info;
@@ -300,11 +302,11 @@ struct Functor
     inputs(tparam.ntp),
     values(cost.ng*xs.size()), s(inputs), 
     functor(0), numDiff(0), lm(0), 
-#ifndef USE_SAMPLE_NUMERICAL_DIFF
+//#ifndef USE_SAMPLE_NUMERICAL_DIFF
     numdiff_stepsize(1e-8)
-#else
-    numdiff_stepsize(1e-4)
-#endif
+//#else
+    //numdiff_stepsize(1e-4)
+//#endif
     {
       if(update)
         this->Update(false);//No need of derivatives
@@ -323,6 +325,13 @@ struct Functor
       delete numDiff;
       delete functor;
     }  
+
+  template <typename T, int _nx, int _nu, int _np, int _ng, int _ntp> 
+    void GnDocp<T, _nx, _nu, _np, _ng, _ntp>::Reset() {
+      delete lm;
+      lm = NULL;
+      return;
+    }
 
   template <typename T, int _nx, int _nu, int _np, int _ng, int _ntp> 
     void GnDocp<T, _nx, _nu, _np, _ng, _ntp>::Iterate() {
@@ -339,6 +348,7 @@ struct Functor
 #endif
       */
       lm->parameters.maxfev = 1e6;//Maximum nof evaluations is very high
+      cout<<"Initializing..."<<endl;
       info = lm->minimizeInit(s);
       cout <<"info="<<info <<endl;
     }

diff --git a/lib/algos/gndoep.h b/lib/algos/gndoep.h
@@ -245,15 +245,19 @@ struct Functor
                                                 vector<T> &xs, 
                                                 vector<Vectorcd > &us,
                                                 Vectormd &p,
-                                                vector<double> &ts1, 
+                                                vector<double> &ts1,
                                                 Func_type _project,
                                                 bool update) : 
-    Doep<T, _nx, _nu, _np, Tz, _nz, T1, _nx1>(sys, sensor, cost, ts, xs, us, p, ts1, _project, update),
+    Doep<T, _nx, _nu, _np, Tz, _nz, T1, _nx1>(sys, sensor, cost, ts, xs, us, p, ts1, _project, false),
     inputs(us.size()*sys.X.n + sys.P.n),
     values((sys.X.n)*us.size()+(sensor.Z.n)*ts1.size()+sys.P.n), s(inputs), 
     functor(0), numDiff(0), lm(0)
     {
       cout <<"inputs=" <<inputs<<" values= "<<values<< endl;
+      if(update)
+      {
+        this->Update(false);
+      }
     }
 
   template <typename T, int _nx, int _nu, int _np, int _ng, typename Tz, int _nz, typename T1, int _nx1> 

diff --git a/lib/systems/costs/lqsensorcost.h b/lib/systems/costs/lqsensorcost.h
@@ -126,6 +126,7 @@ namespace gcop {
       Psqrt.resize(sys.P.n, sys.P.n);
 
       dp.resize(sys.P.n);
+      dz.resize(Z.n);
     }
 
     R.setIdentity();

diff --git a/lib/systems/manifolds/mbsmanifold.cc b/lib/systems/manifolds/mbsmanifold.cc
@@ -22,6 +22,7 @@ void MbsManifold::Lift(VectorXd &v,
   assert(!std::isnan(xa.gs[0](0,0)));
 
   int nb = xa.r.size() + 1;
+  assert(xb.r.size() == xa.r.size());
 
   if (xa.fixed) {
     v.head(nb - 1) = xb.r - xa.r;

diff --git a/lib/systems/manifolds/mbsstate.h b/lib/systems/manifolds/mbsstate.h
@@ -24,6 +24,25 @@ namespace gcop {
       r(x.r), dr(x.dr), ub(x.ub), lb(x.lb), 
       zl(x.zl), zu(x.zu), fixed(x.fixed) {
     }
+
+   /*MbsState& operator = (const MbsState &x)
+   {
+     if(this != &x)
+     {
+       this->gs = x.gs;
+       this->vs = x.vs;
+       this->dgs = x.dgs;
+       this->r = x.r;
+       this->dr = x.dr;
+       this->ub = x.ub;
+       this->lb = x.lb;
+       this->zl = x.zl;
+       this->zu = x.zu;
+       this->fixed = x.fixed;
+     }
+     return *this;
+   }
+   */
 
     vector<Matrix4d> gs;        ///< configurations
     vector<Vector6d> vs;        ///< body-fixed velocities