Delta Model Simulate

[tianrking]

Reachable

[tianrking]

import numpy as np
import plotly.graph_objects as go
from math import sqrt, cos, sin, tan, pi
from plotly.subplots import make_subplots

class DeltaKinematics:
    def __init__(self, arm_length, rod_length, base_tri, platform_tri):
        self.arm_length = arm_length
        self.rod_length = rod_length
        self.base_tri = base_tri
        self.platform_tri = platform_tri
        
        # Constants
        self.sqrt3 = sqrt(3)
        self.pi = pi
        self.sin120 = self.sqrt3 / 2.0
        self.cos120 = -0.5
        self.tan60 = self.sqrt3
        self.sin30 = 0.5
        self.tan30 = 1.0 / self.sqrt3

    def forward_kinematics(self, thetaA, thetaB, thetaC):
        t = (self.base_tri - self.platform_tri) * self.tan30 / 2.0
        dtr = self.pi / 180.0
        
        thetaA *= dtr
        thetaB *= dtr
        thetaC *= dtr
        
        x1 = 0.0
        y1 = -(t + self.arm_length * cos(thetaA))
        z1 = -self.arm_length * sin(thetaA)
        
        y2 = (t + self.arm_length * cos(thetaB)) * self.sin30
        x2 = y2 * self.tan60
        z2 = -self.arm_length * sin(thetaB)
        
        y3 = (t + self.arm_length * cos(thetaC)) * self.sin30
        x3 = -y3 * self.tan60
        z3 = -self.arm_length * sin(thetaC)
        
        dnm = (y2 - y1) * x3 - (y3 - y1) * x2
        if abs(dnm) < 1e-6:
            return None  # Avoid division by zero

        a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1)
        b1 = -((x2 * x2 + y2 * y2 + z2 * z2) - (x1 * x1 + y1 * y1 + z1 * z1)) / 2.0
        
        a2 = -(z2 - z1) * x3 + (z3 - z1) * x2
        b2 = ((x2 * x2 + y2 * y2 + z2 * z2) - (x3 * x3 + y3 * y3 + z3 * z3)) / 2.0

        a = a1 * a1 + a2 * a2 + dnm * dnm
        b = 2 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm)
        c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - self.rod_length * self.rod_length)
        
        discriminant = b * b - 4 * a * c
        if discriminant < 0:
            return None  # No real solutions
        
        z = (-b - sqrt(discriminant)) / (2 * a)
        x = (a1 * z + b1) / dnm
        y = (a2 * z + b2) / dnm

        return x, y, z

    def plot_workspace_interactive(self):
        theta_range = np.linspace(-90, 90, 60)
        points = []
        for thetaA in theta_range:
            for thetaB in theta_range:
                for thetaC in theta_range:
                    result = self.forward_kinematics(thetaA, thetaB, thetaC)
                    if result is not None:
                        points.append(result)
        
        points = np.array(points)

        fig = make_subplots(rows=1, cols=2, specs=[[{'type': 'scatter3d'}, {'type': 'xy'}]], subplot_titles=('3D View', 'Top View at different Z'))
        
        # 3D scatter plot with color scale
        scatter3d = go.Scatter3d(x=points[:, 0], y=points[:, 1], z=points[:, 2], mode='markers', marker=dict(size=3, color=points[:, 2], colorscale='Viridis', showscale=True))
        fig.add_trace(scatter3d, row=1, col=1)

        # Add traces for top views at different z-values, initially invisible
        z_values = np.linspace(points[:, 2].min(), points[:, 2].max(), 30)
        for z_value in z_values:
            z_cut_points = points[np.isclose(points[:, 2], z_value, atol=10)]
            scatter = go.Scatter(x=z_cut_points[:, 0], y=z_cut_points[:, 1], mode='markers', marker=dict(size=5, color='blue'), visible=False)
            fig.add_trace(scatter, row=1, col=2)

        # Create and add slider
        steps = []
        for i, z_value in enumerate(z_values):
            step = dict(
                method="update",
                args=[{"visible": [False] * len(fig.data)}],
                label=f"{z_value:.2f}"
            )
            step["args"][0]["visible"][0] = True  # Always visible 3D scatter
            step["args"][0]["visible"][i + 1] = True  # Make corresponding top view visible
            steps.append(step)
        
        sliders = [dict(
            active=10,
            currentvalue={"prefix": "Z height: "},
            steps=steps
        )]
        
        fig.update_layout(sliders=sliders)
        fig.show()
        fig.write_html('delta_kinematics_workspace_interactive.html')  # Save the plot as an HTML file

# Example usage
dk = DeltaKinematics(232, 336, 119, 120)
dk.plot_workspace_interactive()