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 
44  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
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 
92  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
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 
143  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
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 
199  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
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 }
boost::shared_ptr< PendulumFactor1 > shared_ptr
Definition: Pendulum.h:36
double m_
time step
Definition: Pendulum.h:126
Matrix3f m
double alpha_
gravity
Definition: Pendulum.h:185
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
Definition: Pendulum.h:147
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
Definition: Pendulum.h:96
double alpha_
gravity
Definition: Pendulum.h:129
double mu
ArrayXcf v
Definition: Cwise_arg.cpp:1
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
Definition: Pendulum.h:203
double g_
length
Definition: Pendulum.h:184
NoiseModelFactor3< double, double, double > Base
Definition: Pendulum.h:120
Definition: Half.h:150
double g_
length
Definition: Pendulum.h:128
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
Definition: Pendulum.h:48
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: Pendulum.h:142
const SharedNoiseModel & noiseModel() const
access to the noise model
void g(const string &key, int i)
Definition: testBTree.cpp:43
EIGEN_DEVICE_FUNC const CosReturnType cos() const
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
double m_
time step
Definition: Pendulum.h:182
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
boost::shared_ptr< PendulumFactor2 > shared_ptr
Definition: Pendulum.h:84
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&#39;s key depending on the chosen method, h: time step...
Definition: Pendulum.h:87
Eigen::VectorXd Vector
Definition: Vector.h:38
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
boost::shared_ptr< This > 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
RealScalar alpha
EIGEN_DEVICE_FUNC const Scalar & q
traits
Definition: chartTesting.h:28
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: Pendulum.h:198
const double h
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
Non-linear factor base classes.
float * p
EIGEN_DEVICE_FUNC const SinReturnType sin() const
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: Pendulum.h:91
#define abs(x)
Definition: datatypes.h:17
NoiseModelFactor3< double, double, double > Base
Definition: Pendulum.h:176
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
NoiseModelFactor3< double, double, double > Base
Definition: Pendulum.h:27
NoiseModelFactor3< double, double, double > Base
Definition: Pendulum.h:73
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: Pendulum.h:43


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:25