feat(kcsolve): add to_dict()/from_dict() JSON serialization for all types
All checks were successful
Build and Test / build (pull_request) Successful in 31m19s
All checks were successful
Build and Test / build (pull_request) Successful in 31m19s
Phase 3a of the solver server integration: add dict/JSON serialization
to all KCSolve pybind11 types so SolveContext and SolveResult can be
transported as JSON between the Create client, Silo server, and solver
runners.
Implementation:
- Constexpr enum string mapping tables for all 5 enums (BaseJointKind,
SolveStatus, DiagnosticKind, MotionKind, LimitKind) with template
bidirectional lookup helpers
- File-local to_dict/from_dict conversion functions for all 10 types
(Transform, Part, Constraint::Limit, Constraint, MotionDef,
SimulationParams, SolveContext, ConstraintDiagnostic,
SolveResult::PartResult, SolveResult)
- .def("to_dict") and .def_static("from_dict") on every py::class_<>
binding chain
Serialization details per SOLVER.md §3:
- SolveContext.to_dict() includes api_version field
- SolveContext.from_dict() validates api_version, raises ValueError on
mismatch
- Enums serialize as strings matching pybind11 .value() names
- Transform: {position: [x,y,z], quaternion: [w,x,y,z]}
- Optional simulation serializes as None/null
- Pure pybind11 py::dict construction, no new dependencies
Tests: 16 new tests in TestKCSolveSerialization covering round-trips for
all types, all 24 BaseJointKind values, all 4 SolveStatus values,
json.dumps/loads stdlib round-trip, and error cases (missing key,
invalid enum, bad array length, wrong api_version).
This commit is contained in:
@@ -149,6 +149,289 @@ class TestKCSolveRegistry(unittest.TestCase):
|
||||
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."""
|
||||
|
||||
|
||||
@@ -31,6 +31,7 @@
|
||||
|
||||
#include "PyIKCSolver.h"
|
||||
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
@@ -38,6 +39,456 @@ 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
|
||||
@@ -216,14 +667,18 @@ PYBIND11_MODULE(kcsolve, m)
|
||||
+ 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_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");
|
||||
|
||||
@@ -231,7 +686,9 @@ PYBIND11_MODULE(kcsolve, m)
|
||||
.def(py::init<>())
|
||||
.def_readwrite("kind", &Constraint::Limit::kind)
|
||||
.def_readwrite("value", &Constraint::Limit::value)
|
||||
.def_readwrite("tolerance", &Constraint::Limit::tolerance);
|
||||
.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<>())
|
||||
@@ -243,7 +700,9 @@ PYBIND11_MODULE(kcsolve, m)
|
||||
.def_readwrite("type", &Constraint::type)
|
||||
.def_readwrite("params", &Constraint::params)
|
||||
.def_readwrite("limits", &Constraint::limits)
|
||||
.def_readwrite("activated", &Constraint::activated);
|
||||
.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<>())
|
||||
@@ -252,7 +711,9 @@ PYBIND11_MODULE(kcsolve, m)
|
||||
.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_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<>())
|
||||
@@ -261,7 +722,9 @@ PYBIND11_MODULE(kcsolve, m)
|
||||
.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_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<>())
|
||||
@@ -269,20 +732,26 @@ PYBIND11_MODULE(kcsolve, m)
|
||||
.def_readwrite("constraints", &SolveContext::constraints)
|
||||
.def_readwrite("motions", &SolveContext::motions)
|
||||
.def_readwrite("simulation", &SolveContext::simulation)
|
||||
.def_readwrite("bundle_fixed", &SolveContext::bundle_fixed);
|
||||
.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_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_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<>())
|
||||
@@ -290,7 +759,9 @@ PYBIND11_MODULE(kcsolve, m)
|
||||
.def_readwrite("placements", &SolveResult::placements)
|
||||
.def_readwrite("dof", &SolveResult::dof)
|
||||
.def_readwrite("diagnostics", &SolveResult::diagnostics)
|
||||
.def_readwrite("num_frames", &SolveResult::num_frames);
|
||||
.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) ─────────
|
||||
|
||||
|
||||
Reference in New Issue
Block a user