update viapoint ftn

bindings/python/timeopt/problem.hpp

@@ -38,6 +38,7 @@ namespace timeopt{
         .def("setPhase",&Problem::setPhase, (bp::arg("index"), bp::arg("phase")),"Set ith phase.")
         .def("setMass",&Problem::setMass, (bp::args("mass")),"Set robot mass.")
         .def("setFinalCOM",&Problem::setFinalCOM, (bp::arg("COM")),"Set Final COM (Vector3d).")
+        .def("setViapoint",&Problem::setViapoint, (bp::args("time", "viapoint(Vector3d)")), "Set Viapoint (time, Vector3d).")  
 
         .def("resize",&Problem::resize, (bp::args("size")),"Resize phases size.")
         .def("getTrajectorySize", &Problem::getNumSize, "get Trajectory length")

include/timeopt/interface/contactplanner.hpp

@@ -24,21 +24,7 @@ using namespace std;
         typedef Eigen::Matrix<double, 8, 1> Vector8d;
 
       public:
-        InitialState()
-        : ini_com_(Eigen::Vector3d(.0, .0, .0))
-        , amom_(Eigen::Vector3d(.0, .0, .0))
-        , lmom_(Eigen::Vector3d(.0, .0, .0))
-        , eef_frc_rf_(Eigen::Vector3d(.0, .0, .5))
-        , eef_frc_lf_(Eigen::Vector3d(.0, .0, .5))
-        , eef_frc_rh_(Eigen::Vector3d(.0, .0, .0))
-        , eef_frc_lh_(Eigen::Vector3d(.0, .0, .0))
-        {
-          eef_rf.setZero();
-          eef_lf.setZero();
-          eef_rh.setZero();
-          eef_lh.setZero();
-        };
-
+        InitialState();
         ~InitialState(){};
         
         inline void setCOM(const Eigen::Vector3d& com) {
@@ -61,7 +47,10 @@ using namespace std;
         
       private:
         Eigen::Vector3d ini_com_, amom_, lmom_;
-        Eigen::Vector3d eef_frc_rf_, eef_frc_lf_, eef_frc_lh_, eef_frc_rh_;
+        Eigen::Vector3d eef_frc_rf_;
+        Eigen::Vector3d eef_frc_lf_;
+        Eigen::Vector3d eef_frc_lh_;
+        Eigen::Vector3d eef_frc_rh_;
         Vector8d eef_rf, eef_lf, eef_rh, eef_lh;
         EndeffectorID ee_id;
         YAML::Node init_cfg;
@@ -112,13 +101,11 @@ using namespace std;
     {
       public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        typedef Eigen::Matrix<double, 4, 1> Viapoint;
+        typedef std::vector<Viapoint,Eigen::aligned_allocator<Viapoint>> ViapointVector;
 
       public:
-        ContactPlanner()
-        : time_horizon_(0.0)
-        , robot_mass_(0.0)
-        , com_displacement_(Eigen::Vector3d(.0, .0, .0))
-        {};
+        ContactPlanner();
         ~ContactPlanner(){};
         
         void initialize(const std::string cfg_path, const InitialState& init_state, const ContactState& contact_state);
@@ -132,11 +119,19 @@ using namespace std;
         inline void setFinalCOM(const Eigen::Vector3d & end_com){
           end_com_ = end_com;
         }
+        inline void setViapoint(const double& time, const Eigen::Vector3d & via){
+          Viapoint v;
+          v << time, via(0), via(1), via(2);     
+          viapoints_.push_back(v);
+        }
         void saveToFile();
 
       private:
         double robot_mass_, time_horizon_;
         Eigen::Vector3d com_displacement_, end_com_;
+        unsigned int num_com_viapoints_;
+        ViapointVector viapoints_;
+
         std::string file_location_;
         
         momentumopt::PlannerSetting planner_setting_;

include/timeopt/problem.hpp

@@ -56,6 +56,9 @@
         final_com_ = com;
         planner_->setFinalCOM(com);
       }
+      void setViapoint(const double& time, const Vector3d& point){
+        planner_->setViapoint(time, point);
+      }
       void resize(const Index index){
         phases_.resize(index);
         contact_state_->resize(index);

src/timeopt/interface/contactplanner.cpp

@@ -2,6 +2,20 @@
 
 namespace timeopt
 {
+  InitialState::InitialState()
+        : ini_com_(Eigen::Vector3d(.0, .0, .0))
+        , amom_(Eigen::Vector3d(.0, .0, .0))
+        , lmom_(Eigen::Vector3d(.0, .0, .0))
+        , eef_frc_rf_(Eigen::Vector3d(.0, .0, .5))
+        , eef_frc_lf_(Eigen::Vector3d(.0, .0, .5))
+        , eef_frc_rh_(Eigen::Vector3d(.0, .0, .0))
+        , eef_frc_lh_(Eigen::Vector3d(.0, .0, .0))
+    {
+          eef_rf.setZero();
+          eef_lf.setZero();
+          eef_rh.setZero();
+          eef_lh.setZero();
+    }
   void InitialState::setEEForceRatio(const Eigen::Vector3d& ratio, EndeffectorID ee){
     switch (ee){
       case RF:
@@ -148,6 +162,11 @@ namespace timeopt
     file_out << contact_cfg;
   }
 
+  ContactPlanner::ContactPlanner()
+        : time_horizon_(0.0)
+        , robot_mass_(0.0)
+        , com_displacement_(Eigen::Vector3d(.0, .0, .0))
+        {}
   void ContactPlanner::initialize(const std::string cfg_path, const InitialState& init_state, const ContactState& contact_state){
     planner_setting_.initialize(cfg_path);
     file_location_ = cfg_path;
@@ -170,6 +189,13 @@ namespace timeopt
     com_displacement_ = end_com_ - init_com;
     cfg_pars["planner_variables"]["com_displacement"] = com_displacement_;
     cfg_pars["planner_variables"]["com_displacement"].SetStyle(YAML::EmitterStyle::Flow);
+
+    cfg_pars["planner_variables"]["num_com_viapoints"] = viapoints_.size();
+    
+    for (size_t i=0; i<viapoints_.size(); i++){
+      cfg_pars["planner_variables"]["com_viapoints"]["via"+std::to_string(i)] = viapoints_[i];
+      cfg_pars["planner_variables"]["com_viapoints"]["via"+std::to_string(i)].SetStyle(YAML::EmitterStyle::Flow);
+    }
     std::ofstream file_out(file_location_.substr(0, file_location_.size()-5)+"_final.yaml");
     file_out << init_cfg << endl << endl << contact_cfg << endl << endl << cfg_pars;
   }