From c3c6903f1f06c0d062be71bd689179d6d3b9ee8e Mon Sep 17 00:00:00 2001 From: Aik-Siong Koh Date: Wed, 3 Apr 2024 13:56:16 -0600 Subject: [PATCH] setmotionJoint and TranslationLimit --- OndselSolver/ASMTAssembly.cpp | 37 +++++++++++++++++++++++++++++ OndselSolver/ASMTAssembly.h | 1 + OndselSolver/ASMTLimit.cpp | 5 ++++ OndselSolver/ASMTLimit.h | 1 + OndselSolver/AngleZIecJec.cpp | 3 +++ OndselSolver/DispCompIecJecKeqc.cpp | 1 + OndselSolver/TranslationLimitIJ.cpp | 2 +- testapp/OndselSolver.cpp | 1 + 8 files changed, 50 insertions(+), 1 deletion(-) diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index 541e108..c21c3e9 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -480,6 +480,43 @@ void MbD::ASMTAssembly::runDraggingTest() assembly->runPostDrag(); //Do this after last drag } +void MbD::ASMTAssembly::runDraggingTest2() +{ + //auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt"); + auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt"); + + auto limit1 = ASMTRotationLimit::With(); + limit1->setName("Limit1"); + limit1->setmotionJoint("/Assembly1/Joint1"); + limit1->settype("=>"); + limit1->setlimit("0.0*pi/180.0"); + limit1->settol("1.0e-9"); + assembly->addLimit(limit1); + + auto limit2 = ASMTTranslationLimit::With(); + limit2->setName("Limit2"); + limit2->setmotionJoint("/Assembly1/Joint4"); + limit2->settype("=<"); + limit2->setlimit("0.0"); + limit2->settol("1.0e-9"); + assembly->addLimit(limit2); + + auto& dragPart = assembly->parts->at(0); + auto dragParts = std::make_shared>>(); + dragParts->push_back(dragPart); + assembly->runPreDrag(); //Do this before first drag + FColDsptr pos3D, delta; + pos3D = dragPart->position3D; + delta = std::make_shared>(ListD{ 0.1, 0.2, 0.3 }); + dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta)); + assembly->runDragStep(dragParts); + pos3D = dragPart->position3D; + delta = std::make_shared>(ListD{ 0.3, 0.2, 0.1 }); + dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta)); + assembly->runDragStep(dragParts); + assembly->runPostDrag(); //Do this after last drag +} + void MbD::ASMTAssembly::readWriteFile(const char* fileName) { std::ifstream stream(fileName); diff --git a/OndselSolver/ASMTAssembly.h b/OndselSolver/ASMTAssembly.h index f867d7b..98c750f 100644 --- a/OndselSolver/ASMTAssembly.h +++ b/OndselSolver/ASMTAssembly.h @@ -42,6 +42,7 @@ namespace MbD { static std::shared_ptr assemblyFromFile(const char* chars); static void runFile(const char* chars); static void runDraggingTest(); + static void runDraggingTest2(); static void readWriteFile(const char* chars); void initialize() override; ASMTAssembly* root() override; diff --git a/OndselSolver/ASMTLimit.cpp b/OndselSolver/ASMTLimit.cpp index efd8059..e4ae38e 100644 --- a/OndselSolver/ASMTLimit.cpp +++ b/OndselSolver/ASMTLimit.cpp @@ -87,6 +87,11 @@ void MbD::ASMTLimit::createMbD(std::shared_ptr mbdSys, std::shared_ptrtol = geotol->getValue(); } +void MbD::ASMTLimit::setmotionJoint(std::string _motionJoint) +{ + motionJoint = _motionJoint; +} + void MbD::ASMTLimit::settype(std::string _type) { type = _type; diff --git a/OndselSolver/ASMTLimit.h b/OndselSolver/ASMTLimit.h index fc50c62..83bdcd4 100644 --- a/OndselSolver/ASMTLimit.h +++ b/OndselSolver/ASMTLimit.h @@ -24,6 +24,7 @@ namespace MbD { void readTol(std::vector& lines); void parseASMT(std::vector& lines) override; void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + void setmotionJoint(std::string _motionJoint); void settype(std::string _type); void setlimit(std::string _limit); void settol(std::string _tol); diff --git a/OndselSolver/AngleZIecJec.cpp b/OndselSolver/AngleZIecJec.cpp index 8e6c583..e426bf0 100644 --- a/OndselSolver/AngleZIecJec.cpp +++ b/OndselSolver/AngleZIecJec.cpp @@ -10,6 +10,7 @@ #include "AngleZIecJec.h" #include "Numeric.h" +#include using namespace MbD; @@ -30,6 +31,8 @@ void MbD::AngleZIecJec::calcPostDynCorrectorIteration() auto sumOfSquaresSquared = sumOfSquares * sumOfSquares; auto thez0to2pi = Numeric::arcTan0to2piYoverX(sthez, cthez); thez = std::round((thez - thez0to2pi) / (2.0 * OS_M_PI)) * (2.0 * OS_M_PI) + thez0to2pi; + //std::cout << "AngleZIecJec thez = " << thez << std::endl; + cosOverSSq = cthez / sumOfSquares; sinOverSSq = sthez / sumOfSquares; twoCosSinOverSSqSq = 2.0 * cthez * sthez / sumOfSquaresSquared; diff --git a/OndselSolver/DispCompIecJecKeqc.cpp b/OndselSolver/DispCompIecJecKeqc.cpp index 56212bd..a365064 100644 --- a/OndselSolver/DispCompIecJecKeqc.cpp +++ b/OndselSolver/DispCompIecJecKeqc.cpp @@ -38,6 +38,7 @@ void DispCompIecJecKeqc::calcPostDynCorrectorIteration() aAjOKe = efrmKqc->aAjOe(axisK); rIeJeO = frmJqc->rOeO->minusFullColumn(frmIqc->rOeO); riIeJeKe = aAjOKe->dot(rIeJeO); + //std::cout << "DispCompIecJecKeqc riIeJeKe = " << riIeJeKe << std::endl; pAjOKepEKT = efrmKqc->pAjOepET(axisK); ppAjOKepEKpEK = efrmKqc->ppAjOepEpE(axisK); for (size_t i = 0; i < 4; i++) diff --git a/OndselSolver/TranslationLimitIJ.cpp b/OndselSolver/TranslationLimitIJ.cpp index d1b301b..a59d0e3 100644 --- a/OndselSolver/TranslationLimitIJ.cpp +++ b/OndselSolver/TranslationLimitIJ.cpp @@ -14,7 +14,7 @@ std::shared_ptr MbD::TranslationLimitIJ::With() void MbD::TranslationLimitIJ::initializeGlobally() { if (constraints->empty()) { - auto transConIJ = TranslationConstraintIJ::With(frmJ, frmI, 2); + auto transConIJ = TranslationConstraintIJ::With(frmI, frmJ, 2); transConIJ->setConstant(limit); addConstraint(transConIJ); this->root()->hasChanged = true; diff --git a/testapp/OndselSolver.cpp b/testapp/OndselSolver.cpp index 91e3a0c..32ad321 100644 --- a/testapp/OndselSolver.cpp +++ b/testapp/OndselSolver.cpp @@ -26,6 +26,7 @@ void sharedptrTest(); int main() { + ASMTAssembly::runDraggingTest2(); ASMTAssembly::runDraggingTest(); //ASMTAssembly::runFile("../testapp/pistonWithLimits.asmt"); ASMTAssembly::runFile("../testapp/pistonAllowZRotation.asmt");