Program Listing for File PinVelControl.h#
↰ Return to documentation for file (include/Karana/GeneralKModels/PinVelControl.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 declarations for the PinVelControl KModel.
*/
#pragma once
#include "Karana/GeneralKModels/ProfileGenerator.h"
#include "Karana/SOADyn/KModel.h"
#include "Karana/SOADyn/PhysicalSubhinge.h"
namespace Karana::Models {
namespace km = Karana::Math;
namespace kd = Karana::Dynamics;
// Forward declarations
class PinVelControlParams;
/**
* @class PinVelControlParamsVars
* @brief The Vars for the PinVelControlParams class.
*/
class PinVelControlParamsVars : public Karana::Core::BaseVars {
public:
/**
* @brief PinVelControlParamsVars constructor. The constructor is not meant to be
* called directly. Please use the create(...) method instead to create
* an instance.
*
* @param body The body associated with this PinVelControlParamsVars.
*/
PinVelControlParamsVars(const kc::ks_ptr<const PinVelControlParams> &body);
/**
* @brief Destructor.
*/
~PinVelControlParamsVars();
/**
* @brief Create an instance of the PinVelControlParamsVars.
*
* @param body The body associated with this PinVelControlParamsVars.
* @returns A pointer to the newly created instance of PinVelControlParamsVars.
*/
static kc::ks_ptr<PinVelControlParamsVars>
create(const kc::ks_ptr<const PinVelControlParams> &body);
/**
* @brief Get all the Vars that this PinVelControlParamsVars has.
*
* @returns A map of Vars, where the Var name is the key and the Var is the value.
*/
kc::NestedVars getAllVars() const override;
/// Proportional gain
kc::ks_ptr<Karana::Core::Var_T<double>> kp;
/// Integral gain
kc::ks_ptr<Karana::Core::Var_T<double>> ki;
};
/**
* @class PinVelControlParams
* @brief Parameters for the PinVelControl model.
*/
class PinVelControlParams : public KModelParams {
public:
/**
* @brief Constructor
* @param name - Name of the params.
*/
PinVelControlParams(std::string_view name);
/**
* @brief Used to ensure the params are ready.
*
* @return `true` if the params are ready, `false` otherwise.
*/
bool isReady() const final;
/// Proportional gain
double kp;
/// Integral gain
double ki;
protected:
Karana::Core::ks_ptr<Karana::Core::BaseVars> _getVars() const override;
};
/**
* @class PinVelControl
* @brief Simple PinVelControl controller model.
*/
class PinVelControl : public KModel<PinVelControl, PinVelControlParams> {
public:
/**
* @brief PinVelControl constructor. The constructor is not meant to be called directly.
* Please use the create(...) method instead to create an instance.
*
* The PinVelControl model adds force to a subhinge. The model uses
* the incoming function as the desired trajectory. The output of this
* function should be the desired Q values of the subhinge.
*
* @param name The name of the model.
* @param mm The ModelManager to register this model with.
* @param sh The Subhinge to apply the forces calculated by the PinVelControl controller to.
* @param traj A ProfileGenerator that defines the reference trajectory.
*/
PinVelControl(std::string_view name,
const kc::ks_ptr<kd::ModelManager> &mm,
const kc::ks_ptr<kd::PhysicalSubhinge> &sh,
const kc::ks_ptr<ProfileGenerator<km::ArrayVec>> &traj);
/**
* @brief Destructor for PinVelControl.
*/
~PinVelControl();
/**
* @brief Constructor. The PinVelControl model adds force to a subhinge. The model uses
* the incoming function as the desired trajectory. The output of this
* function should be the desired Q values of the subhinge.
*
* @param name The name of the model.
* @param mm The ModelManager to register this model with.
* @param sh The Subhinge to apply the forces calculated by the PinVelControl controller
* to.
* @param traj A ProfileGenerator that defines the reference trajectory.
* @return A ks_ptr to the newly created instance of the PinVelControl model.
*/
static kc::ks_ptr<PinVelControl>
create(std::string_view name,
const kc::ks_ptr<kd::ModelManager> &mm,
const kc::ks_ptr<kd::PhysicalSubhinge> &sh,
const kc::ks_ptr<ProfileGenerator<km::ArrayVec>> &traj);
/**
* @brief Calculate and apply the output of the PinVelControl model to the subhinge.
* @param t Current time.
* @param x Current state. Not used.
*/
void preDeriv(const km::Ktime &t, const km::Vec &x);
/**
* @brief Calculate the contribution of integral control.
* @param t Current time.
* @param x Current state. Not used.
*/
void preModelStep(const km::Ktime &t, const km::Vec &x);
/**
* @brief Get the Subhinge used by this model.
* @return the physical subhinge being used by the model
*/
const kc::ks_ptr<kd::PhysicalSubhinge> &getSubhinge() const;
/// The ProfileGenerator that defines the reference trajectory
kc::ks_ptr<ProfileGenerator<km::ArrayVec>> traj;
private:
/// The subhinge to apply the PinVelControl output to.
kc::ks_ptr<kd::PhysicalSubhinge> _sh;
/// Tracks the error state of the model
km::ArrayVec _e_prev;
/// The integral control contribution
km::ArrayVec _integral_contrib;
/// The previous time.
km::Ktime _prev_t;
/// Vector to hold the torque. Done here so we don't have to re-allocate each time.
km::Vec _t;
};
} // namespace Karana::Models