Merge branch 'icra_stress'

config/ipopt.opt

@@ -30,7 +30,7 @@ tol 0.001   # usually 0.01
 
 
 #bound_relax_factor 0.01
-max_cpu_time 20
+max_cpu_time 10
 #max_iter 0
 #bound_frac 0.5
 

include/xpp/constraints/swing_constraint.h

@@ -32,6 +32,7 @@ class SwingConstraint : public Constraint {
 private:
   EEMotionNodes::Ptr ee_motion_;
   double t_swing_avg_ = 100.2;
+//  double swing_height_in_world_ = 0.06; //  hacky way to lift legs
 };
 
 

include/xpp/motion_optimizer_facade.h

@@ -40,7 +40,6 @@ class MotionOptimizerFacade {
   std::vector<RobotStateVec> GetIntermediateSolutions(double dt) const;
   RobotStateVec GetTrajectory(double dt) const;
 
-  EndeffectorsPos initial_ee_W_;
   State3dEuler inital_base_;
 
   HeightMap::Ptr terrain_;
@@ -50,8 +49,18 @@ class MotionOptimizerFacade {
 //  const MotionParametersPtr GetMotionParameters() const { return params_;};
 
   void SetFinalState(const StateLin3d& lin, const StateLin3d& ang);
+  void SetIntialFootholds(EndeffectorsPos pos) {initial_ee_W_ = pos; };
+
+  void SetTerrainFromAvgFootholdHeight() const
+  {
+    double avg_height=0.0;
+    for ( auto pos : initial_ee_W_.ToImpl())
+      avg_height += pos.z()/initial_ee_W_.GetCount();
+    terrain_->SetGroundHeight(avg_height);
+  }
 
 private:
+  EndeffectorsPos initial_ee_W_;
   State3dEuler final_base_;
   RobotStateVec GetTrajectory(const OptimizationVariablesPtr&, double dt) const;
 

include/xpp/quadruped_gait_generator.h

@@ -15,6 +15,20 @@
 namespace xpp {
 namespace quad {
 
+
+enum QuadrupedGaits {Stand=0, Leglift, Walk, TrotFly, Trot,  Pace, Bound, Pronk, kNumGaits};
+static std::map<int, std::string> gait_names =
+{
+  {Stand ,  "Stand"},
+  {Leglift, "Leglift"},
+  {Walk  ,  "Walk"},
+  {Trot,    "Trot"},
+  {TrotFly, "TrotFly"},
+  {Pace,    "Pace"},
+  {Bound,   "Bound"},
+  {Pronk,   "Pronk"}
+};
+
 class QuadrupedGaitGenerator {
 public:
   static const int n_ee_ = 4; // number of endeffectors
@@ -27,15 +41,16 @@ class QuadrupedGaitGenerator {
   virtual ~QuadrupedGaitGenerator ();
 
   FootDurations GetContactSchedule() const;
-
-
-  enum QuadrupedGaits {Walk=0, Trot,  Pace, Bound, kNumGaits};
   void SetGait(QuadrupedGaits gait);
 
+  void SetDurationsStand();
+  void SetDurationsLeglift();
   void SetDurationsWalk();
   void SetDurationsTrot();
+  void SetDurationsTrotFly();
   void SetDurationsPace();
   void SetDurationsBound();
+  void SetDurationsPronk();
 
 private:
   VecTimes phase_times_;

include/xpp/state.h

@@ -193,12 +193,11 @@ StateLinXd::operator!=(const StateLinXd &other) const
 
 inline std::ostream& operator<<(std::ostream& out, const StateAng3d& ori)
 {
-  Eigen::Vector3d rpy_rad, rpy_deg;
-  rpy_rad = ori.q.toRotationMatrix().eulerAngles(2,1,0);
-  rpy_deg = rpy_rad * (180.0 / 3.14);
+  Vector3d rpy_rad;
+  rpy_rad = GetEulerZYXAngles(ori.q);
   out << "rpy=" << rpy_rad.transpose() << "  "
-      << "v=" << ori.v.transpose() << "  "
-      << "a=" << ori.a.transpose();
+      << "v="   << ori.v.transpose() << "  "
+      << "a="   << ori.a.transpose();
   return out;
 }
 

include/xpp/variables/node_values.h

@@ -52,7 +52,7 @@ class NodeValues : public Component, public Spline {
     int phase_;
     int poly_id_in_phase_;
     int num_polys_in_phase_;
-    bool is_constant_; // spring_clean_ this shouldn't be here, has to do with phases
+    bool is_constant_; // zmp_ this shouldn't be here, has to do with phases
   };
 
   using PolyInfoVec = std::vector<PolyInfo>;

src/centroidal_model.cc

@@ -194,7 +194,7 @@ HyqModel::SetInitialGait (int gait_id)
 {
   using namespace xpp::quad;
   QuadrupedGaitGenerator gait_gen;
-  gait_gen.SetGait(static_cast<QuadrupedGaitGenerator::QuadrupedGaits>(gait_id));
+  gait_gen.SetGait(static_cast<QuadrupedGaits>(gait_id));
   contact_timings_ = gait_gen.GetContactSchedule();
 }
 
@@ -213,24 +213,25 @@ AnymalModel::AnymalModel ()
   map_id_to_ee_ = quad::kMapIDToEE;
 
   const double x_nominal_b = 0.33;  // wrt to hip 5cm
-  const double y_nominal_b = 0.13; // wrt to hip -3cm
-  const double z_nominal_b = -0.46; //
+  const double y_nominal_b = 0.15; // wrt to hip -3cm
+  const double z_nominal_b = -0.47;
 
   nominal_stance_.At(map_id_to_ee_.at(LF)) <<  x_nominal_b,   y_nominal_b, z_nominal_b;
   nominal_stance_.At(map_id_to_ee_.at(RF)) <<  x_nominal_b,  -y_nominal_b, z_nominal_b;
   nominal_stance_.At(map_id_to_ee_.at(LH)) << -x_nominal_b,   y_nominal_b, z_nominal_b;
   nominal_stance_.At(map_id_to_ee_.at(RH)) << -x_nominal_b,  -y_nominal_b, z_nominal_b;
 
-  max_dev_from_nominal_ << 0.18, 0.13, 0.1; // max leg length 58cm
-  normal_force_max_ = 1000;
+  //spring_clean_ reduced endeffector range of motion
+  max_dev_from_nominal_ << 0.18, 0.08, 0.07; // spring_clean_ reduce y range
+  normal_force_max_ = 1000; // spring_clean_ halved the max force
 };
 
 void
 AnymalModel::SetInitialGait (int gait_id)
 {
   using namespace xpp::quad;
   QuadrupedGaitGenerator gait_gen;
-  gait_gen.SetGait(static_cast<QuadrupedGaitGenerator::QuadrupedGaits>(gait_id));
+  gait_gen.SetGait(static_cast<QuadrupedGaits>(gait_id));
   contact_timings_ = gait_gen.GetContactSchedule();
 
   //  double f = 0.4; // [s] t_free

src/cost_constraint_factory.cc

@@ -88,37 +88,29 @@ CostConstraintFactory::MakeStateConstraint () const
 
 
   // initial base constraints
-  double t = 0.0; // initial time
-  auto dim = {X, Y, Z};
-  auto derivs = {kPos, kVel};
-  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_linear, t, initial_base_.lin, derivs, dim));
-  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_angular, t, initial_base_.ang, derivs, dim));
-
-
-//  // final linear and angular velocities must be zero
+  double t0 = 0.0; // initial time
   double T = params->GetTotalTime();
+  auto Z_         = {Z};
+  auto XY_        = {X,Y};
+  auto XYZ_       = {X, Y, Z};
+  auto Pos_       = {kPos};
+  auto PosVel_    = {kPos, kVel};
+  auto VelAcc_    = {kVel, kAcc};
+  auto PosVelAcc_ = {kPos, kVel, kAcc};
 
-  dim = {X,Y};
-  derivs = {kPos};
-  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_linear, T, final_base_.lin, derivs, dim));
-
-  // final yaw
-  dim = {Z};
-  derivs = {kPos};
-  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_angular, T, final_base_.ang, derivs, dim));
-
-  dim = {X, Y, Z};
-  derivs = {kVel};
-  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_linear, T, final_base_.lin, derivs, dim));
-  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_angular, T, final_base_.ang, derivs, dim));
 
+  // linear base motion
+  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_linear, t0, initial_base_.lin, PosVelAcc_, XYZ_));
+  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_linear, T,  final_base_.lin,   PosVelAcc_, XYZ_));
 
+  // angular base motion
+  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_angular, t0, initial_base_.ang, PosVelAcc_, XYZ_));
+  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_angular, T,  final_base_.ang,   PosVelAcc_, XYZ_));
+//  constraints->AddComponent(std::make_shared<SplineStateConstraint>(opt_vars_, id::base_angular, T,  final_base_.ang,   VelAcc_, XYZ_));
 
   // junction constraints
-  derivs = {kPos, kVel, kAcc};
-  constraints->AddComponent(std::make_shared<SplineJunctionConstraint>(opt_vars_, id::base_linear, derivs));
-  constraints->AddComponent(std::make_shared<SplineJunctionConstraint>(opt_vars_, id::base_angular, derivs));
-
+  constraints->AddComponent(std::make_shared<SplineJunctionConstraint>(opt_vars_, id::base_linear,  PosVelAcc_));
+  constraints->AddComponent(std::make_shared<SplineJunctionConstraint>(opt_vars_, id::base_angular, PosVelAcc_));
 
 
 //  // endeffector constraints

src/motion_optimizer_facade.cc

@@ -34,27 +34,27 @@ MotionOptimizerFacade::MotionOptimizerFacade ()
   model_   = std::make_shared<AnymalModel>();
   terrain_ = std::make_shared<FlatGround>();
 
-  BuildDefaultInitialState();
+//  BuildDefaultInitialState();
 }
 
 MotionOptimizerFacade::~MotionOptimizerFacade ()
 {
 }
 
-void
-MotionOptimizerFacade::BuildDefaultInitialState ()
-{
-  auto p_nom_B = model_->GetNominalStanceInBase();
-
-  inital_base_.lin.p_ << 0.0, 0.0, -p_nom_B.At(E0).z();
-  inital_base_.ang.p_ << 0.0, 0.0, 0.0; // euler (roll, pitch, yaw)
-
-  initial_ee_W_.SetCount(model_->GetEECount());
-  for (auto ee : initial_ee_W_.GetEEsOrdered()) {
-    initial_ee_W_.At(ee) = p_nom_B.At(ee) + inital_base_.lin.p_;
-    initial_ee_W_.At(ee).z() = 0.0;
-  }
-}
+//void
+//MotionOptimizerFacade::BuildDefaultInitialState ()
+//{
+//  auto p_nom_B = model_->GetNominalStanceInBase();
+//
+//  inital_base_.lin.p_ << 0.0, 0.0, -p_nom_B.At(E0).z();
+//  inital_base_.ang.p_ << 0.0, 0.0, 0.0; // euler (roll, pitch, yaw)
+//
+//  initial_ee_W_.SetCount(model_->GetEECount());
+//  for (auto ee : initial_ee_W_.GetEEsOrdered()) {
+//    initial_ee_W_.At(ee) = p_nom_B.At(ee) + inital_base_.lin.p_;
+//    initial_ee_W_.At(ee).z() = 0.0;
+//  }
+//}
 
 MotionOptimizerFacade::OptimizationVariablesPtr
 MotionOptimizerFacade::BuildVariables () const
@@ -92,10 +92,17 @@ MotionOptimizerFacade::BuildVariables () const
                                                      id::GetEEMotionId(ee),
                                                      params_->ee_splines_per_swing_phase_);
 
-    Vector3d final_ee_pos_W = final_base_.lin.p_ + model_->GetNominalStanceInBase().At(ee);
+    double yaw = final_base_.ang.p_.z();
+    Eigen::Matrix3d w_R_b = GetQuaternionFromEulerZYX(yaw, 0.0, 0.0).toRotationMatrix();
+    Vector3d final_ee_pos_W = final_base_.lin.p_ + w_R_b*model_->GetNominalStanceInBase().At(ee);
+
+
+
     ee_motion->InitializeVariables(initial_ee_W_.At(ee), final_ee_pos_W, contact_schedule.at(ee)->GetTimePerPhase());
-    ee_motion->AddStartBound(kPos, {X,Y}, initial_ee_W_.At(ee));   // only xy, z given by terrain
-//    ee_motion->AddFinalBound(kPos, {X,Y}, final_ee_pos_W);         // fixes the final footholds
+    ee_motion->AddStartBound(kPos, {X,Y}, initial_ee_W_.At(ee));   // spring_clean_ only xy, z given by terrain
+
+    //spring_clean_ fixed final footholds
+    ee_motion->AddFinalBound(kPos, {X,Y}, final_ee_pos_W);         // fixes the final footholds
     opt_variables->AddComponent(ee_motion);
   }
 

src/optimization_parameters.cc

@@ -14,17 +14,17 @@ namespace opt {
 OptimizationParameters::OptimizationParameters ()
 {
   order_coeff_polys_  = 4; // used only with coeff_spline representation
-  dt_base_polynomial_ = 0.25;
+  dt_base_polynomial_ = 0.15;
 
 
   // 2 also works quite well. Remember that inbetween the nodes, forces
   // could still be violating unilateral and friction constraints by
   // polynomial interpolation
-  force_splines_per_stance_phase_ = 2;
+  force_splines_per_stance_phase_ = 4; // spring_clean_   this makes it a lot bigger
 
 
   // range of motion constraint
-  dt_range_of_motion_ = 0.10;
+  dt_range_of_motion_ = 0.1;
   // not used, hardcoded for xy and z.
   ee_splines_per_swing_phase_ = 2; // should always be 2 if i want to use swing constraint!