PlannerObjective.h

#ifndef PLANNER_OBJECTIVES_H
#define PLANNER_OBJECTIVES_H

#include <Klampt/Modeling/Robot.h>
#include <Klampt/Modeling/ParabolicRamp.h>
#include <Klampt/Modeling/DynamicPath.h>
#include <KrisLibrary/robotics/IK.h>

class AnyCollection;

namespace Klampt {

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

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

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

  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend) { return 0.0; }

  virtual Real DifferentialCost(Real t,const Vector& q,const Vector& dq) { return 0.0; }

  virtual bool DifferentialTimeInvariant() const { return false; }
  virtual bool TerminalTimeInvariant() const { return false; }
  virtual bool PathInvariant() const { return false; }

  virtual Real IncrementalCost(Real t,const ParabolicRamp::ParabolicRampND& ramp) { return 0.0; }
  virtual Real IncrementalCost(Real t,const ParabolicRamp::DynamicPath& path);

  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);

  virtual Real Delta(PlannerObjectiveBase* priorGoal) { return Inf; }

  virtual bool Read(File& file) { return false; }
  virtual bool Write(File& file) { return false; }
};

class TimeObjective : public PlannerObjectiveBase
{
 public:
  TimeObjective() {}
  virtual ~TimeObjective() {}
  virtual const char* TypeString() { return "time"; }
  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
  virtual Real DifferentialCost(Real t,const Vector& q,const Vector& dq) { return 1.0; }
  virtual Real IncrementalCost(Real t,const ParabolicRamp::ParabolicRampND& ramp);
  virtual Real Delta(PlannerObjectiveBase* priorGoal);
  virtual bool DifferentialTimeInvariant() const { return true; }
  virtual bool TerminalTimeInvariant() const { return true; }
};

class TerminalTimeObjective : public PlannerObjectiveBase
{
 public:
  TerminalTimeObjective(Real tgoal);
  virtual ~TerminalTimeObjective() {}
  virtual const char* TypeString() { return "term_time"; }
  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
  virtual Real Delta(PlannerObjectiveBase* priorGoal);
  virtual bool DifferentialTimeInvariant() const { return true; }

  Real tgoal;
};

class ConfigObjective : public PlannerObjectiveBase
{
 public:
  ConfigObjective(const Config& qgoal);
  virtual ~ConfigObjective() {}
  virtual const char* TypeString() { return "config"; }
  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend) {
    return qend.distance(qgoal);
  }
  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0) {
    return qgoal.distance(Vector(path.ramps.back().x1));
  }
  virtual Real Delta(PlannerObjectiveBase* priorGoal);
  virtual bool TerminalTimeInvariant() const { return true; }
  virtual bool DifferentialTimeInvariant() const { return true; }
  virtual bool PathInvariant() const { return true; }

  Vector qgoal;
};

class VelocityObjective : public PlannerObjectiveBase
{
 public:
  VelocityObjective(const Vector& vgoal);
  virtual ~VelocityObjective() {}
  virtual const char* TypeString() { return "velocity"; }
  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend) {
    return dqend.distance(vgoal);
  }
  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0) {
    return vgoal.distance(Vector(path.ramps.back().dx1));
  }
  virtual Real Delta(PlannerObjectiveBase* priorGoal);
  virtual bool TerminalTimeInvariant() const { return true; }
  virtual bool DifferentialTimeInvariant() const { return true; }
  virtual bool PathInvariant() const { return true; }

  Vector vgoal;
};

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

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

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

  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
  virtual Real DifferentialCost(Real t,const Vector& q,const Vector& dq);
  virtual Real IncrementalCost(Real t,const ParabolicRamp::ParabolicRampND& ramp);
  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
  virtual Real Delta(PlannerObjectiveBase* priorGoal);
  virtual bool TerminalTimeInvariant() const;
  virtual bool DifferentialTimeInvariant() const;
  virtual bool PathInvariant() const;

  Real norm;
  vector<shared_ptr<PlannerObjectiveBase> > components;
  vector<Real> weights;
};


class CartesianObjective : public PlannerObjectiveBase
{
 public:
  CartesianObjective(RobotModel* robot);
  virtual ~CartesianObjective() {}

  virtual const char* TypeString() { return "cartesian"; }

  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0) {
    return TerminalCost(0,Vector(path.ramps.back().x1),Vector(path.ramps.back().dx1));
  }
  virtual Real Delta(PlannerObjectiveBase* priorGoal);
  virtual bool TerminalTimeInvariant() const { return true; }
  virtual bool DifferentialTimeInvariant() const { return true; }
  virtual bool PathInvariant() const { return true; }

  RobotModel* robot;
  IKGoal ikGoal;
};

class IKObjective : public PlannerObjectiveBase
{
 public:
  IKObjective(RobotModel* robot);
  virtual ~IKObjective() {}

  virtual const char* TypeString() { return "ik"; }

  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);

  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0) {
    return TerminalCost(0,Vector(path.ramps.back().x1),Vector(path.ramps.back().dx1));
  }

  virtual Real Delta(PlannerObjectiveBase* priorGoal);

  virtual bool TerminalTimeInvariant() const { return true; }
  virtual bool DifferentialTimeInvariant() const { return true; }
  virtual bool PathInvariant() const { return true; }

  RobotModel* robot;
  IKGoal ikGoal;
  Real posCoeff,oriCoeff;
};

class CartesianTrackingObjective : public PlannerObjectiveBase
{
 public:
  CartesianTrackingObjective(RobotModel* robot);
  virtual ~CartesianTrackingObjective() {}
  virtual const char* TypeString() { return "cartesian_tracking"; }

  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
  virtual Real DifferentialCost(Real t,const Vector& q,const Vector& dq);
  virtual Real IncrementalCost(Real t,const ParabolicRamp::ParabolicRampND& ramp);
  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
  virtual Real Delta(PlannerObjectiveBase* priorGoal);

  int FindSegment(Real t) const;
  //integrates the time range [a,b] over the ramp starting at tstart
  Real IntegrateSegment(int index,Real a,Real b,Real tstart,const ParabolicRamp::ParabolicRampND& ramp);
  //computes desired 3-space position at time t
  Vector3 GetDesiredPosition(Real t) const;
  //differential cost at time t in the form (x-b)^T A (x-b)
  void GetDifferentialCostFunction(Real t,Matrix3& A,Vector3& b) const;
  Real DifferentialCost(Real t,const Vector& q,int index);

  RobotModel* robot;
  //track localPosition on this link 
  int link;
  Vector3 localPosition;
  //3-space trajectory to follow
  vector<Vector3> positions;
  vector<Real> times;
  //optional: scalar weight for each time
  vector<Real> weights;
  //optional: matrix weight e^T A e for each time
  vector<Matrix3> matWeights;
  //optional: terminal cost
  Real endTimeWeight,endPosWeight;
  Matrix3 endPosMatWeight;
};

PlannerObjectiveBase* LoadPlannerObjective(istream& in,RobotModel* robot=NULL);

bool SavePlannerObjective(PlannerObjectiveBase* obj,ostream& out);

PlannerObjectiveBase* LoadPlannerObjective(AnyCollection& msg,RobotModel* robot=NULL);
bool SavePlannerObjective(PlannerObjectiveBase* obj,AnyCollection& msg);

} //namespace Klampt

#endif