Objective.h

#ifndef PLANNING_OBJECTIVES_H
#define PLANNING_OBJECTIVES_H

#include <KrisLibrary/Logger.h>
#include <KrisLibrary/planning/CSpace.h>
#include <KrisLibrary/planning/ControlSpace.h>
#include <KrisLibrary/math/matrix.h>
class MilestonePath;
class AnyCollection;

class ObjectiveFunctionalBase
{
 public:
  ObjectiveFunctionalBase() {}
  virtual ~ObjectiveFunctionalBase() {}

  virtual const char* TypeString() { return NULL; }

  virtual std::string Description() { if(TypeString()) return TypeString(); return ""; }

  virtual Real IncrementalCost(const Interpolator* path) { return 0.0; }
  virtual Real IncrementalCost(const ControlInput& u,const Interpolator* path) { return IncrementalCost(path); }
  virtual Real IncrementalCost(const KinodynamicMilestonePath& path);

  virtual Real TerminalCost(const Config& qend) { return 0.0; }

  virtual bool PathInvariant() const { return false; }

  virtual Real PathCost(const MilestonePath& path);
  virtual Real PathCost(const KinodynamicMilestonePath& path);

  virtual bool SaveParams(AnyCollection& collection) { return false; }
  virtual bool LoadParams(AnyCollection& collection) { return false; }
};

class IntegratorObjectiveFunctional : public ObjectiveFunctionalBase
{
 public:
  IntegratorObjectiveFunctional(Real dt = 0.1,int timeIndex=-1);
  virtual Real DifferentialCost(const State& x,const ControlInput& u) = 0;

  virtual Real Domain(const ControlInput& u,const Interpolator* path);

  virtual Real IncrementalCost(const ControlInput& u,const Interpolator* path);

  Real dt;
  int timeIndex;
};

class LengthObjective : public ObjectiveFunctionalBase
{
 public:
  LengthObjective() {}
  virtual ~LengthObjective() {}
  virtual const char* TypeString() { return "length"; }
  virtual Real IncrementalCost(const Interpolator* path) { return path->Length(); }
};


class TimeObjective : public ObjectiveFunctionalBase
{
 public:
  TimeObjective(int timeIndex=0);
  virtual ~TimeObjective() {}
  virtual const char* TypeString() { return "time"; }
  virtual Real IncrementalCost(const Interpolator* path);

  int timeIndex;
};

class ConfigObjective : public ObjectiveFunctionalBase
{
 public:
  ConfigObjective(const Config& qgoal);
  ConfigObjective(const Config& qgoal,const Vector& weights);
  virtual ~ConfigObjective() {}
  virtual const char* TypeString() { return "config"; }
  virtual Real TerminalCost(const Vector& qend);
  virtual bool PathInvariant() const { return true; }

  Vector qgoal,weights;
};

class QuadraticObjective : public IntegratorObjectiveFunctional
{
 public:
  QuadraticObjective(int timeIndex=0);
  virtual ~QuadraticObjective() {}
  virtual const char* TypeString() { return "quadratic"; }
  virtual Real TerminalCost(const Vector& qend);
  virtual Real DifferentialCost(const State& x,const ControlInput& u);
  
  InterpolatorPtr desiredPath;
  Math::Matrix stateCostMatrix,controlCostMatrix;
  Math::Matrix terminalCostMatrix;
};

class CompositeObjective : public ObjectiveFunctionalBase
{
 public:
  CompositeObjective();
  ~CompositeObjective();

  void Add(const std::shared_ptr<ObjectiveFunctionalBase>& obj,Real weight=1.0);

  virtual const char* TypeString() { return "composite"; }
  virtual std::string Description();

  virtual Real TerminalCost(const Vector& qend);
  virtual Real IncrementalCost(const Interpolator* path);
  virtual bool PathInvariant() const;

  Real norm;
  std::vector<std::shared_ptr<ObjectiveFunctionalBase> > components;
  std::vector<Real> weights;
};


#endif