From 43474cd2ac12a35b16d67c55feb47d912fef1a27 Mon Sep 17 00:00:00 2001 From: paddle Date: Thu, 10 Sep 2026 12:39:36 +0200 Subject: [PATCH 1/5] Fix ASMT record parsing and Release dragging-log replay --- OndselSolver/ASMTAssembly.cpp | 67 ++++++++++++++++++----------------- OndselSolver/ASMTRefItem.cpp | 10 ++---- 2 files changed, 36 insertions(+), 41 deletions(-) diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index 27e90547..99549809 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -381,7 +381,9 @@ std::shared_ptr MbD::ASMTAssembly::assemblyFromFile(const std::str [[maybe_unused]] bool bool1 = str == "freeCAD: 3D CAD with Motion Simulation by askoh.com"; [[maybe_unused]] bool bool2 = str == "OndselSolver"; assert(bool1 || bool2); - assert(assembly->readStringOffTop(lines) == "Assembly"); + if (assembly->readStringOffTop(lines) != "Assembly") { + throw SimulationStoppingError("Expected Assembly record."); + } assembly->setFilename(fileName); assembly->parseASMT(lines); return assembly; @@ -923,34 +925,16 @@ void MbD::ASMTAssembly::readTimes(std::vector& lines) void MbD::ASMTAssembly::readPartSeriesMany(std::vector& lines) { - if (lines.empty()) { - return; - } - assert(lines[0].find("PartSeries") != std::string::npos); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("JointSeries") != std::string::npos; - }); - std::vector partSeriesLines(lines.begin(), it); - while (!partSeriesLines.empty()) { - readPartSeries(partSeriesLines); + while (!lines.empty() && readString(lines.front()).rfind("PartSeries\t", 0) == 0) { + readPartSeries(lines); } - lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readJointSeriesMany(std::vector& lines) { - if (lines.empty()) { - return; + while (!lines.empty() && readString(lines.front()).rfind("JointSeries\t", 0) == 0) { + readJointSeries(lines); } - assert(lines[0].find("JointSeries") != std::string::npos); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("tionSeries") != std::string::npos; - }); - std::vector jointSeriesLines(lines.begin(), it); - while (!jointSeriesLines.empty()) { - readJointSeries(jointSeriesLines); - } - lines.erase(lines.begin(), it); } void MbD::ASMTAssembly::readAssemblySeries(std::vector& lines) @@ -1001,6 +985,13 @@ void MbD::ASMTAssembly::readPartSeries(std::vector& lines) auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr& prt) { return prt->fullName("") == seriesName; }); + if (it == parts->end()) { + std::string message = "PartSeries references an unknown part: " + seriesName + ". Available parts:"; + for (const auto& part : *parts) { + message += " " + part->fullName(""); + } + throw SimulationStoppingError(message); + } auto& part = *it; part->readPartSeries(lines); } @@ -1020,14 +1011,16 @@ void MbD::ASMTAssembly::readJointSeries(std::vector& lines) std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr& jt) { return jt->fullName("") == seriesName; }); + if (it == joints->end()) { + throw SimulationStoppingError("JointSeries references an unknown joint: " + seriesName); + } auto& joint = *it; joint->readJointSeries(lines); } void MbD::ASMTAssembly::readMotionSeriesMany(std::vector& lines) { - while (!lines.empty()) { - assert(lines[0].find("tionSeries") != std::string::npos); + while (!lines.empty() && readString(lines.front()).find("tionSeries\t") != std::string::npos) { readMotionSeries(lines); } } @@ -1047,6 +1040,9 @@ void MbD::ASMTAssembly::readMotionSeries(std::vector& lines) std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr& jt) { return jt->fullName("") == seriesName; }); + if (it == motions->end()) { + throw SimulationStoppingError("MotionSeries references an unknown motion: " + seriesName); + } auto& motion = *it; motion->readMotionSeries(lines); } @@ -1062,21 +1058,26 @@ void MbD::ASMTAssembly::runDraggingLog(const std::string& fileName) while (std::getline(stream, line)) { lines.push_back(line); } - assert(readStringOffTop(lines) == "runPreDrag"); + auto expectRecord = [&](const std::string& expected) { + if (lines.empty() || readStringOffTop(lines) != expected) { + throw SimulationStoppingError("Expected dragging log record: " + expected); + } + }; + expectRecord("runPreDrag"); runPreDrag(); - while (lines[0].find("runDragStep") != std::string::npos) { - assert(readStringOffTop(lines) == "runDragStep"); + while (!lines.empty() && lines[0].find("runDragStep") != std::string::npos) { + expectRecord("runDragStep"); auto dragParts = std::make_shared>>(); - while (lines[0].find("Name") != std::string::npos) { - assert(readStringOffTop(lines) == "Name"); + while (!lines.empty() && lines[0].find("Name") != std::string::npos) { + expectRecord("Name"); auto dragPartName = readStringOffTop(lines); std::string longerName = "/" + name + "/" + dragPartName; auto dragPart = partAt(longerName); dragParts->push_back(dragPart); - assert(readStringOffTop(lines) == "Position3D"); + expectRecord("Position3D"); auto dragPartPosition3D = readColumnOfDoublesOffTop(lines); dragPart->setPosition3D(dragPartPosition3D); - assert(readStringOffTop(lines) == "RotationMatrix"); + expectRecord("RotationMatrix"); auto dragPartRotationMatrix = std::make_shared>(3); for (size_t i = 0; i < 3; i++) { auto row = readRowOfDoublesOffTop(lines); @@ -1086,7 +1087,7 @@ void MbD::ASMTAssembly::runDraggingLog(const std::string& fileName) } runDragStep(dragParts); } - assert(readStringOffTop(lines) == "runPostDrag"); + expectRecord("runPostDrag"); runPostDrag(); } diff --git a/OndselSolver/ASMTRefItem.cpp b/OndselSolver/ASMTRefItem.cpp index 0367e7b4..75a37aa9 100644 --- a/OndselSolver/ASMTRefItem.cpp +++ b/OndselSolver/ASMTRefItem.cpp @@ -8,7 +8,6 @@ #include "ASMTRefItem.h" #include "CREATE.h" -#include using namespace MbD; @@ -23,14 +22,9 @@ void MbD::ASMTRefItem::readMarkers(std::vector& lines) assert(lines[0].find("Markers") != std::string::npos); lines.erase(lines.begin()); markers->clear(); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("RefPoint") != std::string::npos; - }); - std::vector markersLines(lines.begin(), it); - while (!markersLines.empty()) { - readMarker(markersLines); + while (!lines.empty() && readString(lines.front()) == "Marker") { + readMarker(lines); } - lines.erase(lines.begin(), it); } void MbD::ASMTRefItem::readMarker(std::vector& lines) From 10fd71f09309d9d814cf0254c46034348a4e0f6f Mon Sep 17 00:00:00 2001 From: paddle Date: Thu, 10 Sep 2026 12:40:00 +0200 Subject: [PATCH 2/5] Implement forward rigid-body dynamics, loads, events and compliant limits Add adaptive DAE/BDF integration and generalized force Jacobians; expose typed loads, coupled bushings, friction and contact/event callbacks. Preserve existing assembly and kinematic entry points. --- OndselSolver/ASMTAssembly.cpp | 56 ++ OndselSolver/ASMTAssembly.h | 7 +- OndselSolver/ASMTConstraintSet.cpp | 104 ++- OndselSolver/ASMTConstraintSet.h | 25 +- OndselSolver/ASMTDistanceLimit.cpp | 40 + OndselSolver/ASMTDistanceLimit.h | 18 + OndselSolver/ASMTForceTorque.cpp | 479 +++++++++++- OndselSolver/ASMTForceTorque.h | 58 ++ OndselSolver/ASMTItemIJ.cpp | 7 + OndselSolver/ASMTItemIJ.h | 1 + OndselSolver/ASMTLimit.cpp | 99 ++- OndselSolver/ASMTLimit.h | 13 +- OndselSolver/ASMTPart.cpp | 9 + OndselSolver/ASMTPart.h | 4 +- OndselSolver/ASMTRotationLimit.cpp | 5 + OndselSolver/ASMTRotationLimit.h | 3 + OndselSolver/ASMTSpatialContainer.cpp | 35 +- OndselSolver/ASMTSpatialContainer.h | 1 + OndselSolver/ASMTTranslationLimit.cpp | 5 + OndselSolver/ASMTTranslationLimit.h | 3 + OndselSolver/AbsConstraint.cpp | 17 +- OndselSolver/AbsConstraint.h | 2 + OndselSolver/AngleZConstraintIJ.cpp | 24 + OndselSolver/AngleZConstraintIJ.h | 4 + OndselSolver/AngleZConstraintIqcJc.cpp | 58 ++ OndselSolver/AngleZConstraintIqcJc.h | 10 + OndselSolver/AngleZConstraintIqcJqc.cpp | 74 ++ OndselSolver/AngleZConstraintIqcJqc.h | 10 + OndselSolver/AngleZIecJec.cpp | 28 + OndselSolver/AngleZIecJec.h | 4 + OndselSolver/AppliedForceTorque.cpp | 728 ++++++++++++++++++ OndselSolver/AppliedForceTorque.h | 138 ++++ OndselSolver/AtPointConstraintIJ.cpp | 24 + OndselSolver/AtPointConstraintIJ.h | 4 + OndselSolver/AtPointConstraintIqcJc.cpp | 13 + OndselSolver/AtPointConstraintIqcJc.h | 2 + OndselSolver/AtPointConstraintIqcJqc.cpp | 15 + OndselSolver/AtPointConstraintIqcJqc.h | 2 + OndselSolver/BasicDAEIntegrator.cpp | 546 +++++++++++++ OndselSolver/BasicDAEIntegrator.h | 78 ++ OndselSolver/BasicIntegrator.cpp | 132 ++-- OndselSolver/BasicIntegrator.h | 22 +- OndselSolver/CMakeLists.txt | 30 + OndselSolver/CoaxialGearConstraintIJ.cpp | 31 + OndselSolver/CoaxialGearConstraintIJ.h | 3 + OndselSolver/ConstVelConstraintIJ.cpp | 28 + OndselSolver/ConstVelConstraintIJ.h | 4 + OndselSolver/ConstVelConstraintIqcJc.cpp | 11 + OndselSolver/ConstVelConstraintIqcJc.h | 2 + OndselSolver/ConstVelConstraintIqcJqc.cpp | 16 + OndselSolver/ConstVelConstraintIqcJqc.h | 2 + OndselSolver/ConstantGravity.cpp | 27 + OndselSolver/ConstantGravity.h | 5 + OndselSolver/Constraint.cpp | 51 ++ OndselSolver/Constraint.h | 14 + OndselSolver/ConstraintSet.cpp | 60 ++ OndselSolver/ConstraintSet.h | 12 + OndselSolver/DAECorrector.cpp | 160 ++++ OndselSolver/DAECorrector.h | 43 ++ OndselSolver/DAEIntegrator.cpp | 221 ++++++ OndselSolver/DAEIntegrator.h | 55 ++ OndselSolver/DifferenceOperator.cpp | 47 +- OndselSolver/DifferenceOperator.h | 3 + OndselSolver/DirectionCosineConstraintIJ.cpp | 24 + OndselSolver/DirectionCosineConstraintIJ.h | 4 + .../DirectionCosineConstraintIqcJc.cpp | 11 + OndselSolver/DirectionCosineConstraintIqcJc.h | 2 + .../DirectionCosineConstraintIqcJqc.cpp | 16 + .../DirectionCosineConstraintIqcJqc.h | 2 + OndselSolver/DiscontinuityError.h | 5 + OndselSolver/DistanceConstraintIJ.cpp | 24 + OndselSolver/DistanceConstraintIJ.h | 4 + OndselSolver/DistanceConstraintIqcJc.cpp | 100 +++ OndselSolver/DistanceConstraintIqcJc.h | 10 + OndselSolver/DistanceConstraintIqcJqc.cpp | 148 ++++ OndselSolver/DistanceConstraintIqcJqc.h | 10 + OndselSolver/DistanceLimitIJ.cpp | 27 + OndselSolver/DistanceLimitIJ.h | 14 + OndselSolver/DistancexyConstraintIJ.cpp | 28 + OndselSolver/DistancexyConstraintIJ.h | 4 + OndselSolver/DistancexyConstraintIqcJc.cpp | 17 + OndselSolver/DistancexyConstraintIqcJc.h | 2 + OndselSolver/DistancexyConstraintIqcJqc.cpp | 31 + OndselSolver/DistancexyConstraintIqcJqc.h | 2 + OndselSolver/DynIntegrator.cpp | 431 +++++++++++ OndselSolver/DynIntegrator.h | 62 ++ OndselSolver/DynamicEvents.h | 208 +++++ OndselSolver/EndFrameqct.cpp | 24 + OndselSolver/EndFrameqct.h | 3 + OndselSolver/EulerConstraint.cpp | 17 + OndselSolver/EulerConstraint.h | 2 + OndselSolver/Extrapolator.cpp | 36 + OndselSolver/Extrapolator.h | 24 + OndselSolver/FullRow.h | 2 +- OndselSolver/GearConstraintIJ.cpp | 28 + OndselSolver/GearConstraintIJ.h | 4 + OndselSolver/GearConstraintIqcJc.cpp | 17 + OndselSolver/GearConstraintIqcJc.h | 2 + OndselSolver/GearConstraintIqcJqc.cpp | 31 + OndselSolver/GearConstraintIqcJqc.h | 2 + OndselSolver/Integrator.cpp | 76 ++ OndselSolver/Integrator.h | 25 +- OndselSolver/IntegratorInterface.cpp | 175 +++-- OndselSolver/IntegratorInterface.h | 34 +- OndselSolver/Item.cpp | 67 +- OndselSolver/Item.h | 3 + OndselSolver/LimitIJ.cpp | 263 ++++++- OndselSolver/LimitIJ.h | 41 + OndselSolver/MarkerFrame.cpp | 49 ++ OndselSolver/MarkerFrame.h | 9 + OndselSolver/NormalBasicDAEIntegrator.cpp | 195 +++++ OndselSolver/NormalBasicDAEIntegrator.h | 42 + OndselSolver/OrbitAngleZIecJec.cpp | 28 + OndselSolver/OrbitAngleZIecJec.h | 4 + OndselSolver/Part.cpp | 107 ++- OndselSolver/Part.h | 13 + OndselSolver/PartFrame.cpp | 124 ++- OndselSolver/PartFrame.h | 12 + OndselSolver/RackPinConstraintIJ.cpp | 28 + OndselSolver/RackPinConstraintIJ.h | 4 + OndselSolver/RackPinConstraintIqcJc.cpp | 16 + OndselSolver/RackPinConstraintIqcJc.h | 2 + OndselSolver/RackPinConstraintIqcJqc.cpp | 21 + OndselSolver/RackPinConstraintIqcJqc.h | 2 + OndselSolver/RedundantConstraint.cpp | 60 ++ OndselSolver/RedundantConstraint.h | 12 + OndselSolver/ScrewConstraintIJ.cpp | 28 + OndselSolver/ScrewConstraintIJ.h | 4 + OndselSolver/ScrewConstraintIqcJc.cpp | 16 + OndselSolver/ScrewConstraintIqcJc.h | 2 + OndselSolver/ScrewConstraintIqcJqc.cpp | 21 + OndselSolver/ScrewConstraintIqcJqc.h | 2 + OndselSolver/Solver.h | 2 + OndselSolver/SolverStatistics.cpp | 13 + OndselSolver/SolverStatistics.h | 27 + OndselSolver/StableBackwardDifference.cpp | 21 +- OndselSolver/StableBackwardDifference.h | 1 + OndselSolver/StableStartingBDF.cpp | 99 +++ OndselSolver/StableStartingBDF.h | 36 + OndselSolver/StartingBasicDAEIntegrator.cpp | 104 +++ OndselSolver/StartingBasicDAEIntegrator.h | 37 + OndselSolver/System.cpp | 27 + OndselSolver/System.h | 3 + OndselSolver/SystemSolver.cpp | 67 +- OndselSolver/SystemSolver.h | 8 + OndselSolver/TranslationConstraintIJ.cpp | 24 + OndselSolver/TranslationConstraintIJ.h | 4 + OndselSolver/TranslationConstraintIqcJc.cpp | 74 ++ OndselSolver/TranslationConstraintIqcJc.h | 10 + OndselSolver/TranslationConstraintIqcJqc.cpp | 105 +++ OndselSolver/TranslationConstraintIqcJqc.h | 10 + OndselSolver/enum.h | 28 +- 152 files changed, 7012 insertions(+), 283 deletions(-) create mode 100644 OndselSolver/ASMTDistanceLimit.cpp create mode 100644 OndselSolver/ASMTDistanceLimit.h create mode 100644 OndselSolver/AppliedForceTorque.cpp create mode 100644 OndselSolver/AppliedForceTorque.h create mode 100644 OndselSolver/BasicDAEIntegrator.cpp create mode 100644 OndselSolver/BasicDAEIntegrator.h create mode 100644 OndselSolver/DAECorrector.cpp create mode 100644 OndselSolver/DAECorrector.h create mode 100644 OndselSolver/DAEIntegrator.cpp create mode 100644 OndselSolver/DAEIntegrator.h create mode 100644 OndselSolver/DistanceLimitIJ.cpp create mode 100644 OndselSolver/DistanceLimitIJ.h create mode 100644 OndselSolver/DynIntegrator.cpp create mode 100644 OndselSolver/DynIntegrator.h create mode 100644 OndselSolver/DynamicEvents.h create mode 100644 OndselSolver/Extrapolator.cpp create mode 100644 OndselSolver/Extrapolator.h create mode 100644 OndselSolver/NormalBasicDAEIntegrator.cpp create mode 100644 OndselSolver/NormalBasicDAEIntegrator.h create mode 100644 OndselSolver/SolverStatistics.cpp create mode 100644 OndselSolver/SolverStatistics.h create mode 100644 OndselSolver/StableStartingBDF.cpp create mode 100644 OndselSolver/StableStartingBDF.h create mode 100644 OndselSolver/StartingBasicDAEIntegrator.cpp create mode 100644 OndselSolver/StartingBasicDAEIntegrator.h diff --git a/OndselSolver/ASMTAssembly.cpp b/OndselSolver/ASMTAssembly.cpp index 99549809..cb80cec5 100644 --- a/OndselSolver/ASMTAssembly.cpp +++ b/OndselSolver/ASMTAssembly.cpp @@ -60,6 +60,7 @@ #include "ASMTRevRevJoint.h" #include "ASMTLimit.h" #include "ASMTRotationLimit.h" +#include "ASMTDistanceLimit.h" #include "ASMTTranslationLimit.h" #include "ExternalSystem.h" #if __GNUC__ >= 8 @@ -814,6 +815,9 @@ void MbD::ASMTAssembly::readLimits(std::vector& lines) if (limitsLines[0] == "\t\t\tRotationLimit") { limit = ASMTRotationLimit::With(); } + else if (limitsLines[0] == "\t\t\tDistanceLimit") { + limit = ASMTDistanceLimit::With(); + } else if (limitsLines[0] == "\t\t\tTranslationLimit") { limit = ASMTTranslationLimit::With(); } @@ -1440,6 +1444,48 @@ void MbD::ASMTAssembly::restorePosRot() } } +void MbD::ASMTAssembly::runDYNAMIC() +{ + if (!simulationParameters || !constantGravity || !parts || parts->empty()) + throw std::invalid_argument("Dynamics requires simulation parameters, gravity and at least one body"); + const auto& p = *simulationParameters; + auto positive = [](double x) { return std::isfinite(x) && x > 0; }; + if (!std::isfinite(p.tstart) || !std::isfinite(p.tend) || !(p.tend > p.tstart) + || !positive(p.hout) || !positive(p.hmin) || !positive(p.hmax) || p.hmin > p.hmax + || p.tstart+p.hout == p.tstart || p.tstart+p.hmin == p.tstart + || !positive(p.corAbsTol) || !positive(p.corRelTol) + || !positive(p.intAbsTol) || !positive(p.intRelTol) + || !positive(p.errorTolPosKine) || !positive(p.errorTolAccKine) + || p.iterMaxDyn == 0 || p.orderMax == 0 || p.orderMax > 5) + throw std::invalid_argument("Invalid forward-dynamics interval, step sizes, tolerances or iteration/order limits"); + const auto gravity = constantGravity->getg(); + if (!gravity || gravity->size() != 3 + || !std::all_of(gravity->begin(), gravity->end(), [](double x) { return std::isfinite(x); })) + throw std::invalid_argument("Gravity must have three finite components"); + for (const auto& part : *parts) { + if (!part || !part->principalMassMarker) + throw std::invalid_argument("A dynamics body is missing its mass properties"); + if (part->isFixed) continue; + const auto& mass = *part->principalMassMarker; + if (!positive(mass.mass) || !mass.momentOfInertias || mass.momentOfInertias->size() != 3 + || !std::all_of(mass.momentOfInertias->begin(), mass.momentOfInertias->end(), positive)) + throw std::invalid_argument("Moving bodies require finite, positive mass and principal inertias: " + part->name); + } + mbdSystem = std::make_shared(); + mbdSystem->externalSystem->asmtAssembly = this; + mbdSystem->dynamicEvents = dynamicEvents; + // Current placements and velocities are the inputs to each run. Replace + // output histories; callers can explicitly restore the input state first. + times->clear(); + clearResults(); + for (auto& part : *parts) part->clearResults(); + for (auto& joint : *joints) joint->clearResults(); + for (auto& motion : *motions) motion->clearResults(); + for (auto& load : *forcesTorques) load->clearResults(); + for (auto& limit : *limits) limit->clearResults(); + mbdSystem->runDYNAMIC(mbdSystem); +} + void MbD::ASMTAssembly::runKINEMATIC() { mbdSystem = std::make_shared(); @@ -1570,6 +1616,9 @@ void MbD::ASMTAssembly::updateFromMbD() for (auto& forceTorque : *forcesTorques) { forceTorque->updateFromMbD(); } + for (auto& limit : *limits) { + limit->updateFromMbD(); + } } void MbD::ASMTAssembly::compareResults(AnalysisType type) @@ -1618,6 +1667,13 @@ void MbD::ASMTAssembly::addMotion(std::shared_ptr motion) motion->initMarkers(); } +void ASMTAssembly::addForceTorque(std::shared_ptr load) +{ + if (!load) throw std::invalid_argument("Cannot add a null load"); + forcesTorques->push_back(load); + load->owner = this; +} + void MbD::ASMTAssembly::addLimit(std::shared_ptr limit) { limits->push_back(limit); diff --git a/OndselSolver/ASMTAssembly.h b/OndselSolver/ASMTAssembly.h index e9cbc6ef..ba89d8bf 100644 --- a/OndselSolver/ASMTAssembly.h +++ b/OndselSolver/ASMTAssembly.h @@ -28,6 +28,7 @@ namespace MbD { class SystemSolver; class ASMTItemIJ; class ExternalSystem; + class DynamicEvents; class ASMTAssembly : public ASMTSpatialContainer { @@ -101,7 +102,10 @@ namespace MbD { void runDragStep(std::shared_ptr>> dragParts); void runPostDrag(); void restorePosRot(); - void runKINEMATIC(); + void runKINEMATIC(); + // Forward dynamics. Unlike the legacy kinematic wrapper, errors propagate. + void runDYNAMIC(); + std::shared_ptr dynamicEvents; void initprincipalMassMarker(); std::shared_ptr spatialContainerAt(std::shared_ptr self, std::string& longname) const; std::shared_ptr partAt(const std::string& longname) const; @@ -117,6 +121,7 @@ namespace MbD { void outputResults(AnalysisType type) override; void addPart(std::shared_ptr part); void addJoint(std::shared_ptr joint); + void addForceTorque(std::shared_ptr load); void addMotion(std::shared_ptr motion); void addLimit(std::shared_ptr limit); void setConstantGravity(std::shared_ptr constantGravity); diff --git a/OndselSolver/ASMTConstraintSet.cpp b/OndselSolver/ASMTConstraintSet.cpp index f83d730c..6e5cb270 100644 --- a/OndselSolver/ASMTConstraintSet.cpp +++ b/OndselSolver/ASMTConstraintSet.cpp @@ -5,55 +5,87 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #include "ASMTConstraintSet.h" + +#include + #include "ASMTAssembly.h" #include "ASMTMarker.h" -#include "Joint.h" +#include "AppliedForceTorque.h" #include "FullMatrix.h" +#include "Joint.h" using namespace MbD; +void MbD::ASMTConstraintSet::initialize() +{ + ASMTItemIJ::initialize(); + powers = std::make_shared>(); +} + +void MbD::ASMTConstraintSet::clearResults() +{ + ASMTItemIJ::clearResults(); + if (powers) { + powers->clear(); + } +} + void MbD::ASMTConstraintSet::updateFromMbD() { - //" - //MbD returns aFIeO and aTIeO. - //GEO needs aFImO and aTImO. - //For Motion rImIeO is not zero and is changing. - //aFImO = aFIeO. - //aTImO = aTIeO + (rImIeO cross : aFIeO). - //" - auto mbdUnts = mbdUnits(); - auto mbdJoint = std::static_pointer_cast(mbdObject); - auto aFIeO = mbdJoint->aFX()->times(mbdUnts->force); - auto aTIeO = mbdJoint->aTX()->times(mbdUnts->torque); - auto rImIeO = mbdJoint->frmI->rmeO()->times(mbdUnts->length); - auto aFIO = aFIeO; - auto aTIO = aTIeO->plusFullColumn(rImIeO->cross(aFIeO)); - fxs->push_back(aFIO->at(0)); - fys->push_back(aFIO->at(1)); - fzs->push_back(aFIO->at(2)); - txs->push_back(aTIO->at(0)); - tys->push_back(aTIO->at(1)); - tzs->push_back(aTIO->at(2)); + // MbD returns force and torque at connector I. Convert them to the marker-I + // moment expected by the ASMT result format before saving the reaction. + auto mbdUnts = mbdUnits(); + auto mbdJoint = std::static_pointer_cast(mbdObject); + auto aFIeO = mbdJoint->aFX()->times(mbdUnts->force); + auto aTIeO = mbdJoint->aTX()->times(mbdUnts->torque); + auto rImIeO = mbdJoint->frmI->rmeO()->times(mbdUnts->length); + auto aFIO = aFIeO; + auto aTIO = aTIeO->plusFullColumn(rImIeO->cross(aFIeO)); + fxs->push_back(aFIO->at(0)); + fys->push_back(aFIO->at(1)); + fzs->push_back(aFIO->at(2)); + txs->push_back(aTIO->at(0)); + tys->push_back(aTIO->at(1)); + tzs->push_back(aTIO->at(2)); + + const auto i = AppliedForceTorque::frameState(mbdJoint->frmI); + const auto j = AppliedForceTorque::frameState(mbdJoint->frmJ); + const auto dot = [](const auto& first, const auto& second) { + return first[0] * second[0] + first[1] * second[1] + first[2] * second[2]; + }; + const std::array force { + aFIeO->at(0) / mbdUnts->force, + aFIeO->at(1) / mbdUnts->force, + aFIeO->at(2) / mbdUnts->force, + }; + const std::array torque { + aTIeO->at(0) / mbdUnts->torque, + aTIeO->at(1) / mbdUnts->torque, + aTIeO->at(2) / mbdUnts->torque, + }; + const std::array separation { + j.position[0] - i.position[0], + j.position[1] - i.position[1], + j.position[2] - i.position[2], + }; + const std::array oppositeTorque { + separation[1] * force[2] - separation[2] * force[1] - torque[0], + separation[2] * force[0] - separation[0] * force[2] - torque[1], + separation[0] * force[1] - separation[1] * force[0] - torque[2], + }; + const double power = dot(force, i.velocity) + dot(torque, i.omega) + - dot(force, j.velocity) + dot(oppositeTorque, j.omega); + powers->push_back(power * mbdUnts->torque / mbdUnts->angle / mbdUnts->time); } void MbD::ASMTConstraintSet::compareResults(AnalysisType) { - if (infxs == nullptr || infxs->empty()) return; - auto mbdUnts = mbdUnits(); - //auto factor = 1.0e-6; - //auto forceTol = mbdUnts->force * factor; - //auto torqueTol = mbdUnts->torque * factor; - //auto i = fxs->size() - 1; - //assert(Numeric::equaltol(fxs->at(i), infxs->at(i), forceTol)); - //assert(Numeric::equaltol(fys->at(i), infys->at(i), forceTol)); - //assert(Numeric::equaltol(fzs->at(i), infzs->at(i), forceTol)); - //assert(Numeric::equaltol(txs->at(i), intxs->at(i), torqueTol)); - //assert(Numeric::equaltol(tys->at(i), intys->at(i), torqueTol)); - //assert(Numeric::equaltol(tzs->at(i), intzs->at(i), torqueTol)); + if (infxs == nullptr || infxs->empty()) { + return; + } } void MbD::ASMTConstraintSet::outputResults(AnalysisType) -{ -} +{} diff --git a/OndselSolver/ASMTConstraintSet.h b/OndselSolver/ASMTConstraintSet.h index 737164b4..cb988a08 100644 --- a/OndselSolver/ASMTConstraintSet.h +++ b/OndselSolver/ASMTConstraintSet.h @@ -5,22 +5,25 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include "ASMTItemIJ.h" namespace MbD { - class Joint; - class ASMTConstraintSet : public ASMTItemIJ - { - // - public: - void updateFromMbD() override; - void compareResults(AnalysisType type) override; - void outputResults(AnalysisType type) override; +class Joint; + +class ASMTConstraintSet : public ASMTItemIJ +{ +public: + void initialize() override; + void clearResults(); + void updateFromMbD() override; + void compareResults(AnalysisType type) override; + void outputResults(AnalysisType type) override; - }; -} + FRowDsptr powers; +}; +} // namespace MbD diff --git a/OndselSolver/ASMTDistanceLimit.cpp b/OndselSolver/ASMTDistanceLimit.cpp new file mode 100644 index 00000000..677ce636 --- /dev/null +++ b/OndselSolver/ASMTDistanceLimit.cpp @@ -0,0 +1,40 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +#include "ASMTDistanceLimit.h" + +#include "DistanceLimitIJ.h" +#include "Units.h" + +using namespace MbD; + +std::shared_ptr ASMTDistanceLimit::With() +{ + auto result = std::make_shared(); + result->initialize(); + return result; +} + +std::shared_ptr ASMTDistanceLimit::mbdClassNew() +{ + return DistanceLimitIJ::With(); +} + +void ASMTDistanceLimit::storeOnLevel(std::ofstream& os, size_t level) +{ + storeOnLevelString(os, level, "DistanceLimit"); + ASMTLimit::storeOnLevel(os, level); +} + +double ASMTDistanceLimit::coordinateUnit(const Units& units) const +{ + return units.length; +} + +double ASMTDistanceLimit::stiffnessUnit(const Units& units) const +{ + return units.length / units.force; +} + +double ASMTDistanceLimit::dampingUnit(const Units& units) const +{ + return units.velocity / units.force; +} diff --git a/OndselSolver/ASMTDistanceLimit.h b/OndselSolver/ASMTDistanceLimit.h new file mode 100644 index 00000000..e6b2736e --- /dev/null +++ b/OndselSolver/ASMTDistanceLimit.h @@ -0,0 +1,18 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +#pragma once + +#include "ASMTLimit.h" + +namespace MbD +{ +class ASMTDistanceLimit: public ASMTLimit +{ +public: + static std::shared_ptr With(); + std::shared_ptr mbdClassNew() override; + void storeOnLevel(std::ofstream& os, size_t level) override; + double coordinateUnit(const Units& units) const override; + double stiffnessUnit(const Units& units) const override; + double dampingUnit(const Units& units) const override; +}; +} // namespace MbD diff --git a/OndselSolver/ASMTForceTorque.cpp b/OndselSolver/ASMTForceTorque.cpp index 7927575b..7456ba71 100644 --- a/OndselSolver/ASMTForceTorque.cpp +++ b/OndselSolver/ASMTForceTorque.cpp @@ -1,26 +1,481 @@ /*************************************************************************** - * Copyright (c) 2023 Ondsel, Inc. * - * * - * This file is part of OndselSolver. * - * * - * See LICENSE file for details about copyright. * + * Copyright (c) 2023 Ondsel, Inc. + * This file is part of OndselSolver. + * See LICENSE file for details about copyright. ***************************************************************************/ - #include "ASMTForceTorque.h" +#include "AppliedForceTorque.h" +#include "ASMTAssembly.h" +#include "ASMTMarker.h" +#include "ASMTJoint.h" +#include "System.h" +#include "Units.h" +#include "SymbolicParser.h" +#include "BasicUserFunction.h" +#include "Constant.h" +#include "Joint.h" +#include +#include +#include +#include +#include +#include using namespace MbD; -void MbD::ASMTForceTorque::updateFromMbD() +std::shared_ptr ASMTForceTorque::With() { - throw SimulationStoppingError("To be implemented."); + auto item = std::make_shared(); + item->initialize(); + return item; } -void MbD::ASMTForceTorque::compareResults(AnalysisType) +void ASMTForceTorque::initialize() { - throw SimulationStoppingError("To be implemented."); + ASMTItemIJ::initialize(); + powers = std::make_shared>(); + storedEnergies = std::make_shared>(); + dissipatedPowers = std::make_shared>(); } -void MbD::ASMTForceTorque::outputResults(AnalysisType) +void ASMTForceTorque::clearResults() { - throw SimulationStoppingError("To be implemented."); + ASMTItemIJ::clearResults(); + for (auto row : {powers, storedEnergies, dissipatedPowers}) { + if (row) row->clear(); + } +} + +void ASMTForceTorque::setForce3D(double x, double y, double z) +{ + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) + throw std::invalid_argument("Force components must be finite"); + force = {x, y, z}; + contactEvaluator = {}; + formula.clear(); + spring = false; + torsionalSpring = false; + bushing = false; + axisFriction = false; +} + +void ASMTForceTorque::setTorque3D(double x, double y, double z) +{ + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) + throw std::invalid_argument("Torque components must be finite"); + torque = {x, y, z}; + contactEvaluator = {}; + formula.clear(); + spring = false; + torsionalSpring = false; + bushing = false; + axisFriction = false; +} + +namespace { +void setFormula(std::array& direction, std::string& target, + bool& torque, bool isTorque, double x, double y, double z, + const std::string& expression) +{ + const double length = std::sqrt(x*x + y*y + z*z); + if (!std::isfinite(length) || length <= 0 || expression.empty()) + throw std::invalid_argument("A formula load requires a finite direction and expression"); + direction = {x/length, y/length, z/length}; + target = expression; + torque = isTorque; +} +} + +void ASMTForceTorque::setForceFormula3D(double x, double y, double z, + const std::string& expression) +{ + setFormula(formulaDirection, formula, formulaTorque, false, x, y, z, expression); + contactEvaluator = {}; + spring = false; + torsionalSpring = false; + bushing = false; + axisFriction = false; +} + +void ASMTForceTorque::setTorqueFormula3D(double x, double y, double z, + const std::string& expression) +{ + setFormula(formulaDirection, formula, formulaTorque, true, x, y, z, expression); + contactEvaluator = {}; + spring = false; + torsionalSpring = false; + bushing = false; + axisFriction = false; +} + +void ASMTForceTorque::setSpringDamper(double k, double c, double l) +{ + if (!std::isfinite(k) || !std::isfinite(c) || !std::isfinite(l) || k < 0 || c < 0 || l < 0) + throw std::invalid_argument("Spring stiffness, damping and rest length must be finite and nonnegative"); + stiffness = k; + damping = c; + restLength = l; + contactEvaluator = {}; + formula.clear(); + spring = true; + torsionalSpring = false; + bushing = false; + axisFriction = false; +} + +void ASMTForceTorque::setTorsionalSpringDamper(double k, double c, double angle) +{ + if (!std::isfinite(k) || !std::isfinite(c) || !std::isfinite(angle) || k < 0 || c < 0) + throw std::invalid_argument("Torsional stiffness and damping must be finite and nonnegative"); + stiffness = k; + damping = c; + restLength = angle; + contactEvaluator = {}; + formula.clear(); + spring = false; + torsionalSpring = true; + bushing = false; + axisFriction = false; +} + +void ASMTForceTorque::setContactStepValidator(AppliedForceTorque::ContactStepValidator validator) +{ + contactStepValidator = std::move(validator); +} + +void ASMTForceTorque::setContactEvaluator(AppliedForceTorque::ContactEvaluator evaluator) +{ + if (!evaluator) + throw std::invalid_argument("A contact load requires an evaluator"); + contactEvaluator = std::move(evaluator); + formula.clear(); + spring = false; + torsionalSpring = false; + bushing = false; + axisFriction = false; +} + +void ASMTForceTorque::setBushing(const std::array& linearStiffness, + const std::array& linearDamping, + const std::array& angularStiffness, + const std::array& angularDamping) +{ + const auto valid = [](const auto& values) { + return std::all_of(values.begin(), values.end(), [](double value) { + return std::isfinite(value) && value >= 0; + }); + }; + if (!valid(linearStiffness) || !valid(linearDamping) + || !valid(angularStiffness) || !valid(angularDamping)) + throw std::invalid_argument("Bushing stiffness and damping must be finite and nonnegative"); + bushingParameters = {linearStiffness, linearDamping, angularStiffness, angularDamping}; + contactEvaluator = {}; + formula.clear(); + spring = false; + torsionalSpring = false; + bushing = true; + axisFriction = false; +} + +void ASMTForceTorque::setCoupledBushing(const std::array& stiffness, + const std::array& damping) +{ + AppliedForceTorque::validateBushingMatrix(stiffness); + AppliedForceTorque::validateBushingMatrix(damping); + setBushing({}, {}, {}, {}); + bushingParameters.coupled = true; + bushingParameters.stiffnessMatrix = stiffness; + bushingParameters.dampingMatrix = damping; +} + +void ASMTForceTorque::setAxisFriction(double staticMagnitude, + double dynamicMagnitude, double transitionVelocity, + double viscousCoefficient, bool rotational) +{ + AppliedForceTorque::AxisFrictionParameters parameters { + staticMagnitude, + dynamicMagnitude, + transitionVelocity, + viscousCoefficient, + rotational + }; + // Let the solver object own the common validation contract. Constructing a + // temporary is not possible before markers exist, so validate scalars here. + if (!std::isfinite(staticMagnitude) || !std::isfinite(dynamicMagnitude) + || !std::isfinite(transitionVelocity) || !std::isfinite(viscousCoefficient) + || staticMagnitude < dynamicMagnitude || dynamicMagnitude < 0 + || !(transitionVelocity > 0) || viscousCoefficient < 0) + throw std::invalid_argument( + "Axis friction requires static >= dynamic >= 0, positive transition velocity, " + "and nonnegative viscous damping" + ); + axisFrictionParameters = parameters; + reactionJoint.reset(); + contactEvaluator = {}; + formula.clear(); + spring = false; + torsionalSpring = false; + bushing = false; + axisFriction = true; +} + +void ASMTForceTorque::setReactionAxisFriction(double staticCoefficient, + double dynamicCoefficient, double transitionVelocity, + double viscousCoefficient, double effectiveRadius, bool rotational, + const std::shared_ptr& source, bool axialReaction) +{ + if (!source || !std::isfinite(staticCoefficient) + || !std::isfinite(dynamicCoefficient) || !std::isfinite(transitionVelocity) + || !std::isfinite(viscousCoefficient) || !std::isfinite(effectiveRadius) + || staticCoefficient < dynamicCoefficient || dynamicCoefficient < 0 + || !(transitionVelocity > 0) || viscousCoefficient < 0 + || (rotational && !(effectiveRadius > 0))) { + throw std::invalid_argument( + "Reaction friction requires static >= dynamic >= 0, positive transition velocity, " + "nonnegative viscous damping, and a positive rotational effective radius" + ); + } + setAxisFriction(staticCoefficient, dynamicCoefficient, transitionVelocity, + viscousCoefficient, rotational); + axisFrictionParameters.reactionBased = true; + if (axialReaction && !rotational) + throw std::invalid_argument("Thrust bearing friction requires a rotational axis"); + axisFrictionParameters.axialReaction = axialReaction; + axisFrictionParameters.effectiveRadius = rotational ? effectiveRadius : 1; + reactionJoint = source; +} + +void ASMTForceTorque::createMbD(std::shared_ptr system, std::shared_ptr units) +{ + auto i = root()->markerAt(markerI); + auto j = root()->markerAt(markerJ); + if (!i || !j) throw std::invalid_argument("Load attachment marker was not found"); + auto fi = std::dynamic_pointer_cast(i->mbdObject); + auto fj = std::dynamic_pointer_cast(j->mbdObject); + std::shared_ptr load; + if (contactEvaluator) { + load = std::make_shared(fi, fj, contactEvaluator); + } + else if (bushing) { + auto parameters = bushingParameters; + for (size_t row = 0; row < 6; ++row) { + const double effort = row < 3 ? units->force : units->torque; + for (size_t col = 0; col < 6; ++col) { + parameters.stiffnessMatrix[6*row+col] + *= (col < 3 ? units->length : units->angle) / effort; + parameters.dampingMatrix[6*row+col] + *= (col < 3 ? units->velocity : units->omega) / effort; + } + } + for (size_t axis = 0; axis < 3; ++axis) { + parameters.linearStiffness[axis] *= units->length / units->force; + parameters.linearDamping[axis] *= units->velocity / units->force; + parameters.angularStiffness[axis] *= units->angle / units->torque; + parameters.angularDamping[axis] *= units->omega / units->torque; + } + load = std::make_shared(fi, fj, parameters); + } + else if (axisFriction) { + auto parameters = axisFrictionParameters; + if (parameters.reactionBased) { + const auto source = reactionJoint.lock(); + if (!source) { + throw std::invalid_argument("Reaction friction source joint is no longer available"); + } + parameters.reactionJoint = std::dynamic_pointer_cast(source->mbdObject); + if (parameters.reactionJoint.expired()) { + throw std::invalid_argument("Reaction friction source solver joint was not created"); + } + parameters.effectiveRadius /= units->length; + } + if (parameters.rotational) { + if (!parameters.reactionBased) { + parameters.staticMagnitude /= units->torque; + parameters.dynamicMagnitude /= units->torque; + } + parameters.transitionVelocity /= units->omega; + parameters.viscousCoefficient *= units->omega / units->torque; + } + else { + if (!parameters.reactionBased) { + parameters.staticMagnitude /= units->force; + parameters.dynamicMagnitude /= units->force; + } + parameters.transitionVelocity /= units->velocity; + parameters.viscousCoefficient *= units->velocity / units->force; + } + load = std::make_shared(fi, fj, parameters); + } + else if (spring) { + load = std::make_shared(fi, fj, stiffness*units->length/units->force, + damping*units->velocity/units->force, restLength/units->length); + } + else if (torsionalSpring) { + load = std::make_shared(fi, fj, + stiffness*units->angle/units->torque, + damping*units->omega/units->torque, + restLength/units->angle, true); + } + else if (!formula.empty()) { + auto parser = std::make_shared(); + parser->owner = this; + parser->variables->insert(std::make_pair("time", root()->geoTime())); + auto userFunction = std::make_shared(formula, 1.0); + parser->parseUserFunction(userFunction); + auto magnitude = parser->stack->top(); + const double unit = formulaTorque ? units->torque : units->force; + magnitude = Symbolic::times(magnitude, sptrConstant(1.0 / unit)); + magnitude->createMbD(system, units); + magnitude = magnitude->simplified(magnitude); + load = std::make_shared( + fi, fj, formulaDirection, magnitude, formulaTorque + ); + } + else { + auto f = force, t = torque; + for (size_t axis = 0; axis < 3; ++axis) { + f[axis] /= units->force; + t[axis] /= units->torque; + } + load = std::make_shared(fi, fj, f, t); + } + load->setContactStepValidator(contactStepValidator); + load->setFollower(follower); + initialize(); // A rerun replaces, rather than appends to, load histories. + load->name = fullName(""); + mbdObject = load; + system->addForceTorque(load); +} + +void ASMTForceTorque::updateFromMbD() +{ + const auto load = std::static_pointer_cast(mbdObject); + const auto state = load->resultState(); + const auto& f = state.forceOnI; + const auto& t = state.torqueOnI; + const auto& energy = state.energy; + const auto units = root()->mbdUnits; + fxs->push_back(f[0]*units->force); + fys->push_back(f[1]*units->force); + fzs->push_back(f[2]*units->force); + txs->push_back(t[0]*units->torque); + tys->push_back(t[1]*units->torque); + tzs->push_back(t[2]*units->torque); + const double energyUnit = units->torque / units->angle; + const double powerUnit = energyUnit / units->time; + powers->push_back(energy.power * powerUnit); + storedEnergies->push_back(energy.storedEnergy * energyUnit); + dissipatedPowers->push_back(energy.dissipatedPower * powerUnit); +} + +void ASMTForceTorque::compareResults(AnalysisType) {} +void ASMTForceTorque::outputResults(AnalysisType) {} + +bool ASMTForceTorque::isPassive() const +{ + return spring || torsionalSpring || bushing || axisFriction + || static_cast(contactEvaluator); +} + +void ASMTForceTorque::storeOnLevel(std::ofstream& os, size_t level) +{ + if (contactEvaluator) + throw std::invalid_argument("Runtime CAD contact evaluators cannot be serialized"); + if (axisFriction && axisFrictionParameters.reactionBased) + throw std::invalid_argument("Reaction-based joint friction cannot be serialized as a standalone load"); + storeOnLevelString(os, level, "ForceTorque"); + ASMTItemIJ::storeOnLevel(os, level); + std::ostringstream data; + data << std::setprecision(17); + if (axisFriction) { + data << (axisFrictionParameters.rotational ? "RotationalAxisFriction " + : "LinearAxisFriction ") + << axisFrictionParameters.staticMagnitude << ' ' + << axisFrictionParameters.dynamicMagnitude << ' ' + << axisFrictionParameters.transitionVelocity << ' ' + << axisFrictionParameters.viscousCoefficient; + } + else if (bushing && bushingParameters.coupled) { + data << "CoupledBushing"; + for (const auto& values : {bushingParameters.stiffnessMatrix, bushingParameters.dampingMatrix}) + for (double value : values) data << ' ' << value; + } + else if (bushing) { + data << "Bushing"; + for (const auto& values : {bushingParameters.linearStiffness, + bushingParameters.linearDamping, bushingParameters.angularStiffness, + bushingParameters.angularDamping}) + for (double value : values) data << ' ' << value; + } + else if (spring) data << "LinearSpringDamper " << stiffness << ' ' << damping << ' ' << restLength; + else if (torsionalSpring) data << "TorsionalSpringDamper " << stiffness << ' ' << damping << ' ' << restLength; + else if (!formula.empty()) data << (follower + ? (formulaTorque ? "FollowerTorqueFormula " : "FollowerForceFormula ") + : (formulaTorque ? "WorldTorqueFormula " : "WorldForceFormula ")) + << formulaDirection[0] << ' ' << formulaDirection[1] << ' ' << formulaDirection[2] + << ' ' << std::quoted(formula); + else data << (follower ? "FollowerWrench " : "WorldWrench ") << force[0] << ' ' << force[1] << ' ' << force[2] + << ' ' << torque[0] << ' ' << torque[1] << ' ' << torque[2]; + storeOnLevelString(os, level+1, data.str()); +} + +void ASMTForceTorque::parseASMT(std::vector& lines) +{ + // This explicit format does not interpret FreeCADMbD load expressions. + if (lines.size() < 7) throw std::invalid_argument("Incomplete typed load"); + ASMTItemIJ::parseASMT(lines); + std::istringstream data(lines.front()); + std::string kind, extra; + double x, y, z, tx, ty, tz; + data >> kind; + follower = kind.rfind("Follower", 0) == 0; + if ((kind == "WorldWrench" || kind == "FollowerWrench") + && (data >> x >> y >> z >> tx >> ty >> tz) && !(data >> extra)) { + setForce3D(x, y, z); + setTorque3D(tx, ty, tz); + } + else if (kind == "LinearSpringDamper" && (data >> x >> y >> z) && !(data >> extra)) { + setSpringDamper(x, y, z); + } + else if (kind == "TorsionalSpringDamper" && (data >> x >> y >> z) && !(data >> extra)) { + setTorsionalSpringDamper(x, y, z); + } + else if (kind == "CoupledBushing") { + std::array k{}, c{}; + for (auto* matrix : {&k, &c}) + for (double& value : *matrix) + if (!(data >> value)) throw std::invalid_argument("Invalid coupled bushing matrix"); + if (data >> extra) throw std::invalid_argument("Invalid coupled bushing specification"); + setCoupledBushing(k, c); + } + else if (kind == "Bushing") { + std::array linearK, linearC, angularK, angularC; + const auto read = [&](auto& values) { + for (double& value : values) + if (!(data >> value)) throw std::invalid_argument("Invalid bushing specification"); + }; + read(linearK); + read(linearC); + read(angularK); + read(angularC); + data >> std::ws; + if (!data.eof()) throw std::invalid_argument("Invalid bushing specification"); + setBushing(linearK, linearC, angularK, angularC); + } + else if ((kind == "LinearAxisFriction" || kind == "RotationalAxisFriction") + && (data >> x >> y >> z >> tx) && !(data >> extra)) { + setAxisFriction(x, y, z, tx, kind == "RotationalAxisFriction"); + } + else if (kind == "WorldForceFormula" || kind == "WorldTorqueFormula" + || kind == "FollowerForceFormula" || kind == "FollowerTorqueFormula") { + if (!(data >> x >> y >> z >> std::quoted(extra))) + throw std::invalid_argument("Invalid formula load specification"); + data >> std::ws; + if (!data.eof()) + throw std::invalid_argument("Invalid formula load specification"); + if (kind == "WorldForceFormula" || kind == "FollowerForceFormula") setForceFormula3D(x, y, z, extra); + else setTorqueFormula3D(x, y, z, extra); + } + else throw std::invalid_argument("Invalid or unsupported typed load specification"); + lines.erase(lines.begin()); } diff --git a/OndselSolver/ASMTForceTorque.h b/OndselSolver/ASMTForceTorque.h index 68862219..f1b0fbfa 100644 --- a/OndselSolver/ASMTForceTorque.h +++ b/OndselSolver/ASMTForceTorque.h @@ -9,17 +9,75 @@ #pragma once #include "ASMTItemIJ.h" +#include "AppliedForceTorque.h" +#include namespace MbD { + class ASMTJoint; class ASMTForceTorque : public ASMTItemIJ { // public: + static std::shared_ptr With(); + void initialize() override; + void clearResults(); + // Constant world-resolved load on I; the balancing wrench acts on J. + void setForce3D(double x, double y, double z); + void setTorque3D(double x, double y, double z); + // When true, constant/formula vector components are marker-I-local. + void setFollower(bool value) { follower = value; } + void setForceFormula3D(double x, double y, double z, const std::string& expression); + void setTorqueFormula3D(double x, double y, double z, const std::string& expression); + void setSpringDamper(double stiffness, double damping, double restLength); + void setTorsionalSpringDamper(double stiffness, double damping, double restAngle); + void setBushing(const std::array& linearStiffness, + const std::array& linearDamping, + const std::array& angularStiffness, + const std::array& angularDamping); + void setCoupledBushing(const std::array& stiffness, + const std::array& damping); + void setAxisFriction(double staticMagnitude, + double dynamicMagnitude, + double transitionVelocity, + double viscousCoefficient, + bool rotational); + void setReactionAxisFriction(double staticCoefficient, + double dynamicCoefficient, + double transitionVelocity, + double viscousCoefficient, + double effectiveRadius, + bool rotational, + const std::shared_ptr& reactionJoint, + bool axialReaction = false); + void setContactEvaluator(AppliedForceTorque::ContactEvaluator evaluator); + void setContactStepValidator(AppliedForceTorque::ContactStepValidator validator); + void createMbD(std::shared_ptr system, std::shared_ptr units) override; + void parseASMT(std::vector& lines) override; + void storeOnLevel(std::ofstream& os, size_t level) override; void updateFromMbD() override; void compareResults(AnalysisType type) override; void outputResults(AnalysisType type) override; + bool isPassive() const; + FRowDsptr powers, storedEnergies, dissipatedPowers; + + private: + std::array force{}, torque{}; + std::array formulaDirection{}; + std::string formula; + AppliedForceTorque::ContactEvaluator contactEvaluator; + AppliedForceTorque::ContactStepValidator contactStepValidator; + bool formulaTorque = false; + bool follower = false; + bool spring = false; + bool torsionalSpring = false; + bool bushing = false; + bool axisFriction = false; + double stiffness = 0, damping = 0, restLength = 0; + AppliedForceTorque::BushingParameters bushingParameters; + AppliedForceTorque::AxisFrictionParameters axisFrictionParameters; + std::weak_ptr reactionJoint; }; } diff --git a/OndselSolver/ASMTItemIJ.cpp b/OndselSolver/ASMTItemIJ.cpp index 1adbd2ea..19eec074 100644 --- a/OndselSolver/ASMTItemIJ.cpp +++ b/OndselSolver/ASMTItemIJ.cpp @@ -40,6 +40,13 @@ void MbD::ASMTItemIJ::setMarkerI(const std::string& mkrI) markerI = mkrI; } +void ASMTItemIJ::clearResults() +{ + for (auto row : {fxs, fys, fzs, txs, tys, tzs}) { + if (row) row->clear(); + } +} + void MbD::ASMTItemIJ::setMarkerJ(const std::string& mkrJ) { markerJ = mkrJ; diff --git a/OndselSolver/ASMTItemIJ.h b/OndselSolver/ASMTItemIJ.h index 5cdbcc4b..e335a107 100644 --- a/OndselSolver/ASMTItemIJ.h +++ b/OndselSolver/ASMTItemIJ.h @@ -15,6 +15,7 @@ namespace MbD { { // public: + void clearResults(); ASMTItemIJ(); void initialize() override; void setMarkerI(const std::string& mkrI); diff --git a/OndselSolver/ASMTLimit.cpp b/OndselSolver/ASMTLimit.cpp index 5952c92d..e217e426 100644 --- a/OndselSolver/ASMTLimit.cpp +++ b/OndselSolver/ASMTLimit.cpp @@ -4,9 +4,38 @@ #include "SymbolicParser.h" #include "BasicUserFunction.h" #include "Constant.h" +#include "Units.h" +#include "LimitIJ.h" + +#include using namespace MbD; +void ASMTLimit::initialize() +{ + ASMTConstraintSet::initialize(); + storedEnergies = std::make_shared>(); + dissipatedPowers = std::make_shared>(); +} + +void ASMTLimit::clearResults() +{ + ASMTConstraintSet::clearResults(); + if (storedEnergies) storedEnergies->clear(); + if (dissipatedPowers) dissipatedPowers->clear(); +} + +void ASMTLimit::updateFromMbD() +{ + const auto state = std::static_pointer_cast(mbdObject)->energyState(); + const auto units = mbdUnits(); + const double energyUnit = units->torque / units->angle; + const double powerUnit = energyUnit / units->time; + powers->push_back(state.power * powerUnit); + storedEnergies->push_back(state.storedEnergy * energyUnit); + dissipatedPowers->push_back(state.dissipatedPower * powerUnit); +} + void MbD::ASMTLimit::initMarkers() { if (motionJoint == "") { @@ -31,29 +60,43 @@ void MbD::ASMTLimit::storeOnLevel(std::ofstream& os, size_t level) storeOnLevelString(os, level + 2, type); storeOnLevelString(os, level + 1, "Tol"); storeOnLevelString(os, level + 2, tol); + storeOnLevelString(os, level + 1, "Behavior"); + storeOnLevelString(os, level + 2, behavior); + storeOnLevelString(os, level + 1, "Stiffness"); + storeOnLevelString(os, level + 2, stiffness); + storeOnLevelString(os, level + 1, "Damping"); + storeOnLevelString(os, level + 2, damping); } void MbD::ASMTLimit::readMotionJoint(std::vector& lines) { - assert(readStringOffTop(lines) == "MotionJoint"); + if (readStringOffTop(lines) != "MotionJoint") { + throw SimulationStoppingError("Expected MotionJoint record."); + } motionJoint = readStringOffTop(lines); } void MbD::ASMTLimit::readLimit(std::vector& lines) { - assert(readStringOffTop(lines) == "Limit"); + if (readStringOffTop(lines) != "Limit") { + throw SimulationStoppingError("Expected Limit record."); + } limit = readStringOffTop(lines); } void MbD::ASMTLimit::readType(std::vector& lines) { - assert(readStringOffTop(lines) == "Type"); + if (readStringOffTop(lines) != "Type") { + throw SimulationStoppingError("Expected Type record."); + } type = readStringOffTop(lines); } void MbD::ASMTLimit::readTol(std::vector& lines) { - assert(readStringOffTop(lines) == "Tol"); + if (readStringOffTop(lines) != "Tol") { + throw SimulationStoppingError("Expected Tol record."); + } tol = readStringOffTop(lines); } @@ -64,6 +107,22 @@ void MbD::ASMTLimit::parseASMT(std::vector& lines) readLimit(lines); readType(lines); readTol(lines); + if (!lines.empty()) { + std::istringstream next(lines.front()); + std::string record; + next >> record; + if (record == "Behavior") readCompliance(lines); + } +} + +void MbD::ASMTLimit::readCompliance(std::vector& lines) +{ + if (readStringOffTop(lines) != "Behavior") throw SimulationStoppingError("Expected Behavior record."); + behavior = readStringOffTop(lines); + if (readStringOffTop(lines) != "Stiffness") throw SimulationStoppingError("Expected Stiffness record."); + stiffness = readStringOffTop(lines); + if (readStringOffTop(lines) != "Damping") throw SimulationStoppingError("Expected Damping record."); + damping = readStringOffTop(lines); } void MbD::ASMTLimit::createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) @@ -79,7 +138,7 @@ void MbD::ASMTLimit::createMbD(std::shared_ptr mbdSys, std::shared_ptr(limit, 1.0); parser->parseUserFunction(userFunc); auto& geolimit = parser->stack->top(); - geolimit = Symbolic::times(geolimit, sptrConstant(1.0 / mbdUnits->angle)); + geolimit = Symbolic::times(geolimit, sptrConstant(1.0 / coordinateUnit(*mbdUnits))); geolimit->createMbD(mbdSys, mbdUnits); geolimit = geolimit->simplified(geolimit); limitIJ->limit = geolimit->getValue(); @@ -89,10 +148,27 @@ void MbD::ASMTLimit::createMbD(std::shared_ptr mbdSys, std::shared_ptr(tol, 1.0); parser->parseUserFunction(userFunc); auto& geotol = parser->stack->top(); - geotol = Symbolic::times(geotol, sptrConstant(1.0 / mbdUnits->angle)); + geotol = Symbolic::times(geotol, sptrConstant(1.0 / coordinateUnit(*mbdUnits))); geotol->createMbD(mbdSys, mbdUnits); geotol = geotol->simplified(geotol); limitIJ->tol = geotol->getValue(); + if (behavior == "Compliant") { + auto parseConstant = [&](const std::string& expression, double unit) { + auto function = std::make_shared(expression, 1.0); + parser->parseUserFunction(function); + auto value = parser->stack->top(); + value = Symbolic::times(value, sptrConstant(unit)); + value->createMbD(mbdSys, mbdUnits); + return value->simplified(value)->getValue(); + }; + limitIJ->setCompliant( + parseConstant(stiffness, stiffnessUnit(*mbdUnits)), + parseConstant(damping, dampingUnit(*mbdUnits)) + ); + } + else if (behavior != "Rigid") { + throw SimulationStoppingError("Unknown joint-limit behavior."); + } } void MbD::ASMTLimit::setmotionJoint(const std::string& _motionJoint) @@ -114,3 +190,14 @@ void MbD::ASMTLimit::settol(const std::string& _tol) { tol = _tol; } + +void MbD::ASMTLimit::setcompliance(const std::string& newStiffness, const std::string& newDamping) +{ + behavior = "Compliant"; + stiffness = newStiffness; + damping = newDamping; +} + +double MbD::ASMTLimit::coordinateUnit(const Units& units) const { return units.angle; } +double MbD::ASMTLimit::stiffnessUnit(const Units& units) const { return units.angle / units.torque; } +double MbD::ASMTLimit::dampingUnit(const Units& units) const { return units.omega / units.torque; } diff --git a/OndselSolver/ASMTLimit.h b/OndselSolver/ASMTLimit.h index d8f4df8c..d5000a91 100644 --- a/OndselSolver/ASMTLimit.h +++ b/OndselSolver/ASMTLimit.h @@ -12,24 +12,35 @@ #include "ForceTorqueData.h" namespace MbD { + class Units; class ASMTLimit : public ASMTConstraintSet { // public: + void initialize() override; + void clearResults(); + void updateFromMbD() override; virtual void initMarkers(); void storeOnLevel(std::ofstream& os, size_t level) override; void readMotionJoint(std::vector& lines); void readLimit(std::vector& lines); void readType(std::vector& lines); void readTol(std::vector& lines); + void readCompliance(std::vector& lines); void parseASMT(std::vector& lines) override; void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; void setmotionJoint(const std::string& _motionJoint); void settype(const std::string& _type); void setlimit(const std::string& _limit); void settol(const std::string& _tol); + void setcompliance(const std::string& _stiffness, const std::string& _damping); + virtual double coordinateUnit(const Units& units) const; + virtual double stiffnessUnit(const Units& units) const; + virtual double dampingUnit(const Units& units) const; - std::string motionJoint, type, limit, tol; + std::string motionJoint, type, limit, tol; + std::string behavior = "Rigid", stiffness = "0.0", damping = "0.0"; + FRowDsptr storedEnergies, dissipatedPowers; }; } diff --git a/OndselSolver/ASMTPart.cpp b/OndselSolver/ASMTPart.cpp index c6fc6d16..3b0bfebf 100644 --- a/OndselSolver/ASMTPart.cpp +++ b/OndselSolver/ASMTPart.cpp @@ -115,6 +115,15 @@ FColDsptr MbD::ASMTPart::omeOpO() return omega3D; } +void MbD::ASMTPart::setCenterOfMassVelocity3D( + double vx, double vy, double vz, double wx, double wy, double wz) +{ + omega3D = std::make_shared>(ListD{wx, wy, wz}); + auto vOcmO = std::make_shared>(ListD{vx, vy, vz}); + auto rPcmO = rotationMatrix->timesFullColumn(principalMassMarker->position3D); + velocity3D = vOcmO->minusFullColumn(omega3D->cross(rPcmO)); +} + ASMTPart* MbD::ASMTPart::part() { return this; diff --git a/OndselSolver/ASMTPart.h b/OndselSolver/ASMTPart.h index 2554f788..98a15b06 100644 --- a/OndselSolver/ASMTPart.h +++ b/OndselSolver/ASMTPart.h @@ -5,7 +5,7 @@ * * * See LICENSE file for details about copyright. * ***************************************************************************/ - + #pragma once #include "ASMTSpatialContainer.h" @@ -24,6 +24,8 @@ namespace MbD { void readPartSeries(std::vector& lines); FColDsptr vOcmO() override; FColDsptr omeOpO() override; + void setCenterOfMassVelocity3D(double vx, double vy, double vz, + double wx, double wy, double wz); ASMTPart* part() override; void createMbD(std::shared_ptr mbdSys, std::shared_ptr mbdUnits) override; void preMbDrunDragStep(std::shared_ptr mbdSys, std::shared_ptr mbdUnits); diff --git a/OndselSolver/ASMTRotationLimit.cpp b/OndselSolver/ASMTRotationLimit.cpp index 836b104f..aa0ce9ca 100644 --- a/OndselSolver/ASMTRotationLimit.cpp +++ b/OndselSolver/ASMTRotationLimit.cpp @@ -4,6 +4,7 @@ #include "BasicUserFunction.h" #include "Constant.h" #include "RotationLimitIJ.h" +#include "Units.h" using namespace MbD; @@ -14,6 +15,10 @@ std::shared_ptr MbD::ASMTRotationLimit::With() return rotationLimit; } +double MbD::ASMTRotationLimit::coordinateUnit(const Units& units) const { return units.angle; } +double MbD::ASMTRotationLimit::stiffnessUnit(const Units& units) const { return units.angle / units.torque; } +double MbD::ASMTRotationLimit::dampingUnit(const Units& units) const { return units.omega / units.torque; } + std::shared_ptr MbD::ASMTRotationLimit::mbdClassNew() { return RotationLimitIJ::With(); diff --git a/OndselSolver/ASMTRotationLimit.h b/OndselSolver/ASMTRotationLimit.h index 3f763c0f..5b6f9c96 100644 --- a/OndselSolver/ASMTRotationLimit.h +++ b/OndselSolver/ASMTRotationLimit.h @@ -18,6 +18,9 @@ namespace MbD { static std::shared_ptr With(); std::shared_ptr mbdClassNew() override; void storeOnLevel(std::ofstream& os, size_t level) override; + double coordinateUnit(const Units& units) const override; + double stiffnessUnit(const Units& units) const override; + double dampingUnit(const Units& units) const override; }; } diff --git a/OndselSolver/ASMTSpatialContainer.cpp b/OndselSolver/ASMTSpatialContainer.cpp index 9326dba2..600f4035 100644 --- a/OndselSolver/ASMTSpatialContainer.cpp +++ b/OndselSolver/ASMTSpatialContainer.cpp @@ -77,19 +77,22 @@ void MbD::ASMTSpatialContainer::setPrincipalMassMarker(std::shared_ptrclear(); + } +} + void MbD::ASMTSpatialContainer::readRefPoints(std::vector& lines) { assert(lines[0].find("RefPoints") != std::string::npos); lines.erase(lines.begin()); refPoints->clear(); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("RefCurves") != std::string::npos; - }); - std::vector refPointsLines(lines.begin(), it); - while (!refPointsLines.empty()) { - readRefPoint(refPointsLines); + while (!lines.empty() && readString(lines.front()) == "RefPoint") { + readRefPoint(lines); } - lines.erase(lines.begin(), it); } void MbD::ASMTSpatialContainer::readRefPoint(std::vector& lines) @@ -107,14 +110,9 @@ void MbD::ASMTSpatialContainer::readRefCurves(std::vector& lines) assert(lines[0].find("RefCurves") != std::string::npos); lines.erase(lines.begin()); refCurves->clear(); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("RefSurfaces") != std::string::npos; - }); - std::vector refCurvesLines(lines.begin(), it); - while (!refCurvesLines.empty()) { - readRefCurve(refCurvesLines); + while (!lines.empty() && readString(lines.front()) == "RefCurve") { + readRefCurve(lines); } - lines.erase(lines.begin(), it); } void MbD::ASMTSpatialContainer::readRefCurve(std::vector&) @@ -127,14 +125,9 @@ void MbD::ASMTSpatialContainer::readRefSurfaces(std::vector& lines) assert(lines[0].find("RefSurfaces") != std::string::npos); lines.erase(lines.begin()); refSurfaces->clear(); - auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) { - return s.find("Part") != std::string::npos; - }); - std::vector refSurfacesLines(lines.begin(), it); - while (!refSurfacesLines.empty()) { - readRefSurface(refSurfacesLines); + while (!lines.empty() && readString(lines.front()) == "RefSurface") { + readRefSurface(lines); } - lines.erase(lines.begin(), it); } void MbD::ASMTSpatialContainer::readRefSurface(std::vector&) diff --git a/OndselSolver/ASMTSpatialContainer.h b/OndselSolver/ASMTSpatialContainer.h index 01b7e64d..148d4a4a 100644 --- a/OndselSolver/ASMTSpatialContainer.h +++ b/OndselSolver/ASMTSpatialContainer.h @@ -76,6 +76,7 @@ namespace MbD { void updateFromInitiallyAssembledState() override; void updateFromInputState() override; void updateFromMbD() override; + void clearResults(); void compareResults(AnalysisType type) override; void outputResults(AnalysisType type) override; void addRefPoint(std::shared_ptr refPoint); diff --git a/OndselSolver/ASMTTranslationLimit.cpp b/OndselSolver/ASMTTranslationLimit.cpp index 7d87ce66..00ddca56 100644 --- a/OndselSolver/ASMTTranslationLimit.cpp +++ b/OndselSolver/ASMTTranslationLimit.cpp @@ -3,6 +3,7 @@ #include "BasicUserFunction.h" #include "Constant.h" #include "TranslationLimitIJ.h" +#include "Units.h" using namespace MbD; @@ -13,6 +14,10 @@ std::shared_ptr MbD::ASMTTranslationLimit::With() return translationLimit; } +double MbD::ASMTTranslationLimit::coordinateUnit(const Units& units) const { return units.length; } +double MbD::ASMTTranslationLimit::stiffnessUnit(const Units& units) const { return units.length / units.force; } +double MbD::ASMTTranslationLimit::dampingUnit(const Units& units) const { return units.velocity / units.force; } + std::shared_ptr MbD::ASMTTranslationLimit::mbdClassNew() { return TranslationLimitIJ::With(); diff --git a/OndselSolver/ASMTTranslationLimit.h b/OndselSolver/ASMTTranslationLimit.h index bc43ef9b..d68f2267 100644 --- a/OndselSolver/ASMTTranslationLimit.h +++ b/OndselSolver/ASMTTranslationLimit.h @@ -18,6 +18,9 @@ namespace MbD { static std::shared_ptr With(); std::shared_ptr mbdClassNew() override; void storeOnLevel(std::ofstream& os, size_t level) override; + double coordinateUnit(const Units& units) const override; + double stiffnessUnit(const Units& units) const override; + double dampingUnit(const Units& units) const override; }; } diff --git a/OndselSolver/AbsConstraint.cpp b/OndselSolver/AbsConstraint.cpp index f323a4c6..7044f5da 100644 --- a/OndselSolver/AbsConstraint.cpp +++ b/OndselSolver/AbsConstraint.cpp @@ -23,16 +23,17 @@ AbsConstraint::AbsConstraint(size_t i) void AbsConstraint::calcPostDynCorrectorIteration() { if (axis < 3) { - aG = static_cast(owner)->qX->at(axis); + aG = static_cast(owner)->qX->at(axis) - aConstant; } else { - aG = static_cast(owner)->qE->at(axis - 3); + aG = static_cast(owner)->qE->at(axis - 3) - aConstant; } } void AbsConstraint::useEquationNumbers() { - iqXminusOnePlusAxis = static_cast(owner)->iqX + axis; + const auto frame = static_cast(owner); + iqXminusOnePlusAxis = axis < 3 ? frame->iqX + axis : frame->iqE + axis - 3; } std::string MbD::AbsConstraint::constraintSpec() @@ -75,3 +76,13 @@ void AbsConstraint::fillAccICIterError(FColDsptr col) } col->atiplusNumber(iG, sum); } + +void AbsConstraint::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusNumber(iG, iqXminusOnePlusAxis, 1.0); +} + +void AbsConstraint::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusNumber(iqXminusOnePlusAxis, iG, 1.0); +} diff --git a/OndselSolver/AbsConstraint.h b/OndselSolver/AbsConstraint.h index 58debf2e..8f392a18 100644 --- a/OndselSolver/AbsConstraint.h +++ b/OndselSolver/AbsConstraint.h @@ -16,6 +16,8 @@ namespace MbD { { //axis iqXminusOnePlusAxis public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; //AbsConstraint(); //AbsConstraint(const std::string& str); AbsConstraint(size_t axis); diff --git a/OndselSolver/AngleZConstraintIJ.cpp b/OndselSolver/AngleZConstraintIJ.cpp index 01d039d7..e48ac80d 100644 --- a/OndselSolver/AngleZConstraintIJ.cpp +++ b/OndselSolver/AngleZConstraintIJ.cpp @@ -52,6 +52,24 @@ void MbD::AngleZConstraintIJ::initializeLocally() thezIeJe->initializeLocally(); } +void MbD::AngleZConstraintIJ::postDynCorrectorIteration() +{ + thezIeJe->postDynCorrectorIteration(); + Constraint::postDynCorrectorIteration(); +} + +void MbD::AngleZConstraintIJ::postDynOutput() +{ + thezIeJe->postDynOutput(); + Constraint::postDynOutput(); +} + +void MbD::AngleZConstraintIJ::postDynPredictor() +{ + thezIeJe->postDynPredictor(); + Constraint::postDynPredictor(); +} + void MbD::AngleZConstraintIJ::postInput() { assert(aConstant != std::numeric_limits::min()); @@ -70,6 +88,12 @@ void MbD::AngleZConstraintIJ::preAccIC() ConstraintIJ::preAccIC(); } +void MbD::AngleZConstraintIJ::preDynOutput() +{ + thezIeJe->preDynOutput(); + Constraint::preDynOutput(); +} + void MbD::AngleZConstraintIJ::prePosIC() { thezIeJe->prePosIC(); diff --git a/OndselSolver/AngleZConstraintIJ.h b/OndselSolver/AngleZConstraintIJ.h index 1b287819..a94035a3 100644 --- a/OndselSolver/AngleZConstraintIJ.h +++ b/OndselSolver/AngleZConstraintIJ.h @@ -25,9 +25,13 @@ namespace MbD { void initialize() override; void initializeGlobally() override; void initializeLocally() override; + void postDynCorrectorIteration() override; + void postDynOutput() override; + void postDynPredictor() override; void postInput() override; void postPosICIteration() override; void preAccIC() override; + void preDynOutput() override; void prePosIC() override; void preVelIC() override; void simUpdateAll() override; diff --git a/OndselSolver/AngleZConstraintIqcJc.cpp b/OndselSolver/AngleZConstraintIqcJc.cpp index 511b7ed3..59197bf5 100644 --- a/OndselSolver/AngleZConstraintIqcJc.cpp +++ b/OndselSolver/AngleZConstraintIqcJc.cpp @@ -4,6 +4,24 @@ using namespace MbD; +namespace { +void addOuterProduct( + SpMatDsptr mat, + size_t rowStart, + const FRowDsptr& row, + size_t columnStart, + const FRowDsptr& column, + double factor +) +{ + for (size_t i = 0; i < row->size(); ++i) { + for (size_t j = 0; j < column->size(); ++j) { + mat->atijplusNumber(rowStart + i, columnStart + j, factor * row->at(i) * column->at(j)); + } + } +} +} + MbD::AngleZConstraintIqcJc::AngleZConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj) : AngleZConstraintIJ(frmi, frmj) { pGpEI = std::make_shared>(4); @@ -79,8 +97,48 @@ void MbD::AngleZConstraintIqcJc::fillVelICJacob(SpMatDsptr mat) mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); } +void MbD::AngleZConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void MbD::AngleZConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + void MbD::AngleZConstraintIqcJc::useEquationNumbers() { auto frmIeqc = std::static_pointer_cast(frmI); iqEI = frmIeqc->iqE(); } + +double MbD::AngleZConstraintIqcJc::constraintVelocity() const +{ + auto frameI = std::static_pointer_cast(frmI); + return pGpEI->timesFullColumn(frameI->qEdot()); +} + +void MbD::AngleZConstraintIqcJc::fillGeneralizedForce(FColDsptr col, double multiplier) +{ + col->atiplusFullVectortimes(iqEI, pGpEI, multiplier); +} + +void MbD::AngleZConstraintIqcJc::fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative +) +{ + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, multiplier); + addOuterProduct(mat, iqEI, pGpEI, iqEI, pGpEI, derivative); +} + +void MbD::AngleZConstraintIqcJc::fillGeneralizedForceVelocityJacobian( + SpMatDsptr mat, + double derivative +) +{ + addOuterProduct(mat, iqEI, pGpEI, iqEI, pGpEI, derivative); +} diff --git a/OndselSolver/AngleZConstraintIqcJc.h b/OndselSolver/AngleZConstraintIqcJc.h index 7399baab..1854cec0 100644 --- a/OndselSolver/AngleZConstraintIqcJc.h +++ b/OndselSolver/AngleZConstraintIqcJc.h @@ -29,6 +29,16 @@ namespace MbD { void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + double constraintVelocity() const override; + void fillGeneralizedForce(FColDsptr col, double multiplier) override; + void fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative + ) override; + void fillGeneralizedForceVelocityJacobian(SpMatDsptr mat, double derivative) override; void useEquationNumbers() override; FRowDsptr pGpEI; diff --git a/OndselSolver/AngleZConstraintIqcJqc.cpp b/OndselSolver/AngleZConstraintIqcJqc.cpp index c70c4bc0..f7a70b43 100644 --- a/OndselSolver/AngleZConstraintIqcJqc.cpp +++ b/OndselSolver/AngleZConstraintIqcJqc.cpp @@ -4,6 +4,24 @@ using namespace MbD; +namespace { +void addOuterProduct( + SpMatDsptr mat, + size_t rowStart, + const FRowDsptr& row, + size_t columnStart, + const FRowDsptr& column, + double factor +) +{ + for (size_t i = 0; i < row->size(); ++i) { + for (size_t j = 0; j < column->size(); ++j) { + mat->atijplusNumber(rowStart + i, columnStart + j, factor * row->at(i) * column->at(j)); + } + } +} +} + MbD::AngleZConstraintIqcJqc::AngleZConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj) : AngleZConstraintIqcJc(frmi, frmj) { pGpEJ = std::make_shared>(4); @@ -85,6 +103,22 @@ void MbD::AngleZConstraintIqcJqc::fillVelICJacob(SpMatDsptr mat) mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); } +void MbD::AngleZConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + AngleZConstraintIqcJc::fillpFpy(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void MbD::AngleZConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + AngleZConstraintIqcJc::fillpFpydot(mat); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + void MbD::AngleZConstraintIqcJqc::useEquationNumbers() { AngleZConstraintIqcJc::useEquationNumbers(); @@ -96,3 +130,43 @@ std::string MbD::AngleZConstraintIqcJqc::constraintSpec() { return "AngleZConstraintIJ"; } + +double MbD::AngleZConstraintIqcJqc::constraintVelocity() const +{ + auto frameJ = std::static_pointer_cast(frmJ); + return AngleZConstraintIqcJc::constraintVelocity() + + pGpEJ->timesFullColumn(frameJ->qEdot()); +} + +void MbD::AngleZConstraintIqcJqc::fillGeneralizedForce(FColDsptr col, double multiplier) +{ + AngleZConstraintIqcJc::fillGeneralizedForce(col, multiplier); + col->atiplusFullVectortimes(iqEJ, pGpEJ, multiplier); +} + +void MbD::AngleZConstraintIqcJqc::fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative +) +{ + AngleZConstraintIqcJc::fillGeneralizedForcePositionJacobian(mat, multiplier, derivative); + auto crossHessian = ppGpEIpEJ->times(multiplier); + mat->atijplusFullMatrix(iqEI, iqEJ, crossHessian); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, crossHessian); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, multiplier); + addOuterProduct(mat, iqEI, pGpEI, iqEJ, pGpEJ, derivative); + addOuterProduct(mat, iqEJ, pGpEJ, iqEI, pGpEI, derivative); + addOuterProduct(mat, iqEJ, pGpEJ, iqEJ, pGpEJ, derivative); +} + +void MbD::AngleZConstraintIqcJqc::fillGeneralizedForceVelocityJacobian( + SpMatDsptr mat, + double derivative +) +{ + AngleZConstraintIqcJc::fillGeneralizedForceVelocityJacobian(mat, derivative); + addOuterProduct(mat, iqEI, pGpEI, iqEJ, pGpEJ, derivative); + addOuterProduct(mat, iqEJ, pGpEJ, iqEI, pGpEI, derivative); + addOuterProduct(mat, iqEJ, pGpEJ, iqEJ, pGpEJ, derivative); +} diff --git a/OndselSolver/AngleZConstraintIqcJqc.h b/OndselSolver/AngleZConstraintIqcJqc.h index 35c63b1e..b05b1b8c 100644 --- a/OndselSolver/AngleZConstraintIqcJqc.h +++ b/OndselSolver/AngleZConstraintIqcJqc.h @@ -29,6 +29,16 @@ namespace MbD { void fillPosICJacob(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + double constraintVelocity() const override; + void fillGeneralizedForce(FColDsptr col, double multiplier) override; + void fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative + ) override; + void fillGeneralizedForceVelocityJacobian(SpMatDsptr mat, double derivative) override; void useEquationNumbers() override; std::string constraintSpec() override; diff --git a/OndselSolver/AngleZIecJec.cpp b/OndselSolver/AngleZIecJec.cpp index bb544c7d..fd529de3 100644 --- a/OndselSolver/AngleZIecJec.cpp +++ b/OndselSolver/AngleZIecJec.cpp @@ -115,3 +115,31 @@ double MbD::AngleZIecJec::value() { return thez; } + +void AngleZIecJec::postDynPredictor() +{ + aA00IeJe->postDynPredictor(); + aA10IeJe->postDynPredictor(); + KinematicIeJe::postDynPredictor(); +} + +void AngleZIecJec::postDynCorrectorIteration() +{ + aA00IeJe->postDynCorrectorIteration(); + aA10IeJe->postDynCorrectorIteration(); + KinematicIeJe::postDynCorrectorIteration(); +} + +void AngleZIecJec::preDynOutput() +{ + aA00IeJe->preDynOutput(); + aA10IeJe->preDynOutput(); + KinematicIeJe::preDynOutput(); +} + +void AngleZIecJec::postDynOutput() +{ + aA00IeJe->postDynOutput(); + aA10IeJe->postDynOutput(); + KinematicIeJe::postDynOutput(); +} diff --git a/OndselSolver/AngleZIecJec.h b/OndselSolver/AngleZIecJec.h index 18c05de5..691d15dd 100644 --- a/OndselSolver/AngleZIecJec.h +++ b/OndselSolver/AngleZIecJec.h @@ -16,6 +16,10 @@ namespace MbD { { //thez aA00IeJe aA10IeJe cosOverSSq sinOverSSq twoCosSinOverSSqSq dSqOverSSqSq public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; AngleZIecJec(); AngleZIecJec(EndFrmsptr frmi, EndFrmsptr frmj); diff --git a/OndselSolver/AppliedForceTorque.cpp b/OndselSolver/AppliedForceTorque.cpp new file mode 100644 index 00000000..a3894e89 --- /dev/null +++ b/OndselSolver/AppliedForceTorque.cpp @@ -0,0 +1,728 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +#include "AppliedForceTorque.h" +#include "EndFrameqc.h" +#include "EndFrameqct.h" +#include "Constraint.h" +#include "Joint.h" +#include "SimulationStoppingError.h" +#include "Symbolic.h" +#include "System.h" +#include "SymTime.h" +#include +#include +#include + +using namespace MbD; +namespace { +using V = AppliedForceTorque::Vector; +V operator+(const V& a, const V& b) { return {a[0]+b[0], a[1]+b[1], a[2]+b[2]}; } +V operator-(const V& a, const V& b) { return {a[0]-b[0], a[1]-b[1], a[2]-b[2]}; } +V operator*(double s, const V& a) { return {s*a[0], s*a[1], s*a[2]}; } +double dot(const V& a, const V& b) { return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]; } +V cross(const V& a, const V& b) +{ return {a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]}; } +V vector(const FColDsptr& a) { return {a->at(0), a->at(1), a->at(2)}; } +bool finite(const V& a) { return std::isfinite(a[0]) && std::isfinite(a[1]) && std::isfinite(a[2]); } +bool nonnegative(const V& a) { return a[0] >= 0 && a[1] >= 0 && a[2] >= 0; } + +// Position and angular-velocity maps for the seven body coordinates. Derivatives +// are analytical, including offset attachment points and quaternion curvature. +struct Endpoint { + bool moving = false; + V r{}, v{}, w{}; + std::array equation{}; + std::array J{}, W{}, dv{}, dw{}; + std::array, 7> H{}, dW{}; + explicit Endpoint(const std::shared_ptr& frame) + { + r = vector(frame->rOeO); + auto qc = std::dynamic_pointer_cast(frame); + if (!qc) return; + moving = true; + v = vector(qc->qXdot()); + for (size_t a = 0; a < 3; ++a) { + equation[a] = qc->iqX()+a; + J[a][a] = 1; + } + auto B = qc->aBOp(); + for (size_t a = 0; a < 4; ++a) { + equation[a+3] = qc->iqE()+a; + for (size_t xyz = 0; xyz < 3; ++xyz) { + J[a+3][xyz] = qc->prOeOpE->at(xyz)->at(a); + W[a+3][xyz] = 2*B->at(xyz)->at(a); + } + v = v + qc->qEdot()->at(a)*J[a+3]; + for (size_t b = 0; b < 4; ++b) { + H[a+3][b+3] = vector(qc->pprOeOpEpE->at(a)->at(b)); + dv[a+3] = dv[a+3] + qc->qEdot()->at(b)*H[a+3][b+3]; + } + } + for (size_t b = 0; b < 4; ++b) { + std::array e{}; + e[b] = 2; + dW[3][b+3] = {e[3], e[2], -e[1]}; + dW[4][b+3] = {-e[2], e[3], e[0]}; + dW[5][b+3] = {e[1], -e[0], e[3]}; + dW[6][b+3] = {-e[0], -e[1], -e[2]}; + } + for (size_t a = 0; a < 4; ++a) { + w = w + qc->qEdot()->at(a)*W[a+3]; + for (size_t b = 0; b < 4; ++b) + dw[b+3] = dw[b+3] + qc->qEdot()->at(a)*dW[a+3][b+3]; + } + } +}; +void validateFrames(const std::shared_ptr& i, const std::shared_ptr& j) +{ + if (!i || !j) throw std::invalid_argument("A load requires two attachment markers"); + if (std::dynamic_pointer_cast(i) || std::dynamic_pointer_cast(j)) + throw std::invalid_argument("Loads require markers fixed to their bodies"); +} +} + +struct AppliedForceTorque::Evaluation { + Endpoint i, j; + V separation{}, relativeVelocity{}, relativeAngularVelocity{}, unit{}, f{}, t{}, tj{}; + V xI{}, yI{}, xJ{}, yJ{}; + double length = 0, magnitude = 0, twistSin = 0, twistCos = 0; + double frictionStaticMagnitude = 0, frictionDynamicMagnitude = 0; + double storedEnergy = 0, dissipatedPower = 0; + Evaluation(const std::shared_ptr& a, const std::shared_ptr& b) : i(a), j(b) {} +}; + +AppliedForceTorque::AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, V f, V t) + : frameI(i), frameJ(j), force(f), torque(t) +{ + validateFrames(i, j); + if (!finite(f) || !finite(t)) throw std::invalid_argument("Load components must be finite"); +} + +AppliedForceTorque::AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, double k, double c, double l) + : frameI(i), frameJ(j), spring(true), stiffness(k), damping(c), restLength(l) +{ + validateFrames(i, j); + if (!std::isfinite(k) || !std::isfinite(c) || !std::isfinite(l) || k < 0 || c < 0 || l < 0) + throw std::invalid_argument("Spring stiffness, damping and rest length must be finite and nonnegative"); +} + +AppliedForceTorque::AppliedForceTorque(std::shared_ptr i, + std::shared_ptr j, double k, double c, double angle, bool torsional) + : frameI(i), frameJ(j), torsionalSpring(torsional), stiffness(k), damping(c), restLength(angle) +{ + validateFrames(i, j); + if (!torsional || !std::isfinite(k) || !std::isfinite(c) || !std::isfinite(angle) + || k < 0 || c < 0) + throw std::invalid_argument("Torsional stiffness and damping must be finite and nonnegative"); +} + +AppliedForceTorque::AppliedForceTorque(std::shared_ptr i, + std::shared_ptr j, Vector direction, + std::shared_ptr magnitude, bool isTorque) + : frameI(i), frameJ(j), formulaDirection(direction), + formulaMagnitude(std::move(magnitude)), formulaTorque(isTorque) +{ + validateFrames(i, j); + if (!finite(direction) || !formulaMagnitude) + throw std::invalid_argument("Formula load direction and magnitude are required"); +} + +AppliedForceTorque::AppliedForceTorque(std::shared_ptr i, + std::shared_ptr j, ContactEvaluator evaluator) + : frameI(i), frameJ(j), contactEvaluator(std::move(evaluator)) +{ + validateFrames(i, j); + if (!contactEvaluator) + throw std::invalid_argument("A contact load requires an evaluator"); +} + +void AppliedForceTorque::validateBushingMatrix(const std::array& matrix) +{ + // Normalize by the diagonal before Cholesky: translation and rotation + // blocks carry different units and may have very different magnitudes. + double a[6][6] {}; + for (size_t i = 0; i < 6; ++i) { + if (!std::isfinite(matrix[6*i+i]) || matrix[6*i+i] < 0) + throw std::invalid_argument("Bushing matrix requires nonnegative finite diagonals"); + for (size_t j = 0; j < 6; ++j) { + const double value = matrix[6*i+j]; + if (!std::isfinite(value) || value != matrix[6*j+i]) + throw std::invalid_argument("Bushing matrix must be finite and symmetric"); + const double scale = std::sqrt(matrix[6*i+i]) * std::sqrt(matrix[6*j+j]); + if (scale > 0) a[i][j] = value / scale; + else if (value != 0) + throw std::invalid_argument("A zero bushing diagonal requires a zero row and column"); + } + } + for (size_t i = 0; i < 6; ++i) { + if (a[i][i] < -1e-10) + throw std::invalid_argument("Bushing matrix must be positive semidefinite"); + if (a[i][i] <= 1e-10) { + for (size_t j = i + 1; j < 6; ++j) + if (std::abs(a[j][i]) > 1e-10) + throw std::invalid_argument("Bushing matrix must be positive semidefinite"); + continue; + } + for (size_t j = i + 1; j < 6; ++j) + for (size_t k = j; k < 6; ++k) + a[k][j] = a[j][k] = a[k][j] - a[j][i] * a[k][i] / a[i][i]; + } +} + +AppliedForceTorque::AppliedForceTorque(std::shared_ptr i, + std::shared_ptr j, BushingParameters parameters) + : frameI(i), frameJ(j), bushing(true), bushingParameters(parameters) +{ + validateFrames(i, j); + if (parameters.coupled) { + validateBushingMatrix(parameters.stiffnessMatrix); + validateBushingMatrix(parameters.dampingMatrix); + } + if (!finite(parameters.linearStiffness) || !finite(parameters.linearDamping) + || !finite(parameters.angularStiffness) || !finite(parameters.angularDamping) + || !nonnegative(parameters.linearStiffness) || !nonnegative(parameters.linearDamping) + || !nonnegative(parameters.angularStiffness) || !nonnegative(parameters.angularDamping)) + throw std::invalid_argument("Bushing stiffness and damping must be finite and nonnegative"); +} + +AppliedForceTorque::AppliedForceTorque(std::shared_ptr i, + std::shared_ptr j, AxisFrictionParameters parameters) + : frameI(i), frameJ(j), axisFriction(true), axisFrictionParameters(parameters) +{ + validateFrames(i, j); + if (!std::isfinite(parameters.staticMagnitude) + || !std::isfinite(parameters.dynamicMagnitude) + || !std::isfinite(parameters.transitionVelocity) + || !std::isfinite(parameters.viscousCoefficient) + || parameters.staticMagnitude < parameters.dynamicMagnitude + || parameters.dynamicMagnitude < 0 || !(parameters.transitionVelocity > 0) + || parameters.viscousCoefficient < 0 + || (parameters.reactionBased + && (!std::isfinite(parameters.effectiveRadius) + || !(parameters.effectiveRadius > 0) + || parameters.reactionJoint.expired()))) + throw std::invalid_argument( + "Axis friction requires static >= dynamic >= 0, positive transition velocity, " + "nonnegative viscous damping, and a valid reaction source" + ); +} + +AppliedForceTorque::Evaluation AppliedForceTorque::evaluate() const +{ + Evaluation e(frameI, frameJ); + if (!enabled) return e; + e.separation = e.j.r-e.i.r; + e.relativeVelocity = e.j.v-e.i.v; + e.f = force; + e.t = torque; + if (contactEvaluator) { + const auto state = [](const Endpoint& endpoint, const std::shared_ptr& frame) { + FrameState result; + result.time = frame->root()->time->getValue(); + result.position = endpoint.r; + result.velocity = endpoint.v; + result.omega = endpoint.w; + for (size_t row = 0; row < 3; ++row) + for (size_t column = 0; column < 3; ++column) + result.rotation[3*row+column] = frame->aAOe->at(row)->at(column); + return result; + }; + const auto wrench = contactEvaluator(state(e.i, frameI), state(e.j, frameJ)); + if (!finite(wrench.forceOnI) || !finite(wrench.torqueOnI)) + throw SimulationStoppingError("Contact evaluator returned a non-finite wrench"); + e.f = wrench.forceOnI; + e.t = wrench.torqueOnI; + e.storedEnergy = wrench.storedEnergy; + e.dissipatedPower = wrench.dissipatedPower; + } + else if (bushing) { + const auto column = [](const FMatDsptr& matrix, size_t index) { + return V {matrix->at(0)->at(index), matrix->at(1)->at(index), matrix->at(2)->at(index)}; + }; + std::array axesI, axesJ; + std::array strains{}, rates{}; + for (size_t axis = 0; axis < 3; ++axis) { + axesI[axis] = column(frameI->aAOe, axis); + axesJ[axis] = column(frameJ->aAOe, axis); + const double displacement = dot(axesI[axis], e.separation); + // Translational strain is expressed in the rotating I frame. + const double speed = dot(axesI[axis], + e.relativeVelocity - cross(e.i.w, e.separation)); + strains[axis] = displacement; + rates[axis] = speed; + if (bushingParameters.coupled) continue; + e.f = e.f + (bushingParameters.linearStiffness[axis] * displacement + + bushingParameters.linearDamping[axis] * speed) * axesI[axis]; + e.storedEnergy += 0.5 * bushingParameters.linearStiffness[axis] + * displacement * displacement; + e.dissipatedPower += bushingParameters.linearDamping[axis] * speed * speed; + } + + double relative[3][3] {}; + double trace = 0; + for (size_t row = 0; row < 3; ++row) { + for (size_t columnIndex = 0; columnIndex < 3; ++columnIndex) + relative[row][columnIndex] = dot(axesI[row], axesJ[columnIndex]); + trace += relative[row][row]; + } + const double cosine = std::clamp(0.5 * (trace - 1), -1.0, 1.0); + const double angle = std::acos(cosine); + V rotationVector { + relative[2][1] - relative[1][2], + relative[0][2] - relative[2][0], + relative[1][0] - relative[0][1] + }; + const double sine = std::sin(angle); + if (angle < 1e-8) { + rotationVector = 0.5 * rotationVector; + } + else if (std::abs(sine) > 1e-8) { + rotationVector = (angle / (2 * sine)) * rotationVector; + } + else { + V axis { + std::sqrt(std::max(0.0, 0.5 * (relative[0][0] + 1))), + std::sqrt(std::max(0.0, 0.5 * (relative[1][1] + 1))), + std::sqrt(std::max(0.0, 0.5 * (relative[2][2] + 1))) + }; + const auto largest = std::distance(axis.begin(), std::max_element(axis.begin(), axis.end())); + if (axis[largest] > 1e-8) { + const size_t next = (largest + 1) % 3; + const size_t last = (largest + 2) % 3; + axis[next] = (relative[largest][next] + relative[next][largest]) + / (4 * axis[largest]); + axis[last] = (relative[largest][last] + relative[last][largest]) + / (4 * axis[largest]); + } + rotationVector = angle * axis; + } + e.relativeAngularVelocity = e.j.w - e.i.w; + // phiDot = J_l(phi)^-1 * R_I^T * (omega_J - omega_I). + // A torque conjugate to phi is mapped with the transpose Jacobian. + const double a = angle < 1e-4 + ? 1.0 / 12.0 + angle * angle / 720.0 + : (1.0 - 0.5 * angle / std::tan(0.5 * angle)) / (angle * angle); + V localOmega {}; + for (size_t axis = 0; axis < 3; ++axis) + localOmega[axis] = dot(axesI[axis], e.relativeAngularVelocity); + const V strainRate = localOmega - 0.5 * cross(rotationVector, localOmega) + + a * cross(rotationVector, cross(rotationVector, localOmega)); + V conjugate {}; + for (size_t axis = 0; axis < 3; ++axis) { + strains[axis + 3] = rotationVector[axis]; + rates[axis + 3] = strainRate[axis]; + if (bushingParameters.coupled) continue; + conjugate[axis] = bushingParameters.angularStiffness[axis] * rotationVector[axis] + + bushingParameters.angularDamping[axis] * strainRate[axis]; + e.storedEnergy += 0.5 * bushingParameters.angularStiffness[axis] + * rotationVector[axis] * rotationVector[axis]; + e.dissipatedPower += bushingParameters.angularDamping[axis] + * strainRate[axis] * strainRate[axis]; + } + if (bushingParameters.coupled) { + std::array stress{}; + for (size_t row = 0; row < 6; ++row) { + for (size_t column = 0; column < 6; ++column) { + const double elastic = bushingParameters.stiffnessMatrix[6*row+column] + * strains[column]; + const double viscous = bushingParameters.dampingMatrix[6*row+column] + * rates[column]; + stress[row] += elastic + viscous; + e.storedEnergy += 0.5 * strains[row] * elastic; + e.dissipatedPower += rates[row] * viscous; + } + } + for (size_t axis = 0; axis < 3; ++axis) { + e.f = e.f + stress[axis] * axesI[axis]; + conjugate[axis] = stress[axis + 3]; + } + } + const V localTorque = conjugate + 0.5 * cross(rotationVector, conjugate) + + a * cross(rotationVector, cross(rotationVector, conjugate)); + // The translational frame-rotation couple belongs to I. Transporting + // the total opposite wrench below leaves only -rotationalTorque on J. + e.t = cross(e.separation, e.f); + for (size_t axis = 0; axis < 3; ++axis) + e.t = e.t + localTorque[axis] * axesI[axis]; + } + else if (axisFriction) { + e.unit = { + frameI->aAOe->at(0)->at(2), + frameI->aAOe->at(1)->at(2), + frameI->aAOe->at(2)->at(2) + }; + e.relativeAngularVelocity = e.j.w - e.i.w; + const double speed = dot( + e.unit, + axisFrictionParameters.rotational ? e.relativeAngularVelocity : e.relativeVelocity + ); + double scale = 1; + if (axisFrictionParameters.reactionBased) { + const auto joint = axisFrictionParameters.reactionJoint.lock(); + if (!joint) { + throw SimulationStoppingError("Reaction-based friction lost its source joint"); + } + const auto reaction = vector(joint->aFX()); + const auto transverse = axisFrictionParameters.axialReaction + ? dot(reaction, e.unit) * e.unit + : reaction - dot(reaction, e.unit) * e.unit; + scale = std::sqrt(dot(transverse, transverse)); + if (axisFrictionParameters.rotational) { + scale *= axisFrictionParameters.effectiveRadius; + } + } + e.frictionStaticMagnitude = scale * axisFrictionParameters.staticMagnitude; + e.frictionDynamicMagnitude = scale * axisFrictionParameters.dynamicMagnitude; + const double ratio = speed / axisFrictionParameters.transitionVelocity; + const double dryMagnitude = e.frictionDynamicMagnitude + + (e.frictionStaticMagnitude - e.frictionDynamicMagnitude) + * std::exp(-(ratio * ratio)); + const double magnitude = dryMagnitude * std::tanh(ratio) + + axisFrictionParameters.viscousCoefficient * speed; + e.length = speed; + e.magnitude = magnitude; + if (axisFrictionParameters.rotational) + e.t = magnitude * e.unit; + else + e.f = magnitude * e.unit; + e.dissipatedPower = magnitude * speed; + } + else if (formulaMagnitude) { + const double magnitude = formulaMagnitude->getValue(); + if (!std::isfinite(magnitude)) + throw SimulationStoppingError("Load formula returned a non-finite magnitude"); + if (formulaTorque) e.t = magnitude*formulaDirection; + else e.f = magnitude*formulaDirection; + } + else if (spring) { + e.length = std::sqrt(dot(e.separation, e.separation)); + if (!(e.length > 0) || !std::isfinite(e.length)) + throw SimulationStoppingError("Spring attachment points must have a finite, nonzero separation"); + e.unit = (1/e.length)*e.separation; + e.magnitude = stiffness*(e.length-restLength)+damping*dot(e.unit, e.relativeVelocity); + e.f = e.magnitude*e.unit; + const double extension = e.length - restLength; + const double speed = dot(e.unit, e.relativeVelocity); + e.storedEnergy = 0.5 * stiffness * extension * extension; + e.dissipatedPower = damping * speed * speed; + } + else if (torsionalSpring) { + const auto column = [](const FMatDsptr& matrix, size_t index) { + return V {matrix->at(0)->at(index), matrix->at(1)->at(index), matrix->at(2)->at(index)}; + }; + e.xI = column(frameI->aAOe, 0); + e.yI = column(frameI->aAOe, 1); + e.xJ = column(frameJ->aAOe, 0); + e.yJ = column(frameJ->aAOe, 1); + e.unit = column(frameI->aAOe, 2); + const double axisLength = std::sqrt(dot(e.unit, e.unit)); + if (!(axisLength > 0) || !std::isfinite(axisLength)) + throw SimulationStoppingError("Torsional spring axis must be finite and nonzero"); + e.unit = (1/axisLength)*e.unit; + e.twistSin = dot(e.yI, e.xJ)-dot(e.xI, e.yJ); + e.twistCos = dot(e.xI, e.xJ)+dot(e.yI, e.yJ); + const double twistProjection = e.twistSin*e.twistSin+e.twistCos*e.twistCos; + if (!(twistProjection > 1e-20) || !std::isfinite(twistProjection)) + throw SimulationStoppingError("Torsional spring twist is undefined for these attachment frames"); + // Spatial gradient of atan2(sinTwist, cosTwist). For coaxial + // attachments this is I's Z axis; with swing it is not a unit axis. + // Its work-conjugate torque preserves the defined elastic energy. + const V sinGradient = cross(e.xJ, e.yI) - cross(e.yJ, e.xI); + const V cosGradient = cross(e.xJ, e.xI) + cross(e.yJ, e.yI); + e.unit = (1 / twistProjection) + * (e.twistCos * sinGradient - e.twistSin * cosGradient); + const double angle = continuousTwist(std::atan2(e.twistSin, e.twistCos)); + e.relativeAngularVelocity = e.j.w-e.i.w; + e.length = angle; + e.magnitude = stiffness*(angle-restLength) + + damping*dot(e.unit, e.relativeAngularVelocity); + e.t = e.magnitude*e.unit; + const double deflection = angle - restLength; + const double speed = dot(e.unit, e.relativeAngularVelocity); + e.storedEnergy = 0.5 * stiffness * deflection * deflection; + e.dissipatedPower = damping * speed * speed; + } + // Transport the opposite wrench to J; the net force AND moment are zero. + if (follower && !contactEvaluator && !bushing && !axisFriction + && !spring && !torsionalSpring) { + const auto world = [&](const V& local) { + V result{}; + for (size_t row = 0; row < 3; ++row) + for (size_t col = 0; col < 3; ++col) + result[row] += frameI->aAOe->at(row)->at(col) * local[col]; + return result; + }; + e.f = world(e.f); + e.t = world(e.t); + } + e.tj = cross(e.separation, e.f)-e.t; + return e; +} + +double AppliedForceTorque::continuousTwist(double principal) const +{ + const double time = frameI->root()->time->getValue(); + double reference = restLength; + for (const auto& sample : acceptedTwists) { + if (sample.first > time) break; + reference = sample.second; + } + constexpr double twoPi = 6.2831853071795864769; + return reference + std::remainder(principal - reference, twoPi); +} + +void AppliedForceTorque::postDynFirstStep() { postDynStep(); } + +void AppliedForceTorque::setContactStepValidator(ContactStepValidator validator) +{ + contactStepValidator = std::move(validator); +} + +void AppliedForceTorque::preDynStep() +{ + if (contactStepValidator) { + previousI = frameState(frameI); + previousJ = frameState(frameJ); + previousTime = frameI->root()->time->getValue(); + } +} + +bool AppliedForceTorque::acceptDynTrial() const +{ + if (!contactStepValidator) return true; + const auto currentI = frameState(frameI), currentJ = frameState(frameJ); + const double dt = std::abs(frameI->root()->time->getValue() - previousTime); + // Endpoint orientations alone cannot distinguish a full turn from no turn. + // Keep each contact sweep within a small angular interval, including an + // accelerating trial. Rejected trials do not overwrite the starting state. + for (const auto& state : {previousI, previousJ, currentI, currentJ}) + if (dt * std::sqrt(dot(state.omega, state.omega)) > 0.25) return false; + return contactStepValidator(previousI, currentI, previousJ, currentJ); +} + +void AppliedForceTorque::postDynStep() +{ + if (!torsionalSpring) return; + const double time = frameI->root()->time->getValue(); + const double twist = evaluate().length; + while (!acceptedTwists.empty() && acceptedTwists.back().first >= time) + acceptedTwists.pop_back(); + acceptedTwists.emplace_back(time, twist); + // Output interpolation and discontinuity rollback only need the current + // accepted interval. Retain an extra predecessor for a restarted first step. + if (acceptedTwists.size() > 3) acceptedTwists.erase(acceptedTwists.begin()); +} + +double AppliedForceTorque::suggestSmallerOrAcceptDynFirstStepSize(double h) +{ + return suggestSmallerOrAcceptDynStepSize(h); +} + +double AppliedForceTorque::suggestSmallerOrAcceptDynStepSize(double h) +{ + if (!torsionalSpring) return h; + const Endpoint i(frameI), j(frameJ); + const V relative = j.w - i.w; + const double speed = std::sqrt(dot(relative, relative)); + // Sample well before a half turn so a winding cannot be skipped. + return speed > 0 ? std::min(h, 1.0 / speed) : h; +} + +AppliedForceTorque::Vector AppliedForceTorque::forceOnI() const { return evaluate().f; } +AppliedForceTorque::Vector AppliedForceTorque::torqueOnI() const { return evaluate().t; } +AppliedForceTorque::EnergyState AppliedForceTorque::energyState() const +{ + return resultState().energy; +} + +AppliedForceTorque::FrameState AppliedForceTorque::frameState( + const std::shared_ptr& frame +) +{ + const Endpoint endpoint(frame); + FrameState result; + result.time = frame->root()->time->getValue(); + result.position = endpoint.r; + result.velocity = endpoint.v; + result.omega = endpoint.w; + for (size_t row = 0; row < 3; ++row) { + for (size_t column = 0; column < 3; ++column) { + result.rotation[3 * row + column] = frame->aAOe->at(row)->at(column); + } + } + // For a time-prescribed end frame, these are deliberately the velocities + // of the underlying physical body. Including the frame's explicit motion + // would make actuator work cancel against its own virtual reference side. + return result; +} + +AppliedForceTorque::ResultState AppliedForceTorque::resultState() const +{ + const auto e = evaluate(); + const double power = dot(e.f, e.i.v) + dot(e.t, e.i.w) + - dot(e.f, e.j.v) + dot(e.tj, e.j.w); + return {e.f, e.t, {power, e.storedEnergy, e.dissipatedPower}}; +} + +void AppliedForceTorque::fillAccICIterError(FColDsptr col) { fillDynError(col); } +void AppliedForceTorque::fillDynError(FColDsptr col) +{ + const auto e = evaluate(); + for (size_t a = 0; a < 7; ++a) { + if (e.i.moving) col->at(e.i.equation[a]) += dot(e.i.J[a], e.f)+dot(e.i.W[a], e.t); + if (e.j.moving) col->at(e.j.equation[a]) += -dot(e.j.J[a], e.f)+dot(e.j.W[a], e.tj); + } +} + +void AppliedForceTorque::fillpFpy(SpMatDsptr mat) { fillJacobian(mat, false); } +void AppliedForceTorque::fillpFpydot(SpMatDsptr mat) { fillJacobian(mat, true); } +void AppliedForceTorque::fillJacobian(SpMatDsptr mat, bool velocity) const +{ + if (!enabled) return; + // CAD shape contact is an externally evaluated, nonsmooth penalty force. + // Re-evaluate it for every residual, but use a lagged tangent so Newton + // iterations do not multiply expensive collision queries by 14 numerical + // perturbations. Small integration steps provide the required stability. + if (contactEvaluator || bushing || follower) return; + const auto e = evaluate(); + for (size_t side = 0; side < 2; ++side) { + const auto& source = side == 0 ? e.i : e.j; + if (!source.moving) continue; + const double sign = side == 0 ? -1 : 1; + for (size_t b = 0; b < 7; ++b) { + const V dD = velocity ? V{} : sign*source.J[b]; + const V dV = sign*(velocity ? source.J[b] : source.dv[b]); + V dF{}, dT{}; + if (spring) { + const double dl = dot(e.unit, dD); + const V du = (1/e.length)*(dD-dl*e.unit); + dF = (stiffness*dl+damping*(dot(du, e.relativeVelocity)+dot(e.unit, dV)))*e.unit + + e.magnitude*du; + } + else if (torsionalSpring) { + V dAxis{}; + double dAngle = 0; + double dOmega = 0; + if (velocity) { + dOmega = sign*dot(e.unit, source.W[b]); + } + else { + V dxI{}, dyI{}, dxJ{}, dyJ{}; + if (side == 0) { + dxI = cross(source.W[b], e.xI); + dyI = cross(source.W[b], e.yI); + } + else { + dxJ = cross(source.W[b], e.xJ); + dyJ = cross(source.W[b], e.yJ); + } + const double dSin = dot(dyI, e.xJ)+dot(e.yI, dxJ) + -dot(dxI, e.yJ)-dot(e.xI, dyJ); + const double dCos = dot(dxI, e.xJ)+dot(e.xI, dxJ) + +dot(dyI, e.yJ)+dot(e.yI, dyJ); + dAngle = (e.twistCos*dSin-e.twistSin*dCos) + /(e.twistSin*e.twistSin+e.twistCos*e.twistCos); + dOmega = sign*dot(e.unit, source.dw[b]); + const V sinGradient = cross(e.xJ, e.yI) - cross(e.yJ, e.xI); + const V cosGradient = cross(e.xJ, e.xI) + cross(e.yJ, e.yI); + const V dSinGradient = cross(dxJ, e.yI) + cross(e.xJ, dyI) + - cross(dyJ, e.xI) - cross(e.yJ, dxI); + const V dCosGradient = cross(dxJ, e.xI) + cross(e.xJ, dxI) + + cross(dyJ, e.yI) + cross(e.yJ, dyI); + const double projection = e.twistSin*e.twistSin + e.twistCos*e.twistCos; + dAxis = (1 / projection) * (dCos*sinGradient + e.twistCos*dSinGradient + - dSin*cosGradient - e.twistSin*dCosGradient + - (2*(e.twistSin*dSin + e.twistCos*dCos))*e.unit); + dOmega += dot(dAxis, e.relativeAngularVelocity); + } + const double dMagnitude = stiffness*dAngle+damping*dOmega; + dT = dMagnitude*e.unit+e.magnitude*dAxis; + } + else if (axisFriction) { + // The velocity tangent is the part that controls convergence + // through the sharp zero-speed transition. Keep the positional + // orientation dependence lagged; quaternion curvature is still + // included below in the generalized-force transformation. + if (velocity) { + const V dRelative = sign * ( + axisFrictionParameters.rotational ? source.W[b] : source.J[b] + ); + const double dSpeed = dot(e.unit, dRelative); + const double transition = axisFrictionParameters.transitionVelocity; + const double ratio = e.length / transition; + const double exponential = std::exp(-(ratio * ratio)); + const double tangent = std::tanh(ratio); + const double dryMagnitude = e.frictionDynamicMagnitude + + (e.frictionStaticMagnitude - e.frictionDynamicMagnitude) * exponential; + const double drySlope + = (e.frictionStaticMagnitude - e.frictionDynamicMagnitude) + * exponential * (-2 * ratio / transition); + const double slope = drySlope * tangent + + dryMagnitude * (1 - tangent*tangent) / transition + + axisFrictionParameters.viscousCoefficient; + if (axisFrictionParameters.rotational) + dT = slope*dSpeed*e.unit; + else + dF = slope*dSpeed*e.unit; + } + } + const V dTj = cross(dD, e.f)+cross(e.separation, dF); + for (size_t a = 0; a < 7; ++a) { + double qi = dot(e.i.J[a], dF)+dot(e.i.W[a], dT); + double qj = -dot(e.j.J[a], dF)+dot(e.j.W[a], dTj-dT); + if (!velocity && side == 0) + qi += dot(e.i.H[a][b], e.f)+dot(e.i.dW[a][b], e.t); + if (!velocity && side == 1) + qj += -dot(e.j.H[a][b], e.f)+dot(e.j.dW[a][b], e.tj); + if (e.i.moving && qi != 0) mat->atijplusNumber(e.i.equation[a], source.equation[b], qi); + if (e.j.moving && qj != 0) mat->atijplusNumber(e.j.equation[a], source.equation[b], qj); + } + } + } + // Dynamic multipliers are stored in qdot, so this coupling belongs in the + // velocity Jacobian even though it represents a reaction-force dependency. + if (velocity && axisFriction && axisFrictionParameters.reactionBased) { + const auto joint = axisFrictionParameters.reactionJoint.lock(); + const auto reaction = vector(joint->aFX()); + const auto transverse = axisFrictionParameters.axialReaction + ? dot(reaction, e.unit) * e.unit + : reaction - dot(reaction, e.unit) * e.unit; + const double normalLoad = std::sqrt(dot(transverse, transverse)); + if (normalLoad > 1e-14) { + const double ratio = e.length / axisFrictionParameters.transitionVelocity; + const double exponential = std::exp(-(ratio * ratio)); + const double coefficient = axisFrictionParameters.dynamicMagnitude + + (axisFrictionParameters.staticMagnitude + - axisFrictionParameters.dynamicMagnitude) * exponential; + const double speedSign = std::tanh(ratio); + const double radius = axisFrictionParameters.rotational + ? axisFrictionParameters.effectiveRadius + : 1; + joint->constraintsDo([&](const std::shared_ptr& constraint) { + if (constraint->iG == SIZE_MAX || std::abs(constraint->lam) < 1e-14) { + return; + } + auto contribution = std::make_shared>(3, 0.0); + constraint->addToJointForceI(contribution); + const V dReaction = (1 / constraint->lam) * vector(contribution); + const V dTransverse = axisFrictionParameters.axialReaction + ? dot(dReaction, e.unit) * e.unit + : dReaction - dot(dReaction, e.unit) * e.unit; + const double dNormal = dot(transverse, dTransverse) / normalLoad; + const double dMagnitude = radius * coefficient * speedSign * dNormal; + const V dF = axisFrictionParameters.rotational ? V{} : dMagnitude * e.unit; + const V dT = axisFrictionParameters.rotational ? dMagnitude * e.unit : V{}; + const V dTj = cross(e.separation, dF) - dT; + for (size_t a = 0; a < 7; ++a) { + const double qi = dot(e.i.J[a], dF) + dot(e.i.W[a], dT); + const double qj = -dot(e.j.J[a], dF) + dot(e.j.W[a], dTj); + if (e.i.moving && qi != 0) { + mat->atijplusNumber(e.i.equation[a], constraint->iG, qi); + } + if (e.j.moving && qj != 0) { + mat->atijplusNumber(e.j.equation[a], constraint->iG, qj); + } + } + }); + } + } +} diff --git a/OndselSolver/AppliedForceTorque.h b/OndselSolver/AppliedForceTorque.h new file mode 100644 index 00000000..8c04d583 --- /dev/null +++ b/OndselSolver/AppliedForceTorque.h @@ -0,0 +1,138 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +#pragma once + +#include +#include +#include +#include +#include "ForceTorqueItem.h" +#include "EndFramec.h" + +namespace MbD { +class Symbolic; +class Joint; +// A world-resolved wrench on I, with the balancing wrench on J. Linear springs +// act between attachment points; torsional springs act about attachment I's Z axis. +class AppliedForceTorque : public ForceTorqueItem +{ +public: + using Vector = std::array; + using Matrix = std::array; + struct FrameState { + Vector position{}; + Matrix rotation{}; + Vector velocity{}; + Vector omega{}; + double time = 0; + }; + struct ContactWrench { + Vector forceOnI{}; + Vector torqueOnI{}; + double storedEnergy = 0; + double dissipatedPower = 0; + }; + struct EnergyState { + // Positive power adds mechanical energy to the connected bodies. + double power = 0; + double storedEnergy = 0; + // Positive dissipated power removes mechanical energy. + double dissipatedPower = 0; + }; + struct ResultState { + Vector forceOnI{}; + Vector torqueOnI{}; + EnergyState energy; + }; + struct BushingParameters { + Vector linearStiffness{}; + Vector linearDamping{}; + Vector angularStiffness{}; + Vector angularDamping{}; + // Row-major, work-conjugate [x,y,z,rx,ry,rz] matrices in marker I. + // Rotations are rotation-vector strains in radians, not Euler angles. + std::array stiffnessMatrix{}; + std::array dampingMatrix{}; + bool coupled = false; + }; + static void validateBushingMatrix(const std::array& matrix); + struct AxisFrictionParameters { + double staticMagnitude = 0; + double dynamicMagnitude = 0; + double transitionVelocity = 0; + double viscousCoefficient = 0; + bool rotational = false; + // When enabled, the two magnitudes are friction coefficients. The + // normal load is the joint reaction perpendicular to the free axis. + bool reactionBased = false; + double effectiveRadius = 1; + std::weak_ptr reactionJoint; + bool axialReaction = false; // Thrust bearing; otherwise radial bearing load. + }; + using ContactEvaluator = std::function; + using ContactStepValidator = std::function; + void setContactStepValidator(ContactStepValidator validator); + void setFollower(bool value) { follower = value; } + void setEnabled(bool value) { enabled = value; } + void setRuntimeFormula(Vector direction, std::shared_ptr magnitude, bool isTorque) { + formulaDirection = direction; + formulaMagnitude = std::move(magnitude); + formulaTorque = isTorque; + } + void preDynStep() override; + bool acceptDynTrial() const override; + AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, Vector force, Vector torque); + AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, double stiffness, + double damping, double restLength); + AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, double stiffness, + double damping, double restAngle, bool torsional); + AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, + Vector direction, std::shared_ptr magnitude, bool torque); + AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, + ContactEvaluator evaluator); + AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, + BushingParameters parameters); + AppliedForceTorque(std::shared_ptr i, std::shared_ptr j, + AxisFrictionParameters parameters); + void fillAccICIterError(FColDsptr col) override; + void fillDynError(FColDsptr col) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + Vector forceOnI() const; + Vector torqueOnI() const; + EnergyState energyState() const; + ResultState resultState() const; + void postDynFirstStep() override; + void postDynStep() override; + double suggestSmallerOrAcceptDynFirstStepSize(double h) override; + double suggestSmallerOrAcceptDynStepSize(double h) override; + static FrameState frameState(const std::shared_ptr& frame); + +private: + struct Evaluation; + Evaluation evaluate() const; + void fillJacobian(SpMatDsptr mat, bool velocity) const; + std::shared_ptr frameI, frameJ; + Vector force{}, torque{}; + Vector formulaDirection{}; + std::shared_ptr formulaMagnitude; + ContactEvaluator contactEvaluator; + ContactStepValidator contactStepValidator; + FrameState previousI, previousJ; + double previousTime = 0; + bool formulaTorque = false; + bool follower = false; + bool enabled = true; + bool spring = false; + bool torsionalSpring = false; + bool bushing = false; + bool axisFriction = false; + double stiffness = 0, damping = 0, restLength = 0; + BushingParameters bushingParameters; + AxisFrictionParameters axisFrictionParameters; + // Only accepted states are retained. Residual/Jacobian/output evaluation + // never changes winding, including when a corrector retries a step. + std::vector> acceptedTwists; + double continuousTwist(double principal) const; +}; +} diff --git a/OndselSolver/AtPointConstraintIJ.cpp b/OndselSolver/AtPointConstraintIJ.cpp index 6fec67ae..d1ecccbe 100644 --- a/OndselSolver/AtPointConstraintIJ.cpp +++ b/OndselSolver/AtPointConstraintIJ.cpp @@ -77,3 +77,27 @@ void AtPointConstraintIJ::preAccIC() riIeJeO->preAccIC(); Constraint::preAccIC(); } + +void AtPointConstraintIJ::postDynPredictor() +{ + riIeJeO->postDynPredictor(); + ConstraintIJ::postDynPredictor(); +} + +void AtPointConstraintIJ::postDynCorrectorIteration() +{ + riIeJeO->postDynCorrectorIteration(); + ConstraintIJ::postDynCorrectorIteration(); +} + +void AtPointConstraintIJ::preDynOutput() +{ + riIeJeO->preDynOutput(); + ConstraintIJ::preDynOutput(); +} + +void AtPointConstraintIJ::postDynOutput() +{ + riIeJeO->postDynOutput(); + ConstraintIJ::postDynOutput(); +} diff --git a/OndselSolver/AtPointConstraintIJ.h b/OndselSolver/AtPointConstraintIJ.h index 4d4918a4..d0059d84 100644 --- a/OndselSolver/AtPointConstraintIJ.h +++ b/OndselSolver/AtPointConstraintIJ.h @@ -17,6 +17,10 @@ namespace MbD { { //axis riIeJeO public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; AtPointConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj, size_t axisi); void calcPostDynCorrectorIteration() override; diff --git a/OndselSolver/AtPointConstraintIqcJc.cpp b/OndselSolver/AtPointConstraintIqcJc.cpp index 03cfc469..1f512821 100644 --- a/OndselSolver/AtPointConstraintIqcJc.cpp +++ b/OndselSolver/AtPointConstraintIqcJc.cpp @@ -106,3 +106,16 @@ void AtPointConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) auto c2Torque = aBOIp->timesFullColumn(lampGpE->minusFullColumn(fpAOIppEIrIpIeIp)); jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5); } + +void AtPointConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusNumber(iG, iqXIminusOnePlusAxis, -1.0); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void AtPointConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusNumber(iqXIminusOnePlusAxis, iG, -1.0); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} diff --git a/OndselSolver/AtPointConstraintIqcJc.h b/OndselSolver/AtPointConstraintIqcJc.h index fc02f187..f0107b3f 100644 --- a/OndselSolver/AtPointConstraintIqcJc.h +++ b/OndselSolver/AtPointConstraintIqcJc.h @@ -17,6 +17,8 @@ namespace MbD { { //pGpEI ppGpEIpEI iqXIminusOnePlusAxis iqEI public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; AtPointConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj, size_t axisi); void addToJointForceI(FColDsptr col) override; diff --git a/OndselSolver/AtPointConstraintIqcJqc.cpp b/OndselSolver/AtPointConstraintIqcJqc.cpp index e97a42fa..bf1bdb10 100644 --- a/OndselSolver/AtPointConstraintIqcJqc.cpp +++ b/OndselSolver/AtPointConstraintIqcJqc.cpp @@ -93,3 +93,18 @@ void AtPointConstraintIqcJqc::fillAccICIterError(FColDsptr col) sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); col->atiplusNumber(iG, sum); } + +void AtPointConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + AtPointConstraintIqcJc::fillpFpy(mat); + mat->atijplusNumber(iG, iqXJminusOnePlusAxis, 1.0); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void AtPointConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + AtPointConstraintIqcJc::fillpFpydot(mat); + mat->atijplusNumber(iqXJminusOnePlusAxis, iG, 1.0); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} diff --git a/OndselSolver/AtPointConstraintIqcJqc.h b/OndselSolver/AtPointConstraintIqcJqc.h index 2d20f3ab..c61ca579 100644 --- a/OndselSolver/AtPointConstraintIqcJqc.h +++ b/OndselSolver/AtPointConstraintIqcJqc.h @@ -17,6 +17,8 @@ namespace MbD { { //pGpEJ ppGpEJpEJ iqXJminusOnePlusAxis iqEJ public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; AtPointConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj, size_t axisi); void calcPostDynCorrectorIteration() override; diff --git a/OndselSolver/BasicDAEIntegrator.cpp b/OndselSolver/BasicDAEIntegrator.cpp new file mode 100644 index 00000000..5d2a29ac --- /dev/null +++ b/OndselSolver/BasicDAEIntegrator.cpp @@ -0,0 +1,546 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include +#include + +#include "BasicDAEIntegrator.h" +#include "IntegratorInterface.h" +#include "LinearMultiStepMethod.h" +#include "DynIntegrator.h" +#include "DAECorrector.h" +#include "MaximumIterationError.h" +#include "SingularMatrixError.h" +#include "TooManyTriesError.h" +#include "SimulationStoppingError.h" + +using namespace MbD; + +std::shared_ptr BasicDAEIntegrator::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void BasicDAEIntegrator::initialize() +{ + BasicIntegrator::initialize(); + extrapolator = Extrapolator::With(); + extrapolator->timeNodes = tpast; + newtonRaphson = DAECorrector::With(); + newtonRaphson->setSystem(this); + ypast = std::make_shared>(); + ydotpast = std::make_shared>(); +} + +void BasicDAEIntegrator::initializeGlobally() +{ + //"Get info from system and prepare for start of simulation." + //"Integrator asks system for info. Not system setting integrator." + + BasicIntegrator::initializeGlobally(); + auto daeSystem = static_cast(system); + integRelTol = daeSystem->integrationRelativeTolerance(); + integAbsTol = daeSystem->integrationAbsoluteTolerance(); + corRelTol = daeSystem->correctorRelativeTolerance(); + corAbsTol = daeSystem->correctorAbsoluteTolerance(); + auto size = daeSystem->neqn; + y = std::make_shared>(size); + daeSystem->fillY(y); + ydot = std::make_shared>(size); + daeSystem->fillYdot(ydot); + aF = std::make_shared>(size); + pFpy = std::make_shared>(size, size); + pFpydot = std::make_shared>(size, size); +} + +void BasicDAEIntegrator::firstStep() +{ + istep = 0; + firstStepGrowthAllowed = true; + preFirstStep(); + iTry = 1; + orderNew = 1; + selectFirstStepSize(); + incrementTime(); + predictFirstStep(); + correctFirstStep(); + reportTrialStepStats(); + while (isRedoingFirstStep()) { + incrementTry(); + orderNew = 1; + selectFirstStepSize(); + changeTime(); + predictFirstStep(); + correctFirstStep(); + reportTrialStepStats(); + } + postFirstStep(); + reportStepStats(); +} + +bool BasicDAEIntegrator::isRedoingFirstStep() +{ + if (iTry > 100) throw TooManyTriesError(""); + if (!corOK || truncError > 1.0) { + firstStepGrowthAllowed = false; + return true; + } + //"Look at the next step size." + //"Aim for first step size and second step size to be similar in size." + //"Huge disparity causes poor accuracy for subsequent steps." + hnew = selectBasicStepSize(); + hnew = system->suggestSmallerOrAcceptFirstStepSize(hnew); + // Once growth has failed, accept a converged, tolerance-satisfying step. + // Smooth loads starting from rest can otherwise alternate indefinitely + // between an almost-zero-error small step and an excessive enlarged step. + if (firstStepGrowthAllowed && (hnew > (4.0 * h)) && (hnew < system->hmax)) return true; + return false; +} + +void BasicDAEIntegrator::nextStep() +{ + preStep(); + iTry = 1; + selectOrder(); + selectStepSize(); + incrementTime(); + predict(); + correct(); + reportTrialStepStats(); + while (isRedoingStep()) { + incrementTry(); + selectOrder(); + selectStepSize(); + changeTime(); + predict(); + correct(); + reportTrialStepStats(); + } + postStep(); + reportStepStats(); +} + +void BasicDAEIntegrator::reportStepStats() +{ + system->useDAEStepStats(statistics); +} + +void BasicDAEIntegrator::reportTrialStepStats() +{ + statistics->istep = istep; + statistics->t = t; + statistics->h = direction * h; + statistics->order = order; + statistics->truncError = truncError; + system->useTrialStepStats(statistics); +} + +void BasicDAEIntegrator::runInitialConditionTypeSolution() +{ + throw SimulationStoppingError("To be implemented."); +} + +void BasicDAEIntegrator::selectFirstStepSize() +{ + if (iTry == 1) { + auto hout1000 = system->hout / 1000.0; + auto ydotNorm = std::max(integErrorNormFromwrt(ydot, y), 1.0e-15); + auto hydot = 0.5 / ydotNorm; + hnew = std::min(hout1000, hydot); + hnew = std::max(hnew, 1.0e-6); + } + else { + if (corOK) { + hnew = selectBasicStepSize(); + } + else { + hnew = 0.25 * h; + } + } + hnew = system->suggestSmallerOrAcceptFirstStepSize(hnew); +} + +void BasicDAEIntegrator::selectStepSize() +{ + if (corOK) { + selectStepSizeNormal(); + } + else { + hnew = 0.25 * h; + } + hnew = system->suggestSmallerOrAcceptStepSize(hnew); +} + +void BasicDAEIntegrator::predictFirstStep() +{ + auto dynInt = static_cast(system); + dynInt->preDAEPredictor(); + predictValuesAtFirstStep(); + dynInt->y(y); + dynInt->ydot(ydot); + dynInt->postDAEPredictor(); +} + +void BasicDAEIntegrator::correctFirstStep() +{ + auto dynInt = static_cast(system); + try { + try { + dynInt->preDAECorrector(); + correctValuesAtFirstStep(); + dynInt->postDAECorrector(); + if (corOK && !dynInt->acceptTrial()) corOK = false; + } + catch (SingularMatrixError ex) { + //"Step size is probably too small, causing an ill conditioned matrix." + //"Increase step size. Multiply by 4.0d to offset reduction in selectFirstStepSize." + corOK = false; + truncError = 999999.0; + auto hout1000 = dynInt->hout / 1000.0; + auto ydotNorm = std::max(integErrorNormFromwrt(ydot, y), 1.0e-15); + auto hydot = 0.5 / ydotNorm; + h = 4.0 * std::max(hout1000, hydot); + return; + } + } + catch (MaximumIterationError ex) { + corOK = false; + truncError = 888888.0; + return; + } +} + +void BasicDAEIntegrator::changeTime() +{ + setorder(orderNew); + h = hnew; + settnew(tpast->at(0) + (direction * h)); + system->changeTime(tnew); + calcOperatorMatrix(); +} + +double BasicDAEIntegrator::selectBasicStepSize() +{ + //"Shampine's book pp 337-8." + //"Brenan's book pp 128." + //"Using first term of Taylor series remainder." + + double conservativeFactor, coeff, yndotNorm, dum, hdum; + FColDsptr yndot; + conservativeFactor = 0.5; + if (orderNew < order) { + coeff = DifferenceOperator::OneOverFactorials->at(order); + yndot = yDeriv(order); + yndotNorm = std::max(integErrorNormFromwrt(yndot, y), 1.0e-15); + dum = conservativeFactor / (coeff * yndotNorm); + hdum = std::pow(dum, 1.0 / order); + } + else { + coeff = DifferenceOperator::OneOverFactorials->at(order + 1); + yndot = dyOrderPlusOnedt(); + yndotNorm = std::max(integErrorNormFromwrt(yndot, y), 1.0e-15); + dum = conservativeFactor / (coeff * yndotNorm); + hdum = std::pow(dum, 1.0 / (order + 1)); + } + return hdum; +} + +double BasicDAEIntegrator::integErrorNormFromwrt(FColDsptr err, FColDsptr ref) +{ + return rmswrtrelativeTolabsoluteTol(err, ref, integRelTol, integAbsTol); +} + +double BasicDAEIntegrator::rmswrtrelativeTolabsoluteTol(FColDsptr vector, FColDsptr baseVector, FColDsptr relativeTol, FColDsptr absoluteTol) +{ + //"Answer a weighted rms norm." + //"Elements with relativeTol == nil are not included in norm." + //"For m significant digits set relToli = 1.0e-m." + //"Set absToli to value where vectori abs is insignificant." + + auto n = baseVector->size(); + auto count = 0; + auto sumOfSquares = 0.0; + for (size_t i = 0; i < n; i++) + { + auto relToli = relativeTol->at(i); + if (relToli != std::numeric_limits::min()) { + count++; + auto weighti = relToli * std::abs(baseVector->at(i)) + absoluteTol->at(i); + auto vectori = vector->at(i); + auto vectoriOverWeighti = vectori / weighti; + sumOfSquares += (vectoriOverWeighti * vectoriOverWeighti); + } + + } + return std::sqrt(sumOfSquares / count); +} + +void BasicDAEIntegrator::predictValuesAtFirstStep() +{ + y = ypast->at(0)->plusFullColumn(ydotpast->at(0)->times(direction * h)); + ydot = ydotpast->at(0)->copy(); +} + +void BasicDAEIntegrator::predictValuesAtNextStep() +{ + //"ydot := extrapolator valueWith: ydotpast." + //"Poor prediction. Extra iteration of the corrector is needed." + + y = extrapolator->valueWith(ypast); + ydot = yDeriv(1); +} + +void BasicDAEIntegrator::incrementTime() +{ + BasicIntegrator::incrementTime(); + ypast->insert(ypast->begin(), y); + ydotpast->insert(ydotpast->begin(), ydot); + if (ypast->size() > (orderMax + 1)) { + ypast->pop_back(); + ydotpast->pop_back(); + } +} + +void BasicDAEIntegrator::correctValuesAtFirstStep() const +{ + newtonRaphson->run(); +} + +void BasicDAEIntegrator::correctValuesAtNextStep() const +{ + newtonRaphson->run(); +} + +void BasicDAEIntegrator::preDAECorrector() +{ + corOK = false; + truncError = -1.0; +} + +void BasicDAEIntegrator::postDAECorrector() +{ + corOK = true; + calcTruncError(); +} + +FColDsptr BasicDAEIntegrator::fillF() +{ + aF->zeroSelf(); + system->fillF(aF); + return aF; +} + +size_t BasicDAEIntegrator::iterMax() +{ + return system->iterMax(); +} + +SpMatDsptr BasicDAEIntegrator::calcG() +{ + //"It is ok to modify pFpydot since its values are not reused." + pFpy->zeroSelf(); + system->fillpFpy(pFpy); + pFpydot->zeroSelf(); + system->fillpFpydot(pFpydot); + alp = correctorBDF()->pvdotpv(); + pFpydot->magnifySelf(alp); + matG = pFpy->plusSparseMatrix(pFpydot); + return matG; +} + +std::shared_ptr BasicDAEIntegrator::correctorBDF() +{ + throw SimulationStoppingError("To be implemented."); + return std::shared_ptr(); +} + +void BasicDAEIntegrator::calcOperatorMatrix() +{ + BasicIntegrator::calcOperatorMatrix(); + extrapolator->calcOperatorMatrix(); +} + +void BasicDAEIntegrator::setorder(size_t o) +{ + BasicIntegrator::setorder(o); + extrapolator->setorder(o - 1); +} + +void BasicDAEIntegrator::settime(double t) +{ + BasicIntegrator::settime(t); + extrapolator->settime(t); +} + +void BasicDAEIntegrator::iStep(size_t i) +{ + BasicIntegrator::iStep(i); + extrapolator->setiStep(i); +} + +double BasicDAEIntegrator::corErrorNormFromwrt(FColDsptr error, FColDsptr ref) +{ + return rmswrtrelativeTolabsoluteTol(error, ref, corRelTol, corAbsTol); +} + +void BasicDAEIntegrator::updateForDAECorrector() +{ + ydot = yDeriv(1); + system->y(y); + system->ydot(ydot); + system->updateForDAECorrector(); +} + +FColDsptr BasicDAEIntegrator::yDeriv(size_t order) +{ + throw SimulationStoppingError("To be implemented."); + return FColDsptr(); +} + +void BasicDAEIntegrator::calcTruncError() +{ + //"Calculate the leading term of truncation error in Taylor series." + + auto factor = DifferenceOperator::OneOverFactorials->at(order + 1); + auto yndot = dyOrderPlusOnedt(); + auto yndotNorm = integErrorNormFromwrt(yndot, y); + auto hpower = std::pow(h, order + 1); + truncError = factor * yndotNorm * hpower; +} + +FColDsptr BasicDAEIntegrator::dyOrderPlusOnedt() +{ + throw SimulationStoppingError("To be implemented."); + return FColDsptr(); +} + +bool BasicDAEIntegrator::isConvergedForand(size_t iterNo, std::shared_ptr> dyNorms) const +{ + auto dyNormIterNo = dyNorms->at(iterNo); + auto smallEnoughTol = 4 * std::numeric_limits::epsilon() / corAbsTol->at(0); + auto smallEnough = dyNormIterNo < smallEnoughTol; + if (iterNo == 0) return smallEnough; + auto rho = dyNormIterNo / dyNorms->at(iterNo - 1); + return smallEnough || (dyNormIterNo < 1.0 && rho < 1.0 && (rho * dyNormIterNo / (1.0 - rho) < 0.33)); +} + +void BasicDAEIntegrator::postFirstStep() +{ + t = tnew; + static_cast(system)->postDAEFirstStep(); +} + +void BasicDAEIntegrator::postStep() +{ + t = tnew; + auto daeSystem = static_cast(system); + daeSystem->postDAEStep(); +} + +void BasicDAEIntegrator::predict() +{ + auto daeSystem = static_cast(system); + daeSystem->preDAEPredictor(); + predictValuesAtNextStep(); + daeSystem->y(y); + daeSystem->ydot(ydot); + daeSystem->postDAEPredictor(); +} + +void BasicDAEIntegrator::correct() +{ + auto dynInt = static_cast(system); + try { + try { + dynInt->preDAECorrector(); + correctValuesAtNextStep(); + dynInt->postDAECorrector(); + if (corOK && !dynInt->acceptTrial()) corOK = false; + } + catch (SingularMatrixError ex) { + newtonRaphson->matrixSolver->throwSingularMatrixError(""); + } + } + catch (MaximumIterationError ex) { + corOK = false; + truncError = 888888.0; + return; + } +} + +bool BasicDAEIntegrator::isRedoingStep() const +{ + if (iTry > 100) { + throw TooManyTriesError(""); + } + return !corOK || (truncError > 1.0); +} + +void BasicDAEIntegrator::selectStepSizeNormal() +{ + //"Shampine's book pp 337-8." + //"Brenan's book pp 128." + //"Using first term of Taylor series remainder." + + double hdum, twoH, pt9H, pt5H, pt25H; + hdum = selectBasicStepSize(); + if (iTry == 1) { + if (hdum >= h) { + twoH = 2.0 * h; + if (hdum > twoH) { + hnew = twoH; + } + else { + hnew = h; + } + } + else { + pt9H = 0.9 * h; + if (hdum > pt9H) { + hnew = pt9H; + } + else { + pt5H = 0.5 * h; + if (hdum < pt5H) { + hnew = pt5H; + } + else { + hnew = hdum; + } + } + } + } + else { + if (iTry == 2) { + pt9H = 0.9 * h; + hdum = 0.9 * hdum; + if (hdum > pt9H) { + hnew = pt9H; + } + else { + pt25H = 0.25 * h; + if (hdum < pt25H) { + hnew = pt25H; + } + else { + hnew = hdum; + } + } + } + else { + hnew = 0.25 * h; + } + } +} + +void BasicDAEIntegrator::useDAECorrectorStats(std::shared_ptr stats) const +{ + statistics->corIterNo = stats->iterNo; +} diff --git a/OndselSolver/BasicDAEIntegrator.h b/OndselSolver/BasicDAEIntegrator.h new file mode 100644 index 00000000..a61eab7c --- /dev/null +++ b/OndselSolver/BasicDAEIntegrator.h @@ -0,0 +1,78 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "BasicIntegrator.h" +#include "FullColumn.h" +#include "SparseMatrix.h" +#include "Extrapolator.h" +#include "DAECorrector.h" + +namespace MbD { + class BasicDAEIntegrator : public BasicIntegrator + { + //y ydot dy ypast ydotpast aF pFpy pFpydot alp aG extrapolator newtonRaphson corAbsTol corRelTol corOK integAbsTol integRelTol truncError + public: + static std::shared_ptr With(); + void initialize() override; + + void initializeGlobally() override; + void firstStep() override; + bool isRedoingFirstStep(); + bool isRedoingStep() const; + void nextStep() override; + void reportStepStats(); + void reportTrialStepStats(); + void runInitialConditionTypeSolution() override; + void selectFirstStepSize(); + void selectStepSize() override; + void predictFirstStep(); + void correctFirstStep(); + void changeTime(); + double selectBasicStepSize(); + double integErrorNormFromwrt(FColDsptr err, FColDsptr ref); + double rmswrtrelativeTolabsoluteTol(FColDsptr vector, FColDsptr baseVector, FColDsptr relativeTol, FColDsptr absoluteTol); + void predictValuesAtFirstStep(); + void predictValuesAtNextStep(); + void incrementTime() override; + void correctValuesAtFirstStep() const; + void correctValuesAtNextStep() const; + void preDAECorrector(); + void postDAECorrector(); + FColDsptr fillF(); + size_t iterMax() override; + SpMatDsptr calcG(); + virtual std::shared_ptr correctorBDF(); + void calcOperatorMatrix() override; + void setorder(size_t o) override; + void settime(double t) override; + void iStep(size_t i) override; + double corErrorNormFromwrt(FColDsptr error, FColDsptr ref); + void updateForDAECorrector(); + virtual FColDsptr yDeriv(size_t order); + void calcTruncError(); + virtual FColDsptr dyOrderPlusOnedt(); + bool isConvergedForand(size_t iterNo, std::shared_ptr> dyNorms) const; + void postFirstStep(); + void postStep(); + void predict(); + void correct(); + void selectStepSizeNormal(); + void useDAECorrectorStats(std::shared_ptr stats) const; + + FColDsptr y, ydot, dy, aF, corAbsTol, corRelTol, integAbsTol, integRelTol; + std::shared_ptr> ypast, ydotpast; + SpMatDsptr pFpy, pFpydot, matG; + double alp = 0.0, truncError = 0.0; + bool corOK = false; + bool firstStepGrowthAllowed = true; + std::shared_ptr extrapolator; + std::shared_ptr newtonRaphson; + }; +} diff --git a/OndselSolver/BasicIntegrator.cpp b/OndselSolver/BasicIntegrator.cpp index e7f9b1b6..33771535 100644 --- a/OndselSolver/BasicIntegrator.cpp +++ b/OndselSolver/BasicIntegrator.cpp @@ -7,157 +7,175 @@ ***************************************************************************/ #include "BasicIntegrator.h" -#include "CREATE.h" #include "StableBackwardDifference.h" #include "IntegratorInterface.h" +#include "SimulationStoppingError.h" + +#include "CREATE.h" using namespace MbD; +std::shared_ptr BasicIntegrator::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void BasicIntegrator::initialize() +{ + Solver::initialize(); + //statistics = IdentityDictionary new. + tpast = std::make_shared>(); + opBDF = CREATE::With(); + opBDF->timeNodes = tpast; +} + void BasicIntegrator::initializeLocally() { - _continue = true; + _continue = true; } void BasicIntegrator::iStep(size_t integer) { - istep = integer; - opBDF->setiStep(integer); + istep = integer; + opBDF->setiStep(integer); } void BasicIntegrator::postFirstStep() { - t = tnew; - system->postFirstStep(); + t = tnew; + system->postFirstStep(); } void BasicIntegrator::postRun() { + //Do nothing. } void BasicIntegrator::postStep() { - t = tnew; - system->postStep(); + t = tnew; + system->postStep(); } void BasicIntegrator::initializeGlobally() { - //"Get info from system and prepare for start of simulation." - //"Integrator asks system for info. Not system setting integrator." + //"Get info from system and prepare for start of simulation." + //"Integrator asks system for info. Not system setting integrator." - this->sett(system->tstart); - this->direction = system->direction; - this->orderMax = system->orderMax(); + sett(system->tstart); + direction = system->direction; + orderMax = system->orderMax(); } void BasicIntegrator::setSystem(Solver* sys) { - system = static_cast(sys); + system = static_cast(sys); } void BasicIntegrator::calcOperatorMatrix() { - opBDF->calcOperatorMatrix(); + opBDF->calcOperatorMatrix(); } void BasicIntegrator::incrementTime() { - tpast->insert(tpast->begin(), t); + tpast->insert(tpast->begin(), t); - if (tpast->size() > (orderMax + 1)) { tpast->pop_back(); } - auto istepNew = istep + 1; - this->iStep(istepNew); - this->setorder(orderNew); - h = hnew; - this->settnew(t + (direction * h)); - this->calcOperatorMatrix(); - system->incrementTime(tnew); + if (tpast->size() > (orderMax + 1)) { tpast->pop_back(); } + auto istepNew = istep + 1; + iStep(istepNew); + setorder(orderNew); + h = hnew; + settnew(t + (direction * h)); + calcOperatorMatrix(); + system->incrementTime(tnew); } void BasicIntegrator::incrementTry() { - throw SimulationStoppingError("To be implemented."); -} - -void BasicIntegrator::initialize() -{ - Solver::initialize(); - //statistics = IdentityDictionary new. - tpast = std::make_shared>(); - opBDF = CREATE::With(); - opBDF->timeNodes = tpast; + iTry++; } void BasicIntegrator::logString(const std::string& str) { - system->logString(str); + system->logString(str); } void BasicIntegrator::run() { - this->preRun(); - this->initializeLocally(); - this->initializeGlobally(); - this->firstStep(); - this->subsequentSteps(); - this->finalize(); - this->reportStats(); - this->postRun(); + preRun(); + initializeLocally(); + initializeGlobally(); + firstStep(); + subsequentSteps(); + finalize(); + reportStats(); + postRun(); } void BasicIntegrator::selectOrder() { - //"Increase order consecutively with step." - if (iTry == 1) orderNew = std::min(istep + 1, orderMax); + //"Increase order consecutively with step." + if (iTry == 1) orderNew = std::min(istep + 1, orderMax); } void BasicIntegrator::preFirstStep() { - system->preFirstStep(); + system->preFirstStep(); } void BasicIntegrator::preRun() { + //Do nothing. } void BasicIntegrator::preStep() { - system->preStep(); + system->preStep(); } void BasicIntegrator::reportStats() { + //Do nothing. } void BasicIntegrator::setorder(size_t o) { - order = o; - opBDF->setorder(o); + order = o; + opBDF->setorder(o); } void BasicIntegrator::settnew(double t) { - tnew = t; - this->settime(t); + tnew = t; + settime(t); } void BasicIntegrator::sett(double tt) { - t = tt; - opBDF->settime(tt); + t = tt; + opBDF->settime(tt); } void BasicIntegrator::settime(double tt) { - opBDF->settime(tt); + opBDF->settime(tt); +} + +double BasicIntegrator::tprevious() const +{ + return tpast->at(0); } -double BasicIntegrator::tprevious() +FColDsptr BasicIntegrator::yDerivat(size_t n, double time) { - return tpast->at(0); + throw SimulationStoppingError("To be implemented."); + return FColDsptr(); } void BasicIntegrator::subsequentSteps() { - while (_continue) { this->nextStep(); } + while (_continue) { nextStep(); } } diff --git a/OndselSolver/BasicIntegrator.h b/OndselSolver/BasicIntegrator.h index 9a9aa2d2..221f0755 100644 --- a/OndselSolver/BasicIntegrator.h +++ b/OndselSolver/BasicIntegrator.h @@ -8,22 +8,27 @@ #pragma once -#include +//#include #include "Integrator.h" namespace MbD { class IntegratorInterface; - class DifferenceOperator; + class LinearMultiStepMethod; + template + class FullColumn; + using FColDsptr = std::shared_ptr>; class BasicIntegrator : public Integrator { //istep iTry maxTry tpast t tnew h hnew order orderNew orderMax opBDF continue public: + static std::shared_ptr With(); + void initialize() override; + virtual void calcOperatorMatrix(); virtual void incrementTime(); virtual void incrementTry(); - void initialize() override; void initializeGlobally() override; void initializeLocally() override; void iStep(size_t i) override; @@ -43,15 +48,16 @@ namespace MbD { virtual void setorder(size_t o); virtual void settnew(double t); virtual void sett(double t); - void settime(double t); - double tprevious(); + virtual void settime(double t); + double tprevious() const; + virtual FColDsptr yDerivat(size_t _order, double tout); - IntegratorInterface* system; + IntegratorInterface* system = nullptr; size_t istep = 0, iTry = 0, maxTry = 0; std::shared_ptr> tpast; - double t = 0.0, tnew = 0.0, h = 0, hnew = 0.0; + double t = 0.0, tnew = 0.0, h = 0.0, hnew = 0.0; size_t order = 0, orderNew = 0, orderMax = 0; - std::shared_ptr opBDF; + std::shared_ptr opBDF; bool _continue = false; }; } diff --git a/OndselSolver/CMakeLists.txt b/OndselSolver/CMakeLists.txt index 9fc1e822..02fad77c 100644 --- a/OndselSolver/CMakeLists.txt +++ b/OndselSolver/CMakeLists.txt @@ -41,6 +41,7 @@ set(ONDSELSOLVER_SRC ASMTConstantGravity.cpp ASMTConstantVelocityJoint.cpp ASMTConstraintSet.cpp + ASMTDistanceLimit.cpp ASMTCylindricalJoint.cpp ASMTCylSphJoint.cpp ASMTExtrusion.cpp @@ -141,6 +142,7 @@ set(ONDSELSOLVER_SRC DistanceConstraintIqcJc.cpp DistanceConstraintIqcJqc.cpp DistanceConstraintIqctJqc.cpp + DistanceLimitIJ.cpp DistancexyConstraintIJ.cpp DistancexyConstraintIqcJc.cpp DistancexyConstraintIqcJqc.cpp @@ -353,6 +355,7 @@ set(ONDSELSOLVER_HEADERS ASMTConstantGravity.h ASMTConstantVelocityJoint.h ASMTConstraintSet.h + ASMTDistanceLimit.h ASMTCylindricalJoint.h ASMTCylSphJoint.h ASMTExtrusion.h @@ -458,6 +461,7 @@ set(ONDSELSOLVER_HEADERS DistanceConstraintIqcJc.h DistanceConstraintIqcJqc.h DistanceConstraintIqctJqc.h + DistanceLimitIJ.h DistancexyConstraintIJ.h DistancexyConstraintIqcJc.h DistancexyConstraintIqcJqc.h @@ -637,6 +641,32 @@ set(ONDSELSOLVER_HEADERS ZTranslation.h ) +list(APPEND ONDSELSOLVER_SRC + AppliedForceTorque.cpp + DynIntegrator.cpp + DAEIntegrator.cpp + BasicDAEIntegrator.cpp + StartingBasicDAEIntegrator.cpp + NormalBasicDAEIntegrator.cpp + DAECorrector.cpp + StableStartingBDF.cpp + Extrapolator.cpp + SolverStatistics.cpp +) +list(APPEND ONDSELSOLVER_HEADERS + AppliedForceTorque.h + DynIntegrator.h + DAEIntegrator.h + BasicDAEIntegrator.h + DynamicEvents.h + StartingBasicDAEIntegrator.h + NormalBasicDAEIntegrator.h + DAECorrector.h + StableStartingBDF.h + Extrapolator.h + SolverStatistics.h +) + target_sources(OndselSolver PRIVATE "${ONDSELSOLVER_SRC}" "${ONDSELSOLVER_HEADERS}") diff --git a/OndselSolver/CoaxialGearConstraintIJ.cpp b/OndselSolver/CoaxialGearConstraintIJ.cpp index 4f9e0bab..1e69e5d5 100644 --- a/OndselSolver/CoaxialGearConstraintIJ.cpp +++ b/OndselSolver/CoaxialGearConstraintIJ.cpp @@ -122,6 +122,32 @@ void MbD::CoaxialGearConstraintIJ::fillPosICJacob(SpMatDsptr mat) mat->atijplusTransposeFullMatrix(iqEJ, iqEK, ppGpEKpEJlam); } +void MbD::CoaxialGearConstraintIJ::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + mat->atijplusFullRow(iG, iqEK, pGpEK); + + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); + mat->atijplusFullMatrixtimes(iqEK, iqEK, ppGpEKpEK, lam); + + auto ppGpEKpEIlam = ppGpEKpEI->times(lam); + mat->atijplusFullMatrix(iqEK, iqEI, ppGpEKpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqEK, ppGpEKpEIlam); + + auto ppGpEKpEJlam = ppGpEKpEJ->times(lam); + mat->atijplusFullMatrix(iqEK, iqEJ, ppGpEKpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEK, ppGpEKpEJlam); +} + +void MbD::CoaxialGearConstraintIJ::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); + mat->atijplusFullColumn(iqEK, iG, pGpEK->transpose()); +} + void MbD::CoaxialGearConstraintIJ::fillPosKineJacob(SpMatDsptr mat) { mat->atijplusFullRow(iG, iqEI, pGpEI); @@ -204,6 +230,11 @@ double MbD::CoaxialGearConstraintIJ::ratio() const return radiusI / radiusJ; } +void CoaxialGearConstraintIJ::preDyn() +{ + Constraint::preDyn(); +} + void MbD::CoaxialGearConstraintIJ::simUpdateAll() { angleKI->simUpdateAll(); diff --git a/OndselSolver/CoaxialGearConstraintIJ.h b/OndselSolver/CoaxialGearConstraintIJ.h index 69129e22..45661609 100644 --- a/OndselSolver/CoaxialGearConstraintIJ.h +++ b/OndselSolver/CoaxialGearConstraintIJ.h @@ -30,6 +30,8 @@ namespace MbD { void fillAccICIterError(FColDsptr col) override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; void fillPosKineJacob(SpMatDsptr mat) override; void fillVelICJacob(SpMatDsptr mat) override; void initialize() override; @@ -38,6 +40,7 @@ namespace MbD { void postInput() override; void postPosICIteration() override; void preAccIC() override; + void preDyn() override; void prePosIC() override; void preVelIC() override; double ratio() const; diff --git a/OndselSolver/ConstVelConstraintIJ.cpp b/OndselSolver/ConstVelConstraintIJ.cpp index 1a7b02eb..78d22104 100644 --- a/OndselSolver/ConstVelConstraintIJ.cpp +++ b/OndselSolver/ConstVelConstraintIJ.cpp @@ -101,3 +101,31 @@ void ConstVelConstraintIJ::simUpdateAll() aA10IeJe->simUpdateAll(); ConstraintIJ::simUpdateAll(); } + +void ConstVelConstraintIJ::postDynPredictor() +{ + aA01IeJe->postDynPredictor(); + aA10IeJe->postDynPredictor(); + ConstraintIJ::postDynPredictor(); +} + +void ConstVelConstraintIJ::postDynCorrectorIteration() +{ + aA01IeJe->postDynCorrectorIteration(); + aA10IeJe->postDynCorrectorIteration(); + ConstraintIJ::postDynCorrectorIteration(); +} + +void ConstVelConstraintIJ::preDynOutput() +{ + aA01IeJe->preDynOutput(); + aA10IeJe->preDynOutput(); + ConstraintIJ::preDynOutput(); +} + +void ConstVelConstraintIJ::postDynOutput() +{ + aA01IeJe->postDynOutput(); + aA10IeJe->postDynOutput(); + ConstraintIJ::postDynOutput(); +} diff --git a/OndselSolver/ConstVelConstraintIJ.h b/OndselSolver/ConstVelConstraintIJ.h index cf0d70f1..80a9ecbc 100644 --- a/OndselSolver/ConstVelConstraintIJ.h +++ b/OndselSolver/ConstVelConstraintIJ.h @@ -17,6 +17,10 @@ namespace MbD { { //aA01IeJe aA10IeJe public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; ConstVelConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj); static std::shared_ptr With(EndFrmsptr frmi, EndFrmsptr frmj); diff --git a/OndselSolver/ConstVelConstraintIqcJc.cpp b/OndselSolver/ConstVelConstraintIqcJc.cpp index 48bd2358..7d621aa3 100644 --- a/OndselSolver/ConstVelConstraintIqcJc.cpp +++ b/OndselSolver/ConstVelConstraintIqcJc.cpp @@ -106,3 +106,14 @@ void MbD::ConstVelConstraintIqcJc::useEquationNumbers() { iqEI = std::static_pointer_cast(frmI)->iqE(); } + +void ConstVelConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void ConstVelConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} diff --git a/OndselSolver/ConstVelConstraintIqcJc.h b/OndselSolver/ConstVelConstraintIqcJc.h index 3c6ddc76..4cb735e2 100644 --- a/OndselSolver/ConstVelConstraintIqcJc.h +++ b/OndselSolver/ConstVelConstraintIqcJc.h @@ -17,6 +17,8 @@ namespace MbD { { //pGpEI ppGpEIpEI iqEI public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; ConstVelConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj); void calcPostDynCorrectorIteration() override; diff --git a/OndselSolver/ConstVelConstraintIqcJqc.cpp b/OndselSolver/ConstVelConstraintIqcJqc.cpp index c7e3aff2..a4d01d69 100644 --- a/OndselSolver/ConstVelConstraintIqcJqc.cpp +++ b/OndselSolver/ConstVelConstraintIqcJqc.cpp @@ -135,3 +135,19 @@ std::string MbD::ConstVelConstraintIqcJqc::constraintSpec() { return "ConstVelConstraintIJ"; } + +void ConstVelConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + ConstVelConstraintIqcJc::fillpFpy(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void ConstVelConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + ConstVelConstraintIqcJc::fillpFpydot(mat); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} diff --git a/OndselSolver/ConstVelConstraintIqcJqc.h b/OndselSolver/ConstVelConstraintIqcJqc.h index 76237966..49320883 100644 --- a/OndselSolver/ConstVelConstraintIqcJqc.h +++ b/OndselSolver/ConstVelConstraintIqcJqc.h @@ -17,6 +17,8 @@ namespace MbD { { //pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; ConstVelConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj); void calcPostDynCorrectorIteration() override; diff --git a/OndselSolver/ConstantGravity.cpp b/OndselSolver/ConstantGravity.cpp index 744a9c38..7255c929 100644 --- a/OndselSolver/ConstantGravity.cpp +++ b/OndselSolver/ConstantGravity.cpp @@ -18,3 +18,30 @@ void MbD::ConstantGravity::fillAccICIterError(FColDsptr col) col->atiplusFullColumntimes(part->iqX(), gXYZ, part->m); } } + +void ConstantGravity::fillDynError(FColDsptr col) +{ + for (auto& part : *(root()->parts)) { + col->atiplusFullColumntimes(part->iqX(), gXYZ, part->m); + } +} + +void ConstantGravity::postDynCorrectorIteration() +{ + //Do nothing. +} + +void ConstantGravity::preDynOutput() +{ + //Do nothing. +} + +void ConstantGravity::postDynPredictor() +{ + //Do nothing. +} + +void ConstantGravity::postDynOutput() +{ + //Do nothing. +} diff --git a/OndselSolver/ConstantGravity.h b/OndselSolver/ConstantGravity.h index c2fd6e09..d16a3a8b 100644 --- a/OndselSolver/ConstantGravity.h +++ b/OndselSolver/ConstantGravity.h @@ -15,6 +15,11 @@ namespace MbD { { // public: + void fillDynError(FColDsptr col) override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynPredictor() override; + void postDynOutput() override; void fillAccICIterError(FColDsptr col) override; FColDsptr gXYZ; diff --git a/OndselSolver/Constraint.cpp b/OndselSolver/Constraint.cpp index dffa6484..fd21dd5e 100644 --- a/OndselSolver/Constraint.cpp +++ b/OndselSolver/Constraint.cpp @@ -171,6 +171,26 @@ void Constraint::addToJointTorqueI(FColDsptr) { } +double Constraint::constraintVelocity() const +{ + throw SimulationStoppingError("Generalized force is not implemented for this constraint."); +} + +void Constraint::fillGeneralizedForce(FColDsptr, double) +{ + throw SimulationStoppingError("Generalized force is not implemented for this constraint."); +} + +void Constraint::fillGeneralizedForcePositionJacobian(SpMatDsptr, double, double) +{ + throw SimulationStoppingError("Generalized force is not implemented for this constraint."); +} + +void Constraint::fillGeneralizedForceVelocityJacobian(SpMatDsptr, double) +{ + throw SimulationStoppingError("Generalized force is not implemented for this constraint."); +} + void Constraint::fillConstraints(std::shared_ptr>> allConstraints) { Item::fillConstraints(allConstraints); } @@ -190,3 +210,34 @@ void Constraint::fillEssenConstraints(std::shared_ptr>> perpenConstraints) { Item::fillPerpenConstraints(perpenConstraints); } + +void Constraint::setpqsumu(FColDsptr col) +{ + mu = col->at(iG); +} + +void Constraint::setpqsumudot(FColDsptr col) +{ + lam = col->at(iG); +} + +void Constraint::setpqsumuddot(FColDsptr col) +{ + //Do nothing +} + +void Constraint::fillDynError(FColDsptr col) +{ + //"Same as fillPosICError: col." + fillPosICError(col); +} + +void Constraint::fillpqsumu(FColDsptr col) +{ + col->atiput(iG, mu); +} + +void Constraint::fillpqsumudot(FColDsptr col) +{ + col->atiput(iG, lam); +} diff --git a/OndselSolver/Constraint.h b/OndselSolver/Constraint.h index 9a523874..b1af67f8 100644 --- a/OndselSolver/Constraint.h +++ b/OndselSolver/Constraint.h @@ -19,12 +19,26 @@ namespace MbD { { //iG aG lam mu lamDeriv owner public: + void setpqsumu(FColDsptr col) override; + void setpqsumudot(FColDsptr col) override; + void setpqsumuddot(FColDsptr col) override; + void fillDynError(FColDsptr col) override; + void fillpqsumu(FColDsptr col) override; + void fillpqsumudot(FColDsptr col) override; Constraint(); Constraint(const std::string& str); void initialize() override; virtual void addToJointForceI(FColDsptr col); virtual void addToJointTorqueI(FColDsptr col); + virtual double constraintVelocity() const; + virtual void fillGeneralizedForce(FColDsptr col, double multiplier); + virtual void fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative + ); + virtual void fillGeneralizedForceVelocityJacobian(SpMatDsptr mat, double derivative); void fillAccICIterJacob(SpMatDsptr mat) override; void fillConstraints(std::shared_ptr>> allConstraints) override; virtual void fillConstraints(std::shared_ptr sptr, std::shared_ptr>> allConstraints); diff --git a/OndselSolver/ConstraintSet.cpp b/OndselSolver/ConstraintSet.cpp index 59865c4a..5f9f1f83 100644 --- a/OndselSolver/ConstraintSet.cpp +++ b/OndselSolver/ConstraintSet.cpp @@ -192,3 +192,63 @@ void MbD::ConstraintSet::postDynStep() { constraintsDo([](std::shared_ptr constraint) { constraint->postDynStep(); }); } + +void ConstraintSet::fillpqsumu(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillpqsumu(col); }); +} + +void ConstraintSet::fillpqsumudot(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillpqsumudot(col); }); +} + +void ConstraintSet::setpqsumu(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->setpqsumu(col); }); +} + +void ConstraintSet::setpqsumudot(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->setpqsumudot(col); }); +} + +void ConstraintSet::postDynPredictor() +{ + constraintsDo([](std::shared_ptr con) { con->postDynPredictor(); }); +} + +void ConstraintSet::fillDynError(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->fillDynError(col); }); +} + +void ConstraintSet::fillpFpy(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr con) { con->fillpFpy(mat); }); +} + +void ConstraintSet::fillpFpydot(SpMatDsptr mat) +{ + constraintsDo([&](std::shared_ptr con) { con->fillpFpydot(mat); }); +} + +void ConstraintSet::postDynCorrectorIteration() +{ + constraintsDo([](std::shared_ptr con) { con->postDynCorrectorIteration(); }); +} + +void ConstraintSet::postDynOutput() +{ + constraintsDo([](std::shared_ptr con) { con->postDynOutput(); }); +} + +void ConstraintSet::preDynOutput() +{ + constraintsDo([](std::shared_ptr con) { con->preDynOutput(); }); +} + +void ConstraintSet::setpqsumuddot(FColDsptr col) +{ + constraintsDo([&](std::shared_ptr con) { con->setpqsumuddot(col); }); +} diff --git a/OndselSolver/ConstraintSet.h b/OndselSolver/ConstraintSet.h index 66b7a17c..a6240b33 100644 --- a/OndselSolver/ConstraintSet.h +++ b/OndselSolver/ConstraintSet.h @@ -17,6 +17,18 @@ namespace MbD { { // public: + void fillpqsumu(FColDsptr col) override; + void fillpqsumudot(FColDsptr col) override; + void setpqsumu(FColDsptr col) override; + void setpqsumudot(FColDsptr col) override; + void postDynPredictor() override; + void fillDynError(FColDsptr col) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + void postDynCorrectorIteration() override; + void postDynOutput() override; + void preDynOutput() override; + void setpqsumuddot(FColDsptr col) override; ConstraintSet(); ConstraintSet(const std::string& str); void constraintsDo(const std::function )>& f); diff --git a/OndselSolver/DAECorrector.cpp b/OndselSolver/DAECorrector.cpp new file mode 100644 index 00000000..6e3d29e0 --- /dev/null +++ b/OndselSolver/DAECorrector.cpp @@ -0,0 +1,160 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include + +#include "DAECorrector.h" +#include "BasicDAEIntegrator.h" +#include "GESpMatParPvMarkoFast.h" +#include "GESpMatParPvPrecise.h" +#include "SystemSolver.h" +#include "SimulationStoppingError.h" + +#include "CREATE.h" + +using namespace MbD; + +std::shared_ptr DAECorrector::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void DAECorrector::iterate() +{ + //VectorNewtonRaphson::iterate(); //Inlined to help debugging + iterNo = SIZE_MAX; + fillY(); + calcyNorm(); + yNorms->push_back(yNorm); + + while (true) { + incrementIterNo(); + fillPyPx(); + //outputSpreadsheet(); + solveEquations(); + calcDXNormImproveRootCalcYNorm(); + if (isConverged()) { + //std::cout << "iterNo = " << iterNo << std::endl; + break; + } + } +} + +void DAECorrector::fillPyPx() +{ + pypx = daeSystem->calcG(); +} + +void DAECorrector::fillY() +{ + y = daeSystem->fillF(); +} + +void DAECorrector::passRootToSystem() +{ + daeSystem->y = x; +} + +void DAECorrector::calcdxNorm() +{ + dxNorm = daeSystem->corErrorNormFromwrt(dx, x); + std::stringstream ss; + ss << std::setprecision(std::numeric_limits::max_digits10); + ss << "MbD: Convergence = " << dxNorm; + auto str = ss.str(); + daeSystem->logString(str); +} + +void DAECorrector::basicSolveEquations() +{ + dx = matrixSolver->solvewithsaveOriginal(pypx, y->negated(), false); +} + +void DAECorrector::handleSingularMatrix() +{ + if (std::dynamic_pointer_cast(matrixSolver)) { + matrixSolver = CREATE::With(); + solveEquations(); + } + else { + matrixSolver->throwSingularMatrixError("Singular forward-dynamics corrector matrix"); + } +} + +void DAECorrector::initializeGlobally() +{ + iterMax = daeSystem->iterMax(); + x = daeSystem->y; + matrixSolver = CREATE::With(); +} + +void DAECorrector::run() +{ + preRun(); + initializeLocally(); + initializeGlobally(); + iterate(); + finalize(); + reportStats(); + postRun(); +} + +void DAECorrector::preRun() +{ + //auto basicDAEIntegrator = static_cast(system); + daeSystem->preDAECorrector(); +} + +void DAECorrector::askSystemToUpdate() +{ + daeSystem->updateForDAECorrector(); +} + +bool DAECorrector::isConverged() +{ + return daeSystem->isConvergedForand(iterNo, dxNorms); +} + +void DAECorrector::postRun() +{ + daeSystem->postDAECorrector(); +} + +void DAECorrector::setSystem(Solver* sys) +{ + daeSystem = static_cast(sys); +} + +void DAECorrector::reportStats() +{ + statistics->iterNo = iterNo; + daeSystem->useDAECorrectorStats(statistics); +} + +void DAECorrector::outputSpreadsheet() +{ + std::ofstream os("../testapp/spreadsheetcpp.csv"); + os << std::setprecision(std::numeric_limits::max_digits10); + for (size_t i = 0; i < pypx->nrow(); i++) + { + auto rowi = pypx->at(i); + for (size_t j = 0; j < pypx->ncol(); j++) + { + if (rowi->find(j) == rowi->end()) { + os << 0.0; + } + else { + os << rowi->at(j); + } + os << '\t'; + } + os << "\t" << y->at(i) << std::endl; + } +} diff --git a/OndselSolver/DAECorrector.h b/OndselSolver/DAECorrector.h new file mode 100644 index 00000000..ea24f7ab --- /dev/null +++ b/OndselSolver/DAECorrector.h @@ -0,0 +1,43 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "VectorNewtonRaphson.h" +#include "SparseMatrix.h" + +namespace MbD { + class BasicDAEIntegrator; + + class DAECorrector : public VectorNewtonRaphson + { + // + public: + static std::shared_ptr With(); + + void iterate(); + void fillPyPx() override; + void fillY() override; + void passRootToSystem() override; + void basicSolveEquations() override; + void initializeGlobally() override; + void calcdxNorm() override; + void handleSingularMatrix() override; + void run() override; + void preRun() override; + void askSystemToUpdate() override; + bool isConverged() override; + void postRun() override; + void setSystem(Solver* sys) override; + void reportStats() override; + void outputSpreadsheet(); + + SpMatDsptr pypx; + BasicDAEIntegrator* daeSystem; + }; +} diff --git a/OndselSolver/DAEIntegrator.cpp b/OndselSolver/DAEIntegrator.cpp new file mode 100644 index 00000000..4a7328c5 --- /dev/null +++ b/OndselSolver/DAEIntegrator.cpp @@ -0,0 +1,221 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include + +#include "DAEIntegrator.h" +#include "Item.h" +#include "SystemSolver.h" +#include "BasicQuasiIntegrator.h" +#include "SingularMatrixError.h" +#include "SimulationStoppingError.h" +#include "TooSmallStepSizeError.h" +#include "TooManyTriesError.h" +#include "SingularMatrixError.h" +#include "DiscontinuityError.h" +#include "StartingBasicDAEIntegrator.h" +#include "NormalBasicDAEIntegrator.h" + +using namespace MbD; + +std::shared_ptr DAEIntegrator::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void DAEIntegrator::initialize() +{ + Solver::initialize(); + integrator = StartingBasicDAEIntegrator::With(); + integrator->setSystem(this); +} + +void DAEIntegrator::initializeGlobally() +{ + IntegratorInterface::initializeGlobally(); + tout = std::min(tout, tend); + assignEquationNumbers(); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->useEquationNumbers(); }); +} + +void DAEIntegrator::preFirstStep() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->preDynFirstStep(); }); +} + +void DAEIntegrator::checkForOutputThrough(double t) +{ + throw SimulationStoppingError("To be implemented."); +} + +void DAEIntegrator::preRun() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DAEIntegrator::preStep() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->preDynStep(); }); +} + +double DAEIntegrator::suggestSmallerOrAcceptStepSize(double hnew) +{ + throw SimulationStoppingError("To be implemented."); //May not be used? + return 0.0; + //auto hnew2 = system->suggestSmallerOrAcceptStepSize(hnew); + //if (hnew2 > hmax) { + // hnew2 = hmax; + // system->logString("MbD: Step size is at user specified maximum."); + //} + //if (hnew2 < hmin) { + // std::stringstream ss; + // ss << "MbD: Step size " << hnew2 << " < " << hmin << " user specified minimum."; + // auto str = ss.str(); + // system->logString(str); + // throw TooSmallStepSizeError(""); + //} + //return hnew2; +} + +void DAEIntegrator::postRun() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DAEIntegrator::runInitialConditionTypeSolution() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DAEIntegrator::iStep(size_t i) +{ + throw SimulationStoppingError("To be implemented."); +} + +void DAEIntegrator::selectOrder() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DAEIntegrator::checkForDiscontinuity() +{ + throw SimulationStoppingError("To be implemented."); +} + +double DAEIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) +{ + throw SimulationStoppingError("To be implemented."); + return 0.0; +} + +FColDsptr DAEIntegrator::integrationRelativeTolerance() +{ + //"Answer column of tolerances used by the integration error estimator." + //"Algebraic variables are not included in the error estimator." + + auto relTol = system->integrationRelativeTolerance(); + auto col = std::make_shared>(neqn); + for (size_t i = 0; i < neqn - ncon; i++) + { + col->atiput(i, relTol); + } + for (size_t i = neqn - ncon; i < neqn; i++) + { + col->atiput(i, std::numeric_limits::min()); + } + return col; +} + +FColDsptr DAEIntegrator::integrationAbsoluteTolerance() +{ + auto absTol = system->integrationAbsoluteTolerance(); + auto col = std::make_shared>(neqn); + for (size_t i = 0; i < neqn - ncon; i++) + { + col->atiput(i, absTol); + } + for (size_t i = neqn - ncon; i < neqn; i++) + { + col->atiput(i, std::numeric_limits::min()); + } + return col; +} + +FColDsptr DAEIntegrator::correctorRelativeTolerance() +{ + auto corRelTol = system->correctorRelativeTolerance(); + auto col = std::make_shared>(neqn); + for (size_t i = 0; i < neqn; i++) + { + col->atiput(i, corRelTol); + } + return col; +} + +FColDsptr DAEIntegrator::correctorAbsoluteTolerance() +{ + auto corAbsTol = system->correctorAbsoluteTolerance(); + auto col = std::make_shared>(neqn); + for (size_t i = 0; i < neqn; i++) + { + col->atiput(i, corAbsTol); + } + return col; +} + +void DAEIntegrator::y(FColDsptr col) +{ + throw SimulationStoppingError("To be implemented."); + //system->y(col); +} + +void DAEIntegrator::ydot(FColDsptr col) +{ + throw SimulationStoppingError("To be implemented."); + //system->ydot(col); +} + +void DAEIntegrator::preDAEOutput() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DAEIntegrator::postDAEOutput() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DAEIntegrator::useTrialStepStats(std::shared_ptr stats) +{ + throw SimulationStoppingError("To be implemented."); + //system->useDAETrialStepStats(stats); +} + +void DAEIntegrator::useDAEStepStats(std::shared_ptr stats) +{ + system->useDAEStepStats(stats); +} + +void DAEIntegrator::run() +{ + preRun(); + initializeLocally(); + initializeGlobally(); + if (tout <= tend) { + integrator->run(); + auto startingintegrator = std::dynamic_pointer_cast(integrator); + auto normalIntegrator = std::make_shared(startingintegrator); + integrator = normalIntegrator; + integrator->run(); + } + finalize(); + reportStats(); + postRun(); +} diff --git a/OndselSolver/DAEIntegrator.h b/OndselSolver/DAEIntegrator.h new file mode 100644 index 00000000..833f75e2 --- /dev/null +++ b/OndselSolver/DAEIntegrator.h @@ -0,0 +1,55 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +//#include + +#include "IntegratorInterface.h" +#include "enum.h" +#include "FullColumn.h" + +namespace MbD { + class DAEIntegrator : public IntegratorInterface + { + //neqn ncon + public: + static std::shared_ptr With(); + void initialize() override; + + void initializeGlobally() override; + void preFirstStep() override; + void checkForOutputThrough(double t) override; + void preRun() override; + void preStep() override; + double suggestSmallerOrAcceptStepSize(double hnew) override; + //void incrementTime(double tnew) override; + //void throwDiscontinuityError(const std::string& str, std::shared_ptr> discontinuityTypes); + //void interpolateAt(double t) override; + void postRun() override; + void runInitialConditionTypeSolution() override; + void iStep(size_t i) override; + void selectOrder() override; + void checkForDiscontinuity() override; + double suggestSmallerOrAcceptFirstStepSize(double hnew) override; + FColDsptr integrationRelativeTolerance(); + FColDsptr integrationAbsoluteTolerance(); + FColDsptr correctorRelativeTolerance(); + FColDsptr correctorAbsoluteTolerance(); + void y(FColDsptr col) override; + void ydot(FColDsptr col) override; + virtual void preDAEOutput(); + virtual void postDAEOutput(); + void useTrialStepStats(std::shared_ptr stats) override; + void useDAEStepStats(std::shared_ptr stats) override; + void run() override; + + size_t neqn = SIZE_MAX, ncon = SIZE_MAX; + + }; +} diff --git a/OndselSolver/DifferenceOperator.cpp b/OndselSolver/DifferenceOperator.cpp index 0c7c6388..a314639f 100644 --- a/OndselSolver/DifferenceOperator.cpp +++ b/OndselSolver/DifferenceOperator.cpp @@ -16,6 +16,27 @@ using namespace MbD; +void DifferenceOperator::formDegenerateTaylorRow(size_t i) const +{ + auto row = taylorMatrix->at(i); + row->zeroSelf(); + row->at(0) = 1.0; +} + +FColDsptr DifferenceOperator::valueWith(std::shared_ptr> series) +{ + return derivativewith(0, series); +} + +FColDsptr DifferenceOperator::derivativewith(size_t deriv, std::shared_ptr> series) const +{ + const auto coefficients = operatorMatrix->at(deriv); + auto result = series->at(0)->times(coefficients->at(0)); + for (size_t i = 1; i < coefficients->size(); ++i) + result->equalSelfPlusFullVectortimes(series->at(i), coefficients->at(i)); + return result; +} + FRowDsptr DifferenceOperator::OneOverFactorials = []() { auto oneOverFactorials = std::make_shared>(10); for (size_t i = 0; i < oneOverFactorials->size(); i++) @@ -32,12 +53,26 @@ void DifferenceOperator::calcOperatorMatrix() //valuedot(time) : = (operatorMatrix at : 2) timesColumn : series. //valueddot(time) : = (operatorMatrix at : 3) timesColumn : series. - this->formTaylorMatrix(); - try { - operatorMatrix = CREATE::With()->inversesaveOriginal(taylorMatrix, false); - } - catch (const SingularMatrixError& ex) { - } + formTaylorMatrix(); + // Taylor columns scale as powers of the time step. Equilibrate them before + // pivoting instead of ignoring a singular-matrix exception (which can leave + // a stale operator of the previous order). Rescale inverse rows afterwards. + const auto n = taylorMatrix->nrow(); + auto scaled = std::make_shared>(n, n); + std::vector scales(n, 0.0); + for (size_t j = 0; j < n; ++j) { + for (size_t i = 0; i < n; ++i) + scales[j] = std::max(scales[j], std::abs(taylorMatrix->at(i)->at(j))); + if (!(scales[j] > 0) || !std::isfinite(scales[j])) + throw SingularMatrixError("Invalid time nodes in the integration operator"); + for (size_t i = 0; i < n; ++i) + scaled->at(i)->at(j) = taylorMatrix->at(i)->at(j) / scales[j]; + } + auto inverse = CREATE::With()->inversesaveOriginal(scaled, false); + for (size_t i = 0; i < n; ++i) + for (size_t j = 0; j < n; ++j) + inverse->at(i)->at(j) /= scales[i]; + operatorMatrix = inverse; } void DifferenceOperator::initialize() diff --git a/OndselSolver/DifferenceOperator.h b/OndselSolver/DifferenceOperator.h index 117ec2b5..8b7e8590 100644 --- a/OndselSolver/DifferenceOperator.h +++ b/OndselSolver/DifferenceOperator.h @@ -17,6 +17,9 @@ namespace MbD { { //iStep order taylorMatrix operatorMatrix time timeNodes public: + void formDegenerateTaylorRow(size_t i) const; + FColDsptr valueWith(std::shared_ptr> series); + FColDsptr derivativewith(size_t deriv, std::shared_ptr> series) const; virtual ~DifferenceOperator() {} void calcOperatorMatrix(); virtual void initialize(); diff --git a/OndselSolver/DirectionCosineConstraintIJ.cpp b/OndselSolver/DirectionCosineConstraintIJ.cpp index bc4cb736..3fb2c8ae 100644 --- a/OndselSolver/DirectionCosineConstraintIJ.cpp +++ b/OndselSolver/DirectionCosineConstraintIJ.cpp @@ -84,3 +84,27 @@ void DirectionCosineConstraintIJ::preAccIC() aAijIeJe->preAccIC(); ConstraintIJ::preAccIC(); } + +void DirectionCosineConstraintIJ::postDynPredictor() +{ + aAijIeJe->postDynPredictor(); + ConstraintIJ::postDynPredictor(); +} + +void DirectionCosineConstraintIJ::postDynCorrectorIteration() +{ + aAijIeJe->postDynCorrectorIteration(); + ConstraintIJ::postDynCorrectorIteration(); +} + +void DirectionCosineConstraintIJ::preDynOutput() +{ + aAijIeJe->preDynOutput(); + ConstraintIJ::preDynOutput(); +} + +void DirectionCosineConstraintIJ::postDynOutput() +{ + aAijIeJe->postDynOutput(); + ConstraintIJ::postDynOutput(); +} diff --git a/OndselSolver/DirectionCosineConstraintIJ.h b/OndselSolver/DirectionCosineConstraintIJ.h index cb69bc87..978b51ed 100644 --- a/OndselSolver/DirectionCosineConstraintIJ.h +++ b/OndselSolver/DirectionCosineConstraintIJ.h @@ -17,6 +17,10 @@ namespace MbD { { //axisI axisJ aAijIeJe public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; DirectionCosineConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj, size_t axisi, size_t axisj); void calcPostDynCorrectorIteration() override; diff --git a/OndselSolver/DirectionCosineConstraintIqcJc.cpp b/OndselSolver/DirectionCosineConstraintIqcJc.cpp index 37acc6db..a369fee9 100644 --- a/OndselSolver/DirectionCosineConstraintIqcJc.cpp +++ b/OndselSolver/DirectionCosineConstraintIqcJc.cpp @@ -77,3 +77,14 @@ void DirectionCosineConstraintIqcJc::addToJointTorqueI(FColDsptr jointTorque) auto c2Torque = aBOIp->timesFullColumn(lampGpE); jointTorque->equalSelfPlusFullColumntimes(c2Torque, 0.5); } + +void DirectionCosineConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void DirectionCosineConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} diff --git a/OndselSolver/DirectionCosineConstraintIqcJc.h b/OndselSolver/DirectionCosineConstraintIqcJc.h index a8aed530..735fa9e5 100644 --- a/OndselSolver/DirectionCosineConstraintIqcJc.h +++ b/OndselSolver/DirectionCosineConstraintIqcJc.h @@ -17,6 +17,8 @@ namespace MbD { { //pGpEI ppGpEIpEI iqEI public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; DirectionCosineConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj, size_t axisi, size_t axisj); void addToJointTorqueI(FColDsptr col) override; diff --git a/OndselSolver/DirectionCosineConstraintIqcJqc.cpp b/OndselSolver/DirectionCosineConstraintIqcJqc.cpp index 64cfef81..1a754cf1 100644 --- a/OndselSolver/DirectionCosineConstraintIqcJqc.cpp +++ b/OndselSolver/DirectionCosineConstraintIqcJqc.cpp @@ -86,3 +86,19 @@ void DirectionCosineConstraintIqcJqc::fillAccICIterError(FColDsptr col) sum += qEdotJ->transposeTimesFullColumn(ppGpEJpEJ->timesFullColumn(qEdotJ)); col->atiplusNumber(iG, sum); } + +void DirectionCosineConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + DirectionCosineConstraintIqcJc::fillpFpy(mat); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void DirectionCosineConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + DirectionCosineConstraintIqcJc::fillpFpydot(mat); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} diff --git a/OndselSolver/DirectionCosineConstraintIqcJqc.h b/OndselSolver/DirectionCosineConstraintIqcJqc.h index 3643913a..a241edbc 100644 --- a/OndselSolver/DirectionCosineConstraintIqcJqc.h +++ b/OndselSolver/DirectionCosineConstraintIqcJqc.h @@ -17,6 +17,8 @@ namespace MbD { { //pGpEJ ppGpEIpEJ ppGpEJpEJ iqEJ public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; DirectionCosineConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj, size_t axisi, size_t axisj); void calcPostDynCorrectorIteration() override; diff --git a/OndselSolver/DiscontinuityError.h b/OndselSolver/DiscontinuityError.h index 9593707a..418e7f98 100644 --- a/OndselSolver/DiscontinuityError.h +++ b/OndselSolver/DiscontinuityError.h @@ -31,5 +31,10 @@ namespace MbD { } virtual ~DiscontinuityError() noexcept {} + + const std::shared_ptr>& types() const + { + return discontinuityTypes; + } }; } diff --git a/OndselSolver/DistanceConstraintIJ.cpp b/OndselSolver/DistanceConstraintIJ.cpp index 3fece7f8..ef7b2e8b 100644 --- a/OndselSolver/DistanceConstraintIJ.cpp +++ b/OndselSolver/DistanceConstraintIJ.cpp @@ -91,3 +91,27 @@ ConstraintType MbD::DistanceConstraintIJ::type() { return ConstraintType::displacement; } + +void DistanceConstraintIJ::postDynPredictor() +{ + distIeJe->postDynPredictor(); + ConstraintIJ::postDynPredictor(); +} + +void DistanceConstraintIJ::postDynCorrectorIteration() +{ + distIeJe->postDynCorrectorIteration(); + ConstraintIJ::postDynCorrectorIteration(); +} + +void DistanceConstraintIJ::preDynOutput() +{ + distIeJe->preDynOutput(); + ConstraintIJ::preDynOutput(); +} + +void DistanceConstraintIJ::postDynOutput() +{ + distIeJe->postDynOutput(); + ConstraintIJ::postDynOutput(); +} diff --git a/OndselSolver/DistanceConstraintIJ.h b/OndselSolver/DistanceConstraintIJ.h index c40d3662..daa8ee23 100644 --- a/OndselSolver/DistanceConstraintIJ.h +++ b/OndselSolver/DistanceConstraintIJ.h @@ -16,6 +16,10 @@ namespace MbD { { //distIeJe public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; DistanceConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj); static std::shared_ptr With(EndFrmsptr frmi, EndFrmsptr frmj); diff --git a/OndselSolver/DistanceConstraintIqcJc.cpp b/OndselSolver/DistanceConstraintIqcJc.cpp index 7aeebfbc..34873ad3 100644 --- a/OndselSolver/DistanceConstraintIqcJc.cpp +++ b/OndselSolver/DistanceConstraintIqcJc.cpp @@ -13,6 +13,29 @@ using namespace MbD; +namespace +{ +void addOuterProduct( + SpMatDsptr mat, + size_t rowStart, + const FRowDsptr& row, + size_t columnStart, + const FRowDsptr& column, + double factor +) +{ + for (size_t i = 0; i < row->size(); ++i) { + for (size_t j = 0; j < column->size(); ++j) { + mat->atijplusNumber( + rowStart + i, + columnStart + j, + factor * row->at(i) * column->at(j) + ); + } + } +} +} // namespace + MbD::DistanceConstraintIqcJc::DistanceConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj) : DistanceConstraintIJ(frmi, frmj) { } @@ -110,3 +133,80 @@ void MbD::DistanceConstraintIqcJc::useEquationNumbers() iqXI = frmIeqc->iqX(); iqEI = frmIeqc->iqE(); } + +void DistanceConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullMatrixtimes(iqXI, iqXI, ppGpXIpXI, lam); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void DistanceConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} + +double DistanceConstraintIqcJc::constraintVelocity() const +{ + auto frameI = std::static_pointer_cast(frmI); + return pGpXI->timesFullColumn(frameI->qXdot()) + + pGpEI->timesFullColumn(frameI->qEdot()); +} + +void DistanceConstraintIqcJc::fillGeneralizedForce(FColDsptr col, double multiplier) +{ + col->atiplusFullVectortimes(iqXI, pGpXI, multiplier); + col->atiplusFullVectortimes(iqEI, pGpEI, multiplier); +} + +void DistanceConstraintIqcJc::fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative +) +{ + mat->atijplusFullMatrixtimes(iqXI, iqXI, ppGpXIpXI, multiplier); + auto crossHessian = ppGpXIpEI->times(multiplier); + mat->atijplusFullMatrix(iqXI, iqEI, crossHessian); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, crossHessian); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, multiplier); + + const std::pair segments[] = {{iqXI, pGpXI}, {iqEI, pGpEI}}; + for (const auto& row : segments) { + for (const auto& column : segments) { + addOuterProduct( + mat, + row.first, + row.second, + column.first, + column.second, + derivative + ); + } + } +} + +void DistanceConstraintIqcJc::fillGeneralizedForceVelocityJacobian( + SpMatDsptr mat, + double derivative +) +{ + const std::pair segments[] = {{iqXI, pGpXI}, {iqEI, pGpEI}}; + for (const auto& row : segments) { + for (const auto& column : segments) { + addOuterProduct( + mat, + row.first, + row.second, + column.first, + column.second, + derivative + ); + } + } +} diff --git a/OndselSolver/DistanceConstraintIqcJc.h b/OndselSolver/DistanceConstraintIqcJc.h index c0f4d1c9..1344c25a 100644 --- a/OndselSolver/DistanceConstraintIqcJc.h +++ b/OndselSolver/DistanceConstraintIqcJc.h @@ -15,6 +15,16 @@ namespace MbD { { //pGpXI pGpEI ppGpXIpXI ppGpXIpEI ppGpEIpEI iqXI iqEI public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + double constraintVelocity() const override; + void fillGeneralizedForce(FColDsptr col, double multiplier) override; + void fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative + ) override; + void fillGeneralizedForceVelocityJacobian(SpMatDsptr mat, double derivative) override; DistanceConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj); void addToJointForceI(FColDsptr col) override; diff --git a/OndselSolver/DistanceConstraintIqcJqc.cpp b/OndselSolver/DistanceConstraintIqcJqc.cpp index fbb4b93e..8b925ae2 100644 --- a/OndselSolver/DistanceConstraintIqcJqc.cpp +++ b/OndselSolver/DistanceConstraintIqcJqc.cpp @@ -13,6 +13,29 @@ using namespace MbD; +namespace +{ +void addOuterProduct( + SpMatDsptr mat, + size_t rowStart, + const FRowDsptr& row, + size_t columnStart, + const FRowDsptr& column, + double factor +) +{ + for (size_t i = 0; i < row->size(); ++i) { + for (size_t j = 0; j < column->size(); ++j) { + mat->atijplusNumber( + rowStart + i, + columnStart + j, + factor * row->at(i) * column->at(j) + ); + } + } +} +} // namespace + MbD::DistanceConstraintIqcJqc::DistanceConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj) : DistanceConstraintIqcJc(frmi, frmj) { } @@ -121,3 +144,128 @@ std::string MbD::DistanceConstraintIqcJqc::constraintSpec() { return "DistanceConstraintIJ"; } + +void DistanceConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + DistanceConstraintIqcJc::fillpFpy(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + auto ppGpXIpXJlam = ppGpXIpXJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqXJ, ppGpXIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqXI, ppGpXIpXJlam); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + mat->atijplusFullMatrixtimes(iqXJ, iqXJ, ppGpXJpXJ, lam); + auto ppGpXIpEJlam = ppGpXIpEJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqEJ, ppGpXIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXI, ppGpXIpEJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + auto ppGpXJpEJlam = ppGpXJpEJ->times(lam); + mat->atijplusFullMatrix(iqXJ, iqEJ, ppGpXJpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXJ, ppGpXJpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void DistanceConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + DistanceConstraintIqcJc::fillpFpydot(mat); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} + +double DistanceConstraintIqcJqc::constraintVelocity() const +{ + auto frameJ = std::static_pointer_cast(frmJ); + return DistanceConstraintIqcJc::constraintVelocity() + + pGpXJ->timesFullColumn(frameJ->qXdot()) + + pGpEJ->timesFullColumn(frameJ->qEdot()); +} + +void DistanceConstraintIqcJqc::fillGeneralizedForce(FColDsptr col, double multiplier) +{ + DistanceConstraintIqcJc::fillGeneralizedForce(col, multiplier); + col->atiplusFullVectortimes(iqXJ, pGpXJ, multiplier); + col->atiplusFullVectortimes(iqEJ, pGpEJ, multiplier); +} + +void DistanceConstraintIqcJqc::fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative +) +{ + DistanceConstraintIqcJc::fillGeneralizedForcePositionJacobian( + mat, multiplier, derivative + ); + const struct Hessian + { + size_t row; + size_t column; + FMatDsptr value; + } crossTerms[] = { + {iqXI, iqXJ, ppGpXIpXJ}, + {iqEI, iqXJ, ppGpEIpXJ}, + {iqXI, iqEJ, ppGpXIpEJ}, + {iqEI, iqEJ, ppGpEIpEJ}, + {iqXJ, iqEJ, ppGpXJpEJ}, + }; + for (const auto& term : crossTerms) { + auto hessian = term.value->times(multiplier); + mat->atijplusFullMatrix(term.row, term.column, hessian); + mat->atijplusTransposeFullMatrix(term.column, term.row, hessian); + } + mat->atijplusFullMatrixtimes(iqXJ, iqXJ, ppGpXJpXJ, multiplier); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, multiplier); + + const std::pair iSegments[] = {{iqXI, pGpXI}, {iqEI, pGpEI}}; + const std::pair jSegments[] = {{iqXJ, pGpXJ}, {iqEJ, pGpEJ}}; + for (const auto& i : iSegments) { + for (const auto& j : jSegments) { + addOuterProduct(mat, i.first, i.second, j.first, j.second, derivative); + addOuterProduct(mat, j.first, j.second, i.first, i.second, derivative); + } + } + for (const auto& row : jSegments) { + for (const auto& column : jSegments) { + addOuterProduct( + mat, + row.first, + row.second, + column.first, + column.second, + derivative + ); + } + } +} + +void DistanceConstraintIqcJqc::fillGeneralizedForceVelocityJacobian( + SpMatDsptr mat, + double derivative +) +{ + DistanceConstraintIqcJc::fillGeneralizedForceVelocityJacobian(mat, derivative); + const std::pair iSegments[] = {{iqXI, pGpXI}, {iqEI, pGpEI}}; + const std::pair jSegments[] = {{iqXJ, pGpXJ}, {iqEJ, pGpEJ}}; + for (const auto& i : iSegments) { + for (const auto& j : jSegments) { + addOuterProduct(mat, i.first, i.second, j.first, j.second, derivative); + addOuterProduct(mat, j.first, j.second, i.first, i.second, derivative); + } + } + for (const auto& row : jSegments) { + for (const auto& column : jSegments) { + addOuterProduct( + mat, + row.first, + row.second, + column.first, + column.second, + derivative + ); + } + } +} diff --git a/OndselSolver/DistanceConstraintIqcJqc.h b/OndselSolver/DistanceConstraintIqcJqc.h index 18987ceb..416bf694 100644 --- a/OndselSolver/DistanceConstraintIqcJqc.h +++ b/OndselSolver/DistanceConstraintIqcJqc.h @@ -15,6 +15,16 @@ namespace MbD { { //pGpXJ pGpEJ ppGpXIpXJ ppGpEIpXJ ppGpXJpXJ ppGpXIpEJ ppGpEIpEJ ppGpXJpEJ ppGpEJpEJ iqXJ iqEJ public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + double constraintVelocity() const override; + void fillGeneralizedForce(FColDsptr col, double multiplier) override; + void fillGeneralizedForcePositionJacobian( + SpMatDsptr mat, + double multiplier, + double derivative + ) override; + void fillGeneralizedForceVelocityJacobian(SpMatDsptr mat, double derivative) override; DistanceConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj); void calcPostDynCorrectorIteration() override; diff --git a/OndselSolver/DistanceLimitIJ.cpp b/OndselSolver/DistanceLimitIJ.cpp new file mode 100644 index 00000000..3fdd9ad4 --- /dev/null +++ b/OndselSolver/DistanceLimitIJ.cpp @@ -0,0 +1,27 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +#include "DistanceLimitIJ.h" + +#include "DistanceConstraintIJ.h" +#include "System.h" + +using namespace MbD; + +std::shared_ptr DistanceLimitIJ::With() +{ + auto result = std::make_shared(); + result->initialize(); + return result; +} + +void DistanceLimitIJ::initializeGlobally() +{ + if (constraints->empty()) { + auto constraint = DistanceConstraintIJ::With(frmI, frmJ); + constraint->setConstant(limit); + addConstraint(constraint); + root()->hasChanged = true; + } + else { + LimitIJ::initializeGlobally(); + } +} diff --git a/OndselSolver/DistanceLimitIJ.h b/OndselSolver/DistanceLimitIJ.h new file mode 100644 index 00000000..91917645 --- /dev/null +++ b/OndselSolver/DistanceLimitIJ.h @@ -0,0 +1,14 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +#pragma once + +#include "LimitIJ.h" + +namespace MbD +{ +class DistanceLimitIJ: public LimitIJ +{ +public: + static std::shared_ptr With(); + void initializeGlobally() override; +}; +} // namespace MbD diff --git a/OndselSolver/DistancexyConstraintIJ.cpp b/OndselSolver/DistancexyConstraintIJ.cpp index ea3c191d..bcf661b9 100644 --- a/OndselSolver/DistancexyConstraintIJ.cpp +++ b/OndselSolver/DistancexyConstraintIJ.cpp @@ -101,3 +101,31 @@ ConstraintType MbD::DistancexyConstraintIJ::type() { return displacement; } + +void DistancexyConstraintIJ::postDynPredictor() +{ + xIeJeIe->postDynPredictor(); + yIeJeIe->postDynPredictor(); + ConstraintIJ::postDynPredictor(); +} + +void DistancexyConstraintIJ::postDynCorrectorIteration() +{ + xIeJeIe->postDynCorrectorIteration(); + yIeJeIe->postDynCorrectorIteration(); + ConstraintIJ::postDynCorrectorIteration(); +} + +void DistancexyConstraintIJ::preDynOutput() +{ + xIeJeIe->preDynOutput(); + yIeJeIe->preDynOutput(); + ConstraintIJ::preDynOutput(); +} + +void DistancexyConstraintIJ::postDynOutput() +{ + xIeJeIe->postDynOutput(); + yIeJeIe->postDynOutput(); + ConstraintIJ::postDynOutput(); +} diff --git a/OndselSolver/DistancexyConstraintIJ.h b/OndselSolver/DistancexyConstraintIJ.h index 9ce87ab9..715e1731 100644 --- a/OndselSolver/DistancexyConstraintIJ.h +++ b/OndselSolver/DistancexyConstraintIJ.h @@ -16,6 +16,10 @@ namespace MbD { { //xIeJeIe yIeJeIe public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; DistancexyConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj); static std::shared_ptr With(EndFrmsptr frmi, EndFrmsptr frmj); diff --git a/OndselSolver/DistancexyConstraintIqcJc.cpp b/OndselSolver/DistancexyConstraintIqcJc.cpp index 18bb2aa1..d61465ad 100644 --- a/OndselSolver/DistancexyConstraintIqcJc.cpp +++ b/OndselSolver/DistancexyConstraintIqcJc.cpp @@ -150,3 +150,20 @@ void MbD::DistancexyConstraintIqcJc::useEquationNumbers() iqXI = frmIeqc->iqX(); iqEI = frmIeqc->iqE(); } + +void DistancexyConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullMatrixtimes(iqXI, iqXI, ppGpXIpXI, lam); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void DistancexyConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} diff --git a/OndselSolver/DistancexyConstraintIqcJc.h b/OndselSolver/DistancexyConstraintIqcJc.h index 6358e5f6..55ee2c9e 100644 --- a/OndselSolver/DistancexyConstraintIqcJc.h +++ b/OndselSolver/DistancexyConstraintIqcJc.h @@ -15,6 +15,8 @@ namespace MbD { { //pGpXI pGpEI ppGpXIpXI ppGpXIpEI ppGpEIpEI iqXI iqEI public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; DistancexyConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj); void addToJointForceI(FColDsptr col) override; diff --git a/OndselSolver/DistancexyConstraintIqcJqc.cpp b/OndselSolver/DistancexyConstraintIqcJqc.cpp index b7de8d2e..86849877 100644 --- a/OndselSolver/DistancexyConstraintIqcJqc.cpp +++ b/OndselSolver/DistancexyConstraintIqcJqc.cpp @@ -197,3 +197,34 @@ std::string MbD::DistancexyConstraintIqcJqc::constraintSpec() { return "DistancexyConstraintIJ"; } + +void DistancexyConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + DistancexyConstraintIqcJc::fillpFpy(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + auto ppGpXIpXJlam = ppGpXIpXJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqXJ, ppGpXIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqXI, ppGpXIpXJlam); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + mat->atijplusFullMatrixtimes(iqXJ, iqXJ, ppGpXJpXJ, lam); + auto ppGpXIpEJlam = ppGpXIpEJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqEJ, ppGpXIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXI, ppGpXIpEJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + auto ppGpXJpEJlam = ppGpXJpEJ->times(lam); + mat->atijplusFullMatrix(iqXJ, iqEJ, ppGpXJpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXJ, ppGpXJpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void DistancexyConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + DistancexyConstraintIqcJc::fillpFpydot(mat); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} diff --git a/OndselSolver/DistancexyConstraintIqcJqc.h b/OndselSolver/DistancexyConstraintIqcJqc.h index 7358d290..3715d2be 100644 --- a/OndselSolver/DistancexyConstraintIqcJqc.h +++ b/OndselSolver/DistancexyConstraintIqcJqc.h @@ -15,6 +15,8 @@ namespace MbD { { //pGpXJ pGpEJ ppGpXIpXJ ppGpEIpXJ ppGpXJpXJ ppGpXIpEJ ppGpEIpEJ ppGpXJpEJ ppGpEJpEJ iqXJ iqEJ public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; DistancexyConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj); void calc_pGpXJ(); diff --git a/OndselSolver/DynIntegrator.cpp b/OndselSolver/DynIntegrator.cpp new file mode 100644 index 00000000..9ff572b9 --- /dev/null +++ b/OndselSolver/DynIntegrator.cpp @@ -0,0 +1,431 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include +#include + +#include "DynIntegrator.h" +#include "BasicIntegrator.h" +#include "SystemSolver.h" +#include "Solver.h" +#include "Item.h" +#include "SingularMatrixError.h" +#include "SimulationStoppingError.h" +#include "TooSmallStepSizeError.h" +#include "TooManyTriesError.h" +#include "Constraint.h" +#include "Part.h" +#include "DiscontinuityError.h" +#include "BasicDAEIntegrator.h" +#include "System.h" + +using namespace MbD; + +std::shared_ptr DynIntegrator::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void DynIntegrator::assignEquationNumbers() +{ + //"Equation order is p,q,s,u,w,mubar,mu." + + auto parts = system->parts(); + //auto contactEndFrames = system->contactEndFrames(); + //auto uHolders = system->uHolders(); + auto constraints = system->allConstraints(); + ncon = constraints->size(); + auto eqnNo = 0; + for (auto& part : *parts) { + part->ipX = eqnNo; + eqnNo = eqnNo + 3; + part->ipE = eqnNo; + eqnNo = eqnNo + 4; + } + for (auto& part : *parts) { + part->iqX(eqnNo); + eqnNo = eqnNo + 3; + part->iqE(eqnNo); + eqnNo = eqnNo + 4; + } + //for (auto& endFrm : *contactEndFrames) { + // endFrm->is(eqnNo); + // eqnNo = eqnNo + endFrm->sSize(); + //} + //for (auto& uHolder : *uHolders) { + // uHolder->iu(eqnNo); + // eqnNo += 1; + //} + for (auto& con : *constraints) { + con->iG = eqnNo; + eqnNo += 1; + } + neqn = eqnNo; +} + +void DynIntegrator::checkForDiscontinuity() +{ + //"Check for discontinuity in (tpast,t] or [t,tpast) if integrating + //backward." + + auto t = integrator->t; + auto tprevious = integrator->tprevious(); + auto epsilon = std::numeric_limits::epsilon(); + double tstartNew; + if (direction == 0) { + tstartNew = epsilon; + } + else { + epsilon = std::abs(t) * epsilon; + tstartNew = ((direction * t) + epsilon) / direction; + } + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { tstartNew = item->checkForDynDiscontinuityBetweenand(tprevious, tstartNew); }); + if (auto events = system->system->dynamicEvents) { + const double end = std::min({t, tend, tstartNew}); + const double eventTime = events->locate(tprevious, end, [this](double sample) { interpolateAt(sample); }); + interpolateAt(t); + if (std::isfinite(eventTime) && eventTime <= end + events->tolerance) { + checkForOutputThrough(std::nextafter(eventTime, -std::numeric_limits::infinity())); + events->commit(tprevious, eventTime, [this](double sample) { interpolateAt(sample); }); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynStep(); }); + auto types = std::make_shared>(1, EVENT); + if (tstartNew <= t && std::abs(eventTime-tstartNew) <= events->tolerance) { + // Do not lose a joint-limit impact coinciding with an event. + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { + item->discontinuityAtaddTypeTo(tstartNew, types); + }); + } + events->apply(eventTime); + system->tstartPastsAddFirst(tstart); + system->tstart = eventTime; + system->toutFirst = tout <= eventTime + events->tolerance ? std::min(tout + hout, tend) : tout; + throwDiscontinuityError("Event boundary", types); + } + events->commit(tprevious, end, [this](double sample) { interpolateAt(sample); }); + interpolateAt(t); + } + if ((direction * tstartNew) > (direction * t)) { + //"No discontinuity in step" + return; + } + else { + checkForOutputThrough(tstartNew); + interpolateAt(tstartNew); + if (system->system->dynamicEvents) + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynStep(); }); + system->tstartPastsAddFirst(tstart); + system->tstart = tstartNew; + system->toutFirst = tout; + auto discontinuityTypes = std::make_shared>(); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->discontinuityAtaddTypeTo(tstartNew, discontinuityTypes); }); + throwDiscontinuityError("", discontinuityTypes); + } +} + +void DynIntegrator::checkForOutputThrough(double t) +{ + //"Inclusive of t." + + auto tlimit = std::min(t, tend); + auto thereIsOutput = false; + if (direction * tend <= (direction * tlimit)) { + integrator->_continue = false; + } + while (direction * tout <= (direction * tlimit)) { + thereIsOutput = true; + auto yout = integrator->yDerivat(0, tout); + auto ydotout = integrator->yDerivat(1, tout); + auto yddotout = integrator->yDerivat(2, tout); + system->time(tout); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { + item->setpqsumu(yout); + item->setpqsumudot(ydotout); + item->setpqsumuddot(yddotout); + }); + preDAEOutput(); + system->output(); + if (tout == tend) { + tout = std::numeric_limits::infinity(); + } + else { + const double next = tout + hout; + const double roundoff = 16*std::numeric_limits::epsilon() + * std::max({std::abs(tstart), std::abs(tend), std::abs(hout)}); + // Avoid a near-identical regular sample followed by the end sample. + tout = next >= tend-roundoff ? tend : next; + } + } + if (thereIsOutput) { + //"Reset system to integrator time." + system->time(integrator->t); + auto integ = std::static_pointer_cast(integrator); + auto& y = integ->y; + auto& ydot = integ->ydot; + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { + item->setpqsumu(y); + item->setpqsumudot(ydot); + }); + postDAEOutput(); + } +} + +void DynIntegrator::fillF(FColDsptr col) +{ + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->fillDynError(col); }); +} + +void DynIntegrator::fillpFpy(SpMatDsptr mat) +{ + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->fillpFpy(mat); }); +} + +void DynIntegrator::fillpFpydot(SpMatDsptr mat) +{ + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->fillpFpydot(mat); }); +} + +void DynIntegrator::preRun() +{ + std::string str("MbD: Starting dynamic analysis."); + system->logString(str); + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->preDyn(); }); +} + +void DynIntegrator::run() +{ + try { + try { + try { + DAEIntegrator::run(); + } + catch (const SingularMatrixError&) { + std::stringstream ss; + ss << "MbD: Solver has encountered a singular matrix." << std::endl; + ss << "MbD: Check to see if a massless or a very low mass part is under constrained." << std::endl; + ss << "MbD: Check to see if the system is in a locked position." << std::endl; + ss << "MbD: Check to see if the error tolerance is too demanding." << std::endl; + ss << "MbD: Check to see if a curve-curve is about to have multiple contact points." << std::endl; + auto str = ss.str(); + logString(str); + throw SimulationStoppingError(str); + } + } + catch (const TooSmallStepSizeError&) { + std::stringstream ss; + ss << "MbD: Step size is prevented from going below the user specified minimum." << std::endl; + ss << "MbD: Check to see if the system is in a locked position." << std::endl; + ss << "MbD: Check to see if a curve-curve is about to have multiple contact points." << std::endl; + ss << "MbD: If they are not, lower the permitted minimum step size." << std::endl; + auto str = ss.str(); + logString(str); + throw SimulationStoppingError(str); + } + } + catch (const TooManyTriesError&) { + std::stringstream ss; + ss << "MbD: Check to see if the error tolerance is too demanding." << std::endl; + auto str = ss.str(); + logString(str); + throw SimulationStoppingError(str); + } +} + +void DynIntegrator::fillY(FColDsptr y) +{ + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillpqsumu(y); }); +} + +void DynIntegrator::fillYdot(FColDsptr ydot) +{ + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->fillpqsumudot(ydot); }); +} + +void DynIntegrator::incrementTime(double aDouble) +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->storeDynState(); }); + system->time(aDouble); +} + +void DynIntegrator::interpolateAt(double t) +{ + auto yout = integrator->yDerivat(0, t); + auto ydotout = integrator->yDerivat(1, t); + system->time(t); + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { + item->setpqsumu(yout); + item->setpqsumudot(ydotout); + }); + system->partsJointsMotionsLimitsForcesTorquesDo( + [](std::shared_ptr item) { item->postDynPredictor(); } + ); +} + +size_t DynIntegrator::iterMax() +{ + return system->iterMaxDyn; +} + +void DynIntegrator::postDAECorrector() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynCorrector(); }); +} + +bool DynIntegrator::acceptTrial() const +{ + bool accepted = true; + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { + if (!item->acceptDynTrial()) accepted = false; + }); + return accepted; +} + +void DynIntegrator::postDAECorrectorIteration() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DynIntegrator::postDAEFirstStep() +{ + if (system->system->dynamicEvents) { + checkForDiscontinuity(); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynFirstStep(); }); + checkForOutputThrough(integrator->t); + return; + } + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynFirstStep(); }); + if (integrator->istep >= 0) { + //"Noise make checking at the start unreliable." + checkForDiscontinuity(); + } + checkForOutputThrough(integrator->t); +} + +void DynIntegrator::postDAEOutput() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynOutput(); }); +} + +double DynIntegrator::suggestSmallerOrAcceptFirstStepSize(double hnew) +{ + auto hnew2 = hnew; + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynFirstStepSize(hnew2); }); + hnew2 = std::min(hnew2, hmax); + if (hnew2 < hmin) throw TooSmallStepSizeError("Initial dynamics step is below the permitted minimum"); + return hnew2; +} + +double DynIntegrator::suggestSmallerOrAcceptStepSize(double hnew) +{ + auto hnew2 = hnew; + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { hnew2 = item->suggestSmallerOrAcceptDynStepSize(hnew2); }); + if (hnew2 > hmax) { + hnew2 = hmax; + system->logString("MbD: Step size is at user specified maximum."); + } + if (hnew2 < hmin) { + std::stringstream ss; + ss << "MbD: Step size " << hnew2 << " < " << hmin << " user specified minimum."; + auto str = ss.str(); + system->logString(str); + throw TooSmallStepSizeError(""); + } + return hnew2; +} + +void DynIntegrator::updateForDAECorrector() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynCorrectorIteration(); }); +} + +void DynIntegrator::y(FColDsptr col) +{ + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->setpqsumu(col); }); +} + +void DynIntegrator::ydot(FColDsptr col) +{ + system->partsJointsMotionsLimitsDo([&](std::shared_ptr item) { item->setpqsumudot(col); }); +} + +void DynIntegrator::throwDiscontinuityError(const std::string& str, std::shared_ptr> discontinuityTypes) +{ + throw DiscontinuityError(str, discontinuityTypes); +} + +void DynIntegrator::useTrialStepStats(std::shared_ptr stats) +{ + system->useDynTrialStepStats(stats); +} + +void DynIntegrator::useDAEStepStats(std::shared_ptr stats) +{ + //Do nothing. +} + +void DynIntegrator::reportStats() +{ + //Do nothing. +} + +void DynIntegrator::postDAEPredictor() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynPredictor(); }); +} + +void DynIntegrator::postDAEStep() +{ + if (system->system->dynamicEvents) { + checkForDiscontinuity(); + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynStep(); }); + checkForOutputThrough(integrator->t); + return; + } + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postDynStep(); }); + if (integrator->istep >= 0) { + //"Noise make checking at the start unreliable." + checkForDiscontinuity(); + } + checkForOutputThrough(integrator->t); +} + +void DynIntegrator::postRun() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([&](std::shared_ptr item) { item->postDyn(); }); +} + +void DynIntegrator::preDAECorrector() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->preDynCorrector(); }); +} + +void DynIntegrator::preDAECorrectorIteration() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DynIntegrator::preDAEFirstStep() +{ + throw SimulationStoppingError("To be implemented."); +} + +void DynIntegrator::preDAEOutput() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->preDynOutput(); }); +} + +void DynIntegrator::preDAEPredictor() +{ + system->partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->preDynPredictor(); }); +} + +void DynIntegrator::preDAEStep() +{ + throw SimulationStoppingError("To be implemented."); +} diff --git a/OndselSolver/DynIntegrator.h b/OndselSolver/DynIntegrator.h new file mode 100644 index 00000000..c96bb1f2 --- /dev/null +++ b/OndselSolver/DynIntegrator.h @@ -0,0 +1,62 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +//#include + +#include "DAEIntegrator.h" +#include "FullMatrix.h" +#include "enum.h" + +namespace MbD { + class DynIntegrator : public DAEIntegrator + { + // + public: + static std::shared_ptr With(); + + void assignEquationNumbers() override; + void checkForDiscontinuity() override; + void checkForOutputThrough(double t) override; + void fillF(FColDsptr vecF) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + void fillY(FColDsptr y); + void fillYdot(FColDsptr ydot); + void incrementTime(double t) override; + void interpolateAt(double t) override; + size_t iterMax() override; + void postDAECorrector(); + bool acceptTrial() const; + void postDAECorrectorIteration(); + void postDAEFirstStep(); + void postDAEOutput() override; + void postDAEPredictor(); + void postDAEStep(); + void postRun() override; + void preDAECorrector(); + void preDAECorrectorIteration(); + void preDAEFirstStep(); + void preDAEOutput() override; + void preDAEPredictor(); + void preDAEStep(); + void preRun() override; + void run() override; + double suggestSmallerOrAcceptFirstStepSize(double hnew) override; + double suggestSmallerOrAcceptStepSize(double hnew) override; + void updateForDAECorrector(); + void y(FColDsptr col) override; + void ydot(FColDsptr col) override; + void throwDiscontinuityError(const std::string& str, std::shared_ptr> discontinuityTypes); + void useTrialStepStats(std::shared_ptr stats); + void useDAEStepStats(std::shared_ptr stats); + void reportStats() override; + + }; +} diff --git a/OndselSolver/DynamicEvents.h b/OndselSolver/DynamicEvents.h new file mode 100644 index 00000000..64cdb871 --- /dev/null +++ b/OndselSolver/DynamicEvents.h @@ -0,0 +1,208 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace MbD { +// A run-local event scheduler. Geometry/measurement and action adapters belong +// to the caller. Probing dense output never commits an action or changes state. +class DynamicEvents { +public: + struct Event { + enum Trigger { Time, Threshold, After } trigger = Time; + std::string name; + double time = 0, threshold = 0, hysteresis = 0, delay = 0; + size_t predecessor = 0; + bool rising = true, repeat = false, fireInitially = false; + std::function measure; + std::function action; + bool fired = false, armed = true; + double lastFire = -std::numeric_limits::infinity(); + std::deque scheduledTimes; + }; + struct Firing { size_t event; double time; }; + std::vector events; + std::vector firings; + std::function prepare; + double tolerance = 1e-9; + + bool initialize(double time) { + pending.clear(); + firings.clear(); + beforeActions.clear(); + if (!std::isfinite(tolerance) || tolerance <= 0) + throw std::invalid_argument("Event tolerance must be finite and positive"); + for (auto& event : events) { + if (!event.action || !std::isfinite(event.time) || !std::isfinite(event.delay) + || event.delay < 0 || !std::isfinite(event.threshold) + || !std::isfinite(event.hysteresis) || event.hysteresis < 0 + || (event.trigger == Event::Threshold && !event.measure) + || (event.trigger == Event::After && event.predecessor >= events.size())) + throw std::invalid_argument("Invalid event: " + event.name); + event.fired = false; + event.armed = true; + event.lastFire = -std::numeric_limits::infinity(); + event.scheduledTimes.clear(); + } + for (size_t i = 0; i < events.size(); ++i) { + auto& event = events[i]; + if (event.trigger == Event::Threshold) { + const double v = measure(event); + event.armed = !satisfied(event, v); + if (event.fireInitially && satisfied(event, v)) pending.push_back(i); + } + else if (event.trigger == Event::Time && std::abs(event.time-time) <= tolerance) { + pending.push_back(i); + } + } + if (pending.empty()) return false; + apply(time); + return true; + } + + // The step is converged, but not yet committed. At most eight equal probe + // intervals are searched; fast oscillatory signals require a smaller hmax. + double locate(double previous, double current, const std::function& sample) { + pending.clear(); + double earliest = std::numeric_limits::infinity(); + for (size_t i = 0; i < events.size(); ++i) { + const auto& event = events[i]; + if (event.fired && !event.repeat) continue; + double candidate = std::numeric_limits::infinity(); + if (event.trigger != Event::Threshold) { + candidate = scheduled(event); + if (candidate <= previous+tolerance || candidate > current+tolerance) continue; + } + else { + bool armed = event.armed; + sample(previous); + double left = previous, vleft = measure(event); + for (int probe = 1; probe <= 8; ++probe) { + const double right = previous+(current-previous)*probe/8; + sample(right); + const double vright = measure(event); + if (!armed && rearmed(event, vleft)) armed = true; + if (armed && !satisfied(event, vleft) && satisfied(event, vright)) { + double lo = left, hi = right; + for (int iter = 0; iter < 60 && hi-lo > tolerance; ++iter) { + const double mid = (lo+hi)/2; + sample(mid); + if (satisfied(event, measure(event))) hi = mid; + else lo = mid; + } + candidate = hi; + break; + } + left = right; + vleft = vright; + } + } + if (!std::isfinite(candidate)) continue; + if (candidate < earliest-tolerance) { + earliest = candidate; + pending.clear(); + } + if (std::abs(candidate-earliest) <= tolerance) pending.push_back(i); + } + sample(current); + return earliest; + } + + void commit() { + for (auto& event : events) { + if (event.trigger == Event::Threshold && !event.armed && rearmed(event, measure(event))) + event.armed = true; + } + } + + // Rearming can occur inside a step, even if its endpoint has already + // returned into the hysteresis band. Commit only the accepted interval. + void commit(double previous, double current, const std::function& sample) { + for (int probe = 0; probe <= 8; ++probe) { + sample(previous + (current-previous)*probe/8); + commit(); + } + } + + void apply(double time) { + beforeActions.clear(); + for (const auto& event : events) + beforeActions.push_back(event.trigger == Event::Threshold ? measure(event) : 0.0); + auto batch = pending; + pending.clear(); + for (size_t pass = 0; !batch.empty(); ++pass) { + if (pass > events.size() || firings.size()+batch.size() > 10000) + throw std::runtime_error("Event cascade exceeds the safe execution limit"); + for (auto index : batch) { + auto& event = events.at(index); + event.action(time); + event.fired = true; + event.armed = false; + event.lastFire = time; + if (event.trigger == Event::After && !event.scheduledTimes.empty()) + event.scheduledTimes.pop_front(); + firings.push_back({index, time}); + // Preserve each pending occurrence. A later predecessor firing + // must not reset an already-running delay. + for (auto& dependent : events) { + if (dependent.trigger == Event::After && dependent.predecessor == index + && (dependent.repeat || (!dependent.fired && dependent.scheduledTimes.empty()))) + dependent.scheduledTimes.push_back(time + dependent.delay); + } + } + batch.clear(); + for (size_t i = 0; i < events.size(); ++i) { + const auto& event = events[i]; + if (event.trigger == Event::After && (!event.fired || event.repeat) + && std::abs(scheduled(event)-time) <= tolerance) + batch.push_back(i); + } + } + commit(); + } + + // Constraint activation may change velocity instantaneously during IC. + // Detect those crossings after projection, before advancing physical time. + bool settle(double time) { + pending.clear(); + if (beforeActions.size() != events.size()) return false; + for (size_t i = 0; i < events.size(); ++i) { + const auto& event = events[i]; + if (event.trigger == Event::Threshold && event.armed + && (!event.fired || event.repeat) && !satisfied(event, beforeActions[i]) + && satisfied(event, measure(event))) pending.push_back(i); + } + beforeActions.clear(); + if (pending.empty()) { commit(); return false; } + apply(time); + return true; + } + +private: + std::vector pending; + std::vector beforeActions; + static double measure(const Event& event) { + const double result = event.measure(); + if (!std::isfinite(result)) throw std::runtime_error("Non-finite event measurement: "+event.name); + return result; + } + static bool satisfied(const Event& e, double value) { + return e.rising ? value >= e.threshold : value <= e.threshold; + } + static bool rearmed(const Event& e, double value) { + return e.rising ? value < e.threshold-e.hysteresis : value > e.threshold+e.hysteresis; + } + double scheduled(const Event& event) const { + if (event.trigger == Event::Time) return event.fired ? std::numeric_limits::infinity() : event.time; + return event.scheduledTimes.empty() ? std::numeric_limits::infinity() + : event.scheduledTimes.front(); + } +}; +} diff --git a/OndselSolver/EndFrameqct.cpp b/OndselSolver/EndFrameqct.cpp index a898d2e2..fca7a2d2 100644 --- a/OndselSolver/EndFrameqct.cpp +++ b/OndselSolver/EndFrameqct.cpp @@ -366,3 +366,27 @@ bool MbD::EndFrameqct::isEndFrameqc() { return false; } + +void EndFrameqct::postDynPredictor() +{ + time = root()->mbdTimeValue(); + evalrmem(); + evalAme(); + EndFrameqc::postDynPredictor(); +} + +void EndFrameqct::preDynOutput() +{ + time = root()->mbdTimeValue(); + evalrmem(); + evalAme(); + EndFrameqc::preDynOutput(); +} + +void EndFrameqct::postDynOutput() +{ + time = root()->mbdTimeValue(); + evalrmem(); + evalAme(); + EndFrameqc::postDynOutput(); +} diff --git a/OndselSolver/EndFrameqct.h b/OndselSolver/EndFrameqct.h index 924a8343..849623ca 100644 --- a/OndselSolver/EndFrameqct.h +++ b/OndselSolver/EndFrameqct.h @@ -20,6 +20,9 @@ namespace MbD { //time rmemBlks prmemptBlks pprmemptptBlks phiThePsiBlks pPhiThePsiptBlks ppPhiThePsiptptBlks //rmem prmempt pprmemptpt aAme pAmept ppAmeptpt prOeOpt pprOeOpEpt pprOeOptpt pAOept ppAOepEpt ppAOeptpt public: + void postDynPredictor() override; + void preDynOutput() override; + void postDynOutput() override; EndFrameqct(); EndFrameqct(const std::string& str); void initialize() override; diff --git a/OndselSolver/EulerConstraint.cpp b/OndselSolver/EulerConstraint.cpp index 23d5b5a2..b25ceb5b 100644 --- a/OndselSolver/EulerConstraint.cpp +++ b/OndselSolver/EulerConstraint.cpp @@ -88,3 +88,20 @@ std::string MbD::EulerConstraint::constraintSpec() { return "EulerConstraint"; } + +void EulerConstraint::fillpFpy(SpMatDsptr mat) +{ + //"ppGpEpE is a diag(2,2,2,2)." + mat->atijplusFullRow(iG, iqE, pGpE); + auto twolam = 2.0 * lam; + for (size_t i = 0; i < 4; i++) + { + auto ii = iqE + i; + mat->atijplusNumber(ii, ii, twolam); + } +} + +void EulerConstraint::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqE, iG, pGpE->transpose()); +} diff --git a/OndselSolver/EulerConstraint.h b/OndselSolver/EulerConstraint.h index bd6a9c33..75f1c1a8 100644 --- a/OndselSolver/EulerConstraint.h +++ b/OndselSolver/EulerConstraint.h @@ -20,6 +20,8 @@ namespace MbD { { //pGpE iqE public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; EulerConstraint(); EulerConstraint(const std::string& str); void initialize() override; diff --git a/OndselSolver/Extrapolator.cpp b/OndselSolver/Extrapolator.cpp new file mode 100644 index 00000000..72551188 --- /dev/null +++ b/OndselSolver/Extrapolator.cpp @@ -0,0 +1,36 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include "Extrapolator.h" +#include "FullColumn.h" + +using namespace MbD; + +std::shared_ptr Extrapolator::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void Extrapolator::formTaylorMatrix() +{ + //" + //For method order 3: + //| 1 (t1 - t) (t1 - t)^2/2! (t1 - t)^3/3! | |q(t) | = |q(t1) | + //| 1 (t2 - t) (t2 - t)^2/2! (t2 - t)^3/3! | |qd(t) | |q(t2) | + //| 1 (t3 - t) (t3 - t)^2/2! (t3 - t)^3/3! | |qdd(t) | |q(t3) | + //| 1 (t4 - t) (t4 - t)^2/2! (t4 - t)^3/3! | |qddd(t)| |q(t4) | + //" + + instantiateTaylorMatrix(); + for (size_t i = 0; i < order + 1; i++) + { + formTaylorRowwithTimeNodederivative(i, i, 0); + } +} diff --git a/OndselSolver/Extrapolator.h b/OndselSolver/Extrapolator.h new file mode 100644 index 00000000..0f34b366 --- /dev/null +++ b/OndselSolver/Extrapolator.h @@ -0,0 +1,24 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "DifferenceOperator.h" + +namespace MbD { + class Extrapolator : public DifferenceOperator + { + // + public: + static std::shared_ptr With(); + + void formTaylorMatrix() override; + + + }; +} diff --git a/OndselSolver/FullRow.h b/OndselSolver/FullRow.h index 54ff9f4d..f0f51fcf 100644 --- a/OndselSolver/FullRow.h +++ b/OndselSolver/FullRow.h @@ -186,7 +186,7 @@ namespace MbD { { auto ncol = this->size(); auto nelem = vecvec->at(0)->size(); - auto answer = std::make_shared>(nelem); + auto answer = std::make_shared>(nelem); for (size_t k = 0; k < nelem; k++) { auto sum = 0.0; for (size_t i = 0; i < ncol; i++) diff --git a/OndselSolver/GearConstraintIJ.cpp b/OndselSolver/GearConstraintIJ.cpp index 02e777a7..f488c2c4 100644 --- a/OndselSolver/GearConstraintIJ.cpp +++ b/OndselSolver/GearConstraintIJ.cpp @@ -102,3 +102,31 @@ void MbD::GearConstraintIJ::simUpdateAll() orbitJeIe->simUpdateAll(); ConstraintIJ::simUpdateAll(); } + +void GearConstraintIJ::postDynPredictor() +{ + orbitIeJe->postDynPredictor(); + orbitJeIe->postDynPredictor(); + ConstraintIJ::postDynPredictor(); +} + +void GearConstraintIJ::postDynCorrectorIteration() +{ + orbitIeJe->postDynCorrectorIteration(); + orbitJeIe->postDynCorrectorIteration(); + ConstraintIJ::postDynCorrectorIteration(); +} + +void GearConstraintIJ::preDynOutput() +{ + orbitIeJe->preDynOutput(); + orbitJeIe->preDynOutput(); + ConstraintIJ::preDynOutput(); +} + +void GearConstraintIJ::postDynOutput() +{ + orbitIeJe->postDynOutput(); + orbitJeIe->postDynOutput(); + ConstraintIJ::postDynOutput(); +} diff --git a/OndselSolver/GearConstraintIJ.h b/OndselSolver/GearConstraintIJ.h index a4e769b3..2b010c1a 100644 --- a/OndselSolver/GearConstraintIJ.h +++ b/OndselSolver/GearConstraintIJ.h @@ -16,6 +16,10 @@ namespace MbD { { //orbitIeJe orbitJeIe radiusI radiusJ public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; GearConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj); static std::shared_ptr With(EndFrmsptr frmi, EndFrmsptr frmj); diff --git a/OndselSolver/GearConstraintIqcJc.cpp b/OndselSolver/GearConstraintIqcJc.cpp index b5c4b246..fbd89e50 100644 --- a/OndselSolver/GearConstraintIqcJc.cpp +++ b/OndselSolver/GearConstraintIqcJc.cpp @@ -137,3 +137,20 @@ void MbD::GearConstraintIqcJc::useEquationNumbers() iqXI = frmIeqc->iqX(); iqEI = frmIeqc->iqE(); } + +void GearConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); + mat->atijplusFullMatrixtimes(iqXI, iqXI, ppGpXIpXI, lam); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void GearConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} diff --git a/OndselSolver/GearConstraintIqcJc.h b/OndselSolver/GearConstraintIqcJc.h index 99f64fa8..33120433 100644 --- a/OndselSolver/GearConstraintIqcJc.h +++ b/OndselSolver/GearConstraintIqcJc.h @@ -15,6 +15,8 @@ namespace MbD { { //pGpXI pGpEI ppGpXIpXI ppGpXIpEI ppGpEIpEI iqXI iqEI public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; GearConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj); void addToJointForceI(FColDsptr col) override; diff --git a/OndselSolver/GearConstraintIqcJqc.cpp b/OndselSolver/GearConstraintIqcJqc.cpp index 571d6e97..6ac7a988 100644 --- a/OndselSolver/GearConstraintIqcJqc.cpp +++ b/OndselSolver/GearConstraintIqcJqc.cpp @@ -167,3 +167,34 @@ std::string MbD::GearConstraintIqcJqc::constraintSpec() { return "GearConstraintIJ"; } + +void GearConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + GearConstraintIqcJc::fillpFpy(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + auto ppGpXIpXJlam = ppGpXIpXJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqXJ, ppGpXIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqXI, ppGpXIpXJlam); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + mat->atijplusFullMatrixtimes(iqXJ, iqXJ, ppGpXJpXJ, lam); + auto ppGpXIpEJlam = ppGpXIpEJ->times(lam); + mat->atijplusFullMatrix(iqXI, iqEJ, ppGpXIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXI, ppGpXIpEJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + auto ppGpXJpEJlam = ppGpXJpEJ->times(lam); + mat->atijplusFullMatrix(iqXJ, iqEJ, ppGpXJpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqXJ, ppGpXJpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void GearConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + GearConstraintIqcJc::fillpFpydot(mat); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} diff --git a/OndselSolver/GearConstraintIqcJqc.h b/OndselSolver/GearConstraintIqcJqc.h index d2eaecbf..3d488fc1 100644 --- a/OndselSolver/GearConstraintIqcJqc.h +++ b/OndselSolver/GearConstraintIqcJqc.h @@ -15,6 +15,8 @@ namespace MbD { { //pGpXJ pGpEJ ppGpXIpXJ ppGpXIpEJ ppGpEIpXJ ppGpEIpEJ ppGpXJpXJ ppGpXJpEJ ppGpEJpEJ iqXJ iqEJ public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; GearConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj); void calc_pGpEJ(); diff --git a/OndselSolver/Integrator.cpp b/OndselSolver/Integrator.cpp index 2504e0fd..94f534a6 100644 --- a/OndselSolver/Integrator.cpp +++ b/OndselSolver/Integrator.cpp @@ -7,9 +7,85 @@ ***************************************************************************/ #include "Integrator.h" +#include +#include "SimulationStoppingError.h" using namespace MbD; +std::shared_ptr Integrator::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + void Integrator::setSystem(Solver*) { + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::run() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::firstStep() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::preFirstStep() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::postFirstStep() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::subsequentSteps() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::nextStep() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::preStep() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::postStep() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::runInitialConditionTypeSolution() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::iStep(size_t i) +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::selectOrder() +{ + throw SimulationStoppingError("To be implemented."); +} + +void Integrator::selectStepSize() +{ + throw SimulationStoppingError("To be implemented."); +} + +size_t Integrator::iterMax() +{ + throw SimulationStoppingError("To be implemented."); + return 0; } diff --git a/OndselSolver/Integrator.h b/OndselSolver/Integrator.h index 4ebdc1db..0bd4771b 100644 --- a/OndselSolver/Integrator.h +++ b/OndselSolver/Integrator.h @@ -18,17 +18,22 @@ namespace MbD { { //system direction public: + static std::shared_ptr With(); + void setSystem(Solver* sys) override; - virtual void firstStep() = 0; - virtual void preFirstStep() = 0; - virtual void postFirstStep() = 0; - virtual void subsequentSteps() = 0; - virtual void nextStep() = 0; - virtual void preStep() = 0; - virtual void postStep() = 0; - virtual void runInitialConditionTypeSolution() = 0; - virtual void iStep(size_t i) = 0; - virtual void selectOrder() = 0; + void run() override; + virtual void firstStep(); + virtual void preFirstStep(); + virtual void postFirstStep(); + virtual void subsequentSteps(); + virtual void nextStep(); + virtual void preStep(); + virtual void postStep(); + virtual void runInitialConditionTypeSolution(); + virtual void iStep(size_t i); + virtual void selectOrder(); + virtual void selectStepSize(); + virtual size_t iterMax(); double direction = 1; }; diff --git a/OndselSolver/IntegratorInterface.cpp b/OndselSolver/IntegratorInterface.cpp index 5bafd424..e02a6e50 100644 --- a/OndselSolver/IntegratorInterface.cpp +++ b/OndselSolver/IntegratorInterface.cpp @@ -7,33 +7,153 @@ ***************************************************************************/ #include +#include #include "IntegratorInterface.h" #include "SystemSolver.h" #include "BasicQuasiIntegrator.h" -#include "SimulationStoppingError.h" +#include "NormalBasicDAEIntegrator.h" +#include "StartingBasicDAEIntegrator.h" using namespace MbD; +std::shared_ptr IntegratorInterface::With() +{ + //Should not create abstract class. + throw SimulationStoppingError("To be implemented."); + return std::shared_ptr(); +} + void IntegratorInterface::initializeGlobally() { - tstart = system->startTime(); - hout = system->outputStepSize(); - hmax = system->maxStepSize(); - hmin = system->minStepSize(); - tout = system->firstOutputTime(); - tend = system->endTime(); - direction = (tstart < tend) ? 1.0 : -1.0; + tstart = system->startTime(); + hout = system->outputStepSize(); + hmax = system->maxStepSize(); + hmin = system->minStepSize(); + tout = system->firstOutputTime(); + tend = system->endTime(); + direction = (tstart < tend) ? 1.0 : -1.0; +} + +void IntegratorInterface::preRun() +{ + //Subclasses must implement. + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::checkForDiscontinuity() +{ + //Subclasses must implement. + throw SimulationStoppingError("To be implemented."); } void IntegratorInterface::setSystem(Solver* sys) { - system = static_cast(sys); + system = static_cast(sys); } void IntegratorInterface::logString(const std::string& str) { - system->logString(str); + system->logString(str); +} + +size_t IntegratorInterface::orderMax() const +{ + return system->orderMax; +} + +void IntegratorInterface::incrementTime(double tnew) +{ + system->settime(tnew); +} + +void IntegratorInterface::postFirstStep() +{ + throw SimulationStoppingError("To be implemented."); +} + +double IntegratorInterface::suggestSmallerOrAcceptFirstStepSize(double hnew) +{ + //Subclasses must implement. + throw SimulationStoppingError("To be implemented."); + return 0.0; +} + +double IntegratorInterface::suggestSmallerOrAcceptStepSize(double hnew) +{ + //Subclasses must implement. + throw SimulationStoppingError("To be implemented."); + return 0.0; +} + +void IntegratorInterface::checkForOutputThrough(double t) +{ + //Subclasses must implement. + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::interpolateAt(double tArg) +{ + //"Interpolate for system state at tArg and leave system in that state." + throw SimulationStoppingError("To be implemented."); + //auto yout = integrator->yDerivat(0, tArg); + //auto ydotout = integrator->yDerivat(1, tArg); + //auto yddotout = integrator->yDerivat(2, tArg); + //system->time(tArg); + //system->y(yout); + //system->ydot(ydotout); + //system->yddot(yddotout); + //system->simUpdateAll(); +} + +void IntegratorInterface::fillF(FColDsptr vecF) +{ + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::fillpFpy(SpMatDsptr mat) +{ + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::fillpFpydot(SpMatDsptr mat) +{ + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::changeTime(double t) +{ + system->settime(t); +} + +void IntegratorInterface::y(FColDsptr col) +{ + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::ydot(FColDsptr col) +{ + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::updateForDAECorrector() +{ + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::useTrialStepStats(std::shared_ptr stats) +{ + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::useDAEStepStats(std::shared_ptr stats) +{ + throw SimulationStoppingError("To be implemented."); +} + +void IntegratorInterface::useQuasiStepStats(std::shared_ptr stats) +{ + throw SimulationStoppingError("To be implemented."); } void IntegratorInterface::run() @@ -48,38 +168,3 @@ void IntegratorInterface::run() this->reportStats(); this->postRun(); } - -size_t IntegratorInterface::orderMax() -{ - return system->orderMax; -} - -void IntegratorInterface::incrementTime(double tnew) -{ - system->settime(tnew); -} - -void IntegratorInterface::postFirstStep() -{ - throw SimulationStoppingError("To be implemented."); //Not used. - //system->postFirstStep(); - //if (integrator->istep > 0) { - // //"Noise make checking at the start unreliable." - // this->checkForDiscontinuity(); - //} - //this->checkForOutputThrough(integrator->t); -} - -void IntegratorInterface::interpolateAt(double) -{ - //"Interpolate for system state at tArg and leave system in that state." - throw SimulationStoppingError("To be implemented."); - //auto yout = integrator->yDerivat(0, tArg); - //auto ydotout = integrator->yDerivat(1, tArg); - //auto yddotout = integrator->yDerivat(2, tArg); - //system->time(tArg); - //system->y(yout); - //system->ydot(ydotout); - //system->yddot(yddotout); - //system->simUpdateAll(); -} diff --git a/OndselSolver/IntegratorInterface.h b/OndselSolver/IntegratorInterface.h index 77a801f3..ed84c3e6 100644 --- a/OndselSolver/IntegratorInterface.h +++ b/OndselSolver/IntegratorInterface.h @@ -9,35 +9,49 @@ #pragma once #include "Integrator.h" -//#include "BasicQuasiIntegrator.h" +#include "FullColumn.h" +#include "SparseMatrix.h" namespace MbD { - class BasicQuasiIntegrator; + class BasicIntegrator; + class StartingBasicDAEIntegrator; + class NormalBasicDAEIntegrator; class IntegratorInterface : public Integrator { //tout hout hmin hmax tstart tend integrator public: + static std::shared_ptr With(); void initializeGlobally() override; - virtual void preRun() override = 0; - virtual void checkForDiscontinuity() = 0; + void run() override; + virtual void preRun() override; + virtual void checkForDiscontinuity(); void setSystem(Solver* sys) override; void logString(const std::string& str) override; - void run() override; - size_t orderMax(); + size_t orderMax() const; virtual void incrementTime(double tnew); void postFirstStep() override; - virtual double suggestSmallerOrAcceptFirstStepSize(double hnew) = 0; - virtual double suggestSmallerOrAcceptStepSize(double hnew) = 0; - virtual void checkForOutputThrough(double t) = 0; + virtual double suggestSmallerOrAcceptFirstStepSize(double hnew); + virtual double suggestSmallerOrAcceptStepSize(double hnew); + virtual void checkForOutputThrough(double t); virtual void interpolateAt(double t); + virtual void fillF(FColDsptr vecF); + virtual void fillpFpy(SpMatDsptr mat); + virtual void fillpFpydot(SpMatDsptr mat); + void changeTime(double t); + virtual void y(FColDsptr col); + virtual void ydot(FColDsptr col); + virtual void updateForDAECorrector(); + virtual void useTrialStepStats(std::shared_ptr stats); + virtual void useDAEStepStats(std::shared_ptr stats); + virtual void useQuasiStepStats(std::shared_ptr stats); SystemSolver* system = nullptr; double tout = 0.0, hout = 0.0, hmin = 0.0, hmax = 0.0, tstart = 0.0, tend = 0.0; - std::shared_ptr integrator; + std::shared_ptr integrator; }; } diff --git a/OndselSolver/Item.cpp b/OndselSolver/Item.cpp index 994a2536..e960f460 100644 --- a/OndselSolver/Item.cpp +++ b/OndselSolver/Item.cpp @@ -101,19 +101,19 @@ void Item::removeRedundantConstraints(std::shared_ptr>) { } -void MbD::Item::setpqsumu(FColDsptr) +void Item::setpqsumu(FColDsptr) { - throw SimulationStoppingError("To be implemented."); + //Do nothing. } -void MbD::Item::setpqsumuddot(FColDsptr) +void Item::setpqsumuddot(FColDsptr) { - throw SimulationStoppingError("To be implemented."); + //Do nothing. } -void MbD::Item::setpqsumudot(FColDsptr) +void Item::setpqsumudot(FColDsptr) { - throw SimulationStoppingError("To be implemented."); + //Do nothing. } void Item::reactivateRedundantConstraints() @@ -133,14 +133,14 @@ void Item::fillPosKineJacob(SpMatDsptr) { } -void MbD::Item::fillpqsumu(FColDsptr) +void Item::fillpqsumu(FColDsptr) { - throw SimulationStoppingError("To be implemented."); + //Do nothing. } -void MbD::Item::fillpqsumudot(FColDsptr) +void Item::fillpqsumudot(FColDsptr) { - throw SimulationStoppingError("To be implemented."); + //Do nothing. } void Item::fillEssenConstraints(std::shared_ptr>>) @@ -153,14 +153,14 @@ void MbD::Item::fillPerpenConstraints(std::shared_ptr>>) @@ -187,9 +187,9 @@ void MbD::Item::fillDispConstraints(std::shared_ptrpreDynStep(); } -void MbD::Item::preDynOutput() +void Item::preDynOutput() { - throw SimulationStoppingError("To be implemented."); + //"Calculate all instance variables just before output." + calcPostDynCorrectorIteration(); } void MbD::Item::preDynPredictor() { - throw SimulationStoppingError("To be implemented."); + // Default: no predictor preparation is required. } void Item::postDynFirstStep() @@ -281,14 +285,23 @@ void Item::postDynFirstStep() this->postDynStep(); } -void MbD::Item::postDynOutput() +void Item::postDynOutput() { - throw SimulationStoppingError("To be implemented."); + //"Calculate all instance variables just after output." + calcPostDynCorrectorIteration(); } -void MbD::Item::postDynPredictor() +void Item::postDynPredictor() { - throw SimulationStoppingError("To be implemented."); + //"Called after the predictor stage in the dynamic solution." + //"Update only instance variables dependent on p,q,s,u,mu,pdot,qdot,sdot,udot,mudot (lam) + //that are needed for the corrector stage." + //"Needless updating can be expensive here." + //"This is a subset of update." + //"Default is do nothing." + //"updateInSimulation is the interface to the old system." + + calcPostDynCorrectorIteration(); } void Item::preDynStep() diff --git a/OndselSolver/Item.h b/OndselSolver/Item.h index 2ebcf4ac..4a507e84 100644 --- a/OndselSolver/Item.h +++ b/OndselSolver/Item.h @@ -118,6 +118,9 @@ namespace MbD { virtual void preDynOutput(); virtual void preDynPredictor(); virtual void preDynStep(); + // A converged trial may still cross a force discontinuity. Returning + // false retries it at a smaller step without committing any history. + virtual bool acceptDynTrial() const { return true; } virtual void preICRestart(); virtual void prePosIC(); virtual void prePosKine(); diff --git a/OndselSolver/LimitIJ.cpp b/OndselSolver/LimitIJ.cpp index 7b499cc9..61df4a9d 100644 --- a/OndselSolver/LimitIJ.cpp +++ b/OndselSolver/LimitIJ.cpp @@ -1,8 +1,42 @@ #include "LimitIJ.h" #include "Constraint.h" +#include +#include + using namespace MbD; +namespace { +struct ContactForce +{ + double multiplier = 0.0; + double positionDerivative = 0.0; + double velocityDerivative = 0.0; +}; + +ContactForce contactForce(const MbD::LimitIJ& limit) +{ + const auto& constraint = limit.constraints->front(); + const double normal = limit.type == "=>" ? 1.0 : limit.type == "=<" ? -1.0 : 0.0; + if (normal == 0.0) { + throw MbD::SimulationStoppingError("Unknown joint-limit comparison type"); + } + + const double penetration = -normal * constraint->aG; + if (penetration <= 0.0) return {}; + + const double penetrationVelocity = -normal * constraint->constraintVelocity(); + const double magnitude = limit.stiffness * penetration + limit.damping * penetrationVelocity; + if (magnitude <= 0.0) return {}; + + return { + normal * magnitude, + -limit.stiffness, + -limit.damping, + }; +} +} + bool MbD::LimitIJ::satisfied() const { auto& constraint = constraints->front(); @@ -26,6 +60,37 @@ void MbD::LimitIJ::activate() active = true; } +void MbD::LimitIJ::setCompliant(double newStiffness, double newDamping) +{ + if (!(newStiffness > 0.0) || newDamping < 0.0 + || !std::isfinite(newStiffness) || !std::isfinite(newDamping)) { + throw SimulationStoppingError("A compliant joint limit needs positive stiffness and nonnegative damping."); + } + compliant = true; + active = false; + stiffness = newStiffness; + damping = newDamping; +} + +MbD::LimitIJ::EnergyState MbD::LimitIJ::energyState() const +{ + if (!compliant) return {}; + const auto& constraint = constraints->front(); + const double normal = type == "=>" ? 1.0 : type == "=<" ? -1.0 : 0.0; + if (normal == 0.0) throw SimulationStoppingError("Unknown joint-limit comparison type"); + const double penetration = -normal * constraint->aG; + if (penetration <= 0.0) return {}; + const double penetrationVelocity = -normal * constraint->constraintVelocity(); + const double magnitude = stiffness * penetration + damping * penetrationVelocity; + if (magnitude <= 0.0) return {0.0, 0.5 * stiffness * penetration * penetration, 0.0}; + const double dissipated = damping * penetrationVelocity * penetrationVelocity; + return { + -stiffness * penetration * penetrationVelocity - dissipated, + 0.5 * stiffness * penetration * penetration, + dissipated + }; +} + void MbD::LimitIJ::fillConstraints(std::shared_ptr>> allConstraints) { if (active) { @@ -33,6 +98,27 @@ void MbD::LimitIJ::fillConstraints(std::shared_ptr>> allConstraints +) +{ + if (active) ConstraintSet::fillDispConstraints(allConstraints); +} + +void MbD::LimitIJ::fillEssenConstraints( + std::shared_ptr>> allConstraints +) +{ + if (active) ConstraintSet::fillEssenConstraints(allConstraints); +} + +void MbD::LimitIJ::fillPerpenConstraints( + std::shared_ptr>> allConstraints +) +{ + if (active) ConstraintSet::fillPerpenConstraints(allConstraints); +} + void MbD::LimitIJ::fillPosICError(FColDsptr col) { if (active) { @@ -47,6 +133,84 @@ void MbD::LimitIJ::fillPosICJacob(SpMatDsptr mat) } } +void MbD::LimitIJ::fillqsudot(FColDsptr col) +{ + if (active) ConstraintSet::fillqsudot(col); +} + +void MbD::LimitIJ::fillqsuddotlam(FColDsptr col) +{ + if (active) ConstraintSet::fillqsuddotlam(col); +} + +void MbD::LimitIJ::fillVelICError(FColDsptr col) +{ + if (active) ConstraintSet::fillVelICError(col); +} + +void MbD::LimitIJ::fillVelICJacob(SpMatDsptr mat) +{ + if (active) ConstraintSet::fillVelICJacob(mat); +} + +void MbD::LimitIJ::fillAccICIterError(FColDsptr col) +{ + if (compliant) { + auto contact = contactForce(*this); + constraints->front()->fillGeneralizedForce(col, contact.multiplier); + } + else if (active) ConstraintSet::fillAccICIterError(col); +} + +void MbD::LimitIJ::fillAccICIterJacob(SpMatDsptr mat) +{ + if (active) ConstraintSet::fillAccICIterJacob(mat); +} + +void MbD::LimitIJ::fillpqsumu(FColDsptr col) +{ + if (active) ConstraintSet::fillpqsumu(col); +} + +void MbD::LimitIJ::fillpqsumudot(FColDsptr col) +{ + if (active) ConstraintSet::fillpqsumudot(col); +} + +void MbD::LimitIJ::fillDynError(FColDsptr col) +{ + if (compliant) { + auto contact = contactForce(*this); + constraints->front()->fillGeneralizedForce(col, contact.multiplier); + } + else if (active) ConstraintSet::fillDynError(col); +} + +void MbD::LimitIJ::fillpFpy(SpMatDsptr mat) +{ + if (compliant) { + auto contact = contactForce(*this); + constraints->front()->fillGeneralizedForcePositionJacobian( + mat, + contact.multiplier, + contact.positionDerivative + ); + } + else if (active) ConstraintSet::fillpFpy(mat); +} + +void MbD::LimitIJ::fillpFpydot(SpMatDsptr mat) +{ + if (compliant) { + auto contact = contactForce(*this); + constraints->front()->fillGeneralizedForceVelocityJacobian( + mat, + contact.velocityDerivative + ); + } + else if (active) ConstraintSet::fillpFpydot(mat); +} + void MbD::LimitIJ::fillqsulam(FColDsptr col) { if (active) { @@ -61,9 +225,106 @@ void MbD::LimitIJ::setqsulam(FColDsptr col) } } +void MbD::LimitIJ::setqsudotlam(FColDsptr col) +{ + if (active) ConstraintSet::setqsudotlam(col); +} + +void MbD::LimitIJ::setqsuddotlam(FColDsptr col) +{ + if (active) ConstraintSet::setqsuddotlam(col); +} + +void MbD::LimitIJ::setpqsumu(FColDsptr col) +{ + if (active) ConstraintSet::setpqsumu(col); +} + +void MbD::LimitIJ::setpqsumudot(FColDsptr col) +{ + if (active) ConstraintSet::setpqsumudot(col); +} + void MbD::LimitIJ::useEquationNumbers() { - if (active) { + if (active || compliant) { ConstraintSet::useEquationNumbers(); } } + +namespace { +double clearance(const MbD::LimitIJ& limit) +{ + const double gap = limit.constraints->front()->aG; + if (limit.type == "=<") return -gap; + if (limit.type == "=>") return gap; + throw MbD::SimulationStoppingError("Unknown joint-limit comparison type"); +} +} + +void MbD::LimitIJ::preDyn() +{ + ConstraintSet::preDyn(); + previousClearance = clearance(*this); + transitionTime = std::numeric_limits::quiet_NaN(); + previousReaction = normalReaction(); + releasing = false; +} + +double MbD::LimitIJ::normalReaction() const +{ + // This solver uses force-minus-inertia residuals: lambda * grad(G) + // is the physical constraint force. A unilateral stop cannot pull. + return (type == "=>" ? 1.0 : -1.0) * constraints->front()->lam; +} + +bool MbD::LimitIJ::hasTensileReaction() const +{ + return active && !compliant && normalReaction() < -1e-9; +} + +void MbD::LimitIJ::preDynStep() +{ + previousClearance = clearance(*this); + previousReaction = normalReaction(); +} + +double MbD::LimitIJ::checkForDynDiscontinuityBetweenand(double tprev, double t) +{ + if (compliant || !std::isfinite(previousClearance) || !(t > tprev)) return t; + if (active) { + if (!hasTensileReaction()) return t; + const double current = normalReaction(); + const double denominator = previousReaction - current; + const double fraction = denominator > 0 + ? std::clamp(previousReaction / denominator, 0.0, 1.0) : 0.0; + transitionTime = tprev + fraction * (t - tprev); + releasing = true; + return transitionTime; + } + + const double currentClearance = clearance(*this); + if (currentClearance >= -tol || previousClearance < -tol) return t; + + const double denominator = previousClearance - currentClearance; + const double fraction + = denominator > 0.0 ? std::clamp(previousClearance / denominator, 0.0, 1.0) : 0.0; + transitionTime = tprev + fraction * (t - tprev); + releasing = false; + return transitionTime; +} + +void MbD::LimitIJ::discontinuityAtaddTypeTo( + double t, + std::shared_ptr> disconTypes +) +{ + const double scale = std::max({1.0, std::abs(t), std::abs(transitionTime)}); + if ((releasing == active) && std::isfinite(transitionTime) + && std::abs(t - transitionTime) <= 32.0 * std::numeric_limits::epsilon() * scale) { + if (releasing) deactivate(); + else activate(); + transitionTime = std::numeric_limits::quiet_NaN(); + disconTypes->push_back(releasing ? LIFTOFF : TOUCHDOWN); + } +} diff --git a/OndselSolver/LimitIJ.h b/OndselSolver/LimitIJ.h index 195a242f..6d607e93 100644 --- a/OndselSolver/LimitIJ.h +++ b/OndselSolver/LimitIJ.h @@ -17,19 +17,60 @@ namespace MbD { public: LimitIJ() = default; void fillConstraints(std::shared_ptr>> allConstraints) override; + void fillDispConstraints(std::shared_ptr>> constraints) override; + void fillEssenConstraints(std::shared_ptr>> constraints) override; + void fillPerpenConstraints(std::shared_ptr>> constraints) override; void fillPosICError(FColDsptr col) override; void fillPosICJacob(SpMatDsptr mat) override; + void fillqsudot(FColDsptr col) override; void fillqsulam(FColDsptr col) override; + void fillqsuddotlam(FColDsptr col) override; + void fillVelICError(FColDsptr col) override; + void fillVelICJacob(SpMatDsptr mat) override; + void fillAccICIterError(FColDsptr col) override; + void fillAccICIterJacob(SpMatDsptr mat) override; + void fillpqsumu(FColDsptr col) override; + void fillpqsumudot(FColDsptr col) override; + void fillDynError(FColDsptr col) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; void setqsulam(FColDsptr col) override; + void setqsudotlam(FColDsptr col) override; + void setqsuddotlam(FColDsptr col) override; + void setpqsumu(FColDsptr col) override; + void setpqsumudot(FColDsptr col) override; void useEquationNumbers() override; + void preDyn() override; + void preDynStep() override; + double checkForDynDiscontinuityBetweenand(double tprev, double t) override; + void discontinuityAtaddTypeTo( + double t, + std::shared_ptr> disconTypes + ) override; bool satisfied() const; void deactivate(); void activate(); + void setCompliant(double stiffness, double damping); + double normalReaction() const; + bool hasTensileReaction() const; + struct EnergyState { + double power = 0; + double storedEnergy = 0; + double dissipatedPower = 0; + }; + EnergyState energyState() const; double limit = std::numeric_limits::max(); double tol = std::numeric_limits::max(); std::string type; bool active = false; + bool compliant = false; + double stiffness = 0.0; + double damping = 0.0; + double previousClearance = std::numeric_limits::max(); + double transitionTime = std::numeric_limits::quiet_NaN(); + double previousReaction = 0; + bool releasing = false; }; } diff --git a/OndselSolver/MarkerFrame.cpp b/OndselSolver/MarkerFrame.cpp index c0a2d19a..ed86513a 100644 --- a/OndselSolver/MarkerFrame.cpp +++ b/OndselSolver/MarkerFrame.cpp @@ -252,3 +252,52 @@ void MarkerFrame::addEndFrame(EndFrmsptr endFrm) endFrm->setMarkerFrame(this); endFrames->push_back(endFrm); } + +void MarkerFrame::fillpqsumu(FColDsptr col) +{ + endFramesDo([&](const EndFrmsptr& endFrame) { endFrame->fillpqsumu(col); }); +} + +void MarkerFrame::fillpqsumudot(FColDsptr col) +{ + endFramesDo([&](const EndFrmsptr& endFrame) { endFrame->fillpqsumudot(col); }); +} + +void MarkerFrame::preDynOutput() +{ + CartesianFrame::preDynOutput(); + endFramesDo([](EndFrmsptr endFrame) { endFrame->preDynOutput(); }); +} + +void MarkerFrame::setpqsumu(FColDsptr col) +{ + endFramesDo([&](const EndFrmsptr& endFrame) { endFrame->setpqsumu(col); }); +} + +void MarkerFrame::setpqsumudot(FColDsptr col) +{ + endFramesDo([&](const EndFrmsptr& endFrame) { endFrame->setpqsumudot(col); }); +} + +void MarkerFrame::setpqsumuddot(FColDsptr col) +{ + endFramesDo([&](const EndFrmsptr& endFrame) { endFrame->setpqsumuddot(col); }); +} + +void MarkerFrame::postDynPredictor() +{ + CartesianFrame::postDynPredictor(); + endFramesDo([](EndFrmsptr endFrame) { endFrame->postDynPredictor(); }); +} + +void MarkerFrame::postDynOutput() +{ + CartesianFrame::postDynOutput(); + endFramesDo([](EndFrmsptr endFrame) { endFrame->postDynOutput(); }); +} + +void MarkerFrame::postDynCorrectorIteration() +{ + CartesianFrame::postDynCorrectorIteration(); + endFramesDo([](EndFrmsptr endFrame) { endFrame->postDynCorrectorIteration(); }); +} diff --git a/OndselSolver/MarkerFrame.h b/OndselSolver/MarkerFrame.h index 779e445a..44ec2293 100644 --- a/OndselSolver/MarkerFrame.h +++ b/OndselSolver/MarkerFrame.h @@ -27,6 +27,15 @@ namespace MbD { { //partFrame rpmp aApm rOmO aAOm prOmOpE pAOmpE pprOmOpEpE ppAOmpEpE endFrames public: + void fillpqsumu(FColDsptr col) override; + void fillpqsumudot(FColDsptr col) override; + void preDynOutput() override; + void setpqsumu(FColDsptr col) override; + void setpqsumudot(FColDsptr col) override; + void setpqsumuddot(FColDsptr col) override; + void postDynPredictor() override; + void postDynOutput() override; + void postDynCorrectorIteration() override; MarkerFrame(); MarkerFrame(const std::string& str); System* root() override; diff --git a/OndselSolver/NormalBasicDAEIntegrator.cpp b/OndselSolver/NormalBasicDAEIntegrator.cpp new file mode 100644 index 00000000..306d719a --- /dev/null +++ b/OndselSolver/NormalBasicDAEIntegrator.cpp @@ -0,0 +1,195 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include + +#include "NormalBasicDAEIntegrator.h" +#include "LinearMultiStepMethod.h" +#include "StableBackwardDifference.h" +#include "StartingBasicDAEIntegrator.h" +#include "SimulationStoppingError.h" + +#include "CREATE.h" + +using namespace MbD; + +NormalBasicDAEIntegrator::NormalBasicDAEIntegrator(std::shared_ptr startingBasicDAEIntegrator) +{ + //system direction + //istep iTry maxTry tpast t tnew h hnew order orderNew orderMax opBDF continue + //y ydot dy ypast ydotpast aF pFpy pFpydot alp aG extrapolator newtonRaphson corAbsTol corRelTol corOK integAbsTol integRelTol truncError + system = startingBasicDAEIntegrator->system; + direction = startingBasicDAEIntegrator->direction; + istep = startingBasicDAEIntegrator->istep; + iTry = startingBasicDAEIntegrator->iTry; + maxTry = startingBasicDAEIntegrator->maxTry; + tpast = startingBasicDAEIntegrator->tpast; + t = startingBasicDAEIntegrator->t; + tnew = startingBasicDAEIntegrator->tnew; + h = startingBasicDAEIntegrator->h; + hnew = startingBasicDAEIntegrator->hnew; + order = startingBasicDAEIntegrator->order; + orderNew = startingBasicDAEIntegrator->orderNew; + orderMax = startingBasicDAEIntegrator->orderMax; + opBDF = startingBasicDAEIntegrator->opBDF; + _continue = startingBasicDAEIntegrator->_continue; + y = startingBasicDAEIntegrator->y; + ydot = startingBasicDAEIntegrator->ydot; + dy = startingBasicDAEIntegrator->dy; + ypast = startingBasicDAEIntegrator->ypast; + ydotpast = startingBasicDAEIntegrator->ydotpast; + aF = startingBasicDAEIntegrator->aF; + pFpy = startingBasicDAEIntegrator->pFpy; + pFpydot = startingBasicDAEIntegrator->pFpydot; + alp = startingBasicDAEIntegrator->alp; + matG = startingBasicDAEIntegrator->matG; + extrapolator = startingBasicDAEIntegrator->extrapolator; + newtonRaphson = startingBasicDAEIntegrator->newtonRaphson; + newtonRaphson->setSystem(this); + corAbsTol = startingBasicDAEIntegrator->corAbsTol; + corRelTol = startingBasicDAEIntegrator->corRelTol; + corOK = startingBasicDAEIntegrator->corOK; + integAbsTol = startingBasicDAEIntegrator->integAbsTol; + integRelTol = startingBasicDAEIntegrator->integRelTol; + truncError = startingBasicDAEIntegrator->truncError; + opBDFhigher = CREATE::With(); + opBDFhigher->timeNodes = tpast; + opBDFhigher->time = t; + opBDFhigher->iStep = istep; + opBDFhigher->order = order + 1; + //calcOperatorMatrix(); +} + +std::shared_ptr NormalBasicDAEIntegrator::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void NormalBasicDAEIntegrator::initialize() +{ + BasicDAEIntegrator::initialize(); + opBDFhigher = CREATE::With(); + opBDFhigher->timeNodes = tpast; +} + +void NormalBasicDAEIntegrator::initializeLocally() +{ + //"NormalBasicDAEIntegrator is not used for starting an integration." + //"Change to StartingBasicDAEIntegrator." + throw SimulationStoppingError("To be implemented."); +} + +FColDsptr NormalBasicDAEIntegrator::yDerivat(size_t n, double time) +{ + // An order-one position polynomial has no second derivative, but the DAE + // state also carries its first derivative. Interpolate that history to + // retain valid acceleration output when orderMax == 1. + if (n > order) { + return opBDF->derivativeatpresentpast(n - 1, time, ydot, ydotpast); + } + return opBDF->derivativeatpresentpast(n, time, y, ypast); +} + +std::shared_ptr NormalBasicDAEIntegrator::correctorBDF() +{ + return opBDF; +} + +void NormalBasicDAEIntegrator::calcOperatorMatrix() +{ + BasicDAEIntegrator::calcOperatorMatrix(); + opBDFhigher->calcOperatorMatrix(); +} + +void NormalBasicDAEIntegrator::settime(double t) +{ + BasicDAEIntegrator::settime(t); + opBDFhigher->settime(t); +} + +void NormalBasicDAEIntegrator::iStep(size_t i) +{ + BasicDAEIntegrator::iStep(i); + opBDFhigher->setiStep(i); +} + +void NormalBasicDAEIntegrator::setorder(size_t o) +{ + BasicDAEIntegrator::setorder(o); + opBDFhigher->setorder(o + 1); +} + +FColDsptr NormalBasicDAEIntegrator::yDeriv(size_t deriv) +{ + return opBDF->derivativepresentpast(deriv, y, ypast); +} + +FColDsptr NormalBasicDAEIntegrator::dyOrderPlusOnedt() +{ + return opBDFhigher->derivativepresentpast(order + 1, y, ypast); +} + +void NormalBasicDAEIntegrator::run() +{ + subsequentSteps(); + finalize(); + reportStats(); + postRun(); +} + +void NormalBasicDAEIntegrator::selectOrder() +{ + if (tpast->size() < order + 1) return; //Needed to transition from Starting to Normal + selectOrderNormal(); +} + +void NormalBasicDAEIntegrator::selectStepSize() +{ + if (tpast->size() < order + 1) return; //Needed to transition from Starting to Normal + BasicDAEIntegrator::selectStepSize(); +} + +void NormalBasicDAEIntegrator::selectOrderNormal() +{ + //"Brenan's book pp. 126-7" + //"Check last nterm of Taylor series plus first term of remainder." + + auto errorTrunc = std::make_shared>(); + auto nterm = 3; + size_t istart; + FColDsptr yndot; + double yndotNorm, hpower; + if (order > nterm) { + istart = order - nterm + 1; + } + else { + istart = 1; + } + for (size_t i = istart; i < order; i++) + { + yndot = yDeriv(i); + yndotNorm = integErrorNormFromwrt(yndot, y); + hpower = std::pow(h, i); + errorTrunc->push_back(yndotNorm * hpower); + } + + + yndot = dyOrderPlusOnedt(); + yndotNorm = integErrorNormFromwrt(yndot, y); + hpower = std::pow(h, order + 1); + errorTrunc->push_back(yndotNorm * hpower); + orderNew = order; + if (errorTrunc->isIncreasing()) { + if (order > 1) orderNew = order - 1; + } + if (errorTrunc->isDecreasingIfExceptionsAreLessThan(0.01)) { + if (order < orderMax) orderNew = order + 1; + } +} diff --git a/OndselSolver/NormalBasicDAEIntegrator.h b/OndselSolver/NormalBasicDAEIntegrator.h new file mode 100644 index 00000000..8f80c272 --- /dev/null +++ b/OndselSolver/NormalBasicDAEIntegrator.h @@ -0,0 +1,42 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "BasicDAEIntegrator.h" + +namespace MbD { + class StableBackwardDifference; + class StartingBasicDAEIntegrator; + + class NormalBasicDAEIntegrator : public BasicDAEIntegrator + { + // + public: + NormalBasicDAEIntegrator() {} + NormalBasicDAEIntegrator(std::shared_ptr startingBasicDAEIntegrator); + static std::shared_ptr With(); + void initialize() override; + + void initializeLocally() override; + FColDsptr yDerivat(size_t _order, double tout) override; + std::shared_ptr correctorBDF() override; + void calcOperatorMatrix() override; + void settime(double t) override; + void iStep(size_t i) override; + void setorder(size_t o) override; + FColDsptr yDeriv(size_t order); + FColDsptr dyOrderPlusOnedt() override; + void run() override; + void selectOrder() override; + void selectOrderNormal(); + void selectStepSize() override; + + std::shared_ptr opBDFhigher; + }; +} diff --git a/OndselSolver/OrbitAngleZIecJec.cpp b/OndselSolver/OrbitAngleZIecJec.cpp index d6f2ec98..1a903983 100644 --- a/OndselSolver/OrbitAngleZIecJec.cpp +++ b/OndselSolver/OrbitAngleZIecJec.cpp @@ -112,3 +112,31 @@ double MbD::OrbitAngleZIecJec::value() { return thez; } + +void OrbitAngleZIecJec::postDynPredictor() +{ + xIeJeIe->postDynPredictor(); + yIeJeIe->postDynPredictor(); + KinematicIeJe::postDynPredictor(); +} + +void OrbitAngleZIecJec::postDynCorrectorIteration() +{ + xIeJeIe->postDynCorrectorIteration(); + yIeJeIe->postDynCorrectorIteration(); + KinematicIeJe::postDynCorrectorIteration(); +} + +void OrbitAngleZIecJec::preDynOutput() +{ + xIeJeIe->preDynOutput(); + yIeJeIe->preDynOutput(); + KinematicIeJe::preDynOutput(); +} + +void OrbitAngleZIecJec::postDynOutput() +{ + xIeJeIe->postDynOutput(); + yIeJeIe->postDynOutput(); + KinematicIeJe::postDynOutput(); +} diff --git a/OndselSolver/OrbitAngleZIecJec.h b/OndselSolver/OrbitAngleZIecJec.h index 923f86fa..073503fe 100644 --- a/OndselSolver/OrbitAngleZIecJec.h +++ b/OndselSolver/OrbitAngleZIecJec.h @@ -16,6 +16,10 @@ namespace MbD { { //thez xIeJeIe yIeJeIe cosOverSSq sinOverSSq twoCosSinOverSSqSq dSqOverSSqSq public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; OrbitAngleZIecJec(); OrbitAngleZIecJec(EndFrmsptr frmi, EndFrmsptr frmj); diff --git a/OndselSolver/Part.cpp b/OndselSolver/Part.cpp index 380dbdda..b0ecaee2 100644 --- a/OndselSolver/Part.cpp +++ b/OndselSolver/Part.cpp @@ -163,9 +163,9 @@ FColDsptr Part::qXddot() void Part::qEddot(FColDsptr x) { - //ToDo: Should store EulerParametersDDot - //ToDo: Need alpOpO too - partFrame->qXddot = x; + //ToDo: Should store EulerParametersDDot + //ToDo: Need alpOpO too + partFrame->qEddot = x; } FColDsptr Part::qEddot() @@ -584,7 +584,104 @@ void Part::postDynStep() partFrame->postDynStep(); } -void MbD::Part::postAccIC() +void Part::postAccIC() +{ + calcpdot(); +} + +void Part::fillpqsumu(FColDsptr col) +{ + col->atiputFullColumn(ipX, pX); + col->atiputFullColumn(ipE, pE); + partFrame->fillpqsumu(col); +} + +void Part::fillpqsumudot(FColDsptr col) +{ + col->atiputFullColumn(ipX, pXdot); + col->atiputFullColumn(ipE, pEdot); + partFrame->fillpqsumudot(col); +} + +void Part::calcpdot() +{ + pXdot = mX->timesFullColumn(partFrame->qXddot); + pEdot = mEdot->timesFullColumn(partFrame->qEdot)->plusFullColumn(mE->timesFullColumn(partFrame->qEddot)); +} + +void Part::setpqsumu(FColDsptr col) +{ + pX->equalFullColumnAt(col, ipX); + pE->equalFullColumnAt(col, ipE); + partFrame->setpqsumu(col); +} + +void Part::setpqsumudot(FColDsptr col) +{ + pXdot->equalFullColumnAt(col, ipX); + pEdot->equalFullColumnAt(col, ipE); + partFrame->setpqsumudot(col); +} + +void Part::setpqsumuddot(FColDsptr col) +{ + partFrame->setpqsumuddot(col); +} + +void Part::postDynPredictor() +{ + partFrame->postDynPredictor(); + Item::postDynPredictor(); +} + +void Part::fillDynError(FColDsptr col) +{ + partFrame->fillDynError(col); + //ToDo: Check for Units effect. + col->atiplusFullColumn(ipX, pX->minusFullColumn(mX->timesFullColumn(partFrame->qXdot))); + col->atiplusFullColumn(ipE, pE->minusFullColumn(mE->timesFullColumn(partFrame->qEdot))); + col->atiminusFullColumn(partFrame->iqX, pXdot); + col->atiminusFullColumn(partFrame->iqE, pEdot->minusFullColumn(pTpE)); +} + +void Part::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusDiagonalMatrix(ipX, ipX, std::make_shared>(3, 1.0)); + mat->atijplusDiagonalMatrix(ipE, ipE, std::make_shared>(4, 1.0)); + auto iqE = partFrame->iqE; + //ToDo: Check for Units effect. + mat->atijminusTransposeFullMatrix(ipE, iqE, ppTpEpEdot); + mat->atijplusFullMatrix(iqE, iqE, ppTpEpE); + partFrame->fillpFpy(mat); +} + +void Part::fillpFpydot(SpMatDsptr mat) +{ + auto iqX = partFrame->iqX; + auto iqE = partFrame->iqE; + //ToDo: Check for Units effect. + mat->atijminusDiagonalMatrix(ipX, iqX, mX); + mat->atijminusFullMatrix(ipE, iqE, mE); + mat->atijminusDiagonalMatrix(iqX, ipX, std::make_shared>(3, 1.0)); + mat->atijminusDiagonalMatrix(iqE, ipE, std::make_shared>(4, 1.0)); + mat->atijplusFullMatrix(iqE, iqE, ppTpEpEdot); + partFrame->fillpFpydot(mat); +} + +void Part::postDynCorrectorIteration() +{ + partFrame->postDynCorrectorIteration(); + Item::postDynCorrectorIteration(); +} + +void Part::preDynOutput() +{ + partFrame->preDynOutput(); + Item::preDynOutput(); +} + +void Part::postDynOutput() { - //calcpdot(); + partFrame->postDynOutput(); + Item::postDynOutput(); } diff --git a/OndselSolver/Part.h b/OndselSolver/Part.h index 57a884d7..6c889f76 100644 --- a/OndselSolver/Part.h +++ b/OndselSolver/Part.h @@ -22,6 +22,19 @@ namespace MbD { { //ToDo: ipX ipE m aJ partFrame pX pXdot pE pEdot mX mE mEdot pTpE ppTpEpE ppTpEpEdot public: + void fillpqsumu(FColDsptr col) override; + void fillpqsumudot(FColDsptr col) override; + void calcpdot(); + void setpqsumu(FColDsptr col) override; + void setpqsumudot(FColDsptr col) override; + void setpqsumuddot(FColDsptr col) override; + void postDynPredictor() override; + void fillDynError(FColDsptr col) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; Part(); Part(const std::string& str); System* root() override; diff --git a/OndselSolver/PartFrame.cpp b/OndselSolver/PartFrame.cpp index 5219bb8a..60b6edda 100644 --- a/OndselSolver/PartFrame.cpp +++ b/OndselSolver/PartFrame.cpp @@ -512,11 +512,26 @@ void PartFrame::postDynStep() void PartFrame::asFixed() { - for (size_t i = 0; i < 6; i++) { - auto con = CREATE::With(i); - con->owner = this; - aGabs->push_back(con); - } + // Ground at the supplied pose, not at zero coordinates/identity rotation. + // The Euler normalization constraint supplies the fourth quaternion equation. + // Leave the largest component unconstrained so that this remains full rank + // at half-turns as well as near the identity orientation. + size_t dependent = 0; + for (size_t i = 1; i < 4; ++i) { + if (std::abs(qE->at(i)) > std::abs(qE->at(dependent))) { + dependent = i; + } + } + aGabs->clear(); + for (size_t i = 0; i < 7; ++i) { + if (i == 3 + dependent) { + continue; + } + auto con = CREATE::With(i); + con->owner = this; + con->setConstant(i < 3 ? qX->at(i) : qE->at(i - 3)); + aGabs->push_back(con); + } } void PartFrame::postInput() @@ -536,3 +551,102 @@ void PartFrame::calcPostDynCorrectorIteration() qEdot->calcAdotBdotCdot(); qEdot->calcpAdotpE(); } + +void PartFrame::fillpqsumu(FColDsptr col) +{ + //"Fill q, s and lam into col." + col->atiputFullColumn(iqX, qX); + col->atiputFullColumn(iqE, qE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillpqsumu(col); }); + aGeu->fillpqsumu(col); + aGabsDo([&](std::shared_ptr con) { con->fillpqsumu(col); }); +} + +void PartFrame::fillpqsumudot(FColDsptr col) +{ + col->atiputFullColumn(iqX, qXdot); + col->atiputFullColumn(iqE, qEdot); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillpqsumudot(col); }); + aGeu->fillpqsumudot(col); + aGabsDo([&](std::shared_ptr con) { con->fillpqsumudot(col); }); +} + +void PartFrame::setpqsumu(FColDsptr col) +{ + qX->equalFullColumnAt(col, iqX); + qE->equalFullColumnAt(col, iqE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setpqsumu(col); }); + aGeu->setpqsumu(col); + aGabsDo([&](std::shared_ptr con) { con->setpqsumu(col); }); +} + +void PartFrame::setpqsumudot(FColDsptr col) +{ + qXdot->equalFullColumnAt(col, iqX); + qEdot->equalFullColumnAt(col, iqE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setpqsumudot(col); }); + aGeu->setpqsumudot(col); + aGabsDo([&](std::shared_ptr con) { con->setpqsumudot(col); }); +} + +void PartFrame::setpqsumuddot(FColDsptr col) +{ + qXddot->equalFullColumnAt(col, iqX); + qEddot->equalFullColumnAt(col, iqE); + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->setpqsumuddot(col); }); + aGeu->setpqsumuddot(col); + aGabsDo([&](std::shared_ptr con) { con->setpqsumuddot(col); }); +} + +void PartFrame::postDynPredictor() +{ + CartesianFrame::postDynPredictor(); + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postDynPredictor(); }); + aGeu->postDynPredictor(); + aGabsDo([](std::shared_ptr aGab) { aGab->postDynPredictor(); }); +} + +void PartFrame::fillDynError(FColDsptr col) +{ + markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillDynError(col); }); + aGeu->fillDynError(col); + aGabsDo([&](std::shared_ptr con) { con->fillDynError(col); }); +} + +void PartFrame::fillpFpy(SpMatDsptr mat) +{ + //markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillpFpy(mat); }); + aGeu->fillpFpy(mat); + aGabsDo([&](std::shared_ptr con) { con->fillpFpy(mat); }); +} + +void PartFrame::fillpFpydot(SpMatDsptr mat) +{ + //markerFramesDo([&](std::shared_ptr markerFrame) { markerFrame->fillpFpydot(mat); }); + aGeu->fillpFpydot(mat); + aGabsDo([&](std::shared_ptr con) { con->fillpFpydot(mat); }); +} + +void PartFrame::postDynCorrectorIteration() +{ + CartesianFrame::postDynCorrectorIteration(); + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postDynCorrectorIteration(); }); + aGeu->postDynCorrectorIteration(); + aGabsDo([](std::shared_ptr aGab) { aGab->postDynCorrectorIteration(); }); +} + +void PartFrame::preDynOutput() +{ + CartesianFrame::preDynOutput(); + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->preDynOutput(); }); + aGeu->preDynOutput(); + aGabsDo([](std::shared_ptr aGab) { aGab->preDynOutput(); }); +} + +void PartFrame::postDynOutput() +{ + CartesianFrame::postDynOutput(); + markerFramesDo([](std::shared_ptr markerFrame) { markerFrame->postDynOutput(); }); + aGeu->postDynOutput(); + aGabsDo([](std::shared_ptr aGab) { aGab->postDynOutput(); }); +} diff --git a/OndselSolver/PartFrame.h b/OndselSolver/PartFrame.h index 8d254610..a262da5e 100644 --- a/OndselSolver/PartFrame.h +++ b/OndselSolver/PartFrame.h @@ -30,6 +30,18 @@ namespace MbD { { //ToDo: part iqX iqE qX qE qXdot qEdot qXddot qEddot aGeu aGabs markerFrames public: + void fillpqsumu(FColDsptr col) override; + void fillpqsumudot(FColDsptr col) override; + void setpqsumu(FColDsptr col) override; + void setpqsumudot(FColDsptr col) override; + void setpqsumuddot(FColDsptr col) override; + void postDynPredictor() override; + void fillDynError(FColDsptr col) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; PartFrame(); PartFrame(const std::string& str); System* root() override; diff --git a/OndselSolver/RackPinConstraintIJ.cpp b/OndselSolver/RackPinConstraintIJ.cpp index 94eaee53..f8443655 100644 --- a/OndselSolver/RackPinConstraintIJ.cpp +++ b/OndselSolver/RackPinConstraintIJ.cpp @@ -110,3 +110,31 @@ void MbD::RackPinConstraintIJ::simUpdateAll() thezIeJe->simUpdateAll(); ConstraintIJ::simUpdateAll(); } + +void RackPinConstraintIJ::postDynPredictor() +{ + xIeJeIe->postDynPredictor(); + thezIeJe->postDynPredictor(); + ConstraintIJ::postDynPredictor(); +} + +void RackPinConstraintIJ::postDynCorrectorIteration() +{ + xIeJeIe->postDynCorrectorIteration(); + thezIeJe->postDynCorrectorIteration(); + ConstraintIJ::postDynCorrectorIteration(); +} + +void RackPinConstraintIJ::preDynOutput() +{ + xIeJeIe->preDynOutput(); + thezIeJe->preDynOutput(); + ConstraintIJ::preDynOutput(); +} + +void RackPinConstraintIJ::postDynOutput() +{ + xIeJeIe->postDynOutput(); + thezIeJe->postDynOutput(); + ConstraintIJ::postDynOutput(); +} diff --git a/OndselSolver/RackPinConstraintIJ.h b/OndselSolver/RackPinConstraintIJ.h index ab7afef0..fbe80513 100644 --- a/OndselSolver/RackPinConstraintIJ.h +++ b/OndselSolver/RackPinConstraintIJ.h @@ -17,6 +17,10 @@ namespace MbD { { //xIeJeIe thezIeJe pitchRadius public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; RackPinConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj); static std::shared_ptr With(EndFrmsptr frmi, EndFrmsptr frmj); diff --git a/OndselSolver/RackPinConstraintIqcJc.cpp b/OndselSolver/RackPinConstraintIqcJc.cpp index a88ed8f0..d84a89eb 100644 --- a/OndselSolver/RackPinConstraintIqcJc.cpp +++ b/OndselSolver/RackPinConstraintIqcJc.cpp @@ -144,3 +144,19 @@ void MbD::RackPinConstraintIqcJc::useEquationNumbers() iqXI = frmIeqc->iqX(); iqEI = frmIeqc->iqE(); } + +void RackPinConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void RackPinConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} diff --git a/OndselSolver/RackPinConstraintIqcJc.h b/OndselSolver/RackPinConstraintIqcJc.h index f94cd299..a95b1917 100644 --- a/OndselSolver/RackPinConstraintIqcJc.h +++ b/OndselSolver/RackPinConstraintIqcJc.h @@ -15,6 +15,8 @@ namespace MbD { { //pGpXI pGpEI ppGpXIpEI ppGpEIpEI iqXI iqEI public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; RackPinConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj); void initxIeJeIe() override; diff --git a/OndselSolver/RackPinConstraintIqcJqc.cpp b/OndselSolver/RackPinConstraintIqcJqc.cpp index 5052904b..d3e4e77a 100644 --- a/OndselSolver/RackPinConstraintIqcJqc.cpp +++ b/OndselSolver/RackPinConstraintIqcJqc.cpp @@ -146,3 +146,24 @@ std::string MbD::RackPinConstraintIqcJqc::constraintSpec() { return "RackPinConstraintIJ"; } + +void RackPinConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + RackPinConstraintIqcJc::fillpFpy(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void RackPinConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + RackPinConstraintIqcJc::fillpFpydot(mat); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} diff --git a/OndselSolver/RackPinConstraintIqcJqc.h b/OndselSolver/RackPinConstraintIqcJqc.h index bb980bcc..596ee90a 100644 --- a/OndselSolver/RackPinConstraintIqcJqc.h +++ b/OndselSolver/RackPinConstraintIqcJqc.h @@ -17,6 +17,8 @@ namespace MbD { { //pGpXJ pGpEJ ppGpEIpXJ ppGpEIpEJ ppGpEJpEJ iqXJ iqEJ public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; RackPinConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj); void initxIeJeIe() override; diff --git a/OndselSolver/RedundantConstraint.cpp b/OndselSolver/RedundantConstraint.cpp index 3c11eaf1..6ee00a08 100644 --- a/OndselSolver/RedundantConstraint.cpp +++ b/OndselSolver/RedundantConstraint.cpp @@ -117,3 +117,63 @@ std::string MbD::RedundantConstraint::constraintSpec() { return "RedundantConstraint" + constraint->constraintSpec(); } + +void RedundantConstraint::fillpqsumu(FColDsptr col) +{ + //Do nothing. +} + +void RedundantConstraint::fillpqsumudot(FColDsptr col) +{ + //Do nothing. +} + +void RedundantConstraint::setpqsumu(FColDsptr col) +{ + //Do nothing. +} + +void RedundantConstraint::setpqsumudot(FColDsptr col) +{ + //Do nothing. +} + +void RedundantConstraint::setpqsumuddot(FColDsptr col) +{ + //Do nothing. +} + +void RedundantConstraint::postDynPredictor() +{ + //Do nothing. +} + +void RedundantConstraint::fillDynError(FColDsptr col) +{ + //Do nothing. +} + +void RedundantConstraint::fillpFpy(SpMatDsptr mat) +{ + //Do nothing. +} + +void RedundantConstraint::fillpFpydot(SpMatDsptr mat) +{ + //Do nothing. +} + +void RedundantConstraint::postDynCorrectorIteration() +{ + //Do nothing. +} + +void RedundantConstraint::preDynOutput() +{ + //Do nothing. +} + +void RedundantConstraint::postDynOutput() +{ + //Do nothing. +} diff --git a/OndselSolver/RedundantConstraint.h b/OndselSolver/RedundantConstraint.h index b7bbb313..1017aecf 100644 --- a/OndselSolver/RedundantConstraint.h +++ b/OndselSolver/RedundantConstraint.h @@ -15,6 +15,18 @@ namespace MbD { { // public: + void fillpqsumu(FColDsptr col) override; + void fillpqsumudot(FColDsptr col) override; + void setpqsumu(FColDsptr col) override; + void setpqsumudot(FColDsptr col) override; + void setpqsumuddot(FColDsptr col) override; + void postDynPredictor() override; + void fillDynError(FColDsptr col) override; + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; void removeRedundantConstraints(std::shared_ptr> redundantEqnNos) override; bool isRedundant() override; std::string classname() override; diff --git a/OndselSolver/ScrewConstraintIJ.cpp b/OndselSolver/ScrewConstraintIJ.cpp index d9c82df9..9534c5f6 100644 --- a/OndselSolver/ScrewConstraintIJ.cpp +++ b/OndselSolver/ScrewConstraintIJ.cpp @@ -112,3 +112,31 @@ void MbD::ScrewConstraintIJ::simUpdateAll() thezIeJe->simUpdateAll(); ConstraintIJ::simUpdateAll(); } + +void ScrewConstraintIJ::postDynPredictor() +{ + zIeJeIe->postDynPredictor(); + thezIeJe->postDynPredictor(); + ConstraintIJ::postDynPredictor(); +} + +void ScrewConstraintIJ::postDynCorrectorIteration() +{ + zIeJeIe->postDynCorrectorIteration(); + thezIeJe->postDynCorrectorIteration(); + ConstraintIJ::postDynCorrectorIteration(); +} + +void ScrewConstraintIJ::preDynOutput() +{ + zIeJeIe->preDynOutput(); + thezIeJe->preDynOutput(); + ConstraintIJ::preDynOutput(); +} + +void ScrewConstraintIJ::postDynOutput() +{ + zIeJeIe->postDynOutput(); + thezIeJe->postDynOutput(); + ConstraintIJ::postDynOutput(); +} diff --git a/OndselSolver/ScrewConstraintIJ.h b/OndselSolver/ScrewConstraintIJ.h index b6cc905f..4c894ae8 100644 --- a/OndselSolver/ScrewConstraintIJ.h +++ b/OndselSolver/ScrewConstraintIJ.h @@ -17,6 +17,10 @@ namespace MbD { { //zIeJeIe thezIeJe pitch public: + void postDynPredictor() override; + void postDynCorrectorIteration() override; + void preDynOutput() override; + void postDynOutput() override; ScrewConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj); static std::shared_ptr With(EndFrmsptr frmi, EndFrmsptr frmj); diff --git a/OndselSolver/ScrewConstraintIqcJc.cpp b/OndselSolver/ScrewConstraintIqcJc.cpp index 9933eb4d..c99803a6 100644 --- a/OndselSolver/ScrewConstraintIqcJc.cpp +++ b/OndselSolver/ScrewConstraintIqcJc.cpp @@ -146,3 +146,19 @@ void MbD::ScrewConstraintIqcJc::useEquationNumbers() iqXI = frmIeqc->iqX(); iqEI = frmIeqc->iqE(); } + +void ScrewConstraintIqcJc::fillpFpy(SpMatDsptr mat) +{ + mat->atijplusFullRow(iG, iqXI, pGpXI); + mat->atijplusFullRow(iG, iqEI, pGpEI); + auto ppGpXIpEIlam = ppGpXIpEI->times(lam); + mat->atijplusFullMatrix(iqXI, iqEI, ppGpXIpEIlam); + mat->atijplusTransposeFullMatrix(iqEI, iqXI, ppGpXIpEIlam); + mat->atijplusFullMatrixtimes(iqEI, iqEI, ppGpEIpEI, lam); +} + +void ScrewConstraintIqcJc::fillpFpydot(SpMatDsptr mat) +{ + mat->atijplusFullColumn(iqXI, iG, pGpXI->transpose()); + mat->atijplusFullColumn(iqEI, iG, pGpEI->transpose()); +} diff --git a/OndselSolver/ScrewConstraintIqcJc.h b/OndselSolver/ScrewConstraintIqcJc.h index 3647d6a1..d3813096 100644 --- a/OndselSolver/ScrewConstraintIqcJc.h +++ b/OndselSolver/ScrewConstraintIqcJc.h @@ -15,6 +15,8 @@ namespace MbD { { //pGpXI pGpEI ppGpXIpEI ppGpEIpEI iqXI iqEI public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; ScrewConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj); void initzIeJeIe() override; diff --git a/OndselSolver/ScrewConstraintIqcJqc.cpp b/OndselSolver/ScrewConstraintIqcJqc.cpp index 05d5f217..97374ddd 100644 --- a/OndselSolver/ScrewConstraintIqcJqc.cpp +++ b/OndselSolver/ScrewConstraintIqcJqc.cpp @@ -148,3 +148,24 @@ std::string MbD::ScrewConstraintIqcJqc::constraintSpec() { return "ScrewConstraintIJ"; } + +void ScrewConstraintIqcJqc::fillpFpy(SpMatDsptr mat) +{ + ScrewConstraintIqcJc::fillpFpy(mat); + mat->atijplusFullRow(iG, iqXJ, pGpXJ); + mat->atijplusFullRow(iG, iqEJ, pGpEJ); + auto ppGpEIpXJlam = ppGpEIpXJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqXJ, ppGpEIpXJlam); + mat->atijplusTransposeFullMatrix(iqXJ, iqEI, ppGpEIpXJlam); + auto ppGpEIpEJlam = ppGpEIpEJ->times(lam); + mat->atijplusFullMatrix(iqEI, iqEJ, ppGpEIpEJlam); + mat->atijplusTransposeFullMatrix(iqEJ, iqEI, ppGpEIpEJlam); + mat->atijplusFullMatrixtimes(iqEJ, iqEJ, ppGpEJpEJ, lam); +} + +void ScrewConstraintIqcJqc::fillpFpydot(SpMatDsptr mat) +{ + ScrewConstraintIqcJc::fillpFpydot(mat); + mat->atijplusFullColumn(iqXJ, iG, pGpXJ->transpose()); + mat->atijplusFullColumn(iqEJ, iG, pGpEJ->transpose()); +} diff --git a/OndselSolver/ScrewConstraintIqcJqc.h b/OndselSolver/ScrewConstraintIqcJqc.h index d31ee9f8..e3858f3d 100644 --- a/OndselSolver/ScrewConstraintIqcJqc.h +++ b/OndselSolver/ScrewConstraintIqcJqc.h @@ -17,6 +17,8 @@ namespace MbD { { //pGpXJ pGpEJ ppGpEIpXJ ppGpEIpEJ ppGpEJpEJ iqXJ iqEJ public: + void fillpFpy(SpMatDsptr mat) override; + void fillpFpydot(SpMatDsptr mat) override; ScrewConstraintIqcJqc(EndFrmsptr frmi, EndFrmsptr frmj); void initzIeJeIe() override; diff --git a/OndselSolver/Solver.h b/OndselSolver/Solver.h index 126b46f2..8196e3ae 100644 --- a/OndselSolver/Solver.h +++ b/OndselSolver/Solver.h @@ -10,12 +10,14 @@ #include #include "Numeric.h" +#include "SolverStatistics.h" namespace MbD { class Solver { //statistics public: + std::shared_ptr statistics = SolverStatistics::With(); void noop(); virtual ~Solver() {} virtual void initialize(); diff --git a/OndselSolver/SolverStatistics.cpp b/OndselSolver/SolverStatistics.cpp new file mode 100644 index 00000000..756ee636 --- /dev/null +++ b/OndselSolver/SolverStatistics.cpp @@ -0,0 +1,13 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +#include + +#include "SolverStatistics.h" + +using namespace MbD; + +std::shared_ptr SolverStatistics::With() +{ + auto inst = std::make_shared(); + //inst->initialize(); + return inst; +} diff --git a/OndselSolver/SolverStatistics.h b/OndselSolver/SolverStatistics.h new file mode 100644 index 00000000..fc20248c --- /dev/null +++ b/OndselSolver/SolverStatistics.h @@ -0,0 +1,27 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once +#include +#include + +namespace MbD { + class SolverStatistics + { + public: + static std::shared_ptr With(); + + size_t iterNo = SIZE_MAX; + size_t corIterNo = SIZE_MAX; + double h = std::numeric_limits::min(); + size_t istep = SIZE_MAX; + size_t order = SIZE_MAX; + double t = std::numeric_limits::min(); + double truncError = std::numeric_limits::min(); + }; +} diff --git a/OndselSolver/StableBackwardDifference.cpp b/OndselSolver/StableBackwardDifference.cpp index 05511825..001aa32b 100644 --- a/OndselSolver/StableBackwardDifference.cpp +++ b/OndselSolver/StableBackwardDifference.cpp @@ -81,7 +81,7 @@ FColDsptr MbD::StableBackwardDifference::derivativepresentpast(size_t deriv, FCo //"Answer ith derivative given present value and past values." if (deriv == 0) { - return std::static_pointer_cast>(y->clonesptr()); + return y->copy(); } else { if (deriv <= order) { @@ -100,3 +100,22 @@ FColDsptr MbD::StableBackwardDifference::derivativepresentpast(size_t deriv, FCo } } } + +FColDsptr StableBackwardDifference::derivativeatpresentpastpresentDerivativepastDerivative(size_t n, double t, FColDsptr y, std::shared_ptr> ypast, FColDsptr ydot, std::shared_ptr> ydotpast) +{ + //"Interpolate or extrapolate." + //"dfdt(t) = df0dt + d2f0dt2*(t - t0) + d3f0dt3*(t - t0)^2 / 2! + ..." + + auto answer = derivativepresentpastpresentDerivativepastDerivative(n, y, ypast, ydot, ydotpast); + if (t != time) { + auto dt = t - time; + auto dtpower = 1.0; + for (size_t i = n + 1; i <= order; i++) + { + auto diydti = derivativepresentpastpresentDerivativepastDerivative(i, y, ypast, ydot, ydotpast); + dtpower = dtpower * dt; + answer->equalSelfPlusFullColumntimes(diydti, dtpower * OneOverFactorials->at(i - n)); + } + } + return answer; +} diff --git a/OndselSolver/StableBackwardDifference.h b/OndselSolver/StableBackwardDifference.h index 6075fbd8..6701b0b9 100644 --- a/OndselSolver/StableBackwardDifference.h +++ b/OndselSolver/StableBackwardDifference.h @@ -16,6 +16,7 @@ namespace MbD { { // public: + FColDsptr derivativeatpresentpastpresentDerivativepastDerivative(size_t n, double t, FColDsptr y, std::shared_ptr> ypast, FColDsptr ydot, std::shared_ptr> ydotpast); FColDsptr derivativepresentpast(size_t order, FColDsptr y, std::shared_ptr> ypast) override; void instantiateTaylorMatrix() override; void formTaylorRowwithTimeNodederivative(size_t i, size_t ii, size_t k) override; diff --git a/OndselSolver/StableStartingBDF.cpp b/OndselSolver/StableStartingBDF.cpp new file mode 100644 index 00000000..49540547 --- /dev/null +++ b/OndselSolver/StableStartingBDF.cpp @@ -0,0 +1,99 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include "StableStartingBDF.h" +#include "FullColumn.h" + +using namespace MbD; + +std::shared_ptr StableStartingBDF::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void StableStartingBDF::initialize() +{ + StableBackwardDifference::initialize(); + iStep = 0; + order = iStep + 1; +} + +void StableStartingBDF::initializeLocally() +{ + initialize(); +} + +double StableStartingBDF::pvdotpv() +{ + //"pvdotpv = operatorMatrix timesColumn: #(-1.0d ... -1.0d, 0.0d)." + + auto& coeffs = operatorMatrix->at(0); + auto sum = 0.0; + for (size_t i = 0; i < order - 1; i++) + { + sum -= coeffs->at(i); + } + return sum; +} + +void StableStartingBDF::formTaylorMatrix() +{ + //" + //This form is numerically more stable and is prefered over the full Taylor Matrix. + //For method order 3: + //| (t1 - t) (t1 - t)^2/2! (t1 - t)^3/3! | |qd(t) | |q(t1) - q(t) | + //| (t2 - t) (t2 - t)^2/2! (t2 - t)^3/3! | |qdd(t) | |q(t2) - q(t) | + //| 1 (t2 - t) (t2 - t)^2/2! | |qddd(t)| |qd(t2) | + //" + + instantiateTaylorMatrix(); + for (size_t i = 0; i < order - 1; i++) + { + formTaylorRowwithTimeNodederivative(i, i, 0); + } + formTaylorRowwithTimeNodederivative(order - 1, order - 2, 1); +} + +void StableStartingBDF::setorder(size_t o) +{ + //"order is controlled by iStep." + if ((order != o) && (order != o + 1)) throw std::runtime_error("iStep and order must be consistent."); +} + +void StableStartingBDF::setiStep(size_t i) +{ + //"iStep is the current step of interest." + //"iStep must increase consecutively." + + auto iStepNew = iStep + 1; + if (iStepNew == i) { + iStep = iStepNew; + order = iStep + 1; + } + else { + throw std::runtime_error("Not appropriate iStep"); + } +} + +FColDsptr StableStartingBDF::derivativepresentpastpresentDerivativepastDerivative(size_t deriv, + FColDsptr y, std::shared_ptr> ypast, + FColDsptr ydot, std::shared_ptr> ydotpast) +{ + if (deriv == 0) return y->copy(); + auto series = std::make_shared>(order); + for (size_t j = 0; j < order - 1; j++) + { + series->at(j) = ypast->at(j)->minusFullColumn(y); + } + series->at(order - 1) = ydotpast->at(order - 2); + auto& coeffs = operatorMatrix->at(deriv - 1); + auto answer = coeffs->dot(series); + return std::static_pointer_cast>(answer); +} diff --git a/OndselSolver/StableStartingBDF.h b/OndselSolver/StableStartingBDF.h new file mode 100644 index 00000000..780ecbf0 --- /dev/null +++ b/OndselSolver/StableStartingBDF.h @@ -0,0 +1,36 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "StableBackwardDifference.h" + +namespace MbD { + + +#pragma once + class StableStartingBDF : public StableBackwardDifference + { + // + public: + static std::shared_ptr With(); + void initialize() override; + + void initializeLocally() override; + double pvdotpv() override; + void formTaylorMatrix() override; + void setorder(size_t o) override; + void setiStep(size_t i) override; + FColDsptr derivativepresentpastpresentDerivativepastDerivative(size_t n, + FColDsptr y, std::shared_ptr> ypast, + FColDsptr ydot, std::shared_ptr> ydotpast); + FColDsptr derivativewith(size_t deriv, std::shared_ptr> series); + + + }; +} diff --git a/OndselSolver/StartingBasicDAEIntegrator.cpp b/OndselSolver/StartingBasicDAEIntegrator.cpp new file mode 100644 index 00000000..174cf92b --- /dev/null +++ b/OndselSolver/StartingBasicDAEIntegrator.cpp @@ -0,0 +1,104 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#include + +#include "StartingBasicDAEIntegrator.h" + +using namespace MbD; + + +std::shared_ptr StartingBasicDAEIntegrator::With() +{ + auto inst = std::make_shared(); + inst->initialize(); + return inst; +} + +void StartingBasicDAEIntegrator::initialize() +{ + BasicDAEIntegrator::initialize(); + startingBDF = StableStartingBDF::With(); + startingBDF->timeNodes = tpast; +} + +void StartingBasicDAEIntegrator::initializeLocally() +{ + BasicDAEIntegrator::initializeLocally(); + startingBDF->initializeLocally(); +} + +FColDsptr StartingBasicDAEIntegrator::yDerivat(size_t n, double time) +{ + return startingBDF->derivativeatpresentpastpresentDerivativepastDerivative(n, time, y, ypast, ydot, ydotpast); +} + +std::shared_ptr StartingBasicDAEIntegrator::correctorBDF() +{ + return startingBDF; +} + +void StartingBasicDAEIntegrator::calcOperatorMatrix() +{ + BasicDAEIntegrator::calcOperatorMatrix(); + startingBDF->calcOperatorMatrix(); +} + +void StartingBasicDAEIntegrator::setorder(size_t o) +{ + BasicDAEIntegrator::setorder(o); + startingBDF->setorder(o + 1); +} + +void StartingBasicDAEIntegrator::settime(double t) +{ + BasicDAEIntegrator::settime(t); + startingBDF->settime(t); +} + +void StartingBasicDAEIntegrator::iStep(size_t i) +{ + BasicDAEIntegrator::iStep(i); + startingBDF->setiStep(i); +} + +FColDsptr StartingBasicDAEIntegrator::yDeriv(size_t order) +{ + return startingBDF->derivativepresentpastpresentDerivativepastDerivative(order, y, ypast, ydot, ydotpast); +} + +FColDsptr StartingBasicDAEIntegrator::dyOrderPlusOnedt() +{ + return startingBDF->derivativepresentpastpresentDerivativepastDerivative(order + 1, y, ypast, ydot, ydotpast); +} + +void StartingBasicDAEIntegrator::run() +{ + preRun(); + initializeLocally(); + initializeGlobally(); + firstSteps(); +} + +void StartingBasicDAEIntegrator::firstSteps() +{ + firstStep(); + earlySteps(); +} + +void StartingBasicDAEIntegrator::earlySteps() +{ + while (_continue) { + if (istep < orderMax) { + nextStep(); + } + else { + break; + } + } +} diff --git a/OndselSolver/StartingBasicDAEIntegrator.h b/OndselSolver/StartingBasicDAEIntegrator.h new file mode 100644 index 00000000..37a37e08 --- /dev/null +++ b/OndselSolver/StartingBasicDAEIntegrator.h @@ -0,0 +1,37 @@ +/*************************************************************************** + * Copyright (c) 2023 Ondsel, Inc. * + * * + * This file is part of OndselSolver. * + * * + * See LICENSE file for details about copyright. * + ***************************************************************************/ + +#pragma once + +#include "BasicDAEIntegrator.h" +#include "StableStartingBDF.h" + +namespace MbD { + class StartingBasicDAEIntegrator : public BasicDAEIntegrator + { + // + public: + static std::shared_ptr With(); + void initialize() override; + + void initializeLocally() override; + FColDsptr yDerivat(size_t _order, double tout) override; + std::shared_ptr correctorBDF() override; + void calcOperatorMatrix() override; + void setorder(size_t o) override; + void settime(double t) override; + void iStep(size_t i) override; + FColDsptr yDeriv(size_t order); + FColDsptr dyOrderPlusOnedt() override; + void run() override; + void firstSteps(); + void earlySteps(); + + std::shared_ptr startingBDF; + }; +} diff --git a/OndselSolver/System.cpp b/OndselSolver/System.cpp index 9e402dc9..b046b2c8 100644 --- a/OndselSolver/System.cpp +++ b/OndselSolver/System.cpp @@ -89,6 +89,30 @@ void System::runKINEMATIC(std::shared_ptr self) externalSystem->postMbDrun(); } +void System::runDYNAMIC(std::shared_ptr self) +{ + externalSystem->preMbDrun(self); + while (true) { + initializeLocally(); + initializeGlobally(); + if (!hasChanged) break; + } + if (dynamicEvents && dynamicEvents->prepare) dynamicEvents->prepare(); + partsJointsMotionsLimitsForcesTorquesDo([](std::shared_ptr item) { item->postInput(); }); + externalSystem->outputFor(INPUT); + systemSolver->runAllIC(); + systemSolver->releaseSeparatingLimits(); + if (dynamicEvents && dynamicEvents->initialize(mbdTimeValue())) { + do { + systemSolver->runAllIC(); + systemSolver->releaseSeparatingLimits(); + } while (dynamicEvents->settle(mbdTimeValue())); + } + externalSystem->outputFor(INITIALCONDITION); + systemSolver->runBasicDynamic(); + externalSystem->postMbDrun(); +} + void System::initializeLocally() { hasChanged = false; @@ -191,6 +215,7 @@ std::shared_ptr>> System::essentialConst { auto essenConstraints = std::make_shared>>(); this->partsJointsMotionsDo([&](std::shared_ptr item) { item->fillEssenConstraints(essenConstraints); }); + for (const auto& limit : *limits) limit->fillEssenConstraints(essenConstraints); return essenConstraints; } @@ -198,6 +223,7 @@ std::shared_ptr>> System::displacementCo { auto dispConstraints = std::make_shared>>(); this->jointsMotionsDo([&](std::shared_ptr joint) { joint->fillDispConstraints(dispConstraints); }); + for (const auto& limit : *limits) limit->fillDispConstraints(dispConstraints); return dispConstraints; } @@ -205,6 +231,7 @@ std::shared_ptr>> System::perpendicularC { auto perpenConstraints = std::make_shared>>(); this->jointsMotionsDo([&](std::shared_ptr joint) { joint->fillPerpenConstraints(perpenConstraints); }); + for (const auto& limit : *limits) limit->fillPerpenConstraints(perpenConstraints); return perpenConstraints; } diff --git a/OndselSolver/System.h b/OndselSolver/System.h index 2f775b7b..823f8c65 100644 --- a/OndselSolver/System.h +++ b/OndselSolver/System.h @@ -22,6 +22,7 @@ #include "Item.h" #include "LimitIJ.h" +#include "DynamicEvents.h" namespace MbD { class Part; @@ -47,6 +48,7 @@ namespace MbD { void runPreDrag(std::shared_ptr self); void runDragStep(std::shared_ptr self, std::shared_ptr>> dragParts); void runKINEMATIC(std::shared_ptr self); + void runDYNAMIC(std::shared_ptr self); std::shared_ptr> discontinuitiesAtIC(); void jointsMotionsDo(const std::function )>& f); void partsJointsMotionsDo(const std::function )>& f); @@ -85,5 +87,6 @@ namespace MbD { std::shared_ptr systemSolver; std::shared_ptr