Replies: 1 comment
-
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() |
Beta Was this translation helpful? Give feedback.
0 replies
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
-
Reachable
Beta Was this translation helpful? Give feedback.
All reactions