Merge branch 'constraint-interface-update'

bin/body3dshell.cc

@@ -5,8 +5,8 @@
 #include "body3dview.h"
 #include "utils.h"
 #include "body3dcost.h"
-#include "demview.h"
-#include "shell.h"
+#include "sphereview.h"
+#include "sphereconstraint.h"
 #include "constraintcost.h"
 #include "multicost.h"
 
@@ -15,27 +15,20 @@ using namespace Eigen;
 using namespace gcop;
 
 typedef Ddp<Body3dState, 12, 6> Body3dDdp;
-typedef Shell<Body3dState, 12, 6> Body3dShell;
-typedef ConstraintCost<Body3dState, 12, 6, Dynamic, 1> ShellCost;
+typedef SphereConstraint<Body3dState, 12, 6> Body3dSphereConstraint;
+typedef ConstraintCost<Body3dState, 12, 6, Dynamic, 1> SphereConstraintCost;
 
-
-void solver_process(Viewer* viewer)
+int Body3dStateToVector3d(Vector3d &p, const Body3dState &x)
 {
-  //  if (viewer)
-  //    viewer->SetCamera(3.25, 48, -1.15, -1.35, -2.25);
+  p = x.p;
+  return 3;  // index where position coordinates start (state is (R,p,w,v) so this means 3)
+}
 
-  //  viewer->SetCamera(1, 23, -98.2, -77, -49);
-  //  viewer->SetCamera(.87, 23, -57.2, -64, 3.7);
-  //  Dem dem("maps/pic2.ppm", .25, 30);
 
+void solver_process(Viewer* viewer)
+{
   if (viewer)
-    viewer->SetCamera(4.5, 45, -10, -8, -10);
-  Dem dem("maps/simple.ppm", .5, 10);
-  dem.Convolve(1);
-
-  DemView dv(dem);
-  //  if (viewer)
-  //    viewer->Add(dv);
+    viewer->SetCamera(4.5, 45, -7.5, -6.9, -21);
 
   int N = 64;      // discrete trajectory segments
   double tf = 15;   // time-horizon
@@ -47,7 +40,7 @@ void solver_process(Viewer* viewer)
   // cost 
   Body3dState xf;
   xf.Clear();
-  xf.p << 0, 0, 5;
+  xf.p << 0, 0, 0;
 
   Body3dCost<6> cost(sys, tf, xf);
 
@@ -60,9 +53,8 @@ void solver_process(Viewer* viewer)
 
   cost.Qf = s*cost.Qf;
 
-  cost.R(0,0) = .05; cost.R(1,1) = .05; cost.R(2,2) = .05; cost.R(3,3) = .1;
-
-  cost.R = cost.R;
+  cost.R(0,0) = .05; cost.R(1,1) = .05; cost.R(2,2) = .05; 
+  cost.R(3,3) = .1; cost.R(4,4) = .1; cost.R(5,5) = .1;
 
   // times
   vector<double> ts(N+1);
@@ -72,32 +64,37 @@ void solver_process(Viewer* viewer)
   // states
   vector<Body3dState> xs(N+1);
   xs[0].Clear();
-  xs[0].p << 20, 18, 5;
+  xs[0].p << 20, 19, 5;
 
   // initial controls (e.g. hover at one place)
   vector<Vector6d> us(N);
   for (int i = 0; i < N; ++i) {
     us[i].setZero();
-    //    us[i][3] = 9.81*sys.m;
   }
 
   double temp[50000];
 
-  Body3dShell pqp(Vector3d(10,10,5), 3, 0);
+  Sphere sphere(Vector3d(10,10,2.5), 3);
+  Body3dSphereConstraint constraint(sphere, 0);
+  constraint.func = Body3dStateToVector3d;
+
+  SphereView sview(sphere);
+  if (viewer)
+    viewer->Add(sview);
 
   double temp2[50000];
 
-  ShellCost pqpcost(sys, tf, pqp);
-
+  SphereConstraintCost scost(sys, tf, constraint);
+  scost.b = 50;
+
   MultiCost<Body3dState, 12, 6> mcost(sys, tf);
   mcost.costs.push_back(&cost);   
-  mcost.costs.push_back(&pqpcost);
+  mcost.costs.push_back(&scost);
 
   Body3dDdp ddp(sys, mcost, ts, xs, us);
-  ddp.eps = 1e-3;
+  //ddp.eps = 1e-3;
   ddp.mu = 1;
 
-
   Body3dView<6> view(sys, &ddp.xs);
   if (viewer)
     viewer->Add(view);  
@@ -113,7 +110,7 @@ void solver_process(Viewer* viewer)
     long te = timer_us(timer);
     cout << "Iteration #" << i << ": took " << te << " us." << endl;        
     getchar();
-    pqpcost.b = 5;
+    scost.b = 50;
   }
 
   cout << "done!" << endl;

bin/dynvisest.cfg

@@ -69,7 +69,7 @@ maxCams = 9
 sim_x0 = 0,0.0,0, 0,-1,0.5, 0,0,0.0, 0,0.5,0
 
 # if simulated use this as the final time
-sim_tf = .3
+sim_tf = 2
 
 # if simulated use the cam frame time step
 #sim_dt = .025

bin/gcartrack.cc

@@ -178,9 +178,6 @@ void solver_process(Viewer* viewer)
     gview.Unlock();
 
   }
-  //  for (int k = 0; k <= N; ++k)
-  //    cout << ddp.xs[k] << "|" << endl;  
-  //  cout << "xf=" << ddp.xs.back() << endl;
 
 
   cout << "done!" << endl;

lib/algos/ddp.h

@@ -51,16 +51,34 @@ namespace gcop {
      * @param ts (N+1) sequence of discrete times
      * @param xs (N+1) sequence of discrete states
      * @param us (N) sequence of control inputs
+     * @param p parameters (optional)
      * @param update whether to update trajectory xs using initial state xs[0] and inputs us.
      *               This is necessary only if xs was not already generated from us.
      */
     Ddp(System<T, nx, nu, np> &sys, Cost<T, nx, nu, np> &cost, 
         vector<double> &ts, vector<T> &xs, vector<Vectorcd> &us, 
         Vectorpd *p = 0, bool update = true);
-
-    virtual ~Ddp();
 
 
+    /**
+     * Create an optimal control problem using a system, a cost, and 
+     * a trajectory given by a sequence of times, states, and controls. 
+     * The times ts must be given, the initial state xs[0] must be set, and
+     * the controls us will be used as an initial guess for the optimization.
+     *
+     * After initialization, every call to Iterate() will optimize the 
+     * controls us and states xs and modify them accordingly. Problems involving
+     * time-optimization will also modify the sequence of times ts.
+     * 
+     * @param traj trajectory
+     * @param cost cost
+     * @param update whether to update trajectory xs using initial state xs[0] and inputs us.
+     *               This is necessary only if xs was not already generated from us.
+     */
+    Ddp(Trajectory<T, nx, nu, np> &traj, Cost<T, nx, nu, np> &cost, bool update = true);
+
+    virtual ~Ddp();
+
     /**
      * Perform one DDP iteration. Internally calls:
      *
@@ -120,50 +138,6 @@ namespace gcop {
     static const char DDP = 1;      ///< DDP version of algorithm
     static const char LQS = 2;      ///< Linear-Quadratic Subproblem version of algorithm due to Dreyfus / Dunn&Bertsekas / Pantoja
 
-    /*
-    class Fx : public Function<T, n, n> {
-    public:
-    Fx(System &sys) :
-      Function<T>(sys.U.nspace), sys(sys) {
-      }
-      
-      void F(Vectornd &f, const T &x) {
-
-        
-        sys.X.L
-        this->xa = xa;
-        sys.FK(this->xa);
-        sys.ID(f, t, *xb, this->xa, *u, h);
-      }
-      
-      Mbs &sys;
-      
-      double t;
-      const MbsState *xb;
-      const Vectorcd *u;
-      double h;
-    };
-
-    class Fu : public Function<VectorXd> {
-    public:
-    Fu(Mbs &sys) :
-      Function<VectorXd>(sys.U), sys(sys) {
-      }
-      
-      void F(VectorXd &f, const VectorXd &u) {
-        sys.ID(f, t, *xb, *xa, u, h);
-      }
-      
-      Mbs &sys;
-      
-      double t;
-      const MbsState *xa;
-      const MbsState *xb;
-      double h;
-    };
-
-    */
-
     bool pd(const Matrixnd &P) {
       LLT<Matrixnd> llt;     
       llt.compute(P);
@@ -182,6 +156,12 @@ namespace gcop {
   using namespace std;
   using namespace Eigen;
 
+  template <typename T, int nx, int nu, int np> 
+    Ddp<T, nx, nu, np>::Ddp(Trajectory<T, nx, nu, np> &traj, 
+                            Cost<T, nx, nu, np> &cost, 
+                            bool update) : Ddp(traj.sys, cost, traj.ts, traj.xs, traj.us, traj.p, update) {
+  }
+
   template <typename T, int nx, int nu, int np> 
     Ddp<T, nx, nu, np>::Ddp(System<T, nx, nu, np> &sys, 
                             Cost<T, nx, nu, np> &cost, 

lib/algos/docp.h

@@ -14,7 +14,7 @@
 #include <type_traits>
 #include <algorithm>
 #include <iterator>
-#include "system.h"
+#include "trajectory.h"
 #include "cost.h"
 #include <cmath>
 #include "rn.h"
@@ -66,6 +66,24 @@ namespace gcop {
     Docp(System<T, nx, nu, np> &sys, Cost<T, nx, nu, np> &cost, 
          vector<double> &ts, vector<T> &xs, vector<Vectorcd> &us, 
          Vectormd *p = 0, bool update = true);
+
+    /**
+     * Create a discrete optimal control problem using a system, a cost, and 
+     * a trajectory given by a sequence of times, states, and controls. 
+     * The times ts must be given, the initial state xs[0] must be set, and
+     * the controls us will be used as an initial guess for the optimization.
+     *
+     * After initialization, every call to Iterate() will optimize the 
+     * controls us and states xs and modify them accordingly. Problems involving
+     * time-optimization will also modify the sequence of times ts.
+     * 
+     * @param trajectory system trajectory
+     * @param cost cost
+     * @param update whether to update trajectory xs using initial state xs[0] and inputs us.
+     *               This is necessary only if xs was not already generated from us.
+     */
+    Docp(Trajectory<T, nx, nu, np> &traj, Cost<T, nx, nu, np> &cost, 
+         bool update = true);
 
     virtual ~Docp();
 
@@ -138,6 +156,12 @@ namespace gcop {
         Update();
       }
     }
+
+  template <typename T, int nx, int nu, int np> 
+    Docp<T, nx, nu, np>::Docp(Trajectory<T, nx, nu, np> &traj, 
+                              Cost<T, nx, nu, np> &cost, 
+                              bool update) : Docp(traj.sys, cost, traj.ts, traj.xs, traj.us, traj.p, update) {
+  }
 
   template <typename T, int nx, int nu, int np> 
     Docp<T, nx, nu, np>::~Docp()

lib/systems/CMakeLists.txt

@@ -34,6 +34,7 @@ set(sources
 
 set(headers
     system.h
+    trajectory.h
     system_extstep.h
     particle2d.h
     unicycle.h

lib/systems/constraints/CMakeLists.txt

@@ -1,12 +1,15 @@
 set(sources
   disk.cc
+  sphere.cc
 )
 
 set(headers 
   shell.h
   cylinder.h
   disk.h
+  sphere.h
   diskconstraint.h
+  sphereconstraint.h
   constraint.h
 )
 

lib/systems/constraints/sphere.cc

@@ -0,0 +1,8 @@
+#include "sphere.h"
+
+using namespace gcop;
+using namespace Eigen;
+
+Sphere::Sphere(const Vector3d &o, double r) : o(o), r(r)
+{
+}

lib/systems/constraints/sphere.h

@@ -0,0 +1,16 @@
+#ifndef GCOP_SPHERE_H
+#define GCOP_SPHERE_H
+
+#include <Eigen/Dense>
+
+namespace gcop {
+
+  class Sphere {
+  public:
+    Sphere(const Eigen::Vector3d &o, double r);
+
+    Eigen::Vector3d o;      ///< origin
+    double r;        ///< radius  
+  }; 
+}
+#endif