From 2b1ec21b3a0e8a193abe57cf7225068426b5bbfb Mon Sep 17 00:00:00 2001 From: Aik-Siong Koh Date: Tue, 26 Sep 2023 14:03:05 -0600 Subject: [PATCH] add runOndselDoublePendulum for simple case --- MbDCode/ASMTSpatialContainer.cpp | 1 - MbDCode/Array.h | 2 +- MbDCode/CADSystem.cpp | 175 +++++++++++++++++++++++++++++++ MbDCode/CADSystem.h | 1 + MbDCode/FullColumn.h | 2 +- MbDCode/FullRow.h | 2 +- MbDCode/FullVector.h | 2 +- MbDCode/GeneralSpline.cpp | 36 +++---- MbDCode/MarkerFrame.cpp | 4 +- MbDCode/MarkerFrame.h | 2 +- MbDCode/MbDCode.cpp | 23 ++-- MbDCode/MbDCode.vcxproj | 6 ++ MbDCode/Part.cpp | 4 + MbDCode/Part.h | 1 + MbDCode/PartFrame.cpp | 5 + MbDCode/PartFrame.h | 1 + 16 files changed, 230 insertions(+), 37 deletions(-) diff --git a/MbDCode/ASMTSpatialContainer.cpp b/MbDCode/ASMTSpatialContainer.cpp index 0d3a382..4d8aac2 100644 --- a/MbDCode/ASMTSpatialContainer.cpp +++ b/MbDCode/ASMTSpatialContainer.cpp @@ -336,7 +336,6 @@ void MbD::ASMTSpatialContainer::compareResults(AnalysisType type) auto i = xs->size() - 1; //Pos if (!Numeric::equaltol(xs->at(i), inxs->at(i), lengthTol)) { - std::cout << i << " xs " << xs->at(i) << ", " << i<< lengthTol << std::endl; std::cout << i << " xs " << xs->at(i) << ", " << inxs->at(i) << ", " << lengthTol << std::endl; } if (!Numeric::equaltol(ys->at(i), inys->at(i), lengthTol)) { diff --git a/MbDCode/Array.h b/MbDCode/Array.h index b654812..aaa4ca0 100644 --- a/MbDCode/Array.h +++ b/MbDCode/Array.h @@ -29,7 +29,7 @@ namespace MbD { Array(std::vector vec) : std::vector(vec) {} Array(int count) : std::vector(count) {} Array(int count, const T& value) : std::vector(count, value) {} - Array(std::vector::iterator begin, std::vector::iterator end) : std::vector(begin, end) {} + Array(typename typename std::vector::iterator begin, typename typename std::vector::iterator end) : std::vector(begin, end) {} Array(std::initializer_list list) : std::vector{ list } {} virtual void initialize(); void copyFrom(std::shared_ptr> x); diff --git a/MbDCode/CADSystem.cpp b/MbDCode/CADSystem.cpp index 79b1190..c7c6d11 100644 --- a/MbDCode/CADSystem.cpp +++ b/MbDCode/CADSystem.cpp @@ -24,6 +24,7 @@ #include "PartFrame.h" #include "Time.h" #include "StateData.h" +#include "EulerParameters.h" using namespace MbD; @@ -48,6 +49,180 @@ void CADSystem::logString(double value) { } +void CADSystem::runOndselDoublePendulum() +{ + //Double pendulum with easy input numbers for exact port from Smalltalk + //GEOAssembly calcCharacteristicDimensions must set mbdUnits to unity. + std::cout << "runOndselDoublePendulum" << std::endl; + auto& TheSystem = mbdSystem; + TheSystem->clear(); + std::string name = "TheSystem"; + TheSystem->name = name; + std::cout << "TheSystem->name " << TheSystem->name << std::endl; + auto& systemSolver = TheSystem->systemSolver; + systemSolver->errorTolPosKine = 1.0e-6; + systemSolver->errorTolAccKine = 1.0e-6; + systemSolver->iterMaxPosKine = 25; + systemSolver->iterMaxAccKine = 25; + systemSolver->tstart = 0.0; + systemSolver->tend = 0.0; + systemSolver->hmin = 1.0e-9; + systemSolver->hmax = 1.0; + systemSolver->hout = 0.04; + systemSolver->corAbsTol = 1.0e-6; + systemSolver->corRelTol = 1.0e-6; + systemSolver->intAbsTol = 1.0e-6; + systemSolver->intRelTol = 1.0e-6; + systemSolver->iterMaxDyn = 4; + systemSolver->orderMax = 5; + systemSolver->translationLimit = 1.0e10; + systemSolver->rotationLimit = 0.5; + + std::string str; + FColDsptr qX, qE, qXdot, omeOpO, qXddot, alpOpO; + FColDsptr rpmp; + FMatDsptr aAap, aApm; + FRowDsptr fullRow; + // + auto assembly1 = CREATE::With("/Assembly1"); + std::cout << "assembly1->name " << assembly1->name << std::endl; + assembly1->m = 0.0; + assembly1->aJ = std::make_shared>(ListD{ 0, 0, 0 }); + qX = std::make_shared>(ListD{ 0, 0, 0 }); + aAap = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + assembly1->setqX(qX); + assembly1->setaAap(aAap); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + assembly1->setqXdot(qXdot); + assembly1->setomeOpO(omeOpO); + TheSystem->addPart(assembly1); + { + auto& partFrame = assembly1->partFrame; + auto marker2 = CREATE::With("/Assembly1/Marker2"); + rpmp = std::make_shared>(ListD{ 0.0, 0.0, 0.0 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + // + auto marker1 = CREATE::With("/Assembly1/Marker1"); + rpmp = std::make_shared>(ListD{ 0.0, 3.0, 0.0 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 0, 1 }, + { 0, -1, 0 } + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + } + assembly1->asFixed(); + // + auto crankPart1 = CREATE::With("/Assembly1/Part1"); + std::cout << "crankPart1->name " << crankPart1->name << std::endl; + crankPart1->m = 1.0; + crankPart1->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + qX = std::make_shared>(ListD{ 0.4, 0.0, -0.05 }); + aAap = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + crankPart1->setqX(qX); + crankPart1->setaAap(aAap); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + crankPart1->setqXdot(qXdot); + crankPart1->setomeOpO(omeOpO); + TheSystem->addPart(crankPart1); + { + auto& partFrame = crankPart1->partFrame; + auto marker1 = CREATE::With("/Assembly1/Part1/Marker1"); + rpmp = std::make_shared>(ListD{ -0.4, 0.0, 0.05 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + auto marker2 = CREATE::With("/Assembly1/Part1/Marker2"); + rpmp = std::make_shared>(ListD{ 0.4, 0.0, 0.05 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 } + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto conrodPart2 = CREATE::With("/Assembly1/Part2"); + std::cout << "conrodPart2->name " << conrodPart2->name << std::endl; + conrodPart2->m = 1.0; + conrodPart2->aJ = std::make_shared>(ListD{ 1, 1, 1 }); + qX = std::make_shared>(ListD{ 0.15, 0.1, 0.05 }); + qE = std::make_shared>(ListD{ 0.0, 0.0, 1.0, 0.0 }); + auto eulerParameters = CREATE>::With(ListD{ 0.0, 0.0, 1.0, 0.0 }); + eulerParameters->calcABC(); + aAap = eulerParameters->aA; + conrodPart2->setqX(qX); + conrodPart2->setaAap(aAap); + qXdot = std::make_shared>(ListD{ 0, 0, 0 }); + omeOpO = std::make_shared>(ListD{ 0, 0, 0 }); + conrodPart2->setqXdot(qXdot); + conrodPart2->setomeOpO(omeOpO); + TheSystem->addPart(conrodPart2); + { + auto& partFrame = conrodPart2->partFrame; + auto marker1 = CREATE::With("/Assembly1/Part2/Marker1"); + rpmp = std::make_shared>(ListD{ -0.65, 0.0, -0.05 }); + marker1->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }); + marker1->setaApm(aApm); + partFrame->addMarkerFrame(marker1); + // + auto marker2 = CREATE::With("/Assembly1/Part2/Marker2"); + rpmp = std::make_shared>(ListD{ 0.65, 0.0, -0.05 }); + marker2->setrpmp(rpmp); + aApm = std::make_shared>(ListListD{ + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0} + }); + marker2->setaApm(aApm); + partFrame->addMarkerFrame(marker2); + } + // + auto revJoint1 = CREATE::With("/Assembly1/Joint1"); + std::cout << "revJoint1->name " << revJoint1->name << std::endl; + revJoint1->connectsItoJ(assembly1->partFrame->endFrame("/Assembly1/Marker2"), crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker1")); + TheSystem->addJoint(revJoint1); + + auto revJoint2 = CREATE::With("/Assembly1/Joint2"); + std::cout << "revJoint2->name " << revJoint2->name << std::endl; + revJoint2->connectsItoJ(crankPart1->partFrame->endFrame("/Assembly1/Part1/Marker2"), conrodPart2->partFrame->endFrame("/Assembly1/Part2/Marker1")); + TheSystem->addJoint(revJoint2); + // + TheSystem->runKINEMATIC(TheSystem); +} + void CADSystem::runOndselPiston() { //Piston with easy input numbers for exact port from Smalltalk diff --git a/MbDCode/CADSystem.h b/MbDCode/CADSystem.h index 9cc0d7c..f2271d6 100644 --- a/MbDCode/CADSystem.h +++ b/MbDCode/CADSystem.h @@ -27,6 +27,7 @@ namespace MbD { void outputFor(AnalysisType type); void logString(std::string& str); void logString(double value); + void runOndselDoublePendulum(); void runOndselPiston(); void runPiston(); void preMbDrun(std::shared_ptr mbdSys); diff --git a/MbDCode/FullColumn.h b/MbDCode/FullColumn.h index e9dcb36..b72b006 100644 --- a/MbDCode/FullColumn.h +++ b/MbDCode/FullColumn.h @@ -33,7 +33,7 @@ namespace MbD { FullColumn(std::vector vec) : FullVector(vec) {} FullColumn(int count) : FullVector(count) {} FullColumn(int count, const T& value) : FullVector(count, value) {} - FullColumn(std::vector::iterator begin, std::vector::iterator end) : FullVector(begin, end) {} + FullColumn(typename std::vector::iterator begin, typename std::vector::iterator end) : FullVector(begin, end) {} FullColumn(std::initializer_list list) : FullVector{ list } {} FColsptr plusFullColumn(FColsptr fullCol); FColsptr minusFullColumn(FColsptr fullCol); diff --git a/MbDCode/FullRow.h b/MbDCode/FullRow.h index 8ea5619..2ed2052 100644 --- a/MbDCode/FullRow.h +++ b/MbDCode/FullRow.h @@ -35,7 +35,7 @@ namespace MbD { FullRow(std::vector vec) : FullVector(vec) {} FullRow(int count) : FullVector(count) {} FullRow(int count, const T& value) : FullVector(count, value) {} - FullRow(std::vector::iterator begin, std::vector::iterator end) : FullVector(begin, end) {} + FullRow(typename std::vector::iterator begin, typename std::vector::iterator end) : FullVector(begin, end) {} FullRow(std::initializer_list list) : FullVector{ list } {} FRowsptr times(T a); FRowsptr negated(); diff --git a/MbDCode/FullVector.h b/MbDCode/FullVector.h index c4193d0..0216460 100644 --- a/MbDCode/FullVector.h +++ b/MbDCode/FullVector.h @@ -21,7 +21,7 @@ namespace MbD { FullVector(std::vector vec) : Array(vec) {} FullVector(int count) : Array(count) {} FullVector(int count, const T& value) : Array(count, value) {} - FullVector(std::vector::iterator begin, std::vector::iterator end) : Array(begin, end) {} + FullVector(typename std::vector::iterator begin, typename std::vector::iterator end) : Array(begin, end) {} FullVector(std::initializer_list list) : Array{ list } {} double dot(std::shared_ptr> vec); void atiplusNumber(int i, T value); diff --git a/MbDCode/GeneralSpline.cpp b/MbDCode/GeneralSpline.cpp index ba7721b..4f6c054 100644 --- a/MbDCode/GeneralSpline.cpp +++ b/MbDCode/GeneralSpline.cpp @@ -5,7 +5,7 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #include #include "GeneralSpline.h" @@ -27,7 +27,7 @@ double MbD::GeneralSpline::getValue() Symsptr MbD::GeneralSpline::differentiateWRTx() { auto self = clonesptr(); - auto arg = std::static_pointer_cast(self)->xx; + auto& arg = std::static_pointer_cast(self)->xx; auto deriv = std::make_shared(arg, self, 1); return deriv; } @@ -35,14 +35,14 @@ Symsptr MbD::GeneralSpline::differentiateWRTx() void MbD::GeneralSpline::arguments(Symsptr args) { auto array = args->getTerms(); - auto arg = array->at(0); + auto& arg = array->at(0); int order = (int)array->at(1)->getValue(); - int n = (array->size() - 2) / 2; + int n = ((int)array->size() - 2) / 2; auto xarray = std::make_shared>(n); auto yarray = std::make_shared>(n); for (int i = 0; i < n; i++) { - size_t ii = 2 * (i + 1); + size_t ii = 2 * ((size_t)i + 1); xarray->at(i) = array->at(ii)->getValue(); yarray->at(i) = array->at(ii + 1)->getValue(); } @@ -63,7 +63,7 @@ void MbD::GeneralSpline::computeDerivatives() { //"See derivation in MbDTheory 9spline.fodt." if (degree == 0) throw std::runtime_error("ToDo: Use ZeroDegreeSpline"); - auto n = xs->size(); + auto n = (int)xs->size(); auto p = degree; auto np = n * p; auto matrix = std::make_shared>(np, np); @@ -72,7 +72,7 @@ void MbD::GeneralSpline::computeDerivatives() auto hmax = 0.0; for (int i = 0; i < n - 1; i++) { - auto h = xs->at(i + 1) - xs->at(i); + auto h = xs->at((size_t)i + 1) - xs->at(i); hmax = std::max(hmax, std::abs(h)); hs->atiput(i, h); } @@ -94,7 +94,7 @@ void MbD::GeneralSpline::computeDerivatives() matrix->atijput(offset + k - j, offset + k, dum); } } - bvector->atiput(offset, ys->at(i + 1) - ys->at(i)); + bvector->atiput(offset, ys->at((size_t)i + 1) - ys->at(i)); } if (isCyclic()) { for (int j = 1; j < p + 1; j++) @@ -127,7 +127,7 @@ void MbD::GeneralSpline::computeDerivatives() } for (int i = 0; i < n; i++) { - auto derivsi = derivs->at(i); + auto& derivsi = derivs->at(i); derivsi->equalArrayAt(derivsVector, (i - 1) * p + 1); for (int j = 0; j < p; j++) { @@ -147,13 +147,13 @@ double MbD::GeneralSpline::derivativeAt(int n, double xxx) //"d2ydx2(x) := d2ydx2i + d3ydx3i*hi + d4ydx4i*hi^2/2! +" if (n > degree) return 0.0; calcIndexAndDeltaFor(xxx); - auto derivsi = derivs->at(index); + auto& derivsi = derivs->at(index); auto sum = 0.0; for (int j = degree; j >= n + 1; j--) { - sum = (sum + derivsi->at(j - 1)) * delta / (j - n); + sum = (sum + derivsi->at((size_t)j - 1)) * delta / (j - n); } - return derivsi->at(n - 1) + sum; + return derivsi->at((size_t)n - 1) + sum; } void MbD::GeneralSpline::calcIndexAndDeltaFor(double xxx) @@ -185,7 +185,7 @@ void MbD::GeneralSpline::calcNonCyclicIndexAndDelta() } else { if (xvalue >= xLast) { - index = xs->size() - 1; + index = (int)xs->size() - 1; delta = xvalue - xLast; } else { @@ -196,8 +196,8 @@ void MbD::GeneralSpline::calcNonCyclicIndexAndDelta() void MbD::GeneralSpline::calcIndexAndDelta() { - if (!(index < xs->size() - 1) || !(xs->at(index) <= xvalue) || !(xvalue < xs->at(index + 1))) { - searchIndexFromto(0, xs->size()); //Using range. + if (!(index < xs->size() - 1) || !(xs->at(index) <= xvalue) || !(xvalue < xs->at((size_t)index + 1))) { + searchIndexFromto(0, (int)xs->size()); //Using range. } delta = xvalue - xs->at(index); } @@ -209,7 +209,7 @@ void MbD::GeneralSpline::searchIndexFromto(int first, int last) index = first; } else { - auto middle = std::floor((first + last) / 2); + auto middle = (int)std::floor((first + last) / 2); if (xvalue < xs->at(middle)) { searchIndexFromto(first, middle); } @@ -229,11 +229,11 @@ double MbD::GeneralSpline::y(double xxx) //"y(x) := yi + dydxi*hi + d2ydx2i*hi^2/2! + d3ydx3i*hi^3/3! +" calcIndexAndDeltaFor(xxx); - auto derivsi = derivs->at(index); + auto& derivsi = derivs->at(index); auto sum = 0.0; for (int j = degree; j >= 1; j--) { - sum = (sum + derivsi->at(j - 1)) * delta / j; + sum = (sum + derivsi->at((size_t)j - 1)) * delta / j; } return ys->at(index) + sum; } diff --git a/MbDCode/MarkerFrame.cpp b/MbDCode/MarkerFrame.cpp index 08eb95e..db8fd07 100644 --- a/MbDCode/MarkerFrame.cpp +++ b/MbDCode/MarkerFrame.cpp @@ -243,9 +243,9 @@ void MarkerFrame::setrpmp(FColDsptr x) rpmp->copyFrom(x); } -void MarkerFrame::setaApm(FMatDsptr x) +void MarkerFrame::setaApm(FMatDsptr mat) { - aApm->copyFrom(x); + aApm->copyFrom(mat); } void MarkerFrame::addEndFrame(EndFrmsptr endFrm) { diff --git a/MbDCode/MarkerFrame.h b/MbDCode/MarkerFrame.h index 8b441fc..9942961 100644 --- a/MbDCode/MarkerFrame.h +++ b/MbDCode/MarkerFrame.h @@ -34,7 +34,7 @@ namespace MbD { void setPartFrame(PartFrame* partFrm); PartFrame* getPartFrame(); void setrpmp(FColDsptr x); - void setaApm(FMatDsptr x); + void setaApm(FMatDsptr mat); void addEndFrame(EndFrmsptr x); void initializeLocally() override; void initializeGlobally() override; diff --git a/MbDCode/MbDCode.cpp b/MbDCode/MbDCode.cpp index 52212fe..710feb8 100644 --- a/MbDCode/MbDCode.cpp +++ b/MbDCode/MbDCode.cpp @@ -26,19 +26,20 @@ void runSpMat(); int main() { - ASMTAssembly::runFile("piston.asmt"); - ASMTAssembly::runFile("00backhoe.asmt"); - ASMTAssembly::runFile("circular.asmt"); - ASMTAssembly::runFile("cirpendu.asmt"); //Under constrained. Testing ICKine. - ASMTAssembly::runFile("engine1.asmt"); - ASMTAssembly::runFile("fourbar.asmt"); - ASMTAssembly::runFile("fourbot.asmt"); - ASMTAssembly::runFile("wobpump.asmt"); + //ASMTAssembly::runFile("piston.asmt"); + //ASMTAssembly::runFile("00backhoe.asmt"); + //ASMTAssembly::runFile("circular.asmt"); + //ASMTAssembly::runFile("cirpendu.asmt"); //Under constrained. Testing ICKine. + //ASMTAssembly::runFile("engine1.asmt"); + //ASMTAssembly::runFile("fourbar.asmt"); + //ASMTAssembly::runFile("fourbot.asmt"); + //ASMTAssembly::runFile("wobpump.asmt"); auto cadSystem = std::make_shared(); - cadSystem->runOndselPiston(); - cadSystem->runPiston(); - runSpMat(); + cadSystem->runOndselDoublePendulum(); + ////cadSystem->runOndselPiston(); + //cadSystem->runPiston(); + //runSpMat(); } void runSpMat() { diff --git a/MbDCode/MbDCode.vcxproj b/MbDCode/MbDCode.vcxproj index aaace72..30fa987 100644 --- a/MbDCode/MbDCode.vcxproj +++ b/MbDCode/MbDCode.vcxproj @@ -76,6 +76,7 @@ true WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) true + true Console @@ -90,6 +91,7 @@ true WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true + true Console @@ -104,6 +106,8 @@ true _DEBUG;_CONSOLE;%(PreprocessorDefinitions) true + stdcpp17 + true Console @@ -118,6 +122,8 @@ true NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true + stdcpp17 + true Console diff --git a/MbDCode/Part.cpp b/MbDCode/Part.cpp index 1c52f81..6186dbf 100644 --- a/MbDCode/Part.cpp +++ b/MbDCode/Part.cpp @@ -61,6 +61,10 @@ FColDsptr Part::getqX() { return partFrame->getqX(); } +void Part::setaAap(FMatDsptr mat) { + partFrame->setaAap(mat); +} + void Part::setqE(FColDsptr x) { partFrame->setqE(x); } diff --git a/MbDCode/Part.h b/MbDCode/Part.h index 65e2119..399f18f 100644 --- a/MbDCode/Part.h +++ b/MbDCode/Part.h @@ -32,6 +32,7 @@ namespace MbD { void initializeGlobally() override; void setqX(FColDsptr x); FColDsptr getqX(); + void setaAap(FMatDsptr mat); void setqE(FColDsptr x); FColDsptr getqE(); void setqXdot(FColDsptr x); diff --git a/MbDCode/PartFrame.cpp b/MbDCode/PartFrame.cpp index d432566..dafcf89 100644 --- a/MbDCode/PartFrame.cpp +++ b/MbDCode/PartFrame.cpp @@ -64,6 +64,11 @@ void PartFrame::setqE(FColDsptr x) { qE->copyFrom(x); } +void MbD::PartFrame::setaAap(FMatDsptr mat) +{ + qE = mat->asEulerParameters(); +} + FColDsptr PartFrame::getqE() { return qE; } diff --git a/MbDCode/PartFrame.h b/MbDCode/PartFrame.h index aa17c48..24bd58b 100644 --- a/MbDCode/PartFrame.h +++ b/MbDCode/PartFrame.h @@ -42,6 +42,7 @@ namespace MbD { void setqX(FColDsptr x); FColDsptr getqX(); void setqE(FColDsptr x); + void setaAap(FMatDsptr mat); FColDsptr getqE(); void setqXdot(FColDsptr x); FColDsptr getqXdot();