Program Listing for File ProfileGenerator.h#
↰ Return to documentation for file (include/Karana/GeneralKModels/ProfileGenerator.h)
/*
* Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
*
* NOTICE TO USER:
*
* This source code and/or documentation (the "Licensed Materials") is
* the confidential and proprietary information of Karana Dynamics Inc.
* Use of these Licensed Materials is governed by the terms and conditions
* of a separate software license agreement between Karana Dynamics and the
* Licensee ("License Agreement"). Unless expressly permitted under that
* agreement, any reproduction, modification, distribution, or disclosure
* of the Licensed Materials, in whole or in part, to any third party
* without the prior written consent of Karana Dynamics is strictly prohibited.
*
* THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
* KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
* FITNESS FOR A PARTICULAR PURPOSE.
*
* IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
* INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
* OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
*
* U.S. Government End Users: The Licensed Materials are a "commercial item"
* as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
* only as a commercial end item under the terms of this license.
*
* Any use of the Licensed Materials in individual or commercial software must
* include, in the user documentation and internal source code comments,
* this Notice, Disclaimer, and U.S. Government Use Provision.
*/
/**
* @file
* @brief Contains the ProfileGenerator declarations.
*/
#pragma once
#include "Karana/KCore/Allocator.h"
#include "Karana/KCore/BaseWithVars.h"
#include "Karana/KCore/Var.h"
#include "Karana/Math/Defs.h"
#include "Karana/Math/UnitQuaternion.h"
#include "Karana/SOADyn/StatePropagator.h"
#include <stdexcept>
#include <tuple>
namespace Karana::Models {
// Forward declarations
template <typename T> class ProfileGenerator;
/**
* @brief Vars for the ProfileGenerator class.
*
* @tparam T The numeric type used in the ProfileGenerator.
*/
template <typename T> class ProfileGeneratorVars : public Karana::Core::BaseVars {
public:
/**
* @brief ProfileGeneratorVars constructor. The constructor is not meant to be
* called directly. Please use the create(...) method instead to create
* an instance.
*
* @param profile_generator The ProfileGenerator associated with this ProfileGeneratorVars.
*/
ProfileGeneratorVars(
const Karana::Core::ks_ptr<const ProfileGenerator<T>> &profile_generator)
: BaseVars(profile_generator) {}
/**
* @brief Destructor.
*/
~ProfileGeneratorVars(){};
/**
* @brief Create an instance of the ProfileGeneratorVars.
*
* @param profile_generator The ProfileGenerator associated with this ProfileGeneratorVars.
* @returns A pointer to the newly created instance of ProfileGeneratorVars.
*/
static Karana::Core::ks_ptr<ProfileGeneratorVars<T>>
create(const Karana::Core::ks_ptr<const ProfileGenerator<T>> &profile_generator) {
return std::allocate_shared<ProfileGeneratorVars<T>>(
kc::Allocator<ProfileGeneratorVars<T>>{}, profile_generator);
}
/**
* @brief Get a Var that gives the Q value of this ProfileGenerator at the provided
* ModelManager's current time.
*
* @param mm The ModelManager to use to get time from.
* @returns A Var that gives the Q value of this ProfileGenerator at the provided
* ModelManager's current time.
*/
Karana::Core::ks_ptr<Karana::Core::Var<km::Vec>>
getQ(const Karana::Core::ks_ptr<Karana::Dynamics::ModelManager> &mm) const
requires std::same_as<T, Karana::Math::ArrayVec>
{
auto pg = kc::const_pointer_cast<ProfileGenerator<T>>(
kc::static_pointer_cast<const ProfileGenerator<T>>(_bwv));
return Karana::Core::Var<km::Vec>::create(
"Q", [mm, pg]() -> km::Vec { return pg->getQ(mm->getTime()); }, pg->_u.size(), "Q");
}
/**
* @brief Get a Var that gives the Q value of this ProfileGenerator at the provided
* ModelManager's current time.
*
* @param mm The ModelManager to use to get time from.
* @returns A Var that gives the Q value of this ProfileGenerator at the provided
* ModelManager's current time.
*/
Karana::Core::ks_ptr<Karana::Core::Var<T>>
getQ(const Karana::Core::ks_ptr<Karana::Dynamics::ModelManager> &mm) const
requires(not std::same_as<T, Karana::Math::ArrayVec>)
{
auto pg = kc::const_pointer_cast<ProfileGenerator<T>>(
kc::static_pointer_cast<const ProfileGenerator<T>>(_bwv));
return Karana::Core::Var<T>::create(
"Q", [mm, pg]() { return pg->getQ(mm->getTime()); }, "Q");
}
/**
* @brief Get a Var that gives the U value of this ProfileGenerator at the provided
* ModelManager's current time.
*
* @param mm The ModelManager to use to get time from.
* @returns A Var that gives the Q value of this ProfileGenerator at the provided
* ModelManager's current time.
*/
Karana::Core::ks_ptr<Karana::Core::Var<km::Vec>>
getU(const Karana::Core::ks_ptr<Karana::Dynamics::ModelManager> &mm) const
requires std::same_as<T, Karana::Math::ArrayVec>
{
auto pg = kc::const_pointer_cast<ProfileGenerator<T>>(
kc::static_pointer_cast<const ProfileGenerator<T>>(_bwv));
return Karana::Core::Var<km::Vec>::create(
"U", [mm, pg]() -> km::Vec { return pg->getU(mm->getTime()); }, pg->_u.size(), "U");
}
/**
* @brief Get a Var that gives the U value of this ProfileGenerator at the provided
* ModelManager's current time.
*
* @param mm The ModelManager to use to get time from.
* @returns A Var that gives the Q value of this ProfileGenerator at the provided
* ModelManager's current time.
*/
Karana::Core::ks_ptr<Karana::Core::Var<T>>
getU(const Karana::Core::ks_ptr<Karana::Dynamics::ModelManager> &mm) const
requires(not std::same_as<T, Karana::Math::ArrayVec>)
{
auto pg = kc::const_pointer_cast<ProfileGenerator<T>>(
kc::static_pointer_cast<const ProfileGenerator<T>>(_bwv));
return Karana::Core::Var<T>::create(
"U", [mm, pg]() { return pg->getU(mm->getTime()); }, "U");
}
/**
* @brief Get a Var that gives the Udot value of this ProfileGenerator at the provided
* ModelManager's current time.
*
* @param mm The ModelManager to use to get time from.
* @returns A Var that gives the Q value of this ProfileGenerator at the provided
* ModelManager's current time.
*/
Karana::Core::ks_ptr<Karana::Core::Var<km::Vec>>
getUdot(const Karana::Core::ks_ptr<Karana::Dynamics::ModelManager> &mm) const
requires std::same_as<T, Karana::Math::ArrayVec>
{
auto pg = kc::const_pointer_cast<ProfileGenerator<T>>(
kc::static_pointer_cast<const ProfileGenerator<T>>(_bwv));
return Karana::Core::Var<km::Vec>::create(
"Udot",
[mm, pg]() -> km::Vec { return pg->getUdot(mm->getTime()); },
pg->_u.size(),
"Udot");
}
/**
* @brief Get a Var that gives the Udot value of this ProfileGenerator at the provided
* ModelManager's current time.
*
* @param mm The ModelManager to use to get time from.
* @returns A Var that gives the Q value of this ProfileGenerator at the provided
* ModelManager's current time.
*/
Karana::Core::ks_ptr<Karana::Core::Var<T>>
getUdot(const Karana::Core::ks_ptr<Karana::Dynamics::ModelManager> &mm) const
requires(not std::same_as<T, Karana::Math::ArrayVec>)
{
auto pg = kc::const_pointer_cast<ProfileGenerator<T>>(
kc::static_pointer_cast<const ProfileGenerator<T>>(_bwv));
return Karana::Core::Var<T>::create(
"Udot", [mm, pg]() { return pg->getUdot(mm->getTime()); }, "Udot");
}
};
extern template class ProfileGeneratorVars<double>;
extern template class ProfileGeneratorVars<Karana::Math::UnitQuaternion>;
extern template class ProfileGeneratorVars<Karana::Math::ArrayVec>;
/**
* @class ProfileGenerator
* @brief ProfileGenerator base class.
*
* @tparam T The type that the ProfileGenerator produces, e.g., double, UnitQuaternion, etc.
*/
template <typename T> class ProfileGenerator : public Karana::Core::BaseWithVars {
// For access to _u
template <typename U> friend class ProfileGeneratorVars;
public:
/**
* @brief ProfileGenerator constructor.
*
* @param name The name of this profile generator.
* @param u A representative sample value for the velocity.
*/
ProfileGenerator(std::string_view name, const T &u)
: BaseWithVars(name)
, _u(_init_zero(u)) {}
/**
* @brief Get the profile coordinate for the provided time.
*
* @param t The current time
* @return The profile coordinate at the provided time.
*/
virtual const T &getQ(const Karana::Math::Ktime &t) = 0;
/**
* @brief Get the profile velocity for the provided time.
*
* @param t The current time
* @return The profile velocity at the provided time.
*/
virtual const T &getU(const Karana::Math::Ktime &t) = 0;
/**
* @brief Get the profile acceleration for the provided time.
*
* @param t The current time
* @return The profile acceleration at the provided time.
*/
virtual const T &getUdot(const Karana::Math::Ktime &t) = 0;
protected:
/// The profile velocity
T _u;
/**
* @brief Get an initial value of zero based on the type.
*
* @param value The value to use as a template to set zeros for.
* @return A value of type T that is zero.
*/
T _init_zero(const T &value) {
if constexpr (std::is_same_v<T, double>) {
return 0.0;
} else if constexpr (std::is_same_v<T, Karana::Math::UnitQuaternion>) {
return Karana::Math::UnitQuaternion(0.0, 0.0, 0.0, 1.0);
} else {
return T::Zero(value.size());
}
}
Karana::Core::ks_ptr<Karana::Core::BaseVars> _getVars() const override {
return ProfileGeneratorVars<T>::create(
kc::static_pointer_cast<const ProfileGenerator<T>>(
kc::ks_ptr<const Base>(shared_from_this())));
}
};
/**
* @brief A simple constant profile generator.
*
* @tparam T The type of object to generate a profile for.
*/
template <typename T> class ConstantProfileGenerator : public ProfileGenerator<T> {
public:
/**
* @brief ConstantProfileGenerator constructor. The constructor is not meant to be called
* directly. Please use the create(...) method instead to create an instance.
*
* @param name The name of the ConstantProfileGenerator.
* @param q The coordinate of the ConstantProfileGenerator.
*/
ConstantProfileGenerator(std::string_view name, const T &q)
: ProfileGenerator<T>(name, q)
, q(q) {}
/**
* @brief ConstantProfileGenerator constructor.
*
* @param name The name of the ConstantProfileGenerator.
* @param q The coordinate of the ConstantProfileGenerator.
* @returns A pointer to the newly created instance of ConstantProfileGenerator.
*/
static Karana::Core::ks_ptr<ConstantProfileGenerator> create(std::string_view name,
const T &q) {
return std::allocate_shared<ConstantProfileGenerator<T>>(
Karana::Core::Allocator<ConstantProfileGenerator<T>>{}, name, q);
}
/**
* @brief Get the constant profile coordinate; ignores the time argument.
* @param t The current time (not used).
*
* @return The constant profile coordinate.
*/
const T &getQ([[maybe_unused]] const Karana::Math::Ktime &t) override { return q; }
const T &getU([[maybe_unused]] const Karana::Math::Ktime &t) override {
return ProfileGenerator<T>::_u;
}
const T &getUdot([[maybe_unused]] const Karana::Math::Ktime &t) override {
return ProfileGenerator<T>::_u;
}
/// The coordinate for constant interpolation
T q;
};
extern template class ConstantProfileGenerator<double>;
extern template class ConstantProfileGenerator<Karana::Math::UnitQuaternion>;
extern template class ConstantProfileGenerator<Karana::Math::ArrayVec>;
/**
* @brief A simple linear profile generator.
*
* This does linear interpolation between (t_i,q_i) and (t_f,q_f).
* Times provided outside the given range, less than t_i or greater
* than t_f, will yield q_i and q_f, respectively.
*
* @tparam T The type of object to generate a profile for.
*/
template <typename T> class LinearProfileGenerator : public ProfileGenerator<T> {
public:
/**
* @brief LinearProfileGenerator constructor. The constructor is not meant to be called
* directly. Please use the create(...) method instead to create an instance.
*
* @param t_i Time associated with q_i
* @param q_i The starting coordinate for linear interpolation
* @param t_f Time associated with q_f
* @param q_f The ending coordinate for linear interpolation
* @param name The name of the LinearProfileGenerator.
*/
LinearProfileGenerator(std::string_view name,
const Karana::Math::Ktime &t_i,
const T &q_i,
const Karana::Math::Ktime &t_f,
const T &q_f)
: ProfileGenerator<T>(name, q_i)
, t_i(t_i)
, q_i(q_i)
, t_f(t_f)
, q_f(q_f)
, _q(q_i)
, _udot(_init_ddv(q_i)) {}
/**
* @brief LinearProfileGenerator constructor.
*
* @param name The name of the LinearProfileGenerator.
* @param t_i Time associated with q_i
* @param q_i The starting coordinate for linear interpolation
* @param t_f Time associated with q_f
* @param q_f The ending coordinate for linear interpolation
* @returns A pointer to the newly created instance of LinearProfileGenerator.
*/
static Karana::Core::ks_ptr<LinearProfileGenerator> create(std::string_view name,
const Karana::Math::Ktime &t_i,
const T &q_i,
const Karana::Math::Ktime &t_f,
const T &q_f) {
return std::allocate_shared<LinearProfileGenerator<T>>(
Karana::Core::Allocator<LinearProfileGenerator<T>>{}, name, t_i, q_i, t_f, q_f);
}
const T &getQ(const Karana::Math::Ktime &t) override {
if (t < t_i) {
return q_i;
} else if (t > t_f) {
return q_f;
} else {
_q = q_i + (q_f - q_i) / Karana::Math::ktimeToSeconds(t_f - t_i) *
Karana::Math::ktimeToSeconds(t - t_i);
return _q;
}
}
const T &getU(const Karana::Math::Ktime &t) override {
if (t < t_i or t > t_f) {
if constexpr (std::is_same_v<T, double>) {
ProfileGenerator<T>::_u = 0.0;
} else {
ProfileGenerator<T>::_u.setZero();
}
} else {
ProfileGenerator<T>::_u = (q_f - q_i) / Karana::Math::ktimeToSeconds(t_f - t_i);
}
return ProfileGenerator<T>::_u;
}
const T &getUdot([[maybe_unused]] const Karana::Math::Ktime &t) override { return _udot; }
/**
* @brief Indicate whether this LinearProfileGenerator is ready for simulation.
*
* @return true if it is ready, false otherwise.
*/
bool isReady() const override {
bool flag = true;
if (not Karana::Math::isReady(t_i)) {
flag = false;
Karana::Core::warn("t_i of {} is not ready.", Karana::Core::Base::name());
}
if (not Karana::Math::isReady(t_f)) {
flag = false;
Karana::Core::warn("t_f of {} is not ready.", Karana::Core::Base::name());
}
return flag;
}
/// Time associated with q_i
Karana::Math::Ktime t_i;
/// The starting coordinate for linear interpolation
T q_i;
/// Time associated with q_f
Karana::Math::Ktime t_f;
/// The ending coordinate for linear interpolation
T q_f;
private:
/// Represents the current coordinate
T _q;
/// Represents the current acceleration
T _udot;
/**
* @brief Initialize the ddv value with zero of the appropriate type.
*
* @param value The value to use as a template to set zeros for.
* @return A value of type T that is zero.
*/
T _init_ddv(const T &value) {
if constexpr (std::is_same_v<T, double>) {
return 0.0;
} else if constexpr (std::is_same_v<T, Karana::Math::UnitQuaternion>) {
return Karana::Math::UnitQuaternion(0.0, 0.0, 0.0, 1.0);
} else {
return T::Zero(value.size());
}
}
};
extern template class LinearProfileGenerator<double>;
extern template class LinearProfileGenerator<Karana::Math::ArrayVec>;
/**
* @brief A simple cubic spline profile generator.
*
* This does cubic interpolation between (t_i,q_i,u_i) and (t_f,q_f,u_f).
* It uses a Hermite polynomial to match position and velocity at both knot points.
* Times provided outside the given range, less than t_i or greater
* than t_f, will yield q_i and q_f, respectively.
*
* @tparam T The type of object to generate a profile for.
*/
template <typename T> class CubicSplineProfileGenerator : public ProfileGenerator<T> {
public:
/**
* @brief CubicSplineProfileGenerator constructor. The constructor is not meant to be called
* directly. Please use the create(...) method instead to create an instance.
*
* @param name The name of the CubicSplineProfileGenerator.
* @param t_i Time associated with q_i
* @param q_i The starting coordinate
* @param u_i The starting velocity
* @param t_f Time associated with q_f
* @param q_f The ending coordinate
* @param u_f The ending velocity
*/
CubicSplineProfileGenerator(std::string_view name,
const Karana::Math::Ktime &t_i,
const T &q_i,
const T &u_i,
const Karana::Math::Ktime &t_f,
const T &q_f,
const T &u_f)
: ProfileGenerator<T>(name, u_i)
, _udot(_init_ddv(q_i)) {
setValues(t_i, q_i, u_i, t_f, q_f, u_f);
}
/**
* @brief CubicSplineProfileGenerator constructor.
*
* @param name The name of the CubicSplineProfileGenerator.
* @param t_i Time associated with q_i
* @param q_i The starting coordinate
* @param u_i The starting velocity
* @param t_f Time associated with q_f
* @param q_f The ending coordinate
* @param u_f The ending velocity
* @returns A pointer to the newly created instance of CubicSplineProfileGenerator.
*/
static Karana::Core::ks_ptr<CubicSplineProfileGenerator>
create(std::string_view name,
const Karana::Math::Ktime &t_i,
const T &q_i,
const T &u_i,
const Karana::Math::Ktime &t_f,
const T &q_f,
const T &u_f) {
return std::allocate_shared<CubicSplineProfileGenerator<T>>(
Karana::Core::Allocator<CubicSplineProfileGenerator<T>>{},
name,
t_i,
q_i,
u_i,
t_f,
q_f,
u_f);
}
const T &getQ(const Karana::Math::Ktime &t) override {
if (t < _t_i) {
return _q_i;
} else if (t > _t_f) {
return _q_f;
} else {
double tau = Karana::Math::ktimeToSeconds(t - _t_i);
double tau_2 = tau * tau;
_q = _q_i + _u_i * tau + _b * tau_2 + _a * tau_2 * tau;
return _q;
}
}
const T &getU(const Karana::Math::Ktime &t) override {
if (t < _t_i) {
return _u_i;
} else if (t > _t_f) {
return _u_f;
} else {
double tau = Karana::Math::ktimeToSeconds(t - _t_i);
ProfileGenerator<T>::_u = _u_i + 2.0 * _b * tau + 3.0 * _a * tau * tau;
return ProfileGenerator<T>::_u;
}
return ProfileGenerator<T>::_u;
}
const T &getUdot(const Karana::Math::Ktime &t) override {
if (t < _t_i || t > _t_f) {
if constexpr (std::is_same_v<T, double>) {
_udot = 0.0;
} else {
_udot.setZero();
}
} else {
double tau = Karana::Math::ktimeToSeconds(t - _t_i);
_udot = 2.0 * _b + 6.0 * _a * tau;
}
return _udot;
}
/**
* @brief Indicate whether this CubicSplineProfileGenerator is ready for simulation.
*
* @return true if it is ready, false otherwise.
*/
bool isReady() const override {
bool flag = true;
if (not Karana::Math::isReady(_t_i)) {
flag = false;
Karana::Core::warn("Initial time of {} is not ready.", Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_t_f)) {
flag = false;
Karana::Core::warn("Final time of {} is not ready.", Karana::Core::Base::name());
}
if constexpr (std::is_same_v<T, double>) {
if (Karana::Math::isNotReadyNaN(_q_i)) {
flag = false;
Karana::Core::warn("Initial coordinate of {} is not ready.",
Karana::Core::Base::name());
}
if (Karana::Math::isNotReadyNaN(_q_f)) {
flag = false;
Karana::Core::warn("Final coordinate of {} is not ready.",
Karana::Core::Base::name());
}
if (Karana::Math::isNotReadyNaN(_u_i)) {
flag = false;
Karana::Core::warn("Initial velocity of {} is not ready.",
Karana::Core::Base::name());
}
if (Karana::Math::isNotReadyNaN(_u_f)) {
flag = false;
Karana::Core::warn("Final velocity of {} is not ready.",
Karana::Core::Base::name());
}
} else {
if (not Karana::Math::isReady(_q_i)) {
flag = false;
Karana::Core::warn("Initial coordinate of {} is not ready.",
Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_q_f)) {
flag = false;
Karana::Core::warn("Final coordinate of {} is not ready.",
Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_u_i)) {
flag = false;
Karana::Core::warn("Initial velocity of {} is not ready.",
Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_u_f)) {
flag = false;
Karana::Core::warn("Final velocity of {} is not ready.",
Karana::Core::Base::name());
}
}
return flag;
}
/**
* @brief Set the starting/ending coordinates and calculate constants
*
* @param t_i Time associated with q_i.
* @param q_i The starting coordinate.
* @param u_i The starting velocity.
* @param t_f Time associated with q_f.
* @param q_f The ending coordinate.
* @param u_f The ending velocity.
*/
void setValues(const Karana::Math::Ktime &t_i,
const T &q_i,
const T &u_i,
const Karana::Math::Ktime &t_f,
const T &q_f,
const T &u_f) {
_t_i = t_i;
_q_i = q_i;
_u_i = u_i;
_t_f = t_f;
_q_f = q_f;
_u_f = u_f;
// Early out if we have the constant case
if constexpr (std::is_same_v<T, double>) {
if (u_i == 0.0 and u_f == 0.0 and q_f == q_i) {
_a = 0.0;
_b = 0.0;
return;
}
} else {
if (u_i.isZero() and u_f.isZero() and (q_i == q_f).all()) {
// Set _a and _b to zeros of the appropriate size
_a = u_i;
_b = u_i;
return;
}
}
// Cubic Hermite basis functions:
// https://en.wikipedia.org/wiki/Cubic_Hermite_spline
double dt = Karana::Math::ktimeToSeconds(t_f - t_i);
_a = 2.0 * (_q_i - _q_f) / std::pow(dt, 3) + (_u_i + _u_f) / (dt * dt);
_b = 3.0 * (_q_f - _q_i) / (dt * dt) - (2.0 * _u_i + _u_f) / dt;
}
/**
* @brief Return the endpoint constraints used to construct this cubic profile.
*
* @return A tuple containing initial time, coordinate, velocity, then final time,
* coordinate, and velocity.
*/
std::tuple<Karana::Math::Ktime, T, T, Karana::Math::Ktime, T, T> getValues() const {
return {_t_i, _q_i, _u_i, _t_f, _q_f, _u_f};
}
protected:
/// Represents the current coordinate
T _q;
/// Second derivative of the current acceleration
T _udot;
/**
* @brief Initialize the ddv value with zero of the appropriate type.
*
* @param value The value to use as a template to set zeros for.
* @return A value of type T that is zero.
*/
T _init_ddv(const T &value) {
if constexpr (std::is_same_v<T, double>) {
return 0.0;
} else if constexpr (std::is_same_v<T, Karana::Math::UnitQuaternion>) {
return Karana::Math::UnitQuaternion(0.0, 0.0, 0.0, 1.0);
} else {
return T::Zero(value.size());
}
}
/// Time associated with _q_i
Karana::Math::Ktime _t_i;
/// The starting coordinate
T _q_i;
/// The starting velocity
T _u_i;
/// Time associated with _q_f
Karana::Math::Ktime _t_f;
/// The ending coordinate
T _q_f;
/// The ending velocity
T _u_f;
private:
/// Cubic coefficient in interpolation
T _a;
/// Quadratic coefficient in interpolation
T _b;
};
extern template class CubicSplineProfileGenerator<double>;
extern template class CubicSplineProfileGenerator<Karana::Math::ArrayVec>;
/**
* @brief A simple quintic spline profile generator.
*
* This does quintic interpolation between (t_i,q_i,u_i,udot_i) and
* (t_f,q_f,u_f,udot_f).
* It uses a Hermite polynomial to match position, velocity, and acceleration at both knot
* points.
* Times provided outside the given range, less than t_i or greater
* than t_f, will yield q_i and q_f, respectively.
*
* For elapsed time \f$\tau=t-t_i\f$, the profile is
* \f$q(\tau)=c_0+c_1\tau+c_2\tau^2+c_3\tau^3+c_4\tau^4+c_5\tau^5\f$.
* Its six coefficients uniquely satisfy coordinate, velocity, and acceleration at both
* endpoints.
*
* @tparam T The type of object to generate a profile for.
*/
template <typename T> class QuinticSplineProfileGenerator : public ProfileGenerator<T> {
public:
/**
* @brief QuinticSplineProfileGenerator constructor. The constructor is not meant to be
* called directly. Please use the create(...) method instead to create an instance.
*
* @param name The name of the QuinticSplineProfileGenerator.
* @param t_i Time associated with q_i
* @param q_i The starting coordinate
* @param u_i The starting velocity
* @param udot_i The starting acceleration
* @param t_f Time associated with q_f; must be later than t_i
* @param q_f The ending coordinate
* @param u_f The ending velocity
* @param udot_f The ending acceleration
*/
QuinticSplineProfileGenerator(std::string_view name,
const Karana::Math::Ktime &t_i,
const T &q_i,
const T &u_i,
const T &udot_i,
const Karana::Math::Ktime &t_f,
const T &q_f,
const T &u_f,
const T &udot_f)
: ProfileGenerator<T>(name, u_i)
, _q(q_i)
, _udot(_init_ddv(q_i)) {
setValues(t_i, q_i, u_i, udot_i, t_f, q_f, u_f, udot_f);
}
/**
* @brief QuinticSplineProfileGenerator constructor.
*
* @param name The name of the QuinticSplineProfileGenerator.
* @param t_i Time associated with q_i
* @param q_i The starting coordinate
* @param u_i The starting velocity
* @param udot_i The starting acceleration
* @param t_f Time associated with q_f; must be later than t_i
* @param q_f The ending coordinate
* @param u_f The ending velocity
* @param udot_f The ending acceleration
* @returns A pointer to the newly created instance of QuinticSplineProfileGenerator.
*/
static Karana::Core::ks_ptr<QuinticSplineProfileGenerator>
create(std::string_view name,
const Karana::Math::Ktime &t_i,
const T &q_i,
const T &u_i,
const T &udot_i,
const Karana::Math::Ktime &t_f,
const T &q_f,
const T &u_f,
const T &udot_f) {
return std::allocate_shared<QuinticSplineProfileGenerator<T>>(
Karana::Core::Allocator<QuinticSplineProfileGenerator<T>>{},
name,
t_i,
q_i,
u_i,
udot_i,
t_f,
q_f,
u_f,
udot_f);
}
const T &getQ(const Karana::Math::Ktime &t) override {
if (t < _t_i) {
return _q_i;
} else if (t > _t_f) {
return _q_f;
} else {
double tau = Karana::Math::ktimeToSeconds(t - _t_i);
// Horner's method avoids constructing separate powers of tau.
_q = _c0 + tau * (_c1 + tau * (_c2 + tau * (_c3 + tau * (_c4 + tau * _c5))));
return _q;
}
}
const T &getU(const Karana::Math::Ktime &t) override {
if (t < _t_i) {
return _u_i;
} else if (t > _t_f) {
return _u_f;
} else {
double tau = Karana::Math::ktimeToSeconds(t - _t_i);
// Horner's method evaluates the derivative with minimal intermediate work.
ProfileGenerator<T>::_u =
_c1 +
tau * (2.0 * _c2 + tau * (3.0 * _c3 + tau * (4.0 * _c4 + tau * 5.0 * _c5)));
return ProfileGenerator<T>::_u;
}
return ProfileGenerator<T>::_u;
}
const T &getUdot(const Karana::Math::Ktime &t) override {
if (t < _t_i || t > _t_f) {
if constexpr (std::is_same_v<T, double>) {
_udot = 0.0;
} else {
_udot.setZero();
}
} else {
double tau = Karana::Math::ktimeToSeconds(t - _t_i);
// Horner's method also keeps acceleration evaluation compact.
_udot = 2.0 * _c2 + tau * (6.0 * _c3 + tau * (12.0 * _c4 + tau * 20.0 * _c5));
}
return _udot;
}
/**
* @brief Indicate whether this QuinticSplineProfileGenerator is ready for simulation.
*
* @return true if it is ready, false otherwise.
*/
bool isReady() const override {
bool flag = true;
if (not Karana::Math::isReady(_t_i)) {
flag = false;
Karana::Core::warn("Initial time of {} is not ready.", Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_t_f)) {
flag = false;
Karana::Core::warn("Final time of {} is not ready.", Karana::Core::Base::name());
}
if constexpr (std::is_same_v<T, double>) {
if (Karana::Math::isNotReadyNaN(_q_i)) {
flag = false;
Karana::Core::warn("Initial coordinate of {} is not ready.",
Karana::Core::Base::name());
}
if (Karana::Math::isNotReadyNaN(_q_f)) {
flag = false;
Karana::Core::warn("Final coordinate of {} is not ready.",
Karana::Core::Base::name());
}
if (Karana::Math::isNotReadyNaN(_u_i)) {
flag = false;
Karana::Core::warn("Initial velocity of {} is not ready.",
Karana::Core::Base::name());
}
if (Karana::Math::isNotReadyNaN(_u_f)) {
flag = false;
Karana::Core::warn("Final velocity of {} is not ready.",
Karana::Core::Base::name());
}
if (Karana::Math::isNotReadyNaN(_udot_i)) {
flag = false;
Karana::Core::warn("Initial acceleration of {} is not ready.",
Karana::Core::Base::name());
}
if (Karana::Math::isNotReadyNaN(_udot_f)) {
flag = false;
Karana::Core::warn("Final acceleration of {} is not ready.",
Karana::Core::Base::name());
}
} else {
if (not Karana::Math::isReady(_q_i)) {
flag = false;
Karana::Core::warn("Initial coordinate of {} is not ready.",
Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_q_f)) {
flag = false;
Karana::Core::warn("Final coordinate of {} is not ready.",
Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_u_i)) {
flag = false;
Karana::Core::warn("Initial velocity of {} is not ready.",
Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_u_f)) {
flag = false;
Karana::Core::warn("Final velocity of {} is not ready.",
Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_udot_i)) {
flag = false;
Karana::Core::warn("Initial acceleration of {} is not ready.",
Karana::Core::Base::name());
}
if (not Karana::Math::isReady(_udot_f)) {
flag = false;
Karana::Core::warn("Final acceleration of {} is not ready.",
Karana::Core::Base::name());
}
}
return flag;
}
/**
* @brief Set the starting/ending coordinates and calculate constants.
*
* @param t_i Time associated with q_i.
* @param q_i The starting coordinate.
* @param u_i The starting velocity.
* @param udot_i The starting acceleration.
* @param t_f Time associated with q_f; must be later than t_i.
* @param q_f The ending coordinate.
* @param u_f The ending velocity.
* @param udot_f The ending acceleration.
* @throws std::invalid_argument if the time interval is empty or reversed, or vector
* constraints have inconsistent sizes.
*/
void setValues(const Karana::Math::Ktime &t_i,
const T &q_i,
const T &u_i,
const T &udot_i,
const Karana::Math::Ktime &t_f,
const T &q_f,
const T &u_f,
const T &udot_f) {
if (t_f <= t_i) {
throw std::invalid_argument("Quintic profile final time must follow initial time.");
}
if constexpr (std::is_same_v<T, Karana::Math::ArrayVec>) {
const Eigen::Index size = q_i.size();
if (q_f.size() != size || u_i.size() != size || u_f.size() != size ||
udot_i.size() != size || udot_f.size() != size) {
throw std::invalid_argument(
"Quintic profile vector constraints must have equal sizes.");
}
}
_t_i = t_i;
_q_i = q_i;
_u_i = u_i;
_udot_i = udot_i;
_t_f = t_f;
_q_f = q_f;
_u_f = u_f;
_udot_f = udot_f;
// Early out if we have the constant case
if constexpr (std::is_same_v<T, double>) {
if (u_i == 0.0 and u_f == 0.0 and udot_i == 0.0 and udot_f == 0.0 and q_f == q_i) {
_c0 = q_i;
_c1 = 0.0;
_c2 = 0.0;
_c3 = 0.0;
_c4 = 0.0;
_c5 = 0.0;
return;
}
} else {
if (u_i.isZero() and u_f.isZero() and udot_i.isZero() and udot_f.isZero() and
(q_i == q_f).all()) {
// Set coefficients to zeros of the appropriate size
_c0 = q_i;
_c1 = u_i;
_c2 = u_i;
_c3 = u_i;
_c4 = u_i;
_c5 = u_i;
return;
}
}
// Two-point quintic Hermite construction:
// https://en.wikipedia.org/wiki/Hermite_interpolation
double dt = Karana::Math::ktimeToSeconds(t_f - t_i);
// Build each fixed power once so the coefficient equations reuse prior work.
double dt_2 = dt * dt;
double dt_3 = dt_2 * dt;
double dt_4 = dt_3 * dt;
double dt_5 = dt_4 * dt;
const T displacement = _q_f - _q_i;
_c0 = _q_i;
_c1 = _u_i;
_c2 = 0.5 * _udot_i;
_c3 = (20.0 * displacement - (12.0 * _u_i + 8.0 * _u_f) * dt -
(3.0 * _udot_i - _udot_f) * dt_2) /
(2.0 * dt_3);
_c4 = (-30.0 * displacement + (16.0 * _u_i + 14.0 * _u_f) * dt +
(3.0 * _udot_i - 2.0 * _udot_f) * dt_2) /
(2.0 * dt_4);
_c5 = (12.0 * displacement - 6.0 * (_u_i + _u_f) * dt - (_udot_i - _udot_f) * dt_2) /
(2.0 * dt_5);
}
/**
* @brief Return the endpoint constraints used to construct this quintic profile.
*
* @return A tuple containing initial time, coordinate, velocity, and acceleration, then
* final time, coordinate, velocity, and acceleration.
*/
std::tuple<Karana::Math::Ktime, T, T, T, Karana::Math::Ktime, T, T, T> getValues() const {
return {_t_i, _q_i, _u_i, _udot_i, _t_f, _q_f, _u_f, _udot_f};
}
protected:
/// Represents the current coordinate
T _q;
/// Second derivative of the current acceleration
T _udot;
/**
* @brief Initialize the ddv value with zero of the appropriate type.
*
* @param value The value to use as a template to set zeros for.
* @return A value of type T that is zero.
*/
T _init_ddv(const T &value) {
if constexpr (std::is_same_v<T, double>) {
return 0.0;
} else if constexpr (std::is_same_v<T, Karana::Math::UnitQuaternion>) {
return Karana::Math::UnitQuaternion(0.0, 0.0, 0.0, 1.0);
} else {
return T::Zero(value.size());
}
}
/// Time associated with _q_i
Karana::Math::Ktime _t_i;
/// The starting coordinate
T _q_i;
/// The starting velocity
T _u_i;
/// The starting acceleration
T _udot_i;
/// Time associated with _q_f
Karana::Math::Ktime _t_f;
/// The ending coordinate
T _q_f;
/// The ending velocity
T _u_f;
/// The ending acceleration
T _udot_f;
private:
/// Constant coefficient in interpolation
T _c0;
/// Linear coefficient in interpolation
T _c1;
/// Quadratic coefficient in interpolation
T _c2;
/// Cubic coefficient in interpolation
T _c3;
/// Quartic coefficient in interpolation
T _c4;
/// Quintic coefficient in interpolation
T _c5;
};
extern template class QuinticSplineProfileGenerator<double>;
extern template class QuinticSplineProfileGenerator<Karana::Math::ArrayVec>;
} // namespace Karana::Models