diff --git a/config/ipopt.opt b/config/ipopt.opt index e355f93f9..65d1cdab7 100644 --- a/config/ipopt.opt +++ b/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 diff --git a/include/xpp/height_map.h b/include/xpp/height_map.h index 62c50ddcd..5cc247a69 100644 --- a/include/xpp/height_map.h +++ b/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_; }; diff --git a/src/centroidal_model.cc b/src/centroidal_model.cc index 56940fbd2..71f6e8472 100644 --- a/src/centroidal_model.cc +++ b/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 }; } diff --git a/src/motion_optimizer_facade.cc b/src/motion_optimizer_facade.cc index 44a2ea8b7..fa06664fc 100644 --- a/src/motion_optimizer_facade.cc +++ b/src/motion_optimizer_facade.cc @@ -31,7 +31,7 @@ namespace opt { MotionOptimizerFacade::MotionOptimizerFacade () { params_ = std::make_shared(); - model_ = std::make_shared(); + model_ = std::make_shared(); terrain_ = std::make_shared(); BuildDefaultInitialState(); diff --git a/src/optimization_parameters.cc b/src/optimization_parameters.cc index d99018883..34671931c 100644 --- a/src/optimization_parameters.cc +++ b/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();