Program Listing for File CompoundSubhinge.h

↰ Return to documentation for file (include/Karana/SOADyn/CompoundSubhinge.h)

/*
*Copyright(c)2024-2025KaranaDynamicsPtyLtd.Allrightsreserved.
*
*NOTICETOUSER:
*
*Thissourcecodeand/ordocumentation(the"LicensedMaterials")is
*theconfidentialandproprietaryinformationofKaranaDynamicsInc.
*UseoftheseLicensedMaterialsisgovernedbythetermsandconditions
*ofaseparatesoftwarelicenseagreementbetweenKaranaDynamicsandthe
*Licensee("LicenseAgreement").Unlessexpresslypermittedunderthat
*agreement,anyreproduction,modification,distribution,ordisclosure
*oftheLicensedMaterials,inwholeorinpart,toanythirdparty
*withoutthepriorwrittenconsentofKaranaDynamicsisstrictlyprohibited.
*
*THELICENSEDMATERIALSAREPROVIDED"ASIS"WITHOUTWARRANTYOFANYKIND.
*KARANADYNAMICSDISCLAIMSALLWARRANTIES,EXPRESSORIMPLIED,INCLUDING
*BUTNOTLIMITEDTOWARRANTIESOFMERCHANTABILITY,NON-INFRINGEMENT,AND
*FITNESSFORAPARTICULARPURPOSE.
*
*INNOEVENTSHALLKARANADYNAMICSBELIABLEFORANYDAMAGESWHATSOEVER,
*INCLUDINGBUTNOTLIMITEDTOLOSSOFPROFITS,DATA,ORUSE,EVENIF
*ADVISEDOFTHEPOSSIBILITYOFSUCHDAMAGES,WHETHERINCONTRACT,TORT,
*OROTHERWISEARISINGOUTOFORINCONNECTIONWITHTHELICENSEDMATERIALS.
*
*U.S.GovernmentEndUsers:TheLicensedMaterialsarea"commercialitem"
*asdefinedat48C.F.R.2.101,andareprovidedtotheU.S.Government
*onlyasacommercialenditemunderthetermsofthislicense.
*
*AnyuseoftheLicensedMaterialsinindividualorcommercialsoftwaremust
*include,intheuserdocumentationandinternalsourcecodecomments,
*thisNotice,Disclaimer,andU.S.GovernmentUseProvision.
*/


#pragmaonce

#include<map>
#include<unordered_map>
#include<utility>

#include"Karana/KCore/LockingBase.h"
#include"Karana/Math/Defs.h"
#include"Karana/SOADyn/SubhingeBase.h"

namespaceKarana::Dynamics{

namespacekc=Karana::Core;
namespacekm=Karana::Math;

classPhysicalHinge;
classCompoundHinge;
classCompoundBody;
classPhysicalBody;

classCompoundSubhinge:publickc::LockingBase,publicSubhingeBase{

/*foraccesstothe_computeATBI*methods*/
friendclassCompoundBody;

public:
virtual~CompoundSubhinge();

std::string_viewname()constoverride{returnkc::LockingBase::name();}
constkc::id_t&id()constoverride{returnkc::LockingBase::id();}

virtualstd::string_viewtypeString()constnoexceptoverride{
staticstd::stringtype_string="Karana::Dynamics::CompoundSubhinge";
returntype_string;
}

constkm::MatatbiCoordMapMatrix()constoverride{
return_atbi_coord_map_matrix_cache->get();
}

SubhingeBase::SubhingeTypesubhingeType()constoverride;
kc::ks_ptr<kf::FrameToFrame>f2f()constoverride;

std::stringdumpString(std::string_viewprefix,
constKarana::Core::Base::DumpOptions*options)constoverride;

voidsetQ(constEigen::Ref<constkm::Vec>&val)override;
voidsetQ(doublefill_value)override;
constkm::Vec&getQ()constoverride;

voidsetU(constEigen::Ref<constkm::Vec>&val)override;
voidsetU(doublefill_value)override;
constkm::Vec&getU()constoverride;

voidsetT(constEigen::Ref<constkm::Vec>&val)override;
voidsetT(doublefill_value)override;
constkm::Vec&getT()constoverride;

voidsetUdot(constEigen::Ref<constkm::Vec>&val)override;
voidsetUdot(doublefill_value)override;
constkm::Vec&getUdot()constoverride;

constkm::Vec&getQdot()constoverride;

CompoundSubhinge(kc::ks_ptr<CompoundHinge>hge);

protected:
void_localChartSetQ(constEigen::Ref<constkm::Vec>&val)override;
constkm::Vec&_localChartGetQ()constoverride;

structATBIMatrices:publicCoordBase::ATBIMatrices{

km::MatP;

km::MatPplus;//NOLINT(readability-identifier-naming)

km::MatHP;//NOLINT(readability-identifier-naming)

km::MatD;

km::MatDinv;//NOLINT(readability-identifier-naming)

km::MatG;

km::Mattauper;

km::Matpsi;

voidresize(size_thge_n_u,size_tn_bodies){

size_tbody_nU=6*n_bodies;
P.resize(body_nU,body_nU);
P.setZero();
Pplus.resize(body_nU,body_nU);

HP.resize(hge_n_u,body_nU);
D.resize(hge_n_u,hge_n_u);
Dinv.resize(hge_n_u,hge_n_u);
G.resize(body_nU,hge_n_u);
tauper.resize(body_nU,body_nU);
psi.resize(body_nU,body_nU);
km::uninitialize(HP);
km::uninitialize(D);
km::uninitialize(Dinv);
km::uninitialize(G);
km::uninitialize(tauper);
km::uninitialize(psi);
}
};

structATBIFilterVectors:publicCoordBase::ATBIFilterVectors{

public:
km::Vecz;

km::Veczplus;

km::Vecepsilon;

km::Vecnu;

voidresize(size_thge_n_u,size_tn_bodies){
size_tbody_nU=6*n_bodies;
z.resize(body_nU);
zplus.resize(body_nU);
epsilon.resize(hge_n_u);
nu.resize(hge_n_u);
km::uninitialize(epsilon);
km::uninitialize(nu);
}
};

protected:
virtualkm::MatgetUpsilonMatrix()override{returnkm::Mat();};

km::SpatialVector_aprimeAccel(kc::ks_ptr<PhysicalBody>bd)const;

km::Matjacobian(constkf::Frame&/*target*/,bool/*oriented*/)constoverride{
throwstd::runtime_error("jacobianforCompoundSubhingehasnotbeenimpementedyet.");
}

km::MatjacobianDot(constkf::Frame&/*target*/,bool/*oriented*/)constoverride{
throwstd::runtime_error(
"jacobianDotforCompoundSubhingehasnotbeenimpementedyet.");
}

constkm::Mat&_getE_Phi_G()const{return_matE_Phi_G_cache->get();}

void_computeATBICoordMapMatrix(km::Mat&val)const;

void_discard(kc::ks_ptr<kc::Base>&base)override;

km::Mat66_getVrootPnodeATBIMatrixP();

km::SpatialVector_getVrootPnodeATBIFilterZ();

km::SpatialVector_getVrootPnodeInvDynF();

km::SpatialVector_getBaseOnodeATBIFilterZplus(kc::ks_ptr<HingeOnode>base_ond);

void_computeATBIMatrices();
void_computeATBIFilterVectors();
void_computeATBISmootherVectors();
void_computeUpsilonMatrices();

km::SpatialVector_computeAprimeAccel(kc::ks_ptr<PhysicalBody>bd,
kc::ks_ptr<PhysicalBody>prev_bd)const;

km::MatgetATBIMatPsi()constoverride{returnkm::Mat();};

km::MatgetATBID()constoverride;
km::MatgetATBIDinv()constoverride;
km::MatgetATBIG()constoverride{returnkm::Mat();};
km::MatgetATBITauper()constoverride{returnkm::Mat();};

protected:
virtualkm::Vec_getHingeXdotU(PhysicalHinge&/*hge*/)const{returnkm::Vec(0);};

void_computeInvDynGenForce();

kc::ks_ptr<CompoundBody>_parent_body=nullptr;

void_computeE_Phi_G(km::Mat&val);

km::Mat_oframe2pframePsi()constoverride;
km::Mat_oframe2pframePhi()constoverride;

km::Mat_pframe2otherPhi(constkf::Frame&other)constoverride;

protected:
std::map<std::pair<kc::id_t,kc::id_t>,bool>_body_pnodes_orientation_map;

std::unordered_map<kc::id_t,kc::ks_ptr<kc::DataCache<km::SpatialVector>>>_aprime_caches;

kc::ks_ptr<kc::DataCache<km::Mat>>_matE_Phi_G_cache=nullptr;

//kc::ks_ptr<CoordData>_coord_data=nullptr;

size_t_coord_map_rows=0;

kc::ks_ptr<kc::DataCache<km::Mat>>_atbi_coord_map_matrix_cache=nullptr;

protected:
/*
*Wewanttoreturnconstkm::Vec&forthe
*gettermethods.However,weneedtouseCoordData
*forCompoundSubhine.Therefore,weaddtheseasmutable
*variables(sincetheyneedtochangeintheconstfunctions)
*andreturnreferencestothem.
*/

mutablekm::Vec_getQ_cache;

mutablekm::Vec_getQdot_cache;

mutablekm::Vec_getU_cache;

mutablekm::Vec_getUdot_cache;

mutablekm::Vec_getT_cache;

private:
constkm::Mat&_atbiCoordMapMatrix()const{return_atbi_coord_map_matrix_cache->get();}
};

}//namespaceKarana::Dynamics