ready for testing on anymal

ethz-adrl · Sep 8, 2017 · 604996a · 604996a
1 parent f1f927f
commit 604996a
Show file tree

Hide file tree

Showing 6 changed files with 60 additions and 41 deletions.
diff --git a/config/ipopt.opt b/config/ipopt.opt
@@ -30,7 +30,7 @@ tol 0.001   # usually 0.01
 
 
 #bound_relax_factor 0.01
-max_cpu_time 15
+max_cpu_time 10
 #max_iter 0
 #bound_frac 0.5
 

diff --git a/include/xpp/constraints/swing_constraint.h b/include/xpp/constraints/swing_constraint.h
@@ -32,7 +32,7 @@ class SwingConstraint : public Constraint {
 private:
   EEMotionNodes::Ptr ee_motion_;
   double t_swing_avg_ = 100.2;
-//  double swing_height_in_world_ = 0.06; // spring_clean_ hacky way to lift legs
+//  double swing_height_in_world_ = 0.06; //  hacky way to lift legs
 };
 
 

diff --git a/include/xpp/quadruped_gait_generator.h b/include/xpp/quadruped_gait_generator.h
@@ -16,10 +16,11 @@ namespace xpp {
 namespace quad {
 
 
-enum QuadrupedGaits {Stand=0, Walk, TrotFly, Trot,  Pace, Bound, Pronk, kNumGaits};
+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"},
@@ -43,6 +44,7 @@ class QuadrupedGaitGenerator {
   void SetGait(QuadrupedGaits gait);
 
   void SetDurationsStand();
+  void SetDurationsLeglift();
   void SetDurationsWalk();
   void SetDurationsTrot();
   void SetDurationsTrotFly();

diff --git a/src/cost_constraint_factory.cc b/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

diff --git a/src/motion_optimizer_facade.cc b/src/motion_optimizer_facade.cc
@@ -92,12 +92,14 @@ 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);
-    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));   // spring_clean_ only xy, z given by terrain
+    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);
+
 
 
-    std::cout << "pos xy: " << initial_ee_W_.At(ee).transpose();
+    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));   // 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

diff --git a/src/quadruped_gait_generator.cc b/src/quadruped_gait_generator.cc
@@ -54,6 +54,7 @@ QuadrupedGaitGenerator::SetGait (QuadrupedGaits gait)
 {
   switch (gait) {
     case Stand:    SetDurationsStand();break;
+    case Leglift:  SetDurationsLeglift();break;
     case Walk:     SetDurationsWalk(); break;
     case Trot:     SetDurationsTrot(); break;
     case TrotFly:  SetDurationsTrotFly(); break;
@@ -109,16 +110,37 @@ QuadrupedGaitGenerator::SetDurationsStand ()
   };
 }
 
+void
+QuadrupedGaitGenerator::SetDurationsLeglift ()
+{
+  phase_times_ =
+  {
+      0.3,
+      0.3,
+      0.3
+  };
+  phase_contacts_ =
+  {
+      BB_,
+      Bb_,
+      BB_
+  };
+}
+
 void
 QuadrupedGaitGenerator::SetDurationsPronk ()
 {
   phase_times_ =
   {
-      1.0, 0.5, 1.0
+      0.3,
+      0.2, 0.4, 0.2,
+      0.3
   };
   phase_contacts_ =
   {
-      BB_, II_, BB_
+      BB_,
+      II_, BB_, II_,
+      BB_
   };
 }
 
@@ -216,21 +238,22 @@ QuadrupedGaitGenerator::SetDurationsPace ()
 void
 QuadrupedGaitGenerator::SetDurationsBound ()
 {
-  double t_phase = 0.2;
+  double A = 0.4;
+  double B = 0.2;
   phase_times_ =
   {
       0.3,
-      t_phase, t_phase,
-      t_phase, t_phase,
-      t_phase, t_phase,
+      A, B, A, B,
+      A, B, A, B,
+      A, B, A, B,
       0.2,
   };
   phase_contacts_ =
   {
       BB_,
-      BI_, IB_,
-      BI_, IB_,
-      BI_, IB_,
+      BI_, BB_, IB_, BB_,
+      BI_, BB_, IB_, BB_,
+      BI_, BB_, IB_, BB_,
       BB_,
   };
 }