diff --git a/.gitmodules b/.gitmodules index e646a57f29..d20f33113e 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,6 +13,8 @@ [submodule "mods/ztools"] path = mods/ztools url = https://git.kindred-systems.com/forbes/ztools.git + branch = main [submodule "mods/silo"] path = mods/silo url = https://git.kindred-systems.com/kindred/silo-mod.git + branch = main diff --git a/mods/silo b/mods/silo index 43e905c00a..dfa1da97dd 160000 --- a/mods/silo +++ b/mods/silo @@ -1 +1 @@ -Subproject commit 43e905c00a5d568c4fc37e0da1bf697a1b6119f7 +Subproject commit dfa1da97dd1f661a0e69f6b656900e55bd9dfb79 diff --git a/src/Mod/Assembly/App/AppAssembly.cpp b/src/Mod/Assembly/App/AppAssembly.cpp index 118e18f029..b378c23f98 100644 --- a/src/Mod/Assembly/App/AppAssembly.cpp +++ b/src/Mod/Assembly/App/AppAssembly.cpp @@ -26,6 +26,8 @@ #include #include +#include + #include "AssemblyObject.h" #include "AssemblyLink.h" #include "BomObject.h" @@ -54,6 +56,10 @@ PyMOD_INIT_FUNC(AssemblyApp) } PyObject* mod = Assembly::initModule(); + + // Register the built-in OndselSolver adapter with the solver registry. + KCSolve::OndselAdapter::register_solver(); + Base::Console().log("Loading Assembly module... done\n"); diff --git a/src/Mod/Assembly/App/AssemblyObject.cpp b/src/Mod/Assembly/App/AssemblyObject.cpp index ca480c88a5..89400b1f3c 100644 --- a/src/Mod/Assembly/App/AssemblyObject.cpp +++ b/src/Mod/Assembly/App/AssemblyObject.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -43,39 +44,8 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include "AssemblyLink.h" #include "AssemblyObject.h" @@ -87,19 +57,42 @@ FC_LOG_LEVEL_INIT("Assembly", true, true, true) using namespace Assembly; -using namespace MbD; - namespace PartApp = Part; +// ── Transform conversion helpers ─────────────────────────────────── + +static KCSolve::Transform placementToTransform(const Base::Placement& plc) +{ + KCSolve::Transform tf; + Base::Vector3d pos = plc.getPosition(); + tf.position = {pos.x, pos.y, pos.z}; + + // Base::Rotation(q0,q1,q2,q3) = (x,y,z,w) + // KCSolve::Transform quaternion = (w,x,y,z) + double q0, q1, q2, q3; + plc.getRotation().getValue(q0, q1, q2, q3); + tf.quaternion = {q3, q0, q1, q2}; + + return tf; +} + +static Base::Placement transformToPlacement(const KCSolve::Transform& tf) +{ + Base::Vector3d pos(tf.position[0], tf.position[1], tf.position[2]); + // KCSolve (w,x,y,z) → Base::Rotation(x,y,z,w) + Base::Rotation rot(tf.quaternion[1], tf.quaternion[2], tf.quaternion[3], tf.quaternion[0]); + return Base::Placement(pos, rot); +} + + // ================================ Assembly Object ============================ PROPERTY_SOURCE(Assembly::AssemblyObject, App::Part) AssemblyObject::AssemblyObject() - : mbdAssembly(std::make_shared()) - , bundleFixed(false) + : bundleFixed(false) , lastDoF(0) , lastHasConflict(false) , lastHasRedundancies(false) @@ -107,8 +100,6 @@ AssemblyObject::AssemblyObject() , lastHasMalformedConstraints(false) , lastSolverStatus(0) { - mbdAssembly->externalSystem->freecadAssemblyObject = this; - lastDoF = numberOfComponents() * 6; signalSolverUpdate(); } @@ -150,32 +141,47 @@ void AssemblyObject::onChanged(const App::Property* prop) App::Part::onChanged(prop); } + +// ── Solver integration ───────────────────────────────────────────── + +KCSolve::IKCSolver* AssemblyObject::getOrCreateSolver() +{ + if (!solver_) { + solver_ = KCSolve::SolverRegistry::instance().get("ondsel"); + } + return solver_.get(); +} + int AssemblyObject::solve(bool enableRedo, bool updateJCS) { ensureIdentityPlacements(); - mbdAssembly = makeMbdAssembly(); - objectPartMap.clear(); - motions.clear(); + auto* solver = getOrCreateSolver(); + if (!solver) { + FC_ERR("No solver available"); + lastSolverStatus = -1; + return -1; + } - auto groundedObjs = fixGroundedParts(); + partIdToObjs_.clear(); + objToPartId_.clear(); + + auto groundedObjs = getGroundedParts(); if (groundedObjs.empty()) { - // If no part fixed we can't solve. return -6; } std::vector joints = getJoints(updateJCS); - removeUnconnectedJoints(joints, groundedObjs); - jointParts(joints); + KCSolve::SolveContext ctx = buildSolveContext(joints); // Always save placements to enable orientation flip detection savePlacementsForUndo(); try { - mbdAssembly->runPreDrag(); - lastSolverStatus = 0; + lastResult_ = solver->solve(ctx); + lastSolverStatus = static_cast(lastResult_.status); } catch (const std::exception& e) { FC_ERR("Solve failed: " << e.what()); @@ -190,6 +196,11 @@ int AssemblyObject::solve(bool enableRedo, bool updateJCS) return -1; } + if (lastResult_.status == KCSolve::SolveStatus::Failed) { + updateSolveStatus(); + return -1; + } + // Validate that the solve didn't cause any parts to flip orientation if (!validateNewPlacements()) { // Restore previous placements - the solve found an invalid configuration @@ -220,106 +231,96 @@ void AssemblyObject::updateSolveStatus() //+1 because there's a grounded joint to origin lastDoF = (1 + numberOfComponents()) * 6; - if (!mbdAssembly || !mbdAssembly->mbdSystem) { + if (!solver_ || lastResult_.placements.empty()) { solve(); } - if (!mbdAssembly || !mbdAssembly->mbdSystem) { + if (!solver_) { return; } - // Helper lambda to clean up the joint name from the solver - auto cleanJointName = [](const std::string& rawName) -> std::string { - // rawName is like : /OndselAssembly/ground_moves#Joint001 - size_t hashPos = rawName.find_last_of('#'); - if (hashPos != std::string::npos) { - // Return the substring after the '#' - return rawName.substr(hashPos + 1); - } - return rawName; - }; - - - // Iterate through all joints and motions in the MBD system - mbdAssembly->mbdSystem->jointsMotionsDo([&](std::shared_ptr jm) { - if (!jm) { - return; - } - // Base::Console().warning("jm->name %s\n", jm->name); - bool isJointRedundant = false; - - jm->constraintsDo([&](std::shared_ptr con) { - if (!con) { - return; - } - - std::string spec = con->constraintSpec(); - // A constraint is redundant if its spec starts with "Redundant" - if (spec.rfind("Redundant", 0) == 0) { - isJointRedundant = true; - } - // Base::Console().warning(" - %s\n", spec); - --lastDoF; - }); - - const std::string fullName = cleanJointName(jm->name); - App::DocumentObject* docObj = getDocument()->getObject(fullName.c_str()); - - // We only care about objects that are actual joints in the FreeCAD document. - // This effectively filters out the grounding joints, which are named after parts. - if (!docObj || !docObj->getPropertyByName("Reference1")) { - return; - } - - if (isJointRedundant) { - // Check if this joint is already in the list to avoid duplicates - std::string objName = docObj->getNameInDocument(); - if (std::find(lastRedundantJoints.begin(), lastRedundantJoints.end(), objName) - == lastRedundantJoints.end()) { - lastRedundantJoints.push_back(objName); - } - } - }); - - // Update the summary boolean flag - if (!lastRedundantJoints.empty()) { - lastHasRedundancies = true; + // Use DOF from the solver result if available + if (lastResult_.dof >= 0) { + lastDoF = lastResult_.dof; } + // Process diagnostics from the solver result + for (const auto& diag : lastResult_.diagnostics) { + // Filter to only actual FreeCAD joint objects (not grounding joints) + App::DocumentObject* docObj = getDocument()->getObject(diag.constraint_id.c_str()); + if (!docObj || !docObj->getPropertyByName("Reference1")) { + continue; + } + + std::string objName = docObj->getNameInDocument(); + + switch (diag.kind) { + case KCSolve::ConstraintDiagnostic::Kind::Redundant: + if (std::find(lastRedundantJoints.begin(), lastRedundantJoints.end(), objName) + == lastRedundantJoints.end()) { + lastRedundantJoints.push_back(objName); + } + break; + case KCSolve::ConstraintDiagnostic::Kind::Conflicting: + if (std::find(lastConflictingJoints.begin(), lastConflictingJoints.end(), objName) + == lastConflictingJoints.end()) { + lastConflictingJoints.push_back(objName); + } + break; + case KCSolve::ConstraintDiagnostic::Kind::PartiallyRedundant: + if (std::find(lastPartialRedundantJoints.begin(), lastPartialRedundantJoints.end(), objName) + == lastPartialRedundantJoints.end()) { + lastPartialRedundantJoints.push_back(objName); + } + break; + case KCSolve::ConstraintDiagnostic::Kind::Malformed: + if (std::find(lastMalformedJoints.begin(), lastMalformedJoints.end(), objName) + == lastMalformedJoints.end()) { + lastMalformedJoints.push_back(objName); + } + break; + } + } + + lastHasRedundancies = !lastRedundantJoints.empty(); + lastHasConflict = !lastConflictingJoints.empty(); + lastHasPartialRedundancies = !lastPartialRedundantJoints.empty(); + lastHasMalformedConstraints = !lastMalformedJoints.empty(); + signalSolverUpdate(); } int AssemblyObject::generateSimulation(App::DocumentObject* sim) { - mbdAssembly = makeMbdAssembly(); - objectPartMap.clear(); + auto* solver = getOrCreateSolver(); + if (!solver) { + return -1; + } - motions = getMotionsFromSimulation(sim); + partIdToObjs_.clear(); + objToPartId_.clear(); - auto groundedObjs = fixGroundedParts(); + auto groundedObjs = getGroundedParts(); if (groundedObjs.empty()) { - // If no part fixed we can't solve. return -6; } std::vector joints = getJoints(); - removeUnconnectedJoints(joints, groundedObjs); - jointParts(joints); - - create_mbdSimulationParameters(sim); + KCSolve::SolveContext ctx = buildSolveContext(joints, true, sim); try { - mbdAssembly->runKINEMATIC(); + lastResult_ = solver->run_kinematic(ctx); } catch (...) { Base::Console().error("Generation of simulation failed\n"); - motions.clear(); return -1; } - motions.clear(); + if (lastResult_.status == KCSolve::SolveStatus::Failed) { + return -1; + } return 0; } @@ -340,16 +341,16 @@ std::vector AssemblyObject::getMotionsFromSimulation(App:: int Assembly::AssemblyObject::updateForFrame(size_t index, bool updateJCS) { - if (!mbdAssembly) { + if (!solver_) { return -1; } - auto nfrms = mbdAssembly->numberOfFrames(); + auto nfrms = solver_->num_frames(); if (index >= nfrms) { return -1; } - mbdAssembly->updateForFrame(index); + lastResult_ = solver_->update_for_frame(index); setNewPlacements(); auto jointDocs = getJoints(updateJCS); redrawJointPlacements(jointDocs); @@ -358,13 +359,32 @@ int Assembly::AssemblyObject::updateForFrame(size_t index, bool updateJCS) size_t Assembly::AssemblyObject::numberOfFrames() { - return mbdAssembly->numberOfFrames(); + return solver_ ? solver_->num_frames() : 0; } void AssemblyObject::preDrag(std::vector dragParts) { bundleFixed = true; - solve(); + + auto* solver = getOrCreateSolver(); + if (!solver) { + bundleFixed = false; + return; + } + + partIdToObjs_.clear(); + objToPartId_.clear(); + + auto groundedObjs = getGroundedParts(); + if (groundedObjs.empty()) { + bundleFixed = false; + return; + } + + std::vector joints = getJoints(); + removeUnconnectedJoints(joints, groundedObjs); + + KCSolve::SolveContext ctx = buildSolveContext(joints); bundleFixed = false; draggedParts.clear(); @@ -380,60 +400,68 @@ void AssemblyObject::preDrag(std::vector dragParts) } // Some objects have been bundled, we don't want to add these to dragged parts - Base::Placement plc; - for (auto& pair : objectPartMap) { - App::DocumentObject* parti = pair.first; - if (parti != part) { - continue; - } - plc = pair.second.offsetPlc; + auto it = objToPartId_.find(part); + if (it == objToPartId_.end()) { + continue; } - if (!plc.isIdentity()) { - // If not identity, then it's a bundled object. Some bundled objects may - // have identity placement if they have the same position as the main object of - // the bundle. But they're not going to be a problem. + + // Check if this is a bundled (non-primary) object + const auto& mappings = partIdToObjs_[it->second]; + bool isBundled = false; + for (const auto& m : mappings) { + if (m.obj == part && !m.offset.isIdentity()) { + isBundled = true; + break; + } + } + if (isBundled) { continue; } draggedParts.push_back(part); } + + // Build drag part IDs for the solver + std::vector dragPartIds; + for (auto* part : draggedParts) { + auto idIt = objToPartId_.find(part); + if (idIt != objToPartId_.end()) { + dragPartIds.push_back(idIt->second); + } + } + + savePlacementsForUndo(); + + try { + lastResult_ = solver->pre_drag(ctx, dragPartIds); + setNewPlacements(); + } + catch (...) { + // If pre_drag fails, we still need to be in a valid state + } } void AssemblyObject::doDragStep() { try { - std::vector> dragMbdParts; + std::vector dragPlacements; for (auto& part : draggedParts) { if (!part) { continue; } - auto mbdPart = getMbDPart(part); - dragMbdParts.push_back(mbdPart); + auto idIt = objToPartId_.find(part); + if (idIt == objToPartId_.end()) { + continue; + } - // Update the MBD part's position Base::Placement plc = getPlacementFromProp(part, "Placement"); - Base::Vector3d pos = plc.getPosition(); - mbdPart->updateMbDFromPosition3D( - std::make_shared>(ListD {pos.x, pos.y, pos.z}) - ); - - // Update the MBD part's rotation - Base::Rotation rot = plc.getRotation(); - Base::Matrix4D mat; - rot.getValue(mat); - Base::Vector3d r0 = mat.getRow(0); - Base::Vector3d r1 = mat.getRow(1); - Base::Vector3d r2 = mat.getRow(2); - mbdPart->updateMbDFromRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z); + dragPlacements.push_back({idIt->second, placementToTransform(plc)}); } - // Timing mbdAssembly->runDragStep() - auto dragPartsVec = std::make_shared>>(dragMbdParts); - mbdAssembly->runDragStep(dragPartsVec); + lastResult_ = solver_->drag_step(dragPlacements); - // Timing the validation and placement setting if (validateNewPlacements()) { setNewPlacements(); @@ -451,23 +479,6 @@ void AssemblyObject::doDragStep() } } -Base::Placement AssemblyObject::getMbdPlacement(std::shared_ptr mbdPart) -{ - if (!mbdPart) { - return Base::Placement(); - } - - double x, y, z; - mbdPart->getPosition3D(x, y, z); - Base::Vector3d pos = Base::Vector3d(x, y, z); - - double q0, q1, q2, q3; - mbdPart->getQuarternions(q3, q0, q1, q2); - Base::Rotation rot = Base::Rotation(q0, q1, q2, q3); - - return Base::Placement(pos, rot); -} - bool AssemblyObject::validateNewPlacements() { // First we check if a grounded object has moved. It can happen that they flip. @@ -479,12 +490,26 @@ bool AssemblyObject::validateNewPlacements() if (propPlacement) { Base::Placement oldPlc = propPlacement->getValue(); - auto it = objectPartMap.find(obj); - if (it != objectPartMap.end()) { - std::shared_ptr mbdPart = it->second.part; - Base::Placement newPlacement = getMbdPlacement(mbdPart); - if (!it->second.offsetPlc.isIdentity()) { - newPlacement = newPlacement * it->second.offsetPlc; + auto idIt = objToPartId_.find(obj); + if (idIt == objToPartId_.end()) { + continue; + } + + // Find the new placement from lastResult_ + for (const auto& pr : lastResult_.placements) { + if (pr.id != idIt->second) { + continue; + } + + Base::Placement newPlacement = transformToPlacement(pr.placement); + + // Apply bundle offset if present + const auto& mappings = partIdToObjs_[pr.id]; + for (const auto& m : mappings) { + if (m.obj == obj && !m.offset.isIdentity()) { + newPlacement = newPlacement * m.offset; + break; + } } if (!oldPlc.isSame(newPlacement, Precision::Confusion())) { @@ -494,57 +519,66 @@ bool AssemblyObject::validateNewPlacements() ); return false; } + break; } } } // Check if any part has flipped orientation (rotation > 90 degrees from original) - // This prevents joints from "breaking" when the solver finds an alternate configuration for (const auto& savedPair : previousPositions) { App::DocumentObject* obj = savedPair.first; if (!obj) { continue; } - auto it = objectPartMap.find(obj); - if (it == objectPartMap.end()) { + auto idIt = objToPartId_.find(obj); + if (idIt == objToPartId_.end()) { continue; } - std::shared_ptr mbdPart = it->second.part; - if (!mbdPart) { - continue; - } + // Find the new placement from lastResult_ + for (const auto& pr : lastResult_.placements) { + if (pr.id != idIt->second) { + continue; + } - Base::Placement newPlacement = getMbdPlacement(mbdPart); - if (!it->second.offsetPlc.isIdentity()) { - newPlacement = newPlacement * it->second.offsetPlc; - } + Base::Placement newPlacement = transformToPlacement(pr.placement); - const Base::Placement& oldPlc = savedPair.second; + // Apply bundle offset if present + const auto& mappings = partIdToObjs_[pr.id]; + for (const auto& m : mappings) { + if (m.obj == obj && !m.offset.isIdentity()) { + newPlacement = newPlacement * m.offset; + break; + } + } - // Calculate the rotation difference between old and new orientations - Base::Rotation oldRot = oldPlc.getRotation(); - Base::Rotation newRot = newPlacement.getRotation(); + const Base::Placement& oldPlc = savedPair.second; - // Get the relative rotation: how much did the part rotate? - Base::Rotation relativeRot = newRot * oldRot.inverse(); + // Calculate the rotation difference between old and new orientations + Base::Rotation oldRot = oldPlc.getRotation(); + Base::Rotation newRot = newPlacement.getRotation(); - // Get the angle of this rotation - Base::Vector3d axis; - double angle; - relativeRot.getRawValue(axis, angle); + // Get the relative rotation: how much did the part rotate? + Base::Rotation relativeRot = newRot * oldRot.inverse(); - // If the part rotated more than 90 degrees, consider it a flip - // Use 91 degrees to allow for small numerical errors - constexpr double maxAngle = 91.0 * M_PI / 180.0; - if (std::abs(angle) > maxAngle) { - Base::Console().warning( - "Assembly : Ignoring bad solve, part (%s) flipped orientation (%.1f degrees).\n", - obj->getFullLabel(), - std::abs(angle) * 180.0 / M_PI - ); - return false; + // Get the angle of this rotation + Base::Vector3d axis; + double angle; + relativeRot.getRawValue(axis, angle); + + // If the part rotated more than 90 degrees, consider it a flip + // Use 91 degrees to allow for small numerical errors + constexpr double maxAngle = 91.0 * M_PI / 180.0; + if (std::abs(angle) > maxAngle) { + Base::Console().warning( + "Assembly : Ignoring bad solve, part (%s) flipped orientation (%.1f degrees).\n", + obj->getFullLabel(), + std::abs(angle) * 180.0 / M_PI + ); + return false; + } + break; } } @@ -553,7 +587,9 @@ bool AssemblyObject::validateNewPlacements() void AssemblyObject::postDrag() { - mbdAssembly->runPostDrag(); // Do this after last drag + if (solver_) { + solver_->post_drag(); + } purgeTouched(); } @@ -561,23 +597,20 @@ void AssemblyObject::savePlacementsForUndo() { previousPositions.clear(); - for (auto& pair : objectPartMap) { - App::DocumentObject* obj = pair.first; - if (!obj) { - continue; + for (const auto& [partId, mappings] : partIdToObjs_) { + for (const auto& mapping : mappings) { + App::DocumentObject* obj = mapping.obj; + if (!obj) { + continue; + } + + auto* propPlc = dynamic_cast(obj->getPropertyByName("Placement")); + if (!propPlc) { + continue; + } + + previousPositions.push_back({obj, propPlc->getValue()}); } - - std::pair savePair; - savePair.first = obj; - - // Check if the object has a "Placement" property - auto* propPlc = dynamic_cast(obj->getPropertyByName("Placement")); - if (!propPlc) { - continue; - } - savePair.second = propPlc->getValue(); - - previousPositions.push_back(savePair); } } @@ -616,43 +649,61 @@ void AssemblyObject::clearUndo() void AssemblyObject::exportAsASMT(std::string fileName) { - mbdAssembly = makeMbdAssembly(); - objectPartMap.clear(); - fixGroundedParts(); + auto* solver = getOrCreateSolver(); + if (!solver) { + return; + } + partIdToObjs_.clear(); + objToPartId_.clear(); + + auto groundedObjs = getGroundedParts(); std::vector joints = getJoints(); + removeUnconnectedJoints(joints, groundedObjs); - jointParts(joints); + KCSolve::SolveContext ctx = buildSolveContext(joints); - mbdAssembly->outputFile(fileName); + try { + solver->solve(ctx); + } + catch (...) { + // Build anyway for export + } + + solver->export_native(fileName); } void AssemblyObject::setNewPlacements() { - for (auto& pair : objectPartMap) { - App::DocumentObject* obj = pair.first; - std::shared_ptr mbdPart = pair.second.part; - - if (!obj || !mbdPart) { + for (const auto& pr : lastResult_.placements) { + auto it = partIdToObjs_.find(pr.id); + if (it == partIdToObjs_.end()) { continue; } - // Check if the object has a "Placement" property - auto* propPlacement = dynamic_cast( - obj->getPropertyByName("Placement") - ); - if (!propPlacement) { - continue; - } + Base::Placement basePlc = transformToPlacement(pr.placement); + for (const auto& mapping : it->second) { + App::DocumentObject* obj = mapping.obj; + if (!obj) { + continue; + } - Base::Placement newPlacement = getMbdPlacement(mbdPart); - if (!pair.second.offsetPlc.isIdentity()) { - newPlacement = newPlacement * pair.second.offsetPlc; - } - if (!propPlacement->getValue().isSame(newPlacement)) { - propPlacement->setValue(newPlacement); - obj->purgeTouched(); + auto* propPlacement = dynamic_cast( + obj->getPropertyByName("Placement") + ); + if (!propPlacement) { + continue; + } + + Base::Placement newPlacement = basePlc; + if (!mapping.offset.isIdentity()) { + newPlacement = basePlc * mapping.offset; + } + if (!propPlacement->getValue().isSame(newPlacement)) { + propPlacement->setValue(newPlacement); + obj->purgeTouched(); + } } } } @@ -698,20 +749,726 @@ void AssemblyObject::redrawJointPlacement(App::DocumentObject* joint) } } -std::shared_ptr AssemblyObject::makeMbdAssembly() + +// ── SolveContext building ────────────────────────────────────────── + +std::string AssemblyObject::registerPart(App::DocumentObject* obj) { - auto assembly = CREATE::With(); - assembly->externalSystem->freecadAssemblyObject = this; - assembly->setName("OndselAssembly"); + // Check if already registered + auto it = objToPartId_.find(obj); + if (it != objToPartId_.end()) { + return it->second; + } - ParameterGrp::handle hPgr = App::GetApplication().GetParameterGroupByPath( - "User parameter:BaseApp/Preferences/Mod/Assembly" - ); + std::string partId = obj->getFullName(); + Base::Placement plc = getPlacementFromProp(obj, "Placement"); - assembly->setDebug(hPgr->GetBool("LogSolverDebug", false)); - return assembly; + objToPartId_[obj] = partId; + partIdToObjs_[partId].push_back({obj, Base::Placement()}); + + // When bundling fixed joints, recursively discover connected parts + if (bundleFixed) { + auto addConnectedFixedParts = [&](App::DocumentObject* currentPart, auto& self) -> void { + std::vector joints = getJointsOfPart(currentPart); + for (auto* joint : joints) { + JointType jointType = getJointType(joint); + if (jointType == JointType::Fixed) { + App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); + App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); + App::DocumentObject* partToAdd = currentPart == part1 ? part2 : part1; + + if (objToPartId_.find(partToAdd) != objToPartId_.end()) { + continue; // already registered + } + + Base::Placement plci = getPlacementFromProp(partToAdd, "Placement"); + Base::Placement offset = plc.inverse() * plci; + + objToPartId_[partToAdd] = partId; + partIdToObjs_[partId].push_back({partToAdd, offset}); + + self(partToAdd, self); + } + } + }; + + addConnectedFixedParts(obj, addConnectedFixedParts); + } + + return partId; } +KCSolve::SolveContext AssemblyObject::buildSolveContext( + const std::vector& joints, + bool forSimulation, + App::DocumentObject* sim +) +{ + KCSolve::SolveContext ctx; + ctx.bundle_fixed = bundleFixed; + + // ── Parts: register grounded parts ───────────────────────────── + + auto groundedObjs = getGroundedParts(); + for (auto* obj : groundedObjs) { + if (!obj) { + continue; + } + + std::string partId = registerPart(obj); + Base::Placement plc = getPlacementFromProp(obj, "Placement"); + + KCSolve::Part part; + part.id = partId; + part.placement = placementToTransform(plc); + part.mass = getObjMass(obj); + part.grounded = true; + ctx.parts.push_back(std::move(part)); + } + + // ── Constraints: process each joint ──────────────────────────── + + // Collect motions for simulation + std::vector motionObjs; + if (forSimulation && sim) { + motionObjs = getMotionsFromSimulation(sim); + } + + for (auto* joint : joints) { + if (!joint) { + continue; + } + + JointType jointType = getJointType(joint); + + // When bundling fixed joints, skip Fixed type (parts are already bundled) + if (bundleFixed && jointType == JointType::Fixed) { + continue; + } + + // Determine BaseJointKind and params + KCSolve::BaseJointKind kind; + std::vector params; + + switch (jointType) { + case JointType::Fixed: + kind = KCSolve::BaseJointKind::Fixed; + break; + case JointType::Revolute: + kind = KCSolve::BaseJointKind::Revolute; + break; + case JointType::Cylindrical: + kind = KCSolve::BaseJointKind::Cylindrical; + break; + case JointType::Slider: + kind = KCSolve::BaseJointKind::Slider; + break; + case JointType::Ball: + kind = KCSolve::BaseJointKind::Ball; + break; + case JointType::Parallel: + kind = KCSolve::BaseJointKind::Parallel; + break; + case JointType::Perpendicular: + kind = KCSolve::BaseJointKind::Perpendicular; + break; + case JointType::Angle: { + double angle = fabs(Base::toRadians(getJointAngle(joint))); + if (fmod(angle, 2 * std::numbers::pi) < Precision::Confusion()) { + kind = KCSolve::BaseJointKind::Parallel; + } + else { + kind = KCSolve::BaseJointKind::Angle; + params.push_back(angle); + } + break; + } + case JointType::RackPinion: { + kind = KCSolve::BaseJointKind::RackPinion; + params.push_back(getJointDistance(joint)); + break; + } + case JointType::Screw: { + int slidingIndex = slidingPartIndex(joint); + if (slidingIndex == 0) { + continue; // invalid — needs a slider + } + if (slidingIndex != 1) { + swapJCS(joint); + } + kind = KCSolve::BaseJointKind::Screw; + params.push_back(getJointDistance(joint)); + break; + } + case JointType::Gears: { + kind = KCSolve::BaseJointKind::Gear; + params.push_back(getJointDistance(joint)); + params.push_back(getJointDistance2(joint)); + break; + } + case JointType::Belt: { + kind = KCSolve::BaseJointKind::Gear; + params.push_back(getJointDistance(joint)); + params.push_back(-getJointDistance2(joint)); + break; + } + case JointType::Distance: { + // Decompose based on geometry classification + DistanceType distType = getDistanceType(joint); + std::string elt1 = getElementFromProp(joint, "Reference1"); + std::string elt2 = getElementFromProp(joint, "Reference2"); + auto* obj1 = getLinkedObjFromRef(joint, "Reference1"); + auto* obj2 = getLinkedObjFromRef(joint, "Reference2"); + double distance = getJointDistance(joint); + + switch (distType) { + case DistanceType::PointPoint: + if (distance < Precision::Confusion()) { + kind = KCSolve::BaseJointKind::Coincident; + } + else { + kind = KCSolve::BaseJointKind::DistancePointPoint; + params.push_back(distance); + } + break; + + case DistanceType::LineLine: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance); + break; + case DistanceType::LineCircle: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance + getEdgeRadius(obj2, elt2)); + break; + case DistanceType::CircleCircle: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance + getEdgeRadius(obj1, elt1) + getEdgeRadius(obj2, elt2)); + break; + + case DistanceType::PlanePlane: + kind = KCSolve::BaseJointKind::Planar; + params.push_back(distance); + break; + case DistanceType::PlaneCylinder: + kind = KCSolve::BaseJointKind::LineInPlane; + params.push_back(distance + getFaceRadius(obj2, elt2)); + break; + case DistanceType::PlaneSphere: + kind = KCSolve::BaseJointKind::PointInPlane; + params.push_back(distance + getFaceRadius(obj2, elt2)); + break; + case DistanceType::PlaneTorus: + kind = KCSolve::BaseJointKind::Planar; + params.push_back(distance); + break; + + case DistanceType::CylinderCylinder: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + case DistanceType::CylinderSphere: + kind = KCSolve::BaseJointKind::DistanceCylSph; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + case DistanceType::CylinderTorus: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + + case DistanceType::TorusTorus: + kind = KCSolve::BaseJointKind::Planar; + params.push_back(distance); + break; + case DistanceType::TorusSphere: + kind = KCSolve::BaseJointKind::DistanceCylSph; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + case DistanceType::SphereSphere: + kind = KCSolve::BaseJointKind::DistancePointPoint; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + + case DistanceType::PointPlane: + kind = KCSolve::BaseJointKind::PointInPlane; + params.push_back(distance); + break; + case DistanceType::PointCylinder: + kind = KCSolve::BaseJointKind::DistanceCylSph; + params.push_back(distance + getFaceRadius(obj1, elt1)); + break; + case DistanceType::PointSphere: + kind = KCSolve::BaseJointKind::DistancePointPoint; + params.push_back(distance + getFaceRadius(obj1, elt1)); + break; + + case DistanceType::LinePlane: + kind = KCSolve::BaseJointKind::LineInPlane; + params.push_back(distance); + break; + + case DistanceType::PointLine: + kind = KCSolve::BaseJointKind::DistanceCylSph; + params.push_back(distance); + break; + case DistanceType::PointCurve: + kind = KCSolve::BaseJointKind::PointInPlane; + params.push_back(distance); + break; + + default: + kind = KCSolve::BaseJointKind::Planar; + params.push_back(distance); + break; + } + break; + } + default: + continue; + } + + // Validate the joint (skip self-referential bundled parts) + if (!isJointValid(joint)) { + continue; + } + + // Compute marker transforms + std::string partIdI, partIdJ; + KCSolve::Transform markerI, markerJ; + + if (jointType == JointType::RackPinion) { + auto rp = computeRackPinionMarkers(joint); + partIdI = rp.partIdI; + markerI = rp.markerI; + partIdJ = rp.partIdJ; + markerJ = rp.markerJ; + + if (partIdI.empty() || partIdJ.empty()) { + continue; + } + } + else { + // Resolve part IDs from joint references + App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); + App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); + if (!part1 || !part2) { + continue; + } + + // Ensure both parts are registered + partIdI = registerPart(part1); + partIdJ = registerPart(part2); + + markerI = computeMarkerTransform(joint, "Reference1", "Placement1"); + markerJ = computeMarkerTransform(joint, "Reference2", "Placement2"); + } + + // Build the constraint + KCSolve::Constraint c; + c.id = joint->getNameInDocument(); + c.part_i = partIdI; + c.marker_i = markerI; + c.part_j = partIdJ; + c.marker_j = markerJ; + c.type = kind; + c.params = std::move(params); + + // Add limits (only if not in simulation mode — motions may clash) + if (motionObjs.empty()) { + if (jointType == JointType::Slider || jointType == JointType::Cylindrical) { + auto* pLenMin = dynamic_cast(joint->getPropertyByName("LengthMin")); + auto* pLenMax = dynamic_cast(joint->getPropertyByName("LengthMax")); + auto* pMinEnabled = dynamic_cast( + joint->getPropertyByName("EnableLengthMin") + ); + auto* pMaxEnabled = dynamic_cast( + joint->getPropertyByName("EnableLengthMax") + ); + + if (pLenMin && pLenMax && pMinEnabled && pMaxEnabled) { + bool minEnabled = pMinEnabled->getValue(); + bool maxEnabled = pMaxEnabled->getValue(); + double minLength = pLenMin->getValue(); + double maxLength = pLenMax->getValue(); + + if ((minLength > maxLength) && minEnabled && maxEnabled) { + pLenMin->setValue(maxLength); + pLenMax->setValue(minLength); + minLength = maxLength; + maxLength = pLenMax->getValue(); + + pMinEnabled->setValue(maxEnabled); + pMaxEnabled->setValue(minEnabled); + minEnabled = maxEnabled; + maxEnabled = pMaxEnabled->getValue(); + } + + if (minEnabled) { + c.limits.push_back({ + KCSolve::Constraint::Limit::Kind::TranslationMin, + minLength, + 1.0e-9 + }); + } + if (maxEnabled) { + c.limits.push_back({ + KCSolve::Constraint::Limit::Kind::TranslationMax, + maxLength, + 1.0e-9 + }); + } + } + } + if (jointType == JointType::Revolute || jointType == JointType::Cylindrical) { + auto* pRotMin = dynamic_cast(joint->getPropertyByName("AngleMin")); + auto* pRotMax = dynamic_cast(joint->getPropertyByName("AngleMax")); + auto* pMinEnabled = dynamic_cast( + joint->getPropertyByName("EnableAngleMin") + ); + auto* pMaxEnabled = dynamic_cast( + joint->getPropertyByName("EnableAngleMax") + ); + + if (pRotMin && pRotMax && pMinEnabled && pMaxEnabled) { + bool minEnabled = pMinEnabled->getValue(); + bool maxEnabled = pMaxEnabled->getValue(); + double minAngle = pRotMin->getValue(); + double maxAngle = pRotMax->getValue(); + + if ((minAngle > maxAngle) && minEnabled && maxEnabled) { + pRotMin->setValue(maxAngle); + pRotMax->setValue(minAngle); + minAngle = maxAngle; + maxAngle = pRotMax->getValue(); + + pMinEnabled->setValue(maxEnabled); + pMaxEnabled->setValue(minEnabled); + minEnabled = maxEnabled; + maxEnabled = pMaxEnabled->getValue(); + } + + if (minEnabled) { + c.limits.push_back({ + KCSolve::Constraint::Limit::Kind::RotationMin, + minAngle, + 1.0e-9 + }); + } + if (maxEnabled) { + c.limits.push_back({ + KCSolve::Constraint::Limit::Kind::RotationMax, + maxAngle, + 1.0e-9 + }); + } + } + } + } + + ctx.constraints.push_back(std::move(c)); + + // Add motions for simulation + if (forSimulation) { + std::vector done; + for (auto* motion : motionObjs) { + if (std::ranges::find(done, motion) != done.end()) { + continue; + } + + auto* pJoint = dynamic_cast(motion->getPropertyByName("Joint")); + if (!pJoint) { + continue; + } + App::DocumentObject* motionJoint = pJoint->getValue(); + if (joint != motionJoint) { + continue; + } + + auto* pType = dynamic_cast(motion->getPropertyByName("MotionType")); + auto* pFormula = dynamic_cast(motion->getPropertyByName("Formula")); + if (!pType || !pFormula) { + continue; + } + std::string formula = pFormula->getValue(); + if (formula.empty()) { + continue; + } + std::string motionType = pType->getValueAsString(); + + // Check for paired motion (cylindrical joints can have both angular + linear) + for (auto* motion2 : motionObjs) { + auto* pJoint2 = dynamic_cast(motion2->getPropertyByName("Joint")); + if (!pJoint2) { + continue; + } + App::DocumentObject* motionJoint2 = pJoint2->getValue(); + if (joint != motionJoint2 || motion2 == motion) { + continue; + } + + auto* pType2 = dynamic_cast( + motion2->getPropertyByName("MotionType") + ); + auto* pFormula2 = dynamic_cast(motion2->getPropertyByName("Formula")); + if (!pType2 || !pFormula2) { + continue; + } + std::string formula2 = pFormula2->getValue(); + if (formula2.empty()) { + continue; + } + std::string motionType2 = pType2->getValueAsString(); + if (motionType2 == motionType) { + continue; + } + + // Two different motion types on same joint → General motion + KCSolve::MotionDef md; + md.kind = KCSolve::MotionDef::Kind::General; + md.joint_id = joint->getNameInDocument(); + md.marker_i = ""; // Adapter resolves from joint_id + md.marker_j = ""; + md.rotation_expr = motionType == "Angular" ? formula : formula2; + md.translation_expr = motionType == "Angular" ? formula2 : formula; + ctx.motions.push_back(std::move(md)); + + done.push_back(motion2); + } + + // Single motion + KCSolve::MotionDef md; + md.joint_id = joint->getNameInDocument(); + md.marker_i = ""; + md.marker_j = ""; + if (motionType == "Angular") { + md.kind = KCSolve::MotionDef::Kind::Rotational; + md.rotation_expr = formula; + } + else { + md.kind = KCSolve::MotionDef::Kind::Translational; + md.translation_expr = formula; + } + ctx.motions.push_back(std::move(md)); + } + } + } + + // ── Parts: ensure all referenced parts are in the context ────── + + // Some parts may have been registered during constraint processing + // but not yet added to ctx.parts + for (const auto& [partId, mappings] : partIdToObjs_) { + bool alreadyInCtx = false; + for (const auto& p : ctx.parts) { + if (p.id == partId) { + alreadyInCtx = true; + break; + } + } + if (!alreadyInCtx) { + App::DocumentObject* primaryObj = mappings[0].obj; + Base::Placement plc = getPlacementFromProp(primaryObj, "Placement"); + + KCSolve::Part part; + part.id = partId; + part.placement = placementToTransform(plc); + part.mass = getObjMass(primaryObj); + part.grounded = false; + ctx.parts.push_back(std::move(part)); + } + } + + // ── Simulation parameters ────────────────────────────────────── + + if (forSimulation && sim) { + auto valueOf = [](App::DocumentObject* docObj, const char* propName) { + auto* prop = dynamic_cast(docObj->getPropertyByName(propName)); + if (!prop) { + return 0.0; + } + return prop->getValue(); + }; + + KCSolve::SimulationParams sp; + sp.t_start = valueOf(sim, "aTimeStart"); + sp.t_end = valueOf(sim, "bTimeEnd"); + sp.h_out = valueOf(sim, "cTimeStepOutput"); + sp.h_min = 1.0e-9; + sp.h_max = 1.0; + sp.error_tol = valueOf(sim, "fGlobalErrorTolerance"); + ctx.simulation = sp; + } + + return ctx; +} + + +// ── Marker transform computation ─────────────────────────────────── + +KCSolve::Transform AssemblyObject::computeMarkerTransform( + App::DocumentObject* joint, + const char* propRefName, + const char* propPlcName +) +{ + App::DocumentObject* part = getMovingPartFromRef(joint, propRefName); + App::DocumentObject* obj = getObjFromJointRef(joint, propRefName); + + if (!part || !obj) { + Base::Console() + .warning("The property %s of Joint %s is bad.\n", propRefName, joint->getFullName()); + return KCSolve::Transform::identity(); + } + + Base::Placement plc = getPlacementFromProp(joint, propPlcName); + // Now we have plc which is the JCS placement, but its relative to the Object, not to the + // containing Part. + + if (obj->getNameInDocument() != part->getNameInDocument()) { + auto* ref = dynamic_cast(joint->getPropertyByName(propRefName)); + if (!ref) { + return KCSolve::Transform::identity(); + } + + Base::Placement obj_global_plc = getGlobalPlacement(obj, ref); + plc = obj_global_plc * plc; + + Base::Placement part_global_plc = getGlobalPlacement(part, ref); + plc = part_global_plc.inverse() * plc; + } + + // Apply bundle offset if present + auto idIt = objToPartId_.find(part); + if (idIt != objToPartId_.end()) { + const auto& mappings = partIdToObjs_[idIt->second]; + for (const auto& m : mappings) { + if (m.obj == part && !m.offset.isIdentity()) { + plc = m.offset * plc; + break; + } + } + } + + return placementToTransform(plc); +} + +AssemblyObject::RackPinionResult AssemblyObject::computeRackPinionMarkers(App::DocumentObject* joint) +{ + RackPinionResult result; + + // ASMT rack pinion joint must get the rack as I and pinion as J. + int slidingIndex = slidingPartIndex(joint); + if (slidingIndex == 0) { + return result; + } + + if (slidingIndex != 1) { + swapJCS(joint); // make sure that rack is first. + } + + App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); + App::DocumentObject* obj1 = getObjFromJointRef(joint, "Reference1"); + Base::Placement plc1 = getPlacementFromProp(joint, "Placement1"); + + App::DocumentObject* obj2 = getObjFromJointRef(joint, "Reference2"); + Base::Placement plc2 = getPlacementFromProp(joint, "Placement2"); + + if (!part1 || !obj1) { + Base::Console().warning("Reference1 of Joint %s is bad.\n", joint->getFullName()); + return result; + } + + // Ensure parts are registered + result.partIdI = registerPart(part1); + + // For the pinion — use standard marker computation + result.markerJ = computeMarkerTransform(joint, "Reference2", "Placement2"); + + App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); + if (part2) { + result.partIdJ = registerPart(part2); + } + + // For the rack — need to adjust placement so X axis aligns with slider axis + auto* ref1 = dynamic_cast(joint->getPropertyByName("Reference1")); + auto* ref2 = dynamic_cast(joint->getPropertyByName("Reference2")); + if (!ref1 || !ref2) { + return result; + } + + // Make the pinion plc relative to the rack placement + Base::Placement pinion_global_plc = getGlobalPlacement(obj2, ref2); + plc2 = pinion_global_plc * plc2; + Base::Placement rack_global_plc = getGlobalPlacement(obj1, ref1); + plc2 = rack_global_plc.inverse() * plc2; + + // The rot of the rack placement should be the same as the pinion, but with X axis along the + // slider axis. + Base::Rotation rot = plc2.getRotation(); + Base::Vector3d currentZAxis = rot.multVec(Base::Vector3d(0, 0, 1)); + Base::Vector3d currentXAxis = rot.multVec(Base::Vector3d(1, 0, 0)); + Base::Vector3d targetXAxis = plc1.getRotation().multVec(Base::Vector3d(0, 0, 1)); + + double yawAdjustment = currentXAxis.GetAngle(targetXAxis); + + Base::Vector3d crossProd = currentXAxis.Cross(targetXAxis); + if (currentZAxis * crossProd < 0) { + yawAdjustment = -yawAdjustment; + } + + Base::Rotation yawRotation(currentZAxis, yawAdjustment); + Base::Rotation adjustedRotation = rot * yawRotation; + plc1.setRotation(adjustedRotation); + + // Transform to part-relative coordinates (same as handleOneSideOfJoint end logic) + if (obj1->getNameInDocument() != part1->getNameInDocument()) { + plc1 = rack_global_plc * plc1; + Base::Placement part_global_plc = getGlobalPlacement(part1, ref1); + plc1 = part_global_plc.inverse() * plc1; + } + + // Apply bundle offset if present + auto idIt = objToPartId_.find(part1); + if (idIt != objToPartId_.end()) { + const auto& mappings = partIdToObjs_[idIt->second]; + for (const auto& m : mappings) { + if (m.obj == part1 && !m.offset.isIdentity()) { + plc1 = m.offset * plc1; + break; + } + } + } + + result.markerI = placementToTransform(plc1); + + return result; +} + +bool AssemblyObject::isJointValid(App::DocumentObject* joint) +{ + // When dragging, parts connected by fixed joints are bundled. + // A joint that references two parts in the same bundle is self-referential and must be skipped. + App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); + App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); + if (!part1 || !part2) { + return false; + } + + auto it1 = objToPartId_.find(part1); + auto it2 = objToPartId_.find(part2); + if (it1 != objToPartId_.end() && it2 != objToPartId_.end() && it1->second == it2->second) { + Base::Console().warning( + "Assembly: Ignoring joint (%s) because its parts are connected by a fixed " + "joint bundle. This joint is a conflicting or redundant constraint.\n", + joint->getFullLabel() + ); + return false; + } + return true; +} + + +// ── Joint / Part graph helpers (unchanged) ───────────────────────── + App::DocumentObject* AssemblyObject::getJointOfPartConnectingToGround( App::DocumentObject* part, std::string& name, @@ -952,50 +1709,6 @@ std::unordered_set AssemblyObject::getGroundedParts() return groundedSet; } -std::unordered_set AssemblyObject::fixGroundedParts() -{ - auto groundedParts = getGroundedParts(); - - for (auto obj : groundedParts) { - if (!obj) { - continue; - } - - Base::Placement plc = getPlacementFromProp(obj, "Placement"); - std::string str = obj->getFullName(); - fixGroundedPart(obj, plc, str); - } - return groundedParts; -} - -void AssemblyObject::fixGroundedPart(App::DocumentObject* obj, Base::Placement& plc, std::string& name) -{ - if (!obj) { - return; - } - - std::string markerName1 = "marker-" + obj->getFullName(); - auto mbdMarker1 = makeMbdMarker(markerName1, plc); - mbdAssembly->addMarker(mbdMarker1); - - std::shared_ptr mbdPart = getMbDPart(obj); - - std::string markerName2 = "FixingMarker"; - Base::Placement basePlc = Base::Placement(); - auto mbdMarker2 = makeMbdMarker(markerName2, basePlc); - mbdPart->addMarker(mbdMarker2); - - markerName1 = "/OndselAssembly/" + mbdMarker1->name; - markerName2 = "/OndselAssembly/" + mbdPart->name + "/" + mbdMarker2->name; - - auto mbdJoint = CREATE::With(); - mbdJoint->setName(name); - mbdJoint->setMarkerI(markerName1); - mbdJoint->setMarkerJ(markerName2); - - mbdAssembly->addJoint(mbdJoint); -} - bool AssemblyObject::isJointConnectingPartToGround(App::DocumentObject* joint, const char* propname) { if (!joint || !isJointTypeConnecting(joint)) { @@ -1213,641 +1926,6 @@ bool AssemblyObject::isPartConnected(App::DocumentObject* obj) return false; } -void AssemblyObject::jointParts(std::vector joints) -{ - for (auto* joint : joints) { - if (!joint) { - continue; - } - - std::vector> mbdJoints = makeMbdJoint(joint); - for (auto& mbdJoint : mbdJoints) { - mbdAssembly->addJoint(mbdJoint); - } - } -} - -void Assembly::AssemblyObject::create_mbdSimulationParameters(App::DocumentObject* sim) -{ - auto mbdSim = mbdAssembly->simulationParameters; - if (!sim) { - return; - } - auto valueOf = [](DocumentObject* docObj, const char* propName) { - auto* prop = dynamic_cast(docObj->getPropertyByName(propName)); - if (!prop) { - return 0.0; - } - return prop->getValue(); - }; - mbdSim->settstart(valueOf(sim, "aTimeStart")); - mbdSim->settend(valueOf(sim, "bTimeEnd")); - mbdSim->sethout(valueOf(sim, "cTimeStepOutput")); - mbdSim->sethmin(1.0e-9); - mbdSim->sethmax(1.0); - mbdSim->seterrorTol(valueOf(sim, "fGlobalErrorTolerance")); -} - -std::shared_ptr AssemblyObject::makeMbdJointOfType(App::DocumentObject* joint, JointType type) -{ - switch (type) { - case JointType::Fixed: - if (bundleFixed) { - return nullptr; - } - return CREATE::With(); - - case JointType::Revolute: - return CREATE::With(); - - case JointType::Cylindrical: - return CREATE::With(); - - case JointType::Slider: - return CREATE::With(); - - case JointType::Ball: - return CREATE::With(); - - case JointType::Distance: - return makeMbdJointDistance(joint); - - case JointType::Parallel: - return CREATE::With(); - - case JointType::Perpendicular: - return CREATE::With(); - - case JointType::Angle: { - double angle = fabs(Base::toRadians(getJointAngle(joint))); - if (fmod(angle, 2 * std::numbers::pi) < Precision::Confusion()) { - return CREATE::With(); - } - auto mbdJoint = CREATE::With(); - mbdJoint->theIzJz = angle; - return mbdJoint; - } - - case JointType::RackPinion: { - auto mbdJoint = CREATE::With(); - mbdJoint->pitchRadius = getJointDistance(joint); - return mbdJoint; - } - - case JointType::Screw: { - int slidingIndex = slidingPartIndex(joint); - if (slidingIndex == 0) { // invalid this joint needs a slider - return nullptr; - } - - if (slidingIndex != 1) { - swapJCS(joint); // make sure that sliding is first. - } - - auto mbdJoint = CREATE::With(); - mbdJoint->pitch = getJointDistance(joint); - return mbdJoint; - } - - case JointType::Gears: { - auto mbdJoint = CREATE::With(); - mbdJoint->radiusI = getJointDistance(joint); - mbdJoint->radiusJ = getJointDistance2(joint); - return mbdJoint; - } - - case JointType::Belt: { - auto mbdJoint = CREATE::With(); - mbdJoint->radiusI = getJointDistance(joint); - mbdJoint->radiusJ = -getJointDistance2(joint); - return mbdJoint; - } - - default: - return nullptr; - } -} - -std::shared_ptr AssemblyObject::makeMbdJointDistance(App::DocumentObject* joint) -{ - DistanceType type = getDistanceType(joint); - - std::string elt1 = getElementFromProp(joint, "Reference1"); - std::string elt2 = getElementFromProp(joint, "Reference2"); - auto* obj1 = getLinkedObjFromRef(joint, "Reference1"); - auto* obj2 = getLinkedObjFromRef(joint, "Reference2"); - - switch (type) { - case DistanceType::PointPoint: { - // Point to point distance, or ball joint if distance=0. - double distance = getJointDistance(joint); - if (distance < Precision::Confusion()) { - return CREATE::With(); - } - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = distance; - return mbdJoint; - } - - // Edge - edge cases - case DistanceType::LineLine: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::LineCircle: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getEdgeRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::CircleCircle: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getEdgeRadius(obj1, elt1) - + getEdgeRadius(obj2, elt2); - return mbdJoint; - } - - // Face - Face cases - case DistanceType::PlanePlane: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::PlaneCylinder: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint) + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::PlaneSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint) + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::PlaneTorus: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::CylinderCylinder: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::CylinderSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::CylinderTorus: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::TorusTorus: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::TorusSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::SphereSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - // Point - Face cases - case DistanceType::PointPlane: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::PointCylinder: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1); - return mbdJoint; - } - - case DistanceType::PointSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1); - return mbdJoint; - } - - // Edge - Face cases - case DistanceType::LinePlane: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - // Point - Edge cases - case DistanceType::PointLine: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::PointCurve: { - // For other curves we do a point in plane-of-the-curve. - // Maybe it would be best tangent / distance to the conic? - // For arcs and circles we could use ASMTRevSphJoint. But is it better than - // pointInPlane? - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - default: { - // by default we make a planar joint. - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - } -} - -std::vector> AssemblyObject::makeMbdJoint(App::DocumentObject* joint) -{ - if (!joint) { - return {}; - } - - JointType jointType = getJointType(joint); - - std::shared_ptr mbdJoint = makeMbdJointOfType(joint, jointType); - if (!mbdJoint || !isMbDJointValid(joint)) { - return {}; - } - - std::string fullMarkerNameI, fullMarkerNameJ; - if (jointType == JointType::RackPinion) { - getRackPinionMarkers(joint, fullMarkerNameI, fullMarkerNameJ); - } - else { - fullMarkerNameI = handleOneSideOfJoint(joint, "Reference1", "Placement1"); - fullMarkerNameJ = handleOneSideOfJoint(joint, "Reference2", "Placement2"); - } - if (fullMarkerNameI == "" || fullMarkerNameJ == "") { - return {}; - } - - mbdJoint->setName(joint->getFullName()); - mbdJoint->setMarkerI(fullMarkerNameI); - mbdJoint->setMarkerJ(fullMarkerNameJ); - - // Add limits if needed. We do not add if this is a simulation or their might clash. - if (motions.empty()) { - if (jointType == JointType::Slider || jointType == JointType::Cylindrical) { - auto* pLenMin = dynamic_cast(joint->getPropertyByName("LengthMin")); - auto* pLenMax = dynamic_cast(joint->getPropertyByName("LengthMax")); - auto* pMinEnabled = dynamic_cast( - joint->getPropertyByName("EnableLengthMin") - ); - auto* pMaxEnabled = dynamic_cast( - joint->getPropertyByName("EnableLengthMax") - ); - - if (pLenMin && pLenMax && pMinEnabled && pMaxEnabled) { // Make sure properties do exist - // Swap the values if necessary. - bool minEnabled = pMinEnabled->getValue(); - bool maxEnabled = pMaxEnabled->getValue(); - double minLength = pLenMin->getValue(); - double maxLength = pLenMax->getValue(); - - if ((minLength > maxLength) && minEnabled && maxEnabled) { - pLenMin->setValue(maxLength); - pLenMax->setValue(minLength); - minLength = maxLength; - maxLength = pLenMax->getValue(); - - pMinEnabled->setValue(maxEnabled); - pMaxEnabled->setValue(minEnabled); - minEnabled = maxEnabled; - maxEnabled = pMaxEnabled->getValue(); - } - - if (minEnabled) { - auto limit = ASMTTranslationLimit::With(); - limit->setName(joint->getFullName() + "-LimitLenMin"); - limit->setMarkerI(fullMarkerNameI); - limit->setMarkerJ(fullMarkerNameJ); - limit->settype("=>"); - limit->setlimit(std::to_string(minLength)); - limit->settol("1.0e-9"); - mbdAssembly->addLimit(limit); - } - - if (maxEnabled) { - auto limit2 = ASMTTranslationLimit::With(); - limit2->setName(joint->getFullName() + "-LimitLenMax"); - limit2->setMarkerI(fullMarkerNameI); - limit2->setMarkerJ(fullMarkerNameJ); - limit2->settype("=<"); - limit2->setlimit(std::to_string(maxLength)); - limit2->settol("1.0e-9"); - mbdAssembly->addLimit(limit2); - } - } - } - if (jointType == JointType::Revolute || jointType == JointType::Cylindrical) { - auto* pRotMin = dynamic_cast(joint->getPropertyByName("AngleMin")); - auto* pRotMax = dynamic_cast(joint->getPropertyByName("AngleMax")); - auto* pMinEnabled = dynamic_cast( - joint->getPropertyByName("EnableAngleMin") - ); - auto* pMaxEnabled = dynamic_cast( - joint->getPropertyByName("EnableAngleMax") - ); - - if (pRotMin && pRotMax && pMinEnabled && pMaxEnabled) { // Make sure properties do exist - // Swap the values if necessary. - bool minEnabled = pMinEnabled->getValue(); - bool maxEnabled = pMaxEnabled->getValue(); - double minAngle = pRotMin->getValue(); - double maxAngle = pRotMax->getValue(); - if ((minAngle > maxAngle) && minEnabled && maxEnabled) { - pRotMin->setValue(maxAngle); - pRotMax->setValue(minAngle); - minAngle = maxAngle; - maxAngle = pRotMax->getValue(); - - pMinEnabled->setValue(maxEnabled); - pMaxEnabled->setValue(minEnabled); - minEnabled = maxEnabled; - maxEnabled = pMaxEnabled->getValue(); - } - - if (minEnabled) { - auto limit = ASMTRotationLimit::With(); - limit->setName(joint->getFullName() + "-LimitRotMin"); - limit->setMarkerI(fullMarkerNameI); - limit->setMarkerJ(fullMarkerNameJ); - limit->settype("=>"); - limit->setlimit(std::to_string(minAngle) + "*pi/180.0"); - limit->settol("1.0e-9"); - mbdAssembly->addLimit(limit); - } - - if (maxEnabled) { - auto limit2 = ASMTRotationLimit::With(); - limit2->setName(joint->getFullName() + "-LimitRotMax"); - limit2->setMarkerI(fullMarkerNameI); - limit2->setMarkerJ(fullMarkerNameJ); - limit2->settype("=<"); - limit2->setlimit(std::to_string(maxAngle) + "*pi/180.0"); - limit2->settol("1.0e-9"); - mbdAssembly->addLimit(limit2); - } - } - } - } - std::vector done; - // Add motions if needed - for (auto* motion : motions) { - if (std::ranges::find(done, motion) != done.end()) { - continue; // don't process twice (can happen in case of cylindrical) - } - - auto* pJoint = dynamic_cast(motion->getPropertyByName("Joint")); - if (!pJoint) { - continue; - } - App::DocumentObject* motionJoint = pJoint->getValue(); - if (joint != motionJoint) { - continue; - } - - auto* pType = dynamic_cast(motion->getPropertyByName("MotionType")); - auto* pFormula = dynamic_cast(motion->getPropertyByName("Formula")); - if (!pType || !pFormula) { - continue; - } - std::string formula = pFormula->getValue(); - if (formula == "") { - continue; - } - std::string motionType = pType->getValueAsString(); - - // check if there is a second motion as cylindrical can have both, - // in which case the solver needs a general motion. - for (auto* motion2 : motions) { - pJoint = dynamic_cast(motion2->getPropertyByName("Joint")); - if (!pJoint) { - continue; - } - motionJoint = pJoint->getValue(); - if (joint != motionJoint || motion2 == motion) { - continue; - } - - auto* pType2 = dynamic_cast( - motion2->getPropertyByName("MotionType") - ); - auto* pFormula2 = dynamic_cast(motion2->getPropertyByName("Formula")); - if (!pType2 || !pFormula2) { - continue; - } - std::string formula2 = pFormula2->getValue(); - if (formula2 == "") { - continue; - } - std::string motionType2 = pType2->getValueAsString(); - if (motionType2 == motionType) { - continue; // only if both motions are different. ie one angular and one linear. - } - - auto ASMTmotion = CREATE::With(); - ASMTmotion->setName(joint->getFullName() + "-ScrewMotion"); - ASMTmotion->setMarkerI(fullMarkerNameI); - ASMTmotion->setMarkerJ(fullMarkerNameJ); - ASMTmotion->rIJI->atiput(2, motionType == "Angular" ? formula2 : formula); - ASMTmotion->angIJJ->atiput(2, motionType == "Angular" ? formula : formula2); - mbdAssembly->addMotion(ASMTmotion); - - done.push_back(motion2); - } - - if (motionType == "Angular") { - auto ASMTmotion = CREATE::With(); - ASMTmotion->setName(joint->getFullName() + "-AngularMotion"); - ASMTmotion->setMarkerI(fullMarkerNameI); - ASMTmotion->setMarkerJ(fullMarkerNameJ); - ASMTmotion->setRotationZ(formula); - mbdAssembly->addMotion(ASMTmotion); - } - else if (motionType == "Linear") { - auto ASMTmotion = CREATE::With(); - ASMTmotion->setName(joint->getFullName() + "-LinearMotion"); - ASMTmotion->setMarkerI(fullMarkerNameI); - ASMTmotion->setMarkerJ(fullMarkerNameJ); - ASMTmotion->setTranslationZ(formula); - mbdAssembly->addMotion(ASMTmotion); - } - } - - return {mbdJoint}; -} - -std::string AssemblyObject::handleOneSideOfJoint( - App::DocumentObject* joint, - const char* propRefName, - const char* propPlcName -) -{ - App::DocumentObject* part = getMovingPartFromRef(joint, propRefName); - App::DocumentObject* obj = getObjFromJointRef(joint, propRefName); - - if (!part || !obj) { - Base::Console() - .warning("The property %s of Joint %s is bad.\n", propRefName, joint->getFullName()); - return ""; - } - - MbDPartData data = getMbDData(part); - std::shared_ptr mbdPart = data.part; - Base::Placement plc = getPlacementFromProp(joint, propPlcName); - // Now we have plc which is the JCS placement, but its relative to the Object, not to the - // containing Part. - - if (obj->getNameInDocument() != part->getNameInDocument()) { - - auto* ref = dynamic_cast(joint->getPropertyByName(propRefName)); - if (!ref) { - return ""; - } - - Base::Placement obj_global_plc = getGlobalPlacement(obj, ref); - plc = obj_global_plc * plc; - - Base::Placement part_global_plc = getGlobalPlacement(part, ref); - plc = part_global_plc.inverse() * plc; - } - // check if we need to add an offset in case of bundled parts. - if (!data.offsetPlc.isIdentity()) { - plc = data.offsetPlc * plc; - } - - std::string markerName = joint->getFullName(); - auto mbdMarker = makeMbdMarker(markerName, plc); - mbdPart->addMarker(mbdMarker); - - return "/OndselAssembly/" + mbdPart->name + "/" + markerName; -} - -void AssemblyObject::getRackPinionMarkers( - App::DocumentObject* joint, - std::string& markerNameI, - std::string& markerNameJ -) -{ - // ASMT rack pinion joint must get the rack as I and pinion as J. - // - rack marker has to have Z axis parallel to pinion Z axis. - // - rack marker has to have X axis parallel to the sliding axis. - // The user will have selected the sliding marker so we need to transform it. - // And we need to detect which marker is the rack. - - int slidingIndex = slidingPartIndex(joint); - if (slidingIndex == 0) { - return; - } - - if (slidingIndex != 1) { - swapJCS(joint); // make sure that rack is first. - } - - App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); - App::DocumentObject* obj1 = getObjFromJointRef(joint, "Reference1"); - Base::Placement plc1 = getPlacementFromProp(joint, "Placement1"); - - App::DocumentObject* obj2 = getObjFromJointRef(joint, "Reference2"); - Base::Placement plc2 = getPlacementFromProp(joint, "Placement2"); - - if (!part1 || !obj1) { - Base::Console().warning("Reference1 of Joint %s is bad.\n", joint->getFullName()); - return; - } - - // For the pinion nothing special needed : - markerNameJ = handleOneSideOfJoint(joint, "Reference2", "Placement2"); - - // For the rack we need to change the placement : - // make the pinion plc relative to the rack placement. - auto* ref1 = dynamic_cast(joint->getPropertyByName("Reference1")); - auto* ref2 = dynamic_cast(joint->getPropertyByName("Reference2")); - if (!ref1 || !ref2) { - return; - } - Base::Placement pinion_global_plc = getGlobalPlacement(obj2, ref2); - plc2 = pinion_global_plc * plc2; - Base::Placement rack_global_plc = getGlobalPlacement(obj1, ref1); - plc2 = rack_global_plc.inverse() * plc2; - - // The rot of the rack placement should be the same as the pinion, but with X axis along the - // slider axis. - Base::Rotation rot = plc2.getRotation(); - // the yaw of rot has to be the same as plc1 - Base::Vector3d currentZAxis = rot.multVec(Base::Vector3d(0, 0, 1)); - Base::Vector3d currentXAxis = rot.multVec(Base::Vector3d(1, 0, 0)); - Base::Vector3d targetXAxis = plc1.getRotation().multVec(Base::Vector3d(0, 0, 1)); - - // Calculate the angle between the current X axis and the target X axis - double yawAdjustment = currentXAxis.GetAngle(targetXAxis); - - // Determine the direction of the yaw adjustment using cross product - Base::Vector3d crossProd = currentXAxis.Cross(targetXAxis); - if (currentZAxis * crossProd < 0) { // If cross product is in opposite direction to Z axis - yawAdjustment = -yawAdjustment; - } - - // Create a yaw rotation around the Z axis - Base::Rotation yawRotation(currentZAxis, yawAdjustment); - - // Combine the initial rotation with the yaw adjustment - Base::Rotation adjustedRotation = rot * yawRotation; - plc1.setRotation(adjustedRotation); - - // Then end of processing similar to handleOneSideOfJoint : - MbDPartData data1 = getMbDData(part1); - std::shared_ptr mbdPart = data1.part; - if (obj1->getNameInDocument() != part1->getNameInDocument()) { - plc1 = rack_global_plc * plc1; - - Base::Placement part_global_plc = getGlobalPlacement(part1, ref1); - plc1 = part_global_plc.inverse() * plc1; - } - // check if we need to add an offset in case of bundled parts. - if (!data1.offsetPlc.isIdentity()) { - plc1 = data1.offsetPlc * plc1; - } - - std::string markerName = joint->getFullName(); - auto mbdMarker = makeMbdMarker(markerName, plc1); - mbdPart->addMarker(mbdMarker); - - markerNameI = "/OndselAssembly/" + mbdPart->name + "/" + markerName; -} - int AssemblyObject::slidingPartIndex(App::DocumentObject* joint) { App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); @@ -1879,8 +1957,6 @@ int AssemblyObject::slidingPartIndex(App::DocumentObject* joint) } if (found != 0) { - // check the placements plcjt and (jcs1 or jcs2 depending on found value) Z axis are - // colinear ie if their pitch and roll are the same. double y1, p1, r1, y2, p2, r2; plcjt.getRotation().getYawPitchRoll(y1, p1, r1); plci.getRotation().getYawPitchRoll(y2, p2, r2); @@ -1893,130 +1969,6 @@ int AssemblyObject::slidingPartIndex(App::DocumentObject* joint) return slidingFound; } -bool AssemblyObject::isMbDJointValid(App::DocumentObject* joint) -{ - // When dragging a part, we are bundling fixed parts together. - // This may lead to a conflicting joint that is self referencing a MbD part. - // The solver crash when fed such a bad joint. So we make sure it does not happen. - App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); - App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); - if (!part1 || !part2) { - return false; - } - - // If this joint is self-referential it must be ignored. - if (getMbDPart(part1) == getMbDPart(part2)) { - Base::Console().warning( - "Assembly: Ignoring joint (%s) because its parts are connected by a fixed " - "joint bundle. This joint is a conflicting or redundant constraint.\n", - joint->getFullLabel() - ); - return false; - } - return true; -} - -AssemblyObject::MbDPartData AssemblyObject::getMbDData(App::DocumentObject* part) -{ - auto it = objectPartMap.find(part); - if (it != objectPartMap.end()) { - // part has been associated with an ASMTPart before - return it->second; - } - - // part has not been associated with an ASMTPart before - std::string str = part->getFullName(); - Base::Placement plc = getPlacementFromProp(part, "Placement"); - std::shared_ptr mbdPart = makeMbdPart(str, plc); - mbdAssembly->addPart(mbdPart); - MbDPartData data = {mbdPart, Base::Placement()}; - objectPartMap[part] = data; // Store the association - - // Associate other objects connected with fixed joints - if (bundleFixed) { - auto addConnectedFixedParts = [&](App::DocumentObject* currentPart, auto& self) -> void { - std::vector joints = getJointsOfPart(currentPart); - for (auto* joint : joints) { - JointType jointType = getJointType(joint); - if (jointType == JointType::Fixed) { - App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); - App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); - App::DocumentObject* partToAdd = currentPart == part1 ? part2 : part1; - - if (objectPartMap.find(partToAdd) != objectPartMap.end()) { - // already added - continue; - } - - Base::Placement plci = getPlacementFromProp(partToAdd, "Placement"); - MbDPartData partData = {mbdPart, plc.inverse() * plci}; - objectPartMap[partToAdd] = partData; // Store the association - - // Recursively call for partToAdd - self(partToAdd, self); - } - } - }; - - addConnectedFixedParts(part, addConnectedFixedParts); - } - return data; -} - -std::shared_ptr AssemblyObject::getMbDPart(App::DocumentObject* part) -{ - if (!part) { - return nullptr; - } - return getMbDData(part).part; -} - -std::shared_ptr AssemblyObject::makeMbdPart(std::string& name, Base::Placement plc, double mass) -{ - auto mbdPart = CREATE::With(); - mbdPart->setName(name); - - auto massMarker = CREATE::With(); - massMarker->setMass(mass); - massMarker->setDensity(1.0); - massMarker->setMomentOfInertias(1.0, 1.0, 1.0); - mbdPart->setPrincipalMassMarker(massMarker); - - Base::Vector3d pos = plc.getPosition(); - mbdPart->setPosition3D(pos.x, pos.y, pos.z); - - // TODO : replace with quaternion to simplify - Base::Rotation rot = plc.getRotation(); - Base::Matrix4D mat; - rot.getValue(mat); - Base::Vector3d r0 = mat.getRow(0); - Base::Vector3d r1 = mat.getRow(1); - Base::Vector3d r2 = mat.getRow(2); - mbdPart->setRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z); - - return mbdPart; -} - -std::shared_ptr AssemblyObject::makeMbdMarker(std::string& name, Base::Placement& plc) -{ - auto mbdMarker = CREATE::With(); - mbdMarker->setName(name); - - Base::Vector3d pos = plc.getPosition(); - mbdMarker->setPosition3D(pos.x, pos.y, pos.z); - - // TODO : replace with quaternion to simplify - Base::Rotation rot = plc.getRotation(); - Base::Matrix4D mat; - rot.getValue(mat); - Base::Vector3d r0 = mat.getRow(0); - Base::Vector3d r1 = mat.getRow(1); - Base::Vector3d r2 = mat.getRow(2); - mbdMarker->setRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z); - - return mbdMarker; -} - std::vector AssemblyObject::getDownstreamParts( App::DocumentObject* part, App::DocumentObject* joint diff --git a/src/Mod/Assembly/App/AssemblyObject.h b/src/Mod/Assembly/App/AssemblyObject.h index 74bc8e64d5..cffe0d2483 100644 --- a/src/Mod/Assembly/App/AssemblyObject.h +++ b/src/Mod/Assembly/App/AssemblyObject.h @@ -25,24 +25,21 @@ #ifndef ASSEMBLY_AssemblyObject_H #define ASSEMBLY_AssemblyObject_H +#include + #include #include +#include #include #include #include -#include - -namespace MbD +namespace KCSolve { -class ASMTPart; -class ASMTAssembly; -class ASMTJoint; -class ASMTMarker; -class ASMTPart; -} // namespace MbD +class IKCSolver; +} // namespace KCSolve namespace App { @@ -105,7 +102,6 @@ public: void exportAsASMT(std::string fileName); - Base::Placement getMbdPlacement(std::shared_ptr mbdPart); bool validateNewPlacements(); void setNewPlacements(); static void redrawJointPlacements(std::vector joints); @@ -114,42 +110,8 @@ public: // This makes sure that LinkGroups or sub-assemblies have identity placements. void ensureIdentityPlacements(); - // Ondsel Solver interface - std::shared_ptr makeMbdAssembly(); - void create_mbdSimulationParameters(App::DocumentObject* sim); - std::shared_ptr makeMbdPart( - std::string& name, - Base::Placement plc = Base::Placement(), - double mass = 1.0 - ); - std::shared_ptr getMbDPart(App::DocumentObject* obj); - // To help the solver, during dragging, we are bundling parts connected by a fixed joint. - // So several assembly components are bundled in a single ASMTPart. - // So we need to store the plc of each bundled object relative to the bundle origin (first obj - // of objectPartMap). - struct MbDPartData - { - std::shared_ptr part; - Base::Placement offsetPlc; // This is the offset within the bundled parts - }; - MbDPartData getMbDData(App::DocumentObject* part); - std::shared_ptr makeMbdMarker(std::string& name, Base::Placement& plc); - std::vector> makeMbdJoint(App::DocumentObject* joint); - std::shared_ptr makeMbdJointOfType(App::DocumentObject* joint, JointType jointType); - std::shared_ptr makeMbdJointDistance(App::DocumentObject* joint); - std::string handleOneSideOfJoint( - App::DocumentObject* joint, - const char* propRefName, - const char* propPlcName - ); - void getRackPinionMarkers( - App::DocumentObject* joint, - std::string& markerNameI, - std::string& markerNameJ - ); int slidingPartIndex(App::DocumentObject* joint); - void jointParts(std::vector joints); JointGroup* getJointGroup() const; ViewGroup* getExplodedViewGroup() const; template @@ -169,8 +131,6 @@ public: const std::vector& excludeJoints = {} ); std::unordered_set getGroundedParts(); - std::unordered_set fixGroundedParts(); - void fixGroundedPart(App::DocumentObject* obj, Base::Placement& plc, std::string& jointName); bool isJointConnectingPartToGround(App::DocumentObject* joint, const char* partPropName); bool isJointTypeConnecting(App::DocumentObject* joint); @@ -210,7 +170,7 @@ public: std::vector getMotionsFromSimulation(App::DocumentObject* sim); - bool isMbDJointValid(App::DocumentObject* joint); + bool isJointValid(App::DocumentObject* joint); bool isEmpty() const; int numberOfComponents() const; @@ -259,12 +219,56 @@ public: fastsignals::signal signalSolverUpdate; private: - std::shared_ptr mbdAssembly; + // ── Solver integration ───────────────────────────────────────── + + KCSolve::IKCSolver* getOrCreateSolver(); + + KCSolve::SolveContext buildSolveContext( + const std::vector& joints, + bool forSimulation = false, + App::DocumentObject* sim = nullptr + ); + + KCSolve::Transform computeMarkerTransform( + App::DocumentObject* joint, + const char* propRefName, + const char* propPlcName + ); + + struct RackPinionResult + { + std::string partIdI; + KCSolve::Transform markerI; + std::string partIdJ; + KCSolve::Transform markerJ; + }; + RackPinionResult computeRackPinionMarkers(App::DocumentObject* joint); + + // ── Part ↔ solver ID mapping ─────────────────────────────────── + + // Maps a solver part ID to the FreeCAD objects it represents. + // Multiple objects map to one ID when parts are bundled by Fixed joints. + struct PartMapping + { + App::DocumentObject* obj; + Base::Placement offset; // identity for primary, non-identity for bundled + }; + std::unordered_map> partIdToObjs_; + std::unordered_map objToPartId_; + + // Register a part (and recursively its fixed-joint bundle when bundleFixed is set). + // Returns the solver part ID. + std::string registerPart(App::DocumentObject* obj); + + // ── Solver state ─────────────────────────────────────────────── + + std::unique_ptr solver_; + KCSolve::SolveResult lastResult_; + + // ── Existing state (unchanged) ───────────────────────────────── - std::unordered_map objectPartMap; std::vector> objMasses; std::vector draggedParts; - std::vector motions; std::vector> previousPositions; diff --git a/src/Mod/Assembly/App/CMakeLists.txt b/src/Mod/Assembly/App/CMakeLists.txt index 87053222f0..0bff518aec 100644 --- a/src/Mod/Assembly/App/CMakeLists.txt +++ b/src/Mod/Assembly/App/CMakeLists.txt @@ -5,7 +5,7 @@ set(Assembly_LIBS PartDesign Spreadsheet FreeCADApp - OndselSolver + KCSolve ) generate_from_py(AssemblyObject) diff --git a/src/Mod/Assembly/AssemblyTests/TestKCSolvePy.py b/src/Mod/Assembly/AssemblyTests/TestKCSolvePy.py new file mode 100644 index 0000000000..3073197773 --- /dev/null +++ b/src/Mod/Assembly/AssemblyTests/TestKCSolvePy.py @@ -0,0 +1,520 @@ +# SPDX-License-Identifier: LGPL-2.1-or-later +# *************************************************************************** +# * * +# * Copyright (c) 2025 Kindred Systems * +# * * +# * This file is part of FreeCAD. * +# * * +# * FreeCAD is free software: you can redistribute it and/or modify it * +# * under the terms of the GNU Lesser General Public License as * +# * published by the Free Software Foundation, either version 2.1 of the * +# * License, or (at your option) any later version. * +# * * +# * FreeCAD is distributed in the hope that it will be useful, but * +# * WITHOUT ANY WARRANTY; without even the implied warranty of * +# * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * +# * Lesser General Public License for more details. * +# * * +# * You should have received a copy of the GNU Lesser General Public * +# * License along with FreeCAD. If not, see * +# * . * +# * * +# *************************************************************************** + +"""Unit tests for the kcsolve pybind11 module.""" + +import unittest + + +class TestKCSolveImport(unittest.TestCase): + """Verify that the kcsolve module loads and exposes expected symbols.""" + + def test_import(self): + import kcsolve + + for sym in ( + "IKCSolver", + "OndselAdapter", + "Transform", + "Part", + "Constraint", + "SolveContext", + "SolveResult", + "BaseJointKind", + "SolveStatus", + "available", + "load", + "register_solver", + ): + self.assertTrue(hasattr(kcsolve, sym), f"missing symbol: {sym}") + + def test_api_version(self): + import kcsolve + + self.assertEqual(kcsolve.API_VERSION_MAJOR, 1) + + +class TestKCSolveTypes(unittest.TestCase): + """Verify struct/enum bindings behave correctly.""" + + def test_transform_identity(self): + import kcsolve + + t = kcsolve.Transform.identity() + self.assertEqual(list(t.position), [0.0, 0.0, 0.0]) + self.assertEqual(list(t.quaternion), [1.0, 0.0, 0.0, 0.0]) # w,x,y,z + + def test_part_defaults(self): + import kcsolve + + p = kcsolve.Part() + self.assertEqual(p.id, "") + self.assertAlmostEqual(p.mass, 1.0) + self.assertFalse(p.grounded) + + def test_solve_context_construction(self): + import kcsolve + + ctx = kcsolve.SolveContext() + self.assertEqual(len(ctx.parts), 0) + self.assertEqual(len(ctx.constraints), 0) + + p = kcsolve.Part() + p.id = "part1" + # pybind11 def_readwrite on std::vector returns a copy, + # so we must assign the whole list back. + ctx.parts = [p] + self.assertEqual(len(ctx.parts), 1) + self.assertEqual(ctx.parts[0].id, "part1") + + def test_enum_values(self): + import kcsolve + + self.assertEqual(int(kcsolve.SolveStatus.Success), 0) + # BaseJointKind.Fixed should exist + self.assertIsNotNone(kcsolve.BaseJointKind.Fixed) + # DiagnosticKind should exist + self.assertIsNotNone(kcsolve.DiagnosticKind.Redundant) + + def test_constraint_fields(self): + import kcsolve + + c = kcsolve.Constraint() + c.id = "Joint001" + c.part_i = "part1" + c.part_j = "part2" + c.type = kcsolve.BaseJointKind.Fixed + self.assertEqual(c.id, "Joint001") + self.assertEqual(c.type, kcsolve.BaseJointKind.Fixed) + + def test_solve_result_fields(self): + import kcsolve + + r = kcsolve.SolveResult() + self.assertEqual(r.status, kcsolve.SolveStatus.Success) + self.assertEqual(r.dof, -1) + self.assertEqual(len(r.placements), 0) + + +class TestKCSolveRegistry(unittest.TestCase): + """Verify SolverRegistry wrapper functions.""" + + def test_available_returns_list(self): + import kcsolve + + result = kcsolve.available() + self.assertIsInstance(result, list) + + def test_load_ondsel(self): + import kcsolve + + solver = kcsolve.load("ondsel") + # Ondsel should be registered by FreeCAD init + if solver is not None: + self.assertIn("Ondsel", solver.name()) + + def test_load_unknown_returns_none(self): + import kcsolve + + solver = kcsolve.load("nonexistent_solver_xyz") + self.assertIsNone(solver) + + def test_get_set_default(self): + import kcsolve + + original = kcsolve.get_default() + # Setting unknown solver should return False + self.assertFalse(kcsolve.set_default("nonexistent_solver_xyz")) + # Default should be unchanged + self.assertEqual(kcsolve.get_default(), original) + + +class TestKCSolveSerialization(unittest.TestCase): + """Verify to_dict() / from_dict() round-trip on all KCSolve types.""" + + def test_transform_round_trip(self): + import kcsolve + + t = kcsolve.Transform() + t.position = [1.0, 2.0, 3.0] + t.quaternion = [0.5, 0.5, 0.5, 0.5] + d = t.to_dict() + self.assertEqual(list(d["position"]), [1.0, 2.0, 3.0]) + self.assertEqual(list(d["quaternion"]), [0.5, 0.5, 0.5, 0.5]) + t2 = kcsolve.Transform.from_dict(d) + self.assertEqual(list(t2.position), [1.0, 2.0, 3.0]) + self.assertEqual(list(t2.quaternion), [0.5, 0.5, 0.5, 0.5]) + + def test_transform_identity_round_trip(self): + import kcsolve + + t = kcsolve.Transform.identity() + t2 = kcsolve.Transform.from_dict(t.to_dict()) + self.assertEqual(list(t2.position), [0.0, 0.0, 0.0]) + self.assertEqual(list(t2.quaternion), [1.0, 0.0, 0.0, 0.0]) + + def test_part_round_trip(self): + import kcsolve + + p = kcsolve.Part() + p.id = "box" + p.mass = 2.5 + p.grounded = True + p.placement = kcsolve.Transform.identity() + d = p.to_dict() + self.assertEqual(d["id"], "box") + self.assertAlmostEqual(d["mass"], 2.5) + self.assertTrue(d["grounded"]) + p2 = kcsolve.Part.from_dict(d) + self.assertEqual(p2.id, "box") + self.assertAlmostEqual(p2.mass, 2.5) + self.assertTrue(p2.grounded) + + def test_constraint_with_limits_round_trip(self): + import kcsolve + + c = kcsolve.Constraint() + c.id = "Joint001" + c.part_i = "part1" + c.part_j = "part2" + c.type = kcsolve.BaseJointKind.Revolute + c.params = [1.5, 2.5] + lim = kcsolve.Constraint.Limit() + lim.kind = kcsolve.LimitKind.RotationMin + lim.value = -3.14 + lim.tolerance = 0.01 + c.limits = [lim] + d = c.to_dict() + self.assertEqual(d["type"], "Revolute") + self.assertEqual(len(d["limits"]), 1) + self.assertEqual(d["limits"][0]["kind"], "RotationMin") + c2 = kcsolve.Constraint.from_dict(d) + self.assertEqual(c2.type, kcsolve.BaseJointKind.Revolute) + self.assertEqual(len(c2.limits), 1) + self.assertEqual(c2.limits[0].kind, kcsolve.LimitKind.RotationMin) + self.assertAlmostEqual(c2.limits[0].value, -3.14) + + def test_solve_context_full_round_trip(self): + import kcsolve + + ctx = kcsolve.SolveContext() + p = kcsolve.Part() + p.id = "box" + p.grounded = True + ctx.parts = [p] + + c = kcsolve.Constraint() + c.id = "J1" + c.part_i = "box" + c.part_j = "cyl" + c.type = kcsolve.BaseJointKind.Fixed + ctx.constraints = [c] + ctx.bundle_fixed = True + + d = ctx.to_dict() + self.assertEqual(d["api_version"], kcsolve.API_VERSION_MAJOR) + self.assertEqual(len(d["parts"]), 1) + self.assertEqual(len(d["constraints"]), 1) + self.assertTrue(d["bundle_fixed"]) + + ctx2 = kcsolve.SolveContext.from_dict(d) + self.assertEqual(ctx2.parts[0].id, "box") + self.assertTrue(ctx2.parts[0].grounded) + self.assertEqual(ctx2.constraints[0].type, kcsolve.BaseJointKind.Fixed) + self.assertTrue(ctx2.bundle_fixed) + + def test_solve_context_with_simulation(self): + import kcsolve + + ctx = kcsolve.SolveContext() + ctx.parts = [] + ctx.constraints = [] + sim = kcsolve.SimulationParams() + sim.t_start = 0.0 + sim.t_end = 10.0 + sim.h_out = 0.01 + ctx.simulation = sim + d = ctx.to_dict() + self.assertIsNotNone(d["simulation"]) + self.assertAlmostEqual(d["simulation"]["t_end"], 10.0) + ctx2 = kcsolve.SolveContext.from_dict(d) + self.assertIsNotNone(ctx2.simulation) + self.assertAlmostEqual(ctx2.simulation.t_end, 10.0) + + def test_solve_context_simulation_null(self): + import kcsolve + + ctx = kcsolve.SolveContext() + ctx.parts = [] + ctx.constraints = [] + ctx.simulation = None + d = ctx.to_dict() + self.assertIsNone(d["simulation"]) + ctx2 = kcsolve.SolveContext.from_dict(d) + self.assertIsNone(ctx2.simulation) + + def test_solve_result_round_trip(self): + import kcsolve + + r = kcsolve.SolveResult() + r.status = kcsolve.SolveStatus.Success + r.dof = 6 + pr = kcsolve.SolveResult.PartResult() + pr.id = "box" + pr.placement = kcsolve.Transform.identity() + r.placements = [pr] + diag = kcsolve.ConstraintDiagnostic() + diag.constraint_id = "J1" + diag.kind = kcsolve.DiagnosticKind.Redundant + diag.detail = "over-constrained" + r.diagnostics = [diag] + r.num_frames = 100 + + d = r.to_dict() + self.assertEqual(d["status"], "Success") + self.assertEqual(d["dof"], 6) + self.assertEqual(d["num_frames"], 100) + self.assertEqual(len(d["placements"]), 1) + self.assertEqual(len(d["diagnostics"]), 1) + + r2 = kcsolve.SolveResult.from_dict(d) + self.assertEqual(r2.status, kcsolve.SolveStatus.Success) + self.assertEqual(r2.dof, 6) + self.assertEqual(r2.num_frames, 100) + self.assertEqual(r2.placements[0].id, "box") + self.assertEqual(r2.diagnostics[0].kind, kcsolve.DiagnosticKind.Redundant) + + def test_motion_def_round_trip(self): + import kcsolve + + m = kcsolve.MotionDef() + m.kind = kcsolve.MotionKind.Rotational + m.joint_id = "J1" + m.marker_i = "part1" + m.marker_j = "part2" + m.rotation_expr = "2*pi*time" + m.translation_expr = "" + d = m.to_dict() + self.assertEqual(d["kind"], "Rotational") + self.assertEqual(d["joint_id"], "J1") + m2 = kcsolve.MotionDef.from_dict(d) + self.assertEqual(m2.kind, kcsolve.MotionKind.Rotational) + self.assertEqual(m2.rotation_expr, "2*pi*time") + + def test_all_base_joint_kinds_round_trip(self): + import kcsolve + + all_kinds = [ + "Coincident", + "PointOnLine", + "PointInPlane", + "Concentric", + "Tangent", + "Planar", + "LineInPlane", + "Parallel", + "Perpendicular", + "Angle", + "Fixed", + "Revolute", + "Cylindrical", + "Slider", + "Ball", + "Screw", + "Universal", + "Gear", + "RackPinion", + "Cam", + "Slot", + "DistancePointPoint", + "DistanceCylSph", + "Custom", + ] + for name in all_kinds: + c = kcsolve.Constraint() + c.id = "test" + c.part_i = "a" + c.part_j = "b" + c.type = getattr(kcsolve.BaseJointKind, name) + d = c.to_dict() + self.assertEqual(d["type"], name) + c2 = kcsolve.Constraint.from_dict(d) + self.assertEqual(c2.type, getattr(kcsolve.BaseJointKind, name)) + + def test_all_solve_statuses_round_trip(self): + import kcsolve + + for name in ("Success", "Failed", "InvalidFlip", "NoGroundedParts"): + r = kcsolve.SolveResult() + r.status = getattr(kcsolve.SolveStatus, name) + d = r.to_dict() + self.assertEqual(d["status"], name) + r2 = kcsolve.SolveResult.from_dict(d) + self.assertEqual(r2.status, getattr(kcsolve.SolveStatus, name)) + + def test_json_stdlib_round_trip(self): + import json + + import kcsolve + + ctx = kcsolve.SolveContext() + p = kcsolve.Part() + p.id = "box" + p.grounded = True + ctx.parts = [p] + ctx.constraints = [] + d = ctx.to_dict() + json_str = json.dumps(d) + d2 = json.loads(json_str) + ctx2 = kcsolve.SolveContext.from_dict(d2) + self.assertEqual(ctx2.parts[0].id, "box") + + def test_from_dict_missing_required_key(self): + import kcsolve + + with self.assertRaises(KeyError): + kcsolve.Part.from_dict({"mass": 1.0, "grounded": False}) + + def test_from_dict_invalid_enum_string(self): + import kcsolve + + d = { + "id": "J1", + "part_i": "a", + "part_j": "b", + "type": "Bogus", + "marker_i": {"position": [0, 0, 0], "quaternion": [1, 0, 0, 0]}, + "marker_j": {"position": [0, 0, 0], "quaternion": [1, 0, 0, 0]}, + } + with self.assertRaises(ValueError): + kcsolve.Constraint.from_dict(d) + + def test_from_dict_bad_position_length(self): + import kcsolve + + with self.assertRaises(ValueError): + kcsolve.Transform.from_dict( + { + "position": [1.0, 2.0], + "quaternion": [1, 0, 0, 0], + } + ) + + def test_from_dict_bad_api_version(self): + import kcsolve + + d = { + "api_version": 99, + "parts": [], + "constraints": [], + } + with self.assertRaises(ValueError): + kcsolve.SolveContext.from_dict(d) + + +class TestPySolver(unittest.TestCase): + """Verify Python IKCSolver subclassing and registration.""" + + def _make_solver_class(self): + import kcsolve + + class _DummySolver(kcsolve.IKCSolver): + def name(self): + return "DummyPySolver" + + def supported_joints(self): + return [ + kcsolve.BaseJointKind.Fixed, + kcsolve.BaseJointKind.Revolute, + ] + + def solve(self, ctx): + r = kcsolve.SolveResult() + r.status = kcsolve.SolveStatus.Success + parts = ctx.parts # copy from C++ vector + r.dof = len(parts) * 6 + placements = [] + for p in parts: + pr = kcsolve.SolveResult.PartResult() + pr.id = p.id + pr.placement = p.placement + placements.append(pr) + r.placements = placements + return r + + return _DummySolver + + def test_instantiate_python_solver(self): + cls = self._make_solver_class() + solver = cls() + self.assertEqual(solver.name(), "DummyPySolver") + self.assertEqual(len(solver.supported_joints()), 2) + + def test_python_solver_solve(self): + import kcsolve + + cls = self._make_solver_class() + solver = cls() + + ctx = kcsolve.SolveContext() + p = kcsolve.Part() + p.id = "box1" + p.grounded = True + ctx.parts = [p] + + result = solver.solve(ctx) + self.assertEqual(result.status, kcsolve.SolveStatus.Success) + self.assertEqual(result.dof, 6) + self.assertEqual(len(result.placements), 1) + self.assertEqual(result.placements[0].id, "box1") + + def test_register_and_roundtrip(self): + import kcsolve + + cls = self._make_solver_class() + # Use a unique name to avoid collision across test runs + name = "test_dummy_roundtrip" + kcsolve.register_solver(name, cls) + + self.assertIn(name, kcsolve.available()) + + loaded = kcsolve.load(name) + self.assertIsNotNone(loaded) + self.assertEqual(loaded.name(), "DummyPySolver") + + ctx = kcsolve.SolveContext() + result = loaded.solve(ctx) + self.assertEqual(result.status, kcsolve.SolveStatus.Success) + + def test_default_virtuals(self): + """Default implementations of optional virtuals should not crash.""" + import kcsolve + + cls = self._make_solver_class() + solver = cls() + self.assertTrue(solver.is_deterministic()) + self.assertFalse(solver.supports_bundle_fixed()) + + ctx = kcsolve.SolveContext() + diags = solver.diagnose(ctx) + self.assertEqual(len(diags), 0) diff --git a/src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py b/src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py new file mode 100644 index 0000000000..7062876e74 --- /dev/null +++ b/src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py @@ -0,0 +1,216 @@ +# SPDX-License-Identifier: LGPL-2.1-or-later +# /**************************************************************************** +# * +# Copyright (c) 2025 Kindred Systems * +# * +# This file is part of FreeCAD. * +# * +# FreeCAD is free software: you can redistribute it and/or modify it * +# under the terms of the GNU Lesser General Public License as * +# published by the Free Software Foundation, either version 2.1 of the * +# License, or (at your option) any later version. * +# * +# FreeCAD is distributed in the hope that it will be useful, but * +# WITHOUT ANY WARRANTY; without even the implied warranty of * +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * +# Lesser General Public License for more details. * +# * +# You should have received a copy of the GNU Lesser General Public * +# License along with FreeCAD. If not, see * +# . * +# * +# ***************************************************************************/ + +""" +Solver integration tests for Phase 1e (KCSolve refactor). + +These tests verify that the AssemblyObject → IKCSolver → OndselAdapter pipeline +produces correct results via the full FreeCAD stack. They complement the C++ +unit tests in tests/src/Mod/Assembly/Solver/. +""" + +import os +import tempfile +import unittest + +import FreeCAD as App +import JointObject + + +class TestSolverIntegration(unittest.TestCase): + """Full-stack solver regression tests exercising AssemblyObject.solve().""" + + def setUp(self): + doc_name = self.__class__.__name__ + if App.ActiveDocument: + if App.ActiveDocument.Name != doc_name: + App.newDocument(doc_name) + else: + App.newDocument(doc_name) + App.setActiveDocument(doc_name) + self.doc = App.ActiveDocument + + self.assembly = self.doc.addObject("Assembly::AssemblyObject", "Assembly") + self.jointgroup = self.assembly.newObject("Assembly::JointGroup", "Joints") + + def tearDown(self): + App.closeDocument(self.doc.Name) + + # ── Helpers ───────────────────────────────────────────────────── + + def _make_box(self, x=0, y=0, z=0, size=10): + """Create a Part::Box inside the assembly with a given offset.""" + box = self.assembly.newObject("Part::Box", "Box") + box.Length = size + box.Width = size + box.Height = size + box.Placement = App.Placement(App.Vector(x, y, z), App.Rotation()) + return box + + def _ground(self, obj): + """Create a grounded joint for the given object.""" + gnd = self.jointgroup.newObject("App::FeaturePython", "GroundedJoint") + JointObject.GroundedJoint(gnd, obj) + return gnd + + def _make_joint(self, joint_type, ref1, ref2): + """Create a joint of the given type connecting two (obj, subelements) pairs. + + joint_type: integer JointType enum value (0=Fixed, 1=Revolute, etc.) + ref1, ref2: tuples of (obj, [sub_element, sub_element]) + """ + joint = self.jointgroup.newObject("App::FeaturePython", "Joint") + JointObject.Joint(joint, joint_type) + + refs = [ + [ref1[0], ref1[1]], + [ref2[0], ref2[1]], + ] + joint.Proxy.setJointConnectors(joint, refs) + return joint + + # ── Tests ─────────────────────────────────────────────────────── + + def test_solve_fixed_joint(self): + """Two boxes + grounded + fixed joint → placements match.""" + box1 = self._make_box(10, 20, 30) + box2 = self._make_box(40, 50, 60) + self._ground(box2) + + # Fixed joint (type 0) connecting Face6+Vertex7 on each box. + self._make_joint( + 0, + [box2, ["Face6", "Vertex7"]], + [box1, ["Face6", "Vertex7"]], + ) + + # After setJointConnectors, solve() was already called internally. + # Verify that box1 moved to match box2. + self.assertTrue( + box1.Placement.isSame(box2.Placement, 1e-6), + "Fixed joint: box1 should match box2 placement", + ) + + def test_solve_revolute_joint(self): + """Two boxes + grounded + revolute joint → solve succeeds (return 0).""" + box1 = self._make_box(0, 0, 0) + box2 = self._make_box(100, 0, 0) + self._ground(box1) + + # Revolute joint (type 1) connecting Face6+Vertex7. + self._make_joint( + 1, + [box1, ["Face6", "Vertex7"]], + [box2, ["Face6", "Vertex7"]], + ) + + result = self.assembly.solve() + self.assertEqual(result, 0, "Revolute joint solve should succeed") + + def test_solve_returns_code_for_no_ground(self): + """Assembly with no grounded parts → solve returns -6.""" + box1 = self._make_box(0, 0, 0) + box2 = self._make_box(50, 0, 0) + + # Fixed joint but no ground. + joint = self.jointgroup.newObject("App::FeaturePython", "Joint") + JointObject.Joint(joint, 0) + refs = [ + [box1, ["Face6", "Vertex7"]], + [box2, ["Face6", "Vertex7"]], + ] + joint.Proxy.setJointConnectors(joint, refs) + + result = self.assembly.solve() + self.assertEqual(result, -6, "No grounded parts should return -6") + + def test_solve_returns_redundancy(self): + """Over-constrained assembly → solve returns -2 (redundant).""" + box1 = self._make_box(0, 0, 0) + box2 = self._make_box(50, 0, 0) + self._ground(box1) + + # Two fixed joints between the same faces → redundant. + self._make_joint( + 0, + [box1, ["Face6", "Vertex7"]], + [box2, ["Face6", "Vertex7"]], + ) + self._make_joint( + 0, + [box1, ["Face5", "Vertex5"]], + [box2, ["Face5", "Vertex5"]], + ) + + result = self.assembly.solve() + self.assertEqual(result, -2, "Redundant constraints should return -2") + + def test_export_asmt(self): + """exportAsASMT() produces a non-empty file.""" + box1 = self._make_box(0, 0, 0) + box2 = self._make_box(50, 0, 0) + self._ground(box1) + + self._make_joint( + 0, + [box1, ["Face6", "Vertex7"]], + [box2, ["Face6", "Vertex7"]], + ) + + self.assembly.solve() + + with tempfile.NamedTemporaryFile(suffix=".asmt", delete=False) as f: + path = f.name + + try: + self.assembly.exportAsASMT(path) + self.assertTrue(os.path.exists(path), "ASMT file should exist") + self.assertGreater( + os.path.getsize(path), 0, "ASMT file should be non-empty" + ) + finally: + if os.path.exists(path): + os.unlink(path) + + def test_solve_multiple_times_stable(self): + """Solving the same assembly twice produces identical placements.""" + box1 = self._make_box(10, 20, 30) + box2 = self._make_box(40, 50, 60) + self._ground(box2) + + self._make_joint( + 0, + [box2, ["Face6", "Vertex7"]], + [box1, ["Face6", "Vertex7"]], + ) + + self.assembly.solve() + plc_first = App.Placement(box1.Placement) + + self.assembly.solve() + plc_second = box1.Placement + + self.assertTrue( + plc_first.isSame(plc_second, 1e-6), + "Deterministic solver should produce identical results", + ) diff --git a/src/Mod/Assembly/CMakeLists.txt b/src/Mod/Assembly/CMakeLists.txt index f79b42cb35..b8ceb8a5f5 100644 --- a/src/Mod/Assembly/CMakeLists.txt +++ b/src/Mod/Assembly/CMakeLists.txt @@ -11,6 +11,7 @@ else () endif () endif () +add_subdirectory(Solver) add_subdirectory(App) if(BUILD_GUI) @@ -56,6 +57,8 @@ SET(AssemblyTests_SRCS AssemblyTests/__init__.py AssemblyTests/TestCore.py AssemblyTests/TestCommandInsertLink.py + AssemblyTests/TestSolverIntegration.py + AssemblyTests/TestKCSolvePy.py AssemblyTests/mocks/__init__.py AssemblyTests/mocks/MockGui.py ) diff --git a/src/Mod/Assembly/Solver/CMakeLists.txt b/src/Mod/Assembly/Solver/CMakeLists.txt new file mode 100644 index 0000000000..2228f79c23 --- /dev/null +++ b/src/Mod/Assembly/Solver/CMakeLists.txt @@ -0,0 +1,46 @@ +# SPDX-License-Identifier: LGPL-2.1-or-later + +set(KCSolve_SRCS + KCSolveGlobal.h + Types.h + IKCSolver.h + SolverRegistry.h + SolverRegistry.cpp + OndselAdapter.h + OndselAdapter.cpp +) + +add_library(KCSolve SHARED ${KCSolve_SRCS}) + +target_include_directories(KCSolve + PUBLIC + ${CMAKE_SOURCE_DIR}/src + ${CMAKE_BINARY_DIR}/src +) + +target_compile_definitions(KCSolve + PRIVATE + CMAKE_INSTALL_PREFIX="${CMAKE_INSTALL_PREFIX}" +) + +target_link_libraries(KCSolve + PRIVATE + FreeCADBase + OndselSolver +) + +# Platform-specific dynamic loading library +if(NOT WIN32) + target_link_libraries(KCSolve PRIVATE ${CMAKE_DL_LIBS}) +endif() + +if(FREECAD_WARN_ERROR) + target_compile_warn_error(KCSolve) +endif() + +SET_BIN_DIR(KCSolve KCSolve /Mod/Assembly) +INSTALL(TARGETS KCSolve DESTINATION ${CMAKE_INSTALL_LIBDIR}) + +if(FREECAD_USE_PYBIND11) + add_subdirectory(bindings) +endif() diff --git a/src/Mod/Assembly/Solver/IKCSolver.h b/src/Mod/Assembly/Solver/IKCSolver.h new file mode 100644 index 0000000000..4a404d134b --- /dev/null +++ b/src/Mod/Assembly/Solver/IKCSolver.h @@ -0,0 +1,189 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#ifndef KCSOLVE_IKCSOLVER_H +#define KCSOLVE_IKCSOLVER_H + +#include +#include +#include + +#include "Types.h" + +namespace KCSolve +{ + +/// Abstract interface for a pluggable assembly constraint solver. +/// +/// Solver backends implement this interface. The Assembly module calls +/// through it via the SolverRegistry. A minimal solver only needs to +/// implement solve(), name(), and supported_joints() — all other methods +/// have default implementations that either delegate to solve() or +/// return sensible defaults. +/// +/// Method mapping to current AssemblyObject operations: +/// +/// solve() <-> AssemblyObject::solve() +/// pre_drag() <-> AssemblyObject::preDrag() +/// drag_step() <-> AssemblyObject::doDragStep() +/// post_drag() <-> AssemblyObject::postDrag() +/// run_kinematic() <-> AssemblyObject::generateSimulation() +/// num_frames() <-> AssemblyObject::numberOfFrames() +/// update_for_frame() <-> AssemblyObject::updateForFrame() +/// diagnose() <-> AssemblyObject::updateSolveStatus() + +class IKCSolver +{ +public: + virtual ~IKCSolver() = default; + + /// Human-readable solver name (e.g. "OndselSolver (Lagrangian)"). + virtual std::string name() const = 0; + + /// Return the set of BaseJointKind values this solver supports. + /// The registry uses this for capability-based solver selection. + virtual std::vector supported_joints() const = 0; + + // ── Static solve ─────────────────────────────────────────────── + + /// Solve the assembly for static equilibrium. + /// @param ctx Complete description of parts, constraints, and options. + /// @return Result with updated placements and diagnostics. + virtual SolveResult solve(const SolveContext& ctx) = 0; + + /// Incrementally update an already-solved assembly after parameter + /// changes (e.g. joint angle/distance changed during joint creation). + /// Default: delegates to solve(). + virtual SolveResult update(const SolveContext& ctx) + { + return solve(ctx); + } + + // ── Interactive drag ─────────────────────────────────────────── + // + // Three-phase protocol for interactive part dragging: + // 1. pre_drag() — solve initial state, prepare for dragging + // 2. drag_step() — called on each mouse move with updated positions + // 3. post_drag() — finalize and release internal solver state + // + // Solvers can maintain internal state across the drag session for + // better interactive performance. This addresses a known weakness + // in the current direct-OndselSolver integration. + + /// Prepare for an interactive drag session. + /// @param ctx Assembly state before dragging begins. + /// @param drag_parts IDs of parts being dragged. + /// @return Initial solve result. + virtual SolveResult pre_drag(const SolveContext& ctx, + const std::vector& /*drag_parts*/) + { + return solve(ctx); + } + + /// Perform one incremental drag step. + /// @param drag_placements Current placements of the dragged parts + /// (part ID + new transform). + /// @return Updated placements for all affected parts. + virtual SolveResult drag_step( + const std::vector& /*drag_placements*/) + { + return SolveResult {SolveStatus::Success, {}, -1, {}, 0}; + } + + /// End an interactive drag session and finalize state. + virtual void post_drag() + { + } + + // ── Kinematic simulation ─────────────────────────────────────── + + /// Run a kinematic simulation over the time range in ctx.simulation. + /// After this call, num_frames() returns the frame count and + /// update_for_frame(i) retrieves individual frame placements. + /// Default: delegates to solve() (ignoring simulation params). + virtual SolveResult run_kinematic(const SolveContext& /*ctx*/) + { + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + + /// Number of simulation frames available after run_kinematic(). + virtual std::size_t num_frames() const + { + return 0; + } + + /// Retrieve part placements for simulation frame at index. + /// @pre index < num_frames() + virtual SolveResult update_for_frame(std::size_t /*index*/) + { + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + + // ── Diagnostics ──────────────────────────────────────────────── + + /// Analyze the assembly for redundant, conflicting, or malformed + /// constraints. May require a prior solve() call for some solvers. + virtual std::vector diagnose(const SolveContext& /*ctx*/) + { + return {}; + } + + // ── Capability queries ───────────────────────────────────────── + + /// Whether this solver produces deterministic results given + /// identical input. + virtual bool is_deterministic() const + { + return true; + } + + /// Export solver-native debug/diagnostic file (e.g. ASMT for OndselSolver). + /// Default: no-op. Requires a prior solve() or run_kinematic() call. + virtual void export_native(const std::string& /*path*/) + { + } + + /// Whether this solver handles fixed-joint part bundling internally. + /// When false, the caller bundles parts connected by Fixed joints + /// before building the SolveContext. When true, the solver receives + /// unbundled parts and optimizes internally. + virtual bool supports_bundle_fixed() const + { + return false; + } + + // Public default constructor for pybind11 trampoline support. + // The class remains abstract (3 pure virtuals prevent direct instantiation). + IKCSolver() = default; + +private: + // Non-copyable, non-movable (polymorphic base class) + IKCSolver(const IKCSolver&) = delete; + IKCSolver& operator=(const IKCSolver&) = delete; + IKCSolver(IKCSolver&&) = delete; + IKCSolver& operator=(IKCSolver&&) = delete; +}; + +} // namespace KCSolve + +#endif // KCSOLVE_IKCSOLVER_H diff --git a/src/Mod/Assembly/Solver/KCSolveGlobal.h b/src/Mod/Assembly/Solver/KCSolveGlobal.h new file mode 100644 index 0000000000..4985ef6934 --- /dev/null +++ b/src/Mod/Assembly/Solver/KCSolveGlobal.h @@ -0,0 +1,37 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#include + +#ifndef KCSOLVE_GLOBAL_H +#define KCSOLVE_GLOBAL_H + +#ifndef KCSolveExport +# ifdef KCSolve_EXPORTS +# define KCSolveExport FREECAD_DECL_EXPORT +# else +# define KCSolveExport FREECAD_DECL_IMPORT +# endif +#endif + +#endif // KCSOLVE_GLOBAL_H diff --git a/src/Mod/Assembly/Solver/OndselAdapter.cpp b/src/Mod/Assembly/Solver/OndselAdapter.cpp new file mode 100644 index 0000000000..a2c58db8f0 --- /dev/null +++ b/src/Mod/Assembly/Solver/OndselAdapter.cpp @@ -0,0 +1,796 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#include "OndselAdapter.h" +#include "SolverRegistry.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// For System::jointsMotionsDo and diagnostic iteration +#include +#include +#include + +using namespace MbD; + +namespace KCSolve +{ + +// ── Static registration ──────────────────────────────────────────── + +void OndselAdapter::register_solver() +{ + SolverRegistry::instance().register_solver( + "ondsel", + []() { return std::make_unique(); }); +} + + +// ── IKCSolver identity ───────────────────────────────────────────── + +std::string OndselAdapter::name() const +{ + return "OndselSolver (Lagrangian)"; +} + +bool OndselAdapter::is_deterministic() const +{ + return true; +} + +bool OndselAdapter::supports_bundle_fixed() const +{ + return false; +} + +std::vector OndselAdapter::supported_joints() const +{ + return { + BaseJointKind::Coincident, + BaseJointKind::PointOnLine, + BaseJointKind::PointInPlane, + BaseJointKind::Concentric, + BaseJointKind::Tangent, + BaseJointKind::Planar, + BaseJointKind::LineInPlane, + BaseJointKind::Parallel, + BaseJointKind::Perpendicular, + BaseJointKind::Angle, + BaseJointKind::Fixed, + BaseJointKind::Revolute, + BaseJointKind::Cylindrical, + BaseJointKind::Slider, + BaseJointKind::Ball, + BaseJointKind::Screw, + BaseJointKind::Gear, + BaseJointKind::RackPinion, + BaseJointKind::DistancePointPoint, + BaseJointKind::DistanceCylSph, + }; +} + + +// ── Quaternion → rotation matrix ─────────────────────────────────── + +void OndselAdapter::quat_to_matrix(const std::array& q, + double (&mat)[3][3]) +{ + double w = q[0], x = q[1], y = q[2], z = q[3]; + double xx = x * x, yy = y * y, zz = z * z; + double xy = x * y, xz = x * z, yz = y * z; + double wx = w * x, wy = w * y, wz = w * z; + + mat[0][0] = 1.0 - 2.0 * (yy + zz); + mat[0][1] = 2.0 * (xy - wz); + mat[0][2] = 2.0 * (xz + wy); + mat[1][0] = 2.0 * (xy + wz); + mat[1][1] = 1.0 - 2.0 * (xx + zz); + mat[1][2] = 2.0 * (yz - wx); + mat[2][0] = 2.0 * (xz - wy); + mat[2][1] = 2.0 * (yz + wx); + mat[2][2] = 1.0 - 2.0 * (xx + yy); +} + + +// ── Assembly building ────────────────────────────────────────────── + +std::shared_ptr OndselAdapter::make_part(const Part& part) +{ + auto mbdPart = CREATE::With(); + mbdPart->setName(part.id); + + auto massMarker = CREATE::With(); + massMarker->setMass(part.mass); + massMarker->setDensity(1.0); + massMarker->setMomentOfInertias(1.0, 1.0, 1.0); + mbdPart->setPrincipalMassMarker(massMarker); + + const auto& pos = part.placement.position; + mbdPart->setPosition3D(pos[0], pos[1], pos[2]); + + double mat[3][3]; + quat_to_matrix(part.placement.quaternion, mat); + mbdPart->setRotationMatrix( + mat[0][0], mat[0][1], mat[0][2], + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2]); + + return mbdPart; +} + +std::shared_ptr OndselAdapter::make_marker(const std::string& markerName, + const Transform& tf) +{ + auto mbdMarker = CREATE::With(); + mbdMarker->setName(markerName); + + const auto& pos = tf.position; + mbdMarker->setPosition3D(pos[0], pos[1], pos[2]); + + double mat[3][3]; + quat_to_matrix(tf.quaternion, mat); + mbdMarker->setRotationMatrix( + mat[0][0], mat[0][1], mat[0][2], + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2]); + + return mbdMarker; +} + +std::shared_ptr OndselAdapter::create_joint(const Constraint& c) +{ + auto param = [&](std::size_t i, double fallback = 0.0) -> double { + return i < c.params.size() ? c.params[i] : fallback; + }; + + switch (c.type) { + case BaseJointKind::Coincident: + return CREATE::With(); + + case BaseJointKind::PointOnLine: { + auto j = CREATE::With(); + j->distanceIJ = param(0); + return j; + } + + case BaseJointKind::PointInPlane: { + auto j = CREATE::With(); + j->offset = param(0); + return j; + } + + case BaseJointKind::Concentric: { + auto j = CREATE::With(); + j->distanceIJ = param(0); + return j; + } + + case BaseJointKind::Tangent: { + auto j = CREATE::With(); + j->offset = param(0); + return j; + } + + case BaseJointKind::Planar: { + auto j = CREATE::With(); + j->offset = param(0); + return j; + } + + case BaseJointKind::LineInPlane: { + auto j = CREATE::With(); + j->offset = param(0); + return j; + } + + case BaseJointKind::Parallel: + return CREATE::With(); + + case BaseJointKind::Perpendicular: + return CREATE::With(); + + case BaseJointKind::Angle: { + auto j = CREATE::With(); + j->theIzJz = param(0); + return j; + } + + case BaseJointKind::Fixed: + return CREATE::With(); + + case BaseJointKind::Revolute: + return CREATE::With(); + + case BaseJointKind::Cylindrical: + return CREATE::With(); + + case BaseJointKind::Slider: + return CREATE::With(); + + case BaseJointKind::Ball: + return CREATE::With(); + + case BaseJointKind::Screw: { + auto j = CREATE::With(); + j->pitch = param(0); + return j; + } + + case BaseJointKind::Gear: { + auto j = CREATE::With(); + j->radiusI = param(0); + j->radiusJ = param(1); + return j; + } + + case BaseJointKind::RackPinion: { + auto j = CREATE::With(); + j->pitchRadius = param(0); + return j; + } + + case BaseJointKind::DistancePointPoint: { + auto j = CREATE::With(); + j->distanceIJ = param(0); + return j; + } + + case BaseJointKind::DistanceCylSph: { + auto j = CREATE::With(); + j->distanceIJ = param(0); + return j; + } + + // Unsupported types + case BaseJointKind::Universal: + case BaseJointKind::Cam: + case BaseJointKind::Slot: + case BaseJointKind::Custom: + Base::Console().warning( + "KCSolve: OndselAdapter does not support joint kind %d for constraint '%s'\n", + static_cast(c.type), c.id.c_str()); + return nullptr; + } + + return nullptr; // unreachable, but silences compiler warnings +} + +void OndselAdapter::add_limits(const Constraint& c, + const std::string& marker_i, + const std::string& marker_j) +{ + for (const auto& lim : c.limits) { + switch (lim.kind) { + case Constraint::Limit::Kind::TranslationMin: { + auto limit = CREATE::With(); + limit->setName(c.id + "-LimitLenMin"); + limit->setMarkerI(marker_i); + limit->setMarkerJ(marker_j); + limit->settype("=>"); + limit->setlimit(std::to_string(lim.value)); + limit->settol(std::to_string(lim.tolerance)); + assembly_->addLimit(limit); + break; + } + case Constraint::Limit::Kind::TranslationMax: { + auto limit = CREATE::With(); + limit->setName(c.id + "-LimitLenMax"); + limit->setMarkerI(marker_i); + limit->setMarkerJ(marker_j); + limit->settype("=<"); + limit->setlimit(std::to_string(lim.value)); + limit->settol(std::to_string(lim.tolerance)); + assembly_->addLimit(limit); + break; + } + case Constraint::Limit::Kind::RotationMin: { + auto limit = CREATE::With(); + limit->setName(c.id + "-LimitRotMin"); + limit->setMarkerI(marker_i); + limit->setMarkerJ(marker_j); + limit->settype("=>"); + limit->setlimit(std::to_string(lim.value) + "*pi/180.0"); + limit->settol(std::to_string(lim.tolerance)); + assembly_->addLimit(limit); + break; + } + case Constraint::Limit::Kind::RotationMax: { + auto limit = CREATE::With(); + limit->setName(c.id + "-LimitRotMax"); + limit->setMarkerI(marker_i); + limit->setMarkerJ(marker_j); + limit->settype("=<"); + limit->setlimit(std::to_string(lim.value) + "*pi/180.0"); + limit->settol(std::to_string(lim.tolerance)); + assembly_->addLimit(limit); + break; + } + } + } +} + +void OndselAdapter::add_motions(const SolveContext& ctx, + const std::string& marker_i, + const std::string& marker_j, + const std::string& joint_id) +{ + // Collect motions that target this joint. + std::vector joint_motions; + for (const auto& m : ctx.motions) { + if (m.joint_id == joint_id) { + joint_motions.push_back(&m); + } + } + + if (joint_motions.empty()) { + return; + } + + // If there are two motions of different kinds on the same joint, + // combine them into a GeneralMotion (cylindrical joint case). + if (joint_motions.size() == 2 + && joint_motions[0]->kind != joint_motions[1]->kind) { + auto motion = CREATE::With(); + motion->setName(joint_id + "-GeneralMotion"); + motion->setMarkerI(marker_i); + motion->setMarkerJ(marker_j); + + for (const auto* m : joint_motions) { + if (m->kind == MotionDef::Kind::Rotational) { + motion->angIJJ->atiput(2, m->rotation_expr); + } + else { + motion->rIJI->atiput(2, m->translation_expr); + } + } + assembly_->addMotion(motion); + return; + } + + // Single motion or multiple of the same kind. + for (const auto* m : joint_motions) { + switch (m->kind) { + case MotionDef::Kind::Rotational: { + auto motion = CREATE::With(); + motion->setName(joint_id + "-AngularMotion"); + motion->setMarkerI(marker_i); + motion->setMarkerJ(marker_j); + motion->setRotationZ(m->rotation_expr); + assembly_->addMotion(motion); + break; + } + case MotionDef::Kind::Translational: { + auto motion = CREATE::With(); + motion->setName(joint_id + "-LinearMotion"); + motion->setMarkerI(marker_i); + motion->setMarkerJ(marker_j); + motion->setTranslationZ(m->translation_expr); + assembly_->addMotion(motion); + break; + } + case MotionDef::Kind::General: { + auto motion = CREATE::With(); + motion->setName(joint_id + "-GeneralMotion"); + motion->setMarkerI(marker_i); + motion->setMarkerJ(marker_j); + if (!m->rotation_expr.empty()) { + motion->angIJJ->atiput(2, m->rotation_expr); + } + if (!m->translation_expr.empty()) { + motion->rIJI->atiput(2, m->translation_expr); + } + assembly_->addMotion(motion); + break; + } + } + } +} + +void OndselAdapter::fix_grounded_parts(const SolveContext& ctx) +{ + for (const auto& part : ctx.parts) { + if (!part.grounded) { + continue; + } + + auto it = part_map_.find(part.id); + if (it == part_map_.end()) { + continue; + } + + // Assembly-level marker at the part's placement. + std::string asmMarkerName = "ground-" + part.id; + auto asmMarker = make_marker(asmMarkerName, part.placement); + assembly_->addMarker(asmMarker); + + // Part-level marker at identity. + std::string partMarkerName = "FixingMarker"; + auto partMarker = make_marker(partMarkerName, Transform::identity()); + it->second->addMarker(partMarker); + + // Fixed joint connecting them. + auto fixedJoint = CREATE::With(); + fixedJoint->setName("ground-fix-" + part.id); + fixedJoint->setMarkerI("/OndselAssembly/" + asmMarkerName); + fixedJoint->setMarkerJ("/OndselAssembly/" + part.id + "/" + partMarkerName); + assembly_->addJoint(fixedJoint); + } +} + +void OndselAdapter::set_simulation_params(const SimulationParams& params) +{ + auto mbdSim = assembly_->simulationParameters; + mbdSim->settstart(params.t_start); + mbdSim->settend(params.t_end); + mbdSim->sethout(params.h_out); + mbdSim->sethmin(params.h_min); + mbdSim->sethmax(params.h_max); + mbdSim->seterrorTol(params.error_tol); +} + +void OndselAdapter::build_assembly(const SolveContext& ctx) +{ + assembly_ = CREATE::With(); + assembly_->setName("OndselAssembly"); + part_map_.clear(); + + // Do NOT set externalSystem->freecadAssemblyObject — breaking the coupling. + + // Add parts. + for (const auto& part : ctx.parts) { + auto mbdPart = make_part(part); + assembly_->addPart(mbdPart); + part_map_[part.id] = mbdPart; + } + + // Fix grounded parts. + fix_grounded_parts(ctx); + + // Add constraints (joints + limits + motions). + for (const auto& c : ctx.constraints) { + if (!c.activated) { + continue; + } + + auto mbdJoint = create_joint(c); + if (!mbdJoint) { + continue; + } + + // Create markers on the respective parts. + auto it_i = part_map_.find(c.part_i); + auto it_j = part_map_.find(c.part_j); + if (it_i == part_map_.end() || it_j == part_map_.end()) { + Base::Console().warning( + "KCSolve: constraint '%s' references unknown part(s)\n", + c.id.c_str()); + continue; + } + + std::string markerNameI = c.id + "-mkrI"; + std::string markerNameJ = c.id + "-mkrJ"; + + auto mkrI = make_marker(markerNameI, c.marker_i); + it_i->second->addMarker(mkrI); + + auto mkrJ = make_marker(markerNameJ, c.marker_j); + it_j->second->addMarker(mkrJ); + + std::string fullMarkerI = "/OndselAssembly/" + c.part_i + "/" + markerNameI; + std::string fullMarkerJ = "/OndselAssembly/" + c.part_j + "/" + markerNameJ; + + mbdJoint->setName(c.id); + mbdJoint->setMarkerI(fullMarkerI); + mbdJoint->setMarkerJ(fullMarkerJ); + assembly_->addJoint(mbdJoint); + + // Add limits (only when not in simulation mode). + if (!ctx.simulation.has_value() && !c.limits.empty()) { + add_limits(c, fullMarkerI, fullMarkerJ); + } + + // Add motions. + if (!ctx.motions.empty()) { + add_motions(ctx, fullMarkerI, fullMarkerJ, c.id); + } + } + + // Set simulation parameters if present. + if (ctx.simulation.has_value()) { + set_simulation_params(*ctx.simulation); + } +} + + +// ── Result extraction ────────────────────────────────────────────── + +Transform OndselAdapter::extract_part_transform( + const std::shared_ptr& part) const +{ + Transform tf; + double x, y, z; + part->getPosition3D(x, y, z); + tf.position = {x, y, z}; + + double q0, q1, q2, q3; + part->getQuarternions(q0, q1, q2, q3); + // OndselSolver returns (w, x, y, z) — matches our convention. + tf.quaternion = {q0, q1, q2, q3}; + + return tf; +} + +SolveResult OndselAdapter::extract_result() const +{ + SolveResult result; + result.status = SolveStatus::Success; + + for (const auto& [id, mbdPart] : part_map_) { + SolveResult::PartResult pr; + pr.id = id; + pr.placement = extract_part_transform(mbdPart); + result.placements.push_back(std::move(pr)); + } + + return result; +} + +std::vector OndselAdapter::extract_diagnostics() const +{ + std::vector diags; + + if (!assembly_ || !assembly_->mbdSystem) { + return diags; + } + + assembly_->mbdSystem->jointsMotionsDo([&](std::shared_ptr jm) { + if (!jm) { + return; + } + + bool isRedundant = false; + jm->constraintsDo([&](std::shared_ptr con) { + if (!con) { + return; + } + std::string spec = con->constraintSpec(); + if (spec.rfind("Redundant", 0) == 0) { + isRedundant = true; + } + }); + + if (isRedundant) { + // Extract the constraint name from the solver's joint name. + // Format: "/OndselAssembly/ground_moves#Joint001" → "Joint001" + std::string fullName = jm->name; + std::size_t hashPos = fullName.find_last_of('#'); + std::string cleanName = (hashPos != std::string::npos) + ? fullName.substr(hashPos + 1) + : fullName; + + ConstraintDiagnostic diag; + diag.constraint_id = cleanName; + diag.kind = ConstraintDiagnostic::Kind::Redundant; + diag.detail = "Constraint is redundant"; + diags.push_back(std::move(diag)); + } + }); + + return diags; +} + + +// ── Solve operations ─────────────────────────────────────────────── + +SolveResult OndselAdapter::solve(const SolveContext& ctx) +{ + try { + build_assembly(ctx); + assembly_->runPreDrag(); + return extract_result(); + } + catch (const std::exception& e) { + Base::Console().warning("KCSolve: OndselAdapter solve failed: %s\n", e.what()); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + catch (...) { + Base::Console().warning("KCSolve: OndselAdapter solve failed: unknown exception\n"); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } +} + +SolveResult OndselAdapter::update(const SolveContext& ctx) +{ + return solve(ctx); +} + + +// ── Drag protocol ────────────────────────────────────────────────── + +SolveResult OndselAdapter::pre_drag(const SolveContext& ctx, + const std::vector& drag_parts) +{ + drag_part_ids_ = drag_parts; + + try { + build_assembly(ctx); + assembly_->runPreDrag(); + return extract_result(); + } + catch (const std::exception& e) { + Base::Console().warning("KCSolve: OndselAdapter pre_drag failed: %s\n", e.what()); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + catch (...) { + Base::Console().warning("KCSolve: OndselAdapter pre_drag failed: unknown exception\n"); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } +} + +SolveResult OndselAdapter::drag_step( + const std::vector& drag_placements) +{ + if (!assembly_) { + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + + try { + auto dragParts = std::make_shared>>(); + + for (const auto& dp : drag_placements) { + auto it = part_map_.find(dp.id); + if (it == part_map_.end()) { + continue; + } + auto& mbdPart = it->second; + + // Update position. + const auto& pos = dp.placement.position; + mbdPart->updateMbDFromPosition3D(pos[0], pos[1], pos[2]); + + // Update rotation. + double mat[3][3]; + quat_to_matrix(dp.placement.quaternion, mat); + mbdPart->updateMbDFromRotationMatrix( + mat[0][0], mat[0][1], mat[0][2], + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2]); + + dragParts->push_back(mbdPart); + } + + assembly_->runDragStep(dragParts); + return extract_result(); + } + catch (...) { + // Drag step failures are non-fatal — caller will skip this frame. + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } +} + +void OndselAdapter::post_drag() +{ + if (assembly_) { + assembly_->runPostDrag(); + } + drag_part_ids_.clear(); +} + + +// ── Kinematic simulation ─────────────────────────────────────────── + +SolveResult OndselAdapter::run_kinematic(const SolveContext& ctx) +{ + try { + build_assembly(ctx); + assembly_->runKINEMATIC(); + + auto result = extract_result(); + result.num_frames = assembly_->numberOfFrames(); + return result; + } + catch (const std::exception& e) { + Base::Console().warning("KCSolve: OndselAdapter run_kinematic failed: %s\n", e.what()); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + catch (...) { + Base::Console().warning( + "KCSolve: OndselAdapter run_kinematic failed: unknown exception\n"); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } +} + +std::size_t OndselAdapter::num_frames() const +{ + if (!assembly_) { + return 0; + } + return assembly_->numberOfFrames(); +} + +SolveResult OndselAdapter::update_for_frame(std::size_t index) +{ + if (!assembly_) { + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + + if (index >= assembly_->numberOfFrames()) { + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + + assembly_->updateForFrame(index); + return extract_result(); +} + + +// ── Diagnostics ──────────────────────────────────────────────────── + +std::vector OndselAdapter::diagnose(const SolveContext& ctx) +{ + // Ensure we have a solved assembly to inspect. + if (!assembly_ || !assembly_->mbdSystem) { + solve(ctx); + } + return extract_diagnostics(); +} + +// ── Native export ────────────────────────────────────────────────── + +void OndselAdapter::export_native(const std::string& path) +{ + if (assembly_) { + assembly_->outputFile(path); + } +} + +} // namespace KCSolve diff --git a/src/Mod/Assembly/Solver/OndselAdapter.h b/src/Mod/Assembly/Solver/OndselAdapter.h new file mode 100644 index 0000000000..ba26093345 --- /dev/null +++ b/src/Mod/Assembly/Solver/OndselAdapter.h @@ -0,0 +1,129 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#ifndef KCSOLVE_ONDSELADAPTER_H +#define KCSOLVE_ONDSELADAPTER_H + +#include +#include +#include +#include + +#include "IKCSolver.h" +#include "KCSolveGlobal.h" + +namespace MbD +{ +class ASMTAssembly; +class ASMTJoint; +class ASMTMarker; +class ASMTPart; +} // namespace MbD + +namespace KCSolve +{ + +/// IKCSolver implementation wrapping OndselSolver's Lagrangian MBD engine. +/// +/// Translates KCSolve types (SolveContext, BaseJointKind, Transform) to +/// OndselSolver's ASMT hierarchy (ASMTAssembly, ASMTPart, ASMTJoint, etc.) +/// and extracts results back into SolveResult. +/// +/// All OndselSolver #includes are confined to OndselAdapter.cpp. + +class KCSolveExport OndselAdapter : public IKCSolver +{ +public: + OndselAdapter() = default; + + // ── IKCSolver pure virtuals ──────────────────────────────────── + + std::string name() const override; + std::vector supported_joints() const override; + SolveResult solve(const SolveContext& ctx) override; + + // ── IKCSolver overrides ──────────────────────────────────────── + + SolveResult update(const SolveContext& ctx) override; + + SolveResult pre_drag(const SolveContext& ctx, + const std::vector& drag_parts) override; + SolveResult drag_step( + const std::vector& drag_placements) override; + void post_drag() override; + + SolveResult run_kinematic(const SolveContext& ctx) override; + std::size_t num_frames() const override; + SolveResult update_for_frame(std::size_t index) override; + + std::vector diagnose(const SolveContext& ctx) override; + + bool is_deterministic() const override; + bool supports_bundle_fixed() const override; + void export_native(const std::string& path) override; + + /// Register OndselAdapter as "ondsel" in the SolverRegistry. + /// Call once at module init time. + static void register_solver(); + +private: + // ── Assembly building ────────────────────────────────────────── + + void build_assembly(const SolveContext& ctx); + std::shared_ptr make_part(const Part& part); + std::shared_ptr make_marker(const std::string& name, + const Transform& tf); + std::shared_ptr create_joint(const Constraint& c); + void add_limits(const Constraint& c, + const std::string& marker_i, + const std::string& marker_j); + void add_motions(const SolveContext& ctx, + const std::string& marker_i, + const std::string& marker_j, + const std::string& joint_id); + void fix_grounded_parts(const SolveContext& ctx); + void set_simulation_params(const SimulationParams& params); + + // ── Result extraction ────────────────────────────────────────── + + SolveResult extract_result() const; + std::vector extract_diagnostics() const; + Transform extract_part_transform( + const std::shared_ptr& part) const; + + // ── Quaternion ↔ rotation matrix conversion ──────────────────── + + /// Convert unit quaternion (w,x,y,z) to 3×3 rotation matrix (row-major). + static void quat_to_matrix(const std::array& q, + double (&mat)[3][3]); + + // ── Internal state ───────────────────────────────────────────── + + std::shared_ptr assembly_; + std::unordered_map> part_map_; + std::vector drag_part_ids_; +}; + +} // namespace KCSolve + +#endif // KCSOLVE_ONDSELADAPTER_H diff --git a/src/Mod/Assembly/Solver/SolverRegistry.cpp b/src/Mod/Assembly/Solver/SolverRegistry.cpp new file mode 100644 index 0000000000..a3544e939a --- /dev/null +++ b/src/Mod/Assembly/Solver/SolverRegistry.cpp @@ -0,0 +1,346 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#include "SolverRegistry.h" + +#include + +#include +#include +#include +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN +# include +#else +# include +#endif + +namespace fs = std::filesystem; + +namespace +{ + +// Platform extension for shared libraries. +#ifdef _WIN32 +constexpr const char* PLUGIN_EXT = ".dll"; +constexpr char PATH_SEP = ';'; +#elif defined(__APPLE__) +constexpr const char* PLUGIN_EXT = ".dylib"; +constexpr char PATH_SEP = ':'; +#else +constexpr const char* PLUGIN_EXT = ".so"; +constexpr char PATH_SEP = ':'; +#endif + +// Dynamic library loading wrappers. +void* open_library(const char* path) +{ +#ifdef _WIN32 + return static_cast(LoadLibraryA(path)); +#else + return dlopen(path, RTLD_LAZY); +#endif +} + +void* get_symbol(void* handle, const char* symbol) +{ +#ifdef _WIN32 + return reinterpret_cast( + GetProcAddress(static_cast(handle), symbol)); +#else + return dlsym(handle, symbol); +#endif +} + +std::string load_error() +{ +#ifdef _WIN32 + DWORD err = GetLastError(); + char* msg = nullptr; + FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, + nullptr, err, 0, reinterpret_cast(&msg), 0, nullptr); + std::string result = msg ? msg : "unknown error"; + LocalFree(msg); + return result; +#else + const char* err = dlerror(); + return err ? err : "unknown error"; +#endif +} + +/// Parse major version from a version string like "1.0" or "2.1.3". +/// Returns -1 on failure. +int parse_major_version(const char* version_str) +{ + if (!version_str) { + return -1; + } + char* end = nullptr; + long major = std::strtol(version_str, &end, 10); + if (end == version_str || major < 0) { + return -1; + } + return static_cast(major); +} + +} // anonymous namespace + + +namespace KCSolve +{ + +// Plugin C entry point types. +using ApiVersionFn = const char* (*)(); +using CreateFn = IKCSolver* (*)(); + + +// ── Singleton ────────────────────────────────────────────────────── + +SolverRegistry& SolverRegistry::instance() +{ + static SolverRegistry reg; + return reg; +} + +SolverRegistry::SolverRegistry() = default; + +SolverRegistry::~SolverRegistry() +{ + for (void* handle : handles_) { + close_handle(handle); + } +} + +void SolverRegistry::close_handle(void* handle) +{ + if (!handle) { + return; + } +#ifdef _WIN32 + FreeLibrary(static_cast(handle)); +#else + dlclose(handle); +#endif +} + + +// ── Registration ─────────────────────────────────────────────────── + +bool SolverRegistry::register_solver(const std::string& name, CreateSolverFn factory) +{ + std::lock_guard lock(mutex_); + auto [it, inserted] = factories_.emplace(name, std::move(factory)); + if (!inserted) { + Base::Console().warning("KCSolve: solver '%s' already registered, skipping\n", + name.c_str()); + return false; + } + if (default_name_.empty()) { + default_name_ = name; + } + Base::Console().log("KCSolve: registered solver '%s'\n", name.c_str()); + return true; +} + + +// ── Lookup ───────────────────────────────────────────────────────── + +std::unique_ptr SolverRegistry::get(const std::string& name) const +{ + std::lock_guard lock(mutex_); + const std::string& key = name.empty() ? default_name_ : name; + if (key.empty()) { + return nullptr; + } + auto it = factories_.find(key); + if (it == factories_.end()) { + return nullptr; + } + return it->second(); +} + +std::vector SolverRegistry::available() const +{ + std::lock_guard lock(mutex_); + std::vector names; + names.reserve(factories_.size()); + for (const auto& [name, _] : factories_) { + names.push_back(name); + } + return names; +} + +std::vector SolverRegistry::joints_for(const std::string& name) const +{ + auto solver = get(name); + if (!solver) { + return {}; + } + return solver->supported_joints(); +} + +bool SolverRegistry::set_default(const std::string& name) +{ + std::lock_guard lock(mutex_); + if (factories_.find(name) == factories_.end()) { + return false; + } + default_name_ = name; + return true; +} + +std::string SolverRegistry::get_default() const +{ + std::lock_guard lock(mutex_); + return default_name_; +} + + +// ── Plugin scanning ──────────────────────────────────────────────── + +void SolverRegistry::scan(const std::string& directory) +{ + std::error_code ec; + if (!fs::is_directory(directory, ec)) { + // Non-existent directories are not an error — just skip. + return; + } + + Base::Console().log("KCSolve: scanning '%s' for plugins\n", directory.c_str()); + + for (const auto& entry : fs::directory_iterator(directory, ec)) { + if (ec) { + Base::Console().warning("KCSolve: error iterating '%s': %s\n", + directory.c_str(), ec.message().c_str()); + break; + } + + if (!entry.is_regular_file(ec)) { + continue; + } + + const auto& path = entry.path(); + if (path.extension() != PLUGIN_EXT) { + continue; + } + + const std::string path_str = path.string(); + + // Load the shared library. + void* handle = open_library(path_str.c_str()); + if (!handle) { + Base::Console().warning("KCSolve: failed to load '%s': %s\n", + path_str.c_str(), load_error().c_str()); + continue; + } + + // Check API version. + auto version_fn = reinterpret_cast( + get_symbol(handle, "kcsolve_api_version")); + if (!version_fn) { + // Not a KCSolve plugin — silently skip. + close_handle(handle); + continue; + } + + const char* version_str = version_fn(); + int major = parse_major_version(version_str); + if (major != API_VERSION_MAJOR) { + Base::Console().warning( + "KCSolve: plugin '%s' has incompatible API version '%s' " + "(expected major %d)\n", + path_str.c_str(), + version_str ? version_str : "(null)", + API_VERSION_MAJOR); + close_handle(handle); + continue; + } + + // Get the factory symbol. + auto create_fn = reinterpret_cast( + get_symbol(handle, "kcsolve_create")); + if (!create_fn) { + Base::Console().warning( + "KCSolve: plugin '%s' missing kcsolve_create() symbol\n", + path_str.c_str()); + close_handle(handle); + continue; + } + + // Create a temporary instance to get the solver name. + std::unique_ptr probe(create_fn()); + if (!probe) { + Base::Console().warning( + "KCSolve: plugin '%s' kcsolve_create() returned null\n", + path_str.c_str()); + close_handle(handle); + continue; + } + + std::string solver_name = probe->name(); + probe.reset(); + + // Wrap the C function pointer in a factory lambda. + CreateSolverFn factory = [create_fn]() -> std::unique_ptr { + return std::unique_ptr(create_fn()); + }; + + if (register_solver(solver_name, std::move(factory))) { + handles_.push_back(handle); + Base::Console().log("KCSolve: loaded plugin '%s' from '%s'\n", + solver_name.c_str(), path_str.c_str()); + } + else { + // Duplicate name — close the handle. + close_handle(handle); + } + } +} + +void SolverRegistry::scan_default_paths() +{ + // 1. KCSOLVE_PLUGIN_PATH environment variable. + const char* env_path = std::getenv("KCSOLVE_PLUGIN_PATH"); + if (env_path && env_path[0] != '\0') { + std::istringstream stream(env_path); + std::string dir; + while (std::getline(stream, dir, PATH_SEP)) { + if (!dir.empty()) { + scan(dir); + } + } + } + + // 2. System install path: /lib/kcsolve/ + // Derive from the executable location or use a compile-time path. + // For now, use a path relative to the FreeCAD lib directory. + std::error_code ec; + fs::path system_dir = fs::path(CMAKE_INSTALL_PREFIX) / "lib" / "kcsolve"; + if (fs::is_directory(system_dir, ec)) { + scan(system_dir.string()); + } +} + +} // namespace KCSolve diff --git a/src/Mod/Assembly/Solver/SolverRegistry.h b/src/Mod/Assembly/Solver/SolverRegistry.h new file mode 100644 index 0000000000..056274dc33 --- /dev/null +++ b/src/Mod/Assembly/Solver/SolverRegistry.h @@ -0,0 +1,124 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#ifndef KCSOLVE_SOLVERREGISTRY_H +#define KCSOLVE_SOLVERREGISTRY_H + +#include +#include +#include +#include +#include +#include + +#include "IKCSolver.h" +#include "KCSolveGlobal.h" + +namespace KCSolve +{ + +/// Factory function that creates a solver instance. +using CreateSolverFn = std::function()>; + +/// Current KCSolve API major version. Plugins must match this to load. +constexpr int API_VERSION_MAJOR = 1; + +/// Singleton registry for pluggable solver backends. +/// +/// Solver plugins register themselves at module load time via +/// register_solver(). The Assembly module retrieves solvers via get(). +/// +/// Thread safety: all public methods are internally synchronized. +/// +/// Usage: +/// // Registration (at module init): +/// KCSolve::SolverRegistry::instance().register_solver( +/// "ondsel", []() { return std::make_unique(); }); +/// +/// // Retrieval: +/// auto solver = KCSolve::SolverRegistry::instance().get(); // default +/// auto solver = KCSolve::SolverRegistry::instance().get("ondsel"); + +class KCSolveExport SolverRegistry +{ +public: + /// Access the singleton instance. + static SolverRegistry& instance(); + + ~SolverRegistry(); + + /// Register a solver backend. + /// @param name Unique solver name (e.g. "ondsel"). + /// @param factory Factory function that creates solver instances. + /// @return true if registration succeeded, false if name taken. + bool register_solver(const std::string& name, CreateSolverFn factory); + + /// Create an instance of the named solver. + /// @param name Solver name. If empty, uses the default solver. + /// @return Solver instance, or nullptr if not found. + std::unique_ptr get(const std::string& name = {}) const; + + /// Return the names of all registered solvers. + std::vector available() const; + + /// Query which BaseJointKind values a named solver supports. + /// Creates a temporary instance to call supported_joints(). + std::vector joints_for(const std::string& name) const; + + /// Set the default solver name. + /// @return true if the name is registered, false otherwise. + bool set_default(const std::string& name); + + /// Get the default solver name. + std::string get_default() const; + + /// Scan a directory for solver plugin shared libraries. + /// Each plugin must export kcsolve_api_version() and kcsolve_create(). + /// Non-existent or empty directories are handled gracefully. + void scan(const std::string& directory); + + /// Scan all default plugin discovery paths: + /// 1. KCSOLVE_PLUGIN_PATH env var (colon-separated, semicolon on Windows) + /// 2. /lib/kcsolve/ + void scan_default_paths(); + +private: + SolverRegistry(); + + SolverRegistry(const SolverRegistry&) = delete; + SolverRegistry& operator=(const SolverRegistry&) = delete; + SolverRegistry(SolverRegistry&&) = delete; + SolverRegistry& operator=(SolverRegistry&&) = delete; + + /// Close a single plugin handle (platform-specific). + static void close_handle(void* handle); + + mutable std::mutex mutex_; + std::unordered_map factories_; + std::string default_name_; + std::vector handles_; // loaded plugin library handles +}; + +} // namespace KCSolve + +#endif // KCSOLVE_SOLVERREGISTRY_H diff --git a/src/Mod/Assembly/Solver/Types.h b/src/Mod/Assembly/Solver/Types.h new file mode 100644 index 0000000000..6fdcca6fb3 --- /dev/null +++ b/src/Mod/Assembly/Solver/Types.h @@ -0,0 +1,286 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#ifndef KCSOLVE_TYPES_H +#define KCSOLVE_TYPES_H + +#include +#include +#include +#include +#include +#include + +namespace KCSolve +{ + +// ── Transform ────────────────────────────────────────────────────── +// +// Rigid-body transform: position (x, y, z) + unit quaternion (w, x, y, z). +// Semantically equivalent to Base::Placement but free of FreeCAD dependencies +// so that KCSolve headers remain standalone (for future server worker use). +// +// Quaternion convention: (w, x, y, z) — mathematical standard. +// Note: Base::Rotation(q0,q1,q2,q3) uses (x, y, z, w) ordering. +// The adapter layer handles this swap. + +struct Transform +{ + std::array position {0.0, 0.0, 0.0}; + std::array quaternion {1.0, 0.0, 0.0, 0.0}; // w, x, y, z + + static Transform identity() + { + return {}; + } +}; + +// ── BaseJointKind ────────────────────────────────────────────────── +// +// Decomposed primitive constraint types. Uses SOLIDWORKS-inspired vocabulary +// from the INTER_SOLVER.md spec rather than OndselSolver internal names. +// +// The existing Assembly::JointType (13 values) and Assembly::DistanceType +// (35+ values) map to these via the adapter layer. In particular, the +// "Distance" JointType is decomposed based on geometry classification +// (see makeMbdJointDistance in AssemblyObject.cpp). + +enum class BaseJointKind : std::uint8_t +{ + // Point constraints (decomposed from JointType::Distance) + Coincident, // PointOnPoint, d=0 — 3 DOF removed + PointOnLine, // Point constrained to a line — 2 DOF removed + PointInPlane, // Point constrained to a plane — 1 DOF removed + + // Axis/surface constraints (decomposed from JointType::Distance) + Concentric, // Coaxial (line-line, circle-circle, cyl-cyl) — 4 DOF removed + Tangent, // Face-on-face tangency — 1 DOF removed + Planar, // Coplanar faces — 3 DOF removed + LineInPlane, // Line constrained to a plane — 2 DOF removed + + // Axis orientation constraints (direct from JointType) + Parallel, // Parallel axes — 2 DOF removed + Perpendicular, // 90-degree axes — 1 DOF removed + Angle, // Arbitrary axis angle — 1 DOF removed + + // Standard kinematic joints (direct 1:1 from JointType) + Fixed, // Rigid weld — 6 DOF removed + Revolute, // Hinge — 5 DOF removed + Cylindrical, // Rotation + sliding on axis — 4 DOF removed + Slider, // Linear translation — 5 DOF removed + Ball, // Spherical — 3 DOF removed + Screw, // Helical (rotation + coupled translation) — 5 DOF removed + Universal, // U-joint / Cardan — 4 DOF removed (future) + + // Mechanical element constraints + Gear, // Gear pair or belt (sign determines direction) + RackPinion, // Rack-and-pinion + Cam, // Cam-follower (future) + Slot, // Slot constraint (future) + + // Distance variants with non-zero offset + DistancePointPoint, // Point-to-point with offset — 2 DOF removed + DistanceCylSph, // Cylinder-sphere distance — varies + + Custom, // Solver-specific extension point +}; + +// ── Part ─────────────────────────────────────────────────────────── + +struct Part +{ + std::string id; + Transform placement; + double mass {1.0}; + bool grounded {false}; +}; + +// ── Constraint ───────────────────────────────────────────────────── +// +// A constraint between two parts. Built from a FreeCAD JointObject by +// the adapter layer (classifying geometry into the specific BaseJointKind). + +struct Constraint +{ + std::string id; // FreeCAD document object name (e.g. "Joint001") + + std::string part_i; // solver-side part ID for first reference + Transform marker_i; // coordinate system on part_i + + std::string part_j; // solver-side part ID for second reference + Transform marker_j; // coordinate system on part_j + + BaseJointKind type {}; + + // Scalar parameters (interpretation depends on type): + // Angle: params[0] = angle in radians + // RackPinion: params[0] = pitch radius + // Screw: params[0] = pitch + // Gear: params[0] = radiusI, params[1] = radiusJ (negative for belt) + // DistancePointPoint: params[0] = distance + // DistanceCylSph: params[0] = distance + // Planar: params[0] = offset + // Concentric: params[0] = distance + // PointInPlane: params[0] = offset + // LineInPlane: params[0] = offset + std::vector params; + + // Joint limits (length or angle bounds) + struct Limit + { + enum class Kind : std::uint8_t + { + TranslationMin, + TranslationMax, + RotationMin, + RotationMax, + }; + + Kind kind {}; + double value {0.0}; + double tolerance {1.0e-9}; + }; + std::vector limits; + + bool activated {true}; +}; + +// ── MotionDef ────────────────────────────────────────────────────── +// +// A motion driver for kinematic simulation. + +struct MotionDef +{ + enum class Kind : std::uint8_t + { + Rotational, + Translational, + General, + }; + + Kind kind {}; + std::string joint_id; // which constraint this drives + std::string marker_i; + std::string marker_j; + + // Motion law expressions (function of time 't'). + // For General: both are set. Otherwise only the relevant one. + std::string rotation_expr; + std::string translation_expr; +}; + +// ── SimulationParams ─────────────────────────────────────────────── +// +// Parameters for kinematic simulation (run_kinematic). +// Maps to create_mbdSimulationParameters() in AssemblyObject.cpp. + +struct SimulationParams +{ + double t_start {0.0}; + double t_end {1.0}; + double h_out {0.01}; // output time step + double h_min {1.0e-9}; + double h_max {1.0}; + double error_tol {1.0e-6}; +}; + +// ── SolveContext ─────────────────────────────────────────────────── +// +// Complete input to a solve operation. Built by the adapter layer +// from FreeCAD document objects. + +struct SolveContext +{ + std::vector parts; + std::vector constraints; + std::vector motions; + + // Present when running kinematic simulation via run_kinematic(). + std::optional simulation; + + // Hint: bundle parts connected by Fixed joints into single rigid bodies. + // When true and the solver does not support_bundle_fixed(), the adapter + // layer pre-bundles before passing to the solver. + bool bundle_fixed {false}; +}; + +// ── SolveStatus ──────────────────────────────────────────────────── +// +// Matches the return codes from AssemblyObject::solve(). + +enum class SolveStatus : std::int8_t +{ + Success = 0, + Failed = -1, + InvalidFlip = -2, // orientation flipped past threshold + NoGroundedParts = -6, // no grounded parts in assembly +}; + +// ── ConstraintDiagnostic ─────────────────────────────────────────── +// +// Per-constraint diagnostic information from updateSolveStatus(). + +struct ConstraintDiagnostic +{ + enum class Kind : std::uint8_t + { + Redundant, + Conflicting, + PartiallyRedundant, + Malformed, + }; + + std::string constraint_id; // FreeCAD object name + Kind kind {}; + std::string detail; // human-readable description +}; + +// ── SolveResult ──────────────────────────────────────────────────── +// +// Output of a solve operation. + +struct SolveResult +{ + SolveStatus status {SolveStatus::Success}; + + // Updated placements for each part (only parts that moved). + struct PartResult + { + std::string id; + Transform placement; + }; + std::vector placements; + + // Degrees of freedom remaining (-1 = unknown). + int dof {-1}; + + // Constraint diagnostics (redundant, conflicting, etc.). + std::vector diagnostics; + + // For kinematic simulation: number of computed frames. + std::size_t num_frames {0}; +}; + +} // namespace KCSolve + +#endif // KCSOLVE_TYPES_H diff --git a/src/Mod/Assembly/Solver/bindings/CMakeLists.txt b/src/Mod/Assembly/Solver/bindings/CMakeLists.txt new file mode 100644 index 0000000000..0cde656f9d --- /dev/null +++ b/src/Mod/Assembly/Solver/bindings/CMakeLists.txt @@ -0,0 +1,31 @@ +# SPDX-License-Identifier: LGPL-2.1-or-later + +set(KCSolvePy_SRCS + PyIKCSolver.h + kcsolve_py.cpp +) + +add_library(kcsolve_py SHARED ${KCSolvePy_SRCS}) + +target_include_directories(kcsolve_py + PRIVATE + ${CMAKE_SOURCE_DIR}/src + ${CMAKE_BINARY_DIR}/src + ${pybind11_INCLUDE_DIR} +) + +target_link_libraries(kcsolve_py + PRIVATE + pybind11::module + Python3::Python + KCSolve +) + +if(FREECAD_WARN_ERROR) + target_compile_warn_error(kcsolve_py) +endif() + +SET_BIN_DIR(kcsolve_py kcsolve /Mod/Assembly) +SET_PYTHON_PREFIX_SUFFIX(kcsolve_py) + +INSTALL(TARGETS kcsolve_py DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/src/Mod/Assembly/Solver/bindings/PyIKCSolver.h b/src/Mod/Assembly/Solver/bindings/PyIKCSolver.h new file mode 100644 index 0000000000..7d8151f49f --- /dev/null +++ b/src/Mod/Assembly/Solver/bindings/PyIKCSolver.h @@ -0,0 +1,121 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#ifndef KCSOLVE_PYIKCSOLVER_H +#define KCSOLVE_PYIKCSOLVER_H + +#include +#include + +#include + +namespace KCSolve +{ + +/// pybind11 trampoline class for IKCSolver. +/// Enables Python subclasses that override virtual methods. +class PyIKCSolver : public IKCSolver +{ +public: + using IKCSolver::IKCSolver; + + // ── Pure virtuals ────────────────────────────────────────────── + + std::string name() const override + { + PYBIND11_OVERRIDE_PURE(std::string, IKCSolver, name); + } + + std::vector supported_joints() const override + { + PYBIND11_OVERRIDE_PURE(std::vector, IKCSolver, supported_joints); + } + + SolveResult solve(const SolveContext& ctx) override + { + PYBIND11_OVERRIDE_PURE(SolveResult, IKCSolver, solve, ctx); + } + + // ── Virtuals with defaults ───────────────────────────────────── + + SolveResult update(const SolveContext& ctx) override + { + PYBIND11_OVERRIDE(SolveResult, IKCSolver, update, ctx); + } + + SolveResult pre_drag(const SolveContext& ctx, + const std::vector& drag_parts) override + { + PYBIND11_OVERRIDE(SolveResult, IKCSolver, pre_drag, ctx, drag_parts); + } + + SolveResult drag_step( + const std::vector& drag_placements) override + { + PYBIND11_OVERRIDE(SolveResult, IKCSolver, drag_step, drag_placements); + } + + void post_drag() override + { + PYBIND11_OVERRIDE(void, IKCSolver, post_drag); + } + + SolveResult run_kinematic(const SolveContext& ctx) override + { + PYBIND11_OVERRIDE(SolveResult, IKCSolver, run_kinematic, ctx); + } + + std::size_t num_frames() const override + { + PYBIND11_OVERRIDE(std::size_t, IKCSolver, num_frames); + } + + SolveResult update_for_frame(std::size_t index) override + { + PYBIND11_OVERRIDE(SolveResult, IKCSolver, update_for_frame, index); + } + + std::vector diagnose(const SolveContext& ctx) override + { + PYBIND11_OVERRIDE(std::vector, IKCSolver, diagnose, ctx); + } + + bool is_deterministic() const override + { + PYBIND11_OVERRIDE(bool, IKCSolver, is_deterministic); + } + + void export_native(const std::string& path) override + { + PYBIND11_OVERRIDE(void, IKCSolver, export_native, path); + } + + bool supports_bundle_fixed() const override + { + PYBIND11_OVERRIDE(bool, IKCSolver, supports_bundle_fixed); + } +}; + +} // namespace KCSolve + +#endif // KCSOLVE_PYIKCSOLVER_H diff --git a/src/Mod/Assembly/Solver/bindings/kcsolve_py.cpp b/src/Mod/Assembly/Solver/bindings/kcsolve_py.cpp new file mode 100644 index 0000000000..d93940b362 --- /dev/null +++ b/src/Mod/Assembly/Solver/bindings/kcsolve_py.cpp @@ -0,0 +1,830 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include "PyIKCSolver.h" + +#include +#include +#include + +namespace py = pybind11; +using namespace KCSolve; + + +// ── Enum string mapping ──────────────────────────────────────────── +// +// Constexpr tables for bidirectional enum <-> string conversion. +// String values match the py::enum_ .value("Name", ...) names exactly, +// which is also the JSON wire format specified in SOLVER.md §3. + +namespace +{ + +template +struct EnumEntry +{ + E value; + const char* name; +}; + +static constexpr EnumEntry kBaseJointKindEntries[] = { + {BaseJointKind::Coincident, "Coincident"}, + {BaseJointKind::PointOnLine, "PointOnLine"}, + {BaseJointKind::PointInPlane, "PointInPlane"}, + {BaseJointKind::Concentric, "Concentric"}, + {BaseJointKind::Tangent, "Tangent"}, + {BaseJointKind::Planar, "Planar"}, + {BaseJointKind::LineInPlane, "LineInPlane"}, + {BaseJointKind::Parallel, "Parallel"}, + {BaseJointKind::Perpendicular, "Perpendicular"}, + {BaseJointKind::Angle, "Angle"}, + {BaseJointKind::Fixed, "Fixed"}, + {BaseJointKind::Revolute, "Revolute"}, + {BaseJointKind::Cylindrical, "Cylindrical"}, + {BaseJointKind::Slider, "Slider"}, + {BaseJointKind::Ball, "Ball"}, + {BaseJointKind::Screw, "Screw"}, + {BaseJointKind::Universal, "Universal"}, + {BaseJointKind::Gear, "Gear"}, + {BaseJointKind::RackPinion, "RackPinion"}, + {BaseJointKind::Cam, "Cam"}, + {BaseJointKind::Slot, "Slot"}, + {BaseJointKind::DistancePointPoint, "DistancePointPoint"}, + {BaseJointKind::DistanceCylSph, "DistanceCylSph"}, + {BaseJointKind::Custom, "Custom"}, +}; + +static constexpr EnumEntry kSolveStatusEntries[] = { + {SolveStatus::Success, "Success"}, + {SolveStatus::Failed, "Failed"}, + {SolveStatus::InvalidFlip, "InvalidFlip"}, + {SolveStatus::NoGroundedParts, "NoGroundedParts"}, +}; + +static constexpr EnumEntry kDiagnosticKindEntries[] = { + {ConstraintDiagnostic::Kind::Redundant, "Redundant"}, + {ConstraintDiagnostic::Kind::Conflicting, "Conflicting"}, + {ConstraintDiagnostic::Kind::PartiallyRedundant, "PartiallyRedundant"}, + {ConstraintDiagnostic::Kind::Malformed, "Malformed"}, +}; + +static constexpr EnumEntry kMotionKindEntries[] = { + {MotionDef::Kind::Rotational, "Rotational"}, + {MotionDef::Kind::Translational, "Translational"}, + {MotionDef::Kind::General, "General"}, +}; + +static constexpr EnumEntry kLimitKindEntries[] = { + {Constraint::Limit::Kind::TranslationMin, "TranslationMin"}, + {Constraint::Limit::Kind::TranslationMax, "TranslationMax"}, + {Constraint::Limit::Kind::RotationMin, "RotationMin"}, + {Constraint::Limit::Kind::RotationMax, "RotationMax"}, +}; + +template +const char* enum_to_str(E val, const EnumEntry (&table)[N]) +{ + for (std::size_t i = 0; i < N; ++i) { + if (table[i].value == val) { + return table[i].name; + } + } + throw py::value_error("Unknown enum value: " + std::to_string(static_cast(val))); +} + +template +E str_to_enum(const std::string& name, const EnumEntry (&table)[N], + const char* enum_type_name) +{ + for (std::size_t i = 0; i < N; ++i) { + if (name == table[i].name) { + return table[i].value; + } + } + throw py::value_error( + std::string("Invalid ") + enum_type_name + " value: '" + name + "'"); +} + + +// ── Dict conversion helpers ──────────────────────────────────────── +// +// Standalone functions for each type so SolveContext/SolveResult can +// reuse them without duplicating serialization logic. + +py::dict transform_to_dict(const Transform& t) +{ + py::dict d; + d["position"] = py::make_tuple(t.position[0], t.position[1], t.position[2]); + d["quaternion"] = py::make_tuple( + t.quaternion[0], t.quaternion[1], t.quaternion[2], t.quaternion[3]); + return d; +} + +Transform transform_from_dict(const py::dict& d) +{ + Transform t; + auto pos = d["position"].cast(); + if (py::len(pos) != 3) { + throw py::value_error("position must have exactly 3 elements"); + } + for (int i = 0; i < 3; ++i) { + t.position[static_cast(i)] = pos[i].cast(); + } + auto quat = d["quaternion"].cast(); + if (py::len(quat) != 4) { + throw py::value_error("quaternion must have exactly 4 elements"); + } + for (int i = 0; i < 4; ++i) { + t.quaternion[static_cast(i)] = quat[i].cast(); + } + return t; +} + +py::dict part_to_dict(const Part& p) +{ + py::dict d; + d["id"] = p.id; + d["placement"] = transform_to_dict(p.placement); + d["mass"] = p.mass; + d["grounded"] = p.grounded; + return d; +} + +Part part_from_dict(const py::dict& d) +{ + Part p; + p.id = d["id"].cast(); + p.placement = transform_from_dict(d["placement"].cast()); + if (d.contains("mass")) { + p.mass = d["mass"].cast(); + } + if (d.contains("grounded")) { + p.grounded = d["grounded"].cast(); + } + return p; +} + +py::dict limit_to_dict(const Constraint::Limit& lim) +{ + py::dict d; + d["kind"] = enum_to_str(lim.kind, kLimitKindEntries); + d["value"] = lim.value; + d["tolerance"] = lim.tolerance; + return d; +} + +Constraint::Limit limit_from_dict(const py::dict& d) +{ + Constraint::Limit lim; + lim.kind = str_to_enum(d["kind"].cast(), + kLimitKindEntries, "LimitKind"); + lim.value = d["value"].cast(); + if (d.contains("tolerance")) { + lim.tolerance = d["tolerance"].cast(); + } + return lim; +} + +py::dict constraint_to_dict(const Constraint& c) +{ + py::dict d; + d["id"] = c.id; + d["part_i"] = c.part_i; + d["marker_i"] = transform_to_dict(c.marker_i); + d["part_j"] = c.part_j; + d["marker_j"] = transform_to_dict(c.marker_j); + d["type"] = enum_to_str(c.type, kBaseJointKindEntries); + d["params"] = py::cast(c.params); + py::list lims; + for (const auto& lim : c.limits) { + lims.append(limit_to_dict(lim)); + } + d["limits"] = lims; + d["activated"] = c.activated; + return d; +} + +Constraint constraint_from_dict(const py::dict& d) +{ + Constraint c; + c.id = d["id"].cast(); + c.part_i = d["part_i"].cast(); + c.marker_i = transform_from_dict(d["marker_i"].cast()); + c.part_j = d["part_j"].cast(); + c.marker_j = transform_from_dict(d["marker_j"].cast()); + c.type = str_to_enum(d["type"].cast(), + kBaseJointKindEntries, "BaseJointKind"); + if (d.contains("params")) { + c.params = d["params"].cast>(); + } + if (d.contains("limits")) { + for (auto item : d["limits"]) { + c.limits.push_back(limit_from_dict(item.cast())); + } + } + if (d.contains("activated")) { + c.activated = d["activated"].cast(); + } + return c; +} + +py::dict motion_to_dict(const MotionDef& m) +{ + py::dict d; + d["kind"] = enum_to_str(m.kind, kMotionKindEntries); + d["joint_id"] = m.joint_id; + d["marker_i"] = m.marker_i; + d["marker_j"] = m.marker_j; + d["rotation_expr"] = m.rotation_expr; + d["translation_expr"] = m.translation_expr; + return d; +} + +MotionDef motion_from_dict(const py::dict& d) +{ + MotionDef m; + m.kind = str_to_enum(d["kind"].cast(), + kMotionKindEntries, "MotionKind"); + m.joint_id = d["joint_id"].cast(); + if (d.contains("marker_i")) { + m.marker_i = d["marker_i"].cast(); + } + if (d.contains("marker_j")) { + m.marker_j = d["marker_j"].cast(); + } + if (d.contains("rotation_expr")) { + m.rotation_expr = d["rotation_expr"].cast(); + } + if (d.contains("translation_expr")) { + m.translation_expr = d["translation_expr"].cast(); + } + return m; +} + +py::dict sim_to_dict(const SimulationParams& s) +{ + py::dict d; + d["t_start"] = s.t_start; + d["t_end"] = s.t_end; + d["h_out"] = s.h_out; + d["h_min"] = s.h_min; + d["h_max"] = s.h_max; + d["error_tol"] = s.error_tol; + return d; +} + +SimulationParams sim_from_dict(const py::dict& d) +{ + SimulationParams s; + if (d.contains("t_start")) { + s.t_start = d["t_start"].cast(); + } + if (d.contains("t_end")) { + s.t_end = d["t_end"].cast(); + } + if (d.contains("h_out")) { + s.h_out = d["h_out"].cast(); + } + if (d.contains("h_min")) { + s.h_min = d["h_min"].cast(); + } + if (d.contains("h_max")) { + s.h_max = d["h_max"].cast(); + } + if (d.contains("error_tol")) { + s.error_tol = d["error_tol"].cast(); + } + return s; +} + +py::dict diagnostic_to_dict(const ConstraintDiagnostic& diag) +{ + py::dict d; + d["constraint_id"] = diag.constraint_id; + d["kind"] = enum_to_str(diag.kind, kDiagnosticKindEntries); + d["detail"] = diag.detail; + return d; +} + +ConstraintDiagnostic diagnostic_from_dict(const py::dict& d) +{ + ConstraintDiagnostic diag; + diag.constraint_id = d["constraint_id"].cast(); + diag.kind = str_to_enum(d["kind"].cast(), + kDiagnosticKindEntries, "DiagnosticKind"); + if (d.contains("detail")) { + diag.detail = d["detail"].cast(); + } + return diag; +} + +py::dict part_result_to_dict(const SolveResult::PartResult& pr) +{ + py::dict d; + d["id"] = pr.id; + d["placement"] = transform_to_dict(pr.placement); + return d; +} + +SolveResult::PartResult part_result_from_dict(const py::dict& d) +{ + SolveResult::PartResult pr; + pr.id = d["id"].cast(); + pr.placement = transform_from_dict(d["placement"].cast()); + return pr; +} + +py::dict solve_context_to_dict(const SolveContext& ctx) +{ + py::dict d; + d["api_version"] = API_VERSION_MAJOR; + + py::list parts; + for (const auto& p : ctx.parts) { + parts.append(part_to_dict(p)); + } + d["parts"] = parts; + + py::list constraints; + for (const auto& c : ctx.constraints) { + constraints.append(constraint_to_dict(c)); + } + d["constraints"] = constraints; + + py::list motions; + for (const auto& m : ctx.motions) { + motions.append(motion_to_dict(m)); + } + d["motions"] = motions; + + if (ctx.simulation.has_value()) { + d["simulation"] = sim_to_dict(*ctx.simulation); + } + else { + d["simulation"] = py::none(); + } + + d["bundle_fixed"] = ctx.bundle_fixed; + return d; +} + +SolveContext solve_context_from_dict(const py::dict& d) +{ + SolveContext ctx; + + if (d.contains("api_version")) { + int v = d["api_version"].cast(); + if (v != API_VERSION_MAJOR) { + throw py::value_error( + "Unsupported api_version " + std::to_string(v) + + ", expected " + std::to_string(API_VERSION_MAJOR)); + } + } + + for (auto item : d["parts"]) { + ctx.parts.push_back(part_from_dict(item.cast())); + } + + for (auto item : d["constraints"]) { + ctx.constraints.push_back(constraint_from_dict(item.cast())); + } + + if (d.contains("motions")) { + for (auto item : d["motions"]) { + ctx.motions.push_back(motion_from_dict(item.cast())); + } + } + + if (d.contains("simulation") && !d["simulation"].is_none()) { + ctx.simulation = sim_from_dict(d["simulation"].cast()); + } + + if (d.contains("bundle_fixed")) { + ctx.bundle_fixed = d["bundle_fixed"].cast(); + } + + return ctx; +} + +py::dict solve_result_to_dict(const SolveResult& r) +{ + py::dict d; + d["status"] = enum_to_str(r.status, kSolveStatusEntries); + + py::list placements; + for (const auto& pr : r.placements) { + placements.append(part_result_to_dict(pr)); + } + d["placements"] = placements; + + d["dof"] = r.dof; + + py::list diagnostics; + for (const auto& diag : r.diagnostics) { + diagnostics.append(diagnostic_to_dict(diag)); + } + d["diagnostics"] = diagnostics; + + d["num_frames"] = r.num_frames; + return d; +} + +SolveResult solve_result_from_dict(const py::dict& d) +{ + SolveResult r; + r.status = str_to_enum(d["status"].cast(), + kSolveStatusEntries, "SolveStatus"); + + if (d.contains("placements")) { + for (auto item : d["placements"]) { + r.placements.push_back(part_result_from_dict(item.cast())); + } + } + + if (d.contains("dof")) { + r.dof = d["dof"].cast(); + } + + if (d.contains("diagnostics")) { + for (auto item : d["diagnostics"]) { + r.diagnostics.push_back(diagnostic_from_dict(item.cast())); + } + } + + if (d.contains("num_frames")) { + r.num_frames = d["num_frames"].cast(); + } + + return r; +} + +} // anonymous namespace + + +// ── PySolverHolder ───────────────────────────────────────────────── +// +// Wraps a Python IKCSolver subclass instance so it can live inside a +// std::unique_ptr returned by SolverRegistry::get(). +// Prevents Python GC by holding a py::object reference and acquires +// the GIL before every forwarded call. + +class PySolverHolder : public IKCSolver +{ +public: + explicit PySolverHolder(py::object obj) + : obj_(std::move(obj)) + { + solver_ = obj_.cast(); + } + + std::string name() const override + { + py::gil_scoped_acquire gil; + return solver_->name(); + } + + std::vector supported_joints() const override + { + py::gil_scoped_acquire gil; + return solver_->supported_joints(); + } + + SolveResult solve(const SolveContext& ctx) override + { + py::gil_scoped_acquire gil; + return solver_->solve(ctx); + } + + SolveResult update(const SolveContext& ctx) override + { + py::gil_scoped_acquire gil; + return solver_->update(ctx); + } + + SolveResult pre_drag(const SolveContext& ctx, + const std::vector& drag_parts) override + { + py::gil_scoped_acquire gil; + return solver_->pre_drag(ctx, drag_parts); + } + + SolveResult drag_step( + const std::vector& drag_placements) override + { + py::gil_scoped_acquire gil; + return solver_->drag_step(drag_placements); + } + + void post_drag() override + { + py::gil_scoped_acquire gil; + solver_->post_drag(); + } + + SolveResult run_kinematic(const SolveContext& ctx) override + { + py::gil_scoped_acquire gil; + return solver_->run_kinematic(ctx); + } + + std::size_t num_frames() const override + { + py::gil_scoped_acquire gil; + return solver_->num_frames(); + } + + SolveResult update_for_frame(std::size_t index) override + { + py::gil_scoped_acquire gil; + return solver_->update_for_frame(index); + } + + std::vector diagnose(const SolveContext& ctx) override + { + py::gil_scoped_acquire gil; + return solver_->diagnose(ctx); + } + + bool is_deterministic() const override + { + py::gil_scoped_acquire gil; + return solver_->is_deterministic(); + } + + void export_native(const std::string& path) override + { + py::gil_scoped_acquire gil; + solver_->export_native(path); + } + + bool supports_bundle_fixed() const override + { + py::gil_scoped_acquire gil; + return solver_->supports_bundle_fixed(); + } + +private: + py::object obj_; // prevents Python GC + IKCSolver* solver_; // raw pointer into the trampoline inside obj_ +}; + + +// ── Module definition ────────────────────────────────────────────── + +PYBIND11_MODULE(kcsolve, m) +{ + m.doc() = "KCSolve — pluggable assembly constraint solver API"; + m.attr("API_VERSION_MAJOR") = API_VERSION_MAJOR; + + // ── Enums ────────────────────────────────────────────────────── + + py::enum_(m, "BaseJointKind") + .value("Coincident", BaseJointKind::Coincident) + .value("PointOnLine", BaseJointKind::PointOnLine) + .value("PointInPlane", BaseJointKind::PointInPlane) + .value("Concentric", BaseJointKind::Concentric) + .value("Tangent", BaseJointKind::Tangent) + .value("Planar", BaseJointKind::Planar) + .value("LineInPlane", BaseJointKind::LineInPlane) + .value("Parallel", BaseJointKind::Parallel) + .value("Perpendicular", BaseJointKind::Perpendicular) + .value("Angle", BaseJointKind::Angle) + .value("Fixed", BaseJointKind::Fixed) + .value("Revolute", BaseJointKind::Revolute) + .value("Cylindrical", BaseJointKind::Cylindrical) + .value("Slider", BaseJointKind::Slider) + .value("Ball", BaseJointKind::Ball) + .value("Screw", BaseJointKind::Screw) + .value("Universal", BaseJointKind::Universal) + .value("Gear", BaseJointKind::Gear) + .value("RackPinion", BaseJointKind::RackPinion) + .value("Cam", BaseJointKind::Cam) + .value("Slot", BaseJointKind::Slot) + .value("DistancePointPoint", BaseJointKind::DistancePointPoint) + .value("DistanceCylSph", BaseJointKind::DistanceCylSph) + .value("Custom", BaseJointKind::Custom); + + py::enum_(m, "SolveStatus") + .value("Success", SolveStatus::Success) + .value("Failed", SolveStatus::Failed) + .value("InvalidFlip", SolveStatus::InvalidFlip) + .value("NoGroundedParts", SolveStatus::NoGroundedParts); + + py::enum_(m, "DiagnosticKind") + .value("Redundant", ConstraintDiagnostic::Kind::Redundant) + .value("Conflicting", ConstraintDiagnostic::Kind::Conflicting) + .value("PartiallyRedundant", ConstraintDiagnostic::Kind::PartiallyRedundant) + .value("Malformed", ConstraintDiagnostic::Kind::Malformed); + + py::enum_(m, "MotionKind") + .value("Rotational", MotionDef::Kind::Rotational) + .value("Translational", MotionDef::Kind::Translational) + .value("General", MotionDef::Kind::General); + + py::enum_(m, "LimitKind") + .value("TranslationMin", Constraint::Limit::Kind::TranslationMin) + .value("TranslationMax", Constraint::Limit::Kind::TranslationMax) + .value("RotationMin", Constraint::Limit::Kind::RotationMin) + .value("RotationMax", Constraint::Limit::Kind::RotationMax); + + // ── Struct bindings ──────────────────────────────────────────── + + py::class_(m, "Transform") + .def(py::init<>()) + .def_readwrite("position", &Transform::position) + .def_readwrite("quaternion", &Transform::quaternion) + .def_static("identity", &Transform::identity) + .def("__repr__", [](const Transform& t) { + return ""; + }) + .def("to_dict", [](const Transform& t) { return transform_to_dict(t); }) + .def_static("from_dict", [](const py::dict& d) { return transform_from_dict(d); }); + + py::class_(m, "Part") + .def(py::init<>()) + .def_readwrite("id", &Part::id) + .def_readwrite("placement", &Part::placement) + .def_readwrite("mass", &Part::mass) + .def_readwrite("grounded", &Part::grounded) + .def("to_dict", [](const Part& p) { return part_to_dict(p); }) + .def_static("from_dict", [](const py::dict& d) { return part_from_dict(d); }); + + auto constraint_class = py::class_(m, "Constraint"); + + py::class_(constraint_class, "Limit") + .def(py::init<>()) + .def_readwrite("kind", &Constraint::Limit::kind) + .def_readwrite("value", &Constraint::Limit::value) + .def_readwrite("tolerance", &Constraint::Limit::tolerance) + .def("to_dict", [](const Constraint::Limit& l) { return limit_to_dict(l); }) + .def_static("from_dict", [](const py::dict& d) { return limit_from_dict(d); }); + + constraint_class + .def(py::init<>()) + .def_readwrite("id", &Constraint::id) + .def_readwrite("part_i", &Constraint::part_i) + .def_readwrite("marker_i", &Constraint::marker_i) + .def_readwrite("part_j", &Constraint::part_j) + .def_readwrite("marker_j", &Constraint::marker_j) + .def_readwrite("type", &Constraint::type) + .def_readwrite("params", &Constraint::params) + .def_readwrite("limits", &Constraint::limits) + .def_readwrite("activated", &Constraint::activated) + .def("to_dict", [](const Constraint& c) { return constraint_to_dict(c); }) + .def_static("from_dict", [](const py::dict& d) { return constraint_from_dict(d); }); + + py::class_(m, "MotionDef") + .def(py::init<>()) + .def_readwrite("kind", &MotionDef::kind) + .def_readwrite("joint_id", &MotionDef::joint_id) + .def_readwrite("marker_i", &MotionDef::marker_i) + .def_readwrite("marker_j", &MotionDef::marker_j) + .def_readwrite("rotation_expr", &MotionDef::rotation_expr) + .def_readwrite("translation_expr", &MotionDef::translation_expr) + .def("to_dict", [](const MotionDef& m) { return motion_to_dict(m); }) + .def_static("from_dict", [](const py::dict& d) { return motion_from_dict(d); }); + + py::class_(m, "SimulationParams") + .def(py::init<>()) + .def_readwrite("t_start", &SimulationParams::t_start) + .def_readwrite("t_end", &SimulationParams::t_end) + .def_readwrite("h_out", &SimulationParams::h_out) + .def_readwrite("h_min", &SimulationParams::h_min) + .def_readwrite("h_max", &SimulationParams::h_max) + .def_readwrite("error_tol", &SimulationParams::error_tol) + .def("to_dict", [](const SimulationParams& s) { return sim_to_dict(s); }) + .def_static("from_dict", [](const py::dict& d) { return sim_from_dict(d); }); + + py::class_(m, "SolveContext") + .def(py::init<>()) + .def_readwrite("parts", &SolveContext::parts) + .def_readwrite("constraints", &SolveContext::constraints) + .def_readwrite("motions", &SolveContext::motions) + .def_readwrite("simulation", &SolveContext::simulation) + .def_readwrite("bundle_fixed", &SolveContext::bundle_fixed) + .def("to_dict", [](const SolveContext& ctx) { return solve_context_to_dict(ctx); }) + .def_static("from_dict", [](const py::dict& d) { return solve_context_from_dict(d); }); + + py::class_(m, "ConstraintDiagnostic") + .def(py::init<>()) + .def_readwrite("constraint_id", &ConstraintDiagnostic::constraint_id) + .def_readwrite("kind", &ConstraintDiagnostic::kind) + .def_readwrite("detail", &ConstraintDiagnostic::detail) + .def("to_dict", [](const ConstraintDiagnostic& d) { return diagnostic_to_dict(d); }) + .def_static("from_dict", [](const py::dict& d) { return diagnostic_from_dict(d); }); + + auto result_class = py::class_(m, "SolveResult"); + + py::class_(result_class, "PartResult") + .def(py::init<>()) + .def_readwrite("id", &SolveResult::PartResult::id) + .def_readwrite("placement", &SolveResult::PartResult::placement) + .def("to_dict", [](const SolveResult::PartResult& pr) { return part_result_to_dict(pr); }) + .def_static("from_dict", [](const py::dict& d) { return part_result_from_dict(d); }); + + result_class + .def(py::init<>()) + .def_readwrite("status", &SolveResult::status) + .def_readwrite("placements", &SolveResult::placements) + .def_readwrite("dof", &SolveResult::dof) + .def_readwrite("diagnostics", &SolveResult::diagnostics) + .def_readwrite("num_frames", &SolveResult::num_frames) + .def("to_dict", [](const SolveResult& r) { return solve_result_to_dict(r); }) + .def_static("from_dict", [](const py::dict& d) { return solve_result_from_dict(d); }); + + // ── IKCSolver (with trampoline for Python subclassing) ───────── + + py::class_(m, "IKCSolver") + .def(py::init<>()) + .def("name", &IKCSolver::name) + .def("supported_joints", &IKCSolver::supported_joints) + .def("solve", &IKCSolver::solve, py::arg("ctx")) + .def("update", &IKCSolver::update, py::arg("ctx")) + .def("pre_drag", &IKCSolver::pre_drag, + py::arg("ctx"), py::arg("drag_parts")) + .def("drag_step", &IKCSolver::drag_step, + py::arg("drag_placements")) + .def("post_drag", &IKCSolver::post_drag) + .def("run_kinematic", &IKCSolver::run_kinematic, py::arg("ctx")) + .def("num_frames", &IKCSolver::num_frames) + .def("update_for_frame", &IKCSolver::update_for_frame, + py::arg("index")) + .def("diagnose", &IKCSolver::diagnose, py::arg("ctx")) + .def("is_deterministic", &IKCSolver::is_deterministic) + .def("export_native", &IKCSolver::export_native, py::arg("path")) + .def("supports_bundle_fixed", &IKCSolver::supports_bundle_fixed); + + // ── OndselAdapter ────────────────────────────────────────────── + + py::class_(m, "OndselAdapter") + .def(py::init<>()); + + // ── Module-level functions (SolverRegistry wrapper) ──────────── + + m.def("available", []() { + return SolverRegistry::instance().available(); + }, "Return names of all registered solvers."); + + m.def("load", [](const std::string& name) { + return SolverRegistry::instance().get(name); + }, py::arg("name") = "", + "Create an instance of the named solver (default if empty).\n" + "Returns None if the solver is not found."); + + m.def("joints_for", [](const std::string& name) { + return SolverRegistry::instance().joints_for(name); + }, py::arg("name"), + "Query supported joint types for the named solver."); + + m.def("set_default", [](const std::string& name) { + return SolverRegistry::instance().set_default(name); + }, py::arg("name"), + "Set the default solver name. Returns True if the name is registered."); + + m.def("get_default", []() { + return SolverRegistry::instance().get_default(); + }, "Get the current default solver name."); + + m.def("register_solver", [](const std::string& name, py::object py_solver_class) { + auto cls = std::make_shared(std::move(py_solver_class)); + CreateSolverFn factory = [cls]() -> std::unique_ptr { + py::gil_scoped_acquire gil; + py::object instance = (*cls)(); + return std::make_unique(std::move(instance)); + }; + return SolverRegistry::instance().register_solver(name, std::move(factory)); + }, py::arg("name"), py::arg("solver_class"), + "Register a Python solver class with the SolverRegistry.\n" + "solver_class must be a callable that returns an IKCSolver subclass."); +} diff --git a/src/Mod/Assembly/TestAssemblyWorkbench.py b/src/Mod/Assembly/TestAssemblyWorkbench.py index 911a298a6a..87d83297fc 100644 --- a/src/Mod/Assembly/TestAssemblyWorkbench.py +++ b/src/Mod/Assembly/TestAssemblyWorkbench.py @@ -22,11 +22,17 @@ # **************************************************************************/ import TestApp - -from AssemblyTests.TestCore import TestCore from AssemblyTests.TestCommandInsertLink import TestCommandInsertLink - +from AssemblyTests.TestCore import TestCore +from AssemblyTests.TestKCSolvePy import ( + TestKCSolveImport, # noqa: F401 + TestKCSolveRegistry, # noqa: F401 + TestKCSolveTypes, # noqa: F401 + TestPySolver, # noqa: F401 +) +from AssemblyTests.TestSolverIntegration import TestSolverIntegration # Use the modules so that code checkers don't complain (flake8) True if TestCore else False True if TestCommandInsertLink else False +True if TestSolverIntegration else False diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index da69d6f09c..6c9e3a42ea 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -95,6 +95,7 @@ if(BUILD_GUI) endif() if(BUILD_ASSEMBLY) list (APPEND TestExecutables Assembly_tests_run) + list (APPEND TestExecutables KCSolve_tests_run) endif(BUILD_ASSEMBLY) if(BUILD_MATERIAL) list (APPEND TestExecutables Material_tests_run) diff --git a/tests/src/Mod/Assembly/CMakeLists.txt b/tests/src/Mod/Assembly/CMakeLists.txt index 1d3bb763b3..787d743e2d 100644 --- a/tests/src/Mod/Assembly/CMakeLists.txt +++ b/tests/src/Mod/Assembly/CMakeLists.txt @@ -1,6 +1,7 @@ # SPDX-License-Identifier: LGPL-2.1-or-later add_subdirectory(App) +add_subdirectory(Solver) if (NOT FREECAD_USE_EXTERNAL_ONDSELSOLVER) target_include_directories(Assembly_tests_run PUBLIC diff --git a/tests/src/Mod/Assembly/Solver/CMakeLists.txt b/tests/src/Mod/Assembly/Solver/CMakeLists.txt new file mode 100644 index 0000000000..7160170070 --- /dev/null +++ b/tests/src/Mod/Assembly/Solver/CMakeLists.txt @@ -0,0 +1,13 @@ +# SPDX-License-Identifier: LGPL-2.1-or-later + +add_executable(KCSolve_tests_run + SolverRegistry.cpp + OndselAdapter.cpp +) + +target_link_libraries(KCSolve_tests_run + gtest_main + ${Google_Tests_LIBS} + KCSolve + FreeCADApp +) diff --git a/tests/src/Mod/Assembly/Solver/OndselAdapter.cpp b/tests/src/Mod/Assembly/Solver/OndselAdapter.cpp new file mode 100644 index 0000000000..be475bd808 --- /dev/null +++ b/tests/src/Mod/Assembly/Solver/OndselAdapter.cpp @@ -0,0 +1,251 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later + +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +using namespace KCSolve; + +// ── Fixture ──────────────────────────────────────────────────────── + +class OndselAdapterTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + tests::initApplication(); + } + + void SetUp() override + { + adapter_ = std::make_unique(); + } + + /// Build a minimal two-part context with a single constraint. + static SolveContext twoPartContext(BaseJointKind jointKind, + bool groundFirst = true) + { + SolveContext ctx; + + Part p1; + p1.id = "Part1"; + p1.placement = Transform::identity(); + p1.grounded = groundFirst; + ctx.parts.push_back(p1); + + Part p2; + p2.id = "Part2"; + p2.placement = Transform::identity(); + p2.placement.position = {100.0, 0.0, 0.0}; + p2.grounded = false; + ctx.parts.push_back(p2); + + Constraint c; + c.id = "Joint1"; + c.part_i = "Part1"; + c.marker_i = Transform::identity(); + c.part_j = "Part2"; + c.marker_j = Transform::identity(); + c.type = jointKind; + ctx.constraints.push_back(c); + + return ctx; + } + + std::unique_ptr adapter_; +}; + + +// ── Identity / capability tests ──────────────────────────────────── + +TEST_F(OndselAdapterTest, Name) // NOLINT +{ + auto n = adapter_->name(); + EXPECT_FALSE(n.empty()); + EXPECT_NE(n.find("Ondsel"), std::string::npos); +} + +TEST_F(OndselAdapterTest, SupportedJoints) // NOLINT +{ + auto joints = adapter_->supported_joints(); + EXPECT_FALSE(joints.empty()); + + // Must include core kinematic joints. + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Fixed), joints.end()); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Revolute), joints.end()); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Cylindrical), joints.end()); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Ball), joints.end()); + + // Must exclude unsupported types. + EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Universal), joints.end()); + EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Cam), joints.end()); + EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Slot), joints.end()); +} + +TEST_F(OndselAdapterTest, IsDeterministic) // NOLINT +{ + EXPECT_TRUE(adapter_->is_deterministic()); +} + +TEST_F(OndselAdapterTest, SupportsBundleFixed) // NOLINT +{ + EXPECT_FALSE(adapter_->supports_bundle_fixed()); +} + + +// ── Solve round-trips ────────────────────────────────────────────── + +TEST_F(OndselAdapterTest, SolveFixedJoint) // NOLINT +{ + auto ctx = twoPartContext(BaseJointKind::Fixed); + auto result = adapter_->solve(ctx); + + EXPECT_EQ(result.status, SolveStatus::Success); + EXPECT_FALSE(result.placements.empty()); + + // Both parts should end up at the same position (fixed joint). + const auto* pr1 = &result.placements[0]; + const auto* pr2 = &result.placements[1]; + if (pr1->id == "Part2") { + std::swap(pr1, pr2); + } + + // Part1 is grounded — should remain at origin. + EXPECT_NEAR(pr1->placement.position[0], 0.0, 1e-3); + EXPECT_NEAR(pr1->placement.position[1], 0.0, 1e-3); + EXPECT_NEAR(pr1->placement.position[2], 0.0, 1e-3); + + // Part2 should be pulled to Part1's position by the fixed joint + // (markers are both identity, so the parts are welded at the same point). + EXPECT_NEAR(pr2->placement.position[0], 0.0, 1e-3); + EXPECT_NEAR(pr2->placement.position[1], 0.0, 1e-3); + EXPECT_NEAR(pr2->placement.position[2], 0.0, 1e-3); +} + +TEST_F(OndselAdapterTest, SolveRevoluteJoint) // NOLINT +{ + auto ctx = twoPartContext(BaseJointKind::Revolute); + auto result = adapter_->solve(ctx); + + EXPECT_EQ(result.status, SolveStatus::Success); + EXPECT_FALSE(result.placements.empty()); +} + +TEST_F(OndselAdapterTest, SolveNoGroundedParts) // NOLINT +{ + // OndselAdapter itself doesn't require grounded parts — that check + // lives in AssemblyObject. The solver should still attempt to solve. + auto ctx = twoPartContext(BaseJointKind::Fixed, /*groundFirst=*/false); + auto result = adapter_->solve(ctx); + + // May succeed or fail depending on OndselSolver's behavior, but must not crash. + EXPECT_TRUE(result.status == SolveStatus::Success + || result.status == SolveStatus::Failed); +} + +TEST_F(OndselAdapterTest, SolveCatchesException) // NOLINT +{ + // Malformed context: constraint references non-existent parts. + SolveContext ctx; + + Part p; + p.id = "LonePart"; + p.placement = Transform::identity(); + p.grounded = true; + ctx.parts.push_back(p); + + Constraint c; + c.id = "BadJoint"; + c.part_i = "DoesNotExist"; + c.marker_i = Transform::identity(); + c.part_j = "AlsoDoesNotExist"; + c.marker_j = Transform::identity(); + c.type = BaseJointKind::Fixed; + ctx.constraints.push_back(c); + + // Should not crash — returns Failed or succeeds with warnings. + auto result = adapter_->solve(ctx); + SUCCEED(); // If we get here without crashing, the test passes. +} + + +// ── Drag protocol ────────────────────────────────────────────────── + +TEST_F(OndselAdapterTest, DragProtocol) // NOLINT +{ + auto ctx = twoPartContext(BaseJointKind::Revolute); + + auto preResult = adapter_->pre_drag(ctx, {"Part2"}); + EXPECT_EQ(preResult.status, SolveStatus::Success); + + // Move Part2 slightly. + SolveResult::PartResult dragPlc; + dragPlc.id = "Part2"; + dragPlc.placement = Transform::identity(); + dragPlc.placement.position = {10.0, 5.0, 0.0}; + + auto stepResult = adapter_->drag_step({dragPlc}); + // drag_step may fail if the solver can't converge — that's OK. + EXPECT_TRUE(stepResult.status == SolveStatus::Success + || stepResult.status == SolveStatus::Failed); + + // post_drag must not crash. + adapter_->post_drag(); + SUCCEED(); +} + + +// ── Diagnostics ──────────────────────────────────────────────────── + +TEST_F(OndselAdapterTest, DiagnoseRedundant) // NOLINT +{ + // Over-constrained: two fixed joints between the same two parts. + SolveContext ctx; + + Part p1; + p1.id = "PartA"; + p1.placement = Transform::identity(); + p1.grounded = true; + ctx.parts.push_back(p1); + + Part p2; + p2.id = "PartB"; + p2.placement = Transform::identity(); + p2.placement.position = {50.0, 0.0, 0.0}; + p2.grounded = false; + ctx.parts.push_back(p2); + + Constraint c1; + c1.id = "FixedJoint1"; + c1.part_i = "PartA"; + c1.marker_i = Transform::identity(); + c1.part_j = "PartB"; + c1.marker_j = Transform::identity(); + c1.type = BaseJointKind::Fixed; + ctx.constraints.push_back(c1); + + Constraint c2; + c2.id = "FixedJoint2"; + c2.part_i = "PartA"; + c2.marker_i = Transform::identity(); + c2.part_j = "PartB"; + c2.marker_j = Transform::identity(); + c2.type = BaseJointKind::Fixed; + ctx.constraints.push_back(c2); + + auto diags = adapter_->diagnose(ctx); + // With two identical fixed joints, one must be redundant. + bool hasRedundant = std::any_of(diags.begin(), diags.end(), [](const auto& d) { + return d.kind == ConstraintDiagnostic::Kind::Redundant; + }); + EXPECT_TRUE(hasRedundant); +} diff --git a/tests/src/Mod/Assembly/Solver/SolverRegistry.cpp b/tests/src/Mod/Assembly/Solver/SolverRegistry.cpp new file mode 100644 index 0000000000..5c29027620 --- /dev/null +++ b/tests/src/Mod/Assembly/Solver/SolverRegistry.cpp @@ -0,0 +1,131 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later + +#include + +#include +#include +#include + +#include + +using namespace KCSolve; + +// ── Minimal mock solver for registry tests ───────────────────────── + +namespace +{ + +class MockSolver : public IKCSolver +{ +public: + std::string name() const override + { + return "MockSolver"; + } + + std::vector supported_joints() const override + { + return {BaseJointKind::Fixed, BaseJointKind::Revolute}; + } + + SolveResult solve(const SolveContext& /*ctx*/) override + { + return SolveResult {SolveStatus::Success, {}, 0, {}, 0}; + } +}; + +} // namespace + + +// ── Tests ────────────────────────────────────────────────────────── +// +// SolverRegistry is a singleton — tests use unique names to avoid +// interference across test cases. + +TEST(SolverRegistryTest, GetUnknownReturnsNull) // NOLINT +{ + auto solver = SolverRegistry::instance().get("nonexistent_solver_xyz"); + EXPECT_EQ(solver, nullptr); +} + +TEST(SolverRegistryTest, RegisterAndGet) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + bool ok = reg.register_solver("test_reg_get", + []() { return std::make_unique(); }); + EXPECT_TRUE(ok); + + auto solver = reg.get("test_reg_get"); + ASSERT_NE(solver, nullptr); + EXPECT_EQ(solver->name(), "MockSolver"); +} + +TEST(SolverRegistryTest, DuplicateRegistrationFails) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + bool first = reg.register_solver("test_dup", + []() { return std::make_unique(); }); + EXPECT_TRUE(first); + + bool second = reg.register_solver("test_dup", + []() { return std::make_unique(); }); + EXPECT_FALSE(second); +} + +TEST(SolverRegistryTest, AvailableListsSolvers) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + reg.register_solver("test_avail_1", + []() { return std::make_unique(); }); + reg.register_solver("test_avail_2", + []() { return std::make_unique(); }); + + auto names = reg.available(); + EXPECT_NE(std::find(names.begin(), names.end(), "test_avail_1"), names.end()); + EXPECT_NE(std::find(names.begin(), names.end(), "test_avail_2"), names.end()); +} + +TEST(SolverRegistryTest, SetDefaultAndGet) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + reg.register_solver("test_default", + []() { return std::make_unique(); }); + + bool ok = reg.set_default("test_default"); + EXPECT_TRUE(ok); + + // get() with no arg should return the default. + auto solver = reg.get(); + ASSERT_NE(solver, nullptr); + EXPECT_EQ(solver->name(), "MockSolver"); +} + +TEST(SolverRegistryTest, SetDefaultUnknownFails) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + bool ok = reg.set_default("totally_unknown_solver"); + EXPECT_FALSE(ok); +} + +TEST(SolverRegistryTest, JointsForReturnsCapabilities) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + reg.register_solver("test_joints", + []() { return std::make_unique(); }); + + auto joints = reg.joints_for("test_joints"); + EXPECT_EQ(joints.size(), 2u); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Fixed), joints.end()); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Revolute), joints.end()); +} + +TEST(SolverRegistryTest, JointsForUnknownReturnsEmpty) // NOLINT +{ + auto joints = SolverRegistry::instance().joints_for("totally_unknown_solver_2"); + EXPECT_TRUE(joints.empty()); +}