diff --git a/CMakeLists.txt b/CMakeLists.txt index fb4d27b..8efca34 100644 --- a/CMakeLists.txt +++ b/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} diff --git a/include/signals/Function.h b/include/signals/Function.h new file mode 100644 index 0000000..9638378 --- /dev/null +++ b/include/signals/Function.h @@ -0,0 +1,29 @@ +#pragma once +#include "signals/Signal.h" + +using namespace Eigen; + +template +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 + } +}; diff --git a/include/signals/Signal.h b/include/signals/Signal.h index 292b0ea..f73519b 100644 --- a/include/signals/Signal.h +++ b/include/signals/Signal.h @@ -75,6 +75,11 @@ class Signal this->needsSort_ = other.needsSort_; } + static size_t dimension() + { + return TangentSignalSpec::Dimension(); + } + Signal dotSignal() { Signal signalDot; @@ -623,6 +628,10 @@ struct ScalarSignalSpec { return (T)1. / 0.; } + static size_t Dimension() + { + return 1; + } }; template @@ -637,6 +646,10 @@ struct VectorSignalSpec { return Type::Zero(); // TODO fix } + static size_t Dimension() + { + return d; + } }; template @@ -651,6 +664,10 @@ struct ManifoldSignalSpec { return Type::identity(); // TODO fix } + static size_t Dimension() + { + return Type::Log(Type::Identity()).rows(); + } }; template diff --git a/include/signals/Signals.h b/include/signals/Signals.h index 2f2f459..dea3103 100644 --- a/include/signals/Signals.h +++ b/include/signals/Signals.h @@ -4,3 +4,4 @@ #include "signals/Integration.h" #include "signals/State.h" #include "signals/Models.h" +#include "signals/Function.h" diff --git a/tests/FunctionTest.cpp b/tests/FunctionTest.cpp new file mode 100644 index 0000000..3960af9 --- /dev/null +++ b/tests/FunctionTest.cpp @@ -0,0 +1,61 @@ +#include +#include +#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()