Program Listing for File PhysicalSubhinge.h

↰ Return to documentation for file (include/Karana/SOADyn/PhysicalSubhinge.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"Karana/Frame/EdgeFrameToFrame.h"
#include"Karana/SOADyn/SubhingeBase.h"

namespaceKarana::Frame{
classFrame;
}

namespaceKarana::Dynamics{

namespacekc=Karana::Core;
namespacekf=Karana::Frame;

classFramePairHinge;

classPhysicalSubhinge:publickf::EdgeFrameToFrame,publicSubhingeBase{

//foraccesstoid()
friendclassFramePairHinge;

//foraccesstoSubhingeParams
friendclassLoopConstraintHinge;

//foraccessto_computeATBIMatrices()
friendclassHingeOnode;
friendclassPhysicalBody;

//foraccessto_computeATBISmootherVectors()
friendclassHingePnode;

//foraccessto_coriolisAccel
friendclassPhysicalHinge;

//foraccesstoPhysicalSubhingeParams
friendclassOneDofSubhingeData;

//foraccessto_newtonian2oframe
friendclassMultibody;

//foraccesstoATBIMatrices
friendclassSubTree;

//foraccesstoenableChart()
friendclassSubGraph;

//foraccesstogetATBIDinv()
friendclassAlgorithms;
friendclassHingePnode;

public:
virtual~PhysicalSubhinge();

std::string_viewtypeString()constnoexceptoverride{
staticstd::stringtype_string="Karana::Dynamics::PhysicalSubhinge";
returntype_string;
}

std::string_viewname()constoverride{returnkf::EdgeFrameToFrame::name();}

constkc::id_t&id()constoverride{returnkf::EdgeFrameToFrame::id();}

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

virtualconstkm::MatoframeCoordMapMatrix()const=0;

virtualboolsanitizeCoords(){returnfalse;};

virtualboolrequiresCoordSanitization()const{returnfalse;}

virtualvoidresetChart(){};

voidsetPrescribed(boolflag)override;

size_tgetIndex()const{return_index;}

protected:
virtualvoid_enableCoordChart(bool/*flag*/){};

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

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

kc::ks_ptr<PhysicalSubhinge>_getUpstreamSubhinge()const;

km::MatgetUpsilonMatrix()override;

structPhysicalSubhingeParams{
km::VecQ;
km::VecU;
km::VecUdot;//NOLINT(readability-identifier-naming)
km::VecT;
boolprescribed=false;

PhysicalSubhingeParams()=default;
PhysicalSubhingeParams(constPhysicalSubhinge::PhysicalSubhingeParams&p){
Q=p.Q;
U=p.U;
Udot=p.Udot;
T=p.T;
prescribed=p.prescribed;
}

PhysicalSubhingeParams&operator=(constPhysicalSubhinge::PhysicalSubhingeParams&p){
if(this!=&p){
Q=p.Q;
U=p.U;
Udot=p.Udot;
T=p.T;
prescribed=p.prescribed;
}

return*this;
}
};

virtualkc::ks_ptr<PhysicalSubhingeParams>_getParams()const=0;//{returnnullptr;}
virtualvoid_fillCoordParams(PhysicalSubhingeParams&params)const=0;
virtualvoid_setParams(constPhysicalSubhingeParams&params)=0;

virtualvoid_setReversedParams(constPhysicalSubhingeParams&params)=0;

virtualkm::Mat66_computeATBIMatrices(constkm::Mat66&P)=0;
virtualkm::SpatialVector_computeATBIFilterVectors(constkm::SpatialVector&z)=0;
virtualvoid_computeInvDynGenForce(constkm::SpatialVector&pnode_invdyn_f)=0;

virtualkm::SpatialVector_computeATBISmootherVectors(constkm::SpatialVector&alpha)=0;

virtualkm::Mat66_computeUpsilonMatrices(constkm::Mat66&upsilon_plus_oframe)=0;

km::SpatialVector_coriolisAccel()const;

PhysicalSubhinge(kc::ks_ptr<kf::Frame>oframe,
kc::ks_ptr<kf::Frame>pframe,
std::string_viewname,
kc::ks_ptr<FramePairHinge>hge);

protected:
kc::ks_ptr<kf::OrientedChainedFrameToFrame>_oframe2pnode_f2f=nullptr;

kc::ks_ptr<kf::OrientedChainedFrameToFrame>_newtonian2oframe_f2f=nullptr;
kc::ks_ptr<kf::OrientedChainedFrameToFrame>_newtonian2pframe_f2f=nullptr;

km::Mat66_pframe_Upsilon;

size_t_index=std::numeric_limits<std::size_t>::max();
};

template<intNQ,intNU>
classPhysicalSubhinge_T:publicPhysicalSubhinge{//NOLINT(readability-identifier-naming)

//foraccessto
friendclassAlgorithms;

public:
virtual~PhysicalSubhinge_T();

km::MatgetATBIMatPsi()constoverride;
km::MatgetATBID()constoverride;
km::MatgetATBIDinv()constoverride;
km::MatgetATBIG()constoverride;
km::MatgetATBITauper()constoverride;
km::MatpframeCoordMapMatrix()constoverride;

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

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

km::Matjacobian(constkf::Frame&target,booloriented=true)constoverride;

km::MatjacobianDot(constkf::Frame&target,booloriented=true)constoverride;

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

constkm::MatoframeCoordMapMatrix()constoverride{
return_oframe_coord_map_matrix_cache->get();
}

std::string_viewtypeString()constnoexceptoverride{
staticstd::stringtype_string=
std::format("Karana::Dynamics::PhysicalSubhinge_T<{}>",NU);
returntype_string;
}

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

usingkc::Base::dump;

protected:
PhysicalSubhinge_T(kc::ks_ptr<kf::Frame>oframe,
kc::ks_ptr<kf::Frame>pframe,
std::string_viewname,
kc::ks_ptr<FramePairHinge>hge,
size_tn_q,
size_tn_u);

km::Mat66_computeATBIMatrices(constkm::Mat66&P)override;
km::SpatialVector_computeATBIFilterVectors(constkm::SpatialVector&z)override;
km::SpatialVector_computeATBISmootherVectors(constkm::SpatialVector&alpha)override;

km::Mat66_computeUpsilonMatrices(constkm::Mat66&upsilon_plus_oframe)override;

usingbody_6_NU_Mat=std::conditional_t<NU==1,
Eigen::Vector<double,6>,
Eigen::Matrix<double,6,NU,Eigen::RowMajor>>;

structATBIMatrices:publicCoordBase::ATBIMatrices{

km::Mat66P;

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

public:
Eigen::Matrix<double,NU,6,Eigen::RowMajor>
HP;//NOLINT(readability-identifier-naming)

Eigen::Matrix<double,NU,NU,Eigen::RowMajor>D;

Eigen::Matrix<double,NU,NU,Eigen::RowMajor>
Dinv;//NOLINT(readability-identifier-naming)

body_6_NU_MatG;

km::Mat66tauper;

km::Mat66psi;
};

structATBIFilterVectors:publicCoordBase::ATBIFilterVectors{

public:
km::SpatialVectorz;

km::SpatialVectorzplus;

public:
km::Vecepsilon;

km::Vecnu;

public:
voidresize(size_tn_u){
ifconstexpr(NU==Eigen::Dynamic){
epsilon.resize(n_u);
nu.resize(n_u);
km::uninitialize(epsilon);
km::uninitialize(nu);
}
}
};

void_computeATBICoordMapMatrix(body_6_NU_Mat&val)const;

constbody_6_NU_Mat&_atbiCoordMapMatrix()const{
return_atbi_coord_map_matrix_cache->get();
}

constbody_6_NU_Mat&_oframeCoordMapMatrix()const{
return_oframe_coord_map_matrix_cache->get();
}

void_computeInvDynGenForce(constkm::SpatialVector&pnode_invdyn_f)override;

void_computeTransform(km::HomTran&)override;
void_computeVelocity(km::SpatialVector&val)override;
void_computeAccel(km::SpatialVector&val)override;

virtualkc::ks_ptr<PhysicalSubhingeParams>
_getParams()constoverride=0;//{returnnullptr;}
void_fillCoordParams(PhysicalSubhingeParams&params)constoverride;

virtualvoid_setParams(constPhysicalSubhingeParams&params)override;

void_setReversedParams(constPhysicalSubhingeParams&params)override;

protected:
kc::ks_ptr<kc::DataCache<body_6_NU_Mat>>_atbi_coord_map_matrix_cache=nullptr;

kc::ks_ptr<kc::DataCache<body_6_NU_Mat>>_oframe_coord_map_matrix_cache=nullptr;
};

//externtemplateclassPhysicalSubhinge_T<0,0>;
externtemplateclassPhysicalSubhinge_T<1, 1>;
externtemplateclassPhysicalSubhinge_T<3, 3>;
externtemplateclassPhysicalSubhinge_T<4, 3>;

}//namespaceKarana::Dynamics