for video

ethz-adrl · Sep 8, 2017 · 12654ff · 12654ff
1 parent bb286fc
commit 12654ff
Show file tree

Hide file tree

Showing 39 changed files with 322 additions and 231 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 10
+max_cpu_time 100
 #max_iter 0
 #bound_frac 0.5
 

diff --git a/include/xpp/composite.h b/include/xpp/composite.h
@@ -15,7 +15,7 @@
 #include <string>
 #include <vector>
 
-#include "bound.h"
+#include "nlp_bound.h"
 
 namespace xpp {
 namespace opt {

diff --git a/include/xpp/constraints/dynamic_constraint.h b/include/xpp/constraints/dynamic_constraint.h
@@ -13,10 +13,10 @@
 #include <vector>
 
 #include <xpp/angular_state_converter.h>
-#include <xpp/bound.h>
 #include <xpp/cartesian_declarations.h>
 #include <xpp/composite.h>
 #include <xpp/dynamic_model.h>
+#include <xpp/nlp_bound.h>
 #include <xpp/variables/contact_schedule.h>
 #include <xpp/variables/spline.h>
 

diff --git a/include/xpp/constraints/force_constraint.h b/include/xpp/constraints/force_constraint.h
@@ -10,9 +10,9 @@
 
 #include <string>
 
-#include <xpp/bound.h>
 #include <xpp/composite.h>
 #include <xpp/height_map.h>
+#include <xpp/nlp_bound.h>
 #include <xpp/variables/phase_nodes.h>
 
 namespace xpp {

diff --git a/include/xpp/constraints/linear_constraint.h b/include/xpp/constraints/linear_constraint.h
@@ -10,9 +10,9 @@
 
 #include <string>
 
-#include <xpp/bound.h>
 #include <xpp/composite.h>
 #include <xpp/matrix_vector.h>
+#include <xpp/nlp_bound.h>
 
 
 namespace xpp {

diff --git a/include/xpp/constraints/range_of_motion_constraint.h b/include/xpp/constraints/range_of_motion_constraint.h
@@ -13,9 +13,9 @@
 #include <vector>
 
 #include <xpp/angular_state_converter.h>
-#include <xpp/bound.h>
 #include <xpp/composite.h>
 #include <xpp/endeffectors.h>
+#include <xpp/nlp_bound.h>
 #include <xpp/optimization_parameters.h>
 #include <xpp/state.h>
 #include <xpp/variables/contact_schedule.h>

diff --git a/include/xpp/constraints/spline_constraint.h b/include/xpp/constraints/spline_constraint.h
@@ -12,9 +12,9 @@
 #include <string>
 #include <vector>
 
-#include <xpp/bound.h>
 #include <xpp/cartesian_declarations.h>
 #include <xpp/composite.h>
+#include <xpp/nlp_bound.h>
 #include <xpp/polynomial.h>
 #include <xpp/state.h>
 #include <xpp/variables/coeff_spline.h>

diff --git a/include/xpp/constraints/swing_constraint.h b/include/xpp/constraints/swing_constraint.h
@@ -10,8 +10,8 @@
 
 #include <string>
 
-#include <xpp/bound.h>
 #include <xpp/composite.h>
+#include <xpp/nlp_bound.h>
 #include <xpp/variables/phase_nodes.h>
 
 namespace xpp {
@@ -31,7 +31,7 @@ class SwingConstraint : public Constraint {
 
 private:
   EEMotionNodes::Ptr ee_motion_;
-  double t_swing_avg_ = 100.2;
+  double t_swing_avg_ = 0.3;
 //  double swing_height_in_world_ = 0.06; //  hacky way to lift legs
 };
 

diff --git a/include/xpp/constraints/terrain_constraint.h b/include/xpp/constraints/terrain_constraint.h
@@ -10,9 +10,9 @@
 
 #include <string>
 
-#include <xpp/bound.h>
 #include <xpp/composite.h>
 #include <xpp/height_map.h>
+#include <xpp/nlp_bound.h>
 #include <xpp/variables/phase_nodes.h>
 
 namespace xpp {

diff --git a/include/xpp/constraints/time_discretization_constraint.h b/include/xpp/constraints/time_discretization_constraint.h
@@ -11,8 +11,8 @@
 #include <string>
 #include <vector>
 
-#include <xpp/bound.h>
 #include <xpp/composite.h>
+#include <xpp/nlp_bound.h>
 
 
 namespace xpp {

diff --git a/include/xpp/dynamic_model.h b/include/xpp/dynamic_model.h
@@ -64,7 +64,7 @@ class DynamicModel {
 
 
   virtual void SetInitialGait(int gait_id) {}; // does nothing if not overwritten
-  std::vector<double> GetNormalizedInitialTimings(EndeffectorID ee) const;
+  std::vector<double> GetNormalizedInitialTimings(EndeffectorID ee);
 
 protected:
   ComPos com_pos_;
@@ -74,6 +74,7 @@ class DynamicModel {
   std::vector<EndeffectorID> ee_ids_;
 
   ContactTimings contact_timings_;
+  void NormalizeTimesToOne(EndeffectorID ee);
 
   EndeffectorsPos nominal_stance_;
   Vector3d max_dev_from_nominal_;

diff --git 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_     = 2.0;
-  double height_     = 1.0; // [m]
+  double length_     = 1.0;
+  double height_     = 0.4; // [m]
 
-  double eps_ = 0.1; // approximate as slope
+  double eps_ = 0.05; // approximate as slope
   const double slope_ = height_/eps_;
 };
 
@@ -95,7 +95,7 @@ class Stairs : public HeightMap {
   virtual double GetHeight(double x, double y) const override;
 
 private:
-  double first_step_start_  = 0.7;
+  double first_step_start_  = 1.5;
   double first_step_width_  = 0.4;
   double height_first_step  = 0.2;
   double height_second_step = 0.4;
@@ -112,9 +112,9 @@ class Gap : public HeightMap {
   virtual double GetHeightDerivWrtXX(double x, double y) const override;
 
 private:
-  const double gap_start_ = 1.0;
-  const double w = 0.7; // gap width
-  const double h = 1.0;
+  const double gap_start_ = 1.5;
+  const double w = 0.5; // gap width
+  const double h = 0.4;
 
   const double slope_ = h/w;
   const double dx = w/2; // gap witdh 2

diff --git a/include/xpp/nlp.h b/include/xpp/nlp.h
@@ -12,8 +12,8 @@
 #include <vector>
 #include <Eigen/Dense>
 
-#include "bound.h"
 #include <xpp/composite.h>
+#include "nlp_bound.h"
 
 namespace xpp {
 namespace opt {

diff --git a/include/xpp/bound.h → include/xpp/nlp_bound.h b/include/xpp/bound.h → include/xpp/nlp_bound.h
@@ -15,8 +15,8 @@ namespace opt {
 
 /** Upper and lower bound on either constraints or optimization variables
   */
-struct Bound {
-  Bound(double lower = 0.0, double upper = 0.0) {
+struct NLPBound {
+  NLPBound(double lower = 0.0, double upper = 0.0) {
     lower_ = lower;
     upper_ = upper;
   }
@@ -34,12 +34,12 @@ struct Bound {
   }
 };
 
-static const Bound kNoBound_        = Bound(-1.0e20, +1.0e20);;
-static const Bound BoundZero        = Bound(0.0, 0.0);
-static const Bound BoundGreaterZero = Bound(0.0, +1.0e20);
-static const Bound BoundSmallerZero = Bound(-1.0e20, 0.0);
+static const NLPBound kNoBound_        = NLPBound(-1.0e20, +1.0e20);;
+static const NLPBound BoundZero        = NLPBound(0.0, 0.0);
+static const NLPBound BoundGreaterZero = NLPBound(0.0, +1.0e20);
+static const NLPBound BoundSmallerZero = NLPBound(-1.0e20, 0.0);
 
-using VecBound = std::vector<Bound>;
+using VecBound = std::vector<NLPBound>;
 
 }
 }

diff --git a/include/xpp/quadruped_gait_generator.h b/include/xpp/quadruped_gait_generator.h
@@ -16,17 +16,18 @@ namespace xpp {
 namespace quad {
 
 
-enum QuadrupedGaits {Stand=0, Leglift, Walk, TrotFly, Trot,  Pace, Bound, Pronk, kNumGaits};
+enum QuadrupedGaits {Stand=0, Leglift, Walk, WalkOverlap, 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"}
+  {Stand,        "Stand"},
+  {Leglift,      "Leglift"},
+  {Walk,         "Walk"},
+  {WalkOverlap,  "WalkOverlap"},
+  {Trot,         "Trot"},
+  {TrotFly,      "TrotFly"},
+  {Pace,         "Pace"},
+  {Bound,        "Bound"},
+  {Pronk,        "Pronk"}
 };
 
 class QuadrupedGaitGenerator {
@@ -36,25 +37,35 @@ class QuadrupedGaitGenerator {
   using ContactState   = EndeffectorsBool;
   using VecTimes       = std::vector<double>;
   using FootDurations  = std::vector<VecTimes>;
+  using GaiInfo        = std::pair<VecTimes,std::vector<ContactState>>;
 
   QuadrupedGaitGenerator ();
   virtual ~QuadrupedGaitGenerator ();
 
   FootDurations GetContactSchedule() const;
-  void SetGait(QuadrupedGaits gait);
+  void SetGaits(const std::vector<QuadrupedGaits>& gaits);
 
-  void SetDurationsStand();
-  void SetDurationsLeglift();
-  void SetDurationsWalk();
-  void SetDurationsTrot();
-  void SetDurationsTrotFly();
-  void SetDurationsPace();
-  void SetDurationsBound();
-  void SetDurationsPronk();
 
 private:
-  VecTimes phase_times_;
-  std::vector<ContactState> phase_contacts_;
+  std::vector<double> times_;
+  std::vector<ContactState> contacts_;
+
+  GaiInfo GetGait(QuadrupedGaits gait) const;
+
+  GaiInfo GetDurationsStand() const;
+  GaiInfo GetDurationsLeglift() const;
+  GaiInfo GetDurationsWalk() const;
+  GaiInfo GetDurationsWalkOverlap() const;
+  GaiInfo GetDurationsTrot() const;
+  GaiInfo GetDurationsTrotFly() const;
+  GaiInfo GetDurationsPace() const;
+  GaiInfo GetDurationsBound() const;
+  GaiInfo GetDurationsPronk() const;
+
+//  /**
+//   * So the total duration of all phase times equals 1 second.
+//   */
+//  void NormalizeTimesToOne();
 
 
   // naming convention:, where the circle is is contact, front is right ->.

diff --git a/include/xpp/variables/contact_schedule.h b/include/xpp/variables/contact_schedule.h
@@ -12,9 +12,9 @@
 #include <string>
 #include <vector>
 
-#include <xpp/bound.h>
 #include <xpp/composite.h>
 #include <xpp/endeffectors.h>
+#include <xpp/nlp_bound.h>
 
 #include "phase_nodes.h"
 
@@ -58,7 +58,7 @@ class ContactSchedule : public Component {
 private:
   bool first_phase_in_contact_ = true;
   double t_total_;
-  Bound phase_duration_bounds_;
+  NLPBound phase_duration_bounds_;
 
   std::vector<PhaseNodesPtr> observers_;
   PhaseNodesPtr GetObserver(const std::string& id) const;

diff --git a/include/xpp/variables/node_values.h b/include/xpp/variables/node_values.h
@@ -13,9 +13,9 @@
 #include <string>
 #include <vector>
 
-#include <xpp/bound.h>
 #include <xpp/cartesian_declarations.h>
 #include <xpp/composite.h>
+#include <xpp/nlp_bound.h>
 #include <xpp/polynomial.h>
 #include <xpp/state.h>
 

diff --git a/include/xpp/variables/phase_nodes.h b/include/xpp/variables/phase_nodes.h
@@ -11,8 +11,8 @@
 #include <string>
 #include <vector>
 
-#include <xpp/bound.h>
 #include <xpp/composite.h>
+#include <xpp/nlp_bound.h>
 
 #include "node_values.h"