SimpleHelicopter.h
Go to the documentation of this file.
1 /*
2  * @file SimpleHelicopter.h
3  * @brief Implement SimpleHelicopter discrete dynamics model and variational integrator,
4  * following [Kobilarov09siggraph]
5  * @author Duy-Nguyen Ta
6  */
7 
8 #pragma once
9 
11 #include <gtsam/geometry/Pose3.h>
13 #include <cmath>
14 
15 namespace gtsam {
16 
27 class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6> {
28 
29  double h_; // time step
31 public:
32 
33  // Provide access to the Matrix& version of evaluateError:
34  using Base::evaluateError;
35 
36  Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
37  Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey,
38  xiKey), h_(h) {
39  }
40  ~Reconstruction() override {}
41 
44  return std::static_pointer_cast<gtsam::NonlinearFactor>(
46 
48  Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
50  OptionalMatrixType H3) const override {
51 
52  Matrix6 D_exphxi_xi;
53  Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0);
54 
55  Matrix6 D_gkxi_gk, D_gkxi_exphxi;
56  Pose3 gkxi = gk.compose(exphxi, D_gkxi_gk, H3 ? &D_gkxi_exphxi : 0);
57 
58  Matrix6 D_hx_gk1, D_hx_gkxi;
59  Pose3 hx = gkxi.between(gk1, (H2 || H3) ? &D_hx_gkxi : 0, H1 ? &D_hx_gk1 : 0);
60 
61  Matrix6 D_log_hx;
62  Vector error = Pose3::Logmap(hx, D_log_hx);
63 
64  if (H1) *H1 = D_log_hx * D_hx_gk1;
65  if (H2 || H3) {
66  Matrix6 D_log_gkxi = D_log_hx * D_hx_gkxi;
67  if (H2) *H2 = D_log_gkxi * D_gkxi_gk;
68  if (H3) *H3 = D_log_gkxi * D_gkxi_exphxi * D_exphxi_xi * h_;
69  }
70 
71  return error;
72  }
73 
74 };
75 
79 class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector6, Pose3> {
80 
81  double h_;
84  double m_;
87 
88  // TODO: Fk_ and f_ext should be generalized as functions (factor nodes) on control signals and poses/velocities.
89  // This might be needed in control or system identification problems.
90  // We treat them as constant here, since the control inputs are to specify.
91 
93 
94 public:
95 
96  // Provide access to the Matrix& version of evaluateError:
97  using Base::evaluateError;
98 
99  DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
100  double h, const Matrix& Inertia, const Vector& Fu, double m,
101  double mu = 1000.0) :
102  Base(noiseModel::Constrained::All(6, std::abs(mu)), xiKey1, xiKey_1, gKey),
103  h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) {
104  }
106 
109  return std::static_pointer_cast<gtsam::NonlinearFactor>(
111 
117  Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
119  OptionalMatrixType H3) const override {
120 
121  Vector muk = Inertia_*xik;
122  Vector muk_1 = Inertia_*xik_1;
123 
124  // Apply the inverse right-trivialized tangent (derivative) map of the exponential map,
125  // using the trapezoidal Lie-Newmark (TLN) scheme, to a vector.
126  // TLN is just a first order approximation of the dExpInv_exp above, detailed in [Kobilarov09siggraph]
127  // C_TLN formula: I6 - 1/2 ad[xi].
128  Matrix D_adjThxik_muk, D_adjThxik1_muk1;
129  Vector pk = muk - 0.5*Pose3::adjointTranspose(h_*xik, muk, D_adjThxik_muk);
130  Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1);
131 
132  Matrix D_gravityBody_gk;
133  Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, {});
134  Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()).finished();
135 
136  Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
137 
138  if (H1) {
139  Matrix D_pik_xi = Inertia_-0.5*(h_*D_adjThxik_muk + Pose3::adjointMap(h_*xik).transpose()*Inertia_);
140  *H1 = D_pik_xi;
141  }
142 
143  if (H2) {
144  Matrix D_pik1_xik1 = Inertia_-0.5*(-h_*D_adjThxik1_muk1 + Pose3::adjointMap(-h_*xik_1).transpose()*Inertia_);
145  *H2 = -D_pik1_xik1;
146  }
147 
148  if (H3) {
149  *H3 = Z_6x6;
150  insertSub(*H3, -h_*D_gravityBody_gk, 3, 0);
151  }
152 
153  return hx;
154  }
155 
156 #if 0
157  Vector computeError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk) const {
158  Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik;
159  Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
160 
161  Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_));
162  Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
163 
164  Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
165 
166  return hx;
167  }
168 
169  Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
171  OptionalMatrixType H3) const {
172  if (H1) {
173  (*H1) = numericalDerivative31(
174  std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
175  std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
176  ),
177  xik, xik_1, gk, 1e-5
178  );
179  }
180  if (H2) {
181  (*H2) = numericalDerivative32(
182  std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
183  std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
184  ),
185  xik, xik_1, gk, 1e-5
186  );
187  }
188  if (H3) {
189  (*H3) = numericalDerivative33(
190  std::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
191  std::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
192  ),
193  xik, xik_1, gk, 1e-5
194  );
195  }
196 
197  return computeError(xik, xik_1, gk);
198  }
199 #endif
200 
201 };
202 
203 } // namespace gtsam
Matrix3f m
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi={})
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose3.cpp:169
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu=1000.0)
static Matrix6 adjointMap(const Vector6 &xi)
Definition: Pose3.cpp:106
gtsam::NonlinearFactor::shared_ptr clone() const override
Matrix Inertia
static Vector6 Logmap(const Pose3 &pose, OptionalJacobian< 6, 6 > Hpose={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3.cpp:188
double mu
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
NoiseModelFactorN< Pose3, Pose3, Vector6 > Base
Definition: BFloat16.h:88
Some functions to compute numerical derivatives.
gtsam::NonlinearFactor::shared_ptr clone() const override
const SharedNoiseModel & noiseModel() const
access to the noise model
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
NoiseModelFactorN< Vector6, Vector6, Pose3 > Base
mass. For gravity external force f_ext, which has a fixed formula in this case.
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:135
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Vector evaluateError(const Pose3 &gk1, const Pose3 &gk, const Vector6 &xik, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Class compose(const Class &g) const
Definition: Lie.h:48
Vector Fu_
Inertia tensors Inertia = [ J 0; 0 M ].
traits
Definition: chartTesting.h:28
const double h
double error(const Values &c) const override
Non-linear factor base classes.
void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
Definition: base/Matrix.h:194
Class between(const Class &g) const
Definition: Lie.h:52
std::shared_ptr< This > shared_ptr
static Vector6 adjointTranspose(const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi={}, OptionalJacobian< 6, 6 > H_y={})
Definition: Pose3.cpp:134
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Vector3 Point3
Definition: Point3.h:38
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, double h, const Matrix &Inertia, const Vector &Fu, double m, double mu=1000.0)
#define abs(x)
Definition: datatypes.h:17
3D Pose
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Vector evaluateError(const Vector6 &xik, const Vector6 &xik_1, const Pose3 &gk, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:43