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();