1/**
2 * @file Pendulum.h
3 * @brief Three-way factors for the pendulum dynamics as in [Stern06siggraph] for
4 * (1) explicit Euler method, (2) implicit Euler method, and (3) sympletic Euler method.
5 * Note that all methods use the same formulas for the factors. They are only different in
6 * the way we connect variables using those factors in the graph.
7 * @author Duy-Nguyen Ta
8 */
9
10#pragma once
11
12#include <gtsam/nonlinear/NonlinearFactor.h>
13
14namespace gtsam {
15
16//*************************************************************************
17/**
18 * This class implements the first constraint.
19 * - For explicit Euler method: q_{k+1} = q_k + h*v_k
20 * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1}
21 * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
22 */
23class PendulumFactor1: public NoiseModelFactorN<double, double, double> {
24public:
25
26protected:
27 typedef NoiseModelFactorN<double, double, double> Base;
28
29 /** default constructor to allow for serialization */
30 PendulumFactor1() {}
31
32 double h_; // time step
33
34public:
35
36 // Provide access to the Matrix& version of evaluateError:
37 using Base::evaluateError;
38
39 typedef std::shared_ptr<PendulumFactor1> shared_ptr;
40
41 ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
42 PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
43 : Base(noiseModel::Constrained::All(dim: 1, mu: std::abs(x: mu)), k1, k, velKey), h_(h) {}
44
45 /// @return a deep copy of this factor
46 gtsam::NonlinearFactor::shared_ptr clone() const override {
47 return std::static_pointer_cast<gtsam::NonlinearFactor>(
48 r: gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
49
50 /** q_k + h*v - q_k1 = 0, with optional derivatives */
51 Vector evaluateError(const double& qk1, const double& qk, const double& v,
52 OptionalMatrixType H1, OptionalMatrixType H2,
53 OptionalMatrixType H3) const override {
54 const size_t p = 1;
55 if (H1) *H1 = -Matrix::Identity(rows: p,cols: p);
56 if (H2) *H2 = Matrix::Identity(rows: p,cols: p);
57 if (H3) *H3 = Matrix::Identity(rows: p,cols: p)*h_;
58 return (Vector(1) << qk+v*h_-qk1).finished();
59 }
60
61}; // \PendulumFactor1
62
63
64//*************************************************************************
65/**
66 * This class implements the second constraint the
67 * - For explicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
68 * - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1})
69 * - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
70 */
71class PendulumFactor2: public NoiseModelFactorN<double, double, double> {
72public:
73
74protected:
75 typedef NoiseModelFactorN<double, double, double> Base;
76
77 /** default constructor to allow for serialization */
78 PendulumFactor2() {}
79
80 double h_;
81 double g_;
82 double r_;
83
84public:
85
86 // Provide access to the Matrix& version of evaluateError:
87 using Base::evaluateError;
88
89 typedef std::shared_ptr<PendulumFactor2 > shared_ptr;
90
91 ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
92 PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
93 : Base(noiseModel::Constrained::All(dim: 1, mu: std::abs(x: mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
94
95 /// @return a deep copy of this factor
96 gtsam::NonlinearFactor::shared_ptr clone() const override {
97 return std::static_pointer_cast<gtsam::NonlinearFactor>(
98 r: gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
99
100 /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
101 Vector evaluateError(const double & vk1, const double & vk, const double & q,
102 OptionalMatrixType H1, OptionalMatrixType H2,
103 OptionalMatrixType H3) const override {
104 const size_t p = 1;
105 if (H1) *H1 = -Matrix::Identity(rows: p,cols: p);
106 if (H2) *H2 = Matrix::Identity(rows: p,cols: p);
107 if (H3) *H3 = -Matrix::Identity(rows: p,cols: p)*h_*g_/r_*cos(x: q);
108 return (Vector(1) << vk - h_ * g_ / r_ * sin(x: q) - vk1).finished();
109 }
110
111}; // \PendulumFactor2
112
113
114//*************************************************************************
115/**
116 * This class implements the first position-momentum update rule
117 * \f$ 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) \f$
118 * \f$ = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) \f$
119 */
120class PendulumFactorPk: public NoiseModelFactorN<double, double, double> {
121public:
122
123protected:
124 typedef NoiseModelFactorN<double, double, double> Base;
125
126 /** default constructor to allow for serialization */
127 PendulumFactorPk() {}
128
129 double h_; //! time step
130 double m_; //! mass
131 double r_; //! length
132 double g_; //! gravity
133 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.
134
135public:
136
137 // Provide access to the Matrix& version of evaluateError:
138 using Base::evaluateError;
139
140 typedef std::shared_ptr<PendulumFactorPk > shared_ptr;
141
142 ///Constructor
143 PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
144 double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
145 : Base(noiseModel::Constrained::All(dim: 1, mu: std::abs(x: mu)), pKey, qKey, qKey1),
146 h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
147
148 /// @return a deep copy of this factor
149 gtsam::NonlinearFactor::shared_ptr clone() const override {
150 return std::static_pointer_cast<gtsam::NonlinearFactor>(
151 r: gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
152
153 /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
154 Vector evaluateError(const double & pk, const double & qk, const double & qk1,
155 OptionalMatrixType H1, OptionalMatrixType H2,
156 OptionalMatrixType H3) const override {
157 const size_t p = 1;
158
159 double qmid = (1-alpha_)*qk + alpha_*qk1;
160 double mr2_h = 1/h_*m_*r_*r_;
161 double mgrh = m_*g_*r_*h_;
162
163 if (H1) *H1 = -Matrix::Identity(rows: p,cols: p);
164 if (H2) *H2 = Matrix::Identity(rows: p,cols: p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(x: qmid));
165 if (H3) *H3 = Matrix::Identity(rows: p,cols: p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(x: qmid));
166
167 return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(x: qmid) - pk).finished();
168 }
169
170}; // \PendulumFactorPk
171
172//*************************************************************************
173/**
174 * This class implements the second position-momentum update rule
175 * \f$ 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) \f$
176 * \f$ = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) \f$
177 */
178class PendulumFactorPk1: public NoiseModelFactorN<double, double, double> {
179public:
180
181protected:
182 typedef NoiseModelFactorN<double, double, double> Base;
183
184 /** default constructor to allow for serialization */
185 PendulumFactorPk1() {}
186
187 double h_; //! time step
188 double m_; //! mass
189 double r_; //! length
190 double g_; //! gravity
191 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.
192
193public:
194
195 // Provide access to the Matrix& version of evaluateError:
196 using Base::evaluateError;
197
198 typedef std::shared_ptr<PendulumFactorPk1 > shared_ptr;
199
200 ///Constructor
201 PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
202 double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
203 : Base(noiseModel::Constrained::All(dim: 1, mu: std::abs(x: mu)), pKey1, qKey, qKey1),
204 h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
205
206 /// @return a deep copy of this factor
207 gtsam::NonlinearFactor::shared_ptr clone() const override {
208 return std::static_pointer_cast<gtsam::NonlinearFactor>(
209 r: gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
210
211 /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
212 Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
213 OptionalMatrixType H1, OptionalMatrixType H2,
214 OptionalMatrixType H3) const override {
215 const size_t p = 1;
216
217 double qmid = (1-alpha_)*qk + alpha_*qk1;
218 double mr2_h = 1/h_*m_*r_*r_;
219 double mgrh = m_*g_*r_*h_;
220
221 if (H1) *H1 = -Matrix::Identity(rows: p,cols: p);
222 if (H2) *H2 = Matrix::Identity(rows: p,cols: p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(x: qmid));
223 if (H3) *H3 = Matrix::Identity(rows: p,cols: p)*( mr2_h - mgrh*alpha_*alpha_*cos(x: qmid));
224
225 return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(x: qmid) - pk1).finished();
226 }
227
228}; // \PendulumFactorPk1
229
230}
231