diff --git a/.gitignore b/.gitignore index b62b7a8..41d29b1 100644 --- a/.gitignore +++ b/.gitignore @@ -39,3 +39,4 @@ assembly.asmt build cmake-build-debug .idea +temp/ \ No newline at end of file diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index a9e55f4..9856394 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -441,6 +441,12 @@ void MbD::ASMTAssembly::runFile(const char* fileName) } } +void MbD::ASMTAssembly::runDraggingLogTest() +{ + auto assembly = ASMTAssembly::assemblyFromFile("../testapp/runPreDrag.asmt"); + assembly->runDraggingLog("../testapp/dragging.log"); +} + void MbD::ASMTAssembly::runDraggingTest() { //auto assembly = ASMTAssembly::assemblyFromFile("../testapp/pistonWithLimits.asmt"); @@ -915,7 +921,12 @@ void MbD::ASMTAssembly::readTimes(std::vector& lines) assert(pos != std::string::npos); str.erase(0, pos + substr.length()); times = readRowOfDoubles(str); - times->insert(times->begin(), times->at(0)); //The first element is the input state. + if (times->empty()) { + times->insert(times->begin(), 0.0); //The first element is the input state. + } + else { + times->insert(times->begin(), times->at(0)); //The first element is the input state. + } lines.erase(lines.begin()); } @@ -1035,6 +1046,47 @@ void MbD::ASMTAssembly::readMotionSeries(std::vector& lines) motion->readMotionSeries(lines); } +void MbD::ASMTAssembly::runDraggingLog(const char* fileName) +{ + std::ifstream stream(fileName); + if (stream.fail()) { + throw std::invalid_argument("File not found."); + } + std::string line; + std::vector lines; + while (std::getline(stream, line)) { + lines.push_back(line); + } + assert(readStringOffTop(lines) == "runPreDrag"); + runPreDrag(); + while (lines[0].find("runDragStep") != std::string::npos) + { + assert(readStringOffTop(lines) == "runDragStep"); + auto dragParts = std::make_shared>>(); + while (lines[0].find("Name") != std::string::npos) { + assert(readStringOffTop(lines) == "Name"); + auto dragPartName = readStringOffTop(lines); + std::string longerName = "/" + name + "/" + dragPartName; + auto dragPart = partAt(longerName); + dragParts->push_back(dragPart); + assert(readStringOffTop(lines) == "Position3D"); + auto dragPartPosition3D = readColumnOfDoublesOffTop(lines); + dragPart->updateMbDFromPosition3D(dragPartPosition3D); + assert(readStringOffTop(lines) == "RotationMatrix"); + auto dragPartRotationMatrix = std::make_shared>(3); + for (size_t i = 0; i < 3; i++) + { + auto row = readRowOfDoublesOffTop(lines); + dragPartRotationMatrix->atiput(i, row); + } + dragPart->updateMbDFromRotationMatrix(dragPartRotationMatrix); + } + runDragStep(dragParts); + } + assert(readStringOffTop(lines) == "runPostDrag"); + runPostDrag(); +} + void MbD::ASMTAssembly::outputFor(AnalysisType) { assert(false); @@ -1236,15 +1288,10 @@ void MbD::ASMTAssembly::runPreDrag() } mbdSystem = std::make_shared(); mbdSystem->externalSystem->asmtAssembly = this; - try { - mbdSystem->runPreDrag(mbdSystem); - } - catch (SimulationStoppingError ex) { - - } + mbdSystem->runPreDrag(mbdSystem); } -void MbD::ASMTAssembly::runDragStep(std::shared_ptr>> dragParts) const +void MbD::ASMTAssembly::runDragStep(std::shared_ptr>> dragParts) { if (debug) { std::ofstream os("dragging.log", std::ios_base::app); @@ -1264,7 +1311,12 @@ void MbD::ASMTAssembly::runDragStep(std::shared_ptr(dragPart->mbdObject); dragMbDParts->push_back(dragMbDPart); } - mbdSystem->runDragStep(dragMbDParts); + try { + mbdSystem->runDragStep(dragMbDParts); + } + catch (...) { + runPreDrag(); + } } void MbD::ASMTAssembly::runPostDrag() @@ -1278,12 +1330,7 @@ void MbD::ASMTAssembly::runPostDrag() debug = false; mbdSystem = std::make_shared(); mbdSystem->externalSystem->asmtAssembly = this; - try { - mbdSystem->runPreDrag(mbdSystem); - } - catch (SimulationStoppingError ex) { - - } + mbdSystem->runPreDrag(mbdSystem); } void MbD::ASMTAssembly::runKINEMATIC() @@ -1318,6 +1365,14 @@ std::shared_ptr MbD::ASMTAssembly::spatialContainerAt(std: return part; } +std::shared_ptr MbD::ASMTAssembly::partAt(std::string& longname) const +{ + for (auto& part : *parts) { + if (part->fullName("") == longname) return part; + } + return nullptr; +} + std::shared_ptr MbD::ASMTAssembly::markerAt(std::string& longname) const { for (auto& refPoint : *refPoints) { diff --git a/OndselSolver/ASMTAssembly.h b/OndselSolver/ASMTAssembly.h index 0c6373e..40e6db6 100644 --- a/OndselSolver/ASMTAssembly.h +++ b/OndselSolver/ASMTAssembly.h @@ -41,6 +41,7 @@ namespace MbD { static void runSinglePendulum(); static std::shared_ptr assemblyFromFile(const char* chars); static void runFile(const char* chars); + static void runDraggingLogTest(); static void runDraggingTest(); static void runDraggingTest2(); static void runDraggingTest3(); @@ -73,6 +74,7 @@ namespace MbD { void readJointSeries(std::vector& lines); void readMotionSeriesMany(std::vector& lines); void readMotionSeries(std::vector& lines); + void runDraggingLog(const char* chars); void outputFor(AnalysisType type); void preMbDrun(std::shared_ptr mbdSys); @@ -92,11 +94,12 @@ namespace MbD { void solve(); void runPreDrag(); - void runDragStep(std::shared_ptr>> dragParts) const; + void runDragStep(std::shared_ptr>> dragParts); void runPostDrag(); void runKINEMATIC(); void initprincipalMassMarker(); std::shared_ptr spatialContainerAt(std::shared_ptr self, std::string& longname) const; + std::shared_ptr partAt(std::string& longname) const; std::shared_ptr markerAt(std::string& longname) const; std::shared_ptr jointAt(std::string& longname) const; std::shared_ptr motionAt(std::string& longname) const; diff --git a/OndselSolver/ASMTItem.cpp b/OndselSolver/ASMTItem.cpp index 5b959d1..de44e93 100644 --- a/OndselSolver/ASMTItem.cpp +++ b/OndselSolver/ASMTItem.cpp @@ -83,6 +83,12 @@ FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string& line) return readRowOfDoubles; } +FRowDsptr MbD::ASMTItem::readRowOfDoublesOffTop(std::vector& lines) +{ + auto str = popOffTop(lines); + return readRowOfDoubles(str); +} + FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string& line) { std::istringstream iss(line); @@ -94,6 +100,12 @@ FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string& line) return readColumnOfDoubles; } +FColDsptr MbD::ASMTItem::readColumnOfDoublesOffTop(std::vector& lines) +{ + auto str = popOffTop(lines); + return readColumnOfDoubles(str); +} + double MbD::ASMTItem::readDouble(std::string& line) { std::istringstream iss(line); diff --git a/OndselSolver/ASMTItem.h b/OndselSolver/ASMTItem.h index 41c818a..6ec2b20 100644 --- a/OndselSolver/ASMTItem.h +++ b/OndselSolver/ASMTItem.h @@ -30,10 +30,12 @@ namespace MbD { virtual std::string classname(); void setName(std::string str); virtual void parseASMT(std::vector& lines); - std::string popOffTop(std::vector& args); - std::string readStringOffTop(std::vector& args); + std::string popOffTop(std::vector& lines); + std::string readStringOffTop(std::vector& lines); FRowDsptr readRowOfDoubles(std::string& line); + FRowDsptr readRowOfDoublesOffTop(std::vector& lines); FColDsptr readColumnOfDoubles(std::string& line); + FColDsptr readColumnOfDoublesOffTop(std::vector& lines); double readDouble(std::string& line); int readInt(std::string& line); size_t readSize_t(std::string& line); diff --git a/OndselSolver/Part.cpp b/OndselSolver/Part.cpp index a262dc1..2b19cf0 100644 --- a/OndselSolver/Part.cpp +++ b/OndselSolver/Part.cpp @@ -121,9 +121,9 @@ FColDsptr Part::qX() return partFrame->qX; } -void Part::qE(std::shared_ptr> x) +void Part::qE(std::shared_ptr> qe) { - partFrame->qE = x; + partFrame->qE = qe; } std::shared_ptr> Part::qE() diff --git a/OndselSolver/PosICDragLimitNewtonRaphson.cpp b/OndselSolver/PosICDragLimitNewtonRaphson.cpp index 6321914..be7fb7c 100644 --- a/OndselSolver/PosICDragLimitNewtonRaphson.cpp +++ b/OndselSolver/PosICDragLimitNewtonRaphson.cpp @@ -38,7 +38,11 @@ void MbD::PosICDragLimitNewtonRaphson::run() { preRun(); system->deactivateLimits(); - if (system->limitsSatisfied()) return; + if (system->limitsSatisfied()) { + std::string str("MbD: No limits reached. "); + system->logString(str); + return; + } auto limits = system->limits(); std::partition(limits->begin(), limits->end(), [](auto limit) { return !limit->satisfied(); }); //Violated limits are in front. diff --git a/testapp/OndselSolver.cpp b/testapp/OndselSolver.cpp index c021097..1d64a27 100644 --- a/testapp/OndselSolver.cpp +++ b/testapp/OndselSolver.cpp @@ -26,6 +26,7 @@ void sharedptrTest(); int main() { + ASMTAssembly::runDraggingLogTest(); ASMTAssembly::runDraggingTest2(); ASMTAssembly::runDraggingTest3(); ASMTAssembly::runDraggingTest(); diff --git a/testapp/dragging.log b/testapp/dragging.log new file mode 100644 index 0000000..a873595 --- /dev/null +++ b/testapp/dragging.log @@ -0,0 +1,20 @@ +runPreDrag +runDragStep + Name + Part1 + Position3D + 4.801714581503802e-15 0.099999999999995245 0.19999999999999998 + RotationMatrix + 1 -4.7573992180162559e-14 0 + 4.7573992180162559e-14 1 0 + 0 0 1 +runDragStep + Name + Part1 + Position3D + 0.2633974596215562 0.063397459621556157 0 + RotationMatrix + 0.86602540378443871 -0.49999999999999994 0 + 0.49999999999999994 0.86602540378443871 0 + 0 0 1 +runPostDrag diff --git a/testapp/runPreDrag.asmt b/testapp/runPreDrag.asmt new file mode 100644 index 0000000..cfbaf83 --- /dev/null +++ b/testapp/runPreDrag.asmt @@ -0,0 +1,345 @@ +OndselSolver +Assembly + Notes + (Text string: 'CAD: Copyright (C) 2000-2004, Aik-Siong Koh, All Rights Reserved.The piston crank is the most common mechanism to convert rotary motion to reciprocating motion or vice versa. A crank connects to the ground with a revolute joint. Its other end is connected to the connecting rod. And the connecting rod is connected to the piston which slides along an axis on the ground. The crank is given rotary motion causing the piston to slides back and forth is a straight line. Units are SI units. Angles are in radians.If the instructions below are too brief, refer to the Notes in projectile.asm and circular.asm.To load the example for a quick look:Click File/Open/Assembly/ to get a dialog. Enter *.asm for a list of assemblies. Select piston.asm. To create the assembly from scratch:To create crank, connection rod and piston:Create an empty assembly and populate it with two rods (Assembly1Part1, Assembly1Part2) and one cylinder (Assembly1Part3). The rods have dimensions (1.0d, 0.2d, 0.1d) and (1.5d, 0.2d, 0.1d). The cylinder has radius (0.5d) and height (1.0d). Arrange them from bottom up away from the origin. To mark joint attachment points:On the ground, create a marker (Assembly1Marker1) at (0.0d, 0.0d, 0.0d) and another (Assembly1Marker2) at (3.0d, 0.0d, 0.0d). On the first rod, create a marker (Assembly1Part1Marker1) at (0.1d, 0.1d, 0.0d) and another (Assembly1Part1Marker2) at (0.9d, 0.1d, 0.0d) relative to the z-face. On the second rod, create a marker (Assembly1Part2Marker1) at (0.1d, 0.1d, -0.1d) and another (Assembly1Part2Marker2) at (1.4d, 0.1d, -0.1d) relative to the z-face. On the cylinder, create a marker (Assembly1Part3Marker1) at (0.0d, 0.0d, 0.0d) and another (Assembly1Part3Marker2) at (0.0d, 0.0d, -1.0d) relative to the z-face. Tilt the cylinder a little to get a good view of (Assembly1Part3Marker2). RightClick/Rotate/ over it to rotate the marker (90.0d) degrees about the x-axis.Tilt the cylinder upright to help the solver assemble the system later.To create the joints:Connect (Assembly1Marker1) to (Assembly1Part1Marker1) with revolute joint (Assembly1Joint1).Connect (Assembly1Part1Marker2) to (Assembly1Part2Marker1) with revolute joint (Assembly1Joint2).Connect (Assembly1Part2Marker2) to (Assembly1Part3Marker2) with revolute joint (Assembly1Joint3).Connect (Assembly1Marker2) to (Assembly1Part3Marker1) with translational joint (Assembly1Joint3).The translational joint keeps the marker z-axes parallel and colinear. Only relative translation along the z-axis is permitted.To apply motion to the crank:Apply rotation motion (Assembly1Motion1) to (Assembly1Joint1). Enter 2.0d*pi*time.The assembly is now ready for simulation, animation and plotting.' runs: (Core.RunArray runs: #(514 63 14 5 12 1 2 37 89 10 4 36 1 43 295 32 848 21 517 29 151) values: #(nil #underline #(#underline #bold) #underline #(#underline #bold) #underline nil #(#bold #large) nil #bold nil #(#bold #large) nil #bold nil #bold nil #bold nil #bold nil))) + Name + Assembly1 + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Velocity3D + 0 0 0 + Omega3D + 0 0 0 + RefPoints + RefPoint + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker1 + Position3D + 0 3 0 + RotationMatrix + 1 0 0 + 0 0 1 + 0 -1 0 + RefPoint + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker2 + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + RefCurves + RefSurfaces + Parts + Part + Name + Part1 + Position3D + -0.10000000000000001 -0.10000000000000001 -0.10000000000000001 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Velocity3D + 0 0 0 + Omega3D + 0 0 0 + FeatureOrder + PrincipalMassMarker + Name + MassMarker + Position3D + 0.5 0.10000000000000001 0.050000000000000003 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Mass + 0.20000000000000001 + MomentOfInertias + 0.00083333333333333003 0.016833333333332999 0.017333333333332999 + Density + 10 + RefPoints + RefPoint + Position3D + 0 0 0.10000000000000001 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker1 + Position3D + 0.10000000000000001 0.10000000000000001 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + RefPoint + Position3D + 0 0 0.10000000000000001 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker2 + Position3D + 0.90000000000000002 0.10000000000000001 0 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + RefCurves + RefSurfaces + Part + Name + Part2 + Position3D + 0.94036115973815004 -0.017284236661228001 6.9388939039071999e-18 + RotationMatrix + -0.61538461538461997 -0.78822698199689001 -2.4430659816415999e-17 + 0.78822698199689001 -0.61538461538461997 3.1292471039841e-17 + -3.9699822201676001e-17 -4.8559383484174003e-33 1 + Velocity3D + 0 0 0 + Omega3D + 0 0 0 + FeatureOrder + PrincipalMassMarker + Name + MassMarker + Position3D + 0.75 0.10000000000000001 0.050000000000000003 + RotationMatrix + 1 -2.7755575615629002e-16 0 + 2.7755575615629002e-16 1 0 + 0 0 1 + Mass + 0.29999999999999999 + MomentOfInertias + 0.00125 0.056500000000000002 0.057250000000000002 + Density + 10 + RefPoints + RefPoint + Position3D + 0 0 0.10000000000000001 + RotationMatrix + 1 0 0 + 0 1 0 + 0 0 1 + Markers + Marker + Name + Marker1 + Position3D + 0.10000000000000001 0.10000000000000001 -0.10000000000000001 + RotationMatrix + 1 1.6433823482156999e-50 6.1629758220391999e-33 + 1.9277988905447e-49 1 1.055150120668e-32 + 6.1629758220391999e-33 8.8305116654984998e-33 1 + RefPoint + Position3D + -7.0256300777060995e-17 -1.0408340855861e-17 0.10000000000000001 + RotationMatrix + 1 1.7347234759768e-18 -6.9388939039071999e-18 + 1.7347234759768e-18 1 0 + -6.9388939039071999e-18 0 1 + Markers + Marker + Name + Marker2 + Position3D + 1.3999999999999999 0.10000000000000001 -0.10000000000000001 + RotationMatrix + 1 1.9417266172264999e-33 1.873501354055e-16 + 1.4257315131995e-48 1 4.1633363423443e-17 + -2.5673907444456001e-16 -4.1633363423443e-17 1 + RefCurves + RefSurfaces + Part + Name + Part3 + Position3D + 1.8563657024100999e-16 1.0246950765959999 -6.9385028492422e-17 + RotationMatrix + 1 1.7680967979306999e-16 -2.2204460492503e-16 + 2.2204460492503e-16 6.3228810015036994e-17 1 + 1.7680967979306999e-16 -1 6.3228810015036994e-17 + Velocity3D + 0 0 0 + Omega3D + 0 0 0 + FeatureOrder + PrincipalMassMarker + Name + MassMarker + Position3D + -7.9328390680451997e-18 2.9323172983666999e-17 0.5 + RotationMatrix + 9.2444637330587006e-33 1 -1.0785207688569e-32 + 9.9703461330478005e-65 1.0785207688569e-32 1 + 1 -9.2444637330587006e-33 0 + Mass + 7.6536686473018003 + MomentOfInertias + 0.93243354610287998 1.1040224936598999 1.1040224936598999 + Density + 10 + RefPoints + RefPoint + Position3D + 0 0 0 + RotationMatrix + 1 0 0 + 0 -1 0 + 0 0 -1 + Markers + Marker + Name + Marker1 + Position3D + 0 0 0 + RotationMatrix + 1 2.1223636732195001e-32 -2.4651903288157002e-32 + -2.4651903288157002e-32 -2.2204460492503e-16 1 + -1.1179465652455999e-32 -1 -2.2204460492503e-16 + RefPoint + Position3D + 1.0408340855861e-17 5.5511151231258e-17 1 + RotationMatrix + 1 -6.9388939039071999e-18 6.9388939039071999e-18 + -6.9388939039071999e-18 1 0 + 6.9388939039071999e-18 0 1 + Markers + Marker + Name + Marker2 + Position3D + 0 0 0 + RotationMatrix + 1 -4.1633363423442002e-17 4.1633363423442002e-17 + -4.1633363423442002e-17 1 -4.9303806576313002e-32 + 4.1633363423442002e-17 -3.6977854932235e-32 1 + RefCurves + RefSurfaces + KinematicIJs + ConstraintSets + Joints + RevoluteJoint + Name + Joint1 + MarkerI + /Assembly1/Marker2 + MarkerJ + /Assembly1/Part1/Marker1 + RevoluteJoint + Name + Joint2 + MarkerI + /Assembly1/Part1/Marker2 + MarkerJ + /Assembly1/Part2/Marker1 + RevoluteJoint + Name + Joint3 + MarkerI + /Assembly1/Part2/Marker2 + MarkerJ + /Assembly1/Part3/Marker1 + CylindricalJoint + Name + Joint4 + MarkerI + /Assembly1/Part3/Marker2 + MarkerJ + /Assembly1/Marker1 + Motions + Limits + RotationLimit + Name + Limit1 + MarkerI + /Assembly1/Marker2 + MarkerJ + /Assembly1/Part1/Marker1 + MotionJoint + + Limit + 30.0*pi/180.0 + Type + => + Tol + 1.0e-9 + TranslationLimit + Name + Limit2 + MarkerI + /Assembly1/Part3/Marker2 + MarkerJ + /Assembly1/Marker1 + MotionJoint + + Limit + 1.2 + Type + =< + Tol + 1.0e-9 + GeneralConstraintSets + ForceTorques + ConstantGravity + 0 -9.8100000000000005 0 + SimulationParameters + tstart + 0 + tend + 0 + hmin + 1.0000000000000001e-09 + hmax + 1 + hout + 0.040000000000000001 + errorTol + 9.9999999999999995e-07 + AnimationParameters + nframe + 26 + icurrent + 1 + istart + 1 + iend + 26 + isForward + true + framesPerSecond + 30