98051ba0c9
- Move existing OndselSolver, GNN ML layer, and tooling into GNN/ directory for integration in later phases - Add Create addon scaffold: package.xml, Init.py - Add expression DAG with eval, symbolic diff, simplification - Add parameter table with fixed/free variable tracking - Add quaternion rotation as polynomial Expr trees - Add RigidBody entity (7 DOF: position + unit quaternion) - Add constraint classes: Coincident, DistancePointPoint, Fixed - Add Newton-Raphson solver with symbolic Jacobian + numpy lstsq - Add pre-solve passes: substitution + single-equation - Add DOF counting via Jacobian SVD rank - Add KindredSolver IKCSolver bridge for kcsolve integration - Add 82 unit tests covering all modules Registers as 'kindred' solver via kcsolve.register_solver() when loaded by Create's addon_loader.
78 lines
2.8 KiB
Python
78 lines
2.8 KiB
Python
"""Tests for RigidBody entities."""
|
|
|
|
import math
|
|
|
|
import pytest
|
|
from kindred_solver.entities import RigidBody
|
|
from kindred_solver.params import ParamTable
|
|
|
|
|
|
class TestRigidBody:
|
|
def test_identity_world_point(self):
|
|
"""Body at origin with identity rotation: local == world."""
|
|
pt = ParamTable()
|
|
body = RigidBody("p1", pt, (0, 0, 0), (1, 0, 0, 0))
|
|
wx, wy, wz = body.world_point(1.0, 2.0, 3.0)
|
|
env = pt.get_env()
|
|
assert abs(wx.eval(env) - 1.0) < 1e-10
|
|
assert abs(wy.eval(env) - 2.0) < 1e-10
|
|
assert abs(wz.eval(env) - 3.0) < 1e-10
|
|
|
|
def test_translated(self):
|
|
"""Body translated by (10, 20, 30) with identity rotation."""
|
|
pt = ParamTable()
|
|
body = RigidBody("p1", pt, (10, 20, 30), (1, 0, 0, 0))
|
|
wx, wy, wz = body.world_point(1.0, 2.0, 3.0)
|
|
env = pt.get_env()
|
|
assert abs(wx.eval(env) - 11.0) < 1e-10
|
|
assert abs(wy.eval(env) - 22.0) < 1e-10
|
|
assert abs(wz.eval(env) - 33.0) < 1e-10
|
|
|
|
def test_rotated_90_z(self):
|
|
"""90-degree rotation about Z: (1,0,0) -> (0,1,0)."""
|
|
pt = ParamTable()
|
|
# 90-deg about Z: q = (cos(45), 0, 0, sin(45))
|
|
c = math.cos(math.pi / 4)
|
|
s = math.sin(math.pi / 4)
|
|
body = RigidBody("p1", pt, (0, 0, 0), (c, 0, 0, s))
|
|
wx, wy, wz = body.world_point(1.0, 0.0, 0.0)
|
|
env = pt.get_env()
|
|
assert abs(wx.eval(env) - 0.0) < 1e-10
|
|
assert abs(wy.eval(env) - 1.0) < 1e-10
|
|
assert abs(wz.eval(env) - 0.0) < 1e-10
|
|
|
|
def test_quat_norm_residual(self):
|
|
"""Normalization residual is zero for unit quaternion."""
|
|
pt = ParamTable()
|
|
body = RigidBody("p1", pt, (0, 0, 0), (1, 0, 0, 0))
|
|
r = body.quat_norm_residual()
|
|
env = pt.get_env()
|
|
assert abs(r.eval(env)) < 1e-10
|
|
|
|
def test_grounded(self):
|
|
"""Grounded body has all params fixed."""
|
|
pt = ParamTable()
|
|
body = RigidBody("p1", pt, (1, 2, 3), (1, 0, 0, 0), grounded=True)
|
|
assert pt.n_free() == 0
|
|
assert body.grounded
|
|
|
|
def test_extract(self):
|
|
"""Extract position and quaternion from env."""
|
|
pt = ParamTable()
|
|
body = RigidBody("p1", pt, (1, 2, 3), (0.5, 0.5, 0.5, 0.5))
|
|
env = pt.get_env()
|
|
pos = body.extract_position(env)
|
|
quat = body.extract_quaternion(env)
|
|
assert pos == (1.0, 2.0, 3.0)
|
|
assert quat == (0.5, 0.5, 0.5, 0.5)
|
|
|
|
def test_differentiation(self):
|
|
"""World point expressions are differentiable w.r.t. body params."""
|
|
pt = ParamTable()
|
|
body = RigidBody("p1", pt, (0, 0, 0), (1, 0, 0, 0))
|
|
wx, wy, wz = body.world_point(1.0, 0.0, 0.0)
|
|
# d(wx)/d(tx) should be 1
|
|
dtx = wx.diff("p1/tx").simplify()
|
|
env = pt.get_env()
|
|
assert abs(dtx.eval(env) - 1.0) < 1e-10
|