Add forward rigid-body dynamics with loads, stops and analytical regression tests

CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.16)
 
-project(OndselSolver VERSION 1.0.1 DESCRIPTION "Assembly Constraints and Multibody Dynamics code")
+project(OndselSolver VERSION 2.0.0 DESCRIPTION "Assembly Constraints and Multibody Dynamics code")
 
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED True)

DYNAMICS.md

@@ -0,0 +1,112 @@
+# Forward rigid-body dynamics
+
+Forward dynamics is implemented in the bundled OndselSolver library. FreeCADMbD
+is a source of ported code, **not** an additional solver dependency or runtime
+backend. The existing assembly-solving and `runKINEMATIC()` entry points remain.
+
+Kinematic simulations retain upstream's drag-only limit behavior. Forward
+dynamics includes active stops in both initial-condition and integration
+equations, so impacts and separating reactions are handled during the run.
+
+This release changes C++ class layouts and virtual interfaces. Rebuild callers
+against the new headers and library; it is not a binary-compatible replacement
+for the version-1 shared library. The shared-library ABI version is now 2.
+
+## Entry point and inputs
+
+Construct an `ASMTAssembly` using the existing parts, mass markers, attachment
+markers and joint classes, then call `assembly->runDYNAMIC()`.
+
+Moving bodies need finite positive mass and three finite positive principal
+inertias about their centre of mass. Place/orient the principal mass marker at
+that centre in the body's coordinate system. Supply consistent length, mass and
+time units throughout; there is no automatic conversion from FreeCAD's document
+units in this solver API. Angles and angular velocities use radians.
+
+Set `constantGravity` using the existing `ASMTConstantGravity` API, including an
+explicit zero vector if gravity is unwanted. `ASMTSimulationParameters` supplies
+the increasing time interval, output interval, positive minimum/maximum steps,
+integration and corrector tolerances, maximum iterations and BDF order (1–5).
+`seterrorTol()` sets both kinematic and dynamic tolerances; the individual fields
+allow finer control. `iterMaxDyn` is separate from the kinematic iteration limit.
+
+Current placements and velocities are the initial inputs to each run. As in the
+existing solver API, producing results updates the ASMT objects. Explicitly
+restore the desired initial state before repeating a simulation. A dynamics run
+clears previous output histories. Exceptions propagate to the caller; a failed
+run can leave a partial history and must not be presented as a completed result.
+
+## Loads
+
+`ASMTForceTorque::With()` creates a typed load. Set its name and the full names of
+markers I and J, configure it, and attach it with `assembly->addForceTorque(load)`.
+
+```cpp
+auto load = MbD::ASMTForceTorque::With();
+load->setName("DriveTorque");
+load->setMarkerI(bodyMarker->fullName(""));
+load->setMarkerJ(groundMarker->fullName(""));
+load->setTorque3D(0.0, 0.0, 0.8);
+assembly->addForceTorque(load);
+assembly->runDYNAMIC();
+```
+
+`setForce3D(x,y,z)` and `setTorque3D(x,y,z)` select a constant, **world-resolved**
+wrench acting at marker I. The balancing wrench acts at J. For separated markers,
+the opposite force is transported to J with the balancing couple, so total force
+and moment are both zero. To apply an external load to a body, use that body's
+marker as I and a grounded marker as J. These are not body-following vectors.
+
+Alternatively, `setSpringDamper(k,c,l0)` selects a line spring/damper. With
+`d = rJ-rI`, `l = |d|`, `u = d/l`, and `v = vJ-vI`, its force on I is:
+
+```
+F_I = [k (l-l0) + c (u dot v)] u
+F_J = -F_I
+```
+
+Stiffness, damping and rest length must be finite and nonnegative. Coincident
+attachment points are rejected because the force direction is undefined. Load
+markers must be fixed to their bodies. The force/torque setters switch back to
+constant-wrench mode. Analytical position and velocity Jacobians include marker
+offsets and quaternion derivatives. No symbolic load-expression parser is added.
+
+The ASMT serialization uses explicit `WorldWrench` and `LinearSpringDamper` load
+records. It does not interpret FreeCADMbD's expression-based load records.
+
+## Outputs and scope
+
+Existing ASMT histories contain sampled poses, velocities, accelerations and
+joint/motion reactions. Load `fxs/fys/fzs` and `txs/tys/tzs` histories are world
+components acting at marker I (torque is about that marker). The output includes
+input and assembled-initial-condition samples at the start, regular output times,
+and the exact requested end time, even when it is off the regular output grid.
+Check convergence separately for motion and reactions: a satisfactory trajectory
+does not by itself establish sufficiently accurate acceleration or joint forces.
+
+This is the solver foundation; FreeCAD's Assembly module supplies the mass/unit
+adapter, persistent simulation inputs, task panels, plotting and playback.
+Translation and rotation limits support both rigid unilateral constraints and
+compliant stiffness/damping behavior. `ASMTDistanceLimit` supplies the same
+behavior for exact sphere-sphere contact in Assembly, including rigid drag
+separation and compliant dynamic impact. Runtime-evaluated force callbacks let
+FreeCAD's Assembly module supply transformed BRep contact forces without adding
+an OpenCASCADE dependency to OndselSolver; Assembly also uses these callbacks
+for regularized tangential contact friction. General solver-native friction, flexible bodies, FEM
+coupling and cancellable background integration are not implemented or qualified
+for forward dynamics.
+Existing kinematic use of unsupported features is unchanged.
+
+## Port provenance
+
+The DAE/BDF integrator, corrector and associated dynamics hooks were adapted from
+`aiksiongkoh/FreeCADMbD`, revision `ef7d572` (2025-12-09), which retains the
+constraint architecture used by this bundled solver. Copyright/license headers
+are retained. The port preserves C++17 and does not introduce Boost or a second
+solver library. Later FreeCADMbD class/API restructuring was not imported.
+
+Local adaptations include the typed load layer, tolerance wiring, initial
+momentum derivatives, the quaternion-acceleration setter fix, safe column-vector
+results, scaled Taylor-matrix inversion with propagated failures, startup
+interpolation, final-time sampling, input validation and rerun history handling.
+See `tests/README.md` and `tests/TestDynamics.cpp` for executable qualification.

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
@@ -381,7 +382,9 @@ std::shared_ptr<ASMTAssembly> 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;
@@ -812,6 +815,9 @@ void MbD::ASMTAssembly::readLimits(std::vector<std::string>& 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();
         }
@@ -923,34 +929,16 @@ void MbD::ASMTAssembly::readTimes(std::vector<std::string>& lines)
 
 void MbD::ASMTAssembly::readPartSeriesMany(std::vector<std::string>& lines)
 {
-    if (lines.empty()) {
-        return;
+    while (!lines.empty() && readString(lines.front()).rfind("PartSeries\t", 0) == 0) {
+        readPartSeries(lines);
     }
-    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<std::string> partSeriesLines(lines.begin(), it);
-    while (!partSeriesLines.empty()) {
-        readPartSeries(partSeriesLines);
-    }
-    lines.erase(lines.begin(), it);
 }
 
 void MbD::ASMTAssembly::readJointSeriesMany(std::vector<std::string>& lines)
 {
-    if (lines.empty()) {
-        return;
-    }
-    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<std::string> jointSeriesLines(lines.begin(), it);
-    while (!jointSeriesLines.empty()) {
-        readJointSeries(jointSeriesLines);
+    while (!lines.empty() && readString(lines.front()).rfind("JointSeries\t", 0) == 0) {
+        readJointSeries(lines);
     }
-    lines.erase(lines.begin(), it);
 }
 
 void MbD::ASMTAssembly::readAssemblySeries(std::vector<std::string>& lines)
@@ -1001,6 +989,13 @@ void MbD::ASMTAssembly::readPartSeries(std::vector<std::string>& lines)
     auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& 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 +1015,16 @@ void MbD::ASMTAssembly::readJointSeries(std::vector<std::string>& lines)
         std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr<ASMTJoint>& 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<std::string>& 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 +1044,9 @@ void MbD::ASMTAssembly::readMotionSeries(std::vector<std::string>& lines)
         std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr<ASMTMotion>& 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 +1062,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<std::vector<std::shared_ptr<ASMTPart>>>();
-        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<FullMatrix<double>>(3);
             for (size_t i = 0; i < 3; i++) {
                 auto row = readRowOfDoublesOffTop(lines);
@@ -1086,7 +1091,7 @@ void MbD::ASMTAssembly::runDraggingLog(const std::string& fileName)
         }
         runDragStep(dragParts);
     }
-    assert(readStringOffTop(lines) == "runPostDrag");
+    expectRecord("runPostDrag");
     runPostDrag();
 }
 
@@ -1439,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<System>();
+    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<System>();
@@ -1569,6 +1616,11 @@ void MbD::ASMTAssembly::updateFromMbD()
     for (auto& forceTorque : *forcesTorques) {
         forceTorque->updateFromMbD();
     }
+    if (mbdSystem->runMode != System::RunMode::Kinematic) {
+        for (auto& limit : *limits) {
+            limit->updateFromMbD();
+        }
+    }
 }
 
 void MbD::ASMTAssembly::compareResults(AnalysisType type)
@@ -1617,6 +1669,13 @@ void MbD::ASMTAssembly::addMotion(std::shared_ptr<ASMTMotion> motion)
     motion->initMarkers();
 }
 
+void ASMTAssembly::addForceTorque(std::shared_ptr<ASMTForceTorque> 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<ASMTLimit> limit)
 {
     limits->push_back(limit);

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<std::vector<std::shared_ptr<ASMTPart>>> dragParts);
 		void runPostDrag();
 		void restorePosRot();
-		void runKINEMATIC();
+        void runKINEMATIC();
+        // Forward dynamics. Unlike the legacy kinematic wrapper, errors propagate.
+        void runDYNAMIC();
+        std::shared_ptr<DynamicEvents> dynamicEvents;
 		void initprincipalMassMarker();
 		std::shared_ptr<ASMTSpatialContainer> spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname) const;
 		std::shared_ptr<ASMTPart> partAt(const std::string& longname) const;
@@ -117,6 +121,7 @@ namespace MbD {
 		void outputResults(AnalysisType type) override;
 		void addPart(std::shared_ptr<ASMTPart> part);
 		void addJoint(std::shared_ptr<ASMTJoint> joint);
+        void addForceTorque(std::shared_ptr<ASMTForceTorque> load);
 		void addMotion(std::shared_ptr<ASMTMotion> motion);
 		void addLimit(std::shared_ptr<ASMTLimit> limit);
 		void setConstantGravity(std::shared_ptr<ASMTConstantGravity> constantGravity);