created new quintic swingleg_z_height polynomial

include/xpp/opt/ee_motion.h

@@ -54,7 +54,7 @@ class EEMotion : public Parametrization {
 
 private:
   int GetPhase(double t_global) const;
-  void AddPhase(double t, const Vector3d& goal, double lift_height = 0.03);
+  void AddPhase(double t, const Vector3d& goal, double lift_height);
   void UpdateSwingMotions();
 
   ContactPositions contacts_;

include/xpp/opt/ee_swing_motion.h

@@ -1,5 +1,5 @@
 /**
- @file    ee_height_z_polynomial.h
+ @file    ee_swing_motion.h
  @author  Alexander W. Winkler ([email protected])
  @date    Jan 16, 2017
  @brief   Brief description
@@ -23,7 +23,7 @@ namespace opt {
 class EESwingMotion {
 public:
   using PolyXY     = PolynomialXd<CubicPolynomial, StateLin2d>;
-  using PolyZ      = QuinticPolynomial;
+  using PolyZ      = LiftHeightPolynomial;
 
   EESwingMotion ();
   virtual ~EESwingMotion ();
@@ -55,10 +55,7 @@ class EESwingMotion {
 private:
   PolyZ poly_z_;
   PolyXY poly_xy_;
-
   double T_ = 0.0;   ///< the duration [s] of the motion
-  double h_ = 0.03;  ///< proportional to the lift height between contacts
-  int n_    = 6;     ///< determines the shape of the swing motion
 };
 
 } /* namespace opt */

include/xpp/opt/polynomial.h

@@ -121,6 +121,31 @@ class QuinticPolynomial : public Polynomial {
 private:
   void SetPolynomialCoefficients(double T, const StateLin1d& start, const StateLin1d& end);
 };
+
+/** @brief Creates a smooth up and down motion for e.g. swinging a leg.
+ *
+ * see matlab script "swingleg_z_height.m" for generation of these values.
+ */
+class LiftHeightPolynomial : public Polynomial {
+public:
+  LiftHeightPolynomial() {};
+  ~LiftHeightPolynomial() {};
+
+  static int GetNumCoeff() { return 6; }; //A,B,C,D,E,F
+
+  /** Determines how quick the height rises/drops.
+   *
+   * h is not the exact height between the contact points, but the height
+   * that the swingleg has as 1/n_*T and (n-1)/n*T, e.g. shortly after lift-off
+   * and right before touchdown. The lift-height in the center is higher.
+   */
+  void SetShape(int n, double h);
+
+private:
+  int n_ = 6;        ///< determines the shape of the swing motion
+  double h_ = 0.03;  ///< proportional to the lift height between contacts
+  void SetPolynomialCoefficients(double T, const StateLin1d& start, const StateLin1d& end);
+};
 /** @} */
 
 } // namespace opt

src/ee_motion.cc

@@ -35,7 +35,8 @@ EEMotion::AddStancePhase (double t)
 void
 EEMotion::AddSwingPhase (double t, const Vector3d& goal)
 {
-  AddPhase(t, goal);
+  double light_height = 0.03;
+  AddPhase(t, goal, light_height);
   Contact c(contacts_.back().id +1 , contacts_.back().ee, goal);
   contacts_.push_back(c);
   is_contact_phase_.push_back(false);

src/ee_swing_motion.cc

@@ -1,5 +1,5 @@
 /**
- @file    ee_height_z_polynomial.cc
+ @file    ee_swing_motion.cc
  @author  Alexander W. Winkler ([email protected])
  @date    Jan 16, 2017
  @brief   Brief description
@@ -23,7 +23,7 @@ EESwingMotion::Init (double T, double h, const Vector3d& start,
                      const Vector3d& end)
 {
   T_ = T;
-  h_ = h;
+  poly_z_.SetShape(6,h);
   SetContacts(start, end);
 }
 
@@ -34,32 +34,13 @@ EESwingMotion::GetDuration () const
 }
 
 void
-EESwingMotion::SetContacts (const Vector3d& start_pos, const Vector3d& end_pos)
+EESwingMotion::SetContacts (const Vector3d& start_pos,
+                            const Vector3d& end_pos)
 {
   StateLin3d start_state(start_pos);
   StateLin3d end_state(end_pos);
   poly_xy_.SetBoundary(T_, start_state.Get2D(), end_state.Get2D());
-
-  double n2 = std::pow(n_,2);
-  double n3 = std::pow(n_,3);
-  double n4 = std::pow(n_,4);
-  double n5 = std::pow(n_,5);
-
-  double t2 = std::pow(T_,2);
-  double t3 = std::pow(T_,3);
-  double t4 = std::pow(T_,4);
-  double t5 = std::pow(T_,5);
-
-  double z_start = start_pos.z();
-  double z_end = end_pos.z();
-  // see matlab script "swingleg_z_height.m" for generation of these values
-  poly_z_.c[Polynomial::A] = -(2*(2*n2*z_end - 3*n3*z_end - 2*n2*z_start + 3*n3*z_start))/(t5*(n_ - 2)*(n2 - 2*n_ + 1));
-  poly_z_.c[Polynomial::B] = -(2*h_*n4 - h_*n5 - 10*n2*z_end + 15*n3*z_end + 10*n2*z_start - 15*n3*z_start)/(t4*(n_ - 2)*(n2 - 2*n_ + 1));
-  poly_z_.c[Polynomial::C] =  (2*(2*z_end - 2*z_start - 5*n_*z_end + 5*n_*z_start + 2*h_*n4 - h_*n5 + 5*n3*z_end - 5*n3*z_start))/((n_ - 2)*(n2*t3 - 2*n_*t3 + t3));
-  poly_z_.c[Polynomial::D] =  (6*z_end - 6*z_start - 15*n_*z_end + 15*n_*z_start + 2*h_*n4 - h_*n5 + 10*n2*z_end - 10*n2*z_start)/(- n3*t2 + 4*n2*t2 - 5*n_*t2 + 2*t2);
-  poly_z_.c[Polynomial::E] =  0;
-  poly_z_.c[Polynomial::F] =  z_start;
-  poly_z_.duration = T_;
+  poly_z_.SetBoundary(T_,start_state.Get1d(Z), end_state.Get1d(Z));
 }
 
 StateLin3d

src/polynomial.cc

@@ -36,9 +36,9 @@ bool Polynomial::GetPoint(const double dt, StateLin1d& out) const
   double dt5 = dt1 * dt4;
 
   out.p   = c[F] + c[E]*dt1 +   c[D]*dt2 +   c[C]*dt3 +    c[B]*dt4 +    c[A]*dt5;
-  out.v  =        c[E]     + 2*c[D]*dt1 + 3*c[C]*dt2 +  4*c[B]*dt3 +  5*c[A]*dt4;
-  out.a =                   2*c[D]     + 6*c[C]*dt1 + 12*c[B]*dt2 + 20*c[A]*dt3;
-  out.j=                                6*c[C]     + 24*c[B]*dt1 + 60*c[A]*dt2;
+  out.v  =         c[E]     + 2*c[D]*dt1 + 3*c[C]*dt2 +  4*c[B]*dt3 +  5*c[A]*dt4;
+  out.a =                     2*c[D]     + 6*c[C]*dt1 + 12*c[B]*dt2 + 20*c[A]*dt3;
+  out.j=                                   6*c[C]     + 24*c[B]*dt1 + 60*c[A]*dt2;
 
   return true;
 }
@@ -81,5 +81,39 @@ void QuinticPolynomial::SetPolynomialCoefficients(double T, const StateLin1d& st
   c[A] = -( 12*start.p - 12*end.p + T1*(   start.a*T1 -   end.a*T1 +  6*(start.v +    end.v))) / (2*T5);
 }
 
+void
+LiftHeightPolynomial::SetShape (int n, double h)
+{
+  n_ = n;
+  h_ = h;
+}
+
+void
+LiftHeightPolynomial::SetPolynomialCoefficients (
+    double T, const StateLin1d& start, const StateLin1d& end)
+{
+  double n2 = std::pow(n_,2);
+  double n3 = std::pow(n_,3);
+  double n4 = std::pow(n_,4);
+  double n5 = std::pow(n_,5);
+
+  double T1 = T;
+  double T2 = T1 * T1;
+  double T3 = T1 * T2;
+  double T4 = T1 * T3;
+  double T5 = T1 * T4;
+
+  // see matlab script "swingleg_z_height.m" for generation of these values
+  c[A] = -(2*(2*n2*end.p - 3*n3*end.p - 2*n2*start.p + 3*n3*start.p))/(T5*(n_ - 2)*(n2 - 2*n_ + 1));
+  c[B] = -(2*h_*n4 - h_*n5 - 10*n2*end.p + 15*n3*end.p + 10*n2*start.p - 15*n3*start.p)/(T4*(n_ - 2)*(n2 - 2*n_ + 1));
+  c[C] =  (2*(2*end.p - 2*start.p - 5*n_*end.p + 5*n_*start.p + 2*h_*n4 - h_*n5 + 5*n3*end.p - 5*n3*start.p))/((n_ - 2)*(n2*T3 - 2*n_*T3 + T3));
+  c[D] =  (6*end.p - 6*start.p - 15*n_*end.p + 15*n_*start.p + 2*h_*n4 - h_*n5 + 10*n2*end.p - 10*n2*start.p)/(- n3*T2 + 4*n2*T2 - 5*n_*T2 + 2*T2);
+  c[E] =  0;
+  c[F] =  start.p;
+}
+
+
 } // namespace opt
 } // namespace xpp
+
+