From 994f90d7ae6fa4055446e4808b2cc45f05825ccb Mon Sep 17 00:00:00 2001 From: Aik-Siong Koh Date: Wed, 3 Apr 2024 13:56:16 -0600 Subject: [PATCH 1/2] 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"); From abd5d4af44895bc062ca868cc694d9aa43974903 Mon Sep 17 00:00:00 2001 From: Aik-Siong Koh Date: Fri, 5 Apr 2024 15:41:20 -0600 Subject: [PATCH 2/2] updateMbDFromRotationMatrix --- OndselSolver/ASMTSpatialContainer.cpp | 25 +++ OndselSolver/ASMTSpatialContainer.h | 173 ++++++++++--------- OndselSolver/PosICDragLimitNewtonRaphson.cpp | 7 +- OndselSolver/ScrewConstraintIJ.cpp | 4 +- 4 files changed, 123 insertions(+), 86 deletions(-) diff --git a/OndselSolver/ASMTSpatialContainer.cpp b/OndselSolver/ASMTSpatialContainer.cpp index 7492d91..c71e156 100644 --- a/OndselSolver/ASMTSpatialContainer.cpp +++ b/OndselSolver/ASMTSpatialContainer.cpp @@ -301,6 +301,31 @@ void MbD::ASMTSpatialContainer::updateMbDFromPosition3D(FColDsptr vec) mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length)); } +void MbD::ASMTSpatialContainer::updateMbDFromPosition3D(double a, double b, double c) +{ + auto pos3D = std::make_shared>(ListD{ a, b, c }); + updateMbDFromPosition3D(pos3D); +} + +void MbD::ASMTSpatialContainer::updateMbDFromRotationMatrix(FMatDsptr mat) +{ + rotationMatrix = mat; + auto mbdPart = std::static_pointer_cast(mbdObject); + auto mbdUnits = this->mbdUnits(); + mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length)); + mbdPart->qE(qEp()); +} + +void MbD::ASMTSpatialContainer::updateMbDFromRotationMatrix(double v11, double v12, double v13, double v21, double v22, double v23, double v31, double v32, double v33) +{ + auto rotMat = std::make_shared>(ListListD{ + { v11, v12, v13 }, + { v21, v22, v23 }, + { v31, v32, v33 } + }); + updateMbDFromRotationMatrix(rotMat); +} + FColDsptr MbD::ASMTSpatialContainer::rOcmO() { auto& rOPO = position3D; diff --git a/OndselSolver/ASMTSpatialContainer.h b/OndselSolver/ASMTSpatialContainer.h index ac86254..b0452ad 100644 --- a/OndselSolver/ASMTSpatialContainer.h +++ b/OndselSolver/ASMTSpatialContainer.h @@ -5,13 +5,13 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include "ASMTSpatialItem.h" -//#include "ASMTRefPoint.h" -//#include "ASMTRefCurve.h" -//#include "ASMTRefSurface.h" + //#include "ASMTRefPoint.h" + //#include "ASMTRefCurve.h" + //#include "ASMTRefSurface.h" #include "ASMTPrincipalMassMarker.h" //#include "Units.h" //#include "ASMTPart.h" @@ -19,88 +19,93 @@ //#include "ASMTMotion.h" namespace MbD { - class ASMTRefPoint; - class ASMTRefCurve; - class ASMTRefSurface; - class ASMTPrincipalMassMarker; - class Units; - class ASMTPart; - class ASMTJoint; - class ASMTMotion; - class ASMTMarker; + class ASMTRefPoint; + class ASMTRefCurve; + class ASMTRefSurface; + class ASMTPrincipalMassMarker; + class Units; + class ASMTPart; + class ASMTJoint; + class ASMTMotion; + class ASMTMarker; - class ASMTSpatialContainer : public ASMTSpatialItem - { - // - public: - ASMTSpatialContainer(); - void initialize() override; - void setPrincipalMassMarker(std::shared_ptr aJ); - void readRefPoints(std::vector& lines); - void readRefPoint(std::vector& lines); - void readRefCurves(std::vector& lines); - void readRefCurve(std::vector& lines); - void readRefSurfaces(std::vector& lines); - void readRefSurface(std::vector& lines); - void readXs(std::vector& lines); - void readYs(std::vector& lines); - void readZs(std::vector& lines); - void readBryantxs(std::vector& lines); - void readBryantys(std::vector& lines); - void readBryantzs(std::vector& lines); - void readVXs(std::vector& lines); - void readVYs(std::vector& lines); - void readVZs(std::vector& lines); - void readOmegaXs(std::vector& lines); - void readOmegaYs(std::vector& lines); - void readOmegaZs(std::vector& lines); - void readAXs(std::vector& lines); - void readAYs(std::vector& lines); - void readAZs(std::vector& lines); - void readAlphaXs(std::vector& lines); - void readAlphaYs(std::vector& lines); - void readAlphaZs(std::vector& lines); - void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; - void updateMbDFromPosition3D(FColDsptr position3D); - FColDsptr rOcmO(); - std::shared_ptr> qEp(); - virtual FColDsptr vOcmO(); - virtual FColDsptr omeOpO(); - ASMTSpatialContainer* partOrAssembly() override; - void updateFromMbD() override; - void compareResults(AnalysisType type) override; - void outputResults(AnalysisType type) override; - void addRefPoint(std::shared_ptr refPoint); - void addMarker(std::shared_ptr marker); - std::string generateUniqueMarkerName(); - std::shared_ptr>> markerList(); - void setVelocity3D(FColDsptr velocity3D); - void setOmega3D(FColDsptr omega3D); - void readVelocity3D(std::vector& lines); - void readOmega3D(std::vector& lines); - void setVelocity3D(double a, double b, double c); - void setOmega3D(double a, double b, double c); - void storeOnLevel(std::ofstream& os, size_t level) override; - void storeOnLevelVelocity(std::ofstream& os, size_t level); - void storeOnLevelOmega(std::ofstream& os, size_t level); - void storeOnLevelRefPoints(std::ofstream& os, size_t level); - void storeOnLevelRefCurves(std::ofstream& os, size_t level); - void storeOnLevelRefSurfaces(std::ofstream& os, size_t level); - void storeOnTimeSeries(std::ofstream& os) override; - FColDsptr getVelocity3D(size_t i); - FColDsptr getOmega3D(size_t i); + class ASMTSpatialContainer : public ASMTSpatialItem + { + // + public: + ASMTSpatialContainer(); + void initialize() override; + void setPrincipalMassMarker(std::shared_ptr aJ); + void readRefPoints(std::vector& lines); + void readRefPoint(std::vector& lines); + void readRefCurves(std::vector& lines); + void readRefCurve(std::vector& lines); + void readRefSurfaces(std::vector& lines); + void readRefSurface(std::vector& lines); + void readXs(std::vector& lines); + void readYs(std::vector& lines); + void readZs(std::vector& lines); + void readBryantxs(std::vector& lines); + void readBryantys(std::vector& lines); + void readBryantzs(std::vector& lines); + void readVXs(std::vector& lines); + void readVYs(std::vector& lines); + void readVZs(std::vector& lines); + void readOmegaXs(std::vector& lines); + void readOmegaYs(std::vector& lines); + void readOmegaZs(std::vector& lines); + void readAXs(std::vector& lines); + void readAYs(std::vector& lines); + void readAZs(std::vector& lines); + void readAlphaXs(std::vector& lines); + void readAlphaYs(std::vector& lines); + void readAlphaZs(std::vector& lines); + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; + void updateMbDFromPosition3D(FColDsptr position3D); + void updateMbDFromPosition3D(double a, double b, double c); + void updateMbDFromRotationMatrix(FMatDsptr mat); + void updateMbDFromRotationMatrix(double v11, double v12, double v13, + double v21, double v22, double v23, + double v31, double v32, double v33); + FColDsptr rOcmO(); + std::shared_ptr> qEp(); + virtual FColDsptr vOcmO(); + virtual FColDsptr omeOpO(); + ASMTSpatialContainer* partOrAssembly() override; + void updateFromMbD() override; + void compareResults(AnalysisType type) override; + void outputResults(AnalysisType type) override; + void addRefPoint(std::shared_ptr refPoint); + void addMarker(std::shared_ptr marker); + std::string generateUniqueMarkerName(); + std::shared_ptr>> markerList(); + void setVelocity3D(FColDsptr velocity3D); + void setOmega3D(FColDsptr omega3D); + void readVelocity3D(std::vector& lines); + void readOmega3D(std::vector& lines); + void setVelocity3D(double a, double b, double c); + void setOmega3D(double a, double b, double c); + void storeOnLevel(std::ofstream& os, size_t level) override; + void storeOnLevelVelocity(std::ofstream& os, size_t level); + void storeOnLevelOmega(std::ofstream& os, size_t level); + void storeOnLevelRefPoints(std::ofstream& os, size_t level); + void storeOnLevelRefCurves(std::ofstream& os, size_t level); + void storeOnLevelRefSurfaces(std::ofstream& os, size_t level); + void storeOnTimeSeries(std::ofstream& os) override; + FColDsptr getVelocity3D(size_t i); + FColDsptr getOmega3D(size_t i); - FColDsptr velocity3D = std::make_shared>(3); - FColDsptr omega3D = std::make_shared>(3); - std::shared_ptr>> refPoints; - std::shared_ptr>> refCurves; - std::shared_ptr>> refSurfaces; - FRowDsptr vxs, vys, vzs, omexs, omeys, omezs; - FRowDsptr axs, ays, azs, alpxs, alpys, alpzs; - FRowDsptr invxs, invys, invzs, inomexs, inomeys, inomezs; - FRowDsptr inaxs, inays, inazs, inalpxs, inalpys, inalpzs; - std::shared_ptr principalMassMarker = ASMTPrincipalMassMarker::With(); + FColDsptr velocity3D = std::make_shared>(3); + FColDsptr omega3D = std::make_shared>(3); + std::shared_ptr>> refPoints; + std::shared_ptr>> refCurves; + std::shared_ptr>> refSurfaces; + FRowDsptr vxs, vys, vzs, omexs, omeys, omezs; + FRowDsptr axs, ays, azs, alpxs, alpys, alpzs; + FRowDsptr invxs, invys, invzs, inomexs, inomeys, inomezs; + FRowDsptr inaxs, inays, inazs, inalpxs, inalpys, inalpzs; + std::shared_ptr principalMassMarker = ASMTPrincipalMassMarker::With(); - }; + }; } diff --git a/OndselSolver/PosICDragLimitNewtonRaphson.cpp b/OndselSolver/PosICDragLimitNewtonRaphson.cpp index 46f66ef..6321914 100644 --- a/OndselSolver/PosICDragLimitNewtonRaphson.cpp +++ b/OndselSolver/PosICDragLimitNewtonRaphson.cpp @@ -3,6 +3,7 @@ #include "SimulationStoppingError.h" #include "Part.h" #include "Constraint.h" +#include using namespace MbD; @@ -38,7 +39,11 @@ void MbD::PosICDragLimitNewtonRaphson::run() preRun(); system->deactivateLimits(); if (system->limitsSatisfied()) return; - for (auto& limit : *system->limits()) { + auto limits = system->limits(); + std::partition(limits->begin(), limits->end(), [](auto limit) { return !limit->satisfied(); }); + //Violated limits are in front. + for (auto it = limits->begin(); it != limits->end(); it++) { + auto limit = *it; limit->activate(); preRun(); initializeLocally(); diff --git a/OndselSolver/ScrewConstraintIJ.cpp b/OndselSolver/ScrewConstraintIJ.cpp index 996253f..04e7617 100644 --- a/OndselSolver/ScrewConstraintIJ.cpp +++ b/OndselSolver/ScrewConstraintIJ.cpp @@ -72,7 +72,9 @@ void MbD::ScrewConstraintIJ::postInput() { zIeJeIe->postInput(); thezIeJe->postInput(); - aConstant = (2.0 * OS_M_PI * zIeJeIe->value()) - (thezIeJe->value() * pitch); + if (aConstant == std::numeric_limits::min()) { + aConstant = (2.0 * OS_M_PI * zIeJeIe->value()) - (thezIeJe->value() * pitch); + } ConstraintIJ::postInput(); }