gtsam  4.1.0
gtsam
Pendulum.h
Go to the documentation of this file.
1 
10 #pragma once
11 
13 
14 namespace gtsam {
15 
16 //*************************************************************************
23 class PendulumFactor1: public NoiseModelFactor3<double, double, double> {
24 public:
25 
26 protected:
28 
31 
32  double h_; // time step
33 
34 public:
35 
36  typedef boost::shared_ptr<PendulumFactor1> shared_ptr;
37 
39  PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
40  : Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {}
41 
43  gtsam::NonlinearFactor::shared_ptr clone() const override {
44  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
45  gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
46 
48  Vector evaluateError(const double& qk1, const double& qk, const double& v,
49  boost::optional<Matrix&> H1 = boost::none,
50  boost::optional<Matrix&> H2 = boost::none,
51  boost::optional<Matrix&> H3 = boost::none) const override {
52  const size_t p = 1;
53  if (H1) *H1 = -Matrix::Identity(p,p);
54  if (H2) *H2 = Matrix::Identity(p,p);
55  if (H3) *H3 = Matrix::Identity(p,p)*h_;
56  return (Vector(1) << qk+v*h_-qk1).finished();
57  }
58 
59 }; // \PendulumFactor1
60 
61 
62 //*************************************************************************
69 class PendulumFactor2: public NoiseModelFactor3<double, double, double> {
70 public:
71 
72 protected:
74 
77 
78  double h_;
79  double g_;
80  double r_;
81 
82 public:
83 
84  typedef boost::shared_ptr<PendulumFactor2 > shared_ptr;
85 
87  PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
88  : Base(noiseModel::Constrained::All(1, std::abs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
89 
91  gtsam::NonlinearFactor::shared_ptr clone() const override {
92  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
93  gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
94 
96  Vector evaluateError(const double & vk1, const double & vk, const double & q,
97  boost::optional<Matrix&> H1 = boost::none,
98  boost::optional<Matrix&> H2 = boost::none,
99  boost::optional<Matrix&> H3 = boost::none) const override {
100  const size_t p = 1;
101  if (H1) *H1 = -Matrix::Identity(p,p);
102  if (H2) *H2 = Matrix::Identity(p,p);
103  if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q);
104  return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished();
105  }
106 
107 }; // \PendulumFactor2
108 
109 
110 //*************************************************************************
116 class PendulumFactorPk: public NoiseModelFactor3<double, double, double> {
117 public:
118 
119 protected:
121 
124 
125  double h_;
126  double m_;
127  double r_;
128  double g_;
129  double alpha_;
130 
131 public:
132 
133  typedef boost::shared_ptr<PendulumFactorPk > shared_ptr;
134 
136  PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
137  double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
138  : Base(noiseModel::Constrained::All(1, std::abs(mu)), pKey, qKey, qKey1),
139  h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
140 
142  gtsam::NonlinearFactor::shared_ptr clone() const override {
143  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
144  gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
145 
147  Vector evaluateError(const double & pk, const double & qk, const double & qk1,
148  boost::optional<Matrix&> H1 = boost::none,
149  boost::optional<Matrix&> H2 = boost::none,
150  boost::optional<Matrix&> H3 = boost::none) const override {
151  const size_t p = 1;
152 
153  double qmid = (1-alpha_)*qk + alpha_*qk1;
154  double mr2_h = 1/h_*m_*r_*r_;
155  double mgrh = m_*g_*r_*h_;
156 
157  if (H1) *H1 = -Matrix::Identity(p,p);
158  if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
159  if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
160 
161  return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished();
162  }
163 
164 }; // \PendulumFactorPk
165 
166 //*************************************************************************
172 class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> {
173 public:
174 
175 protected:
177 
180 
181  double h_;
182  double m_;
183  double r_;
184  double g_;
185  double alpha_;
186 
187 public:
188 
189  typedef boost::shared_ptr<PendulumFactorPk1 > shared_ptr;
190 
192  PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
193  double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
194  : Base(noiseModel::Constrained::All(1, std::abs(mu)), pKey1, qKey, qKey1),
195  h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
196 
198  gtsam::NonlinearFactor::shared_ptr clone() const override {
199  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
200  gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
201 
203  Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
204  boost::optional<Matrix&> H1 = boost::none,
205  boost::optional<Matrix&> H2 = boost::none,
206  boost::optional<Matrix&> H3 = boost::none) const override {
207  const size_t p = 1;
208 
209  double qmid = (1-alpha_)*qk + alpha_*qk1;
210  double mr2_h = 1/h_*m_*r_*r_;
211  double mgrh = m_*g_*r_*h_;
212 
213  if (H1) *H1 = -Matrix::Identity(p,p);
214  if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
215  if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
216 
217  return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished();
218  }
219 
220 }; // \PendulumFactorPk1
221 
222 }
gtsam::PendulumFactorPk::shared_ptr
boost::shared_ptr< PendulumFactorPk > shared_ptr
in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can ...
Definition: Pendulum.h:133
gtsam::PendulumFactor1::evaluateError
Vector evaluateError(const double &qk1, const double &qk, const double &v, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
q_k + h*v - q_k1 = 0, with optional derivatives
Definition: Pendulum.h:48
gtsam::PendulumFactorPk1::m_
double m_
time step
Definition: Pendulum.h:182
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
gtsam::PendulumFactor2
This class implements the second constraint the.
Definition: Pendulum.h:69
gtsam::PendulumFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: Pendulum.h:91
gtsam::PendulumFactorPk::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: Pendulum.h:142
gtsam::PendulumFactorPk1::PendulumFactorPk1
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)
Constructor.
Definition: Pendulum.h:192
gtsam::PendulumFactor1::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: Pendulum.h:43
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::PendulumFactorPk1::r_
double r_
mass
Definition: Pendulum.h:183
gtsam::PendulumFactor2::PendulumFactor2
PendulumFactor2()
default constructor to allow for serialization
Definition: Pendulum.h:76
gtsam::PendulumFactorPk1::shared_ptr
boost::shared_ptr< PendulumFactorPk1 > shared_ptr
in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can ...
Definition: Pendulum.h:189
gtsam::PendulumFactorPk::PendulumFactorPk
PendulumFactorPk()
default constructor to allow for serialization
Definition: Pendulum.h:123
gtsam::PendulumFactorPk1::evaluateError
Vector evaluateError(const double &pk1, const double &qk, const double &qk1, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives
Definition: Pendulum.h:203
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::PendulumFactorPk1
This class implements the second position-momentum update rule p_k1 = D_2 L_d(q_k,...
Definition: Pendulum.h:172
gtsam::PendulumFactor1
This class implements the first constraint.
Definition: Pendulum.h:23
gtsam::PendulumFactor2::evaluateError
Vector evaluateError(const double &vk1, const double &vk, const double &q, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives
Definition: Pendulum.h:96
gtsam::PendulumFactorPk1::alpha_
double alpha_
gravity
Definition: Pendulum.h:185
gtsam::PendulumFactorPk::m_
double m_
time step
Definition: Pendulum.h:126
gtsam::PendulumFactorPk::evaluateError
Vector evaluateError(const double &pk, const double &qk, const double &qk1, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives
Definition: Pendulum.h:147
gtsam::PendulumFactor2::PendulumFactor2
PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r=1.0, double g=9.81, double mu=1000.0)
Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step.
Definition: Pendulum.h:87
gtsam::PendulumFactorPk::g_
double g_
length
Definition: Pendulum.h:128
NonlinearFactor.h
Non-linear factor base classes.
gtsam::PendulumFactorPk
This class implements the first position-momentum update rule p_k = -D_1 L_d(q_k,q_{k+1},...
Definition: Pendulum.h:116
gtsam::PendulumFactorPk::r_
double r_
mass
Definition: Pendulum.h:127
gtsam::PendulumFactorPk::PendulumFactorPk
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)
Constructor.
Definition: Pendulum.h:136
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::PendulumFactorPk1::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: Pendulum.h:198
gtsam::PendulumFactorPk1::PendulumFactorPk1
PendulumFactorPk1()
default constructor to allow for serialization
Definition: Pendulum.h:179
gtsam::PendulumFactorPk::alpha_
double alpha_
gravity
Definition: Pendulum.h:129
gtsam::PendulumFactor1::PendulumFactor1
PendulumFactor1()
default constructor to allow for serialization
Definition: Pendulum.h:30
gtsam::PendulumFactorPk1::g_
double g_
length
Definition: Pendulum.h:184
gtsam::PendulumFactor1::PendulumFactor1
PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu=1000.0)
Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method,...
Definition: Pendulum.h:39