8.3  Kinematics - Constraint Solving

import numpy as np
import matplotlib.pyplot as plt
import scipy
from matplotlib.animation import FuncAnimation
from IPython.display import HTML
import matplotlib
matplotlib.rcParams['animation.embed_limit'] = 60  # Increase if needed
from matplotlib.colors import to_rgba

r_OA = 100.0  # Length of link OA
r_CB = 75.0   # Length of link CB
rr_OC = np.array([250, 50])  
r_AB = np.sqrt((250 - 75)**2 + (100 - 50)**2)

def equations(vars, theta):
    """
    Args:
        vars: A list or tuple [phi, gamma] containing the angles
              of links OA and AB in radians.
    Returns:
        The distance between points B from A and B from C, this distance is zero for the correct set of phi and gamma.
    """
    phi, gamma = vars

    rr_OA = r_OA * np.array([np.sin(phi), np.cos(phi)]) 
    rr_AB = r_AB * np.array([np.sin(gamma), np.cos(gamma)])
    # Position of B calculated from chain O->A->B
    rr_OAB = rr_OA + rr_AB

    # Position of B calculated from chain C->B
    # [-sind(θ), cosd(θ)] which corresponds to an angle measured from the +y axis
    rr_CB = r_CB * np.array([-np.sin(theta), np.cos(theta)])
    rr_OCB = rr_OC + rr_CB

    return rr_OAB - rr_OCB

def is_valid_solution(theta, initial_guess, tolerance=1e-3):
    """Check if theta has a valid kinematic solution"""
    try:
        solution, info, ier, msg = scipy.optimize.fsolve(
            equations, initial_guess, args=(theta,), full_output=True
        )
        # Check if solver converged and residual is small
        residual = np.linalg.norm(info['fvec'])
        return ier == 1 and residual < tolerance, solution
    except:
        return False, None

# Find valid theta range by scanning
print("Finding valid theta range...")
theta_test = np.linspace(np.deg2rad(-30), np.deg2rad(300), 30+300)
thetas = []
initial_guess = np.deg2rad([50, 70])

for theta in theta_test:
    is_valid, sol = is_valid_solution(theta, initial_guess)
    if is_valid:
        thetas.append(theta)
        initial_guess = sol  # Use previous solution as next guess
        # print(f"Valid theta found: {np.rad2deg(theta):.1f}°")

if len(thetas) > 0:
    theta_min = thetas[0]
    theta_max = thetas[-1]
    print(f"Valid theta range: [{np.rad2deg(theta_min):.1f}°, {np.rad2deg(theta_max):.1f}°]")
    print(f"Number of valid configurations: {len(thetas)}")
else:
    print("No valid solutions found!")
    theta_min, theta_max = 0, 0

def plot_mechanism(rr_OA, rr_OB, rr_OC, theta):
    # Plot the links of the mechanism
    ax.plot(*zip([0,0], rr_OA), '-ko',lw=2)
    ax.plot(*zip(rr_OC, rr_OB), '-ko',lw=2)
    ax.plot(*zip(rr_OA, rr_OB), '-ko',lw=2)

    ax.text(rr_OA[0] + 5, rr_OA[1], 'A', fontsize=12)
    ax.text(rr_OB[0] + 5, rr_OB[1], 'B', fontsize=12)
    ax.text(rr_OC[0] + 5, rr_OC[1], 'C', fontsize=12)
    ax.text(5, 5, 'O', fontsize=12)
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_title(f'Three-Bar Linkage (θ = {np.rad2deg(theta):.1f}°)')
    ax.grid(True)
    ax.set_aspect('equal', adjustable='box') # Ensures correct proportions
    ax.set_xlim(-50, 300)
    ax.set_ylim(-50, 150)

# Only animate valid theta values
# thetas = np.linspace(theta_min, theta_max, 60)

fig, ax = plt.subplots(figsize=(10, 8))

def animate(frame_idx):
    """Animation function for each frame"""
    ax.clear()
    
    theta = thetas[frame_idx]
    initial_guess = np.deg2rad([50, 70]) 
    solution_rad = scipy.optimize.fsolve(equations, initial_guess, args=(theta,))
    phi_sol, gamma_sol = solution_rad

    rr_OA = r_OA * np.array([np.sin(phi_sol), np.cos(phi_sol)])
    rr_OB = rr_OA + r_AB * np.array([np.sin(gamma_sol), np.cos(gamma_sol)])

    plot_mechanism(rr_OA, rr_OB, rr_OC, theta)

# Create figure and axis (close to prevent static display)
plt.close(fig)  # Prevent static image from showing

# Create animation
anim = FuncAnimation(fig, animate, frames=len(thetas), 
                     interval=100, repeat=True)

# Get the HTML and wrap it with responsive CSS
anim_html = anim.to_jshtml()

# Wrap with CSS to make it responsive and fit to 100% width
responsive_html = f"""
<div style="width: 100%; max-width: 100%; overflow: hidden;">
    <style>
        .animation-container {{
            width: 100% !important;
            max-width: 100% !important;
        }}
        .animation-container img {{
            width: 100% !important;
            height: auto !important;
        }}
    </style>
    <div class="animation-container">
        {anim_html}
    </div>
</div>
"""
HTML(responsive_html)

# video_html = anim.to_html5_video()
# video_html = video_html.replace('controls', 'controls autoplay loop muted')
# video_html = video_html.replace(
#     '<video ', 
#     '<video style="width: 100%; height: auto;" '
# )
# HTML(video_html)