Proof-of-concept function auto-diff and inversion

CMakeLists.txt

@@ -24,6 +24,7 @@ if (BUILD_TESTS)
         tests/SignalTest.cpp
         tests/IntegrationTest.cpp
         tests/DynamicsTest.cpp
+        tests/FunctionTest.cpp
     )
     target_link_libraries(${UNIT_TEST}
         ${PROJ_NAME}

include/signals/Function.h

@@ -0,0 +1,29 @@
+#pragma once
+#include "signals/Signal.h"
+
+using namespace Eigen;
+
+template<typename FuncSpec>
+struct Function
+{
+    using InputType  = typename FuncSpec::InputSignalType::BaseType;
+    using OutputType = typename FuncSpec::OutputSignalType::BaseType;
+
+    OutputType operator()(const InputType& input) const
+    {
+        return FuncSpec::Function(input);
+    }
+
+    MatrixXd J(const InputType& input) const
+    {
+        MatrixXd jacobian;
+        return jacobian; // TODO implement; need to take chart map into account
+        // TODO maybe move this out into its own vector/scalar-only function
+    }
+
+    InputType inputCorrespondingTo(const OutputType& output) const
+    {
+        InputType input;
+        return input; // TODO implement
+    }
+};

include/signals/Signal.h

@@ -75,6 +75,11 @@ class Signal
         this->needsSort_          = other.needsSort_;
     }
 
+    static size_t dimension()
+    {
+        return TangentSignalSpec::Dimension();
+    }
+
     Signal<TangentSignalSpec, TangentSignalSpec> dotSignal()
     {
         Signal<TangentSignalSpec, TangentSignalSpec> signalDot;
@@ -623,6 +628,10 @@ struct ScalarSignalSpec
     {
         return (T)1. / 0.;
     }
+    static size_t Dimension()
+    {
+        return 1;
+    }
 };
 
 template<typename T, size_t d>
@@ -637,6 +646,10 @@ struct VectorSignalSpec
     {
         return Type::Zero(); // TODO fix
     }
+    static size_t Dimension()
+    {
+        return d;
+    }
 };
 
 template<typename ManifoldType>
@@ -651,6 +664,10 @@ struct ManifoldSignalSpec
     {
         return Type::identity(); // TODO fix
     }
+    static size_t Dimension()
+    {
+        return Type::Log(Type::Identity()).rows();
+    }
 };
 
 template<typename T>

include/signals/Signals.h

@@ -4,3 +4,4 @@
 #include "signals/Integration.h"
 #include "signals/State.h"
 #include "signals/Models.h"
+#include "signals/Function.h"

tests/FunctionTest.cpp

@@ -0,0 +1,61 @@
+#include <boost/test/unit_test.hpp>
+#include <Eigen/Core>
+#include "signals/Function.h"
+
+using namespace Eigen;
+
+BOOST_AUTO_TEST_SUITE(TestFunction) // ^^^^ TODO
+
+BOOST_AUTO_TEST_CASE(TestEulerIntegrator)
+{
+    const double dt = 0.001;
+    double       t  = 0.;
+    double       tf = 5. * M_PI / 4.;
+
+    ScalardSignal v_ref, v_int;
+
+    while (t <= tf)
+    {
+        BOOST_CHECK(v_ref.update(t, sin(t), cos(t), true));
+        t += dt;
+    }
+
+    ScalardSignal v_ref_dot = v_ref.dotSignal();
+
+    BOOST_CHECK(v_int.update(0., 0.));
+
+    BOOST_CHECK(EulerIntegrator::integrate(v_int, v_ref_dot, t, dt, true));
+
+    BOOST_CHECK_CLOSE(v_int(0.), v_ref(0.), 1.);
+    BOOST_CHECK_CLOSE(v_int(M_PI / 4.), v_ref(M_PI / 4.), 1.);
+    BOOST_CHECK_CLOSE(v_int(M_PI / 2.), v_ref(M_PI / 2.), 1.);
+    BOOST_CHECK_CLOSE(v_int(3. * M_PI / 4.), v_ref(3. * M_PI / 4.), 1.);
+}
+
+BOOST_AUTO_TEST_CASE(TestTrapezoidalIntegrator)
+{
+    const double dt = 0.001;
+    double       t  = 0.;
+    double       tf = 5. * M_PI / 4.;
+
+    ScalardSignal v_ref, v_int;
+
+    while (t <= tf)
+    {
+        BOOST_CHECK(v_ref.update(t, sin(t), cos(t), true));
+        t += dt;
+    }
+
+    ScalardSignal v_ref_dot = v_ref.dotSignal();
+
+    BOOST_CHECK(v_int.update(0., 0.));
+
+    BOOST_CHECK(TrapezoidalIntegrator::integrate(v_int, v_ref_dot, t, dt, true));
+
+    BOOST_CHECK_CLOSE(v_int(0.), v_ref(0.), 1.);
+    BOOST_CHECK_CLOSE(v_int(M_PI / 4.), v_ref(M_PI / 4.), 1.);
+    BOOST_CHECK_CLOSE(v_int(M_PI / 2.), v_ref(M_PI / 2.), 1.);
+    BOOST_CHECK_CLOSE(v_int(3. * M_PI / 4.), v_ref(3. * M_PI / 4.), 1.);
+}
+
+BOOST_AUTO_TEST_SUITE_END()