A visual teaching tool for understanding 3D poses, motion through waypoints, and the difference between vector accumulation and dot-product style projection reasoning.
import numpy as np
import plotly.graph_objects as go
from scipy.spatial.transform import Rotation as R, Slerp
# ============================================================================
# Core Math Functions
# ============================================================================
def orthonormalize(R_mat: np.ndarray) -> np.ndarray:
"""Force R_mat to be a proper orthonormal rotation via SVD."""
U, _, Vt = np.linalg.svd(R_mat)
R_ortho = U @ Vt
if np.linalg.det(R_ortho) < 0:
U[:, -1] *= -1
R_ortho = U @ Vt
return R_ortho
def make_pose(R_mat: np.ndarray, t_vec: np.ndarray) -> np.ndarray:
"""Create a 4x4 pose from rotation and translation."""
pose = np.eye(4, dtype=float)
pose[:3, :3] = orthonormalize(np.asarray(R_mat, dtype=float))
pose[:3, 3] = np.asarray(t_vec, dtype=float)
return pose
def interpolate_poses_slerp(poseA: np.ndarray, poseB: np.ndarray, n_frames: int = 30) -> list:
"""Interpolate n_frames poses from poseA to poseB using SLERP."""
if n_frames < 2:
raise ValueError("n_frames must be at least 2.")
poseA = np.array(poseA, dtype=float, copy=True)
poseB = np.array(poseB, dtype=float, copy=True)
poseA[:3, :3] = orthonormalize(poseA[:3, :3])
poseB[:3, :3] = orthonormalize(poseB[:3, :3])
R_A = poseA[:3, :3]
R_B = poseB[:3, :3]
tA = poseA[:3, 3]
tB = poseB[:3, 3]
rA = R.from_matrix(R_A)
rB = R.from_matrix(R_B)
slerp = Slerp([0, 1], R.from_quat([rA.as_quat(), rB.as_quat()]))
poses = []
for i in range(n_frames):
alpha = i / (n_frames - 1)
t_i = (1 - alpha) * tA + alpha * tB
R_i = slerp([alpha]).as_matrix()[0]
poses.append(make_pose(R_i, t_i))
return poses
# ============================================================================
# Trace Building Functions
# ============================================================================
def create_origin_sphere(radius=0.25, resolution=20, color="white"):
"""Create a sphere at the origin."""
phi = np.linspace(0, np.pi, resolution)
theta = np.linspace(0, 2 * np.pi, resolution)
phi, theta = np.meshgrid(phi, theta)
x = radius * np.sin(phi) * np.cos(theta)
y = radius * np.sin(phi) * np.sin(theta)
z = radius * np.cos(phi)
return go.Surface(
x=x,
y=y,
z=z,
surfacecolor=np.zeros_like(x),
colorscale=[[0, color], [1, color]],
cmin=0,
cmax=1,
showscale=False,
opacity=1.0,
hoverinfo="skip",
showlegend=False,
)
def create_pose_traces(pose: np.ndarray, axis_length=2.0):
"""Create coordinate frame traces (3 lines + 3 cones) for a pose."""
R_mat = orthonormalize(pose[:3, :3])
t_vec = pose[:3, 3]
colors = ["red", "green", "blue"]
traces = []
for i, color in enumerate(colors):
axis_end = t_vec + R_mat[:, i] * axis_length
traces.append(
go.Scatter3d(
x=[t_vec[0], axis_end[0]],
y=[t_vec[1], axis_end[1]],
z=[t_vec[2], axis_end[2]],
mode="lines",
line=dict(color=color, width=5),
showlegend=False,
hoverinfo="skip",
)
)
traces.append(
go.Cone(
x=[axis_end[0]],
y=[axis_end[1]],
z=[axis_end[2]],
u=[R_mat[0, i]],
v=[R_mat[1, i]],
w=[R_mat[2, i]],
anchor="tip",
showscale=False,
colorscale=[[0, color], [1, color]],
sizemode="absolute",
sizeref=0.2 * axis_length,
hoverinfo="skip",
showlegend=False,
)
)
return traces
def create_arrow_traces(poseA: np.ndarray, poseB: np.ndarray, arrow_color="cyan", label_text=""):
"""Create arrow traces (line + cone) between two poses."""
tA = poseA[:3, 3]
tB = poseB[:3, 3]
d = tB - tA
length = np.linalg.norm(d) or 1e-9
traces = [
go.Scatter3d(
x=[tA[0], tB[0]],
y=[tA[1], tB[1]],
z=[tA[2], tB[2]],
mode="lines",
line=dict(color=arrow_color, width=5),
hoverinfo="skip",
showlegend=False,
),
go.Cone(
x=[tB[0]],
y=[tB[1]],
z=[tB[2]],
u=[d[0]],
v=[d[1]],
w=[d[2]],
anchor="tip",
showscale=False,
colorscale=[[0, arrow_color], [1, arrow_color]],
sizemode="absolute",
sizeref=0.2 * length,
hoverinfo="skip",
showlegend=False,
),
]
if label_text:
midpoint = (tA + tB) / 2
traces.append(
go.Scatter3d(
x=[midpoint[0]],
y=[midpoint[1]],
z=[midpoint[2]],
mode="text",
text=[label_text],
textposition="top center",
textfont=dict(size=12, color=arrow_color),
hoverinfo="skip",
showlegend=False,
)
)
return traces
# ============================================================================
# Main Animation Builder
# ============================================================================
def build_pose_animation(poses, frames_per_segment=30):
"""
Build an animated 3D path showing waypoints and segment progression.
Args:
poses: List of 4x4 pose matrices
frames_per_segment: Number of interpolation frames per segment
Returns:
Plotly Figure with animation
"""
if len(poses) < 2:
raise ValueError("At least 2 poses needed.")
# Normalize all poses
poses = [np.array(p, dtype=float, copy=True) for p in poses]
for i in range(len(poses)):
poses[i][:3, :3] = orthonormalize(poses[i][:3, :3])
frames = []
completed_arrows = []
frame_count = 0
# Initial frame: show poses[0], poses[1], and first arrow
initial_data = []
initial_data.extend(create_pose_traces(poses[0], 2.0))
if len(poses) > 1:
initial_data.extend(create_pose_traces(poses[1], 2.0))
# Vector addition: cyan arrow from origin showing accumulated displacement
initial_data.extend(create_arrow_traces(poses[0], poses[1], arrow_color="#00D9FF", label_text="(1→2)"))
# Placeholder arrows for future poses
for s in range(2, len(poses)):
initial_data.extend(create_arrow_traces(poses[s - 1], poses[s - 1]))
# Dot product visualization: gold/orange arrow from origin
initial_data.extend(create_arrow_traces(poses[0], poses[0], arrow_color="#FFB923"))
# Build frames for each segment
for seg_i in range(1, len(poses)):
oldp = poses[seg_i - 1]
newp = poses[seg_i]
partial_poses = interpolate_poses_slerp(oldp, newp, n_frames=frames_per_segment)
# Partial frames (all but the last interpolated pose)
for fidx in range(frames_per_segment - 1):
frac = partial_poses[fidx]
partial_label = f"({seg_i}->{seg_i + 1})" if fidx > 0 else ""
partial_origin = f"(1->{seg_i + 1})" if fidx > 0 else ""
fdata = []
fdata.extend(create_pose_traces(poses[0], 2.0))
fdata.extend(create_pose_traces(frac, 2.0))
fdata.extend(completed_arrows)
# Vector addition: cyan from origin to current position
fdata.extend(create_arrow_traces(poses[0], frac, arrow_color="#00D9FF", label_text=partial_origin))
# Segment step: lime green for current segment increment
fdata.extend(create_arrow_traces(oldp, frac, arrow_color="#39FF14", label_text=partial_label))
# Placeholder arrows for future poses
for j in range(seg_i + 1, len(poses)):
fdata.extend(create_arrow_traces(poses[j - 1], poses[j - 1]))
# Dot product visualization: gold/orange
fdata.extend(create_arrow_traces(poses[0], poses[0], arrow_color="#FFB923"))
frames.append(go.Frame(name=f"frame_{frame_count}", data=fdata))
frame_count += 1
# Final frame for this segment
final_fdata = []
final_fdata.extend(create_pose_traces(poses[0], 2.0))
final_fdata.extend(create_pose_traces(newp, 2.0))
final_fdata.extend(completed_arrows)
# Vector addition: cyan from origin to current position
final_fdata.extend(create_arrow_traces(poses[0], newp, arrow_color="#00D9FF", label_text=f"(1->{seg_i + 1})"))
# Segment step: lime green
final_fdata.extend(create_arrow_traces(oldp, newp, arrow_color="#39FF14", label_text=f"({seg_i}->{seg_i + 1})"))
# Placeholder arrows for future poses
for j in range(seg_i + 1, len(poses)):
final_fdata.extend(create_arrow_traces(poses[j - 1], poses[j - 1]))
# Dot product visualization: gold/orange
final_fdata.extend(create_arrow_traces(poses[0], poses[0], arrow_color="#FFB923"))
frames.append(go.Frame(name=f"frame_{frame_count}", data=final_fdata))
frame_count += 1
# Add this segment's arrow to completed list
completed_arrows.extend(
create_arrow_traces(oldp, newp, arrow_color="#39FF14", label_text=f"({seg_i}->{seg_i + 1})")
)
# Build slider steps
steps = []
for i in range(frame_count):
steps.append(
dict(
method="animate",
label=str(i),
args=[[f"frame_{i}"], dict(mode="immediate", frame=dict(duration=0, redraw=True))],
)
)
# Build figure and layout
fig = go.Figure(data=initial_data, frames=frames)
fig.update_layout(
width=1000,
height=800,
margin=dict(l=0, r=0, b=0, t=0),
scene=dict(
xaxis=dict(range=[-6, 6], autorange=False),
yaxis=dict(range=[-6, 6], autorange=False),
zaxis=dict(range=[-1, 8], autorange=False),
aspectmode="cube",
camera=dict(center=dict(x=0, y=0, z=0), eye=dict(x=1.2, y=1.2, z=0.8)),
bgcolor="rgba(20, 20, 30, 0.95)",
),
paper_bgcolor="rgb(20, 20, 30)",
plot_bgcolor="rgb(20, 20, 30)",
updatemenus=[
dict(
type="buttons",
showactive=False,
x=0.05,
y=1.15,
buttons=[
dict(
label="Play",
method="animate",
args=[
[f"frame_{k}" for k in range(frame_count)],
dict(
frame=dict(duration=100, redraw=True),
transition=dict(duration=0),
fromcurrent=True,
mode="immediate",
),
],
),
dict(
label="Pause",
method="animate",
args=[
[None],
dict(
frame=dict(duration=0, redraw=False),
transition=dict(duration=0),
mode="immediate",
),
],
)
],
)
],
sliders=[
dict(
active=0,
currentvalue={"prefix": "Frame: "},
pad={"b": 10, "t": 50},
steps=steps,
)
],
)
return fig
# ============================================================================
# Interesting Path: Helix with Rotation
# Creates a spiral path that demonstrates both spatial movement and orientation
# ============================================================================
def build_helix_poses(n_points=8, height=6.0, radius=3.0, rotations=2.0):
"""
Create waypoints along a helix with rotating coordinate frames.
Args:
n_points: Number of waypoints
height: Total height of helix
radius: Radius of helix spiral
rotations: Number of full rotations around the helix
"""
poses = []
angles = np.linspace(0, 2 * np.pi * rotations, n_points)
z_vals = np.linspace(0, height, n_points)
for angle, z in zip(angles, z_vals):
# Position on helix
x = radius * np.cos(angle)
y = radius * np.sin(angle)
# Rotate frame to follow the spiral tangent
tangent_angle = angle + np.pi / 2 # Perpendicular to radius
# Create rotation matrix that follows the helix
R_z = R.from_euler('z', np.degrees(angle)).as_matrix()
R_x = R.from_euler('x', np.degrees(angle * 0.3)).as_matrix() # Twist
rotation = R_z @ R_x
pose = make_pose(rotation, np.array([x, y, z]))
poses.append(pose)
return poses
# ============================================================================
# Demo: Helix path showing vector addition and projection
# ============================================================================
poses = build_helix_poses(n_points=10, height=6.0, radius=3.0, rotations=2.0)
fig = build_pose_animation(poses, frames_per_segment=25)
fig.update_layout(
scene=dict(
xaxis=dict(
showgrid=False,
zeroline=False,
showline=False,
showticklabels=False,
showbackground=False,
title=''
),
yaxis=dict(
showgrid=False,
zeroline=False,
showline=False,
showticklabels=False,
showbackground=False,
title=''
),
zaxis=dict(
showgrid=False,
zeroline=False,
showline=False,
showticklabels=False,
showbackground=False,
title=''
)
)
)
fig.update_layout(
scene=dict(
xaxis=dict(showspikes=False),
yaxis=dict(showspikes=False),
zaxis=dict(showspikes=False)
)
)
fig.show()We built a Plotly-based 3D animation system that:
- Interpolates smoothly between 4x4 poses
- Uses SLERP for rotation and linear interpolation for translation
- Renders each pose as a coordinate frame with axis arrows
- Animates travel across a multi-point spatial path
- Preserves tracing arrows so motion history stays visible
- Uses button-based playback controls (Play and Pause) with a frame slider
This is a compact pose animation engine for:
- Education and intuition building in spatial math
- Visualizing rigid transforms and orientation change
- Demonstrating path traversal in 3D
- Explaining how local segment motion differs from global displacement
Each rotation matrix is orthonormalized with SVD before use.
This keeps frames physically valid and avoids drift from imperfect matrices.
For each pair of poses:
- Rotation is interpolated with SLERP
- Translation is interpolated linearly
- Partial frames are generated plus a final frame per segment
Each animation frame is built from:
- The base/reference pose frame
- The current interpolated pose frame
- Completed segment arrows (history)
- Current segment arrow
- Root-to-current arrow from the first pose
- Optional placeholder traces for future segments for visual continuity
Controls are button-driven:
- Play runs in immediate mode
- Pause explicitly halts animation
- Zero transition easing prevents residual drift
Color mapping is intentional:
- Cyan: cumulative/root displacement vector
- Lime green: segment step vectors and path progression
- Gold/orange: projection and dot-product concept channel
- Red/green/blue: local coordinate axes
The dark scene theme improves contrast and focus on vector geometry.
This helps students and teams understand:
- How poses move and rotate through space
- How interpolation changes orientation over time
- How segment-by-segment movement differs from overall displacement
- How transform math maps to visible geometry
- How path history reveals structure in motion
Useful in:
- Robotics and kinematics education
- Graphics and game transform teaching
- AR/VR camera path understanding
- Any curriculum involving 3D spatial reasoning
Transform math is often taught symbolically and can feel abstract.
This visualization makes it concrete:
- You see translation and rotation happen
- You compare local and global movement directly
- You build intuition faster through motion and trace persistence
- Add explicit tip-to-tail vector addition overlays
- Add true projection visuals for dot product onto selected directions
- Show dynamic magnitude, angle, and dot values as labels
- Add short-lived trails and ghost frames for temporal context
- Extract into a lightweight module with a tiny public API
- Provide path presets (helix, figure-8, sharp corners, loops)
- Add scene toggles for teaching modes (addition, projection, mixed)
- Add narrated lesson steps tied to frame ranges
- Add pause points with guided questions
- Add quiz mode for prediction before reveal
- Treat poses as entities with state and behavior
- Add checkpoints, targets, and event triggers
- Support multiple simultaneous moving agents
- Integrate with runtime input for interactive stepping
The project now has:
- Clear mathematical foundations
- Stable and visually readable animation behavior
- A simple usage pattern for rapid experimentation
- A strong base for both educational content and future engine-style extension