gtsam
Loading...
Searching...
No Matches
Pendulum.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
20
21#pragma once
22
23#include <gtsam/config.h>
24
25#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
26
27#include <gtsam/nonlinear/NonlinearFactor.h>
28#include <gtsam/nonlinear/NoiseModelFactorN.h>
29
30namespace gtsam {
31
32//*************************************************************************
40class PendulumFactor1
41 : public NoiseModelFactorT<Vector1, double, double, double> {
42public:
43
44protected:
45 typedef NoiseModelFactorT<Vector1, double, double, double> Base;
46
48 PendulumFactor1() {}
49
50 double h_; // time step
51
52public:
53
54 // Provide access to the Matrix& version of evaluateError:
55 using Base::evaluateError;
56
57 typedef std::shared_ptr<PendulumFactor1> shared_ptr;
58
60 PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
61 : Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {}
62
64 gtsam::NonlinearFactor::shared_ptr clone() const override {
65 return std::static_pointer_cast<gtsam::NonlinearFactor>(
66 gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
67
69 Vector1 evaluateError(const double& qk1, const double& qk, const double& v,
70 OptionalMatrixType H1, OptionalMatrixType H2,
71 OptionalMatrixType H3) const override {
72 const size_t p = 1;
73 if (H1) *H1 = -Matrix::Identity(p,p);
74 if (H2) *H2 = Matrix::Identity(p,p);
75 if (H3) *H3 = Matrix::Identity(p,p)*h_;
76 return Vector{{qk + v * h_ - qk1}};
77 }
78
79}; // \PendulumFactor1
80
81
82//*************************************************************************
90class PendulumFactor2
91 : public NoiseModelFactorT<Vector1, double, double, double> {
92public:
93
94protected:
95 typedef NoiseModelFactorT<Vector1, double, double, double> Base;
96
98 PendulumFactor2() {}
99
100 double h_;
101 double g_;
102 double r_;
103
104public:
105
106 // Provide access to the Matrix& version of evaluateError:
107 using Base::evaluateError;
108
109 typedef std::shared_ptr<PendulumFactor2 > shared_ptr;
110
112 PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
113 : Base(noiseModel::Constrained::All(1, std::abs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
114
116 gtsam::NonlinearFactor::shared_ptr clone() const override {
117 return std::static_pointer_cast<gtsam::NonlinearFactor>(
118 gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
119
121 Vector1 evaluateError(const double & vk1, const double & vk, const double & q,
122 OptionalMatrixType H1, OptionalMatrixType H2,
123 OptionalMatrixType H3) const override {
124 const size_t p = 1;
125 if (H1) *H1 = -Matrix::Identity(p,p);
126 if (H2) *H2 = Matrix::Identity(p,p);
127 if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q);
128 return Vector{{vk - h_ * g_ / r_ * sin(q) - vk1}};
129 }
130
131}; // \PendulumFactor2
132
133
134//*************************************************************************
141class PendulumFactorPk
142 : public NoiseModelFactorT<Vector1, double, double, double> {
143public:
144
145protected:
146 typedef NoiseModelFactorT<Vector1, double, double, double> Base;
147
149 PendulumFactorPk() {}
150
151 double h_;
152 double m_;
153 double r_;
154 double g_;
155 double alpha_;
156
157public:
158
159 // Provide access to the Matrix& version of evaluateError:
160 using Base::evaluateError;
161
162 typedef std::shared_ptr<PendulumFactorPk > shared_ptr;
163
165 PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
166 double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
167 : Base(noiseModel::Constrained::All(1, std::abs(mu)), pKey, qKey, qKey1),
168 h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
169
171 gtsam::NonlinearFactor::shared_ptr clone() const override {
172 return std::static_pointer_cast<gtsam::NonlinearFactor>(
173 gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
174
176 Vector1 evaluateError(const double & pk, const double & qk,
177 const double & qk1, OptionalMatrixType H1,
178 OptionalMatrixType H2,
179 OptionalMatrixType H3) const override {
180 const size_t p = 1;
181
182 double qmid = (1-alpha_)*qk + alpha_*qk1;
183 double mr2_h = 1/h_*m_*r_*r_;
184 double mgrh = m_*g_*r_*h_;
185
186 if (H1) *H1 = -Matrix::Identity(p,p);
187 if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
188 if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
189
190 return Vector{{mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk}};
191 }
192
193}; // \PendulumFactorPk
194
195//*************************************************************************
202class PendulumFactorPk1
203 : public NoiseModelFactorT<Vector1, double, double, double> {
204public:
205
206protected:
207 typedef NoiseModelFactorT<Vector1, double, double, double> Base;
208
210 PendulumFactorPk1() {}
211
212 double h_;
213 double m_;
214 double r_;
215 double g_;
216 double alpha_;
217
218public:
219
220 // Provide access to the Matrix& version of evaluateError:
221 using Base::evaluateError;
222
223 typedef std::shared_ptr<PendulumFactorPk1 > shared_ptr;
224
226 PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
227 double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
228 : Base(noiseModel::Constrained::All(1, std::abs(mu)), pKey1, qKey, qKey1),
229 h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
230
232 gtsam::NonlinearFactor::shared_ptr clone() const override {
233 return std::static_pointer_cast<gtsam::NonlinearFactor>(
234 gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
235
237 Vector1 evaluateError(const double & pk1, const double & qk,
238 const double & qk1, OptionalMatrixType H1,
239 OptionalMatrixType H2,
240 OptionalMatrixType H3) const override {
241 const size_t p = 1;
242
243 double qmid = (1-alpha_)*qk + alpha_*qk1;
244 double mr2_h = 1/h_*m_*r_*r_;
245 double mgrh = m_*g_*r_*h_;
246
247 if (H1) *H1 = -Matrix::Identity(p,p);
248 if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
249 if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
250
251 return Vector{{mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1}};
252 }
253
254}; // \PendulumFactorPk1
255
256}
257
258#endif // GTSAM_ALLOW_DEPRECATED_SINCE_V43
NoiseModelFactorN.h
Base class for noise model factors with N variables.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::NoiseModelFactorT
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NoiseModelFactorN.h:155