1/**
2 * @file testPendulumExplicitEuler.cpp
3 * @author Duy-Nguyen Ta
4 */
5
6#include <CppUnitLite/TestHarness.h>
7#include <gtsam/base/numericalDerivative.h>
8#include <gtsam/inference/Symbol.h>
9#include <gtsam_unstable/dynamics/SimpleHelicopter.h>
10#include "gtsam/base/Vector.h"
11#include "gtsam/geometry/Pose3.h"
12
13/* ************************************************************************* */
14using namespace std::placeholders;
15using namespace gtsam;
16using namespace gtsam::symbol_shorthand;
17
18const double tol=1e-5;
19const double h = 0.01;
20
21//const double deg2rad = M_PI/180.0;
22//Pose3 g1(Rot3::Ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
23Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0));
24//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished());
25Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished());
26Vector6 V1_g1 = g1.inverse().Adjoint(xi_b: V1_w);
27Pose3 g2(g1.expmap(v: h*V1_g1));
28//Vector6 v2 = Pose3::Logmap(g1.between(g2));
29
30double mass = 100.0;
31Vector gamma2 = Vector2(0.0, 0.0); // no shape
32Vector u2 = Vector2(0.0, 0.0); // no control at time 2
33double distT = 1.0; // distance from the body-centered x axis to the big top motor
34double distR = 5.0; // distance from the body-centered z axis to the small motor
35Matrix Mass = ((Vector(3) << mass, mass, mass).finished()).asDiagonal();
36Matrix Inertia = (Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished().asDiagonal();
37
38Vector computeFu(const Vector& gamma, const Vector& control) {
39 double gamma_r = gamma(0), gamma_p = gamma(1);
40
41 Matrix F = (Matrix(6, 2) << distT*sin(x: gamma_r), 0.0,
42 distT*sin(x: gamma_p*cos(x: gamma_r)), 0.0,
43 0.0, distR,
44 sin(x: gamma_p)*cos(x: gamma_r), 0.0,
45 -sin(x: gamma_r), -1.0,
46 cos(x: gamma_p)*sin(x: gamma_r), 0.0
47 ).finished();
48 return F*control;
49}
50
51/* ************************************************************************* */
52TEST( Reconstruction, evaluateError) {
53 // hard constraints don't need a noise model
54 Reconstruction constraint(G(j: 2), G(j: 1), V(j: 1), h);
55
56 // verify error function
57 Matrix H1, H2, H3;
58 EXPECT(
59 assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol));
60
61 std::function<Vector(const Pose3&, const Pose3&, const Vector6&)> f =
62 [&constraint](const Pose3& a1, const Pose3& a2, const Vector6& a3) {
63 return constraint.evaluateError(x: a1, x: a2, x: a3);
64 };
65
66 Matrix numericalH1 = numericalDerivative31(h: f, x1: g2, x2: g1, x3: V1_g1, delta: 1e-5);
67
68 Matrix numericalH2 = numericalDerivative32(h: f, x1: g2, x2: g1, x3: V1_g1, delta: 1e-5);
69
70 Matrix numericalH3 = numericalDerivative33(h: f, x1: g2, x2: g1, x3: V1_g1, delta: 1e-5);
71
72 EXPECT(assert_equal(numericalH1,H1,1e-5));
73 EXPECT(assert_equal(numericalH2,H2,1e-5));
74#ifdef GTSAM_USE_QUATERNIONS // TODO: why is the quaternion version much less accurate??
75 EXPECT(assert_equal(numericalH3,H3,1e-3));
76#else
77 EXPECT(assert_equal(numericalH3,H3,1e-3));
78#endif
79}
80
81/* ************************************************************************* */
82// Implement Newton-Euler equation for rigid body dynamics
83Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
84 Matrix W = Pose3::adjointMap(xi: (Vector(6) << Vb(0), Vb(1), Vb(2), 0., 0., 0.).finished());
85 Vector dV = Inertia.inverse()*(Fb - W*Inertia*Vb);
86 return dV;
87}
88
89TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
90 Vector Fu = computeFu(gamma: gamma2, control: u2);
91 Vector fGravity_g1 = Z_6x1;
92 fGravity_g1.segment<3>(start: 3) = g1.rotation().unrotate(p: Vector3(0, 0, -mass*9.81)); // gravity force in g1 frame
93 Vector Fb = Fu+fGravity_g1;
94
95 Vector dV = newtonEuler(Vb: V1_g1, Fb, Inertia);
96 Vector V2_g1 = dV*h + V1_g1;
97 Pose3 g21 = g2.between(g: g1);
98 Vector V2_g2 = g21.Adjoint(xi_b: V2_g1); // convert the new velocity to g2's frame
99
100 Vector6 expectedv2(V2_g2);
101
102 // hard constraints don't need a noise model
103 DiscreteEulerPoincareHelicopter constraint(V(j: 2), V(j: 1), G(j: 2), h,
104 Inertia, Fu, mass);
105
106 // verify error function
107 Matrix H1, H2, H3;
108 EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
109
110 std::function<Vector(const Vector6&, const Vector6&, const Pose3&)> f =
111 [&constraint](const Vector6& a1, const Vector6& a2, const Pose3& a3) {
112 return constraint.evaluateError(x: a1, x: a2, x: a3);
113 };
114
115 Matrix numericalH1 = numericalDerivative31(h: f, x1: expectedv2, x2: V1_g1, x3: g2, delta: 1e-5);
116
117 Matrix numericalH2 = numericalDerivative32(h: f, x1: expectedv2, x2: V1_g1, x3: g2, delta: 1e-5);
118
119 Matrix numericalH3 = numericalDerivative33(h: f, x1: expectedv2, x2: V1_g1, x3: g2, delta: 1e-5);
120
121 EXPECT(assert_equal(numericalH1,H1,1e-5));
122 EXPECT(assert_equal(numericalH2,H2,1e-5));
123 EXPECT(assert_equal(numericalH3,H3,5e-5));
124}
125
126/* ************************************************************************* */
127int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
128/* ************************************************************************* */
129