problem_description.h

Go to the documentation of this file.

#pragma once
#include <trajopt/common.hpp>
#include <trajopt/json_marshal.hpp>
#include <trajopt_sco/optimizers.hpp>
 
#include <moveit/robot_model/robot_model.h>
#include <moveit/planning_scene/planning_scene.h>
 
#include <memory>
 
namespace trajopt_interface
{
struct TermInfo;
MOVEIT_CLASS_FORWARD(TermInfo);  // Defines TermInfoPtr, ConstPtr, WeakPtr... etc
 
class TrajOptProblem;
MOVEIT_CLASS_FORWARD(TrajOptProblem);  // Defines TrajOptProblemPtr, ConstPtr, WeakPtr... etc
 
struct JointPoseTermInfo;
MOVEIT_CLASS_FORWARD(JointPoseTermInfo);  // Defines JointPoseTermInfoPtr, ConstPtr, WeakPtr... etc
 
struct CartPoseTermInfo;
MOVEIT_CLASS_FORWARD(CartPoseTermInfo);  // Defines CartPoseTermInfoPtr, ConstPtr, WeakPtr... etc
 
struct JointVelTermInfo;
MOVEIT_CLASS_FORWARD(JointVelTermInfo);  // Defines JointVelTermInfoPtr, ConstPtr, WeakPtr... etc
 
struct ProblemInfo;
TrajOptProblemPtr ConstructProblem(const ProblemInfo&);
 
enum TermType
{
  TT_COST = 0x1,      // 0000 0001
  TT_CNT = 0x2,       // 0000 0010
  TT_USE_TIME = 0x4,  // 0000 0100
};
 
struct BasicInfo
{
  bool start_fixed;
  int n_steps;
  sco::IntVec dofs_fixed;        // optional
  sco::ModelType convex_solver;  // which convex solver to use
 
  bool use_time = false;
  double dt_upper_lim = 1.0;
  double dt_lower_lim = 1.0;
};
 
struct InitInfo
{
  enum Type
  {
    STATIONARY,
    JOINT_INTERPOLATED,
    GIVEN_TRAJ,
  };
  Type type;
  trajopt::TrajArray data;
  //  Eigen::VectorXd data_vec;
  //  trajopt::TrajArray data_trajectory;
  double dt = 1.0;
};
 
struct TermInfo
{
  std::string name;
  int term_type;
  int getSupportedTypes()
  {
    return supported_term_types_;
  }
  //  virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0;
  virtual void addObjectiveTerms(TrajOptProblem& prob) = 0;
 
  static TermInfoPtr fromName(const std::string& type);
 
  using MakerFunc = TermInfoPtr (*)(void);
  static void RegisterMaker(const std::string& type, MakerFunc);
 
  TermInfo() = default;
  TermInfo(const TermInfo&) = default;
  TermInfo(TermInfo&&) = default;
  TermInfo& operator=(const TermInfo&) = default;
  TermInfo& operator=(TermInfo&&) = default;
  virtual ~TermInfo() = default;
 
protected:
  TermInfo(int supported_term_types) : supported_term_types_(supported_term_types)
  {
  }
 
private:
  static std::map<std::string, MakerFunc> name_to_maker_;
  int supported_term_types_;
};
 
struct ProblemInfo
{
public:
  BasicInfo basic_info;
  sco::BasicTrustRegionSQPParameters opt_info;
  std::vector<TermInfoPtr> cost_infos;
  std::vector<TermInfoPtr> cnt_infos;
  InitInfo init_info;
 
  planning_scene::PlanningSceneConstPtr planning_scene;
  std::string planning_group_name;
 
  ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg)
    : planning_scene(ps), planning_group_name(pg)
  {
  }
};
 
class TrajOptProblem : public sco::OptProb
{
public:
  TrajOptProblem();
  TrajOptProblem(const ProblemInfo& problem_info);
  virtual ~TrajOptProblem() = default;
  sco::VarVector GetVarRow(int i, int start_col, int num_col)
  {
    return matrix_traj_vars.rblock(i, start_col, num_col);
  }
  sco::VarVector GetVarRow(int i)
  {
    return matrix_traj_vars.row(i);
  }
  sco::Var& GetVar(int i, int j)
  {
    return matrix_traj_vars.at(i, j);
  }
  trajopt::VarArray& GetVars()
  {
    return matrix_traj_vars;
  }
  int GetNumSteps()
  {
    return matrix_traj_vars.rows();
  }
  int GetNumDOF()
  {
    return matrix_traj_vars.cols();
  }
  int GetActiveGroupNumDOF()
  {
    return dof_;
  }
  planning_scene::PlanningSceneConstPtr GetPlanningScene()
  {
    return planning_scene_;
  }
  void SetInitTraj(const trajopt::TrajArray& x)
  {
    matrix_init_traj = x;
  }
  trajopt::TrajArray GetInitTraj()
  {
    return matrix_init_traj;
  }
  //  friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&);
  bool GetHasTime()
  {
    return has_time;
  }
  void SetHasTime(bool tmp)
  {
    has_time = tmp;
  }
 
private:
  bool has_time;
  trajopt::VarArray matrix_traj_vars;
  planning_scene::PlanningSceneConstPtr planning_scene_;
  std::string planning_group_;
  int dof_;
  trajopt::TrajArray matrix_init_traj;
};
 
struct CartPoseTermInfo : public TermInfo
{
  //  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  int timestep;
  Eigen::Vector3d xyz;
  Eigen::Vector4d wxyz;
  Eigen::Vector3d pos_coeffs, rot_coeffs;
  std::string link;
  Eigen::Isometry3d tcp;
 
  CartPoseTermInfo();
 
  //  void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override;
  void addObjectiveTerms(TrajOptProblem& prob) override;
 
  static TermInfoPtr create()
  {
    return std::make_shared<CartPoseTermInfo>();
  }
};
 
struct JointPoseTermInfo : public TermInfo
{
  trajopt::DblVec coeffs;
  trajopt::DblVec targets;
  trajopt::DblVec upper_tols;
  trajopt::DblVec lower_tols;
  int first_step = 0;
  int last_step = -1;
 
  JointPoseTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME)
  {
  }
 
  void addObjectiveTerms(TrajOptProblem& prob) override;
 
  static TermInfoPtr create()
  {
    return std::make_shared<JointPoseTermInfo>();
  }
};
 
struct JointVelTermInfo : public TermInfo
{
  trajopt::DblVec coeffs;
  trajopt::DblVec targets;
  trajopt::DblVec upper_tols;
  trajopt::DblVec lower_tols;
  int first_step = 0;
  int last_step = -1;
 
  JointVelTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME)
  {
  }
 
  void addObjectiveTerms(TrajOptProblem& prob) override;
 
  static TermInfoPtr create()
  {
    return std::make_shared<JointVelTermInfo>();
  }
};
 
void generateInitialTrajectory(const ProblemInfo& pci, const std::vector<double>& current_joint_values,
                               trajopt::TrajArray& init_traj);
 
}  // namespace trajopt_interface