factors for the pendulum discrete mechanics in position-momentum form to use with variational integrator

gtsam_unstable/dynamics/Pendulum.h

@@ -14,11 +14,12 @@
 
 namespace gtsam {
 
+//*************************************************************************
 /**
  * This class implements the first constraint.
- *    - For explicit Euler method:  q_{k+1} = q_k + dt*v_k
+ *    - For explicit Euler method:  q_{k+1} = q_k + h*v_k
- *    - For implicit Euler method:  q_{k+1} = q_k + dt*v_{k+1}
+ *    - For implicit Euler method:  q_{k+1} = q_k + h*v_{k+1}
- *    - For sympletic Euler method: q_{k+1} = q_k + dt*v_{k+1}
+ *    - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
  */
 class PendulumFactor1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
 public:
@@ -29,22 +30,22 @@ protected:
   /** default constructor to allow for serialization */
   PendulumFactor1() {}
 
-  double dt_;
+  double h_;  // time step
 
 public:
 
   typedef boost::shared_ptr<PendulumFactor1> shared_ptr;
 
-  ///Constructor.  k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, dt: time step
+  ///Constructor.  k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
-  PendulumFactor1(Key k1, Key k, Key velKey, double dt, double mu = 1000.0)
+  PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
-  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), dt_(dt) {}
+  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), h_(h) {}
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
 
-  /** q_k + dt*v - q_k1 = 0, with optional derivatives */
+  /** q_k + h*v - q_k1 = 0, with optional derivatives */
   Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
@@ -52,18 +53,19 @@ public:
     const size_t p = LieScalar::Dim();
     if (H1) *H1 = -eye(p);
     if (H2) *H2 = eye(p);
-    if (H3) *H3 = eye(p)*dt_;
+    if (H3) *H3 = eye(p)*h_;
-    return qk1.localCoordinates(qk.compose(LieScalar(v*dt_)));
+    return qk1.localCoordinates(qk.compose(LieScalar(v*h_)));
   }
 
 }; // \PendulumFactor1
 
 
+//*************************************************************************
 /**
  * This class implements the second constraint the
- *    - For explicit Euler method:  v_{k+1} = v_k - dt*g/L*sin(q_k)
+ *    - For explicit Euler method:  v_{k+1} = v_k - h*g/L*sin(q_k)
- *    - For implicit Euler method:  v_{k+1} = v_k - dt*g/L*sin(q_{k+1})
+ *    - For implicit Euler method:  v_{k+1} = v_k - h*g/L*sin(q_{k+1})
- *    - For sympletic Euler method: v_{k+1} = v_k - dt*g/L*sin(q_k)
+ *    - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
  */
 class PendulumFactor2: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
 public:
@@ -74,24 +76,24 @@ protected:
   /** default constructor to allow for serialization */
   PendulumFactor2() {}
 
-  double dt_;
+  double h_;
   double g_;
-  double L_;
+  double r_;
 
 public:
 
   typedef boost::shared_ptr<PendulumFactor2 > shared_ptr;
 
-  ///Constructor.  vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, dt: time step
+  ///Constructor.  vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
-  PendulumFactor2(Key vk1, Key vk, Key qkey, double dt, double L = 1.0, double g = 9.81, double mu = 1000.0)
+  PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
-  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), dt_(dt), g_(g), L_(L) {}
+  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
 
-  /**  v_k - dt*g/L*sin(q) - v_k1 = 0, with optional derivatives */
+  /**  v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
   Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
@@ -99,10 +101,123 @@ public:
     const size_t p = LieScalar::Dim();
     if (H1) *H1 = -eye(p);
     if (H2) *H2 = eye(p);
-    if (H3) *H3 = -eye(p)*dt_*g_/L_*cos(q.value());
+    if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value());
-    return vk1.localCoordinates(LieScalar(vk - dt_*g_/L_*sin(q)));
+    return vk1.localCoordinates(LieScalar(vk - h_*g_/r_*sin(q)));
   }
 
 }; // \PendulumFactor2
 
+
+//*************************************************************************
+/**
+ * This class implements the first position-momentum update rule
+ *    p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
+ *    = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1})
+ */
+class PendulumFactorPk: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+public:
+
+protected:
+  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+
+  /** default constructor to allow for serialization */
+  PendulumFactorPk() {}
+
+  double h_;  //! time step
+  double m_;  //! mass
+  double r_;  //! length
+  double g_;  //! gravity
+  double alpha_; //! in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can be obtained as a special case when alpha = 0.
+
+public:
+
+  typedef boost::shared_ptr<PendulumFactorPk > shared_ptr;
+
+  ///Constructor
+  PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
+      double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
+  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey, qKey, qKey1),
+    h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
+
+  /**  1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
+  Vector evaluateError(const LieScalar& pk, const LieScalar& qk, const LieScalar& qk1,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none,
+      boost::optional<Matrix&> H3 = boost::none) const {
+    const size_t p = LieScalar::Dim();
+
+    double qmid = (1-alpha_)*qk + alpha_*qk1;
+    double mr2_h = 1/h_*m_*r_*r_;
+    double mgrh  = m_*g_*r_*h_;
+
+    if (H1) *H1 = -eye(p);
+    if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
+    if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
+
+    return pk.localCoordinates(LieScalar(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid)));
+  }
+
+}; // \PendulumFactorPk
+
+//*************************************************************************
+/**
+ * This class implements the second position-momentum update rule
+ *    p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
+ *    = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1})
+ */
+class PendulumFactorPk1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+public:
+
+protected:
+  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+
+  /** default constructor to allow for serialization */
+  PendulumFactorPk1() {}
+
+  double h_;  //! time step
+  double m_;  //! mass
+  double r_;  //! length
+  double g_;  //! gravity
+  double alpha_; //! in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can be obtained as a special case when alpha = 0.
+
+public:
+
+  typedef boost::shared_ptr<PendulumFactorPk1 > shared_ptr;
+
+  ///Constructor
+  PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
+      double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
+  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey1, qKey, qKey1),
+    h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
+
+  /**  1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
+  Vector evaluateError(const LieScalar& pk1, const LieScalar& qk, const LieScalar& qk1,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none,
+      boost::optional<Matrix&> H3 = boost::none) const {
+    const size_t p = LieScalar::Dim();
+
+    double qmid = (1-alpha_)*qk + alpha_*qk1;
+    double mr2_h = 1/h_*m_*r_*r_;
+    double mgrh  = m_*g_*r_*h_;
+
+    if (H1) *H1 = -eye(p);
+    if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
+    if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
+
+    return pk1.localCoordinates(LieScalar(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid)));
+  }
+
+}; // \PendulumFactorPk1
+
 }

gtsam_unstable/dynamics/tests/testPendulumFactors.cpp

@@ -12,17 +12,17 @@ using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 
 const double tol=1e-5;
-const double dt = 0.1;
+const double h = 0.1;
 const double g = 9.81, l = 1.0;
 
 const double deg2rad = M_PI/180.0;
 LieScalar origin, q1(deg2rad*30.0), q2(deg2rad*31.0);
-LieScalar v1(deg2rad*1.0/dt), v2((v1-dt*g/l*sin(q1)));
+LieScalar v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1)));
 
 /* ************************************************************************* */
 TEST( testPendulumFactor1, evaluateError) {
   // hard constraints don't need a noise model
-  PendulumFactor1 constraint(Q(2), Q(1), V(1), dt);
+  PendulumFactor1 constraint(Q(2), Q(1), V(1), h);
 
   // verify error function
   EXPECT(assert_equal(zero(1), constraint.evaluateError(q2, q1, v1), tol));
@@ -31,12 +31,34 @@ TEST( testPendulumFactor1, evaluateError) {
 /* ************************************************************************* */
 TEST( testPendulumFactor2, evaluateError) {
   // hard constraints don't need a noise model
-  PendulumFactor2 constraint(V(2), V(1), Q(1), dt);
+  PendulumFactor2 constraint(V(2), V(1), Q(1), h);
 
   // verify error function
   EXPECT(assert_equal(zero(1), constraint.evaluateError(v2, v1, q1), tol));
 }
 
+/* ************************************************************************* */
+TEST( testPendulumFactorPk, evaluateError) {
+  // hard constraints don't need a noise model
+  PendulumFactorPk constraint(P(1), Q(1), Q(2), h);
+
+  LieScalar pk( 1/h * (q2-q1) + h*g*sin(q1) );
+
+  // verify error function
+  EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol));
+}
+
+/* ************************************************************************* */
+TEST( testPendulumFactorPk1, evaluateError) {
+  // hard constraints don't need a noise model
+  PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h);
+
+  LieScalar pk1( 1/h * (q2-q1) );
+
+  // verify error function
+  EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol));
+}
+
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

gtsam_unstable/gtsam_unstable.h

@@ -398,6 +398,21 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor {
   Vector evaluateError(const gtsam::LieScalar& vk1, const gtsam::LieScalar& vk, const gtsam::LieScalar& q) const;
 };
 
+virtual class PendulumFactorPk : gtsam::NonlinearFactor {
+  /** Standard constructor */
+  PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
+
+  Vector evaluateError(const gtsam::LieScalar& pk, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
+};
+
+virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
+  /** Standard constructor */
+  PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
+
+  Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
+};
+
+
 //*************************************************************************
 // nonlinear
 //*************************************************************************