Merge branch 'main' into docs/solver-spec-update
All checks were successful
Build and Test / build (pull_request) Successful in 30m48s
All checks were successful
Build and Test / build (pull_request) Successful in 30m48s
This commit is contained in:
2
.gitmodules
vendored
2
.gitmodules
vendored
@@ -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
|
||||
|
||||
Submodule mods/silo updated: 43e905c00a...dfa1da97dd
@@ -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
@@ -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;
|
||||
|
||||
|
||||
@@ -5,7 +5,7 @@ set(Assembly_LIBS
|
||||
PartDesign
|
||||
Spreadsheet
|
||||
FreeCADApp
|
||||
OndselSolver
|
||||
KCSolve
|
||||
)
|
||||
|
||||
generate_from_py(AssemblyObject)
|
||||
|
||||
520
src/Mod/Assembly/AssemblyTests/TestKCSolvePy.py
Normal file
520
src/Mod/Assembly/AssemblyTests/TestKCSolvePy.py
Normal 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)
|
||||
216
src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py
Normal file
216
src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py
Normal 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",
|
||||
)
|
||||
@@ -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
|
||||
)
|
||||
|
||||
46
src/Mod/Assembly/Solver/CMakeLists.txt
Normal file
46
src/Mod/Assembly/Solver/CMakeLists.txt
Normal 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()
|
||||
189
src/Mod/Assembly/Solver/IKCSolver.h
Normal file
189
src/Mod/Assembly/Solver/IKCSolver.h
Normal 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
|
||||
37
src/Mod/Assembly/Solver/KCSolveGlobal.h
Normal file
37
src/Mod/Assembly/Solver/KCSolveGlobal.h
Normal 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
|
||||
796
src/Mod/Assembly/Solver/OndselAdapter.cpp
Normal file
796
src/Mod/Assembly/Solver/OndselAdapter.cpp
Normal 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
|
||||
129
src/Mod/Assembly/Solver/OndselAdapter.h
Normal file
129
src/Mod/Assembly/Solver/OndselAdapter.h
Normal 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
|
||||
346
src/Mod/Assembly/Solver/SolverRegistry.cpp
Normal file
346
src/Mod/Assembly/Solver/SolverRegistry.cpp
Normal 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
|
||||
124
src/Mod/Assembly/Solver/SolverRegistry.h
Normal file
124
src/Mod/Assembly/Solver/SolverRegistry.h
Normal 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
|
||||
286
src/Mod/Assembly/Solver/Types.h
Normal file
286
src/Mod/Assembly/Solver/Types.h
Normal 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
|
||||
31
src/Mod/Assembly/Solver/bindings/CMakeLists.txt
Normal file
31
src/Mod/Assembly/Solver/bindings/CMakeLists.txt
Normal 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})
|
||||
121
src/Mod/Assembly/Solver/bindings/PyIKCSolver.h
Normal file
121
src/Mod/Assembly/Solver/bindings/PyIKCSolver.h
Normal 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
|
||||
830
src/Mod/Assembly/Solver/bindings/kcsolve_py.cpp
Normal file
830
src/Mod/Assembly/Solver/bindings/kcsolve_py.cpp
Normal 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.");
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
13
tests/src/Mod/Assembly/Solver/CMakeLists.txt
Normal file
13
tests/src/Mod/Assembly/Solver/CMakeLists.txt
Normal 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
|
||||
)
|
||||
251
tests/src/Mod/Assembly/Solver/OndselAdapter.cpp
Normal file
251
tests/src/Mod/Assembly/Solver/OndselAdapter.cpp
Normal 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);
|
||||
}
|
||||
131
tests/src/Mod/Assembly/Solver/SolverRegistry.cpp
Normal file
131
tests/src/Mod/Assembly/Solver/SolverRegistry.cpp
Normal 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());
|
||||
}
|
||||
Reference in New Issue
Block a user