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_T.h"
#include "Karana/Math/Defs.h"
#include "Karana/Math/UnitQuaternion.h"
#include "Karana/SOADyn/StatePropagator.h"
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_T<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_T<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<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<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_T<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_T<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<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<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_T<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_T<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<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<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).
* 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;
}
}
// Otherwise, set the cubic values
double del = Karana::Math::ktimeToSeconds(t_f - t_i);
_a = 2.0 * (_q_i - _q_f) / std::pow(del, 3) + (_u_i + _u_f) / (del * del);
_b = 3.0 * (_q_f - _q_i) / (del * del) - (2.0 * _u_i + _u_f) / del;
}
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>;
} // namespace Karana::Models