diff --git a/check_constraints.py b/check_constraints.py new file mode 100644 index 0000000..7cb049c --- /dev/null +++ b/check_constraints.py @@ -0,0 +1,123 @@ +import json +import math + +# Load robot.json +with open(r'robot.json') as f: + robot = json.load(f) + +# Extract markers by link +markers_by_link = {} +for link_name, link_data in robot['links'].items(): + markers = link_data.get('markers', []) + if markers: + markers_by_link[link_name] = markers + +print("=" * 70) +print("MARKER DISTRIBUTION BY LINK") +print("=" * 70) +for link, markers in markers_by_link.items(): + print(f"\n{link}: {len(markers)} markers") + for m in markers: + pos = m['position'] + print(f" ID {m['id']:3d}: pos={pos}") + +# Check rigid body constraints: distances within each link +print("\n" + "=" * 70) +print("RIGID BODY CONSTRAINT CHECK (Arm1, Ellbow, Arm2)") +print("=" * 70) + +for link_name in ['Arm1', 'Ellbow', 'Arm2']: + if link_name not in markers_by_link: + continue + + markers = markers_by_link[link_name] + positions = [(m['position'][0], m['position'][1], m['position'][2]) for m in markers] + + print(f"\n{link_name}:") + if len(positions) > 1: + for i in range(len(positions)): + for j in range(i+1, len(positions)): + dx = positions[i][0] - positions[j][0] + dy = positions[i][1] - positions[j][1] + dz = positions[i][2] - positions[j][2] + dist = math.sqrt(dx*dx + dy*dy + dz*dz) + print(f" Marker {markers[i]['id']:3d} <-> {markers[j]['id']:3d}: {dist:7.2f} mm") + else: + print(f" Only {len(positions)} marker(s)") + +# Check X-distances between links +print("\n" + "=" * 70) +print("X-POSITION CONSTRAINT CHECK (Link relationships)") +print("=" * 70) + +# Get marker positions per link +link_x_positions = {} +for link_name in ['Board', 'Base', 'Arm1', 'Ellbow', 'Arm2']: + if link_name in markers_by_link: + x_vals = [m['position'][0] for m in markers_by_link[link_name]] + link_x_positions[link_name] = { + 'min_x': min(x_vals), + 'max_x': max(x_vals), + 'count': len(x_vals) + } + +print("\nX-position ranges by link (local coords):") +for link, data in link_x_positions.items(): + print(f" {link:10s}: x in [{data['min_x']:7.2f}, {data['max_x']:7.2f}] mm ({data['count']} markers)") + +# Check X-distance between Arm1 and Ellbow +print("\nKey constraint analysis:") +arm1_x = [m['position'][0] for m in markers_by_link['Arm1']] +ellbow_x = [m['position'][0] for m in markers_by_link['Ellbow']] +print(f" Arm1 local X: {arm1_x}") +print(f" Ellbow local X: {ellbow_x}") +print(f" => BOTH rotate around X-axis, so X-spread within each link is FIXED") +print(f" => X-distance between links is FIXED in world (different local X values)") + +# Check Arm2 structure +print("\n" + "=" * 70) +print("ARM2 KINEMATIC CHECK (sin(a) dependency)") +print("=" * 70) + +arm2_markers = markers_by_link['Arm2'] +print(f"\nArm2 has {len(arm2_markers)} markers") +print("Arm2 marker positions (local coords, before 'a' rotation around Y):") +for m in arm2_markers: + pos = m['position'] + print(f" ID {m['id']:3d}: x={pos[0]:8.2f}, y={pos[1]:8.2f}, z={pos[2]:8.2f}") + +print("\nWhen Arm2 rotates around Y-axis (variable 'a'):") +print(" Rotation matrix Ry(a) acts on [x_local, y_local, z_local]:") +print(" X_world = 90 + x_local * cos(a) - z_local * sin(a)") +print(" Y_world = y_local (unchanged)") +print(" Z_world = x_local * sin(a) + z_local * cos(a)") +print("\n=> X_world DEPENDS on sin(a) and z_local values!") + +# Verify: X differences between Arm2 markers +arm2_x_vals = [m['position'][0] for m in arm2_markers] +arm2_z_vals = [m['position'][2] for m in arm2_markers] +print(f"\nArm2 local X values: {sorted(set(arm2_x_vals))}") +print(f"Arm2 local Z values: {sorted(set(arm2_z_vals))}") +if len(set(arm2_z_vals)) > 1: + print(f"=> Multiple Z values present: X_world will differ by sin(a) contributions") + +print("\n" + "=" * 70) +print("SUMMARY: WHICH CONSTRAINTS MAKE SENSE?") +print("=" * 70) +print(""" +1. RIGID BODY DISTANCES within Arm1/Ellbow/Arm2: + ✓ VALID - Fixed distances between markers on same rigid link + ✓ HELPS with: Preventing deformation, reducing degrees of freedom + +2. X-DISTANCES between links (Arm1, Ellbow, Base): + ✓ VALID - Both rotate around X-axis, so relative X is fixed + ✓ HELPS with: Preventing sliding along links + +3. X-POSITION of Arm2 dependent on sin(a): + ⚠ PARTIALLY VALID - Different markers have different dependencies + ✓ HELPS with: Constraining the a variable from Z-spread observation + +IMPLEMENTATION RECOMMENDATION: +- Start with constraints 1 & 2 (rigid body + inter-link X) +- Use constraint 3 as a sanity check on 'a' estimation +""") diff --git a/pipeline/2_Multiview.py b/pipeline/2_Multiview.py index 59c3b1e..948fb88 100644 --- a/pipeline/2_Multiview.py +++ b/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