From 6a139bd0f8c49310607b78758b884f6fa3c19185 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Mon, 15 Apr 2013 21:31:47 +0000 Subject: [PATCH] Factors for the simple Pendulum dynamics for being used with explicit/implicit/sympletic Euler integrators as in [Stern06siggraph] --- gtsam_unstable/dynamics/Pendulum.h | 108 ++++++++++++++++++ .../dynamics/tests/testPendulumFactors.cpp | 43 +++++++ gtsam_unstable/gtsam_unstable.h | 15 +++ 3 files changed, 166 insertions(+) create mode 100644 gtsam_unstable/dynamics/Pendulum.h create mode 100644 gtsam_unstable/dynamics/tests/testPendulumFactors.cpp diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h new file mode 100644 index 000000000..2f90cd931 --- /dev/null +++ b/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 +#include + +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 { +public: + +protected: + typedef NoiseModelFactor3 Base; + + /** default constructor to allow for serialization */ + PendulumFactor1() {} + + double dt_; + +public: + + typedef boost::shared_ptr 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::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 H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional 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 { +public: + +protected: + typedef NoiseModelFactor3 Base; + + /** default constructor to allow for serialization */ + PendulumFactor2() {} + + double dt_; + double g_; + double L_; + +public: + + typedef boost::shared_ptr 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::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 H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional 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 + +} diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp new file mode 100644 index 000000000..2a23f6931 --- /dev/null +++ b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp @@ -0,0 +1,43 @@ +/** + * @file testPendulumExplicitEuler.cpp + * @author Duy-Nguyen Ta + */ + +#include +#include +#include + +/* ************************************************************************* */ +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); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 24547b49e..e2ec13c07 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/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 +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 //*************************************************************************