Program Listing for File ConstraintKinematicsSolver.h

↰ Return to documentation for file (include/Karana/SOADyn/ConstraintKinematicsSolver.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<string>
#include<unordered_map>
#include<vector>

#include"JacobianGenerator.h"
#include"Karana/Frame/Frame.h"
#include"Karana/KCore/LockingBase.h"
#include"Karana/Math/NonlinearSolver.h"

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

namespaceKarana::Dynamics{

template<int,int,int>classCoordBase_T;
classCoordData;
//classJacobianGenerator;
classLoopConstraint;
classSubGraph;

classConstraintKinematicsSolver:publickc::LockingBase{

/*foraccessto_velJacobian()*/
friendclassAlgorithms;

public:
statickc::ks_ptr<ConstraintKinematicsSolver>create(conststd::string&name,
kc::ks_ptr<SubGraph>sg);

ConstraintKinematicsSolver(conststd::string&name,kc::ks_ptr<SubGraph>sg);

~ConstraintKinematicsSolver();

//----------------------------

doublesolveQ();

doublesolveU();

doublesolveUdot();

#if0
kc::ks_ptr<MultiJacobianGenerator>jacobianGenerator()const{
return_hinge_loops_multi_jacgen;
}
#endif

voidfreezeCoord(kc::ks_ptr<CoordBase>coord,size_tindex);

voidunfreezeCoord(kc::ks_ptr<CoordBase>coord,size_tindex);

voidclearFrozenCoords();

std::vector<unsignedint>frozenCoords()const;

size_tnonfrozenNU()const;

km::VecgetNonfrozenQ()const;

voidsetNonfrozenQ(constEigen::Ref<constkm::Vec>&Q);

km::VecgetNonfrozenU()const;

voidsetNonfrozenU(constEigen::Ref<constkm::Vec>&U);

km::VecgetNonfrozenUdot()const;

voidsetNonfrozenUdot(constEigen::Ref<constkm::Vec>&Udot);

voiddump(conststd::string&prefix="")const;

boolcheckVelJacobian(doubletolerance=1e-6)const;

protected:
kc::ks_ptr<km::NonlinearSolver>nlSolver(){return_nl_solver;}

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

void_makeCurrent()override;

void_setup();

void_makeNlSolver();

voidupdateDependentCoordsQ();

//void_selectPoseColumns();

km::Mat_velJacobianNumDiff()const;

void_recomputeNonFrozenCoord();

protected:
kc::ks_ptr<SubGraph>_sg;

kc::ks_ptr<MultiJacobianGenerator>_hinge_loops_multi_jacgen=nullptr;

kc::ks_ptr<MultiJacobianGenerator>_non_hinge_loops_multi_jacgen=nullptr;

kc::ks_ptr<km::NonlinearSolver>_nl_solver=nullptr;

std::vector<unsignedint>_Q_dependent_coord_indices;

std::vector<unsignedint>_frozen_indices;

std::vector<unsignedint>_nonfrozen_indices;

//std::vector<kc::ks_ptr<CoordBase>>_frozen_coords_for_kin;

//size_t_nU_nonfrozen=0;
};

}//namespaceKarana::Dynamics