From 32a80c5789d8a8bf434781e3b8566ccee62fc491 Mon Sep 17 00:00:00 2001 From: Andrew Torgesen Date: Wed, 24 Dec 2025 18:34:12 -0800 Subject: [PATCH 1/6] Expose zeroized objects from signal class --- include/signals/Signal.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/include/signals/Signal.h b/include/signals/Signal.h index 292b0ea..31b91fe 100644 --- a/include/signals/Signal.h +++ b/include/signals/Signal.h @@ -255,6 +255,16 @@ class Signal return true; } + BaseType baseZero() + { + return BaseSignalSpec::ZeroType(); + } + + TangentType tangentZero() + { + return TangentSignalSpec::ZeroType(); + } + private: double t_; BaseType x_; From 07fe68734b219e5a00f8f54590ee2861f54bb7e5 Mon Sep 17 00:00:00 2001 From: Andrew Torgesen Date: Wed, 24 Dec 2025 18:41:40 -0800 Subject: [PATCH 2/6] make static --- include/signals/Signal.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/signals/Signal.h b/include/signals/Signal.h index 31b91fe..ccbdb36 100644 --- a/include/signals/Signal.h +++ b/include/signals/Signal.h @@ -255,12 +255,12 @@ class Signal return true; } - BaseType baseZero() + static BaseType baseZero() { return BaseSignalSpec::ZeroType(); } - TangentType tangentZero() + static TangentType tangentZero() { return TangentSignalSpec::ZeroType(); } From c0b581b790713c945f6848490e1aa384a8a93eda Mon Sep 17 00:00:00 2001 From: Andrew Torgesen Date: Wed, 24 Dec 2025 20:16:57 -0800 Subject: [PATCH 3/6] inline --- include/signals/Signal.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/signals/Signal.h b/include/signals/Signal.h index ccbdb36..3224cb8 100644 --- a/include/signals/Signal.h +++ b/include/signals/Signal.h @@ -255,12 +255,12 @@ class Signal return true; } - static BaseType baseZero() + static inline BaseType baseZero() { return BaseSignalSpec::ZeroType(); } - static TangentType tangentZero() + static inline TangentType tangentZero() { return TangentSignalSpec::ZeroType(); } From 08aa58681fceb162a9c9ea0b9734df93f6b92606 Mon Sep 17 00:00:00 2001 From: Andrew Torgesen Date: Thu, 25 Dec 2025 17:27:58 -0800 Subject: [PATCH 4/6] add norms --- include/signals/Integration.h | 46 ++++++++++---------- include/signals/Signal.h | 82 ++++++++++++++++++++++------------- include/signals/State.h | 8 ++-- tests/SignalTest.cpp | 8 ++++ 4 files changed, 88 insertions(+), 56 deletions(-) diff --git a/include/signals/Integration.h b/include/signals/Integration.h index 7105ea9..6e1666a 100644 --- a/include/signals/Integration.h +++ b/include/signals/Integration.h @@ -4,11 +4,11 @@ template struct Integrator { - template - static bool integrate(Signal& xInt, - const Signal& x, - const double& tf, - const bool& insertIntoHistory = false) + template + static bool integrate(Signal& xInt, + const Signal& x, + const double& tf, + const bool& insertIntoHistory = false) { double t0 = xInt.t(); double dt; @@ -19,12 +19,12 @@ struct Integrator return IntegratorType::integrate(xInt, x, t0, tf, insertIntoHistory); } - template - static bool integrate(Signal& xInt, - const Signal& x, - const double& tf, - const double& dt, - const bool& insertIntoHistory = false) + template + static bool integrate(Signal& xInt, + const Signal& x, + const double& tf, + const double& dt, + const bool& insertIntoHistory = false) { double t_k = xInt.t(); bool success = true; @@ -48,12 +48,12 @@ struct Integrator struct EulerIntegratorSpec { - template - static bool integrate(Signal& xInt, - const Signal& x, - const double& t0, - const double& tf, - const bool& insertIntoHistory) + template + static bool integrate(Signal& xInt, + const Signal& x, + const double& t0, + const double& tf, + const bool& insertIntoHistory) { double dt = tf - t0; return xInt.update(tf, xInt() + x(tf) * dt, x(tf), insertIntoHistory); @@ -62,12 +62,12 @@ struct EulerIntegratorSpec struct TrapezoidalIntegratorSpec { - template - static bool integrate(Signal& xInt, - const Signal& x, - const double& t0, - const double& tf, - const bool& insertIntoHistory) + template + static bool integrate(Signal& xInt, + const Signal& x, + const double& t0, + const double& tf, + const bool& insertIntoHistory) { double dt = tf - t0; return xInt.update(tf, xInt() + (xInt.dot() + x(tf)) * dt / 2.0, x(tf), insertIntoHistory); diff --git a/include/signals/Signal.h b/include/signals/Signal.h index 3224cb8..41738bc 100644 --- a/include/signals/Signal.h +++ b/include/signals/Signal.h @@ -29,7 +29,7 @@ enum DerivativeMethod FINITE_DIFF }; -template +template class Signal { public: @@ -75,9 +75,9 @@ class Signal this->needsSort_ = other.needsSort_; } - Signal dotSignal() + Signal dotSignal() { - Signal signalDot; + Signal signalDot; for (auto signalDP : signalHistory_) { signalDot.update(signalDP.t, signalDP.xdot, true); @@ -86,17 +86,17 @@ class Signal return signalDot; } - template - friend Signal operator+(const Signal& l, const Signal& r); + template + friend Signal operator+(const Signal& l, const Signal& r); - template - friend Signal operator-(const Signal& l, const Signal& r); + template + friend Signal operator-(const Signal& l, const Signal& r); - template - friend Signal operator*(const double& l, const Signal& r); + template + friend Signal operator*(const double& l, const Signal& r); - template - friend Signal operator*(const Signal& l, const double& r); + template + friend Signal operator*(const Signal& l, const double& r); double t() const { @@ -265,6 +265,16 @@ class Signal return TangentSignalSpec::ZeroType(); } + static inline T baseNorm(const BaseType& x) + { + return BaseSignalSpec::Norm(x); + } + + static inline T tangentNorm(const TangentType& x) + { + return TangentSignalSpec::Norm(x); + } + private: double t_; BaseType x_; @@ -553,11 +563,11 @@ class Signal } }; -template -Signal operator+(const Signal& l, - const Signal& r) +template +Signal operator+(const Signal& l, + const Signal& r) { - Signal lpr = l; + Signal lpr = l; lpr.x_ += r(l.t()); lpr.xdot_ += r.dot(l.t()); for (auto& signalDP : lpr.signalHistory_) @@ -568,11 +578,11 @@ Signal operator+(const Signal -Signal operator-(const Signal& l, - const Signal& r) +template +Signal operator-(const Signal& l, + const Signal& r) { - Signal lmr; + Signal lmr; lmr.interpolationMethod = l.interpolationMethod; lmr.extrapolationMethod = l.extrapolationMethod; lmr.derivativeMethod = l.derivativeMethod; @@ -593,10 +603,11 @@ Signal operator-(const Signal -Signal operator*(const double& l, const Signal& r) +template +Signal operator*(const double& l, + const Signal& r) { - Signal lr = r; + Signal lr = r; lr.x_ *= l; lr.xdot_ *= l; for (auto& signalDP : lr.signalHistory_) @@ -607,10 +618,11 @@ Signal operator*(const double& l, const Signa return lr; } -template -Signal operator*(const Signal& l, const double& r) +template +Signal operator*(const Signal& l, + const double& r) { - Signal lr = l; + Signal lr = l; lr.x_ *= r; lr.xdot_ *= r; for (auto& signalDP : lr.signalHistory_) @@ -633,6 +645,10 @@ struct ScalarSignalSpec { return (T)1. / 0.; } + static T Norm(const Type& a) + { + return a; + } }; template @@ -647,9 +663,13 @@ struct VectorSignalSpec { return Type::Zero(); // TODO fix } + static T Norm(const Type& a) + { + return a.norm(); + } }; -template +template struct ManifoldSignalSpec { using Type = ManifoldType; @@ -661,10 +681,14 @@ struct ManifoldSignalSpec { return Type::identity(); // TODO fix } + static T Norm(const Type& a) + { + return ManifoldType::Log(a).norm(); + } }; template -using ScalarSignal = Signal, ScalarSignalSpec>; +using ScalarSignal = Signal, ScalarSignalSpec>; template inline std::ostream& operator<<(std::ostream& os, const ScalarSignal& x) @@ -674,7 +698,7 @@ inline std::ostream& operator<<(std::ostream& os, const ScalarSignal& x) } template -using VectorSignal = Signal, VectorSignalSpec>; +using VectorSignal = Signal, VectorSignalSpec>; template inline std::ostream& operator<<(std::ostream& os, const VectorSignal& x) @@ -684,7 +708,7 @@ inline std::ostream& operator<<(std::ostream& os, const VectorSignal& x) } template -using ManifoldSignal = Signal, VectorSignalSpec>; +using ManifoldSignal = Signal, VectorSignalSpec>; template inline std::ostream& operator<<(std::ostream& os, const ManifoldSignal& x) diff --git a/include/signals/State.h b/include/signals/State.h index 84008be..86fca72 100644 --- a/include/signals/State.h +++ b/include/signals/State.h @@ -121,7 +121,7 @@ VectorStateType operator/(const VectorStateType& l, const double& r) } template -using ManifoldStateType = State, PD, VectorSignalSpec, TD>; +using ManifoldStateType = State, PD, VectorSignalSpec, TD>; template inline std::ostream& operator<<(std::ostream& os, const ManifoldStateType& x) @@ -204,7 +204,7 @@ struct ManifoldStateSignalSpec }; template -using ScalarStateSignal = Signal, ScalarStateSignalSpec>; +using ScalarStateSignal = Signal, ScalarStateSignalSpec>; template inline std::ostream& operator<<(std::ostream& os, const ScalarStateSignal& x) @@ -214,7 +214,7 @@ inline std::ostream& operator<<(std::ostream& os, const ScalarStateSignal& x) } template -using VectorStateSignal = Signal, VectorStateSignalSpec>; +using VectorStateSignal = Signal, VectorStateSignalSpec>; template inline std::ostream& operator<<(std::ostream& os, const VectorStateSignal& x) @@ -225,7 +225,7 @@ inline std::ostream& operator<<(std::ostream& os, const VectorStateSignal& } template -using ManifoldStateSignal = Signal, VectorStateSignalSpec>; +using ManifoldStateSignal = Signal, VectorStateSignalSpec>; template inline std::ostream& operator<<(std::ostream& os, const ManifoldStateSignal& x) diff --git a/tests/SignalTest.cpp b/tests/SignalTest.cpp index a63a03c..8a8b8c8 100644 --- a/tests/SignalTest.cpp +++ b/tests/SignalTest.cpp @@ -48,6 +48,14 @@ BOOST_AUTO_TEST_CASE(TestLinearInterpolation) BOOST_CHECK_CLOSE(q(-0.5).z(), 0., 1e-8); } +BOOST_AUTO_TEST_CASE(TestNorms) +{ + SO3d q = SO3d::identity(); + Vector3d v(3., 4., 0.); + BOOST_CHECK_CLOSE(SO3dSignal::baseNorm(q), 0.0, 1e-8); + BOOST_CHECK_CLOSE(SO3dSignal::tangentNorm(v), 5.0, 1e-8); +} + BOOST_AUTO_TEST_CASE(TestDirtyDerivative) { Vector2dSignal v; From 77bcf5b20172ed2b52923471fba04c83d6aaf26d Mon Sep 17 00:00:00 2001 From: Andrew Torgesen Date: Fri, 26 Dec 2025 09:28:51 -0800 Subject: [PATCH 5/6] fix state norm --- include/signals/State.h | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/include/signals/State.h b/include/signals/State.h index 86fca72..5162bbf 100644 --- a/include/signals/State.h +++ b/include/signals/State.h @@ -30,6 +30,13 @@ struct State return x; } + T norm() + { + T poseNorm = PoseTypeSpec::Norm(pose); + T twistNorm = TwistTypeSpec::Norm(twist); + return std::sqrt(poseNorm * poseNorm + twistNorm * twistNorm); + } + State& operator*=(const double& s) { pose *= s; @@ -173,6 +180,10 @@ struct ScalarStateSignalSpec { return Type::identity(); // TODO fix } + static T Norm(const Type& a) + { + return a.norm(); + } }; template @@ -187,6 +198,10 @@ struct VectorStateSignalSpec { return Type::identity(); // TODO fix } + static T Norm(const Type& a) + { + return a.norm(); + } }; template @@ -201,6 +216,10 @@ struct ManifoldStateSignalSpec { return Type::identity(); // TODO fix } + static T Norm(const Type& a) + { + return a.norm(); + } }; template From 1de07cb3228ed42e879eea09325b0a816e65a1da Mon Sep 17 00:00:00 2001 From: Andrew Torgesen Date: Fri, 26 Dec 2025 12:04:45 -0800 Subject: [PATCH 6/6] fix const --- include/signals/State.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/signals/State.h b/include/signals/State.h index 5162bbf..5569581 100644 --- a/include/signals/State.h +++ b/include/signals/State.h @@ -30,10 +30,10 @@ struct State return x; } - T norm() + T norm() const { - T poseNorm = PoseTypeSpec::Norm(pose); - T twistNorm = TwistTypeSpec::Norm(twist); + const T poseNorm = PoseTypeSpec::Norm(pose); + const T twistNorm = TwistTypeSpec::Norm(twist); return std::sqrt(poseNorm * poseNorm + twistNorm * twistNorm); }