diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index cb2522edf..8cfc3cbcd 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/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 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} + * - For explicit Euler method: q_{k+1} = q_k + h*v_k + * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1} + * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1} */ class PendulumFactor1: public NoiseModelFactor3 { public: @@ -29,22 +30,22 @@ protected: /** default constructor to allow for serialization */ PendulumFactor1() {} - double dt_; + double h_; // time step 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) {} + ///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 h, double mu = 1000.0) + : 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::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 H1 = boost::none, boost::optional 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_; - return qk1.localCoordinates(qk.compose(LieScalar(v*dt_))); + if (H3) *H3 = eye(p)*h_; + 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 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) + * - For explicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_k) + * - 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 - h*g/L*sin(q_k) */ class PendulumFactor2: public NoiseModelFactor3 { 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 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) {} + ///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 h, double r = 1.0, double g = 9.81, double mu = 1000.0) + : 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::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 H1 = boost::none, boost::optional 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()); - return vk1.localCoordinates(LieScalar(vk - dt_*g_/L_*sin(q))); + if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value()); + 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 { +public: + +protected: + typedef NoiseModelFactor3 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 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::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 H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional 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 { +public: + +protected: + typedef NoiseModelFactor3 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 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::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 H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional 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 + } diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index 2a23f6931..ead92f46c 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/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); } diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index e2ec13c07..4b4d874e7 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/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 //*************************************************************************