Factors for the simple Pendulum dynamics for being used with explicit/implicit/sympletic Euler integrators as in [Stern06siggraph]

gtsam_unstable/dynamics/Pendulum.h

@@ -0,0 +1,108 @@
+/**
+ * @file Pendulum.h
+ * @brief Three-way factors for the pendulum dynamics as in [Stern06siggraph] for
+ *        (1) explicit Euler method, (2) implicit Euler method, and (3) sympletic Euler method.
+ *        Note that all methods use the same formulas for the factors. They are only different in
+ *        the way we connect variables using those factors in the graph.
+ * @author Duy-Nguyen Ta
+ */
+
+#pragma once
+
+#include <gtsam/base/LieScalar.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * This class implements the first constraint.
+ *    - For explicit Euler method:  q_{k+1} = q_k + dt*v_k
+ *    - For implicit Euler method:  q_{k+1} = q_k + dt*v_{k+1}
+ *    - For sympletic Euler method: q_{k+1} = q_k + dt*v_{k+1}
+ */
+class PendulumFactor1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+public:
+
+protected:
+  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+
+  /** default constructor to allow for serialization */
+  PendulumFactor1() {}
+
+  double dt_;
+
+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
+  PendulumFactor1(Key k1, Key k, Key velKey, double dt, double mu = 1000.0)
+  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), dt_(dt) {}
+
+  /// @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 */
+  Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v,
+      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();
+    if (H1) *H1 = -eye(p);
+    if (H2) *H2 = eye(p);
+    if (H3) *H3 = eye(p)*dt_;
+    return qk1.localCoordinates(qk.compose(LieScalar(v*dt_)));
+  }
+
+}; // \PendulumFactor1
+
+
+/**
+ * This class implements the second constraint the
+ *    - For explicit Euler method:  v_{k+1} = v_k - dt*g/L*sin(q_k)
+ *    - For implicit Euler method:  v_{k+1} = v_k - dt*g/L*sin(q_{k+1})
+ *    - For sympletic Euler method: v_{k+1} = v_k - dt*g/L*sin(q_k)
+ */
+class PendulumFactor2: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+public:
+
+protected:
+  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+
+  /** default constructor to allow for serialization */
+  PendulumFactor2() {}
+
+  double dt_;
+  double g_;
+  double L_;
+
+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
+  PendulumFactor2(Key vk1, Key vk, Key qkey, double dt, double L = 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) {}
+
+  /// @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 */
+  Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q,
+      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();
+    if (H1) *H1 = -eye(p);
+    if (H2) *H2 = eye(p);
+    if (H3) *H3 = -eye(p)*dt_*g_/L_*sin(q.value());
+    return vk1.localCoordinates(LieScalar(vk - dt_*g_/L_*sin(q)));
+  }
+
+}; // \PendulumFactor2
+
+}

gtsam_unstable/dynamics/tests/testPendulumFactors.cpp

@@ -0,0 +1,43 @@
+/**
+ * @file testPendulumExplicitEuler.cpp
+ * @author Duy-Nguyen Ta
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam_unstable/dynamics/Pendulum.h>
+
+/* ************************************************************************* */
+using namespace gtsam;
+using namespace gtsam::symbol_shorthand;
+
+const double tol=1e-5;
+const double dt = 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)));
+
+/* ************************************************************************* */
+TEST( testPendulumFactor1, evaluateError) {
+  // hard constraints don't need a noise model
+  PendulumFactor1 constraint(Q(2), Q(1), V(1), dt);
+
+  // verify error function
+  EXPECT(assert_equal(zero(1), constraint.evaluateError(q2, q1, v1), tol));
+}
+
+/* ************************************************************************* */
+TEST( testPendulumFactor2, evaluateError) {
+  // hard constraints don't need a noise model
+  PendulumFactor2 constraint(V(2), V(1), Q(1), dt);
+
+  // verify error function
+  EXPECT(assert_equal(zero(1), constraint.evaluateError(v2, v1, q1), tol));
+}
+
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

gtsam_unstable/gtsam_unstable.h

@@ -383,6 +383,21 @@ virtual class VelocityConstraint3 : gtsam::NonlinearFactor {
   Vector evaluateError(const gtsam::LieScalar& x1, const gtsam::LieScalar& x2, const gtsam::LieScalar& v) const;
 };
 
+#include <gtsam_unstable/dynamics/Pendulum.h>
+virtual class PendulumFactor1 : gtsam::NonlinearFactor {
+  /** Standard constructor */
+  PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt);
+
+  Vector evaluateError(const gtsam::LieScalar& qk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& v) const;
+};
+
+virtual class PendulumFactor2 : gtsam::NonlinearFactor {
+  /** Standard constructor */
+  PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g);
+
+  Vector evaluateError(const gtsam::LieScalar& vk1, const gtsam::LieScalar& vk, const gtsam::LieScalar& q) const;
+};
+
 //*************************************************************************
 // nonlinear
 //*************************************************************************