1/**
2 * @file testPendulumExplicitEuler.cpp
3 * @author Duy-Nguyen Ta
4 */
5
6#include <CppUnitLite/TestHarness.h>
7#include <gtsam/inference/Symbol.h>
8#include <gtsam_unstable/dynamics/Pendulum.h>
9
10/* ************************************************************************* */
11using namespace gtsam;
12using namespace gtsam::symbol_shorthand;
13
14namespace {
15
16 const double tol=1e-5;
17 const double h = 0.1;
18 const double g = 9.81, l = 1.0;
19
20 const double deg2rad = M_PI/180.0;
21 double q1(deg2rad*30.0), q2(deg2rad*31.0);
22 double v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(x: q1)));
23
24}
25
26/* ************************************************************************* */
27TEST( testPendulumFactor1, evaluateError) {
28 // hard constraints don't need a noise model
29 PendulumFactor1 constraint(Q(j: 2), Q(j: 1), V(j: 1), h);
30
31 // verify error function
32 EXPECT(assert_equal(Z_1x1, constraint.evaluateError(q2, q1, v1), tol));
33}
34
35/* ************************************************************************* */
36TEST( testPendulumFactor2, evaluateError) {
37 // hard constraints don't need a noise model
38 PendulumFactor2 constraint(V(j: 2), V(j: 1), Q(j: 1), h);
39
40 // verify error function
41 EXPECT(assert_equal(Z_1x1, constraint.evaluateError(v2, v1, q1), tol));
42}
43
44/* ************************************************************************* */
45TEST( testPendulumFactorPk, evaluateError) {
46 // hard constraints don't need a noise model
47 PendulumFactorPk constraint(P(j: 1), Q(j: 1), Q(j: 2), h);
48
49 double pk( 1/h * (q2-q1) + h*g*sin(x: q1) );
50
51 // verify error function
52 EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk, q1, q2), tol));
53}
54
55/* ************************************************************************* */
56TEST( testPendulumFactorPk1, evaluateError) {
57 // hard constraints don't need a noise model
58 PendulumFactorPk1 constraint(P(j: 2), Q(j: 1), Q(j: 2), h);
59
60 double pk1( 1/h * (q2-q1) );
61
62 // verify error function
63 EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk1, q1, q2), tol));
64}
65
66
67/* ************************************************************************* */
68int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
69/* ************************************************************************* */
70