Merge branch 'main' into docs/solver-spec-update
All checks were successful
Build and Test / build (pull_request) Successful in 30m48s

This commit is contained in:
2026-02-20 18:03:53 +00:00
26 changed files with 5160 additions and 1119 deletions

2
.gitmodules vendored
View File

@@ -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

View File

@@ -26,6 +26,8 @@
#include <Base/Interpreter.h>
#include <Base/PyObjectBase.h>
#include <Mod/Assembly/Solver/OndselAdapter.h>
#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");

File diff suppressed because it is too large Load Diff

View File

@@ -25,24 +25,21 @@
#ifndef ASSEMBLY_AssemblyObject_H
#define ASSEMBLY_AssemblyObject_H
#include <memory>
#include <boost/signals2.hpp>
#include <Mod/Assembly/AssemblyGlobal.h>
#include <Mod/Assembly/Solver/Types.h>
#include <App/FeaturePython.h>
#include <App/Part.h>
#include <App/PropertyLinks.h>
#include <OndselSolver/enum.h>
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<MbD::ASMTPart> mbdPart);
bool validateNewPlacements();
void setNewPlacements();
static void redrawJointPlacements(std::vector<App::DocumentObject*> 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<MbD::ASMTAssembly> makeMbdAssembly();
void create_mbdSimulationParameters(App::DocumentObject* sim);
std::shared_ptr<MbD::ASMTPart> makeMbdPart(
std::string& name,
Base::Placement plc = Base::Placement(),
double mass = 1.0
);
std::shared_ptr<MbD::ASMTPart> 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<MbD::ASMTPart> part;
Base::Placement offsetPlc; // This is the offset within the bundled parts
};
MbDPartData getMbDData(App::DocumentObject* part);
std::shared_ptr<MbD::ASMTMarker> makeMbdMarker(std::string& name, Base::Placement& plc);
std::vector<std::shared_ptr<MbD::ASMTJoint>> makeMbdJoint(App::DocumentObject* joint);
std::shared_ptr<MbD::ASMTJoint> makeMbdJointOfType(App::DocumentObject* joint, JointType jointType);
std::shared_ptr<MbD::ASMTJoint> 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<App::DocumentObject*> joints);
JointGroup* getJointGroup() const;
ViewGroup* getExplodedViewGroup() const;
template<typename T>
@@ -169,8 +131,6 @@ public:
const std::vector<App::DocumentObject*>& excludeJoints = {}
);
std::unordered_set<App::DocumentObject*> getGroundedParts();
std::unordered_set<App::DocumentObject*> 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<App::DocumentObject*> 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<void()> signalSolverUpdate;
private:
std::shared_ptr<MbD::ASMTAssembly> mbdAssembly;
// ── Solver integration ─────────────────────────────────────────
KCSolve::IKCSolver* getOrCreateSolver();
KCSolve::SolveContext buildSolveContext(
const std::vector<App::DocumentObject*>& 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<std::string, std::vector<PartMapping>> partIdToObjs_;
std::unordered_map<App::DocumentObject*, std::string> 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<KCSolve::IKCSolver> solver_;
KCSolve::SolveResult lastResult_;
// ── Existing state (unchanged) ─────────────────────────────────
std::unordered_map<App::DocumentObject*, MbDPartData> objectPartMap;
std::vector<std::pair<App::DocumentObject*, double>> objMasses;
std::vector<App::DocumentObject*> draggedParts;
std::vector<App::DocumentObject*> motions;
std::vector<std::pair<App::DocumentObject*, Base::Placement>> previousPositions;

View File

@@ -5,7 +5,7 @@ set(Assembly_LIBS
PartDesign
Spreadsheet
FreeCADApp
OndselSolver
KCSolve
)
generate_from_py(AssemblyObject)

View File

@@ -0,0 +1,520 @@
# SPDX-License-Identifier: LGPL-2.1-or-later
# ***************************************************************************
# * *
# * Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
# * *
# * 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 *
# * <https://www.gnu.org/licenses/>. *
# * *
# ***************************************************************************
"""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)

View File

@@ -0,0 +1,216 @@
# SPDX-License-Identifier: LGPL-2.1-or-later
# /****************************************************************************
# *
# Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
# *
# 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 *
# <https://www.gnu.org/licenses/>. *
# *
# ***************************************************************************/
"""
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",
)

View File

@@ -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
)

View File

@@ -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()

View File

@@ -0,0 +1,189 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#ifndef KCSOLVE_IKCSOLVER_H
#define KCSOLVE_IKCSOLVER_H
#include <cstddef>
#include <string>
#include <vector>
#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<BaseJointKind> 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<std::string>& /*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<SolveResult::PartResult>& /*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<ConstraintDiagnostic> 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

View File

@@ -0,0 +1,37 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#include <FCGlobal.h>
#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

View File

@@ -0,0 +1,796 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#include "OndselAdapter.h"
#include "SolverRegistry.h"
#include <Base/Console.h>
#include <OndselSolver/CREATE.h>
#include <OndselSolver/ASMTAssembly.h>
#include <OndselSolver/ASMTAngleJoint.h>
#include <OndselSolver/ASMTConstantGravity.h>
#include <OndselSolver/ASMTCylSphJoint.h>
#include <OndselSolver/ASMTCylindricalJoint.h>
#include <OndselSolver/ASMTFixedJoint.h>
#include <OndselSolver/ASMTGearJoint.h>
#include <OndselSolver/ASMTGeneralMotion.h>
#include <OndselSolver/ASMTLineInPlaneJoint.h>
#include <OndselSolver/ASMTMarker.h>
#include <OndselSolver/ASMTParallelAxesJoint.h>
#include <OndselSolver/ASMTPart.h>
#include <OndselSolver/ASMTPerpendicularJoint.h>
#include <OndselSolver/ASMTPlanarJoint.h>
#include <OndselSolver/ASMTPointInPlaneJoint.h>
#include <OndselSolver/ASMTRackPinionJoint.h>
#include <OndselSolver/ASMTRevCylJoint.h>
#include <OndselSolver/ASMTRevoluteJoint.h>
#include <OndselSolver/ASMTRotationLimit.h>
#include <OndselSolver/ASMTRotationalMotion.h>
#include <OndselSolver/ASMTScrewJoint.h>
#include <OndselSolver/ASMTSimulationParameters.h>
#include <OndselSolver/ASMTSphSphJoint.h>
#include <OndselSolver/ASMTSphericalJoint.h>
#include <OndselSolver/ASMTTranslationLimit.h>
#include <OndselSolver/ASMTTranslationalJoint.h>
#include <OndselSolver/ASMTTranslationalMotion.h>
#include <OndselSolver/ExternalSystem.h>
// For System::jointsMotionsDo and diagnostic iteration
#include <OndselSolver/Constraint.h>
#include <OndselSolver/Joint.h>
#include <OndselSolver/System.h>
using namespace MbD;
namespace KCSolve
{
// ── Static registration ────────────────────────────────────────────
void OndselAdapter::register_solver()
{
SolverRegistry::instance().register_solver(
"ondsel",
[]() { return std::make_unique<OndselAdapter>(); });
}
// ── 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<BaseJointKind> 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<double, 4>& 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<ASMTPart> OndselAdapter::make_part(const Part& part)
{
auto mbdPart = CREATE<ASMTPart>::With();
mbdPart->setName(part.id);
auto massMarker = CREATE<ASMTPrincipalMassMarker>::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<ASMTMarker> OndselAdapter::make_marker(const std::string& markerName,
const Transform& tf)
{
auto mbdMarker = CREATE<ASMTMarker>::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<ASMTJoint> 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<ASMTSphericalJoint>::With();
case BaseJointKind::PointOnLine: {
auto j = CREATE<ASMTCylSphJoint>::With();
j->distanceIJ = param(0);
return j;
}
case BaseJointKind::PointInPlane: {
auto j = CREATE<ASMTPointInPlaneJoint>::With();
j->offset = param(0);
return j;
}
case BaseJointKind::Concentric: {
auto j = CREATE<ASMTRevCylJoint>::With();
j->distanceIJ = param(0);
return j;
}
case BaseJointKind::Tangent: {
auto j = CREATE<ASMTPlanarJoint>::With();
j->offset = param(0);
return j;
}
case BaseJointKind::Planar: {
auto j = CREATE<ASMTPlanarJoint>::With();
j->offset = param(0);
return j;
}
case BaseJointKind::LineInPlane: {
auto j = CREATE<ASMTLineInPlaneJoint>::With();
j->offset = param(0);
return j;
}
case BaseJointKind::Parallel:
return CREATE<ASMTParallelAxesJoint>::With();
case BaseJointKind::Perpendicular:
return CREATE<ASMTPerpendicularJoint>::With();
case BaseJointKind::Angle: {
auto j = CREATE<ASMTAngleJoint>::With();
j->theIzJz = param(0);
return j;
}
case BaseJointKind::Fixed:
return CREATE<ASMTFixedJoint>::With();
case BaseJointKind::Revolute:
return CREATE<ASMTRevoluteJoint>::With();
case BaseJointKind::Cylindrical:
return CREATE<ASMTCylindricalJoint>::With();
case BaseJointKind::Slider:
return CREATE<ASMTTranslationalJoint>::With();
case BaseJointKind::Ball:
return CREATE<ASMTSphericalJoint>::With();
case BaseJointKind::Screw: {
auto j = CREATE<ASMTScrewJoint>::With();
j->pitch = param(0);
return j;
}
case BaseJointKind::Gear: {
auto j = CREATE<ASMTGearJoint>::With();
j->radiusI = param(0);
j->radiusJ = param(1);
return j;
}
case BaseJointKind::RackPinion: {
auto j = CREATE<ASMTRackPinionJoint>::With();
j->pitchRadius = param(0);
return j;
}
case BaseJointKind::DistancePointPoint: {
auto j = CREATE<ASMTSphSphJoint>::With();
j->distanceIJ = param(0);
return j;
}
case BaseJointKind::DistanceCylSph: {
auto j = CREATE<ASMTCylSphJoint>::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<int>(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<ASMTTranslationLimit>::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<ASMTTranslationLimit>::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<ASMTRotationLimit>::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<ASMTRotationLimit>::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<const MotionDef*> 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<ASMTGeneralMotion>::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<ASMTRotationalMotion>::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<ASMTTranslationalMotion>::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<ASMTGeneralMotion>::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<ASMTFixedJoint>::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<ASMTAssembly>::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<ASMTPart>& 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<ConstraintDiagnostic> OndselAdapter::extract_diagnostics() const
{
std::vector<ConstraintDiagnostic> diags;
if (!assembly_ || !assembly_->mbdSystem) {
return diags;
}
assembly_->mbdSystem->jointsMotionsDo([&](std::shared_ptr<Joint> jm) {
if (!jm) {
return;
}
bool isRedundant = false;
jm->constraintsDo([&](std::shared_ptr<MbD::Constraint> 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<std::string>& 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<SolveResult::PartResult>& drag_placements)
{
if (!assembly_) {
return SolveResult {SolveStatus::Failed, {}, -1, {}, 0};
}
try {
auto dragParts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
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<ConstraintDiagnostic> 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

View File

@@ -0,0 +1,129 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#ifndef KCSOLVE_ONDSELADAPTER_H
#define KCSOLVE_ONDSELADAPTER_H
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#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<BaseJointKind> 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<std::string>& drag_parts) override;
SolveResult drag_step(
const std::vector<SolveResult::PartResult>& 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<ConstraintDiagnostic> 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<MbD::ASMTPart> make_part(const Part& part);
std::shared_ptr<MbD::ASMTMarker> make_marker(const std::string& name,
const Transform& tf);
std::shared_ptr<MbD::ASMTJoint> 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<ConstraintDiagnostic> extract_diagnostics() const;
Transform extract_part_transform(
const std::shared_ptr<MbD::ASMTPart>& 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<double, 4>& q,
double (&mat)[3][3]);
// ── Internal state ─────────────────────────────────────────────
std::shared_ptr<MbD::ASMTAssembly> assembly_;
std::unordered_map<std::string, std::shared_ptr<MbD::ASMTPart>> part_map_;
std::vector<std::string> drag_part_ids_;
};
} // namespace KCSolve
#endif // KCSOLVE_ONDSELADAPTER_H

View File

@@ -0,0 +1,346 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#include "SolverRegistry.h"
#include <Base/Console.h>
#include <cstdlib>
#include <cstring>
#include <filesystem>
#include <sstream>
#ifdef _WIN32
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
#else
# include <dlfcn.h>
#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<void*>(LoadLibraryA(path));
#else
return dlopen(path, RTLD_LAZY);
#endif
}
void* get_symbol(void* handle, const char* symbol)
{
#ifdef _WIN32
return reinterpret_cast<void*>(
GetProcAddress(static_cast<HMODULE>(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<char*>(&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<int>(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<HMODULE>(handle));
#else
dlclose(handle);
#endif
}
// ── Registration ───────────────────────────────────────────────────
bool SolverRegistry::register_solver(const std::string& name, CreateSolverFn factory)
{
std::lock_guard<std::mutex> 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<IKCSolver> SolverRegistry::get(const std::string& name) const
{
std::lock_guard<std::mutex> 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<std::string> SolverRegistry::available() const
{
std::lock_guard<std::mutex> lock(mutex_);
std::vector<std::string> names;
names.reserve(factories_.size());
for (const auto& [name, _] : factories_) {
names.push_back(name);
}
return names;
}
std::vector<BaseJointKind> 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<std::mutex> lock(mutex_);
if (factories_.find(name) == factories_.end()) {
return false;
}
default_name_ = name;
return true;
}
std::string SolverRegistry::get_default() const
{
std::lock_guard<std::mutex> 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<ApiVersionFn>(
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<CreateFn>(
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<IKCSolver> 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<IKCSolver> {
return std::unique_ptr<IKCSolver>(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: <install_prefix>/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

View File

@@ -0,0 +1,124 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#ifndef KCSOLVE_SOLVERREGISTRY_H
#define KCSOLVE_SOLVERREGISTRY_H
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <unordered_map>
#include <vector>
#include "IKCSolver.h"
#include "KCSolveGlobal.h"
namespace KCSolve
{
/// Factory function that creates a solver instance.
using CreateSolverFn = std::function<std::unique_ptr<IKCSolver>()>;
/// 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<OndselAdapter>(); });
///
/// // 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<IKCSolver> get(const std::string& name = {}) const;
/// Return the names of all registered solvers.
std::vector<std::string> available() const;
/// Query which BaseJointKind values a named solver supports.
/// Creates a temporary instance to call supported_joints().
std::vector<BaseJointKind> 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. <install_prefix>/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<std::string, CreateSolverFn> factories_;
std::string default_name_;
std::vector<void*> handles_; // loaded plugin library handles
};
} // namespace KCSolve
#endif // KCSOLVE_SOLVERREGISTRY_H

View File

@@ -0,0 +1,286 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#ifndef KCSOLVE_TYPES_H
#define KCSOLVE_TYPES_H
#include <array>
#include <cstddef>
#include <cstdint>
#include <optional>
#include <string>
#include <vector>
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<double, 3> position {0.0, 0.0, 0.0};
std::array<double, 4> 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<double> 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<Limit> 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<Part> parts;
std::vector<Constraint> constraints;
std::vector<MotionDef> motions;
// Present when running kinematic simulation via run_kinematic().
std::optional<SimulationParams> 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<PartResult> placements;
// Degrees of freedom remaining (-1 = unknown).
int dof {-1};
// Constraint diagnostics (redundant, conflicting, etc.).
std::vector<ConstraintDiagnostic> diagnostics;
// For kinematic simulation: number of computed frames.
std::size_t num_frames {0};
};
} // namespace KCSolve
#endif // KCSOLVE_TYPES_H

View File

@@ -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})

View File

@@ -0,0 +1,121 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#ifndef KCSOLVE_PYIKCSOLVER_H
#define KCSOLVE_PYIKCSOLVER_H
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <Mod/Assembly/Solver/IKCSolver.h>
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<BaseJointKind> supported_joints() const override
{
PYBIND11_OVERRIDE_PURE(std::vector<BaseJointKind>, 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<std::string>& drag_parts) override
{
PYBIND11_OVERRIDE(SolveResult, IKCSolver, pre_drag, ctx, drag_parts);
}
SolveResult drag_step(
const std::vector<SolveResult::PartResult>& 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<ConstraintDiagnostic> diagnose(const SolveContext& ctx) override
{
PYBIND11_OVERRIDE(std::vector<ConstraintDiagnostic>, 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

View File

@@ -0,0 +1,830 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
/****************************************************************************
* *
* Copyright (c) 2025 Kindred Systems <development@kindred-systems.com> *
* *
* 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 *
* <https://www.gnu.org/licenses/>. *
* *
***************************************************************************/
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <Mod/Assembly/Solver/IKCSolver.h>
#include <Mod/Assembly/Solver/OndselAdapter.h>
#include <Mod/Assembly/Solver/SolverRegistry.h>
#include <Mod/Assembly/Solver/Types.h>
#include "PyIKCSolver.h"
#include <cstddef>
#include <memory>
#include <string>
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<typename E>
struct EnumEntry
{
E value;
const char* name;
};
static constexpr EnumEntry<BaseJointKind> 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<SolveStatus> kSolveStatusEntries[] = {
{SolveStatus::Success, "Success"},
{SolveStatus::Failed, "Failed"},
{SolveStatus::InvalidFlip, "InvalidFlip"},
{SolveStatus::NoGroundedParts, "NoGroundedParts"},
};
static constexpr EnumEntry<ConstraintDiagnostic::Kind> kDiagnosticKindEntries[] = {
{ConstraintDiagnostic::Kind::Redundant, "Redundant"},
{ConstraintDiagnostic::Kind::Conflicting, "Conflicting"},
{ConstraintDiagnostic::Kind::PartiallyRedundant, "PartiallyRedundant"},
{ConstraintDiagnostic::Kind::Malformed, "Malformed"},
};
static constexpr EnumEntry<MotionDef::Kind> kMotionKindEntries[] = {
{MotionDef::Kind::Rotational, "Rotational"},
{MotionDef::Kind::Translational, "Translational"},
{MotionDef::Kind::General, "General"},
};
static constexpr EnumEntry<Constraint::Limit::Kind> kLimitKindEntries[] = {
{Constraint::Limit::Kind::TranslationMin, "TranslationMin"},
{Constraint::Limit::Kind::TranslationMax, "TranslationMax"},
{Constraint::Limit::Kind::RotationMin, "RotationMin"},
{Constraint::Limit::Kind::RotationMax, "RotationMax"},
};
template<typename E, std::size_t N>
const char* enum_to_str(E val, const EnumEntry<E> (&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<int>(val)));
}
template<typename E, std::size_t N>
E str_to_enum(const std::string& name, const EnumEntry<E> (&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<py::sequence>();
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<std::size_t>(i)] = pos[i].cast<double>();
}
auto quat = d["quaternion"].cast<py::sequence>();
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<std::size_t>(i)] = quat[i].cast<double>();
}
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<std::string>();
p.placement = transform_from_dict(d["placement"].cast<py::dict>());
if (d.contains("mass")) {
p.mass = d["mass"].cast<double>();
}
if (d.contains("grounded")) {
p.grounded = d["grounded"].cast<bool>();
}
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<std::string>(),
kLimitKindEntries, "LimitKind");
lim.value = d["value"].cast<double>();
if (d.contains("tolerance")) {
lim.tolerance = d["tolerance"].cast<double>();
}
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<std::string>();
c.part_i = d["part_i"].cast<std::string>();
c.marker_i = transform_from_dict(d["marker_i"].cast<py::dict>());
c.part_j = d["part_j"].cast<std::string>();
c.marker_j = transform_from_dict(d["marker_j"].cast<py::dict>());
c.type = str_to_enum(d["type"].cast<std::string>(),
kBaseJointKindEntries, "BaseJointKind");
if (d.contains("params")) {
c.params = d["params"].cast<std::vector<double>>();
}
if (d.contains("limits")) {
for (auto item : d["limits"]) {
c.limits.push_back(limit_from_dict(item.cast<py::dict>()));
}
}
if (d.contains("activated")) {
c.activated = d["activated"].cast<bool>();
}
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<std::string>(),
kMotionKindEntries, "MotionKind");
m.joint_id = d["joint_id"].cast<std::string>();
if (d.contains("marker_i")) {
m.marker_i = d["marker_i"].cast<std::string>();
}
if (d.contains("marker_j")) {
m.marker_j = d["marker_j"].cast<std::string>();
}
if (d.contains("rotation_expr")) {
m.rotation_expr = d["rotation_expr"].cast<std::string>();
}
if (d.contains("translation_expr")) {
m.translation_expr = d["translation_expr"].cast<std::string>();
}
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<double>();
}
if (d.contains("t_end")) {
s.t_end = d["t_end"].cast<double>();
}
if (d.contains("h_out")) {
s.h_out = d["h_out"].cast<double>();
}
if (d.contains("h_min")) {
s.h_min = d["h_min"].cast<double>();
}
if (d.contains("h_max")) {
s.h_max = d["h_max"].cast<double>();
}
if (d.contains("error_tol")) {
s.error_tol = d["error_tol"].cast<double>();
}
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<std::string>();
diag.kind = str_to_enum(d["kind"].cast<std::string>(),
kDiagnosticKindEntries, "DiagnosticKind");
if (d.contains("detail")) {
diag.detail = d["detail"].cast<std::string>();
}
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<std::string>();
pr.placement = transform_from_dict(d["placement"].cast<py::dict>());
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<int>();
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<py::dict>()));
}
for (auto item : d["constraints"]) {
ctx.constraints.push_back(constraint_from_dict(item.cast<py::dict>()));
}
if (d.contains("motions")) {
for (auto item : d["motions"]) {
ctx.motions.push_back(motion_from_dict(item.cast<py::dict>()));
}
}
if (d.contains("simulation") && !d["simulation"].is_none()) {
ctx.simulation = sim_from_dict(d["simulation"].cast<py::dict>());
}
if (d.contains("bundle_fixed")) {
ctx.bundle_fixed = d["bundle_fixed"].cast<bool>();
}
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<std::string>(),
kSolveStatusEntries, "SolveStatus");
if (d.contains("placements")) {
for (auto item : d["placements"]) {
r.placements.push_back(part_result_from_dict(item.cast<py::dict>()));
}
}
if (d.contains("dof")) {
r.dof = d["dof"].cast<int>();
}
if (d.contains("diagnostics")) {
for (auto item : d["diagnostics"]) {
r.diagnostics.push_back(diagnostic_from_dict(item.cast<py::dict>()));
}
}
if (d.contains("num_frames")) {
r.num_frames = d["num_frames"].cast<std::size_t>();
}
return r;
}
} // anonymous namespace
// ── PySolverHolder ─────────────────────────────────────────────────
//
// Wraps a Python IKCSolver subclass instance so it can live inside a
// std::unique_ptr<IKCSolver> 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<IKCSolver*>();
}
std::string name() const override
{
py::gil_scoped_acquire gil;
return solver_->name();
}
std::vector<BaseJointKind> 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<std::string>& drag_parts) override
{
py::gil_scoped_acquire gil;
return solver_->pre_drag(ctx, drag_parts);
}
SolveResult drag_step(
const std::vector<SolveResult::PartResult>& 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<ConstraintDiagnostic> 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_<BaseJointKind>(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_<SolveStatus>(m, "SolveStatus")
.value("Success", SolveStatus::Success)
.value("Failed", SolveStatus::Failed)
.value("InvalidFlip", SolveStatus::InvalidFlip)
.value("NoGroundedParts", SolveStatus::NoGroundedParts);
py::enum_<ConstraintDiagnostic::Kind>(m, "DiagnosticKind")
.value("Redundant", ConstraintDiagnostic::Kind::Redundant)
.value("Conflicting", ConstraintDiagnostic::Kind::Conflicting)
.value("PartiallyRedundant", ConstraintDiagnostic::Kind::PartiallyRedundant)
.value("Malformed", ConstraintDiagnostic::Kind::Malformed);
py::enum_<MotionDef::Kind>(m, "MotionKind")
.value("Rotational", MotionDef::Kind::Rotational)
.value("Translational", MotionDef::Kind::Translational)
.value("General", MotionDef::Kind::General);
py::enum_<Constraint::Limit::Kind>(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_<Transform>(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 "<kcsolve.Transform pos=["
+ std::to_string(t.position[0]) + ", "
+ std::to_string(t.position[1]) + ", "
+ std::to_string(t.position[2]) + "]>";
})
.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_<Part>(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_<Constraint>(m, "Constraint");
py::class_<Constraint::Limit>(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_<MotionDef>(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_<SimulationParams>(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_<SolveContext>(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_<ConstraintDiagnostic>(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_<SolveResult>(m, "SolveResult");
py::class_<SolveResult::PartResult>(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_<IKCSolver, PyIKCSolver>(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_<OndselAdapter, IKCSolver>(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<py::object>(std::move(py_solver_class));
CreateSolverFn factory = [cls]() -> std::unique_ptr<IKCSolver> {
py::gil_scoped_acquire gil;
py::object instance = (*cls)();
return std::make_unique<PySolverHolder>(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.");
}

View File

@@ -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

View File

@@ -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)

View File

@@ -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

View File

@@ -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
)

View File

@@ -0,0 +1,251 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
#include <gtest/gtest.h>
#include <FCConfig.h>
#include <App/Application.h>
#include <Mod/Assembly/Solver/IKCSolver.h>
#include <Mod/Assembly/Solver/OndselAdapter.h>
#include <Mod/Assembly/Solver/Types.h>
#include <src/App/InitApplication.h>
#include <algorithm>
#include <cmath>
using namespace KCSolve;
// ── Fixture ────────────────────────────────────────────────────────
class OndselAdapterTest : public ::testing::Test
{
protected:
static void SetUpTestSuite()
{
tests::initApplication();
}
void SetUp() override
{
adapter_ = std::make_unique<OndselAdapter>();
}
/// 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<OndselAdapter> 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);
}

View File

@@ -0,0 +1,131 @@
// SPDX-License-Identifier: LGPL-2.1-or-later
#include <gtest/gtest.h>
#include <Mod/Assembly/Solver/IKCSolver.h>
#include <Mod/Assembly/Solver/SolverRegistry.h>
#include <Mod/Assembly/Solver/Types.h>
#include <algorithm>
using namespace KCSolve;
// ── Minimal mock solver for registry tests ─────────────────────────
namespace
{
class MockSolver : public IKCSolver
{
public:
std::string name() const override
{
return "MockSolver";
}
std::vector<BaseJointKind> 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<MockSolver>(); });
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<MockSolver>(); });
EXPECT_TRUE(first);
bool second = reg.register_solver("test_dup",
[]() { return std::make_unique<MockSolver>(); });
EXPECT_FALSE(second);
}
TEST(SolverRegistryTest, AvailableListsSolvers) // NOLINT
{
auto& reg = SolverRegistry::instance();
reg.register_solver("test_avail_1",
[]() { return std::make_unique<MockSolver>(); });
reg.register_solver("test_avail_2",
[]() { return std::make_unique<MockSolver>(); });
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<MockSolver>(); });
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<MockSolver>(); });
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());
}