Skip to content

Expose zeroized objects from signal class #11

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
goromal merged 6 commits into from
Dec 26, 2025
Merged

Expose zeroized objects from signal class #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
32a80c5
Expose zeroized objects from signal class
goromal Dec 25, 2025
07fe687
make static
goromal Dec 25, 2025
c0b581b
inline
goromal Dec 25, 2025
08aa586
add norms
goromal Dec 26, 2025
77bcf5b
fix state norm
goromal Dec 26, 2025
1de07cb
fix const
goromal Dec 26, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 23 additions & 23 deletions include/signals/Integration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,11 @@
template<typename IntegratorType>
struct Integrator
{
template<typename BaseSignalSpec, typename TangentSignalSpec>
static bool integrate(Signal<BaseSignalSpec, TangentSignalSpec>& xInt,
const Signal<TangentSignalSpec, TangentSignalSpec>& x,
const double& tf,
const bool& insertIntoHistory = false)
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
static bool integrate(Signal<T, BaseSignalSpec, TangentSignalSpec>& xInt,
const Signal<T, TangentSignalSpec, TangentSignalSpec>& x,
const double& tf,
const bool& insertIntoHistory = false)
{
double t0 = xInt.t();
double dt;
Expand All @@ -19,12 +19,12 @@ struct Integrator
return IntegratorType::integrate(xInt, x, t0, tf, insertIntoHistory);
}

template<typename BaseSignalSpec, typename TangentSignalSpec>
static bool integrate(Signal<BaseSignalSpec, TangentSignalSpec>& xInt,
const Signal<TangentSignalSpec, TangentSignalSpec>& x,
const double& tf,
const double& dt,
const bool& insertIntoHistory = false)
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
static bool integrate(Signal<T, BaseSignalSpec, TangentSignalSpec>& xInt,
const Signal<T, TangentSignalSpec, TangentSignalSpec>& x,
const double& tf,
const double& dt,
const bool& insertIntoHistory = false)
{
double t_k = xInt.t();
bool success = true;
Expand All @@ -48,12 +48,12 @@ struct Integrator

struct EulerIntegratorSpec
{
template<typename BaseSignalSpec, typename TangentSignalSpec>
static bool integrate(Signal<BaseSignalSpec, TangentSignalSpec>& xInt,
const Signal<TangentSignalSpec, TangentSignalSpec>& x,
const double& t0,
const double& tf,
const bool& insertIntoHistory)
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
static bool integrate(Signal<T, BaseSignalSpec, TangentSignalSpec>& xInt,
const Signal<T, TangentSignalSpec, TangentSignalSpec>& 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);
Expand All @@ -62,12 +62,12 @@ struct EulerIntegratorSpec

struct TrapezoidalIntegratorSpec
{
template<typename BaseSignalSpec, typename TangentSignalSpec>
static bool integrate(Signal<BaseSignalSpec, TangentSignalSpec>& xInt,
const Signal<TangentSignalSpec, TangentSignalSpec>& x,
const double& t0,
const double& tf,
const bool& insertIntoHistory)
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
static bool integrate(Signal<T, BaseSignalSpec, TangentSignalSpec>& xInt,
const Signal<T, TangentSignalSpec, TangentSignalSpec>& 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);
Expand Down
92 changes: 63 additions & 29 deletions include/signals/Signal.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ enum DerivativeMethod
FINITE_DIFF
};

template<typename BaseSignalSpec, typename TangentSignalSpec>
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
class Signal
{
public:
Expand Down Expand Up @@ -75,9 +75,9 @@ class Signal
this->needsSort_ = other.needsSort_;
}

Signal<TangentSignalSpec, TangentSignalSpec> dotSignal()
Signal<T, TangentSignalSpec, TangentSignalSpec> dotSignal()
{
Signal<TangentSignalSpec, TangentSignalSpec> signalDot;
Signal<T, TangentSignalSpec, TangentSignalSpec> signalDot;
for (auto signalDP : signalHistory_)
{
signalDot.update(signalDP.t, signalDP.xdot, true);
Expand All @@ -86,17 +86,17 @@ class Signal
return signalDot;
}

template<typename BSS, typename TSS>
friend Signal<BSS, TSS> operator+(const Signal<BSS, TSS>& l, const Signal<TSS, TSS>& r);
template<typename S, typename BSS, typename TSS>
friend Signal<S, BSS, TSS> operator+(const Signal<S, BSS, TSS>& l, const Signal<S, TSS, TSS>& r);

template<typename BSS, typename TSS>
friend Signal<TSS, TSS> operator-(const Signal<BSS, TSS>& l, const Signal<BSS, TSS>& r);
template<typename S, typename BSS, typename TSS>
friend Signal<S, TSS, TSS> operator-(const Signal<S, BSS, TSS>& l, const Signal<S, BSS, TSS>& r);

template<typename BSS, typename TSS>
friend Signal<BSS, TSS> operator*(const double& l, const Signal<BSS, TSS>& r);
template<typename S, typename BSS, typename TSS>
friend Signal<S, BSS, TSS> operator*(const double& l, const Signal<S, BSS, TSS>& r);

template<typename BSS, typename TSS>
friend Signal<BSS, TSS> operator*(const Signal<BSS, TSS>& l, const double& r);
template<typename S, typename BSS, typename TSS>
friend Signal<S, BSS, TSS> operator*(const Signal<S, BSS, TSS>& l, const double& r);

double t() const
{
Expand Down Expand Up @@ -255,6 +255,26 @@ class Signal
return true;
}

static inline BaseType baseZero()
{
return BaseSignalSpec::ZeroType();
}

static inline TangentType tangentZero()
{
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_;
Expand Down Expand Up @@ -543,11 +563,11 @@ class Signal
}
};

template<typename BaseSignalSpec, typename TangentSignalSpec>
Signal<BaseSignalSpec, TangentSignalSpec> operator+(const Signal<BaseSignalSpec, TangentSignalSpec>& l,
const Signal<TangentSignalSpec, TangentSignalSpec>& r)
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
Signal<T, BaseSignalSpec, TangentSignalSpec> operator+(const Signal<T, BaseSignalSpec, TangentSignalSpec>& l,
const Signal<T, TangentSignalSpec, TangentSignalSpec>& r)
{
Signal<BaseSignalSpec, TangentSignalSpec> lpr = l;
Signal<T, BaseSignalSpec, TangentSignalSpec> lpr = l;
lpr.x_ += r(l.t());
lpr.xdot_ += r.dot(l.t());
for (auto& signalDP : lpr.signalHistory_)
Expand All @@ -558,11 +578,11 @@ Signal<BaseSignalSpec, TangentSignalSpec> operator+(const Signal<BaseSignalSpec,
return lpr;
}

template<typename BaseSignalSpec, typename TangentSignalSpec>
Signal<TangentSignalSpec, TangentSignalSpec> operator-(const Signal<BaseSignalSpec, TangentSignalSpec>& l,
const Signal<BaseSignalSpec, TangentSignalSpec>& r)
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
Signal<T, TangentSignalSpec, TangentSignalSpec> operator-(const Signal<T, BaseSignalSpec, TangentSignalSpec>& l,
const Signal<T, BaseSignalSpec, TangentSignalSpec>& r)
{
Signal<TangentSignalSpec, TangentSignalSpec> lmr;
Signal<T, TangentSignalSpec, TangentSignalSpec> lmr;
lmr.interpolationMethod = l.interpolationMethod;
lmr.extrapolationMethod = l.extrapolationMethod;
lmr.derivativeMethod = l.derivativeMethod;
Expand All @@ -583,10 +603,11 @@ Signal<TangentSignalSpec, TangentSignalSpec> operator-(const Signal<BaseSignalSp
return lmr;
}

template<typename BaseSignalSpec, typename TangentSignalSpec>
Signal<BaseSignalSpec, TangentSignalSpec> operator*(const double& l, const Signal<BaseSignalSpec, TangentSignalSpec>& r)
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
Signal<T, BaseSignalSpec, TangentSignalSpec> operator*(const double& l,
const Signal<T, BaseSignalSpec, TangentSignalSpec>& r)
{
Signal<BaseSignalSpec, TangentSignalSpec> lr = r;
Signal<T, BaseSignalSpec, TangentSignalSpec> lr = r;
lr.x_ *= l;
lr.xdot_ *= l;
for (auto& signalDP : lr.signalHistory_)
Expand All @@ -597,10 +618,11 @@ Signal<BaseSignalSpec, TangentSignalSpec> operator*(const double& l, const Signa
return lr;
}

template<typename BaseSignalSpec, typename TangentSignalSpec>
Signal<BaseSignalSpec, TangentSignalSpec> operator*(const Signal<BaseSignalSpec, TangentSignalSpec>& l, const double& r)
template<typename T, typename BaseSignalSpec, typename TangentSignalSpec>
Signal<T, BaseSignalSpec, TangentSignalSpec> operator*(const Signal<T, BaseSignalSpec, TangentSignalSpec>& l,
const double& r)
{
Signal<BaseSignalSpec, TangentSignalSpec> lr = l;
Signal<T, BaseSignalSpec, TangentSignalSpec> lr = l;
lr.x_ *= r;
lr.xdot_ *= r;
for (auto& signalDP : lr.signalHistory_)
Expand All @@ -623,6 +645,10 @@ struct ScalarSignalSpec
{
return (T)1. / 0.;
}
static T Norm(const Type& a)
{
return a;
}
};

template<typename T, size_t d>
Expand All @@ -637,9 +663,13 @@ struct VectorSignalSpec
{
return Type::Zero(); // TODO fix
}
static T Norm(const Type& a)
{
return a.norm();
}
};

template<typename ManifoldType>
template<typename T, typename ManifoldType>
struct ManifoldSignalSpec
{
using Type = ManifoldType;
Expand All @@ -651,10 +681,14 @@ struct ManifoldSignalSpec
{
return Type::identity(); // TODO fix
}
static T Norm(const Type& a)
{
return ManifoldType::Log(a).norm();
}
};

template<typename T>
using ScalarSignal = Signal<ScalarSignalSpec<T>, ScalarSignalSpec<T>>;
using ScalarSignal = Signal<T, ScalarSignalSpec<T>, ScalarSignalSpec<T>>;

template<typename T>
inline std::ostream& operator<<(std::ostream& os, const ScalarSignal<T>& x)
Expand All @@ -664,7 +698,7 @@ inline std::ostream& operator<<(std::ostream& os, const ScalarSignal<T>& x)
}

template<typename T, size_t d>
using VectorSignal = Signal<VectorSignalSpec<T, d>, VectorSignalSpec<T, d>>;
using VectorSignal = Signal<T, VectorSignalSpec<T, d>, VectorSignalSpec<T, d>>;

template<typename T, size_t d>
inline std::ostream& operator<<(std::ostream& os, const VectorSignal<T, d>& x)
Expand All @@ -674,7 +708,7 @@ inline std::ostream& operator<<(std::ostream& os, const VectorSignal<T, d>& x)
}

template<typename T, typename ManifoldType, size_t d>
using ManifoldSignal = Signal<ManifoldSignalSpec<ManifoldType>, VectorSignalSpec<T, d>>;
using ManifoldSignal = Signal<T, ManifoldSignalSpec<T, ManifoldType>, VectorSignalSpec<T, d>>;

template<typename T, typename ManifoldType, size_t d>
inline std::ostream& operator<<(std::ostream& os, const ManifoldSignal<T, ManifoldType, d>& x)
Expand Down
27 changes: 23 additions & 4 deletions include/signals/State.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,13 @@ struct State
return x;
}

T norm() const
{
const T poseNorm = PoseTypeSpec::Norm(pose);
const T twistNorm = TwistTypeSpec::Norm(twist);
return std::sqrt(poseNorm * poseNorm + twistNorm * twistNorm);
}

State& operator*=(const double& s)
{
pose *= s;
Expand Down Expand Up @@ -121,7 +128,7 @@ VectorStateType<T, d> operator/(const VectorStateType<T, d>& l, const double& r)
}

template<typename T, typename ManifoldType, size_t PD, size_t TD>
using ManifoldStateType = State<T, ManifoldSignalSpec<ManifoldType>, PD, VectorSignalSpec<T, TD>, TD>;
using ManifoldStateType = State<T, ManifoldSignalSpec<T, ManifoldType>, PD, VectorSignalSpec<T, TD>, TD>;

template<typename T, typename ManifoldType, size_t PD, size_t TD>
inline std::ostream& operator<<(std::ostream& os, const ManifoldStateType<T, ManifoldType, PD, TD>& x)
Expand Down Expand Up @@ -173,6 +180,10 @@ struct ScalarStateSignalSpec
{
return Type::identity(); // TODO fix
}
static T Norm(const Type& a)
{
return a.norm();
}
};

template<typename T, size_t d>
Expand All @@ -187,6 +198,10 @@ struct VectorStateSignalSpec
{
return Type::identity(); // TODO fix
}
static T Norm(const Type& a)
{
return a.norm();
}
};

template<typename T, typename ManifoldType, size_t PD, size_t TD>
Expand All @@ -201,10 +216,14 @@ struct ManifoldStateSignalSpec
{
return Type::identity(); // TODO fix
}
static T Norm(const Type& a)
{
return a.norm();
}
};

template<typename T>
using ScalarStateSignal = Signal<ScalarStateSignalSpec<T>, ScalarStateSignalSpec<T>>;
using ScalarStateSignal = Signal<T, ScalarStateSignalSpec<T>, ScalarStateSignalSpec<T>>;

template<typename T>
inline std::ostream& operator<<(std::ostream& os, const ScalarStateSignal<T>& x)
Expand All @@ -214,7 +233,7 @@ inline std::ostream& operator<<(std::ostream& os, const ScalarStateSignal<T>& x)
}

template<typename T, size_t d>
using VectorStateSignal = Signal<VectorStateSignalSpec<T, d>, VectorStateSignalSpec<T, d>>;
using VectorStateSignal = Signal<T, VectorStateSignalSpec<T, d>, VectorStateSignalSpec<T, d>>;

template<typename T, size_t d>
inline std::ostream& operator<<(std::ostream& os, const VectorStateSignal<T, d>& x)
Expand All @@ -225,7 +244,7 @@ inline std::ostream& operator<<(std::ostream& os, const VectorStateSignal<T, d>&
}

template<typename T, typename ManifoldType, size_t PD, size_t TD>
using ManifoldStateSignal = Signal<ManifoldStateSignalSpec<T, ManifoldType, PD, TD>, VectorStateSignalSpec<T, TD>>;
using ManifoldStateSignal = Signal<T, ManifoldStateSignalSpec<T, ManifoldType, PD, TD>, VectorStateSignalSpec<T, TD>>;

template<typename T, typename ManifoldType, size_t PD, size_t TD>
inline std::ostream& operator<<(std::ostream& os, const ManifoldStateSignal<T, ManifoldType, PD, TD>& x)
Expand Down
8 changes: 8 additions & 0 deletions tests/SignalTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading