ResoLoopCutDistObsrvs.hh

 
//
// Copyright (C) 2011-2022 Yokokura, Yuki
// This program is free software;
// you can redistribute it and/or modify it under the terms of the FreeBSD License.
// For details, see the License.txt file.
 
#ifndef RESOLOOPCUTDISTOBSRVS
#define RESOLOOPCUTDISTOBSRVS
 
#include <cassert>
#include <array>
#include "ResoLoopCutDistObsrv.hh"
 
// ARCS組込み用マクロ
#ifdef ARCS_IN
    // ARCSに組み込まれる場合
    #include "ARCSassert.hh"
    #include "ARCSeventlog.hh"
#else
    // ARCSに組み込まれない場合
    #define arcs_assert(a) (assert(a))
    #define PassedLog()
    #define EventLog(a)
    #define EventLogVar(a)
#endif
 
namespace ARCS {    // ARCS名前空間
template <size_t N>
class ResoLoopCutDistObsrvs {
    public:
        ResoLoopCutDistObsrvs(const std::array<struct TwoInertiaParams, N>& Params, const double Bandwidth, const double SmplTime)
            : RLCDObs()
        {
            for(size_t i = 0; i < N; ++i) RLCDObs.at(i).SetParameters(Params.at(i), Bandwidth, SmplTime);
            PassedLog();
        }
 
        ResoLoopCutDistObsrvs(ResoLoopCutDistObsrvs&& r)
            : RLCDObs(r.RLCDObs)
        {
            
        }
        
        ~ResoLoopCutDistObsrvs(){
            PassedLog();
        }
        
        void SetBandwidth(const double Bandwidth){
            for(size_t i = 0; i < N; ++i) RLCDObs.at(i).SetBandwidth(Bandwidth);
        }
        
        void GetCompTorque(const Matrix<1,N>& TorqueRef, const Matrix<1,N>& TorsionTorque, Matrix<1,N>& CompTorque){
            for(size_t i = 1; i <= N; ++i) CompTorque[i] = RLCDObs.at(i - 1).GetCompTorque(TorqueRef[i], TorsionTorque[i]);
        }
        
        Matrix<1,N> GetCompTorque(const Matrix<1,N>& TorqueRef, const Matrix<1,N>& TorsionTorque){
            Matrix<1,N> ret;
            GetCompTorque(TorqueRef, TorsionTorque, ret);
            return ret;
        }
        
        void GetPDgainForRLCTTC(const double wt, const double zt, Matrix<1,N>& Kpt, Matrix<1,N>& Kdt){
            for(size_t i = 1; i <= N; ++i) RLCDObs.at(i - 1).GetPDgainForRLCTTC(wt, zt, Kpt[i], Kdt[i]);
        }
        
    private:
        ResoLoopCutDistObsrvs(const ResoLoopCutDistObsrvs&) = delete;                   
        const ResoLoopCutDistObsrvs& operator=(const ResoLoopCutDistObsrvs&) = delete;  
        std::array<ResoLoopCutDistObsrv, N> RLCDObs;                                    
};
}
 
#endif