From 84cb979354b00ec145d1ae2d7510129f774c2878 Mon Sep 17 00:00:00 2001 From: Aik-Siong Koh Date: Fri, 13 Oct 2023 13:01:39 -0600 Subject: [PATCH] crank_slider.mbd MBDyn file working --- OndselSolver/ASMTAssembly.cpp | 113 +++++- OndselSolver/ASMTAssembly.h | 7 +- OndselSolver/ASMTFixedJoint.h | 2 +- OndselSolver/ASMTGeneralMotion.cpp | 18 +- OndselSolver/ASMTGeneralMotion.h | 2 +- OndselSolver/ASMTItem.cpp | 12 +- OndselSolver/ASMTItem.h | 7 +- OndselSolver/ASMTItemIJ.cpp | 10 + OndselSolver/ASMTItemIJ.h | 1 + OndselSolver/ASMTMarker.cpp | 2 +- OndselSolver/ASMTMarker.h | 1 + OndselSolver/ASMTNoRotationJoint.cpp | 17 + .../{MBDynNodes.h => ASMTNoRotationJoint.h} | 12 +- OndselSolver/ASMTPart.cpp | 14 +- OndselSolver/ASMTPart.h | 3 + OndselSolver/ASMTPointInLineJoint.cpp | 17 + ...MBDynElements.h => ASMTPointInLineJoint.h} | 18 +- OndselSolver/ASMTPointInPlaneJoint.h | 2 +- OndselSolver/ASMTRevoluteJoint.h | 2 +- OndselSolver/ASMTRotationalMotion.cpp | 4 +- OndselSolver/ASMTSimulationParameters.cpp | 6 + OndselSolver/ASMTSimulationParameters.h | 1 + OndselSolver/ASMTSpatialContainer.cpp | 60 ++- OndselSolver/ASMTSpatialContainer.h | 5 +- OndselSolver/ASMTSphericalJoint.h | 2 +- OndselSolver/ASMTTranslationalMotion.cpp | 4 +- OndselSolver/ASMTUniversalJoint.h | 2 +- OndselSolver/AnyPosICNewtonRaphson.cpp | 5 +- OndselSolver/Array.h | 4 - OndselSolver/BasicUserFunction.cpp | 1 + OndselSolver/ClassDiagram1.cd | 11 + OndselSolver/CylindricalJoint.cpp | 2 +- OndselSolver/EigenDecomposition.cpp | 1 + .../{MBDynLabels.h => EigenDecomposition.h} | 13 +- OndselSolver/EndFramec.h | 2 +- OndselSolver/EndFrameqct.cpp | 2 +- OndselSolver/EulerAngles.h | 32 ++ OndselSolver/EulerAngleszxz.h | 9 +- OndselSolver/EulerParameters.h | 12 +- OndselSolver/FullMatrix.h | 70 ++++ OndselSolver/FullVector.h | 4 +- OndselSolver/InLineJoint.h | 3 +- OndselSolver/InPlaneJoint.h | 3 +- OndselSolver/Item.cpp | 5 + OndselSolver/Item.h | 1 + OndselSolver/LineInPlaneJoint.cpp | 2 +- OndselSolver/MBDynBody.cpp | 70 +++- OndselSolver/MBDynBody.h | 10 +- OndselSolver/MBDynCase.mbd | 349 ++++++++++++++++++ OndselSolver/MBDynControlData.cpp | 92 ++++- OndselSolver/MBDynControlData.h | 10 + OndselSolver/MBDynData.cpp | 2 +- OndselSolver/MBDynElements.cpp | 37 -- OndselSolver/MBDynInitialValue.cpp | 85 ++++- OndselSolver/MBDynInitialValue.h | 11 +- OndselSolver/MBDynItem.cpp | 298 ++++++++++++--- OndselSolver/MBDynItem.h | 34 +- OndselSolver/MBDynJoint.cpp | 231 +++++++++++- OndselSolver/MBDynJoint.h | 10 +- OndselSolver/MBDynLabels.cpp | 31 -- OndselSolver/MBDynMarker.cpp | 55 +++ .../{MBDynVariables.h => MBDynMarker.h} | 11 +- OndselSolver/MBDynNode.cpp | 35 ++ OndselSolver/MBDynNode.h | 1 + OndselSolver/MBDynNodes.cpp | 28 -- OndselSolver/MBDynReference.cpp | 39 +- OndselSolver/MBDynReference.h | 4 +- OndselSolver/MBDynReferences.cpp | 29 -- OndselSolver/MBDynReferences.h | 24 -- OndselSolver/MBDynStructural.cpp | 64 ++-- OndselSolver/MBDynStructural.h | 9 +- OndselSolver/MBDynSystem.cpp | 286 +++++++++++--- OndselSolver/MBDynSystem.h | 44 ++- OndselSolver/MBDynVariables.cpp | 38 -- OndselSolver/MarkerFrame.h | 4 +- OndselSolver/MomentOfInertiaSolver.cpp | 157 ++++++++ OndselSolver/MomentOfInertiaSolver.h | 50 +++ OndselSolver/OndselSolver.cpp | 21 +- OndselSolver/OndselSolver.vcxproj | 21 +- OndselSolver/OndselSolver.vcxproj.filters | 61 +-- OndselSolver/PlanarJoint.cpp | 2 +- OndselSolver/PointInLineJoint.cpp | 2 +- OndselSolver/PointInPlaneJoint.cpp | 2 +- OndselSolver/PosKineNewtonRaphson.cpp | 10 +- OndselSolver/Solver.cpp | 5 + OndselSolver/Solver.h | 1 + OndselSolver/SymbolicParser.cpp | 7 + OndselSolver/SymbolicParser.h | 1 + OndselSolver/System.cpp | 12 +- OndselSolver/Time.cpp | 4 +- OndselSolver/TranslationalJoint.cpp | 2 +- OndselSolver/crank_slider.mbd | 2 +- OndselSolver/crank_slider.mov | 204 ++++++++++ OndselSolver/crank_sliderX.mbd | 176 +++++++++ OndselSolver/crank_sliderX.mov | 44 +++ 95 files changed, 2693 insertions(+), 558 deletions(-) create mode 100644 OndselSolver/ASMTNoRotationJoint.cpp rename OndselSolver/{MBDynNodes.h => ASMTNoRotationJoint.h} (69%) create mode 100644 OndselSolver/ASMTPointInLineJoint.cpp rename OndselSolver/{MBDynElements.h => ASMTPointInLineJoint.h} (68%) create mode 100644 OndselSolver/ClassDiagram1.cd create mode 100644 OndselSolver/EigenDecomposition.cpp rename OndselSolver/{MBDynLabels.h => EigenDecomposition.h} (74%) create mode 100644 OndselSolver/MBDynCase.mbd delete mode 100644 OndselSolver/MBDynElements.cpp delete mode 100644 OndselSolver/MBDynLabels.cpp create mode 100644 OndselSolver/MBDynMarker.cpp rename OndselSolver/{MBDynVariables.h => MBDynMarker.h} (75%) delete mode 100644 OndselSolver/MBDynNodes.cpp delete mode 100644 OndselSolver/MBDynReferences.cpp delete mode 100644 OndselSolver/MBDynReferences.h delete mode 100644 OndselSolver/MBDynVariables.cpp create mode 100644 OndselSolver/MomentOfInertiaSolver.cpp create mode 100644 OndselSolver/MomentOfInertiaSolver.h create mode 100644 OndselSolver/crank_slider.mov create mode 100644 OndselSolver/crank_sliderX.mbd create mode 100644 OndselSolver/crank_sliderX.mov diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index 8b1eed9..c3b7df0 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -43,6 +43,11 @@ using namespace MbD; +MbD::ASMTAssembly::ASMTAssembly() : ASMTSpatialContainer() +{ + times = std::make_shared>(); +} + void MbD::ASMTAssembly::runSinglePendulumSuperSimplified() { //In this version we skip declaration of variables that don't need as they use default values. @@ -82,6 +87,64 @@ void MbD::ASMTAssembly::runSinglePendulumSuperSimplified() assembly->runKINEMATIC(); } +void MbD::ASMTAssembly::runSinglePendulumSuperSimplified2() +{ + //In this version we skip declaration of variables that don't need as they use default values. + auto assembly = CREATE::With(); + assembly->setName("OndselAssembly"); + + auto mkr = CREATE::With(); + mkr->setName("marker1"); + assembly->addMarker(mkr); + + auto part = CREATE::With(); + part->setName("part1"); + assembly->addPart(part); + + auto marker1 = CREATE::With(); + marker1->setName("FixingMarker"); + part->addMarker(marker1); + + auto marker2 = CREATE::With(); + marker2->setName("marker2"); + marker2->setPosition3D(20.0, 10.0, 0.0); + part->addMarker(marker2); + + auto part2 = CREATE::With(); + part2->setName("part2"); + part2->setPosition3D(20.0, 10.0, 0.0); + assembly->addPart(part2); + + auto marker3 = CREATE::With(); + marker3->setName("marker2"); + marker3->setPosition3D(50.0, 10.0, 0.0); + part2->addMarker(marker3); + + /*Ground joint*/ + auto joint = CREATE::With(); + joint->setName("Joint1"); + joint->setMarkerI("/OndselAssembly/marker1"); + joint->setMarkerJ("/OndselAssembly/part1/FixingMarker"); + assembly->addJoint(joint); + + auto joint2 = CREATE::With(); + joint2->setName("Joint2"); + joint2->setMarkerI("/OndselAssembly/part1/marker2"); + joint2->setMarkerJ("/OndselAssembly/part2/marker2"); + assembly->addJoint(joint2); + + auto simulationParameters = CREATE::With(); + simulationParameters->settstart(0.0); + simulationParameters->settend(0.0); //tstart == tend Initial Conditions only. + simulationParameters->sethmin(1.0e-9); + simulationParameters->sethmax(1.0); + simulationParameters->sethout(0.04); + simulationParameters->seterrorTol(1.0e-6); + assembly->setSimulationParameters(simulationParameters); + + assembly->runKINEMATIC(); +} + void MbD::ASMTAssembly::runSinglePendulumSimplified() { auto assembly = CREATE::With(); @@ -96,7 +159,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified() assembly->setVelocity3D(0, 0, 0); assembly->setOmega3D(0, 0, 0); - auto massMarker = CREATE::With(); + auto massMarker = std::make_shared(); massMarker->setMass(0.0); massMarker->setDensity(0.0); massMarker->setMomentOfInertias(0, 0, 0); @@ -110,7 +173,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified() auto mkr = CREATE::With(); mkr->setName("Marker1"); mkr->setPosition3D(0, 0, 0); - massMarker->setRotationMatrix( + mkr->setRotationMatrix( 1, 0, 0, 0, 1, 0, 0, 0, 1); @@ -127,7 +190,7 @@ void MbD::ASMTAssembly::runSinglePendulumSimplified() part->setOmega3D(0, 0, 0); assembly->addPart(part); - massMarker = CREATE::With(); + massMarker = std::make_shared(); massMarker->setMass(0.2); massMarker->setDensity(10.0); massMarker->setMomentOfInertias(8.3333333333333e-4, 0.016833333333333, 0.017333333333333); @@ -195,7 +258,7 @@ void MbD::ASMTAssembly::runSinglePendulum() auto ome3D = std::make_shared>(ListD{ 0, 0, 0 }); assembly->setOmega3D(ome3D); // - auto massMarker = CREATE::With(); + auto massMarker = std::make_shared(); massMarker->setMass(0.0); massMarker->setDensity(0.0); auto aJ = std::make_shared>(ListD{ 0, 0, 0 }); @@ -220,7 +283,7 @@ void MbD::ASMTAssembly::runSinglePendulum() { 0, 1, 0 }, { 0, 0, 1 } }); - massMarker->setRotationMatrix(rotMat); + mkr->setRotationMatrix(rotMat); assembly->addMarker(mkr); // auto part = CREATE::With(); @@ -240,7 +303,7 @@ void MbD::ASMTAssembly::runSinglePendulum() part->setOmega3D(ome3D); assembly->addPart(part); // - massMarker = CREATE::With(); + massMarker = std::make_shared(); massMarker->setMass(0.2); massMarker->setDensity(10.0); aJ = std::make_shared>(ListD{ 8.3333333333333e-4, 0.016833333333333, 0.017333333333333 }); @@ -729,6 +792,7 @@ void MbD::ASMTAssembly::calcCharacteristicDimensions() auto unitLength = this->calcCharacteristicLength(); auto unitAngle = 1.0; this->mbdUnits = std::make_shared(unitTime, unitMass, unitLength, unitAngle); + this->mbdUnits = std::make_shared(1.0, 1.0, 1.0, 1.0); //for debug } double MbD::ASMTAssembly::calcCharacteristicTime() @@ -861,31 +925,20 @@ void MbD::ASMTAssembly::solve() void MbD::ASMTAssembly::runKINEMATIC() { - auto mbdSystem = CREATE::With(); + auto mbdSystem = std::make_shared(); mbdObject = mbdSystem; mbdSystem->externalSystem->asmtAssembly = this; mbdSystem->runKINEMATIC(mbdSystem); } -// -//void MbD::ASMTAssembly::initprincipalMassMarker() -//{ -// //Choose first refPoint as center of mass -// assert(!refPoints->empty()); -// auto refPoint = refPoints->at(0); -// principalMassMarker = CREATE::With(); -// principalMassMarker->position3D = refPoint->position3D; -// principalMassMarker->rotationMatrix = refPoint->rotationMatrix; -//} void MbD::ASMTAssembly::initprincipalMassMarker() { - principalMassMarker = CREATE::With(); + principalMassMarker = std::make_shared(); principalMassMarker->mass = 0.0; principalMassMarker->density = 0.0; principalMassMarker->momentOfInertias = std::make_shared>(3, 0); //principalMassMarker->position3D = std::make_shared>(3, 0); - //principalMassMarker->rotationMatrix = std::make_shared>(3, 3); - //principalMassMarker->rotationMatrix->identity(); + //principalMassMarker->rotationMatrix = FullMatrix>::identitysptr(3); } std::shared_ptr MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr self, std::string& longname) @@ -961,6 +1014,7 @@ void MbD::ASMTAssembly::updateFromMbD() { ASMTSpatialContainer::updateFromMbD(); times->push_back(asmtTime->getValue()); + std::cout << "Time = " << asmtTime->getValue() << std::endl; for (auto& part : *parts) part->updateFromMbD(); for (auto& joint : *joints) joint->updateFromMbD(); for (auto& motion : *motions) motion->updateFromMbD(); @@ -1016,4 +1070,23 @@ void MbD::ASMTAssembly::setSimulationParameters(std::shared_ptrowner = this; } +std::shared_ptr MbD::ASMTAssembly::partNamed(std::string partName) +{ + auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { + return prt->fullName("") == partName; + }); + auto& part = *it; + return part; +} + +std::shared_ptr MbD::ASMTAssembly::partPartialNamed(std::string partialName) +{ + auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { + auto fullName = prt->fullName(""); + return fullName.find(partialName) != std::string::npos; + }); + auto part = *it; + return part; +} + diff --git a/OndselSolver/ASMTAssembly.h b/OndselSolver/ASMTAssembly.h index 1351147..9ed76cb 100644 --- a/OndselSolver/ASMTAssembly.h +++ b/OndselSolver/ASMTAssembly.h @@ -12,6 +12,7 @@ #include "ASMTSpatialContainer.h" #include "FullColumn.h" #include "FullMatrix.h" +#include "MBDynSystem.h" namespace MbD { class ASMTRefPoint; @@ -35,7 +36,9 @@ namespace MbD { { // public: + ASMTAssembly(); static void runSinglePendulumSuperSimplified(); + static void runSinglePendulumSuperSimplified2(); static void runSinglePendulumSimplified(); static void runSinglePendulum(); static void runFile(const char* chars); @@ -101,6 +104,8 @@ namespace MbD { void addMotion(std::shared_ptr motion); void setConstantGravity(std::shared_ptr constantGravity); void setSimulationParameters(std::shared_ptr simulationParameters); + std::shared_ptr partNamed(std::string partName); + std::shared_ptr partPartialNamed(std::string partialName); std::string notes; std::shared_ptr>> parts = std::make_shared>>(); @@ -115,7 +120,7 @@ namespace MbD { std::shared_ptr> times; std::shared_ptr asmtTime = std::make_shared(); std::shared_ptr mbdUnits; - + MBDynSystem* mbdynItem = nullptr; }; } diff --git a/OndselSolver/ASMTFixedJoint.h b/OndselSolver/ASMTFixedJoint.h index c1c95b0..46d1a37 100644 --- a/OndselSolver/ASMTFixedJoint.h +++ b/OndselSolver/ASMTFixedJoint.h @@ -16,7 +16,7 @@ namespace MbD { { // public: - virtual std::shared_ptr mbdClassNew() override; + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTGeneralMotion.cpp b/OndselSolver/ASMTGeneralMotion.cpp index bc76b11..a30abab 100644 --- a/OndselSolver/ASMTGeneralMotion.cpp +++ b/OndselSolver/ASMTGeneralMotion.cpp @@ -30,7 +30,7 @@ void MbD::ASMTGeneralMotion::parseASMT(std::vector& lines) void MbD::ASMTGeneralMotion::readrIJI(std::vector& lines) { - rIJI = std::make_shared>(3); + rIJI = std::make_shared>(3); assert(lines[0].find("rIJI1") != std::string::npos); lines.erase(lines.begin()); @@ -48,7 +48,7 @@ void MbD::ASMTGeneralMotion::readrIJI(std::vector& lines) void MbD::ASMTGeneralMotion::readangIJJ(std::vector& lines) { - angIJJ = std::make_shared>(3); + angIJJ = std::make_shared>(3); assert(lines[0].find("angIJJ1") != std::string::npos); lines.erase(lines.begin()); @@ -80,7 +80,7 @@ std::shared_ptr MbD::ASMTGeneralMotion::mbdClassNew() void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) { ASMTMotion::createMbD(mbdSys, mbdUnits); - auto parser = CREATE::With(); + auto parser = std::make_shared(); parser->owner = this; auto geoTime = owner->root()->geoTime(); parser->variables->insert(std::make_pair("time", geoTime)); @@ -88,21 +88,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr mbdSys, std::shar auto fullMotion = std::static_pointer_cast(mbdObject); //rIJI - userFunc = CREATE::With(rIJI->at(0), 1.0); + userFunc = std::make_shared(rIJI->at(0), 1.0); parser->parseUserFunction(userFunc); auto geoX = parser->stack->top(); geoX = Symbolic::times(geoX, std::make_shared(1.0 / mbdUnits->length)); geoX->createMbD(mbdSys, mbdUnits); auto xBlk = geoX->simplified(geoX); - userFunc = CREATE::With(rIJI->at(1), 1.0); + userFunc = std::make_shared(rIJI->at(1), 1.0); parser->parseUserFunction(userFunc); auto geoY = parser->stack->top(); geoY = Symbolic::times(geoY, std::make_shared(1.0 / mbdUnits->length)); geoY->createMbD(mbdSys, mbdUnits); auto yBlk = geoY->simplified(geoY); - userFunc = CREATE::With(rIJI->at(2), 1.0); + userFunc = std::make_shared(rIJI->at(2), 1.0); parser->parseUserFunction(userFunc); auto geoZ = parser->stack->top(); geoZ = Symbolic::times(geoZ, std::make_shared(1.0 / mbdUnits->length)); @@ -113,21 +113,21 @@ void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr mbdSys, std::shar fullMotion->frIJI = std::make_shared>(xyzBlkList); //angIJJ - userFunc = CREATE::With(angIJJ->at(0), 1.0); + userFunc = std::make_shared(angIJJ->at(0), 1.0); parser->parseUserFunction(userFunc); auto geoPhi = parser->stack->top(); geoPhi = Symbolic::times(geoPhi, std::make_shared(1.0 / mbdUnits->angle)); geoPhi->createMbD(mbdSys, mbdUnits); auto phiBlk = geoPhi->simplified(geoPhi); - userFunc = CREATE::With(angIJJ->at(1), 1.0); + userFunc = std::make_shared(angIJJ->at(1), 1.0); parser->parseUserFunction(userFunc); auto geoThe = parser->stack->top(); geoThe = Symbolic::times(geoThe, std::make_shared(1.0 / mbdUnits->angle)); geoThe->createMbD(mbdSys, mbdUnits); auto theBlk = geoThe->simplified(geoThe); - userFunc = CREATE::With(angIJJ->at(2), 1.0); + userFunc = std::make_shared(angIJJ->at(2), 1.0); parser->parseUserFunction(userFunc); auto geoPsi = parser->stack->top(); geoPsi = Symbolic::times(geoPsi, std::make_shared(1.0 / mbdUnits->angle)); diff --git a/OndselSolver/ASMTGeneralMotion.h b/OndselSolver/ASMTGeneralMotion.h index 70e2bc0..089b05c 100644 --- a/OndselSolver/ASMTGeneralMotion.h +++ b/OndselSolver/ASMTGeneralMotion.h @@ -22,7 +22,7 @@ namespace MbD { std::shared_ptr mbdClassNew() override; void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; - std::shared_ptr> rIJI, angIJJ; + std::shared_ptr> rIJI, angIJJ; std::string rotationOrder; }; diff --git a/OndselSolver/ASMTItem.cpp b/OndselSolver/ASMTItem.cpp index 5739233..65ee189 100644 --- a/OndselSolver/ASMTItem.cpp +++ b/OndselSolver/ASMTItem.cpp @@ -19,7 +19,12 @@ ASMTAssembly* MbD::ASMTItem::root() return owner->root(); } -std::shared_ptr MbD::ASMTItem::part() +ASMTSpatialContainer* MbD::ASMTItem::partOrAssembly() +{ + return owner->partOrAssembly(); +} + +ASMTPart* MbD::ASMTItem::part() { return owner->part(); } @@ -28,6 +33,11 @@ void MbD::ASMTItem::initialize() { } +void MbD::ASMTItem::noop() +{ + //No Operations +} + void MbD::ASMTItem::setName(std::string str) { name = str; diff --git a/OndselSolver/ASMTItem.h b/OndselSolver/ASMTItem.h index d923d90..07dd5c9 100644 --- a/OndselSolver/ASMTItem.h +++ b/OndselSolver/ASMTItem.h @@ -14,15 +14,18 @@ namespace MbD { class ASMTAssembly; class Units; class ASMTSpatialContainer; + class ASMTPart; class EXPORT ASMTItem { // public: virtual ASMTAssembly* root(); - virtual std::shared_ptr part(); + virtual ASMTSpatialContainer* partOrAssembly(); + virtual ASMTPart* part(); virtual void initialize(); + void noop(); void setName(std::string str); virtual void parseASMT(std::vector& lines); FRowDsptr readRowOfDoubles(std::string& line); @@ -43,7 +46,7 @@ namespace MbD { std::shared_ptr sptrConstant(double value); std::string name; - ASMTItem* owner; + ASMTItem* owner = nullptr; std::shared_ptr mbdObject; }; } diff --git a/OndselSolver/ASMTItemIJ.cpp b/OndselSolver/ASMTItemIJ.cpp index 62f3571..71472e6 100644 --- a/OndselSolver/ASMTItemIJ.cpp +++ b/OndselSolver/ASMTItemIJ.cpp @@ -8,6 +8,16 @@ #include "ASMTItemIJ.h" +MbD::ASMTItemIJ::ASMTItemIJ() +{ + fxs = std::make_shared>(); + fys = std::make_shared>(); + fzs = std::make_shared>(); + txs = std::make_shared>(); + tys = std::make_shared>(); + tzs = std::make_shared>(); +} + void MbD::ASMTItemIJ::initialize() { fxs = std::make_shared>(); diff --git a/OndselSolver/ASMTItemIJ.h b/OndselSolver/ASMTItemIJ.h index 2603c58..87004e0 100644 --- a/OndselSolver/ASMTItemIJ.h +++ b/OndselSolver/ASMTItemIJ.h @@ -15,6 +15,7 @@ namespace MbD { { // public: + ASMTItemIJ(); void initialize() override; void setMarkerI(std::string mkrI); void setMarkerJ(std::string mkrJ); diff --git a/OndselSolver/ASMTMarker.cpp b/OndselSolver/ASMTMarker.cpp index d5f63be..6b0d729 100644 --- a/OndselSolver/ASMTMarker.cpp +++ b/OndselSolver/ASMTMarker.cpp @@ -53,7 +53,7 @@ FMatDsptr MbD::ASMTMarker::aApm() void MbD::ASMTMarker::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) { auto mkr = CREATE::With(name.c_str()); - auto prt = std::static_pointer_cast(part()->mbdObject); + auto prt = std::static_pointer_cast(partOrAssembly()->mbdObject); prt->partFrame->addMarkerFrame(mkr); mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length); diff --git a/OndselSolver/ASMTMarker.h b/OndselSolver/ASMTMarker.h index e68c03e..fc8f7f1 100644 --- a/OndselSolver/ASMTMarker.h +++ b/OndselSolver/ASMTMarker.h @@ -11,6 +11,7 @@ #include "ASMTSpatialItem.h" #include "FullColumn.h" #include "FullMatrix.h" +#include "ASMTPart.h" namespace MbD { class EXPORT ASMTMarker : public ASMTSpatialItem diff --git a/OndselSolver/ASMTNoRotationJoint.cpp b/OndselSolver/ASMTNoRotationJoint.cpp new file mode 100644 index 0000000..446295e --- /dev/null +++ b/OndselSolver/ASMTNoRotationJoint.cpp @@ -0,0 +1,17 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include "ASMTNoRotationJoint.h" +#include "NoRotationJoint.h" + +using namespace MbD; + +std::shared_ptr MbD::ASMTNoRotationJoint::mbdClassNew() +{ + return CREATE::With(); +} diff --git a/OndselSolver/MBDynNodes.h b/OndselSolver/ASMTNoRotationJoint.h similarity index 69% rename from OndselSolver/MBDynNodes.h rename to OndselSolver/ASMTNoRotationJoint.h index cb36b78..d39dbaa 100644 --- a/OndselSolver/MBDynNodes.h +++ b/OndselSolver/ASMTNoRotationJoint.h @@ -7,17 +7,15 @@ ***************************************************************************/ #pragma once -#include "MBDynItem.h" + +#include "ASMTJoint.h" namespace MbD { - class MBDynNode; - - class MBDynNodes : public MBDynItem + class EXPORT ASMTNoRotationJoint : public ASMTJoint { + // public: - void initialize() override; - void parseMBDyn(std::vector& lines) override; + std::shared_ptr mbdClassNew() override; - std::shared_ptr>> nodes; }; } diff --git a/OndselSolver/ASMTPart.cpp b/OndselSolver/ASMTPart.cpp index 28c7c1c..3a2405b 100644 --- a/OndselSolver/ASMTPart.cpp +++ b/OndselSolver/ASMTPart.cpp @@ -9,6 +9,7 @@ #include "ASMTPart.h" #include "CREATE.h" #include "ASMTPrincipalMassMarker.h" +#include "Part.h" using namespace MbD; @@ -55,7 +56,7 @@ void MbD::ASMTPart::readPrincipalMassMarker(std::vector& lines) { assert(lines[0].find("PrincipalMassMarker") != std::string::npos); lines.erase(lines.begin()); - principalMassMarker = CREATE::With(); + principalMassMarker = std::make_shared(); principalMassMarker->parseASMT(lines); principalMassMarker->owner = this; } @@ -104,3 +105,14 @@ FColDsptr MbD::ASMTPart::omeOpO() { return omega3D; } + +ASMTPart* MbD::ASMTPart::part() +{ + return this; +} + +void MbD::ASMTPart::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) +{ + ASMTSpatialContainer::createMbD(mbdSys, mbdUnits); + if (isFixed) std::static_pointer_cast(mbdObject)->asFixed(); +} diff --git a/OndselSolver/ASMTPart.h b/OndselSolver/ASMTPart.h index c26c17f..8a7c8fb 100644 --- a/OndselSolver/ASMTPart.h +++ b/OndselSolver/ASMTPart.h @@ -23,9 +23,12 @@ namespace MbD { void readPartSeries(std::vector& lines); FColDsptr vOcmO() override; FColDsptr omeOpO() override; + ASMTPart* part() override; + void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; //std::shared_ptr>> featureOrder; std::shared_ptr>> partSeries; + bool isFixed = false; }; } diff --git a/OndselSolver/ASMTPointInLineJoint.cpp b/OndselSolver/ASMTPointInLineJoint.cpp new file mode 100644 index 0000000..954361d --- /dev/null +++ b/OndselSolver/ASMTPointInLineJoint.cpp @@ -0,0 +1,17 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include "ASMTPointInLineJoint.h" +#include "PointInLineJoint.h" + +using namespace MbD; + +std::shared_ptr MbD::ASMTPointInLineJoint::mbdClassNew() +{ + return CREATE::With(); +} \ No newline at end of file diff --git a/OndselSolver/MBDynElements.h b/OndselSolver/ASMTPointInLineJoint.h similarity index 68% rename from OndselSolver/MBDynElements.h rename to OndselSolver/ASMTPointInLineJoint.h index b72d81b..4cabf83 100644 --- a/OndselSolver/MBDynElements.h +++ b/OndselSolver/ASMTPointInLineJoint.h @@ -7,17 +7,15 @@ ***************************************************************************/ #pragma once -#include "MBDynBlock.h" + +#include "ASMTJoint.h" namespace MbD { - class MBDynElement; + class EXPORT ASMTPointInLineJoint : public ASMTJoint + { + // + public: + std::shared_ptr mbdClassNew() override; - class MBDynElements : public MBDynBlock - { - public: - void initialize() override; - void parseMBDyn(std::vector& lines) override; - - std::shared_ptr>> elements; - }; + }; } diff --git a/OndselSolver/ASMTPointInPlaneJoint.h b/OndselSolver/ASMTPointInPlaneJoint.h index aab6731..3ae82b7 100644 --- a/OndselSolver/ASMTPointInPlaneJoint.h +++ b/OndselSolver/ASMTPointInPlaneJoint.h @@ -15,7 +15,7 @@ namespace MbD { { // public: - virtual std::shared_ptr mbdClassNew() override; + std::shared_ptr mbdClassNew() override; void parseASMT(std::vector& lines) override; void readOffset(std::vector& lines); void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; diff --git a/OndselSolver/ASMTRevoluteJoint.h b/OndselSolver/ASMTRevoluteJoint.h index 93f0d88..befde2c 100644 --- a/OndselSolver/ASMTRevoluteJoint.h +++ b/OndselSolver/ASMTRevoluteJoint.h @@ -16,7 +16,7 @@ namespace MbD { { // public: - virtual std::shared_ptr mbdClassNew() override; + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTRotationalMotion.cpp b/OndselSolver/ASMTRotationalMotion.cpp index bf0014a..ad01528 100644 --- a/OndselSolver/ASMTRotationalMotion.cpp +++ b/OndselSolver/ASMTRotationalMotion.cpp @@ -50,11 +50,11 @@ void MbD::ASMTRotationalMotion::initMarkers() void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) { ASMTMotion::createMbD(mbdSys, mbdUnits); - auto parser = CREATE::With(); + auto parser = std::make_shared(); parser->owner = this; auto geoTime = owner->root()->geoTime(); parser->variables->insert(std::make_pair("time", geoTime)); - auto userFunc = CREATE::With(rotationZ, 1.0); + auto userFunc = std::make_shared(rotationZ, 1.0); parser->parseUserFunction(userFunc); auto geoPhi = parser->stack->top(); geoPhi = Symbolic::times(geoPhi, std::make_shared(1.0 / mbdUnits->angle)); diff --git a/OndselSolver/ASMTSimulationParameters.cpp b/OndselSolver/ASMTSimulationParameters.cpp index 587a3b1..d866e51 100644 --- a/OndselSolver/ASMTSimulationParameters.cpp +++ b/OndselSolver/ASMTSimulationParameters.cpp @@ -72,3 +72,9 @@ void MbD::ASMTSimulationParameters::seterrorTol(double tol) { errorTol = tol; } + +void MbD::ASMTSimulationParameters::setmaxIter(int maxIter) +{ + iterMaxPosKine = maxIter; + iterMaxAccKine = maxIter; +} diff --git a/OndselSolver/ASMTSimulationParameters.h b/OndselSolver/ASMTSimulationParameters.h index 1e7ad50..620eae1 100644 --- a/OndselSolver/ASMTSimulationParameters.h +++ b/OndselSolver/ASMTSimulationParameters.h @@ -22,6 +22,7 @@ namespace MbD { void sethmax(double hmax); void sethout(double hout); void seterrorTol(double errorTol); + void setmaxIter(int maxIter); double tstart = 0.0, tend = 1.0, hmin = 1.0e-9, hmax = 1.0e9, hout = 0.1, errorTol = 1.0e-6; double errorTolPosKine = 1.0e-6, errorTolAccKine = 1.0e-6, corAbsTol = 1.0e-6, corRelTol = 1.0e-6; diff --git a/OndselSolver/ASMTSpatialContainer.cpp b/OndselSolver/ASMTSpatialContainer.cpp index bbef5f7..3a4578d 100644 --- a/OndselSolver/ASMTSpatialContainer.cpp +++ b/OndselSolver/ASMTSpatialContainer.cpp @@ -13,11 +13,38 @@ #include "ASMTRefPoint.h" #include "ASMTRefCurve.h" #include "ASMTRefSurface.h" +#include //#include "ASMTPrincipalMassMarker.h" using namespace MbD; +MbD::ASMTSpatialContainer::ASMTSpatialContainer() +{ + refPoints = std::make_shared>>(); + refCurves = std::make_shared>>(); + refSurfaces = std::make_shared>>(); + + xs = std::make_shared>(); + ys = std::make_shared>(); + zs = std::make_shared>(); + bryxs = std::make_shared>(); + bryys = std::make_shared>(); + bryzs = std::make_shared>(); + vxs = std::make_shared>(); + vys = std::make_shared>(); + vzs = std::make_shared>(); + omexs = std::make_shared>(); + omeys = std::make_shared>(); + omezs = std::make_shared>(); + axs = std::make_shared>(); + ays = std::make_shared>(); + azs = std::make_shared>(); + alpxs = std::make_shared>(); + alpys = std::make_shared>(); + alpzs = std::make_shared>(); +} + void MbD::ASMTSpatialContainer::initialize() { refPoints = std::make_shared>>(); @@ -294,9 +321,9 @@ FColDsptr MbD::ASMTSpatialContainer::omeOpO() return FColDsptr(); } -std::shared_ptr MbD::ASMTSpatialContainer::part() +ASMTSpatialContainer* MbD::ASMTSpatialContainer::partOrAssembly() { - return std::make_shared(*this); + return this; } void MbD::ASMTSpatialContainer::updateFromMbD() @@ -433,3 +460,32 @@ void MbD::ASMTSpatialContainer::addMarker(std::shared_ptr marker) addRefPoint(refPoint); refPoint->addMarker(marker); } + +std::string MbD::ASMTSpatialContainer::generateUniqueMarkerName() +{ + auto aItemList = markerList(); + auto markerNames = std::vector(); + for (auto& mkr : *aItemList) { + markerNames.push_back(mkr->name); + } + std::stringstream ss; + auto count = 0; + while (true) { + ss.str(""); + ss << "Marker"; + ss << count; + if (std::find(markerNames.begin(), markerNames.end(), ss.str()) == markerNames.end()) break; + count++; + } + return ss.str(); +} + +std::shared_ptr>> MbD::ASMTSpatialContainer::markerList() +{ + auto markers = std::make_shared>>(); + for (auto& refPoint : *refPoints) { + auto refmarkers = refPoint->markers; + markers->insert(markers->end(), refmarkers->begin(), refmarkers->end()); + } + return markers; +} diff --git a/OndselSolver/ASMTSpatialContainer.h b/OndselSolver/ASMTSpatialContainer.h index 0c72483..fbe7323 100644 --- a/OndselSolver/ASMTSpatialContainer.h +++ b/OndselSolver/ASMTSpatialContainer.h @@ -33,6 +33,7 @@ namespace MbD { { // public: + ASMTSpatialContainer(); void initialize() override; void setPrincipalMassMarker(std::shared_ptr aJ); void readRefPoints(std::vector& lines); @@ -64,12 +65,14 @@ namespace MbD { std::shared_ptr> qEp(); virtual FColDsptr vOcmO(); virtual FColDsptr omeOpO(); - std::shared_ptr part() override; + 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(); std::shared_ptr>> refPoints; std::shared_ptr>> refCurves; diff --git a/OndselSolver/ASMTSphericalJoint.h b/OndselSolver/ASMTSphericalJoint.h index de853ca..d975354 100644 --- a/OndselSolver/ASMTSphericalJoint.h +++ b/OndselSolver/ASMTSphericalJoint.h @@ -16,7 +16,7 @@ namespace MbD { { // public: - virtual std::shared_ptr mbdClassNew() override; + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/ASMTTranslationalMotion.cpp b/OndselSolver/ASMTTranslationalMotion.cpp index ababba8..9d425cf 100644 --- a/OndselSolver/ASMTTranslationalMotion.cpp +++ b/OndselSolver/ASMTTranslationalMotion.cpp @@ -49,11 +49,11 @@ void MbD::ASMTTranslationalMotion::initMarkers() void MbD::ASMTTranslationalMotion::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) { ASMTMotion::createMbD(mbdSys, mbdUnits); - auto parser = CREATE::With(); + auto parser = std::make_shared(); parser->owner = this; auto geoTime = owner->root()->geoTime(); parser->variables->insert(std::make_pair("time", geoTime)); - auto userFunc = CREATE::With(translationZ, 1.0); + auto userFunc = std::make_shared(translationZ, 1.0); parser->parseUserFunction(userFunc); auto zIJ = parser->stack->top(); zIJ = Symbolic::times(zIJ, std::make_shared(1.0 / mbdUnits->length)); diff --git a/OndselSolver/ASMTUniversalJoint.h b/OndselSolver/ASMTUniversalJoint.h index 2518a41..87de82c 100644 --- a/OndselSolver/ASMTUniversalJoint.h +++ b/OndselSolver/ASMTUniversalJoint.h @@ -15,7 +15,7 @@ namespace MbD { { // public: - virtual std::shared_ptr mbdClassNew() override; + std::shared_ptr mbdClassNew() override; }; } diff --git a/OndselSolver/AnyPosICNewtonRaphson.cpp b/OndselSolver/AnyPosICNewtonRaphson.cpp index 94854dc..824a0e1 100644 --- a/OndselSolver/AnyPosICNewtonRaphson.cpp +++ b/OndselSolver/AnyPosICNewtonRaphson.cpp @@ -44,9 +44,10 @@ void AnyPosICNewtonRaphson::fillY() y->atiminusFullColumn(0, (qsuWeights->timesFullColumn(newMinusOld))); system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillPosICError(y); - //std::cout << *y << std::endl; + //std::cout << item->name << *y << std::endl; + //noop(); }); - //std::cout << *y << std::endl; + //std::cout << "Final" << *y << std::endl; } void AnyPosICNewtonRaphson::fillPyPx() diff --git a/OndselSolver/Array.h b/OndselSolver/Array.h index 808e366..821ad7e 100644 --- a/OndselSolver/Array.h +++ b/OndselSolver/Array.h @@ -41,11 +41,7 @@ namespace MbD { virtual double maxMagnitude() = 0; double maxMagnitudeOfVector(); void equalArrayAt(std::shared_ptr> array, int i); - //virtual void normalizeSelf(); - //virtual void conditionSelf(); - //virtual void conditionSelfWithTol(double tol); virtual void atiput(int i, T value); - //double length(); void magnifySelf(T factor); void atitimes(int i, double factor); diff --git a/OndselSolver/BasicUserFunction.cpp b/OndselSolver/BasicUserFunction.cpp index f0430ea..da199aa 100644 --- a/OndselSolver/BasicUserFunction.cpp +++ b/OndselSolver/BasicUserFunction.cpp @@ -14,6 +14,7 @@ using namespace MbD; MbD::BasicUserFunction::BasicUserFunction(const std::string& expression, double myUnt) : funcText(expression), myUnit(myUnt) { + units = std::make_shared(); } void MbD::BasicUserFunction::initialize() diff --git a/OndselSolver/ClassDiagram1.cd b/OndselSolver/ClassDiagram1.cd new file mode 100644 index 0000000..54af3c4 --- /dev/null +++ b/OndselSolver/ClassDiagram1.cd @@ -0,0 +1,11 @@ + + + + + + AKAAAgAAAAAAgAAAEAEAAIABAAAAAAAACIAAIAAAAAI= + Solver.h + + + + \ No newline at end of file diff --git a/OndselSolver/CylindricalJoint.cpp b/OndselSolver/CylindricalJoint.cpp index f795a56..8649524 100644 --- a/OndselSolver/CylindricalJoint.cpp +++ b/OndselSolver/CylindricalJoint.cpp @@ -32,6 +32,6 @@ void CylindricalJoint::initializeGlobally() this->root()->hasChanged = true; } else { - InLineJoint::initializeGlobally(); + Joint::initializeGlobally(); } } diff --git a/OndselSolver/EigenDecomposition.cpp b/OndselSolver/EigenDecomposition.cpp new file mode 100644 index 0000000..2e67d64 --- /dev/null +++ b/OndselSolver/EigenDecomposition.cpp @@ -0,0 +1 @@ +#include "EigenDecomposition.h" diff --git a/OndselSolver/MBDynLabels.h b/OndselSolver/EigenDecomposition.h similarity index 74% rename from OndselSolver/MBDynLabels.h rename to OndselSolver/EigenDecomposition.h index e5012e3..102ada2 100644 --- a/OndselSolver/MBDynLabels.h +++ b/OndselSolver/EigenDecomposition.h @@ -7,15 +7,16 @@ ***************************************************************************/ #pragma once -#include "MBDynItem.h" + +#include "MatrixSolver.h" namespace MbD { - class MBDynLabels : public MBDynItem + class EigenDecomposition : public MatrixSolver { - public: - void initialize() override; - void parseMBDyn(std::vector& lines) override; - std::shared_ptr> labels; + // + public: + }; } + diff --git a/OndselSolver/EndFramec.h b/OndselSolver/EndFramec.h index f2e6a93..add9a5a 100644 --- a/OndselSolver/EndFramec.h +++ b/OndselSolver/EndFramec.h @@ -44,7 +44,7 @@ namespace MbD { MarkerFrame* markerFrame; //Use raw pointer when pointing backwards. FColDsptr rOeO = std::make_shared>(3); - FMatDsptr aAOe = std::make_shared>(3, 3); + FMatDsptr aAOe = FullMatrix::identitysptr(3); }; //using EndFrmsptr = std::shared_ptr; } diff --git a/OndselSolver/EndFrameqct.cpp b/OndselSolver/EndFrameqct.cpp index ba9630d..2fc215e 100644 --- a/OndselSolver/EndFrameqct.cpp +++ b/OndselSolver/EndFrameqct.cpp @@ -31,7 +31,7 @@ void EndFrameqct::initialize() rmem = std::make_shared>(3); prmempt = std::make_shared>(3); pprmemptpt = std::make_shared>(3); - aAme = std::make_shared>(3, 3); + aAme = FullMatrix::identitysptr(3); pAmept = std::make_shared>(3, 3); ppAmeptpt = std::make_shared>(3, 3); pprOeOpEpt = std::make_shared>(3, 4); diff --git a/OndselSolver/EulerAngles.h b/OndselSolver/EulerAngles.h index 29b5ab7..a5e1502 100644 --- a/OndselSolver/EulerAngles.h +++ b/OndselSolver/EulerAngles.h @@ -30,6 +30,7 @@ namespace MbD { void initialize() override; void calc() override; std::shared_ptr> differentiateWRT(T var); + void setRotOrder(int i, int j, int k); std::shared_ptr> rotOrder; FColFMatDsptr cA; @@ -64,6 +65,29 @@ namespace MbD { } aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2))); } + template<> + inline void EulerAngles::calc() + { + cA = std::make_shared>(3); + for (int i = 0; i < 3; i++) + { + auto axis = rotOrder->at(i); + auto angle = this->at(i); + if (axis == 1) { + cA->atiput(i, FullMatrix::rotatex(angle)); + } + else if (axis == 2) { + cA->atiput(i, FullMatrix::rotatey(angle)); + } + else if (axis == 3) { + cA->atiput(i, FullMatrix::rotatez(angle)); + } + else { + throw std::runtime_error("Euler angle rotation order must be any permutation of 1,2,3 without consecutive repeats."); + } + } + aA = cA->at(0)->timesFullMatrix(cA->at(1)->timesFullMatrix(cA->at(2))); + } template inline void EulerAngles::calc() { @@ -79,5 +103,13 @@ namespace MbD { derivatives->aEulerAngles = this; return derivatives; } + template + inline void EulerAngles::setRotOrder(int i, int j, int k) + { + rotOrder = std::make_shared>(3); + rotOrder->at(0) = i; + rotOrder->at(1) = j; + rotOrder->at(2) = k; + } } diff --git a/OndselSolver/EulerAngleszxz.h b/OndselSolver/EulerAngleszxz.h index 5e6367d..55dc48f 100644 --- a/OndselSolver/EulerAngleszxz.h +++ b/OndselSolver/EulerAngleszxz.h @@ -29,12 +29,9 @@ namespace MbD { template inline void EulerAngleszxz::initialize() { - phiA = std::make_shared>(3, 3); - phiA->identity(); - theA = std::make_shared>(3, 3); - theA->identity(); - psiA = std::make_shared>(3, 3); - psiA->identity(); + phiA = FullMatrix::identitysptr(3); + theA = FullMatrix::identitysptr(3); + psiA = FullMatrix::identitysptr(3); } template inline void EulerAngleszxz::calc() diff --git a/OndselSolver/EulerParameters.h b/OndselSolver/EulerParameters.h index 3d486c6..7503040 100644 --- a/OndselSolver/EulerParameters.h +++ b/OndselSolver/EulerParameters.h @@ -28,6 +28,16 @@ namespace MbD { EulerParameters(int count) : EulerArray(count) {} EulerParameters(int count, const T& value) : EulerArray(count, value) {} EulerParameters(std::initializer_list list) : EulerArray{ list } {} + EulerParameters(FColDsptr axis, double theta) : EulerArray(4) { + auto halfTheta = theta / 2.0; + auto sinHalfTheta = std::sin(halfTheta); + auto cosHalfTheta = std::cos(halfTheta); + axis->normalizeSelf(); + this->atiputFullColumn(0, axis->times(sinHalfTheta)); + this->atiput(3, cosHalfTheta); + this->conditionSelf(); + this->calc(); + } static std::shared_ptr>> ppApEpEtimesColumn(FColDsptr col); static FMatDsptr pCpEtimesColumn(FColDsptr col); @@ -201,7 +211,7 @@ namespace MbD { template<> inline void EulerParameters::initialize() { - aA = std::make_shared>(3, 3); + aA = FullMatrix::identitysptr(3); aB = std::make_shared>(3, 4); aC = std::make_shared>(3, 4); pApE = std::make_shared>(4); diff --git a/OndselSolver/FullMatrix.h b/OndselSolver/FullMatrix.h index 0b2817e..a5899c0 100644 --- a/OndselSolver/FullMatrix.h +++ b/OndselSolver/FullMatrix.h @@ -12,6 +12,7 @@ #include #include "RowTypeMatrix.h" +#include "FullColumn.h" namespace MbD { template @@ -26,6 +27,9 @@ namespace MbD { class FullRow; template class EulerParameters; + template + class DiagonalMatrix; + using FMatFColDsptr = std::shared_ptr>; using FMatFMatDsptr = std::shared_ptr>; using FColFMatDsptr = std::shared_ptr>; @@ -66,6 +70,8 @@ namespace MbD { static FMatsptr rotatexrotDotrotDDot(T angle, T angleDot, T angleDDot); static FMatsptr rotateyrotDotrotDDot(T angle, T angleDot, T angleDDot); static FMatsptr rotatezrotDotrotDDot(T angle, T angleDot, T angleDDot); + static FMatsptr identitysptr(int n); + static FMatsptr tildeMatrix(FColDsptr col); void identity(); FColsptr column(int j); FColsptr timesFullColumn(FColsptr fullCol); @@ -95,6 +101,9 @@ namespace MbD { T trace(); double maxMagnitude() override; FColsptr bryantAngles(); + bool isDiagonal(); + std::shared_ptr> asDiagonalMatrix(); + void conditionSelfWithTol(double tol); std::ostream& printOn(std::ostream& s) const override; }; @@ -296,6 +305,33 @@ namespace MbD { row2->atiput(2, 0.0); return rotMat; } + template + inline FMatsptr FullMatrix::identitysptr(int n) + { + auto mat = std::make_shared>(n, n); + mat->identity(); + return mat; + } + template + inline FMatsptr FullMatrix::tildeMatrix(FColDsptr col) + { + //"tildeMatrix is skew symmetric matrix related to angular velocity and cross product." + if (col->size() != 3) throw std::runtime_error("Column is not of dimension 3"); + auto tilde = std::make_shared>(3, 3); + auto c0 = col->at(0); + auto c1 = col->at(1); + auto c2 = col->at(2); + tilde->atijput(0, 0, 0.0); + tilde->atijput(1, 1, 0.0); + tilde->atijput(2, 2, 0.0); + tilde->atijput(1, 2, -c0); + tilde->atijput(0, 2, c1); + tilde->atijput(0, 1, -c2); + tilde->atijput(1, 0, c2); + tilde->atijput(2, 0, -c1); + tilde->atijput(2, 1, c0); + return tilde; + } template<> inline void FullMatrix::zeroSelf() { @@ -628,6 +664,40 @@ namespace MbD { return answer; } template + inline bool FullMatrix::isDiagonal() + { + auto m = this->nrow(); + auto n = this->ncol(); + if (m != n) return false; + for (int i = 0; i < m; i++) + { + auto rowi = this->at(i); + for (int j = 0; j < n; j++) + { + if (i != j && rowi->at(j) != 0) return false; + } + } + return true; + } + template + inline std::shared_ptr> FullMatrix::asDiagonalMatrix() + { + int nrow = this->nrow(); + auto diagMat = std::make_shared>(nrow); + for (int i = 0; i < nrow; i++) + { + diagMat->atiput(i, this->at(i)->at(i)); + } + return diagMat; + } + template + inline void FullMatrix::conditionSelfWithTol(double tol) + { + for (auto row : *this) { + row->conditionSelfWithTol(tol); + } + } + template inline FColsptr FullMatrix::timesFullColumn(FColsptr fullCol) { return this->timesFullColumn(fullCol.get()); diff --git a/OndselSolver/FullVector.h b/OndselSolver/FullVector.h index 8959c2b..5e1f9ba 100644 --- a/OndselSolver/FullVector.h +++ b/OndselSolver/FullVector.h @@ -5,7 +5,7 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include @@ -37,7 +37,7 @@ namespace MbD { void normalizeSelf(); double length(); virtual void conditionSelf(); - virtual void conditionSelfWithTol(double tol); + void conditionSelfWithTol(double tol); std::ostream& printOn(std::ostream& s) const override; diff --git a/OndselSolver/InLineJoint.h b/OndselSolver/InLineJoint.h index 8f6a5c6..334b449 100644 --- a/OndselSolver/InLineJoint.h +++ b/OndselSolver/InLineJoint.h @@ -13,10 +13,11 @@ namespace MbD { class InLineJoint : public Joint { - // + //Abstract class. Create subclasses only. public: InLineJoint(); InLineJoint(const char* str); + virtual void initializeGlobally() = 0; //To prevent instantiation of this class void createInLineConstraints(); diff --git a/OndselSolver/InPlaneJoint.h b/OndselSolver/InPlaneJoint.h index 410cf83..a7adfe3 100644 --- a/OndselSolver/InPlaneJoint.h +++ b/OndselSolver/InPlaneJoint.h @@ -17,7 +17,8 @@ namespace MbD { public: InPlaneJoint(); InPlaneJoint(const char* str); - + virtual void initializeGlobally() = 0; //To prevent instantiation of this class + void createInPlaneConstraint(); double offset; diff --git a/OndselSolver/Item.cpp b/OndselSolver/Item.cpp index 820b2e6..577b404 100644 --- a/OndselSolver/Item.cpp +++ b/OndselSolver/Item.cpp @@ -28,6 +28,11 @@ System* Item::root() return owner->root(); } +void MbD::Item::noop() +{ + //No Operations +} + void Item::initialize() { } diff --git a/OndselSolver/Item.h b/OndselSolver/Item.h index 8e846b9..8ab95f8 100644 --- a/OndselSolver/Item.h +++ b/OndselSolver/Item.h @@ -29,6 +29,7 @@ namespace MbD { Item(); Item(const char* str); virtual System* root(); + void noop(); virtual void calcPostDynCorrectorIteration(); virtual void checkForCollisionDiscontinuityBetweenand(double impulsePrevious, double impulse); diff --git a/OndselSolver/LineInPlaneJoint.cpp b/OndselSolver/LineInPlaneJoint.cpp index 8a8bdda..1dd3e40 100644 --- a/OndselSolver/LineInPlaneJoint.cpp +++ b/OndselSolver/LineInPlaneJoint.cpp @@ -29,6 +29,6 @@ void MbD::LineInPlaneJoint::initializeGlobally() this->root()->hasChanged = true; } else { - InPlaneJoint::initializeGlobally(); + Joint::initializeGlobally(); } } diff --git a/OndselSolver/MBDynBody.cpp b/OndselSolver/MBDynBody.cpp index ff17f7c..26e48ef 100644 --- a/OndselSolver/MBDynBody.cpp +++ b/OndselSolver/MBDynBody.cpp @@ -1,9 +1,10 @@ #include "MBDynBody.h" -#include "MBDynReferences.h" #include "MBDynReference.h" +#include "MBDynStructural.h" #include "SymbolicParser.h" #include "BasicUserFunction.h" -#include "MBDynVariables.h" +#include "ASMTPart.h" +#include "ASMTAssembly.h" using namespace MbD; @@ -18,31 +19,76 @@ void MbD::MBDynBody::parseMBDyn(std::string line) auto pos = line.find(":"); auto front = line.substr(previousPos, pos - previousPos); assert(front.find("body") != std::string::npos); - auto arguments = std::make_shared>(); + auto arguments = std::vector(); std::string argument; while (true) { previousPos = pos; pos = line.find(",", pos + 1); if (pos != std::string::npos) { argument = line.substr(previousPos + 1, pos - previousPos - 1); - arguments->push_back(argument); + arguments.push_back(argument); } else { argument = line.substr(previousPos + 1); - arguments->push_back(argument); + arguments.push_back(argument); break; } } - auto iss = std::istringstream(arguments->at(0)); + auto iss = std::istringstream(arguments.at(0)); iss >> name; - arguments->erase(arguments->begin()); - iss = std::istringstream(arguments->at(0)); - iss >> node; - arguments->erase(arguments->begin()); - rOfO = readPosition(arguments); + arguments.erase(arguments.begin()); + iss = std::istringstream(arguments.at(0)); + iss >> nodeName; + arguments.erase(arguments.begin()); + readMass(arguments); + rPcmP = readPosition(arguments); readInertiaMatrix(arguments); } -void MbD::MBDynBody::readInertiaMatrix(std::shared_ptr>& args) +void MbD::MBDynBody::readMass(std::vector& args) { + auto parser = std::make_shared(); + parser->variables = mbdynVariables(); + auto userFunc = std::make_shared(popOffTop(args), 1.0); + parser->parseUserFunction(userFunc); + auto sym = parser->stack->top(); + mass = sym->getValue(); +} + +void MbD::MBDynBody::readInertiaMatrix(std::vector& args) +{ + auto parser = std::make_shared(); + parser->variables = mbdynVariables(); + aJmat = std::make_shared>(3, 3); + auto& str = args.at(0); + if (str.find("diag") != std::string::npos) { + args.erase(args.begin()); + for (int i = 0; i < 3; i++) + { + auto userFunc = std::make_shared(popOffTop(args), 1.0); + parser->parseUserFunction(userFunc); + auto sym = parser->stack->top(); + aJmat->at(i)->at(i) = sym->getValue(); + } + } + else if (str.find("eye") != std::string::npos) { + args.erase(args.begin()); + aJmat->identity(); + } + else { + aJmat = readBasicOrientation(args); + } +} + +void MbD::MBDynBody::createASMT() +{ + auto asmtMassMarker = std::make_shared(); + asmtItem = asmtMassMarker; + asmtMassMarker->setMass(mass); + assert(aJmat->isDiagonal()); + asmtMassMarker->setMomentOfInertias(aJmat->asDiagonalMatrix()); + asmtMassMarker->setPosition3D(rPcmP); + asmtMassMarker->setRotationMatrix(FullMatrix::identitysptr(3)); + auto asmtPart = asmtAssembly()->partPartialNamed(nodeName); + asmtPart->setPrincipalMassMarker(asmtMassMarker); } diff --git a/OndselSolver/MBDynBody.h b/OndselSolver/MBDynBody.h index b1148ee..99afed4 100644 --- a/OndselSolver/MBDynBody.h +++ b/OndselSolver/MBDynBody.h @@ -15,12 +15,14 @@ namespace MbD { public: void initialize() override; void parseMBDyn(std::string line); - void readInertiaMatrix(std::shared_ptr>& args); + void readMass(std::vector& args); + void readInertiaMatrix(std::vector& args); + void createASMT() override; - std::string bodyString, name, node; + std::string bodyString, name, nodeName; double mass; - FColDsptr rOfO; - FMatDsptr aAOf; + FColDsptr rPcmP; + FMatDsptr aJmat; }; } diff --git a/OndselSolver/MBDynCase.mbd b/OndselSolver/MBDynCase.mbd new file mode 100644 index 0000000..f3bb82e --- /dev/null +++ b/OndselSolver/MBDynCase.mbd @@ -0,0 +1,349 @@ +###################################################################################################################### + # MBDyn is a free and open-source general purpose multi-body dynamics software. See https://www.mbdyn.org/ for details. + # This input file was automatically generated by the FreeCAD "MBD workbench". + # To learn more about MBDyn input files, you can visit the website: https://www.sky-engin.jp/en/MBDynTutorial/index.html + # Details about the structure of input files can be studied in the input file manuals: https://www.mbdyn.org/?Software_Download + # Although MBDyn has already reached a mature stage and is used by several industries, + # the FreeCAD dynamics workbench is still under development, and it is likely to have bugs. + # Please be aware of this fact before you use this input file for any critical application. + # If you think you have found a bug or have any suggestion, please send your comments to Jose Egas: + # josegegas@gmail.com + ###################################################################################################################### + + #----------------------------------------------------------------------------- + # [Data Block] + + begin: data; + problem: initial value; + end: data; + + #----------------------------------------------------------------------------- + # [Problem Block] + + begin: initial value; + initial time: 0.0; + final time: 8.0; + time step: 0.01; + max iterations: 100; + tolerance: 1e-06; + derivatives tolerance: 0.0001; + derivatives max iterations: 100; + derivatives coefficient: auto; + end: initial value; + + #----------------------------------------------------------------------------- + # [Control Data Block] + + begin: control data; + max iterations: 1000; + default orientation: euler321; + omega rotates: no; + print: none; + initial stiffness: 1.0, 1.0; + structural nodes: 4; + rigid bodies: 3; + joints: 7; + end: control data; + + #----------------------------------------------------------------------------- + # [Design Variables] + + #Generic bodies + + #body: 2 + set: integer body_2 = 2; #body label + set: real mass_2 = 1.4486361883511716; #mass [kg] + set: real volume_2 = 0.0001833716694115407; #volume [m^3] + + #body: 3 + set: integer body_3 = 3; #body label + set: real mass_3 = 2.85294865570677; #mass [kg] + set: real volume_3 = 0.00036113274122870505; #volume [m^3] + + #body: 4 + set: integer body_4 = 4; #body label + set: real mass_4 = 10.859427202622147; #mass [kg] + set: real volume_4 = 0.0013746110383066007; #volume [m^3] + + #Nodes + + #node: 1 + set: integer structural_node_1 = 1; #node label + + #node: 2 + set: integer structural_node_2 = 2; #node label + + #node: 3 + set: integer structural_node_3 = 3; #node label + + #node: 4 + set: integer structural_node_4 = 4; #node label + + #Joints + + #joint: 1 + set: integer joint_1 = 1; #joint label + + #joint: 2 + set: integer joint_2 = 2; #joint label + + #joint: 3 + set: integer joint_3 = 3; #joint label + + #joint: 4 + set: integer joint_4 = 4; #joint label + + #joint: 5 + set: integer joint_5 = 5; #joint label + + #joint: 6 + set: integer joint_6 = 6; #joint label + + #joint: 7 + set: integer joint_7 = 7; #joint label + + #Nodes: initial conditions + + #node: 1 + set: real Px_1 = -0.121; #X component of the absolute position [m] + set: real Py_1 = -8.796847998598882e-19; #Y component of the absolute position [m] + set: real Pz_1 = -0.08; #Z component of the absolute position [m] + + set: real Vx_1 = 0.0; #X component of the absolute velocity [m/s] + set: real Vy_1 = 0.0; #Y component of the absolute velocity [m/s] + set: real Vz_1 = 0.0; #Z component of the absolute velocity [m/s] + + set: real Wx_1 = 0.0; #X component of the absolute angular velocity [rad/s] + set: real Wy_1 = 0.0; #Y component of the absolute angular velocity [rad/s] + set: real Wz_1 = 0.0; #Z component of the absolute angular velocity [rad/s] + + #node: 2 + set: real Px_2 = -0.015; #X component of the absolute position [m] + set: real Py_2 = 0.092; #Y component of the absolute position [m] + set: real Pz_2 = 0.008; #Z component of the absolute position [m] + + set: real Vx_2 = 0.0; #X component of the absolute velocity [m/s] + set: real Vy_2 = 0.0; #Y component of the absolute velocity [m/s] + set: real Vz_2 = 0.0; #Z component of the absolute velocity [m/s] + + set: real Wx_2 = 0.0; #X component of the absolute angular velocity [rad/s] + set: real Wy_2 = 0.0; #Y component of the absolute angular velocity [rad/s] + set: real Wz_2 = 0.0; #Z component of the absolute angular velocity [rad/s] + + #node: 3 + set: real Px_3 = 0.088; #X component of the absolute position [m] + set: real Py_3 = -0.055; #Y component of the absolute position [m] + set: real Pz_3 = 0.05; #Z component of the absolute position [m] + + set: real Vx_3 = 0.0; #X component of the absolute velocity [m/s] + set: real Vy_3 = 0.0; #Y component of the absolute velocity [m/s] + set: real Vz_3 = 0.0; #Z component of the absolute velocity [m/s] + + set: real Wx_3 = 0.0; #X component of the absolute angular velocity [rad/s] + set: real Wy_3 = 0.0; #Y component of the absolute angular velocity [rad/s] + set: real Wz_3 = 0.0; #Z component of the absolute angular velocity [rad/s] + + #node: 4 + set: real Px_4 = 0.3200000010688326; #X component of the absolute position [m] + set: real Py_4 = 1.4796688528733327e-10; #Y component of the absolute position [m] + set: real Pz_4 = 0.04999999225971574; #Z component of the absolute position [m] + + set: real Vx_4 = 0.0; #X component of the absolute velocity [m/s] + set: real Vy_4 = 0.0; #Y component of the absolute velocity [m/s] + set: real Vz_4 = 0.0; #Z component of the absolute velocity [m/s] + + set: real Wx_4 = 0.0; #X component of the absolute angular velocity [rad/s] + set: real Wy_4 = 0.0; #Y component of the absolute angular velocity [rad/s] + set: real Wz_4 = 0.0; #Z component of the absolute angular velocity [rad/s] + + #----------------------------------------------------------------------------- + # [Intermediate Variables] + + #Moments of inertia and relative center of mass + + #body 2: + set: real Ixx_2 = 0.0012769356301204219; #moment of inertia [kg*m^2] + set: real Ixy_2 = 4.67e-20; #moment of inertia [kg*m^2] + set: real Ixz_2 = -0.0009495625871945231; #moment of inertia [kg*m^2] + + set: real Iyx_2 = 4.67e-20; #moment of inertia [kg*m^2] + set: real Iyy_2 = 0.0028717510150978666; #moment of inertia [kg*m^2] + set: real Iyz_2 = 4.4900000000000004e-20; #moment of inertia [kg*m^2] + + set: real Izx_2 = -0.0009495625871945231; #moment of inertia [kg*m^2] + set: real Izy_2 = 4.4900000000000004e-20; #moment of inertia [kg*m^2] + set: real Izz_2 = 0.002296471669735477; #moment of inertia [kg*m^2] + + set: real Rx_2 = -0.03260146715730948; #X component of the relative center of mass [m] + set: real Ry_2 = 0.0; #Y component of the relative center of mass [m] + set: real Rz_2 = -0.028286762255221056; #Z component of the relative center of mass [m] + + #body 3: + set: real Ixx_3 = 0.0019563103180210077; #moment of inertia [kg*m^2] + set: real Ixy_3 = -7.580852e-16; #moment of inertia [kg*m^2] + set: real Ixz_3 = 5.386829000000001e-16; #moment of inertia [kg*m^2] + + set: real Iyx_3 = -7.580852e-16; #moment of inertia [kg*m^2] + set: real Iyy_3 = 0.03371514809951082; #moment of inertia [kg*m^2] + set: real Iyz_3 = -4.95494e-17; #moment of inertia [kg*m^2] + + set: real Izx_3 = 5.386829000000001e-16; #moment of inertia [kg*m^2] + set: real Izy_3 = -4.95494e-17; #moment of inertia [kg*m^2] + set: real Izz_3 = 0.03383792198797204; #moment of inertia [kg*m^2] + + set: real Rx_3 = 7.87281351222191e-15; #X component of the relative center of mass [m] + set: real Ry_3 = 1.0302869668521452e-15; #Y component of the relative center of mass [m] + set: real Rz_3 = -0.024999999999998045; #Z component of the relative center of mass [m] + + #body 4: + set: real Ixx_4 = 0.07706742098795094; #moment of inertia [kg*m^2] + set: real Ixy_4 = 8.01291908309e-10; #moment of inertia [kg*m^2] + set: real Ixz_4 = -1.2135214817691901e-08; #moment of inertia [kg*m^2] + + set: real Iyx_4 = 8.01291908309e-10; #moment of inertia [kg*m^2] + set: real Iyy_4 = 0.06635181579849883; #moment of inertia [kg*m^2] + set: real Iyz_4 = -3.32678908792e-11; #moment of inertia [kg*m^2] + + set: real Izx_4 = -1.2135214817691901e-08; #moment of inertia [kg*m^2] + set: real Izy_4 = -3.32678908792e-11; #moment of inertia [kg*m^2] + set: real Izz_4 = 0.06179235045624136; #moment of inertia [kg*m^2] + + set: real Rx_4 = 0.045580834634119355; #X component of the relative center of mass [m] + set: real Ry_4 = 2.0299354041266675e-10; #Y component of the relative center of mass [m] + set: real Rz_4 = -1.2562251448855477e-08; #Z component of the relative center of mass [m] + + #----------------------------------------------------------------------------- + # [Nodes Block] + + begin: nodes; + + structural: structural_node_1, + static, + Px_1, Py_1, Pz_1, # [m] + 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, # + Vx_1, Vy_1, Vz_1, # [m/s] + Wx_1, Wy_1, Wz_1; # [rad/s] + + structural: structural_node_2, + dynamic, + Px_2, Py_2, Pz_2, # [m] + 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, # + Vx_2, Vy_2, Vz_2, # [m/s] + Wx_2, Wy_2, Wz_2; # [rad/s] + + structural: structural_node_3, + dynamic, + Px_3, Py_3, Pz_3, # [m] + 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, # + Vx_3, Vy_3, Vz_3, # [m/s] + Wx_3, Wy_3, Wz_3; # [rad/s] + + structural: structural_node_4, + dynamic, + Px_4, Py_4, Pz_4, # [m] + 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, # + Vx_4, Vy_4, Vz_4, # [m/s] + Wx_4, Wy_4, Wz_4; # [rad/s] + + end: nodes; + + #----------------------------------------------------------------------------- + # [Elements Block] + + begin: elements; + + #----------------------------------------------------------------------------- + # [Bodies] + + #IMPORTANT NOTE: FreeCAD provides the inertia matrix in the global reference frame, + #while by default, MBDyn assumes it to be in the reference frame of the node, + #thus, the matrix of inertia must be rotated, using the inverse of the placement + #matrix of the node. This is done using the inertial keyword and the following + #orientation matrix. + + body: body_2, + structural_node_2, # + mass_2, # [kg] + Rx_2, Ry_2, Rz_2, # [m] + Ixx_2, Ixy_2, Ixz_2, # [kg*m^2] + Iyx_2, Iyy_2, Iyz_2, + Izx_2, Izy_2, Izz_2, + orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; + + body: body_3, + structural_node_3, # + mass_3, # [kg] + Rx_3, Ry_3, Rz_3, # [m] + Ixx_3, Ixy_3, Ixz_3, # [kg*m^2] + Iyx_3, Iyy_3, Iyz_3, + Izx_3, Izy_3, Izz_3, + orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; + + body: body_4, + structural_node_4, # + mass_4, # [kg] + Rx_4, Ry_4, Rz_4, # [m] + Ixx_4, Ixy_4, Ixz_4, # [kg*m^2] + Iyx_4, Iyy_4, Iyz_4, + Izx_4, Izy_4, Izz_4, + orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; + + #----------------------------------------------------------------------------- + # [Joints] + + joint: joint_1, + clamp, + structural_node_1, # + -0.121, -8.796847998598882e-19, -0.08, # [m] + 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; # + + joint: joint_2, + spherical hinge, + structural_node_1, # + 0.0, 0.0, 0.0, # [m] + structural_node_2, # + -0.07, 0.0, -0.055; # [m] + + joint: joint_3, + revolute hinge, + structural_node_2, # + 0.0, 0.0, 0.0, # [m] + orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, # + structural_node_3, # + -0.14, 0.0, -0.024999999999999998, # [m] + orientation, 3, 0.0, 0.0, 1.0, 2, guess; # + + joint: joint_4, + drive hinge, + structural_node_1, # + orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, # + structural_node_2, # + orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, # + single, 0., 0., 1., # + string, "0"; # [rad] + + joint: joint_5, + in line, + structural_node_4, # + 0.0, 0.0, 0.0, # [m] + 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, # + structural_node_3, # + offset, 0.14, 0.0, -0.024999999999999998; # [m] + + joint: joint_6, + prismatic, + structural_node_1, # + orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0, #relative_orientation_matrix_1> + structural_node_4, # + orientation, 3, 0.0, 0.0, 1.0, 2, 0.0, 1.0, 0.0; #relative_orientation_matrix_2> + + joint: joint_7, + in line, + structural_node_1, # + 0.0, 8.796847998604521e-19, 0.08, # [m] + 3, 1.0, -2.220446049250313e-16, 2.220446049250313e-16, 2, -2.220446049250313e-16, 0.0, 1.0, # + structural_node_4, # + offset, 0.0, 0.0, 0.0; # [m] + + end: elements; + diff --git a/OndselSolver/MBDynControlData.cpp b/OndselSolver/MBDynControlData.cpp index 9f71e11..0a48c7a 100644 --- a/OndselSolver/MBDynControlData.cpp +++ b/OndselSolver/MBDynControlData.cpp @@ -8,42 +8,126 @@ void MbD::MBDynControlData::initialize() void MbD::MBDynControlData::parseMBDyn(std::vector& lines) { + readMaxIterations(lines); + readDefaultOrientation(lines); + readOmegaRotates(lines); + readPrint(lines); + readInitialStiffness(lines); readStructuralNodes(lines); readRigidBodies(lines); readJoints(lines); + assert(lines.size() == 2); +} + +void MbD::MBDynControlData::readMaxIterations(std::vector& lines) +{ + //max iterations: 1000; + std::vector tokens{ "max", "iterations:" }; + auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; + std::istringstream iss(*it); + std::string str; + iss >> str; + iss >> str; + iss >> maxIterations; + lines.erase(it); +} + +void MbD::MBDynControlData::readDefaultOrientation(std::vector& lines) +{ + //default orientation: euler321; + std::vector tokens{ "default", "orientation:" }; + auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; + std::istringstream iss(*it); + std::string str; + iss >> str; + iss >> str; + iss >> defaultOrientation; + lines.erase(it); +} + +void MbD::MBDynControlData::readOmegaRotates(std::vector& lines) +{ + //omega rotates: no; + std::vector tokens{ "omega", "rotates:" }; + auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; + std::istringstream iss(*it); + std::string str; + iss >> str; + iss >> str; + iss >> omegaRotates; + lines.erase(it); +} + +void MbD::MBDynControlData::readPrint(std::vector& lines) +{ + //print: none; + std::vector tokens{ "print:" }; + auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; + std::istringstream iss(*it); + std::string str; + iss >> str; + iss >> str; + iss >> print; + lines.erase(it); +} + +void MbD::MBDynControlData::readInitialStiffness(std::vector& lines) +{ + //initial stiffness: 1.0, 1.0; + std::vector tokens{ "initial", "stiffness:" }; + auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; + std::istringstream iss(*it); + std::string str; + iss >> str; + iss >> str; + iss >> initialStiffness; + iss >> str; + initialStiffness.append(str); + lines.erase(it); } void MbD::MBDynControlData::readStructuralNodes(std::vector& lines) -{ +{ //structural nodes: 4; - std::vector tokens{"structural", "nodes:"}; + std::vector tokens{ "structural", "nodes:" }; auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; std::istringstream iss(*it); std::string str; iss >> str; iss >> str; iss >> structuralNodes; + lines.erase(it); } void MbD::MBDynControlData::readRigidBodies(std::vector& lines) { //rigid bodies: 3; - std::vector tokens{"rigid", "bodies:"}; + std::vector tokens{ "rigid", "bodies:" }; auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; std::istringstream iss(*it); std::string str; iss >> str; iss >> str; iss >> rigidBodies; + lines.erase(it); } void MbD::MBDynControlData::readJoints(std::vector& lines) { //joints: 6; - std::vector tokens{"joints:"}; + std::vector tokens{ "joints:" }; auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; std::istringstream iss(*it); std::string str; iss >> str; iss >> joints; + lines.erase(it); } diff --git a/OndselSolver/MBDynControlData.h b/OndselSolver/MBDynControlData.h index 5697944..afd1c56 100644 --- a/OndselSolver/MBDynControlData.h +++ b/OndselSolver/MBDynControlData.h @@ -15,10 +15,20 @@ namespace MbD { public: void initialize() override; void parseMBDyn(std::vector& lines) override; + void readMaxIterations(std::vector& lines); + void readDefaultOrientation(std::vector& lines); + void readOmegaRotates(std::vector& lines); + void readPrint(std::vector& lines); + void readInitialStiffness(std::vector& lines); void readStructuralNodes(std::vector& lines); void readRigidBodies(std::vector& lines); void readJoints(std::vector& lines); + int maxIterations = 1000; + std::string defaultOrientation = "euler321"; + std::string omegaRotates = "no"; + std::string print = "none"; + std::string initialStiffness = "1.0, 1.0"; int structuralNodes, rigidBodies, joints; }; } diff --git a/OndselSolver/MBDynData.cpp b/OndselSolver/MBDynData.cpp index 7111acd..383da24 100644 --- a/OndselSolver/MBDynData.cpp +++ b/OndselSolver/MBDynData.cpp @@ -9,7 +9,7 @@ void MbD::MBDynData::initialize() void MbD::MBDynData::parseMBDyn(std::vector& lines) { assert(lines.size() == 3); - std::vector tokens{"problem:", "initial", "value;"}; + std::vector tokens{ "problem:", "initial", "value" }; auto problemit = findLineWith(lines, tokens); assert(problemit != lines.end()); } diff --git a/OndselSolver/MBDynElements.cpp b/OndselSolver/MBDynElements.cpp deleted file mode 100644 index 1d0fafe..0000000 --- a/OndselSolver/MBDynElements.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "MBDynElements.h" -#include "MBDynBody.h" - -using namespace MbD; - -void MbD::MBDynElements::initialize() -{ -} - -void MbD::MBDynElements::parseMBDyn(std::vector& lines) -{ - elements = std::make_shared>>(); - std::vector bodyToken{ "body:" }; - std::vector jointToken{ "joint:" }; - std::vector::iterator it; - while (true) { - it = findLineWith(lines, bodyToken); - if (it != lines.end()) { - auto body = std::make_shared(); - body->owner = this; - body->parseMBDyn(*it); - elements->push_back(body); - lines.erase(it); - continue; - } - it = findLineWith(lines, jointToken); - if (it != lines.end()) { - auto body = std::make_shared(); - body->owner = this; - body->parseMBDyn(*it); - elements->push_back(body); - lines.erase(it); - continue; - } - break; - } -} diff --git a/OndselSolver/MBDynInitialValue.cpp b/OndselSolver/MBDynInitialValue.cpp index 578a60d..7fab3da 100644 --- a/OndselSolver/MBDynInitialValue.cpp +++ b/OndselSolver/MBDynInitialValue.cpp @@ -1,4 +1,6 @@ #include "MBDynInitialValue.h" +#include "ASMTSimulationParameters.h" +#include "ASMTAssembly.h" using namespace MbD; @@ -13,63 +15,134 @@ void MbD::MBDynInitialValue::parseMBDyn(std::vector& lines) readTimeStep(lines); readMaxIterations(lines); readTolerance(lines); + readDerivativesTolerance(lines); + readDerivativesMaxIterations(lines); + readDerivativesCoefficient(lines); + assert(lines.size() == 2); } void MbD::MBDynInitialValue::readInitialTime(std::vector& lines) { //initial time: 0.; - std::vector tokens{"initial", "time:"}; + std::vector tokens{ "initial", "time:" }; auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; std::istringstream iss(*it); std::string str; iss >> str; iss >> str; - iss >> initialTime; + iss >> initialTime; + lines.erase(it); } void MbD::MBDynInitialValue::readFinalTime(std::vector& lines) { //final time: 5.; - std::vector tokens{"final", "time:"}; + std::vector tokens{ "final", "time:" }; auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; std::istringstream iss(*it); std::string str; iss >> str; iss >> str; iss >> finalTime; + lines.erase(it); } void MbD::MBDynInitialValue::readTimeStep(std::vector& lines) { //time step: 1.e-2; - std::vector tokens{"time", "step:"}; + std::vector tokens{ "time", "step:" }; auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; std::istringstream iss(*it); std::string str; iss >> str; iss >> str; iss >> timeStep; + lines.erase(it); } void MbD::MBDynInitialValue::readMaxIterations(std::vector& lines) { //max iterations: 10; - std::vector tokens{"max", "iterations:"}; + std::vector tokens{ "max", "iterations:" }; auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; std::istringstream iss(*it); std::string str; iss >> str; iss >> str; iss >> maxIterations; + lines.erase(it); } void MbD::MBDynInitialValue::readTolerance(std::vector& lines) { //tolerance: 1.e-6; - std::vector tokens{"tolerance:"}; + std::vector tokens{ "tolerance:" }; auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; std::istringstream iss(*it); std::string str; iss >> str; iss >> tolerance; + lines.erase(it); +} + +void MbD::MBDynInitialValue::readDerivativesTolerance(std::vector& lines) +{ + //derivatives tolerance: 0.0001; + std::vector tokens{ "derivatives", "tolerance:" }; + auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; + std::istringstream iss(*it); + std::string str; + iss >> str; + iss >> str; + iss >> derivativesTolerance; + lines.erase(it); +} + +void MbD::MBDynInitialValue::readDerivativesMaxIterations(std::vector& lines) +{ + //derivatives max iterations: 100; + std::vector tokens{ "derivatives", "max", "iterations:" }; + auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; + std::istringstream iss(*it); + std::string str; + iss >> str; + iss >> str; + iss >> str; + iss >> derivativesMaxIterations; + lines.erase(it); +} + +void MbD::MBDynInitialValue::readDerivativesCoefficient(std::vector& lines) +{ + //derivatives coefficient: auto; + std::vector tokens{ "derivatives", "coefficient:" }; + auto it = findLineWith(lines, tokens); + if (it == lines.end()) return; + std::istringstream iss(*it); + std::string str; + iss >> str; + iss >> str; + iss >> derivativesCoefficient; + lines.erase(it); +} + +void MbD::MBDynInitialValue::createASMT() +{ + auto simulationParameters = std::make_shared(); + asmtItem = simulationParameters; + simulationParameters->settstart(initialTime); + simulationParameters->settend(finalTime); //tstart == tend Initial Conditions only. + simulationParameters->sethmin(1.0e-9); + simulationParameters->sethmax(1.0); + simulationParameters->sethout(timeStep); + simulationParameters->seterrorTol(tolerance); + simulationParameters->setmaxIter(maxIterations); + asmtAssembly()->setSimulationParameters(simulationParameters); } diff --git a/OndselSolver/MBDynInitialValue.h b/OndselSolver/MBDynInitialValue.h index 08cefaa..8c1a4c4 100644 --- a/OndselSolver/MBDynInitialValue.h +++ b/OndselSolver/MBDynInitialValue.h @@ -20,8 +20,15 @@ namespace MbD { void readTimeStep(std::vector& lines); void readMaxIterations(std::vector& lines); void readTolerance(std::vector& lines); + void readDerivativesTolerance(std::vector& lines); + void readDerivativesMaxIterations(std::vector& lines); + void readDerivativesCoefficient(std::vector& lines); + void createASMT() override; - double initialTime, finalTime, timeStep, tolerance; - int maxIterations; + double initialTime = 0.0, finalTime = 5.0, timeStep = 1.0e-2, tolerance = 1.0e-6; + int maxIterations = 10; + double derivativesTolerance = 1.0e-4; + int derivativesMaxIterations = 100; + std::string derivativesCoefficient = "auto"; }; } diff --git a/OndselSolver/MBDynItem.cpp b/OndselSolver/MBDynItem.cpp index 40482c4..e87fdf2 100644 --- a/OndselSolver/MBDynItem.cpp +++ b/OndselSolver/MBDynItem.cpp @@ -1,12 +1,13 @@ #include "MBDynItem.h" #include "MBDynSystem.h" -#include "MBDynVariables.h" #include "SymbolicParser.h" #include "BasicUserFunction.h" #include "EulerAngles.h" #include "Constant.h" -#include "MBDynReferences.h" #include "MBDynReference.h" +#include "FullMatrix.h" +#include "ASMTItem.h" +#include "MBDynBody.h" using namespace MbD; @@ -21,6 +22,11 @@ void MbD::MBDynItem::initialize() assert(false); } +void MbD::MBDynItem::noop() +{ + //No Operations +} + void MbD::MBDynItem::parseMBDyn(std::vector& lines) { assert(false); @@ -45,108 +51,288 @@ bool MbD::MBDynItem::lineHasTokens(const std::string& line, std::vector MbD::MBDynItem::mbdynVariables() +std::shared_ptr>> MbD::MBDynItem::mbdynNodes() +{ + return owner->mbdynNodes(); +} + +std::vector MbD::MBDynItem::nodeNames() +{ + return owner->nodeNames(); +} + +std::shared_ptr> MbD::MBDynItem::mbdynVariables() { return owner->mbdynVariables(); } -std::shared_ptr MbD::MBDynItem::mbdynReferences() +std::shared_ptr>> MbD::MBDynItem::mbdynReferences() { return owner->mbdynReferences(); } -FColDsptr MbD::MBDynItem::readPosition(std::shared_ptr>& args) +void MbD::MBDynItem::createASMT() +{ + assert(false); +} + +std::shared_ptr MbD::MBDynItem::nodeAt(std::string nodeName) +{ + return owner->nodeAt(nodeName); +} + +int MbD::MBDynItem::nodeidAt(std::string nodeName) +{ + return owner->nodeidAt(nodeName); +} + +std::shared_ptr MbD::MBDynItem::bodyWithNode(std::string nodeName) +{ + return owner->bodyWithNode(nodeName); +} + +std::shared_ptr MbD::MBDynItem::asmtAssembly() +{ + return owner->asmtAssembly(); +} + +FColDsptr MbD::MBDynItem::readVector3(std::vector& args) +{ + auto parser = std::make_shared(); + parser->variables = mbdynVariables(); + auto rFfF = std::make_shared>(3); + auto& str = args.at(0); + if (str.find("null") != std::string::npos) { + args.erase(args.begin()); + } + else { + for (int i = 0; i < 3; i++) + { + auto userFunc = std::make_shared(popOffTop(args), 1.0); + parser->parseUserFunction(userFunc); + auto sym = parser->stack->top(); + rFfF->at(i) = sym->getValue(); + } + + } + return rFfF; +} + +FColDsptr MbD::MBDynItem::readPosition(std::vector& args) { auto rOfO = std::make_shared>(3); - auto str = args->at(0); - if (str.find("reference") != std::string::npos) { - args->erase(args->begin()); - std::string refName; - std::istringstream iss(args->at(0)); - iss >> refName; - auto ref = mbdynReferences()->references->at(refName); - args->erase(args->begin()); + if (args.empty()) return rOfO; + auto& str = args.at(0); + if (str.find("orientation") != std::string::npos) { + //Do nothing + } + else if (str.find("reference") != std::string::npos) { + args.erase(args.begin()); + auto refName = readStringOffTop(args); + auto ref = mbdynReferences()->at(refName); auto rFfF = readBasicPosition(args); auto rOFO = ref->rOfO; auto aAOF = ref->aAOf; rOfO = rOFO->plusFullColumn(aAOF->timesFullColumn(rFfF)); } + else if (str.find("offset") != std::string::npos) { + args.erase(args.begin()); + rOfO = readBasicPosition(args); + } + else if (str.find("null") != std::string::npos) { + args.erase(args.begin()); + } else { rOfO = readBasicPosition(args); } return rOfO; } -FColDsptr MbD::MBDynItem::readBasicPosition(std::shared_ptr>& args) +FColDsptr MbD::MBDynItem::readBasicPosition(std::vector& args) { - auto parser = CREATE::With(); - parser->variables = mbdynVariables()->variables; - auto rFfF = std::make_shared>(3); - std::string str; - for (int i = 0; i < 3; i++) - { - str = args->at(0); - args->erase(args->begin()); - auto userFunc = CREATE::With(str, 1.0); - parser->parseUserFunction(userFunc); - auto sym = parser->stack->top(); - rFfF->at(i) = sym->getValue(); - } - return rFfF; + return readVector3(args); } -FMatDsptr MbD::MBDynItem::readOrientation(std::shared_ptr>& args) +FMatDsptr MbD::MBDynItem::readOrientation(std::vector& args) { - auto aAOf = std::make_shared>(3, 3); - auto str = args->at(0); + auto aAOf = FullMatrix::identitysptr(3); + if (args.empty()) return aAOf; + auto& str = args.at(0); if (str.find("reference") != std::string::npos) { - args->erase(args->begin()); - std::string refName; - std::istringstream iss(args->at(0)); - iss >> refName; - auto ref = mbdynReferences()->references->at(refName); - args->erase(args->begin()); + args.erase(args.begin()); + auto refName = readStringOffTop(args); + auto ref = mbdynReferences()->at(refName); auto aAFf = readBasicOrientation(args); auto aAOF = ref->aAOf; aAOf = aAOF->timesFullMatrix(aAFf); } + else if (str.find("hinge") != std::string::npos) { + args.erase(args.begin()); + aAOf = readOrientation(args); + } + else if (str.find("orientation") != std::string::npos) { + args.erase(args.begin()); + aAOf = readOrientation(args); + } else { aAOf = readBasicOrientation(args); } return aAOf; } -FMatDsptr MbD::MBDynItem::readBasicOrientation(std::shared_ptr>& args) +FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector& args) { - auto parser = CREATE::With(); - parser->variables = mbdynVariables()->variables; - auto euler = std::make_shared>(); - auto str = args->at(0); + auto parser = std::make_shared(); + parser->variables = mbdynVariables(); + auto& str = args.at(0); if (str.find("euler") != std::string::npos) { - args->erase(args->begin()); + args.erase(args.begin()); + auto euler = std::make_shared>(); euler->rotOrder = std::make_shared>(std::initializer_list{ 1, 2, 3 }); for (int i = 0; i < 3; i++) { - str = args->at(0); - args->erase(args->begin()); - auto userFunc = CREATE::With(str, 1.0); + auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); auto sym = parser->stack->top(); euler->at(i) = sym; } + euler->calc(); + auto aAFf = euler->aA; + return aAFf; } - else if (str.find("eye") != std::string::npos) { - args->erase(args->begin()); - euler->rotOrder = std::make_shared>(std::initializer_list{ 1, 2, 3 }); - for (int i = 0; i < 3; i++) + if (str.find("eye") != std::string::npos) { + args.erase(args.begin()); + auto aAFf = FullMatrix::identitysptr(3); + return aAFf; + } + auto iss = std::istringstream(str); + int integer; + iss >> integer; + if (integer == 1) { + args.erase(args.begin()); + FColDsptr vecX, vecY, vecZ, vec; + vecX = readPosition(args); + vecX->normalizeSelf(); + auto axis = stoi(popOffTop(args)); + str = args.at(0); + if (str.find("guess") != std::string::npos) { + args.erase(args.begin()); + double min = std::numeric_limits::max(); + double max = -1.0; + int imin, imax; + for (int i = 0; i < 3; i++) + { + auto mag = std::abs(vecX->at(i)); + if (mag > max) { + imax = i; + max = mag; + } + if (mag < min) { + imin = i; + min = mag; + } + } + vec = std::make_shared>(3); + vec->at(imin) = 1.0; + vec->at(imax) = -vecX->at(imin) / vecX->at(imax); + } + else { + vec = readPosition(args); + } + vec->normalizeSelf(); + if (axis == 2) { + vecZ = vecX->cross(vec); + vecY = vecZ->cross(vecX); + } + else if (axis == 3) { + vecY = vec->cross(vecX); + vecZ = vecX->cross(vecY); + } + else { + assert(false); + } + auto aAFf = FullMatrix::identitysptr(3); + aAFf->atijputFullColumn(0, 0, vecX); + aAFf->atijputFullColumn(0, 1, vecY); + aAFf->atijputFullColumn(0, 2, vecZ); + return aAFf; + } + if (integer == 3) { + args.erase(args.begin()); + FColDsptr vecX, vecY, vecZ, vec; + vecZ = readPosition(args); + vecZ->normalizeSelf(); + auto axis = stoi(popOffTop(args)); + str = args.at(0); + if (str.find("guess") != std::string::npos) { + args.erase(args.begin()); + double min = std::numeric_limits::max(); + double max = -1.0; + int imin, imax; + for (int i = 0; i < 3; i++) + { + auto mag = std::abs(vecZ->at(i)); + if (mag > max) { + imax = i; + max = mag; + } + if (mag < min) { + imin = i; + min = mag; + } + } + vec = std::make_shared>(3); + vec->at(imin) = 1.0; + vec->at(imax) = -vecZ->at(imin) / vecZ->at(imax); + } + else { + vec = readPosition(args); + } + vec->normalizeSelf(); + if (axis == 2) { + vecX = vec->cross(vecZ); + vecY = vecZ->cross(vecX); + } + else if (axis == 1) { + vecY = vecZ->cross(vec); + vecX = vecY->cross(vecZ); + } + else { + assert(false); + } + auto aAFf = FullMatrix::identitysptr(3); + aAFf->atijputFullColumn(0, 0, vecX); + aAFf->atijputFullColumn(0, 1, vecY); + aAFf->atijputFullColumn(0, 2, vecZ); + return aAFf; + } + auto aAFf = FullMatrix::identitysptr(3); + for (int i = 0; i < 3; i++) + { + auto rowi = aAFf->at(i); + for (int j = 0; j < 3; j++) { - euler->at(i) = std::make_shared(0.0); + auto userFunc = std::make_shared(popOffTop(args), 1.0); + parser->parseUserFunction(userFunc); + auto sym = parser->stack->top(); + rowi->at(j) = sym->getValue(); } } - else { - assert(false); - } - euler->calc(); - auto aAFf = euler->aA; return aAFf; } + +std::string MbD::MBDynItem::popOffTop(std::vector& args) +{ + auto str = args.at(0); + args.erase(args.begin()); + return str; +} + +std::string MbD::MBDynItem::readStringOffTop(std::vector& args) +{ + auto iss = std::istringstream(args.at(0)); + args.erase(args.begin()); + std::string str; + iss >> str; + return str; +} diff --git a/OndselSolver/MBDynItem.h b/OndselSolver/MBDynItem.h index f9c6fb8..cc403b0 100644 --- a/OndselSolver/MBDynItem.h +++ b/OndselSolver/MBDynItem.h @@ -11,8 +11,12 @@ namespace MbD { class MBDynSystem; - class MBDynVariables; - class MBDynReferences; + class MBDynVariable; + class MBDynReference; + class MBDynNode; + class ASMTItem; + class MBDynBody; + class ASMTAssembly; class MBDynItem { @@ -21,20 +25,32 @@ namespace MbD { virtual MBDynSystem* root(); virtual void initialize(); + void noop(); //void setName(std::string str); virtual void parseMBDyn(std::vector& lines); std::vector::iterator findLineWith(std::vector& lines, std::vector& tokens); bool lineHasTokens(const std::string& line, std::vector& tokens); - virtual std::shared_ptr mbdynVariables(); - virtual std::shared_ptr mbdynReferences(); - FColDsptr readPosition(std::shared_ptr>& args); - FColDsptr readBasicPosition(std::shared_ptr>& args); - FMatDsptr readOrientation(std::shared_ptr>& args); - FMatDsptr readBasicOrientation(std::shared_ptr>& args); + virtual std::shared_ptr>> mbdynNodes(); + virtual std::vector nodeNames(); + virtual std::shared_ptr> mbdynVariables(); + virtual std::shared_ptr>> mbdynReferences(); + virtual void createASMT(); + virtual std::shared_ptr nodeAt(std::string nodeName); + virtual int nodeidAt(std::string nodeName); + virtual std::shared_ptr bodyWithNode(std::string nodeName); + virtual std::shared_ptr asmtAssembly(); + + FColDsptr readVector3(std::vector& args); + FColDsptr readPosition(std::vector& args); + FColDsptr readBasicPosition(std::vector& args); + FMatDsptr readOrientation(std::vector& args); + FMatDsptr readBasicOrientation(std::vector& args); + std::string popOffTop(std::vector& args); + std::string readStringOffTop(std::vector& args); std::string name; MBDynItem* owner; - std::shared_ptr mbdObject; + std::shared_ptr asmtItem; }; diff --git a/OndselSolver/MBDynJoint.cpp b/OndselSolver/MBDynJoint.cpp index bbceb66..be29cf7 100644 --- a/OndselSolver/MBDynJoint.cpp +++ b/OndselSolver/MBDynJoint.cpp @@ -1,4 +1,14 @@ +#include + #include "MBDynJoint.h" +#include "ASMTMarker.h" +#include "ASMTPart.h" +#include "ASMTAssembly.h" +#include "ASMTRevoluteJoint.h" +#include "ASMTRotationalMotion.h" +#include "ASMTPointInLineJoint.h" +#include "ASMTNoRotationJoint.h" +#include "ASMTFixedJoint.h" using namespace MbD; @@ -6,7 +16,224 @@ void MbD::MBDynJoint::initialize() { } -void MbD::MBDynJoint::parseMBDyn(std::vector& lines) +void MbD::MBDynJoint::parseMBDyn(std::string line) { - assert(false); + jointString = line; + size_t previousPos = 0; + auto pos = line.find(":"); + auto front = line.substr(previousPos, pos - previousPos); + assert(front.find("joint") != std::string::npos); + auto arguments = std::vector(); + std::string argument; + while (true) { + previousPos = pos; + pos = line.find(",", pos + 1); + if (pos != std::string::npos) { + argument = line.substr(previousPos + 1, pos - previousPos - 1); + arguments.push_back(argument); + } + else { + argument = line.substr(previousPos + 1); + arguments.push_back(argument); + break; + } + } + auto ss = std::stringstream(); + auto iss = std::istringstream(arguments.at(0)); + iss >> name; + arguments.erase(arguments.begin()); + iss = std::istringstream(arguments.at(0)); + iss >> joint_type; + if (joint_type == "axial") { + ss << joint_type; + iss >> joint_type; + if (joint_type == "rotation") { + ss << " " << joint_type; + joint_type = ss.str(); + } + else { + assert(false); + } + arguments.erase(arguments.begin()); + readMarkerI(arguments); + readMarkerJ(arguments); + readFunction(arguments); + return; + } + else if (joint_type == "revolute") { + ss << joint_type; + iss >> joint_type; + if (joint_type == "hinge") { + ss << " " << joint_type; + joint_type = ss.str(); + } + else { + assert(false); + } + } + else if (joint_type == "spherical") { + ss << joint_type; + iss >> joint_type; + if (joint_type == "hinge") { + ss << " " << joint_type; + joint_type = ss.str(); + } + else { + assert(false); + } + } + else if (joint_type == "drive") { + ss << joint_type; + iss >> joint_type; + if (joint_type == "hinge") { + ss << " " << joint_type; + joint_type = ss.str(); + } + else { + assert(false); + } + arguments.erase(arguments.begin()); + readMarkerI(arguments); + readMarkerJ(arguments); + readFunction(arguments); + return; + } + else if (joint_type == "in") { + ss << joint_type; + iss >> joint_type; + if (joint_type == "line") { + ss << " " << joint_type; + joint_type = ss.str(); + } + else { + assert(false); + } + } + else if (joint_type == "clamp") { + //mkr1 should be on assembly which doesn't exist in MBDyn + //mkr2 is on the node + arguments.erase(arguments.begin()); + mkr1 = std::make_shared(); + mkr1->owner = this; + mkr1->nodeStr = "Assembly"; + mkr1->rPmP = std::make_shared>(3); + mkr1->aAPm = FullMatrix::identitysptr(3); + readMarkerJ(arguments); + return; + } + else if (joint_type == "prismatic") { + noop(); + } + else { + assert(false); + } + arguments.erase(arguments.begin()); + readMarkerI(arguments); + readMarkerJ(arguments); +} + +void MbD::MBDynJoint::readMarkerI(std::vector& args) +{ + mkr1 = std::make_shared(); + mkr1->owner = this; + mkr1->nodeStr = readStringOffTop(args); + auto _nodeNames = nodeNames(); + std::string nodeName; + auto it = std::find_if(args.begin(), args.end(), [&](const std::string& s) { + auto iss = std::istringstream(s); + iss >> nodeName; + if (std::find(_nodeNames.begin(), _nodeNames.end(), nodeName) != _nodeNames.end()) return true; + return false; + }); + auto markerArgs = std::vector(args.begin(), it); + args.erase(args.begin(), it); + mkr1->parseMBDyn(markerArgs); +} + +void MbD::MBDynJoint::readMarkerJ(std::vector& args) +{ + if (args.empty()) return; + mkr2 = std::make_shared(); + mkr2->owner = this; + mkr2->nodeStr = readStringOffTop(args); + mkr2->parseMBDyn(args); +} + +void MbD::MBDynJoint::readFunction(std::vector& args) +{ + if (args.empty()) return; + std::string str; + auto iss = std::istringstream(args.at(0)); + iss >> str; + if (str.find("ramp") != std::string::npos) { + args.erase(args.begin()); + std::string slope, initValue, initTime, finalTime; + slope = popOffTop(args); + initTime = popOffTop(args); + finalTime = popOffTop(args); + initValue = popOffTop(args); + //f = slope*(time - t0) + f0 + auto ss = std::stringstream(); + ss << slope << "*(time - " << initTime << ") + " << initValue; + formula = ss.str(); + } else if (str.find("single") != std::string::npos) { + args.erase(args.begin()); + auto vec3 = readVector3(args); + assert(vec3->at(0) == 0 && vec3->at(1) == 0 && vec3->at(2) == 1); + assert(readStringOffTop(args) == "string"); + formula = popOffTop(args); + formula = std::regex_replace(formula, std::regex("\""), ""); + } + else { + assert(false); + } +} + +void MbD::MBDynJoint::createASMT() +{ + mkr1->createASMT(); + if (mkr2) mkr2->createASMT(); + std::shared_ptr asmtJoint; + if (joint_type == "clamp") { + auto asmtAsm = asmtAssembly(); + asmtJoint = std::make_shared(); + asmtJoint->setName(name); + asmtJoint->setMarkerI(mkr1->asmtItem->fullName("")); + asmtJoint->setMarkerJ(mkr2->asmtItem->fullName("")); + asmtAsm->addJoint(asmtJoint); + return; + } + if (joint_type == "axial rotation") { + auto asmtAsm = asmtAssembly(); + asmtJoint = std::make_shared(); + asmtItem = asmtJoint; + asmtJoint->setName(name); + asmtJoint->setMarkerI(mkr1->asmtItem->fullName("")); + asmtJoint->setMarkerJ(mkr2->asmtItem->fullName("")); + asmtAsm->addJoint(asmtJoint); + auto asmtMotion = std::make_shared(); + asmtItem = asmtMotion; + asmtMotion->setName(name.append("Motion")); + asmtMotion->setMotionJoint(asmtJoint->fullName("")); + asmtMotion->setRotationZ(formula); + asmtAsm->addMotion(asmtMotion); + return; + } + if (joint_type == "revolute hinge") { + asmtJoint = std::make_shared(); + } + else if (joint_type == "in line") { + asmtJoint = std::make_shared(); + } + else if (joint_type == "prismatic") { + asmtJoint = std::make_shared(); + } + else { + assert(false); + } + asmtItem = asmtJoint; + asmtJoint->setName(name); + asmtJoint->setMarkerI(mkr1->asmtItem->fullName("")); + asmtJoint->setMarkerJ(mkr2->asmtItem->fullName("")); + asmtAssembly()->addJoint(asmtJoint); } diff --git a/OndselSolver/MBDynJoint.h b/OndselSolver/MBDynJoint.h index b49f5c3..09dc0fb 100644 --- a/OndselSolver/MBDynJoint.h +++ b/OndselSolver/MBDynJoint.h @@ -8,13 +8,21 @@ #pragma once #include "MBDynElement.h" +#include "MBDynMarker.h" namespace MbD { class MBDynJoint : public MBDynElement { public: void initialize() override; - void parseMBDyn(std::vector& lines) override; + void parseMBDyn(std::string line); + void readMarkerI(std::vector& args); + void readMarkerJ(std::vector& args); + void readFunction(std::vector& args); + void createASMT() override; + std::string jointString, joint_type, node_1_label, node_2_label; + std::shared_ptr mkr1, mkr2; + std::string formula; }; } diff --git a/OndselSolver/MBDynLabels.cpp b/OndselSolver/MBDynLabels.cpp deleted file mode 100644 index 04df4f7..0000000 --- a/OndselSolver/MBDynLabels.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include "MBDynLabels.h" - -using namespace MbD; - -void MbD::MBDynLabels::initialize() -{ -} - -void MbD::MBDynLabels::parseMBDyn(std::vector& lines) -{ - labels = std::make_shared>(); - std::string str, label; - int intValue; - std::vector tokens{"set:", "integer"}; - while (true) { - auto it = findLineWith(lines, tokens); - if (it != lines.end()) { - std::istringstream iss(*it); - iss >> str; - iss >> str; - iss >> label; - iss >> str; - iss >> intValue; - labels->insert(std::make_pair(label, intValue)); - lines.erase(it); - } - else { - break; - } - } -} diff --git a/OndselSolver/MBDynMarker.cpp b/OndselSolver/MBDynMarker.cpp new file mode 100644 index 0000000..a8ac3a3 --- /dev/null +++ b/OndselSolver/MBDynMarker.cpp @@ -0,0 +1,55 @@ +#include "MBDynMarker.h" +#include "MBDynStructural.h" +#include "ASMTMarker.h" +#include "ASMTPart.h" +#include "ASMTAssembly.h" +#include "MBDynBody.h" +#include "ASMTSpatialContainer.h" + +using namespace MbD; + +void MbD::MBDynMarker::parseMBDyn(std::vector& args) +{ + rPmP = std::make_shared>(3); + aAPm = FullMatrix::identitysptr(3); + if (args.empty()) return; + auto& str = args.at(0); + if (str.find("reference") != std::string::npos) { + auto strucNode = std::static_pointer_cast(nodeAt(nodeStr)); + auto rOPO = strucNode->rOfO; + auto aAOP = strucNode->aAOf; + auto rOmO = readPosition(args); + auto aAOm = readOrientation(args); + rPmP = aAOP->transposeTimesFullColumn(rOmO->minusFullColumn(rOPO)); + aAPm = aAOP->transposeTimesFullMatrix(aAOm); + } + else if (str.find("offset") != std::string::npos) { + rPmP = readPosition(args); + } + else { + rPmP = readPosition(args); + aAPm = readOrientation(args); + } +} + +void MbD::MBDynMarker::createASMT() +{ + auto asmtAsm = asmtAssembly(); + if (nodeStr == "Assembly") { + auto mkr = std::make_shared(); + asmtItem = mkr; + mkr->setName(asmtAsm->generateUniqueMarkerName()); + mkr->setPosition3D(rPmP); + mkr->setRotationMatrix(aAPm); + asmtAsm->addMarker(mkr); + } + else { + auto asmtPart = asmtAsm->partPartialNamed(nodeStr); + auto mkr = std::make_shared(); + asmtItem = mkr; + mkr->setName(asmtPart->generateUniqueMarkerName()); + mkr->setPosition3D(rPmP); + mkr->setRotationMatrix(aAPm); + asmtPart->addMarker(mkr); + } +} diff --git a/OndselSolver/MBDynVariables.h b/OndselSolver/MBDynMarker.h similarity index 75% rename from OndselSolver/MBDynVariables.h rename to OndselSolver/MBDynMarker.h index 1968435..1b3dca7 100644 --- a/OndselSolver/MBDynVariables.h +++ b/OndselSolver/MBDynMarker.h @@ -10,12 +10,15 @@ #include "MBDynItem.h" namespace MbD { - class MBDynVariables : public MBDynItem + + class MBDynMarker : public MBDynItem { public: - void initialize() override; - void parseMBDyn(std::vector& lines) override; + void parseMBDyn(std::vector& args); + void createASMT() override; - std::shared_ptr> variables; + std::string nodeStr; + FColDsptr rPmP; //part to marker + FMatDsptr aAPm; }; } diff --git a/OndselSolver/MBDynNode.cpp b/OndselSolver/MBDynNode.cpp index 04d5c74..675f5fa 100644 --- a/OndselSolver/MBDynNode.cpp +++ b/OndselSolver/MBDynNode.cpp @@ -1,4 +1,6 @@ #include "MBDynNode.h" +#include "ASMTPart.h" +#include "EulerAngles.h" using namespace MbD; @@ -10,3 +12,36 @@ void MbD::MBDynNode::parseMBDyn(std::vector& lines) { assert(false); } + +void MbD::MBDynNode::outputLine(int i, std::ostream& os) +{ + auto id = nodeidAt(name); + auto asmtPart = std::static_pointer_cast(asmtItem); + auto x = asmtPart->xs->at(i); + auto y = asmtPart->ys->at(i); + auto z = asmtPart->zs->at(i); + auto bryantAngles = std::make_shared>(); + bryantAngles->setRotOrder(1, 2, 3); + bryantAngles->at(0) = asmtPart->bryxs->at(i); + bryantAngles->at(1) = asmtPart->bryys->at(i); + bryantAngles->at(2) = asmtPart->bryzs->at(i); + bryantAngles->calc(); + auto aA = bryantAngles->aA; + auto vx = asmtPart->vxs->at(i); + auto vy = asmtPart->vys->at(i); + auto vz = asmtPart->vzs->at(i); + auto omex = asmtPart->omexs->at(i); + auto omey = asmtPart->omeys->at(i); + auto omez = asmtPart->omezs->at(i); + os << id << " "; + os << x << " " << y << " " << z << " "; + auto row = aA->at(0); + os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; + row = aA->at(1); + os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; + row = aA->at(2); + os << row->at(0) << " " << row->at(1) << " " << row->at(2) << " "; + os << vx << " " << vy << " " << vz << " "; + os << omex << " " << omey << " " << omez << " "; + os << std::endl; +} diff --git a/OndselSolver/MBDynNode.h b/OndselSolver/MBDynNode.h index 28604c6..7842820 100644 --- a/OndselSolver/MBDynNode.h +++ b/OndselSolver/MBDynNode.h @@ -15,6 +15,7 @@ namespace MbD { public: void initialize() override; void parseMBDyn(std::vector& lines) override; + void outputLine(int i, std::ostream& os); }; } diff --git a/OndselSolver/MBDynNodes.cpp b/OndselSolver/MBDynNodes.cpp deleted file mode 100644 index f19ee3c..0000000 --- a/OndselSolver/MBDynNodes.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "MBDynNodes.h" -#include "MBDynNode.h" -#include "MBDynStructural.h" - -using namespace MbD; - -void MbD::MBDynNodes::initialize() -{ -} - -void MbD::MBDynNodes::parseMBDyn(std::vector& lines) -{ - nodes = std::make_shared>>(); - std::vector tokens{ "structural:" }; - while (true) { - auto it = findLineWith(lines, tokens); - if (it != lines.end()) { - auto structural = std::make_shared(); - structural->owner = this; - structural->parseMBDyn(*it); - nodes->push_back(structural); - lines.erase(it); - } - else { - break; - } - } -} diff --git a/OndselSolver/MBDynReference.cpp b/OndselSolver/MBDynReference.cpp index 9c125bf..f59a98e 100644 --- a/OndselSolver/MBDynReference.cpp +++ b/OndselSolver/MBDynReference.cpp @@ -1,5 +1,4 @@ #include "MBDynReference.h" -#include "MBDynVariables.h" #include "SymbolicParser.h" #include "BasicUserFunction.h" #include "EulerAngles.h" @@ -17,24 +16,24 @@ void MbD::MBDynReference::parseMBDyn(std::string line) auto pos = line.find(":"); auto front = line.substr(previousPos, pos - previousPos); assert(front.find("reference") != std::string::npos); - auto arguments = std::make_shared>(); + auto arguments = std::vector(); std::string argument; while (true) { previousPos = pos; pos = line.find(",", pos + 1); if (pos != std::string::npos) { argument = line.substr(previousPos + 1, pos - previousPos - 1); - arguments->push_back(argument); + arguments.push_back(argument); } else { argument = line.substr(previousPos + 1); - arguments->push_back(argument); + arguments.push_back(argument); break; } } - std::istringstream iss(arguments->at(0)); + std::istringstream iss(arguments.at(0)); iss >> name; - arguments->erase(arguments->begin()); + arguments.erase(arguments.begin()); rOfO = readPosition(arguments); aAOf = readOrientation(arguments); @@ -42,22 +41,20 @@ void MbD::MBDynReference::parseMBDyn(std::string line) readOmega(arguments); } -void MbD::MBDynReference::readVelocity(std::shared_ptr>& args) +void MbD::MBDynReference::readVelocity(std::vector& args) { - auto parser = CREATE::With(); - parser->variables = mbdynVariables()->variables; + auto parser = std::make_shared(); + parser->variables = mbdynVariables(); vOfO = std::make_shared>(3); - auto str = args->at(0); + auto& str = args.at(0); if (str.find("null") != std::string::npos) { - args->erase(args->begin()); + args.erase(args.begin()); return; } else { for (int i = 0; i < 3; i++) { - str = args->at(0); - args->erase(args->begin()); - auto userFunc = CREATE::With(str, 1.0); + auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); auto sym = parser->stack->top(); vOfO->at(i) = sym->getValue(); @@ -65,22 +62,20 @@ void MbD::MBDynReference::readVelocity(std::shared_ptr> } } -void MbD::MBDynReference::readOmega(std::shared_ptr>& args) +void MbD::MBDynReference::readOmega(std::vector& args) { - auto parser = CREATE::With(); - parser->variables = mbdynVariables()->variables; + auto parser = std::make_shared(); + parser->variables = mbdynVariables(); omeOfO = std::make_shared>(3); - auto str = args->at(0); + auto& str = args.at(0); if (str.find("null") != std::string::npos) { - args->erase(args->begin()); + args.erase(args.begin()); return; } else { for (int i = 0; i < 3; i++) { - str = args->at(0); - args->erase(args->begin()); - auto userFunc = CREATE::With(str, 1.0); + auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); auto sym = parser->stack->top(); omeOfO->at(i) = sym->getValue(); diff --git a/OndselSolver/MBDynReference.h b/OndselSolver/MBDynReference.h index a73e6c1..5867816 100644 --- a/OndselSolver/MBDynReference.h +++ b/OndselSolver/MBDynReference.h @@ -16,8 +16,8 @@ namespace MbD { public: void initialize() override; void parseMBDyn(std::string line); - void readVelocity(std::shared_ptr>& args); - void readOmega(std::shared_ptr>& args); + void readVelocity(std::vector& args); + void readOmega(std::vector& args); std::string refString, name; FColDsptr rOfO, vOfO, omeOfO; diff --git a/OndselSolver/MBDynReferences.cpp b/OndselSolver/MBDynReferences.cpp deleted file mode 100644 index 6aec4a1..0000000 --- a/OndselSolver/MBDynReferences.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "MBDynReferences.h" -#include "MBDynReference.h" - -using namespace MbD; - -void MbD::MBDynReferences::initialize() -{ -} - -void MbD::MBDynReferences::parseMBDyn(std::vector& lines) -{ - references = std::make_shared>>(); - std::string str, refName; - double doubleValue; - std::vector tokens{"reference:"}; - while (true) { - auto it = findLineWith(lines, tokens); - if (it != lines.end()) { - auto reference = std::make_shared(); - reference->owner = this; - reference->parseMBDyn(*it); - references->insert(std::make_pair(reference->name, reference)); - lines.erase(it); - } - else { - break; - } - } -} diff --git a/OndselSolver/MBDynReferences.h b/OndselSolver/MBDynReferences.h deleted file mode 100644 index 613ba84..0000000 --- a/OndselSolver/MBDynReferences.h +++ /dev/null @@ -1,24 +0,0 @@ -/*************************************************************************** - * Copyright (c) 2023 Ondsel, Inc. * - * * - * This file is part of OndselSolver. * - * * - * See LICENSE file for details about copyright. * - ***************************************************************************/ - -#pragma once -#include "MBDynItem.h" - -namespace MbD { - class MBDynReference; - - class MBDynReferences : public MBDynItem - { - public: - void initialize() override; - void parseMBDyn(std::vector& lines) override; - - std::shared_ptr>> references; - - }; -} diff --git a/OndselSolver/MBDynStructural.cpp b/OndselSolver/MBDynStructural.cpp index 70267b2..d53f5a2 100644 --- a/OndselSolver/MBDynStructural.cpp +++ b/OndselSolver/MBDynStructural.cpp @@ -1,16 +1,20 @@ #include "MBDynStructural.h" -#include "MBDynVariables.h" #include "SymbolicParser.h" #include "BasicUserFunction.h" #include "EulerAngles.h" #include "Constant.h" -#include "MBDynReferences.h" #include "MBDynReference.h" +#include "ASMTPart.h" +#include "ASMTAssembly.h" using namespace MbD; -void MbD::MBDynStructural::initialize() +MbD::MBDynStructural::MBDynStructural() { + rOfO = std::make_shared>(3); + aAOf = FullMatrix::identitysptr(3); + vOfO = std::make_shared>(3); + omeOfO = std::make_shared>(3); } void MbD::MBDynStructural::parseMBDyn(std::string line) @@ -20,27 +24,27 @@ void MbD::MBDynStructural::parseMBDyn(std::string line) auto pos = line.find(":"); auto front = line.substr(previousPos, pos - previousPos); assert(front.find("structural") != std::string::npos); - auto arguments = std::make_shared>(); + auto arguments = std::vector(); std::string argument; while (true) { previousPos = pos; pos = line.find(",", pos + 1); if (pos != std::string::npos) { argument = line.substr(previousPos + 1, pos - previousPos - 1); - arguments->push_back(argument); + arguments.push_back(argument); } else { argument = line.substr(previousPos + 1); - arguments->push_back(argument); + arguments.push_back(argument); break; } } - std::istringstream iss(arguments->at(0)); + std::istringstream iss(arguments.at(0)); iss >> name; - arguments->erase(arguments->begin()); - iss = std::istringstream(arguments->at(0)); + arguments.erase(arguments.begin()); + iss = std::istringstream(arguments.at(0)); iss >> type; - arguments->erase(arguments->begin()); + arguments.erase(arguments.begin()); rOfO = readPosition(arguments); aAOf = readOrientation(arguments); @@ -48,22 +52,20 @@ void MbD::MBDynStructural::parseMBDyn(std::string line) readOmega(arguments); } -void MbD::MBDynStructural::readVelocity(std::shared_ptr>& args) +void MbD::MBDynStructural::readVelocity(std::vector& args) { - auto parser = CREATE::With(); - parser->variables = mbdynVariables()->variables; + auto parser = std::make_shared(); + parser->variables = mbdynVariables(); vOfO = std::make_shared>(3); - auto str = args->at(0); + auto& str = args.at(0); if (str.find("null") != std::string::npos) { - args->erase(args->begin()); + args.erase(args.begin()); return; } else { for (int i = 0; i < 3; i++) { - str = args->at(0); - args->erase(args->begin()); - auto userFunc = CREATE::With(str, 1.0); + auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); auto sym = parser->stack->top(); vOfO->at(i) = sym->getValue(); @@ -71,25 +73,35 @@ void MbD::MBDynStructural::readVelocity(std::shared_ptr } } -void MbD::MBDynStructural::readOmega(std::shared_ptr>& args) +void MbD::MBDynStructural::readOmega(std::vector& args) { - auto parser = CREATE::With(); - parser->variables = mbdynVariables()->variables; + auto parser = std::make_shared(); + parser->variables = mbdynVariables(); omeOfO = std::make_shared>(3); - auto str = args->at(0); + auto& str = args.at(0); if (str.find("null") != std::string::npos) { - args->erase(args->begin()); + args.erase(args.begin()); return; } else { for (int i = 0; i < 3; i++) { - str = args->at(0); - args->erase(args->begin()); - auto userFunc = CREATE::With(str, 1.0); + auto userFunc = std::make_shared(popOffTop(args), 1.0); parser->parseUserFunction(userFunc); auto sym = parser->stack->top(); omeOfO->at(i) = sym->getValue(); } } } + +void MbD::MBDynStructural::createASMT() +{ + auto asmtPart = std::make_shared(); + asmtItem = asmtPart; + asmtPart->setName(name); + asmtPart->setPosition3D(rOfO); + asmtPart->setRotationMatrix(aAOf); + asmtPart->setVelocity3D(vOfO); + asmtPart->setOmega3D(omeOfO); + asmtAssembly()->addPart(asmtPart); +} diff --git a/OndselSolver/MBDynStructural.h b/OndselSolver/MBDynStructural.h index 84f5a07..d38b3d3 100644 --- a/OndselSolver/MBDynStructural.h +++ b/OndselSolver/MBDynStructural.h @@ -13,12 +13,13 @@ namespace MbD { class MBDynStructural : public MBDynNode { public: - void initialize() override; + MBDynStructural(); void parseMBDyn(std::string line); - void readVelocity(std::shared_ptr>& args); - void readOmega(std::shared_ptr>& args); + void readVelocity(std::vector& args); + void readOmega(std::vector& args); + void createASMT() override; - std::string strucString, name, type; + std::string strucString, type; FColDsptr rOfO, vOfO, omeOfO; FMatDsptr aAOf; }; diff --git a/OndselSolver/MBDynSystem.cpp b/OndselSolver/MBDynSystem.cpp index 920264d..6d20e83 100644 --- a/OndselSolver/MBDynSystem.cpp +++ b/OndselSolver/MBDynSystem.cpp @@ -10,11 +10,16 @@ #include "MBDynInitialValue.h" #include "MBDynData.h" #include "MBDynControlData.h" -#include "MBDynLabels.h" -#include "MBDynVariables.h" -#include "MBDynReferences.h" -#include "MBDynNodes.h" -#include "MBDynElements.h" +#include "ASMTAssembly.h" +#include "ASMTConstantGravity.h" +#include "ASMTTime.h" +#include "MBDynBody.h" +#include "MBDynStructural.h" +#include "SymbolicParser.h" +#include "BasicUserFunction.h" +#include "MBDynJoint.h" +#include "MBDynReference.h" +#include using namespace MbD; @@ -48,19 +53,95 @@ void MbD::MBDynSystem::parseMBDyn(std::vector& lines) } -std::shared_ptr MbD::MBDynSystem::mbdynVariables() +std::shared_ptr>> MbD::MBDynSystem::mbdynNodes() +{ + return nodes; +} + +std::shared_ptr> MbD::MBDynSystem::mbdynVariables() { return variables; } -std::shared_ptr MbD::MBDynSystem::mbdynReferences() +std::shared_ptr>> MbD::MBDynSystem::mbdynReferences() { return references; } +void MbD::MBDynSystem::createASMT() +{ + auto asmtAsm = std::make_shared(); + asmtAsm->mbdynItem = this; + asmtItem = asmtAsm; + asmtItem->setName("Assembly"); + initialValue->createASMT(); + for (auto& node : *nodes) node->createASMT(); + for (auto& element : *elements) element->createASMT(); +} + +std::shared_ptr MbD::MBDynSystem::nodeAt(std::string nodeName) +{ + for (auto& node : *nodes) { + if (node->name == nodeName) return node; + } + return nullptr; +} + +int MbD::MBDynSystem::nodeidAt(std::string nodeName) +{ + return labels->at(nodeName); +} + +std::shared_ptr MbD::MBDynSystem::bodyWithNode(std::string nodeName) +{ + for (auto& element : *elements) { + std::string str = typeid(element).name(); + if (str.find("MBDynBody") != std::string::npos) { + auto body = std::static_pointer_cast(element); + if (body->nodeName == nodeName) return body; + } + } + return nullptr; +} + +std::shared_ptr MbD::MBDynSystem::asmtAssembly() +{ + return std::static_pointer_cast(asmtItem); +} + +std::vector MbD::MBDynSystem::nodeNames() +{ + auto nodeNames = std::vector(); + for (auto& node : *nodes) { + nodeNames.push_back(node->name); + } + return nodeNames; +} + void MbD::MBDynSystem::runKINEMATIC() { - assert(false); + createASMT(); + std::static_pointer_cast(asmtItem)->runKINEMATIC(); + outputFiles(); +} + +void MbD::MBDynSystem::outputFiles() +{ + auto movFile = filename.substr(0, filename.find_last_of('.')) + ".mov"; + auto asmtAsm = asmtAssembly(); + auto asmtTimes = asmtAsm->times; + auto asmtParts = asmtAsm->parts; + auto asmtJoints = asmtAsm->joints; + auto asmtMotions = asmtAsm->motions; + std::ofstream os(movFile); + os << std::setprecision(std::numeric_limits::digits10 + 1); + for (int i = 1; i < asmtTimes->size(); i++) + { + for (auto& node : *nodes) { + node->outputLine(i, os); + } + } + os.close(); } void MbD::MBDynSystem::setFilename(std::string str) @@ -70,87 +151,75 @@ void MbD::MBDynSystem::setFilename(std::string str) void MbD::MBDynSystem::readDataBlock(std::vector& lines) { - std::vector tokens{"begin:", "data;"}; + std::vector tokens{ "begin:", "data" }; auto beginit = findLineWith(lines, tokens); - std::vector tokens1{"end:", "data;"}; + std::vector tokens1{ "end:", "data" }; auto endit = findLineWith(lines, tokens1); std::vector blocklines = { beginit, endit + 1 }; - dataBlk = std::make_shared(); - dataBlk->owner = this; - dataBlk->parseMBDyn(blocklines); + parseMBDynData(blocklines); lines.erase(beginit, endit + 1); } void MbD::MBDynSystem::readInitialValueBlock(std::vector& lines) { - std::vector tokens{"begin:", "initial", "value"}; + std::vector tokens{ "begin:", "initial", "value" }; auto beginit = findLineWith(lines, tokens); - std::vector tokens1{"end:", "initial", "value"}; + std::vector tokens1{ "end:", "initial", "value" }; auto endit = findLineWith(lines, tokens1); std::vector blocklines = { beginit, endit + 1 }; - initialValueBlk = std::make_shared(); - initialValueBlk->owner = this; - initialValueBlk->parseMBDyn(blocklines); + initialValue = std::make_shared(); + initialValue->owner = this; + initialValue->parseMBDyn(blocklines); lines.erase(beginit, endit + 1); } void MbD::MBDynSystem::readControlDataBlock(std::vector& lines) { - std::vector tokens{"begin:", "control", "data"}; + std::vector tokens{ "begin:", "control", "data" }; auto beginit = findLineWith(lines, tokens); - std::vector tokens1{"end:", "control", "data"}; + std::vector tokens1{ "end:", "control", "data" }; auto endit = findLineWith(lines, tokens1); std::vector blocklines = { beginit, endit + 1 }; - controlDataBlk = std::make_shared(); - controlDataBlk->owner = this; - controlDataBlk->parseMBDyn(blocklines); + controlData = std::make_shared(); + controlData->owner = this; + controlData->parseMBDyn(blocklines); lines.erase(beginit, endit + 1); } void MbD::MBDynSystem::readLabels(std::vector& lines) { - labels = std::make_shared(); - labels->owner = this; - labels->parseMBDyn(lines); + parseMBDynLabels(lines); } void MbD::MBDynSystem::readVariables(std::vector& lines) { - variables = std::make_shared(); - variables->owner = this; - variables->parseMBDyn(lines); + parseMBDynVariables(lines); } void MbD::MBDynSystem::readReferences(std::vector& lines) { - references = std::make_shared(); - references->owner = this; - references->parseMBDyn(lines); + parseMBDynReferences(lines); } void MbD::MBDynSystem::readNodesBlock(std::vector& lines) { - std::vector tokens{ "begin:", "nodes;" }; + std::vector tokens{ "begin:", "nodes" }; auto beginit = findLineWith(lines, tokens); - std::vector tokens1{ "end:", "nodes;" }; + std::vector tokens1{ "end:", "nodes" }; auto endit = findLineWith(lines, tokens1); std::vector blocklines = { beginit, endit + 1 }; - nodesBlk = std::make_shared(); - nodesBlk->owner = this; - nodesBlk->parseMBDyn(blocklines); + parseMBDynNodes(blocklines); lines.erase(beginit, endit + 1); } void MbD::MBDynSystem::readElementsBlock(std::vector& lines) { - std::vector tokens{ "begin:", "elements;" }; + std::vector tokens{ "begin:", "elements" }; auto beginit = findLineWith(lines, tokens); - std::vector tokens1{ "end:", "elements;" }; + std::vector tokens1{ "end:", "elements" }; auto endit = findLineWith(lines, tokens1); std::vector blocklines = { beginit, endit + 1 }; - elementsBlk = std::make_shared(); - elementsBlk->owner = this; - elementsBlk->parseMBDyn(blocklines); + parseMBDynElements(blocklines); lines.erase(beginit, endit + 1); } @@ -181,7 +250,7 @@ std::vector MbD::MBDynSystem::collectStatements(std::vector i + 1) { auto remainder = line.substr(i + 1); auto it = std::find_if(remainder.begin(), remainder.end(), [](unsigned char ch) { return !std::isspace(ch); }); @@ -201,3 +270,134 @@ std::vector MbD::MBDynSystem::collectStatements(std::vector& lines) +{ + assert(lines.size() == 3); + std::vector tokens{ "problem:", "initial", "value" }; + auto problemit = findLineWith(lines, tokens); + assert(problemit != lines.end()); + data = *problemit; +} + +void MbD::MBDynSystem::parseMBDynNodes(std::vector& lines) +{ + nodes = std::make_shared>>(); + std::vector tokens{ "structural:" }; + while (true) { + auto it = findLineWith(lines, tokens); + if (it != lines.end()) { + auto structural = std::make_shared(); + structural->owner = this; + structural->parseMBDyn(*it); + nodes->push_back(structural); + lines.erase(it); + } + else { + break; + } + } +} + +void MbD::MBDynSystem::parseMBDynElements(std::vector& lines) +{ + elements = std::make_shared>>(); + std::vector bodyToken{ "body:" }; + std::vector jointToken{ "joint:" }; + std::vector::iterator it; + while (true) { + it = findLineWith(lines, bodyToken); + if (it != lines.end()) { + auto body = std::make_shared(); + body->owner = this; + body->parseMBDyn(*it); + elements->push_back(body); + lines.erase(it); + continue; + } + it = findLineWith(lines, jointToken); + if (it != lines.end()) { + auto joint = std::make_shared(); + joint->owner = this; + joint->parseMBDyn(*it); + elements->push_back(joint); + lines.erase(it); + continue; + } + break; + } +} + +void MbD::MBDynSystem::parseMBDynVariables(std::vector& lines) +{ + variables = std::make_shared>(); + std::string str, variable; + double doubleValue; + std::vector tokens{ "set:", "real" }; + while (true) { + auto it = findLineWith(lines, tokens); + if (it != lines.end()) { + std::istringstream iss(*it); + iss >> str; + iss >> str; + iss >> variable; + iss >> str; + iss >> str; + auto parser = std::make_shared(); + parser->variables = variables; + auto userFunc = std::make_shared(str, 1.0); + parser->parseUserFunction(userFunc); + auto sym = parser->stack->top(); + variables->insert(std::make_pair(variable, sym)); + lines.erase(it); + } + else { + break; + } + } +} + +void MbD::MBDynSystem::parseMBDynLabels(std::vector& lines) +{ + labels = std::make_shared>(); + std::string str, label; + int intValue; + std::vector tokens{ "set:", "integer" }; + while (true) { + auto it = findLineWith(lines, tokens); + if (it != lines.end()) { + std::istringstream iss(*it); + iss >> str; + iss >> str; + iss >> label; + iss >> str; + iss >> intValue; + labels->insert(std::make_pair(label, intValue)); + lines.erase(it); + } + else { + break; + } + } +} + +void MbD::MBDynSystem::parseMBDynReferences(std::vector& lines) +{ + references = std::make_shared>>(); + std::string str, refName; + double doubleValue; + std::vector tokens{ "reference:" }; + while (true) { + auto it = findLineWith(lines, tokens); + if (it != lines.end()) { + auto reference = std::make_shared(); + reference->owner = this; + reference->parseMBDyn(*it); + references->insert(std::make_pair(reference->name, reference)); + lines.erase(it); + } + else { + break; + } + } +} diff --git a/OndselSolver/MBDynSystem.h b/OndselSolver/MBDynSystem.h index 69f3d3d..cbfb812 100644 --- a/OndselSolver/MBDynSystem.h +++ b/OndselSolver/MBDynSystem.h @@ -16,11 +16,11 @@ namespace MbD { class MBDynData; class MBDynInitialValue; class MBDynControlData; - class MBDynNodes; - class MBDynElements; - class MBDynVariables; - class MBDynLabels; - class MBDynReferences; + class MBDynNode; + class MBDynElement; + class MBDynVariable; + class MBDynLabel; + class MBDynReference; class MBDynSystem : public MBDynItem { @@ -30,10 +30,24 @@ namespace MbD { static std::vector collectStatements(std::vector& lines); void initialize() override; void parseMBDyn(std::vector& lines) override; - std::shared_ptr mbdynVariables() override; - std::shared_ptr mbdynReferences() override; + void parseMBDynData(std::vector& lines); + void parseMBDynNodes(std::vector& lines); + void parseMBDynElements(std::vector& lines); + void parseMBDynVariables(std::vector& lines); + void parseMBDynLabels(std::vector& lines); + void parseMBDynReferences(std::vector& lines); + std::shared_ptr>> mbdynNodes() override; + std::shared_ptr> mbdynVariables() override; + std::shared_ptr>> mbdynReferences() override; + void createASMT() override; + std::shared_ptr nodeAt(std::string nodeName) override; + int nodeidAt(std::string nodeName) override; + std::shared_ptr bodyWithNode(std::string nodeName) override; + std::shared_ptr asmtAssembly() override; + std::vector nodeNames() override; void runKINEMATIC(); + void outputFiles(); void setFilename(std::string filename); void readDataBlock(std::vector& lines); void readInitialValueBlock(std::vector& lines); @@ -45,14 +59,14 @@ namespace MbD { void readElementsBlock(std::vector& lines); std::string filename = ""; - std::shared_ptr dataBlk; - std::shared_ptr initialValueBlk; - std::shared_ptr controlDataBlk; - std::shared_ptr nodesBlk; - std::shared_ptr elementsBlk; - std::shared_ptr variables; - std::shared_ptr labels; - std::shared_ptr references; + std::string data; + std::shared_ptr initialValue; + std::shared_ptr controlData; + std::shared_ptr>> nodes; + std::shared_ptr>> elements; + std::shared_ptr> variables; + std::shared_ptr> labels; + std::shared_ptr>> references; }; } diff --git a/OndselSolver/MBDynVariables.cpp b/OndselSolver/MBDynVariables.cpp deleted file mode 100644 index ab2b704..0000000 --- a/OndselSolver/MBDynVariables.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "MBDynVariables.h" -#include "SymbolicParser.h" -#include "BasicUserFunction.h" - -using namespace MbD; - -void MbD::MBDynVariables::initialize() -{ -} - -void MbD::MBDynVariables::parseMBDyn(std::vector& lines) -{ - variables = std::make_shared>(); - std::string str, variable; - double doubleValue; - std::vector tokens{"set:", "real"}; - while (true) { - auto it = findLineWith(lines, tokens); - if (it != lines.end()) { - std::istringstream iss(*it); - iss >> str; - iss >> str; - iss >> variable; - iss >> str; - iss >> str; - auto parser = CREATE::With(); - parser->variables = variables; - auto userFunc = CREATE::With(str, 1.0); - parser->parseUserFunction(userFunc); - auto sym = parser->stack->top(); - variables->insert(std::make_pair(variable, sym)); - lines.erase(it); - } - else { - break; - } - } -} diff --git a/OndselSolver/MarkerFrame.h b/OndselSolver/MarkerFrame.h index 9942961..7c67474 100644 --- a/OndselSolver/MarkerFrame.h +++ b/OndselSolver/MarkerFrame.h @@ -72,9 +72,9 @@ namespace MbD { PartFrame* partFrame; //Use raw pointer when pointing backwards. FColDsptr rpmp = std::make_shared>(3); - FMatDsptr aApm = std::make_shared>(3, 3); + FMatDsptr aApm = FullMatrix::identitysptr(3); FColDsptr rOmO = std::make_shared>(3); - FMatDsptr aAOm = std::make_shared>(3, 3); + FMatDsptr aAOm = FullMatrix::identitysptr(3); FMatDsptr prOmOpE; FColFMatDsptr pAOmpE; FMatFColDsptr pprOmOpEpE; diff --git a/OndselSolver/MomentOfInertiaSolver.cpp b/OndselSolver/MomentOfInertiaSolver.cpp new file mode 100644 index 0000000..81abade --- /dev/null +++ b/OndselSolver/MomentOfInertiaSolver.cpp @@ -0,0 +1,157 @@ +#include "MomentOfInertiaSolver.h" +#include "EulerParameters.h" +#include + +using namespace MbD; + +void MbD::MomentOfInertiaSolver::example1() +{ + auto aJpp = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 2, 0 }, + { 0, 0, 3 } + }); + + auto rpPp = std::make_shared>(ListD{ 0, 0, 1 }); + auto axis = std::make_shared>(ListD{ 0, 0, 1 }); + auto aApP = std::make_shared>(axis, 0.1)->aA; + auto solver = std::make_shared(); + solver->setm(4.0); + solver->setJPP(aJpp); + solver->setrPoP(rpPp); + solver->setAPo(aApP); + auto rPcmP = std::make_shared>(ListD{ 0, 0, 0 }); + solver->setrPcmP(rPcmP); + solver->calc(); + auto aJPP = solver->getJoo(); + std::cout << solver->getJoo(); + std::cout << solver->getJpp(); + std::cout << solver->getAPp(); + std::cout << std::endl; + solver->setm(4.0); + solver->setJPP(aJPP); + auto rPoP = aApP->transposeTimesFullColumn(rpPp->negated()); + solver->setrPoP(rPoP); + auto aAPo = aApP->transpose(); + solver->setAPo(aAPo); + solver->setrPcmP(rPoP); + solver->calc(); + std::cout << solver->getJoo(); + std::cout << solver->getJpp(); + std::cout << solver->getAPp(); + std::cout << std::endl; +} + +void MbD::MomentOfInertiaSolver::forwardEliminateWithPivot(int p) +{ +} + +void MbD::MomentOfInertiaSolver::backSubstituteIntoDU() +{ +} + +void MbD::MomentOfInertiaSolver::postSolve() +{ +} + +FColDsptr MbD::MomentOfInertiaSolver::basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +{ + return FColDsptr(); +} + +FColDsptr MbD::MomentOfInertiaSolver::basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +{ + return FColDsptr(); +} + +void MbD::MomentOfInertiaSolver::preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) +{ +} + +void MbD::MomentOfInertiaSolver::preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) +{ +} + +double MbD::MomentOfInertiaSolver::getmatrixArowimaxMagnitude(int i) +{ + return 0.0; +} + +void MbD::MomentOfInertiaSolver::doPivoting(int p) +{ +} + +void MbD::MomentOfInertiaSolver::setm(double mass) +{ + m = mass; +} + +void MbD::MomentOfInertiaSolver::setJPP(FMatDsptr mat) +{ + aJPP = mat; +} + +void MbD::MomentOfInertiaSolver::setrPoP(FColDsptr col) +{ + rPoP = col; +} + +void MbD::MomentOfInertiaSolver::setAPo(FMatDsptr mat) +{ + aAPo = mat; +} + +void MbD::MomentOfInertiaSolver::setrPcmP(FColDsptr col) +{ + rPcmP = col; +} + +FMatDsptr MbD::MomentOfInertiaSolver::getJoo() +{ + return aJoo; +} + +FMatDsptr MbD::MomentOfInertiaSolver::getJpp() +{ + return aJpp; +} + +FMatDsptr MbD::MomentOfInertiaSolver::getAPp() +{ + return aAPp; +} + +void MbD::MomentOfInertiaSolver::calc() +{ + calcJoo(); + calcJpp(); + calcAPp(); +} + +void MbD::MomentOfInertiaSolver::calcJoo() +{ + //"aJoo = aAPoT*[aJPP + mass*(rPoPTilde*rPoPTilde + rPoPTilde*rocmPTilde + rocmPTilde*rPoPTilde)]*aAPo" + + if (!rPoP) { + rPoP = rPcmP; + aAPo = FullMatrix::identitysptr(3); + } + auto rocmPtilde = FullMatrix::tildeMatrix(rPcmP->minusFullColumn(rPoP)); + auto rPoPtilde = FullMatrix::tildeMatrix(rPoP); + auto term1 = aJPP; + auto term21 = rPoPtilde->timesFullMatrix(rPoPtilde); + auto term22 = rPoPtilde->timesFullMatrix(rocmPtilde); + auto term23 = term22->transpose(); + auto term2 = term21->plusFullMatrix(term22)->plusFullMatrix(term23)->times(m); + aJoo = aAPo->transposeTimesFullMatrix(term1->plusFullMatrix(term2))->timesFullMatrix(aAPo); + aJoo->symLowerWithUpper(); + aJoo->conditionSelfWithTol(aJoo->maxMagnitude() * 1.0e-6); +} + +void MbD::MomentOfInertiaSolver::calcJpp() +{ +} + +void MbD::MomentOfInertiaSolver::calcAPp() +{ +} diff --git a/OndselSolver/MomentOfInertiaSolver.h b/OndselSolver/MomentOfInertiaSolver.h new file mode 100644 index 0000000..a08f39e --- /dev/null +++ b/OndselSolver/MomentOfInertiaSolver.h @@ -0,0 +1,50 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "EigenDecomposition.h" + +namespace MbD { + class MomentOfInertiaSolver : public EigenDecomposition + { + //See document 9100moment.fodt + //m aJPP aJoo rPoP aAPo aJcmP aJcmPcopy rPcmP aJpp aAPp colOrder + //aJoo == aJpp when rPoP == rPcmP and aAPo == aAPp + public: + static void example1(); + void forwardEliminateWithPivot(int p) override; + void backSubstituteIntoDU() override; + void postSolve() override; + FColDsptr basicSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; + FColDsptr basicSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + void preSolvewithsaveOriginal(FMatDsptr fullMat, FColDsptr fullCol, bool saveOriginal) override; + void preSolvewithsaveOriginal(SpMatDsptr spMat, FColDsptr fullCol, bool saveOriginal) override; + double getmatrixArowimaxMagnitude(int i) override; + void doPivoting(int p) override; + + void setm(double mass); + void setJPP(FMatDsptr mat); + void setrPoP(FColDsptr col); + void setAPo(FMatDsptr mat); + void setrPcmP(FColDsptr col); + FMatDsptr getJoo(); + FMatDsptr getJpp(); + FMatDsptr getAPp(); + void calc(); + void calcJoo(); + void calcJpp(); + void calcAPp(); + + double m; + FMatDsptr aJPP, aJoo, aAPo, aJcmP, aJcmPcopy, aJpp, aAPp; + FColDsptr rPoP, rPcmP; + std::shared_ptr> colOrder; + + }; +} diff --git a/OndselSolver/OndselSolver.cpp b/OndselSolver/OndselSolver.cpp index 4ffea9c..b0da8b5 100644 --- a/OndselSolver/OndselSolver.cpp +++ b/OndselSolver/OndselSolver.cpp @@ -5,12 +5,12 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - -/********************************************************************* - * @file MbDCode.cpp - * - * @brief Program to assemble a piston crank system. - *********************************************************************/ + + /********************************************************************* + * @file MbDCode.cpp + * + * @brief Program to assemble a piston crank system. + *********************************************************************/ #include #include "CADSystem.h" @@ -24,10 +24,13 @@ void runSpMat(); int main() { - //MBDynSystem::runFile("crank_slider.mbd"); //To be completed + + //MBDynSystem::runFile("MBDynCase.mbd"); //To be completed + MBDynSystem::runFile("crank_slider.mbd"); //To be completed //ASMTAssembly::runSinglePendulumSuperSimplified(); //Mass is missing - ASMTAssembly::runSinglePendulumSimplified(); - ASMTAssembly::runSinglePendulum(); + //ASMTAssembly::runSinglePendulumSuperSimplified2(); //DOF has infinite acceleration due to zero mass and inertias + //ASMTAssembly::runSinglePendulumSimplified(); + ASMTAssembly::runSinglePendulum(); ASMTAssembly::runFile("piston.asmt"); ASMTAssembly::runFile("00backhoe.asmt"); //ASMTAssembly::runFile("circular.asmt"); //Needs checking diff --git a/OndselSolver/OndselSolver.vcxproj b/OndselSolver/OndselSolver.vcxproj index 7acc593..da879e1 100644 --- a/OndselSolver/OndselSolver.vcxproj +++ b/OndselSolver/OndselSolver.vcxproj @@ -159,12 +159,14 @@ + + @@ -206,6 +208,7 @@ + @@ -224,18 +227,15 @@ - - + - - - + @@ -445,12 +445,14 @@ + + @@ -492,6 +494,7 @@ + @@ -510,18 +513,15 @@ - - + - - - + @@ -715,6 +715,7 @@ + diff --git a/OndselSolver/OndselSolver.vcxproj.filters b/OndselSolver/OndselSolver.vcxproj.filters index 582eecd..b5257f6 100644 --- a/OndselSolver/OndselSolver.vcxproj.filters +++ b/OndselSolver/OndselSolver.vcxproj.filters @@ -819,9 +819,6 @@ Source Files - - Source Files - Source Files @@ -831,18 +828,9 @@ Source Files - - Source Files - Source Files - - Source Files - - - Source Files - Source Files @@ -858,15 +846,27 @@ Source Files - - Source Files - Source Files Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -1679,9 +1679,6 @@ Header Files - - Header Files - Header Files @@ -1691,18 +1688,9 @@ Header Files - - Header Files - Header Files - - Header Files - - - Header Files - Header Files @@ -1718,15 +1706,27 @@ Header Files - - Header Files - Header Files Header Files + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + @@ -1746,5 +1746,6 @@ + \ No newline at end of file diff --git a/OndselSolver/PlanarJoint.cpp b/OndselSolver/PlanarJoint.cpp index 9ee86bf..5475775 100644 --- a/OndselSolver/PlanarJoint.cpp +++ b/OndselSolver/PlanarJoint.cpp @@ -30,6 +30,6 @@ void MbD::PlanarJoint::initializeGlobally() this->root()->hasChanged = true; } else { - InPlaneJoint::initializeGlobally(); + Joint::initializeGlobally(); } } diff --git a/OndselSolver/PointInLineJoint.cpp b/OndselSolver/PointInLineJoint.cpp index bc0e242..9ecafae 100644 --- a/OndselSolver/PointInLineJoint.cpp +++ b/OndselSolver/PointInLineJoint.cpp @@ -28,6 +28,6 @@ void MbD::PointInLineJoint::initializeGlobally() this->root()->hasChanged = true; } else { - InLineJoint::initializeGlobally(); + Joint::initializeGlobally(); } } diff --git a/OndselSolver/PointInPlaneJoint.cpp b/OndselSolver/PointInPlaneJoint.cpp index 944b926..f3c5994 100644 --- a/OndselSolver/PointInPlaneJoint.cpp +++ b/OndselSolver/PointInPlaneJoint.cpp @@ -27,6 +27,6 @@ void MbD::PointInPlaneJoint::initializeGlobally() this->root()->hasChanged = true; } else { - InPlaneJoint::initializeGlobally(); + Joint::initializeGlobally(); } } diff --git a/OndselSolver/PosKineNewtonRaphson.cpp b/OndselSolver/PosKineNewtonRaphson.cpp index e2ffa6c..7071609 100644 --- a/OndselSolver/PosKineNewtonRaphson.cpp +++ b/OndselSolver/PosKineNewtonRaphson.cpp @@ -5,12 +5,13 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #include "PosKineNewtonRaphson.h" #include "SystemSolver.h" #include "Part.h" #include "NotKinematicError.h" #include "Constraint.h" +#include using namespace MbD; @@ -80,5 +81,10 @@ void PosKineNewtonRaphson::preRun() void PosKineNewtonRaphson::fillY() { y->zeroSelf(); - system->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillPosKineError(y); }); + system->partsJointsMotionsDo([&](std::shared_ptr item) { + item->fillPosKineError(y); + //std::cout << item->name << *y << std::endl; + //noop(); + }); + //std::cout << "Final" << *y << std::endl; } diff --git a/OndselSolver/Solver.cpp b/OndselSolver/Solver.cpp index 953abca..4b966bd 100644 --- a/OndselSolver/Solver.cpp +++ b/OndselSolver/Solver.cpp @@ -13,6 +13,11 @@ using namespace MbD; +void MbD::Solver::noop() +{ + //No Operations +} + void Solver::initialize() { } diff --git a/OndselSolver/Solver.h b/OndselSolver/Solver.h index bd90905..3e252b4 100644 --- a/OndselSolver/Solver.h +++ b/OndselSolver/Solver.h @@ -15,6 +15,7 @@ namespace MbD { { //statistics public: + void noop(); virtual void initialize(); virtual void initializeLocally(); virtual void initializeGlobally(); diff --git a/OndselSolver/SymbolicParser.cpp b/OndselSolver/SymbolicParser.cpp index a4d9832..42f68d3 100644 --- a/OndselSolver/SymbolicParser.cpp +++ b/OndselSolver/SymbolicParser.cpp @@ -26,6 +26,13 @@ #include "GeneralSpline.h" #include "ArcSine.h" +MbD::SymbolicParser::SymbolicParser() +{ + variables = std::make_shared>(); + stack = std::make_shared>(); + buffer = std::make_shared(); +} + void MbD::SymbolicParser::initialize() { variables = std::make_shared>(); diff --git a/OndselSolver/SymbolicParser.h b/OndselSolver/SymbolicParser.h index 47283f1..72b46a5 100644 --- a/OndselSolver/SymbolicParser.h +++ b/OndselSolver/SymbolicParser.h @@ -21,6 +21,7 @@ namespace MbD { { // public: + SymbolicParser(); void initialize(); void parseUserFunction(Symsptr userFunc); void parseString(std::string expr); diff --git a/OndselSolver/System.cpp b/OndselSolver/System.cpp index eee074c..d10d62e 100644 --- a/OndselSolver/System.cpp +++ b/OndselSolver/System.cpp @@ -21,6 +21,12 @@ using namespace MbD; System::System() { + externalSystem = std::make_shared(); + time = std::make_shared