Program Listing for File PID.h

↰ Return to documentation for file (include/Karana/GeneralKModels/PID.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 PID 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;

    /**
     * @class PIDParams
     * @brief Parameters for the PID model.
     */
    class PIDParams : public KModelParams {
      public:
        /**
         * @brief Constructor
         * @param name - Name of the params.
         */
        PIDParams(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;

        /// Derivative gain
        double kd;

        /// Integral gain
        double ki;
    };

    /**
     * @class PID
     * @brief Simple PID controller model.
     */
    class PID : public KModel<PID, PIDParams> {
      public:
        /**
         * @brief PID constructor. The constructor is not meant to be called directly.
         *        Please use the create(...) method instead to create an instance.
         *
         * The PID 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 PID controller to.
         * @param q_traj A ProfileGenerator that defines the reference trajectory for Q.
         * @param u_traj A ProfileGenerator that defines the reference trajectory for U.
         */
        PID(std::string_view name,
            const kc::ks_ptr<kd::ModelManager> &mm,
            const kc::ks_ptr<kd::PhysicalSubhinge> &sh,
            const kc::ks_ptr<ProfileGenerator<km::Vec>> &q_traj,
            const kc::ks_ptr<ProfileGenerator<km::Vec>> &u_traj);

        /**
         * @brief Destructor for PID.
         */
        ~PID();

        /**
         * @brief Constructor. The PID 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 PID controller to.
         * @param q_traj A ProfileGenerator that defines the reference trajectory for Q.
         * @param u_traj A ProfileGenerator that defines the reference trajectory for U.
         * @return A ks_ptr to the newly created instance of the PID model.
         */
        static kc::ks_ptr<PID> 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::Vec>> &q_traj,
                                      const kc::ks_ptr<ProfileGenerator<km::Vec>> &u_traj);

        /**
         * @brief Calculate and apply the output of the PID 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);

        /// The ProfileGenerator that defines the reference trajectory for Q
        kc::ks_ptr<ProfileGenerator<km::Vec>> q_traj;

        /// The ProfileGenerator that defines the reference trajectory for U
        kc::ks_ptr<ProfileGenerator<km::Vec>> u_traj;

      private:
        /// The subhinge to apply the PID output to.
        kc::ks_ptr<kd::PhysicalSubhinge> _sh;

        /// Tracks the error state of the model
        km::Vec _e_prev;

        /// The integral control contribution
        km::Vec _integral_contrib;

        /// The previous time.
        km::Ktime _prev_t;
    };
} // namespace Karana::Models