diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0a746ae47..f03800f6b 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/config/ipopt.opt b/config/ipopt.opt
index d7e2540ba..f46cab09c 100644
--- a/config/ipopt.opt
+++ b/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
 
diff --git a/include/xpp/opt/ee_motion.h b/include/xpp/opt/ee_motion.h
index 91c98d988..247c9172a 100644
--- a/include/xpp/opt/ee_motion.h
+++ b/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_;
 
 };
 
diff --git a/include/xpp/opt/ee_swing_motion.h b/include/xpp/opt/ee_phase_motion.h
similarity index 78%
rename from include/xpp/opt/ee_swing_motion.h
rename to include/xpp/opt/ee_phase_motion.h
index ae5340b32..e2f80a4bc 100644
--- a/include/xpp/opt/ee_swing_motion.h
+++ b/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 (winklera@ethz.ch)
  @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_;
diff --git a/include/xpp/opt/impl/polynomial_xd-impl.h b/include/xpp/opt/impl/polynomial_xd-impl.h
index 8e47aa8bc..7977277c4 100644
--- a/include/xpp/opt/impl/polynomial_xd-impl.h
+++ b/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
diff --git a/include/xpp/opt/polynomial.h b/include/xpp/opt/polynomial.h
index ae9350115..67b26d047 100644
--- a/include/xpp/opt/polynomial.h
+++ b/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,6 +107,9 @@ 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() {};
@@ -112,6 +117,9 @@ class CubicPolynomial : public Polynomial {
 
   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);
 };
diff --git a/include/xpp/opt/polynomial_xd.h b/include/xpp/opt/polynomial_xd.h
index 3fb1f0a5d..0699ef3be 100644
--- a/include/xpp/opt/polynomial_xd.h
+++ b/include/xpp/opt/polynomial_xd.h
@@ -36,6 +36,7 @@ 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;
@@ -43,6 +44,9 @@ class PolynomialXd {
   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
diff --git a/matlab/third_order_poly.m b/matlab/third_order_poly.m
index 934c9c194..a65a5e339 100644
--- a/matlab/third_order_poly.m
+++ b/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))
\ No newline at end of file
+pretty(subs(p))
+
+
+% Get the derivative w.r.t the initial and final position
+jac_p = jacobian(subs(p), [p0 p1])
\ No newline at end of file
diff --git a/src/ee_motion.cc b/src/ee_motion.cc
index 53afcbe8f..a3f5fc693 100644
--- a/src/ee_motion.cc
+++ b/src/ee_motion.cc
@@ -21,14 +21,14 @@ EEMotion::~EEMotion ()
 void
 EEMotion::SetInitialPos (const Vector3d& pos, EndeffectorID ee)
 {
-  contacts_.push_front(Contact(0, ee, pos));
-  UpdateSwingMotions();
+  first_contact_ = Contact(0, ee, pos);
 }
 
 void
 EEMotion::AddStancePhase (double t)
 {
-  AddPhase(t, contacts_.back().p, 0.0); // stay at same position and don't lift leg
+  Contact c_back = GetLastContact();
+  AddPhase(t, c_back.p, 0.0, c_back.id); // stay at same position and don't lift leg
   is_contact_phase_.push_back(true);
 }
 
@@ -36,39 +36,35 @@ void
 EEMotion::AddSwingPhase (double t, const Vector3d& 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);
+  AddPhase(t, goal, light_height, GetLastContact().id + 1);
   is_contact_phase_.push_back(false);
+  n_steps++;
 }
 
 void
-EEMotion::AddPhase (double t, const Vector3d& goal, double lift_height)
+EEMotion::AddPhase (double t, const Vector3d& goal, double lift_height, int id_goal)
 {
-  assert(!contacts_.empty()); // SetInitialPos() must be called before
+  Contact c_prev = GetLastContact();
 
-  EESwingMotion motion;
-  motion.Init(t, lift_height, contacts_.back().p, goal);
+  EEPhaseMotion motion;
+  motion.Init(t, lift_height, c_prev.p, goal);
   phase_motion_.push_back(motion);
+
+
+  Contact c_goal(id_goal, c_prev.ee, goal);
+  PhaseContacts phase{c_prev, c_goal};
+  phase_contacts_.push_back(phase);
 }
 
 StateLin3d
 EEMotion::GetState (double t_global) const
 {
-  int phase = GetPhase(t_global);
-  double t_local = t_global;
-  for (int i=0; i<phase; ++i)
-    t_local -= phase_motion_.at(i).GetDuration();
+  int phase      = GetPhase(t_global);
+  double t_local = GetLocalTime(t_global, phase);
 
   return phase_motion_.at(phase).GetState(t_local);
 }
 
-//EEMotion::JacobianRow
-//EEMotion::GetJacobianPos (double t, d2::Coords dimension) const
-//{
-//  assert(false); // implement
-//}
-
 int
 EEMotion::GetPhase (double t_global) const
 {
@@ -94,15 +90,9 @@ EEMotion::ContactPositions
 EEMotion::GetContact (double t) const
 {
   ContactPositions contact;
-  if (IsInContact(t)) {
-
-    // add up all swing phases until then
-    int previous_swing_phases = 0;
-    for (int p=0; p<GetPhase(t); ++p)
-      previous_swing_phases += !is_contact_phase_.at(p);
-
-    contact.push_back(contacts_.at(previous_swing_phases));
-  }
+  int phase = GetPhase(t);
+  if (IsInContact(t))
+    contact.push_back(phase_contacts_.at(phase).front()); // .back() would also work, b/c stance phase
 
   return contact;
 }
@@ -110,11 +100,19 @@ EEMotion::GetContact (double t) const
 VectorXd
 EEMotion::GetOptimizationParameters () const
 {
-  // only optimize over contact xy positions for now
-  VectorXd x(contacts_.size()*kDim2d);
-  for (const auto& c: contacts_)
-    for (auto dim : d2::AllDimensions)
-      x(Index(c.id,dim)) = c.p(dim);
+  int id_prev = -1;
+  VectorXd x((1+n_steps)*kDim2d); // initial position plus goal steps-xy
+
+  for (const PhaseContacts& contacts: phase_contacts_) {
+    for (const Contact& c : contacts) { // only optimize over position at end of phase
+      if (c.id != id_prev) {
+        for (auto dim : d2::AllDimensions) {
+          x(Index(c.id,dim)) = c.p(dim);
+        }
+      }
+      id_prev = c.id;
+    }
+  }
 
   return x;
 }
@@ -122,13 +120,47 @@ EEMotion::GetOptimizationParameters () const
 void
 EEMotion::SetOptimizationParameters (const VectorXd& x)
 {
-  for (auto& c: contacts_)
-    for (auto dim : d2::AllDimensions)
-      c.p(dim) = x(Index(c.id,dim));
+  for (PhaseContacts& phase: phase_contacts_)
+    for (Contact& c : phase)
+      for (auto dim : d2::AllDimensions)
+        c.p(dim) = x(Index(c.id,dim));
 
   UpdateSwingMotions();
 }
 
+JacobianRow
+EEMotion::GetJacobianPos (double t_global, d2::Coords dimension) const
+{
+  JacobianRow jac(GetOptVarCount());
+
+  assert(false);
+
+//  // figure out which contacts affect the motion
+//  int phase      = GetPhase(t_global);
+//  double t_local = GetLocalTime(t_global, phase);
+//  bool is_contact_phase = is_contact_phase_.at(phase);
+//
+//  ContactPositions relevant_contacts;
+//  if (is_contact_phase) {
+//    relevant_contacts.push_back(
+//  }
+//
+//
+//  for (auto dim : d2::AllDimensions) {
+//    // for a contact phase it's this (same start and goal, only one contact affects it all)
+//    double dpdc_start = phase_motion_.at(phase).GetDerivativeOfPosWrtContactsXY(dim, t_local, Polynomial::Start);
+//
+//    // for a swingphase the goal position is also relevant
+//    double dpdc_goal  = phase_motion_.at(phase).GetDerivativeOfPosWrtContactsXY(dim, t_local, Polynomial::Goal);
+//
+//    jac(Index(c.id,dim)) = ...
+//  }
+
+
+
+  return jac;
+}
+
 int
 EEMotion::Index (int id, d2::Coords dimension) const
 {
@@ -138,16 +170,11 @@ EEMotion::Index (int id, d2::Coords dimension) const
 void
 EEMotion::UpdateSwingMotions ()
 {
-  int k=0; // contact
-  int i=0; // phase
-
-  for (auto& p : phase_motion_) {
-    if (is_contact_phase_.at(i++))
-      p.SetContacts(contacts_.at(k).p, contacts_.at(k).p);
-    else {
-      p.SetContacts(contacts_.at(k).p, contacts_.at(k+1).p);
-      k++;
-    }
+  int phase = 0;
+  for (auto& motion : phase_motion_) {
+    auto contacts = phase_contacts_.at(phase);
+    motion.SetContacts(contacts.front().p, contacts.back().p);
+    phase++;
   }
 }
 
@@ -155,12 +182,28 @@ double
 EEMotion::GetTotalTime () const
 {
   double T = 0.0;
-  for (auto p : phase_motion_) {
+  for (auto p : phase_motion_)
     T += p.GetDuration();
-  }
 
   return T;
 }
 
+double
+EEMotion::GetLocalTime (double t_global, int phase) const
+{
+  double t_local = t_global;
+  for (int i=0; i<phase; ++i)
+    t_local -= phase_motion_.at(i).GetDuration();
+
+  return t_local;
+}
+
+Contact
+EEMotion::GetLastContact() const
+{
+  bool first_phase = phase_contacts_.empty();
+  return first_phase? first_contact_ : phase_contacts_.back().back();
+}
+
 } /* namespace opt */
 } /* namespace xpp */
diff --git a/src/ee_swing_motion.cc b/src/ee_phase_motion.cc
similarity index 59%
rename from src/ee_swing_motion.cc
rename to src/ee_phase_motion.cc
index 2c367fa84..dd07e988d 100644
--- a/src/ee_swing_motion.cc
+++ b/src/ee_phase_motion.cc
@@ -1,25 +1,25 @@
 /**
- @file    ee_swing_motion.cc
+ @file    ee_phase_motion.cc
  @author  Alexander W. Winkler (winklera@ethz.ch)
  @date    Jan 16, 2017
  @brief   Brief description
  */
 
-#include <xpp/opt/ee_swing_motion.h>
+#include <xpp/opt/ee_phase_motion.h>
 
 namespace xpp {
 namespace opt {
 
-EESwingMotion::EESwingMotion ()
+EEPhaseMotion::EEPhaseMotion ()
 {
 }
 
-EESwingMotion::~EESwingMotion ()
+EEPhaseMotion::~EEPhaseMotion ()
 {
 }
 
 void
-EESwingMotion::Init (double T, double h, const Vector3d& start,
+EEPhaseMotion::Init (double T, double h, const Vector3d& start,
                      const Vector3d& end)
 {
   T_ = T;
@@ -28,13 +28,13 @@ EESwingMotion::Init (double T, double h, const Vector3d& start,
 }
 
 double
-EESwingMotion::GetDuration () const
+EEPhaseMotion::GetDuration () const
 {
   return T_;
 }
 
 void
-EESwingMotion::SetContacts (const Vector3d& start_pos,
+EEPhaseMotion::SetContacts (const Vector3d& start_pos,
                             const Vector3d& end_pos)
 {
   StateLin3d start_state(start_pos);
@@ -44,7 +44,7 @@ EESwingMotion::SetContacts (const Vector3d& start_pos,
 }
 
 StateLin3d
-EESwingMotion::GetState (double t_local) const
+EEPhaseMotion::GetState (double t_local) const
 {
   StateLin1d z;
   poly_z_.GetPoint(t_local, z);
@@ -60,5 +60,12 @@ EESwingMotion::GetState (double t_local) const
   return ee;
 }
 
+double
+EEPhaseMotion::GetDerivativeOfPosWrtContactsXY (d2::Coords dim, double t_local,
+                                                Polynomial::PointType p) const
+{
+  return poly_xy_.GetDim(dim).GetDerivativeOfPosWrtPos(t_local, p);
+}
+
 } /* namespace opt */
 } /* namespace xpp */
diff --git a/src/polynomial.cc b/src/polynomial.cc
index 7bf1b5ebf..0ff3f37fc 100644
--- a/src/polynomial.cc
+++ b/src/polynomial.cc
@@ -74,12 +74,25 @@ void CubicPolynomial::SetPolynomialCoefficients(double T, const StateLin1d& star
 
   c[F] = start.p;
   c[E] = start.v;
-  c[D] = - ((3 * c[F]) - (3 * end.p) + (2 * T1 * c[E]) + (T1 * end.v)) / T2;
-  c[C] =   ((2 * c[F]) - (2 * end.p) +      T1 * (c[E] +       end.v ))/ T3;
+  c[D] = -( 3*(start.p - end.p) +  T1*(2*start.v + end.v) ) / T2;
+  c[C] =  ( 2*(start.p - end.p) +  T1*(  start.v + end.v) ) / T3;
 
   c[B] = c[A] = 0.0;
 }
 
+double
+CubicPolynomial::GetDerivativeOfPosWrtPos (double t, PointType p) const
+{
+  double T2 = duration*duration;
+  double T3 = T2*duration;
+
+  switch (p) {
+    case Start: return (2*t*t*t)/T3 - (3*t*t)/T2 + 1;
+    case Goal:  return (3*t*t)/T2   - (2*t*t*t)/T3;
+    default: assert(false); // point type not defined
+  }
+}
+
 void QuinticPolynomial::SetPolynomialCoefficients(double T, const StateLin1d& start, const StateLin1d& end)
 {
   double T1 = T;
@@ -130,5 +143,3 @@ LiftHeightPolynomial::SetPolynomialCoefficients (
 
 } // namespace opt
 } // namespace xpp
-
-
diff --git a/test/google/ee_swing_motion_test.cc b/test/google/ee_phase_motion_test.cc
similarity index 93%
rename from test/google/ee_swing_motion_test.cc
rename to test/google/ee_phase_motion_test.cc
index 0446389eb..3b147ecf2 100644
--- a/test/google/ee_swing_motion_test.cc
+++ b/test/google/ee_phase_motion_test.cc
@@ -5,7 +5,7 @@
  @brief   Brief description
  */
 
-#include "../../include/xpp/opt/ee_swing_motion.h"
+#include "../../include/xpp/opt/ee_phase_motion.h"
 
 #include <gtest/gtest.h>