added clean way to generate different quadruped gaits

config/ipopt.opt

@@ -23,7 +23,7 @@ derivative_test_tol 1e-3   # deviations between provided and finite-difference a
 
 #fixed_variable_treatment relax-bounds
 
-tol 0.001   # usually 0.01
+tol 0.01   # usually 0.01
 #constr_viol_tol 1e-3
 #dual_inf_tol 1e10
 #compl_inf_tol 1e10

include/xpp/height_map.h

@@ -81,10 +81,10 @@ class Block : public HeightMap {
 
 private:
   double block_start = 1.5;
-  double length_     = 1.0;
-  double height_     = 0.4; // [m]
+  double length_     = 3.5;
+  double height_     = 0.8; // [m]
 
-  double eps_ = 0.05; // approximate as slope
+  double eps_ = 0.03; // approximate as slope
   const double slope_ = height_/eps_;
 };
 
@@ -114,10 +114,10 @@ class Gap : public HeightMap {
 private:
   const double gap_start_ = 1.5;
   const double w = 0.5; // gap width
-  const double h = 0.4;
+  const double h = 1.6;
 
   const double slope_ = h/w;
-  const double dx = w/2; // gap witdh 2
+  const double dx = w/2.0; // gap witdh 2
   const double xc = gap_start_ + dx; // gap center
   const double gap_end_x = gap_start_ + w;
 
@@ -130,7 +130,6 @@ class Gap : public HeightMap {
   const double a = (4*h)/(w*w);
   const double b = -(8*h*xc)/(w*w);
   const double c = -(h*(w - 2*xc)*(w + 2*xc))/(w*w);
-
 };
 
 
@@ -140,10 +139,10 @@ class Slope : public HeightMap {
   virtual double GetHeightDerivWrtX(double x, double y) const override;
 
 private:
-  const double slope_start_ = 0.5;
+  const double slope_start_ = 1.0;
   const double up_length_   = 1.0;
   const double down_length_ = 1.0;
-  const double height_center = 0.5;
+  const double height_center = 0.7;
 
   const double x_down_start_ = slope_start_+up_length_;
   const double x_flat_start_ = x_down_start_ + down_length_;
@@ -158,10 +157,10 @@ class Chimney : public HeightMap {
   virtual double GetHeightDerivWrtY(double x, double y) const override;
 
 private:
-  const double x_start_ = 0.5;
-  const double length_  = 1.0;
+  const double x_start_ = 1.5;
+  const double length_  = 1.5;
   const double y_start_ = 0.5; // distance to start of slope from center at z=0
-  const double slope_   = 3;
+  const double slope_   = 3;   // 2 or 3
 
   const double x_end_ = x_start_+length_;
 };

src/centroidal_model.cc

@@ -134,12 +134,12 @@ MonopedModel::MonopedModel ()
 {
   map_id_to_ee_["E0"] = E0;
   nominal_stance_.At(E0) = Vector3d( 0.0, 0.0, -0.58);
-  max_dev_from_nominal_ << 0.15, 0.15, 0.15;
-  normal_force_max_ = 2000;
+  max_dev_from_nominal_ << 0.25, 0.15, 0.2;
+  normal_force_max_ = 800;
 
   double f   = 0.2;
-  double fh  = 0.3;
-  double c   = 0.15;
+  double fh  = 0.2;
+  double c   = 0.2;
   contact_timings_.at(E0) = {c, f, c, f, c, f, c, fh, c, 0.4, c,
                             f, c, f, c, fh, c, f, c, f, c, f, c};
 }
@@ -157,7 +157,7 @@ BipedModel::BipedModel ()
   nominal_stance_.At(map_id_to_ee_.at(L)) << 0.0,  y_nominal_b, z_nominal_b;
   nominal_stance_.At(map_id_to_ee_.at(R)) << 0.0, -y_nominal_b, z_nominal_b;
 
-  max_dev_from_nominal_  << 0.15, 0.15, 0.15;
+  max_dev_from_nominal_  << 0.25, 0.15, 0.18;
   normal_force_max_ = 400;
 
 
@@ -232,8 +232,8 @@ AnymalModel::AnymalModel ()
 
   //spring_clean_ reduced endeffector range of motion
 //  max_dev_from_nominal_ << 0.18, 0.08, 0.07; // for motions on real ANYmal
-  max_dev_from_nominal_ << 0.18, 0.13, 0.09; // spring_clean_ reduce y range
-  normal_force_max_ = 2000; // spring_clean_ halved the max force
+  max_dev_from_nominal_ << 0.23, 0.13, 0.09; // spring_clean_ reduce y range
+  normal_force_max_ = 500; // spring_clean_ halved the max force
 };
 
 void
@@ -248,10 +248,10 @@ AnymalModel::SetInitialGait (int gait_id)
     double f = 0.4; // [s] t_free
     double c = 0.4; // [s] t_contact
     double t_offset = f;
-    contact_timings_.at(kMapIDToEE.at(LH)) = {t_offset + c, f, c, f, c, f, c, f, c           };
-    contact_timings_.at(kMapIDToEE.at(LF)) = {           c, f, c, f, c, f, c, f, c + t_offset};
-    contact_timings_.at(kMapIDToEE.at(RH)) = {           c, f, c, f, c, f, c, f, c + t_offset};
-    contact_timings_.at(kMapIDToEE.at(RF)) = {t_offset + c, f, c, f, c, f, c, f, c           };
+    contact_timings_.at(kMapIDToEE.at(LH)) = {t_offset + c, f, c, f, c, f, c, f, c, f, c           };
+    contact_timings_.at(kMapIDToEE.at(LF)) = {           c, f, c, f, c, f, c, f, c, f, c + t_offset};
+    contact_timings_.at(kMapIDToEE.at(RH)) = {           c, f, c, f, c, f, c, f, c, f, c + t_offset};
+    contact_timings_.at(kMapIDToEE.at(RF)) = {t_offset + c, f, c, f, c, f, c, f, c, f, c           };
 }
 
 

src/motion_optimizer_facade.cc

@@ -31,7 +31,7 @@ namespace opt {
 MotionOptimizerFacade::MotionOptimizerFacade ()
 {
   params_  = std::make_shared<OptimizationParameters>();
-  model_   = std::make_shared<AnymalModel>();
+  model_   = std::make_shared<MonopedModel>();
   terrain_ = std::make_shared<FlatGround>();
 
   BuildDefaultInitialState();

src/optimization_parameters.cc

@@ -26,11 +26,11 @@ OptimizationParameters::OptimizationParameters ()
   // range of motion constraint
   dt_range_of_motion_ = 0.1;
   // not used, hardcoded for xy and z.
-  ee_splines_per_swing_phase_ = 1; // should always be 2 if i want to use swing constraint!
+  ee_splines_per_swing_phase_ = 2; // should always be 2 if i want to use swing constraint!
 
 
   min_phase_duration_ = 0.1;
-  double max_time = 10.0;
+  double max_time = 1.0; // spring_clean_ this helps convergence
   max_phase_duration_ = max_time>GetTotalTime()?  GetTotalTime() : max_time;
 //  max_phase_duration_ = GetTotalTime()/contact_timings_.size();