Multiview v1.5

pipeline/2_Multiview.py

@@ -41,6 +41,87 @@ from scipy.optimize import least_squares
 
 STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"]
 
+# ------------------------------------------------------------------
+# Constraint definitions and validation
+# ------------------------------------------------------------------
+
+class ConstraintResult:
+    """Result of validating/applying a single constraint"""
+    def __init__(self, name: str, enabled: bool, reason: str = ""):
+        self.name = name
+        self.enabled = enabled
+        self.reason = reason
+        self.residuals = []
+
+    def __str__(self) -> str:
+        status = "✓ ENABLED" if self.enabled else "✗ DISABLED"
+        return f"{self.name:40s} {status:12s}  {self.reason}"
+
+
+def validate_constraints(robot: Dict[str, Any], robot_markers: Dict[int, Dict[str, Any]]) -> Dict[str, ConstraintResult]:
+    """
+    Validate which constraints can be applied based on robot geometry.
+    Returns a dict of constraint_name -> ConstraintResult
+    """
+    results = {}
+
+    # --- Constraint 1: Rigid body distances within each link ---
+    rigid_body_result = ConstraintResult("RigidBodyDistances", False)
+    try:
+        rigid_body_count = 0
+        for link_name in ['Arm1', 'Ellbow', 'Arm2']:
+            link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name]
+            if len(link_markers) >= 2:
+                rigid_body_count += 1
+        if rigid_body_count >= 2:
+            rigid_body_result.enabled = True
+            rigid_body_result.reason = f"Found {rigid_body_count} links with 2+ markers each"
+        else:
+            rigid_body_result.reason = "Not enough rigid links with multiple markers"
+    except Exception as e:
+        rigid_body_result.reason = f"Error: {str(e)}"
+    results['RigidBodyDistances'] = rigid_body_result
+
+    # --- Constraint 2: Fixed X-distances between links (rotation around X-axis) ---
+    inter_link_x_result = ConstraintResult("InterLinkXDistances", False)
+    try:
+        links_with_markers = set(m['link_name'] for m in robot_markers.values())
+        x_rotated_links = []
+        for link_name in ['Arm1', 'Ellbow']:
+            if link_name in links_with_markers:
+                link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name]
+                if len(link_markers) >= 1:
+                    x_rotated_links.append(link_name)
+        if len(x_rotated_links) >= 2:
+            inter_link_x_result.enabled = True
+            inter_link_x_result.reason = f"Found {len(x_rotated_links)} X-rotation links: {', '.join(x_rotated_links)}"
+        else:
+            inter_link_x_result.reason = "Not enough X-rotation links"
+    except Exception as e:
+        inter_link_x_result.reason = f"Error: {str(e)}"
+    results['InterLinkXDistances'] = inter_link_x_result
+
+    # --- Sanity check (not a hard constraint): Arm2 sin(a) dependency ---
+    arm2_sina_result = ConstraintResult("Arm2SinADependency", True, "Sanity check only (not enforced)")
+    try:
+        arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2']
+        if len(arm2_markers) >= 2:
+            z_values = set(m['position_m'][2] for m in arm2_markers)
+            if len(z_values) > 1:
+                arm2_sina_result.enabled = True
+                arm2_sina_result.reason = "Multiple Z-values detected; sin(a) dependency confirmed"
+            else:
+                arm2_sina_result.enabled = False
+                arm2_sina_result.reason = "No Z-variation in Arm2 markers (cannot use sin(a) constraint)"
+        else:
+            arm2_sina_result.enabled = False
+            arm2_sina_result.reason = "Not enough Arm2 markers"
+    except Exception as e:
+        arm2_sina_result.reason = f"Error: {str(e)}"
+    results['Arm2SinADependency'] = arm2_sina_result
+
+    return results
+
 
 # ------------------------------------------------------------------
 # JSON helpers