import numpy as np
from roboticstoolbox.tools.trajectory import mstraj

# Define via points (each row is a joint configuration)
viapoints = np.array([
    [0, 0, 0],         # Start
    [0.5, 0.2, -0.1],  # Intermediate
    [1.0, 0.4, 0.2]    # End
])

# Time step
dt = 0.01  # seconds

# Acceleration time
tacc = 0.2  # seconds

# Maximum joint velocity per joint (same length as number of joints)
qdmax = [0.5, 0.3, 0.4]  # radians per second

# Optional: starting position (otherwise uses first viapoint)
q0 = viapoints[0]

# Generate the trajectory
traj = mstraj(
    viapoints=viapoints,
    dt=dt,
    tacc=tacc,
    qdmax=qdmax,
)

# Extract trajectory
time = traj.t          # Time vector
positions = traj.q     # Joint angles (shape: K x N)

print("Time vector:", time)
print("Joint positions:\n", positions)