ee_motion_phases now parametrized with p_start and p_end each

CMakeLists.txt

@@ -92,7 +92,7 @@ add_library(${MOTION_OPT_LIB}
   src/motion_factory.cc
   src/linear_spline_equations.cc
   # endeffector
-  src/ee_swing_motion.cc
+  src/ee_phase_motion.cc
   src/ee_motion.cc
   src/endeffectors_motion.cc
   src/endeffector_load.cc
@@ -174,7 +174,7 @@ if(BUILD_TESTS)
 #                        test/google/ipopt_test.cc
 #                        # general
 #                        test/google/sparse_matrix_test.cc
-                         test/google/ee_swing_motion_test.cc
+                         test/google/ee_phase_motion_test.cc
                          test/google/ee_motion_test.cc
                          ## these should be kept well maintained
                          test/dynamic_constraint_test.cc

config/ipopt.opt

@@ -29,7 +29,7 @@ tol 0.01   # usually 0.01
 
 
 #bound_relax_factor 0.01
-max_cpu_time 5.1  
+max_cpu_time 15.1  
 #max_iter 2
 #bound_frac 0.5
 

include/xpp/opt/ee_motion.h

@@ -8,12 +8,12 @@
 #ifndef XPP_XPP_OPT_INCLUDE_XPP_OPT_EE_MOTION_H_
 #define XPP_XPP_OPT_INCLUDE_XPP_OPT_EE_MOTION_H_
 
-#include <xpp/opt/ee_swing_motion.h>
 #include <xpp/contact.h>
 #include <xpp/parametrization.h>
 
 #include <Eigen/Sparse>
 #include <deque>
+#include "ee_phase_motion.h"
 
 namespace xpp {
 namespace opt {
@@ -23,12 +23,11 @@ namespace opt {
 class EEMotion : public Parametrization {
 public:
   using ContactPositions = std::deque<Contact>;
-  using JacobianRow = Eigen::SparseVector<double, Eigen::RowMajor>;
+  using PhaseContacts    = std::array<Contact, 2>;
 
   EEMotion ();
   virtual ~EEMotion ();
 
-
   void SetInitialPos(const Vector3d& pos, EndeffectorID);
   void AddStancePhase(double t);
   void AddSwingPhase(double t, const Vector3d& goal);
@@ -44,22 +43,29 @@ class EEMotion : public Parametrization {
   ContactPositions GetContact(double t_global) const;
 
 
-
   VectorXd GetOptimizationParameters() const override;
   void SetOptimizationParameters(const VectorXd&) override;
   // haven't yet implemented the derivative during swing phase
-//  JacobianRow GetJacobianPos(double t, d2::Coords dimension) const;
+  JacobianRow GetJacobianPos(double t, d2::Coords dimension) const;
   int Index(int id, d2::Coords dimension) const;
 
 
 private:
   int GetPhase(double t_global) const;
-  void AddPhase(double t, const Vector3d& goal, double lift_height);
+  void AddPhase(double t, const Vector3d& goal, double lift_height, int id_goal);
   void UpdateSwingMotions();
+  double GetLocalTime(double t_global, int phase) const;
+
+  Contact GetLastContact() const;
+
+//  ContactPositions contacts_;
+  Contact first_contact_;
+  int n_steps = 0;
+
+  std::vector<PhaseContacts> phase_contacts_;
 
-  ContactPositions contacts_;
   std::deque<bool> is_contact_phase_; // zmp_ this deserves a separate class
-  std::vector<EESwingMotion> phase_motion_;
+  std::vector<EEPhaseMotion> phase_motion_;
 
 };
 

include/xpp/opt/ee_swing_motion.h → include/xpp/opt/ee_phase_motion.h

@@ -1,5 +1,5 @@
 /**
- @file    ee_swing_motion.h
+ @file    ee_phase_motion.h
  @author  Alexander W. Winkler ([email protected])
  @date    Jan 16, 2017
  @brief   Brief description
@@ -17,16 +17,16 @@ namespace opt {
 
 /** Parametrizes the motion from one 3D point to another.
   *
-  * This can be used to generate the swingleg motion given two footholds.
-  * See xpp_opt/matlab/swingleg_z_height.m for the generation of these values.
+  * This can be used to generate the swingleg motion given two footholds or
+  * represent a leg in stance ("movement" between the same start/goal).
   */
-class EESwingMotion {
+class EEPhaseMotion {
 public:
-  using PolyXY     = PolynomialXd<CubicPolynomial, StateLin2d>;
-  using PolyZ      = LiftHeightPolynomial;
+  using PolyXY      = PolynomialXd<CubicPolynomial, StateLin2d>;
+  using PolyZ       = LiftHeightPolynomial;
 
-  EESwingMotion ();
-  virtual ~EESwingMotion ();
+  EEPhaseMotion ();
+  virtual ~EEPhaseMotion ();
 
   /** Completely parametrizes the motion.
     *
@@ -52,6 +52,9 @@ class EESwingMotion {
   StateLin3d GetState(double t_local) const;
   double GetDuration() const;
 
+  double GetDerivativeOfPosWrtContactsXY(d2::Coords dim, double t_local,
+                                         Polynomial::PointType p) const;
+
 private:
   PolyZ poly_z_;
   PolyXY poly_xy_;

include/xpp/opt/impl/polynomial_xd-impl.h

@@ -78,6 +78,12 @@ bool PolynomialXd<PolynomialType, PointType>::GetPoint(const double dt,
   return true;
 }
 
+template<typename PolynomialType, typename PointType>
+PolynomialType
+PolynomialXd<PolynomialType, PointType>::GetDim (int dim) const
+{
+  return polynomials_.at(dim);
+}
 
 } // namespace opt
 } // namespace xpp

include/xpp/opt/polynomial.h

@@ -25,9 +25,9 @@ namespace opt {
 /** Constructs a polynomial given start and end states.
   *
   * The polynomial types are:
-  * Linear:  Ex + F;
-  * Cubic:   Dx^2 + Ex + F;
-  * Quintic: At^5 + Bt^4 + Ct^3 + Dt^2 + Et + f
+  * Linear:  Et + F;
+  * Cubic:   Ct^3 + Dt^2 + Et + F;
+  * Quintic: At^5 + Bt^4 + Ct^3 + Dt^2 + Et + F
   */
 class Polynomial {
 public:
@@ -36,6 +36,8 @@ class Polynomial {
   enum PolynomialCoeff { A=0, B, C, D, E, F };
   static constexpr std::array<PolynomialCoeff, 6> AllSplineCoeff = {{A,B,C,D,E,F}};
 
+  enum PointType {Start=0, Goal=1};
+
 public:
   Polynomial();
   virtual ~Polynomial() {};
@@ -61,10 +63,10 @@ class Polynomial {
   double GetDuration() const;
 
 protected:
+  double duration;
   std::array< double, AllSplineCoeff.size() > c; //!< coefficients of spline
 
 private:
-  double duration;
   /**
    * @brief Calculates all spline coeff of current spline.
    *
@@ -105,13 +107,19 @@ class LinearPolynomial : public Polynomial {
   void SetPolynomialCoefficients(double T, const StateLin1d& start, const StateLin1d& end);
 };
 
+/** @brief a polynomial of the form ct^3 + dt^2 + et + f.
+ * see matlab script "third_order_poly.m" for generation of these values.
+ */
 class CubicPolynomial : public Polynomial {
 public:
   CubicPolynomial() {};
   ~CubicPolynomial() {};
 
   static int GetNumCoeff() { return 4; }; //C,D,E,F
 
+  // spring_clean_ move up to base class?
+  double GetDerivativeOfPosWrtPos(double t, PointType p) const;
+
 private:
   void SetPolynomialCoefficients(double T, const StateLin1d& start, const StateLin1d& end);
 };

include/xpp/opt/polynomial_xd.h

@@ -36,13 +36,17 @@ class PolynomialXd {
   double GetCoefficient(int dim, PolyCoeff coeff) const;
   void   SetCoefficients(int dim, PolyCoeff coeff, double value);
 
+
   static int GetNumCoeff() { return PolynomialType::GetNumCoeff(); };
 
   double GetDuration() const;
 
   uint GetId()            const { return id_; };
   void SetId(uint id)           { id_ = id;   };
 
+  PolynomialType GetDim(int dim) const;
+
+
 private:
   std::array<PolynomialType, kNumDim> polynomials_; ///< X,Y,Z dimensions
   uint id_; // to identify the order relative to other polynomials

matlab/third_order_poly.m

@@ -2,21 +2,21 @@
 clear all;
 
 % 3rd order poly
-syms a b c d t p1 p2 t1 t2
+syms a b c d t p0 p1 v0 v1 T
 p   = a*t^3 + b*t^2 + c*t + d;
 pv  = diff(p, t);
 
 
 % initial position and velocity are zero
 t = 0;
-eq_init_p = subs(p) == p1
-eq_init_v = subs(pv) == 0
+eq_init_p = subs(p) == p0
+eq_init_v = subs(pv) == v0
 
 
 % final position and velocity are zero (at time t=1)
-t = t2;
-eq_final_p = subs(p) == p2
-eq_final_v = subs(pv) == 0
+t = T;
+eq_final_p = subs(p) == p1
+eq_final_v = subs(pv) == v1
 
 
 % solve the system of equations for the polynomial coefficients
@@ -28,4 +28,8 @@
 d = S.d
 
 syms t;
-pretty(subs(p))
+pretty(subs(p))
+
+
+% Get the derivative w.r.t the initial and final position
+jac_p = jacobian(subs(p), [p0 p1])