fa34dc0d49
When a PlanarConstraint has distance=0 (point already on plane), the half-space tracker was using dot(z_i, z_j) as its indicator and providing a correction function that reflects the body through the plane. When combined with a Cylindrical joint, legitimate rotation about the cylinder axis causes the body's planar face normal to rotate. After ~90 degrees, the dot product crosses zero, triggering the correction which teleports the body to the other side of the plane. The C++ validator then rejects every subsequent drag step as 'flipped orientation'. Fix: return a tracking-only HalfSpace (no correction_fn) when the point is already on the plane. This matches the pattern used by Cylindrical, Revolute, and Concentric half-space trackers. The cross-product residuals in PlanarConstraint already enforce normal alignment via the solver itself, so the correction is redundant and harmful during drag.
675 lines
22 KiB
Python
675 lines
22 KiB
Python
"""Solution preference: half-space tracking and minimum-movement weighting.
|
|
|
|
Half-space tracking preserves the initial configuration branch across
|
|
Newton iterations. For constraints with multiple valid solutions
|
|
(e.g. distance can be satisfied on either side), we record which
|
|
"half-space" the initial state lives in and correct the solver step
|
|
if it crosses to the wrong branch.
|
|
|
|
Minimum-movement weighting scales the Newton/BFGS step so that
|
|
quaternion parameters (rotation) are penalised more than translation
|
|
parameters, yielding the physically-nearest solution.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
from dataclasses import dataclass, field
|
|
from typing import Callable, List
|
|
|
|
import numpy as np
|
|
|
|
from .constraints import (
|
|
AngleConstraint,
|
|
ConcentricConstraint,
|
|
ConstraintBase,
|
|
CylindricalConstraint,
|
|
DistancePointPointConstraint,
|
|
LineInPlaneConstraint,
|
|
ParallelConstraint,
|
|
PerpendicularConstraint,
|
|
PlanarConstraint,
|
|
PointInPlaneConstraint,
|
|
RevoluteConstraint,
|
|
ScrewConstraint,
|
|
SliderConstraint,
|
|
UniversalConstraint,
|
|
)
|
|
from .geometry import cross3, dot3, marker_z_axis, point_plane_distance, sub3
|
|
from .params import ParamTable
|
|
|
|
|
|
@dataclass
|
|
class HalfSpace:
|
|
"""Tracks which branch of a branching constraint the solution should stay in."""
|
|
|
|
constraint_index: int # index in ctx.constraints
|
|
reference_sign: float # +1.0 or -1.0, captured at setup
|
|
indicator_fn: Callable[[dict[str, float]], float] # returns signed value
|
|
param_names: list[str] = field(default_factory=list) # params to flip
|
|
correction_fn: Callable[[ParamTable, float], None] | None = None
|
|
|
|
|
|
def compute_half_spaces(
|
|
constraint_objs: list[ConstraintBase],
|
|
constraint_indices: list[int],
|
|
params: ParamTable,
|
|
) -> list[HalfSpace]:
|
|
"""Build half-space trackers for all branching constraints.
|
|
|
|
Evaluates each constraint's indicator function at the current
|
|
parameter values to capture the reference sign.
|
|
"""
|
|
env = params.get_env()
|
|
half_spaces: list[HalfSpace] = []
|
|
|
|
for i, obj in enumerate(constraint_objs):
|
|
hs = _build_half_space(obj, constraint_indices[i], env, params)
|
|
if hs is not None:
|
|
half_spaces.append(hs)
|
|
|
|
return half_spaces
|
|
|
|
|
|
def apply_half_space_correction(
|
|
params: ParamTable,
|
|
half_spaces: list[HalfSpace],
|
|
) -> None:
|
|
"""Check each half-space and correct if the solver crossed a branch.
|
|
|
|
Called as a post_step callback from newton_solve.
|
|
"""
|
|
if not half_spaces:
|
|
return
|
|
|
|
env = params.get_env()
|
|
for hs in half_spaces:
|
|
current_val = hs.indicator_fn(env)
|
|
current_sign = (
|
|
math.copysign(1.0, current_val)
|
|
if abs(current_val) > 1e-14
|
|
else hs.reference_sign
|
|
)
|
|
if current_sign != hs.reference_sign and hs.correction_fn is not None:
|
|
hs.correction_fn(params, current_val)
|
|
# Re-read env after correction for subsequent half-spaces
|
|
env = params.get_env()
|
|
|
|
|
|
def _build_half_space(
|
|
obj: ConstraintBase,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Build a HalfSpace for a branching constraint, or None if not branching."""
|
|
|
|
if isinstance(obj, DistancePointPointConstraint) and obj.distance > 0:
|
|
return _distance_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, ParallelConstraint):
|
|
return _parallel_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, AngleConstraint):
|
|
return _angle_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, PerpendicularConstraint):
|
|
return _perpendicular_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, PlanarConstraint):
|
|
return _planar_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, RevoluteConstraint):
|
|
return _revolute_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, ConcentricConstraint):
|
|
return _concentric_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, PointInPlaneConstraint):
|
|
return _point_in_plane_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, LineInPlaneConstraint):
|
|
return _line_in_plane_half_space(obj, constraint_idx, env, params)
|
|
|
|
if isinstance(obj, CylindricalConstraint):
|
|
return _axis_direction_half_space(obj, constraint_idx, env)
|
|
|
|
if isinstance(obj, SliderConstraint):
|
|
return _axis_direction_half_space(obj, constraint_idx, env)
|
|
|
|
if isinstance(obj, ScrewConstraint):
|
|
return _axis_direction_half_space(obj, constraint_idx, env)
|
|
|
|
if isinstance(obj, UniversalConstraint):
|
|
return _universal_half_space(obj, constraint_idx, env)
|
|
|
|
return None
|
|
|
|
|
|
def _distance_half_space(
|
|
obj: DistancePointPointConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Half-space for DistancePointPoint: track displacement direction.
|
|
|
|
The indicator is the dot product of the current displacement with
|
|
the reference displacement direction. If the solver flips to the
|
|
opposite side, we reflect the moving body's position.
|
|
"""
|
|
p_i, p_j = obj.world_points()
|
|
|
|
# Evaluate reference displacement direction
|
|
dx = p_i[0].eval(env) - p_j[0].eval(env)
|
|
dy = p_i[1].eval(env) - p_j[1].eval(env)
|
|
dz = p_i[2].eval(env) - p_j[2].eval(env)
|
|
dist = math.sqrt(dx * dx + dy * dy + dz * dz)
|
|
|
|
if dist < 1e-14:
|
|
return None # points coincident, no branch to track
|
|
|
|
# Reference unit direction
|
|
nx, ny, nz = dx / dist, dy / dist, dz / dist
|
|
|
|
# Build indicator: dot(displacement, reference_direction)
|
|
# Use Expr evaluation for speed
|
|
disp_x, disp_y, disp_z = p_i[0] - p_j[0], p_i[1] - p_j[1], p_i[2] - p_j[2]
|
|
|
|
def indicator(e: dict[str, float]) -> float:
|
|
return disp_x.eval(e) * nx + disp_y.eval(e) * ny + disp_z.eval(e) * nz
|
|
|
|
ref_sign = math.copysign(1.0, indicator(env))
|
|
|
|
# Correction: reflect body_j position along reference direction
|
|
# (or body_i if body_j is grounded)
|
|
moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
|
|
if moving_body.grounded:
|
|
return None # both grounded, nothing to correct
|
|
|
|
px_name = f"{moving_body.part_id}/tx"
|
|
py_name = f"{moving_body.part_id}/ty"
|
|
pz_name = f"{moving_body.part_id}/tz"
|
|
|
|
sign_flip = -1.0 if moving_body is obj.body_j else 1.0
|
|
|
|
def correction(p: ParamTable, _val: float) -> None:
|
|
# Reflect displacement: negate the component along reference direction
|
|
e = p.get_env()
|
|
cur_dx = disp_x.eval(e)
|
|
cur_dy = disp_y.eval(e)
|
|
cur_dz = disp_z.eval(e)
|
|
# Project displacement onto reference direction
|
|
proj = cur_dx * nx + cur_dy * ny + cur_dz * nz
|
|
# Reflect: subtract 2*proj*n from the moving body's position
|
|
if not p.is_fixed(px_name):
|
|
p.set_value(px_name, p.get_value(px_name) + sign_flip * 2.0 * proj * nx)
|
|
if not p.is_fixed(py_name):
|
|
p.set_value(py_name, p.get_value(py_name) + sign_flip * 2.0 * proj * ny)
|
|
if not p.is_fixed(pz_name):
|
|
p.set_value(pz_name, p.get_value(pz_name) + sign_flip * 2.0 * proj * nz)
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=indicator,
|
|
param_names=[px_name, py_name, pz_name],
|
|
correction_fn=correction,
|
|
)
|
|
|
|
|
|
def _parallel_half_space(
|
|
obj: ParallelConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace:
|
|
"""Half-space for Parallel: track same-direction vs opposite-direction.
|
|
|
|
Indicator: dot(z_i, z_j). Positive = same direction, negative = opposite.
|
|
"""
|
|
z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
|
|
z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
dot_expr = dot3(z_i, z_j)
|
|
|
|
def indicator(e: dict[str, float]) -> float:
|
|
return dot_expr.eval(e)
|
|
|
|
ref_val = indicator(env)
|
|
ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
|
|
|
|
# No geometric correction — just let the indicator track.
|
|
# The Newton solver naturally handles this via the cross-product residual.
|
|
# We only need to detect and report branch flips.
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=indicator,
|
|
)
|
|
|
|
|
|
def _planar_half_space(
|
|
obj: PlanarConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Half-space for Planar: track which side of the plane the point is on
|
|
AND which direction the normals face.
|
|
|
|
A Planar constraint has parallel normals (cross product = 0) plus
|
|
point-in-plane (signed distance = 0). Both have branch ambiguity:
|
|
the normals can be same-direction or opposite, and the point can
|
|
approach the plane from either side. We track the signed distance
|
|
from marker_i to the plane defined by marker_j — this captures
|
|
the plane-side and is the primary drift mode during drag.
|
|
"""
|
|
# Point-in-plane signed distance as indicator
|
|
p_i = obj.body_i.world_point(*obj.marker_i_pos)
|
|
p_j = obj.body_j.world_point(*obj.marker_j_pos)
|
|
z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
d_expr = point_plane_distance(p_i, p_j, z_j)
|
|
|
|
d_val = d_expr.eval(env)
|
|
|
|
# Also track normal alignment (same as Parallel half-space)
|
|
z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
|
|
dot_expr = dot3(z_i, z_j)
|
|
dot_val = dot_expr.eval(env)
|
|
normal_ref_sign = math.copysign(1.0, dot_val) if abs(dot_val) > 1e-14 else 1.0
|
|
|
|
# If offset is zero and distance is near-zero, we still need the normal
|
|
# direction indicator to prevent flipping through the plane.
|
|
# Use the normal dot product as the primary indicator when the point
|
|
# is already on the plane (distance ≈ 0).
|
|
if abs(d_val) < 1e-10:
|
|
# Point is on the plane — track normal direction only.
|
|
# No correction: when combined with rotational joints (Cylindrical,
|
|
# Revolute), the body's marker normal rotates legitimately and
|
|
# reflecting through the plane would fight the rotation.
|
|
def indicator(e: dict[str, float]) -> float:
|
|
return dot_expr.eval(e)
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=normal_ref_sign,
|
|
indicator_fn=indicator,
|
|
)
|
|
else:
|
|
# Point is off-plane — track which side
|
|
def indicator(e: dict[str, float]) -> float:
|
|
return d_expr.eval(e)
|
|
|
|
ref_sign = math.copysign(1.0, d_val)
|
|
|
|
# Correction: reflect the moving body's position through the plane
|
|
moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
|
|
if moving_body.grounded:
|
|
return None
|
|
|
|
px_name = f"{moving_body.part_id}/tx"
|
|
py_name = f"{moving_body.part_id}/ty"
|
|
pz_name = f"{moving_body.part_id}/tz"
|
|
|
|
def correction(p: ParamTable, _val: float) -> None:
|
|
e = p.get_env()
|
|
# Recompute signed distance and normal direction
|
|
d_cur = d_expr.eval(e)
|
|
nx = z_j[0].eval(e)
|
|
ny = z_j[1].eval(e)
|
|
nz = z_j[2].eval(e)
|
|
n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
|
|
if n_len < 1e-15:
|
|
return
|
|
nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
|
|
# Reflect through plane: move body by -2*d along normal
|
|
sign = -1.0 if moving_body is obj.body_j else 1.0
|
|
if not p.is_fixed(px_name):
|
|
p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
|
|
if not p.is_fixed(py_name):
|
|
p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
|
|
if not p.is_fixed(pz_name):
|
|
p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=indicator,
|
|
correction_fn=correction,
|
|
)
|
|
|
|
|
|
def _revolute_half_space(
|
|
obj: RevoluteConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Half-space for Revolute: track hinge axis direction.
|
|
|
|
A revolute has coincident origins + parallel Z-axes. The parallel
|
|
axes can flip direction (same ambiguity as Parallel). Track
|
|
dot(z_i, z_j) to prevent the axis from inverting.
|
|
"""
|
|
z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
|
|
z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
dot_expr = dot3(z_i, z_j)
|
|
|
|
ref_val = dot_expr.eval(env)
|
|
ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=lambda e: dot_expr.eval(e),
|
|
)
|
|
|
|
|
|
def _concentric_half_space(
|
|
obj: ConcentricConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Half-space for Concentric: track axis direction.
|
|
|
|
Concentric has parallel axes + point-on-line. The parallel axes
|
|
can flip direction. Track dot(z_i, z_j).
|
|
"""
|
|
z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
|
|
z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
dot_expr = dot3(z_i, z_j)
|
|
|
|
ref_val = dot_expr.eval(env)
|
|
ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=lambda e: dot_expr.eval(e),
|
|
)
|
|
|
|
|
|
def _point_in_plane_half_space(
|
|
obj: PointInPlaneConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Half-space for PointInPlane: track which side of the plane.
|
|
|
|
The signed distance to the plane can be satisfied from either side.
|
|
Track which side the initial configuration is on.
|
|
"""
|
|
p_i = obj.body_i.world_point(*obj.marker_i_pos)
|
|
p_j = obj.body_j.world_point(*obj.marker_j_pos)
|
|
n_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
d_expr = point_plane_distance(p_i, p_j, n_j)
|
|
|
|
d_val = d_expr.eval(env)
|
|
if abs(d_val) < 1e-10:
|
|
return None # already on the plane, no branch to track
|
|
|
|
ref_sign = math.copysign(1.0, d_val)
|
|
|
|
moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
|
|
if moving_body.grounded:
|
|
return None
|
|
|
|
px_name = f"{moving_body.part_id}/tx"
|
|
py_name = f"{moving_body.part_id}/ty"
|
|
pz_name = f"{moving_body.part_id}/tz"
|
|
|
|
def correction(p: ParamTable, _val: float) -> None:
|
|
e = p.get_env()
|
|
d_cur = d_expr.eval(e)
|
|
nx = n_j[0].eval(e)
|
|
ny = n_j[1].eval(e)
|
|
nz = n_j[2].eval(e)
|
|
n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
|
|
if n_len < 1e-15:
|
|
return
|
|
nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
|
|
sign = -1.0 if moving_body is obj.body_j else 1.0
|
|
if not p.is_fixed(px_name):
|
|
p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
|
|
if not p.is_fixed(py_name):
|
|
p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
|
|
if not p.is_fixed(pz_name):
|
|
p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=lambda e: d_expr.eval(e),
|
|
correction_fn=correction,
|
|
)
|
|
|
|
|
|
def _line_in_plane_half_space(
|
|
obj: LineInPlaneConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Half-space for LineInPlane: track which side of the plane.
|
|
|
|
Same plane-side ambiguity as PointInPlane.
|
|
"""
|
|
p_i = obj.body_i.world_point(*obj.marker_i_pos)
|
|
p_j = obj.body_j.world_point(*obj.marker_j_pos)
|
|
n_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
d_expr = point_plane_distance(p_i, p_j, n_j)
|
|
|
|
d_val = d_expr.eval(env)
|
|
if abs(d_val) < 1e-10:
|
|
return None
|
|
|
|
ref_sign = math.copysign(1.0, d_val)
|
|
|
|
moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
|
|
if moving_body.grounded:
|
|
return None
|
|
|
|
px_name = f"{moving_body.part_id}/tx"
|
|
py_name = f"{moving_body.part_id}/ty"
|
|
pz_name = f"{moving_body.part_id}/tz"
|
|
|
|
def correction(p: ParamTable, _val: float) -> None:
|
|
e = p.get_env()
|
|
d_cur = d_expr.eval(e)
|
|
nx = n_j[0].eval(e)
|
|
ny = n_j[1].eval(e)
|
|
nz = n_j[2].eval(e)
|
|
n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
|
|
if n_len < 1e-15:
|
|
return
|
|
nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
|
|
sign = -1.0 if moving_body is obj.body_j else 1.0
|
|
if not p.is_fixed(px_name):
|
|
p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
|
|
if not p.is_fixed(py_name):
|
|
p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
|
|
if not p.is_fixed(pz_name):
|
|
p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=lambda e: d_expr.eval(e),
|
|
correction_fn=correction,
|
|
)
|
|
|
|
|
|
def _axis_direction_half_space(
|
|
obj,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
) -> HalfSpace | None:
|
|
"""Half-space for any constraint with parallel Z-axes (Cylindrical, Slider, Screw).
|
|
|
|
Tracks dot(z_i, z_j) to prevent axis inversion.
|
|
"""
|
|
z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
|
|
z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
dot_expr = dot3(z_i, z_j)
|
|
|
|
ref_val = dot_expr.eval(env)
|
|
ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=lambda e: dot_expr.eval(e),
|
|
)
|
|
|
|
|
|
def _universal_half_space(
|
|
obj: UniversalConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
) -> HalfSpace | None:
|
|
"""Half-space for Universal: track which quadrant of perpendicularity.
|
|
|
|
Universal has dot(z_i, z_j) = 0 (perpendicular). The cross product
|
|
sign distinguishes which "side" of perpendicular.
|
|
"""
|
|
z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
|
|
z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
cx, cy, cz = cross3(z_i, z_j)
|
|
|
|
cx_val = cx.eval(env)
|
|
cy_val = cy.eval(env)
|
|
cz_val = cz.eval(env)
|
|
|
|
components = [
|
|
(abs(cx_val), cx, cx_val),
|
|
(abs(cy_val), cy, cy_val),
|
|
(abs(cz_val), cz, cz_val),
|
|
]
|
|
_, best_expr, best_val = max(components, key=lambda t: t[0])
|
|
|
|
if abs(best_val) < 1e-14:
|
|
return None
|
|
|
|
ref_sign = math.copysign(1.0, best_val)
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=lambda e: best_expr.eval(e),
|
|
)
|
|
|
|
|
|
# ============================================================================
|
|
# Minimum-movement weighting
|
|
# ============================================================================
|
|
|
|
# Scale factor so that a 1-radian rotation is penalised as much as a
|
|
# (180/pi)-unit translation. This makes the weighted minimum-norm
|
|
# step prefer translating over rotating for the same residual reduction.
|
|
QUAT_WEIGHT = (180.0 / math.pi) ** 2 # ~3283
|
|
|
|
|
|
def build_weight_vector(params: ParamTable) -> np.ndarray:
|
|
"""Build diagonal weight vector: 1.0 for translation, QUAT_WEIGHT for quaternion.
|
|
|
|
Returns a 1-D array of length ``params.n_free()``.
|
|
"""
|
|
free = params.free_names()
|
|
w = np.ones(len(free))
|
|
quat_suffixes = ("/qw", "/qx", "/qy", "/qz")
|
|
for i, name in enumerate(free):
|
|
if any(name.endswith(s) for s in quat_suffixes):
|
|
w[i] = QUAT_WEIGHT
|
|
return w
|
|
|
|
|
|
def _angle_half_space(
|
|
obj: AngleConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Half-space for Angle: track sign of sin(angle) via cross product.
|
|
|
|
For angle constraints, the dot product is fixed (= cos(angle)),
|
|
but sin can be +/-. We track the cross product magnitude sign.
|
|
"""
|
|
if abs(obj.angle) < 1e-14 or abs(obj.angle - math.pi) < 1e-14:
|
|
return None # 0 or 180 degrees — no branch ambiguity
|
|
|
|
z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
|
|
z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
cx, cy, cz = cross3(z_i, z_j)
|
|
|
|
# Use the magnitude of the cross product's z-component as indicator
|
|
# (or whichever component is largest at setup time)
|
|
cx_val = cx.eval(env)
|
|
cy_val = cy.eval(env)
|
|
cz_val = cz.eval(env)
|
|
|
|
# Pick the dominant cross product component
|
|
components = [
|
|
(abs(cx_val), cx, cx_val),
|
|
(abs(cy_val), cy, cy_val),
|
|
(abs(cz_val), cz, cz_val),
|
|
]
|
|
_, best_expr, best_val = max(components, key=lambda t: t[0])
|
|
|
|
if abs(best_val) < 1e-14:
|
|
return None
|
|
|
|
def indicator(e: dict[str, float]) -> float:
|
|
return best_expr.eval(e)
|
|
|
|
ref_sign = math.copysign(1.0, best_val)
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=indicator,
|
|
)
|
|
|
|
|
|
def _perpendicular_half_space(
|
|
obj: PerpendicularConstraint,
|
|
constraint_idx: int,
|
|
env: dict[str, float],
|
|
params: ParamTable,
|
|
) -> HalfSpace | None:
|
|
"""Half-space for Perpendicular: track which quadrant.
|
|
|
|
The dot product is constrained to 0, but the cross product sign
|
|
distinguishes which "side" of perpendicular.
|
|
"""
|
|
z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
|
|
z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
|
|
cx, cy, cz = cross3(z_i, z_j)
|
|
|
|
# Pick the dominant cross product component
|
|
cx_val = cx.eval(env)
|
|
cy_val = cy.eval(env)
|
|
cz_val = cz.eval(env)
|
|
|
|
components = [
|
|
(abs(cx_val), cx, cx_val),
|
|
(abs(cy_val), cy, cy_val),
|
|
(abs(cz_val), cz, cz_val),
|
|
]
|
|
_, best_expr, best_val = max(components, key=lambda t: t[0])
|
|
|
|
if abs(best_val) < 1e-14:
|
|
return None
|
|
|
|
def indicator(e: dict[str, float]) -> float:
|
|
return best_expr.eval(e)
|
|
|
|
ref_sign = math.copysign(1.0, best_val)
|
|
|
|
return HalfSpace(
|
|
constraint_index=constraint_idx,
|
|
reference_sign=ref_sign,
|
|
indicator_fn=indicator,
|
|
)
|