Multiview v1.5
This commit is contained in:
chk
123
check_constraints.py
Normal file
123
check_constraints.py
Normal file
@@ -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
|
||||
""")
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user