Trajectory planning bridges the gap between knowing where a robot should go and actually getting it there smoothly. In this lesson, we design trajectories for a warehouse mobile manipulator performing pick-and-place operations, covering polynomial interpolation, spline paths through multiple waypoints, and trapezoidal velocity profiles that respect actuator limits. #TrajectoryPlanning #MotionControl #Robotics
By the end of this lesson, you will be able to:
A warehouse mobile manipulator must pick boxes from storage shelves and place them on a conveyor belt for shipping. The robot arm (a 6-DOF articulated manipulator mounted on a mobile base) needs to move between shelf locations and the conveyor with smooth, predictable motions. Jerky trajectories cause the gripper to drop boxes, damage products, and wear out actuators prematurely. The system must handle boxes at different shelf heights, plan paths that avoid shelf edges, and maintain a cycle time under 8 seconds per pick-and-place operation.
A trajectory that commands instantaneous velocity changes requires infinite acceleration, which no real motor can produce. The result is vibration, tracking error, and mechanical stress. Smooth trajectories (with continuous velocity, and ideally continuous acceleration) let the motor controllers track the commanded path accurately, reduce energy consumption, and extend the lifespan of gears and bearings. In a warehouse setting, a single dropped box can halt an entire packing line, so smoothness directly affects throughput and reliability.
There are two fundamental approaches to planning a robot trajectory. Joint-space planning interpolates directly between joint angle vectors, while task-space planning interpolates the end-effector pose (position and orientation) in Cartesian coordinates. Each approach has distinct advantages and trade-offs.
How it works: Given start joint angles
Advantages:
Disadvantages:
Best for: Point-to-point moves where only the start and end positions matter, such as moving between widely separated waypoints in a pick-and-place cycle.
How it works: Given start pose
Advantages:
Disadvantages:
Best for: Motions where the Cartesian path is critical, such as descending vertically onto a box or following a shelf edge.
The simplest trajectory linearly interpolates between start and goal:
The velocity is constant:
The problem: velocity jumps from zero to a constant value at
A cubic polynomial provides the minimum-degree trajectory that can satisfy four boundary conditions: position and velocity at both the start and end of the motion. This gives us smooth velocity transitions with zero velocity at the endpoints, which is exactly what we need for a robot that starts and stops at rest.
The cubic polynomial trajectory is:
with velocity and acceleration:
For a rest-to-rest motion from
Applying the boundary conditions:
From
From
From
Starting from the four equations:
Substituting
From the velocity equation:
Substituting into the position equation:
And therefore:
A quintic (5th-degree) polynomial provides six boundary conditions: position, velocity, and acceleration at both endpoints. This ensures the acceleration profile is also smooth at the start and end of the motion, which reduces jerk (the derivative of acceleration) and produces even gentler motion.
For rest-to-rest motion with zero acceleration at endpoints (
In industrial applications, actuators have hard limits on maximum velocity and maximum acceleration. The trapezoidal velocity profile is the time-optimal trajectory under these constraints. It consists of three phases: constant acceleration, constant velocity (cruise), and constant deceleration. The velocity profile looks like a trapezoid when plotted against time.
Given maximum velocity
Phase 1: Acceleration (
Phase 2: Cruise (
Phase 3: Deceleration (
The timing parameters:
When a trajectory must pass through multiple via points (for example, a sequence of shelf locations), we connect consecutive pairs with cubic polynomials and enforce continuity of position, velocity, and acceleration at each via point. This produces a cubic spline trajectory.
For
With boundary conditions
When time-stamps are given but intermediate velocities are not specified, a common heuristic is to set the velocity at each interior via point to the average of the linear velocities on either side:
If the two adjacent segments have opposite signs (the joint reverses direction), the via-point velocity is set to zero.
A complete pick-and-place operation is a sequence of trajectory segments, each designed for a specific purpose. The segments are connected end-to-end with matching positions and velocities at the transition points. Here is the full phase breakdown for our warehouse manipulator.
Move to approach position Joint-space cubic polynomial from the home configuration to a position above the target box. This is a large free-space motion where the Cartesian path does not matter.
Descend to object Task-space straight-line descent (vertical) from the approach position to the grasp position. This uses Cartesian trajectory planning with IK at each step to ensure the gripper approaches the box from directly above.
Grasp Close the gripper. The arm holds position while the gripper actuates. Typical dwell time: 200-500 ms.
Lift Task-space straight-line vertical lift to a safe clearance height above the shelf. This prevents the box from clipping the shelf edge.
Transport Joint-space trajectory from the lifted position above the shelf to a position above the conveyor. This is the longest segment and should use a trapezoidal velocity profile for time-optimal motion.
Lower to placement Task-space straight-line descent to the conveyor placement position.
Release Open the gripper. Dwell time: 200-500 ms.
Return home Joint-space trajectory back to the home configuration, ready for the next cycle.
In practice, stopping at every via point (zero velocity) wastes time. Via-point blending replaces the stop-and-go transitions with smooth curves that pass near (but not exactly through) the via points. The robot trades a small amount of path accuracy for significant cycle time improvement.
A common blending approach defines a blend radius
For task-space segments (descent, lift, placement), we generate a straight line in Cartesian space and convert to joint trajectories using the Jacobian from Lesson 4.
Given a desired Cartesian velocity
Or using the pseudoinverse for redundant robots:
The trajectory is generated by discretizing the Cartesian path into small steps and solving IK (or using the resolved-rate method above) at each step.
import numpy as npimport matplotlib.pyplot as plt
def cubic_trajectory(q0, qf, tf, dt=0.01): """Generate a cubic polynomial rest-to-rest trajectory.
Parameters ---------- q0 : float Start position (rad or m). qf : float End position (rad or m). tf : float Duration (s). dt : float Time step (s).
Returns ------- t : ndarray Time vector. q : ndarray Position profile. qd : ndarray Velocity profile. qdd : ndarray Acceleration profile. """ # Compute coefficients a0 = q0 a1 = 0.0 a2 = 3.0 * (qf - q0) / tf**2 a3 = -2.0 * (qf - q0) / tf**3
t = np.arange(0, tf + dt, dt) q = a0 + a1 * t + a2 * t**2 + a3 * t**3 qd = a1 + 2 * a2 * t + 3 * a3 * t**2 qdd = 2 * a2 + 6 * a3 * t
return t, q, qd, qdd
def quintic_trajectory(q0, qf, tf, dt=0.01): """Generate a quintic polynomial rest-to-rest trajectory.
Parameters ---------- q0, qf : float Start and end positions. tf : float Duration (s). dt : float Time step (s).
Returns ------- t, q, qd, qdd : ndarray Time, position, velocity, acceleration. """ a0 = q0 a1 = 0.0 a2 = 0.0 a3 = 10.0 * (qf - q0) / tf**3 a4 = -15.0 * (qf - q0) / tf**4 a5 = 6.0 * (qf - q0) / tf**5
t = np.arange(0, tf + dt, dt) q = a0 + a1*t + a2*t**2 + a3*t**3 + a4*t**4 + a5*t**5 qd = a1 + 2*a2*t + 3*a3*t**2 + 4*a4*t**3 + 5*a5*t**4 qdd = 2*a2 + 6*a3*t + 12*a4*t**2 + 20*a5*t**3
return t, q, qd, qdd
# === Example: Joint 1 moves from 0 to 90 degrees in 2 seconds ===q0 = 0.0qf = np.radians(90)tf = 2.0
t_c, q_c, qd_c, qdd_c = cubic_trajectory(q0, qf, tf)t_q, q_q, qd_q, qdd_q = quintic_trajectory(q0, qf, tf)
fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
# Positionaxes[0].plot(t_c, np.degrees(q_c), 'b-', label='Cubic', linewidth=2)axes[0].plot(t_q, np.degrees(q_q), 'r--', label='Quintic', linewidth=2)axes[0].set_ylabel('Position (deg)')axes[0].legend()axes[0].set_title('Cubic vs Quintic Polynomial Trajectory')axes[0].grid(True, alpha=0.3)
# Velocityaxes[1].plot(t_c, np.degrees(qd_c), 'b-', label='Cubic', linewidth=2)axes[1].plot(t_q, np.degrees(qd_q), 'r--', label='Quintic', linewidth=2)axes[1].set_ylabel('Velocity (deg/s)')axes[1].legend()axes[1].grid(True, alpha=0.3)
# Accelerationaxes[2].plot(t_c, np.degrees(qdd_c), 'b-', label='Cubic', linewidth=2)axes[2].plot(t_q, np.degrees(qdd_q), 'r--', label='Quintic', linewidth=2)axes[2].set_ylabel('Acceleration (deg/s²)')axes[2].set_xlabel('Time (s)')axes[2].legend()axes[2].grid(True, alpha=0.3)
plt.tight_layout()plt.savefig('cubic_vs_quintic_trajectory.png', dpi=150)plt.show()
print(f"Cubic - peak velocity: {np.degrees(max(qd_c)):.1f} deg/s")print(f"Quintic - peak velocity: {np.degrees(max(qd_q)):.1f} deg/s")print(f"Cubic - peak accel: {np.degrees(max(abs(qdd_c))):.1f} deg/s²")print(f"Quintic - peak accel: {np.degrees(max(abs(qdd_q))):.1f} deg/s²")import numpy as npimport matplotlib.pyplot as plt
def trapezoidal_profile(q0, qf, v_max, a_max, dt=0.001): """Generate a trapezoidal velocity profile trajectory.
Parameters ---------- q0, qf : float Start and end positions. v_max : float Maximum velocity. a_max : float Maximum acceleration (and deceleration). dt : float Time step (s).
Returns ------- t, q, qd, qdd : ndarray Time, position, velocity, acceleration arrays. """ dq = qf - q0 sign = np.sign(dq) dq = abs(dq)
# Check if we can reach v_max if dq < v_max**2 / a_max: # Triangular profile: cannot reach v_max v_peak = np.sqrt(a_max * dq) t_a = v_peak / a_max t_c = 0.0 t_d = t_a else: # Trapezoidal profile t_a = v_max / a_max t_c = dq / v_max - v_max / a_max t_d = t_a v_peak = v_max
t_f = t_a + t_c + t_d t = np.arange(0, t_f + dt, dt)
q = np.zeros_like(t) qd = np.zeros_like(t) qdd = np.zeros_like(t)
for i, ti in enumerate(t): if ti <= t_a: # Acceleration phase qdd[i] = a_max qd[i] = a_max * ti q[i] = 0.5 * a_max * ti**2 elif ti <= t_a + t_c: # Cruise phase dt_cruise = ti - t_a qdd[i] = 0.0 qd[i] = v_peak q[i] = 0.5 * a_max * t_a**2 + v_peak * dt_cruise else: # Deceleration phase dt_decel = ti - t_a - t_c qdd[i] = -a_max qd[i] = v_peak - a_max * dt_decel q[i] = (0.5 * a_max * t_a**2 + v_peak * t_c + v_peak * dt_decel - 0.5 * a_max * dt_decel**2)
# Apply direction and offset q = q0 + sign * q qd = sign * qd qdd = sign * qdd
return t, q, qd, qdd
# === Example: Joint moves 90 degrees with actuator limits ===q0 = 0.0qf = np.radians(90)v_max = np.radians(60) # 60 deg/s max velocitya_max = np.radians(120) # 120 deg/s² max acceleration
t, q, qd, qdd = trapezoidal_profile(q0, qf, v_max, a_max)
fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
axes[0].plot(t, np.degrees(q), 'b-', linewidth=2)axes[0].set_ylabel('Position (deg)')axes[0].set_title('Trapezoidal Velocity Profile Trajectory')axes[0].grid(True, alpha=0.3)
axes[1].plot(t, np.degrees(qd), 'g-', linewidth=2)axes[1].axhline(y=np.degrees(v_max), color='r', linestyle='--', alpha=0.5, label='v_max')axes[1].set_ylabel('Velocity (deg/s)')axes[1].legend()axes[1].grid(True, alpha=0.3)
axes[2].plot(t, np.degrees(qdd), 'm-', linewidth=2)axes[2].axhline(y=np.degrees(a_max), color='r', linestyle='--', alpha=0.5, label='a_max')axes[2].axhline(y=-np.degrees(a_max), color='r', linestyle='--', alpha=0.5)axes[2].set_ylabel('Acceleration (deg/s²)')axes[2].set_xlabel('Time (s)')axes[2].legend()axes[2].grid(True, alpha=0.3)
plt.tight_layout()plt.savefig('trapezoidal_profile.png', dpi=150)plt.show()
t_a_val = np.degrees(v_max) / np.degrees(a_max)print(f"Acceleration time: {t_a_val:.3f} s")print(f"Total time: {t[-1]:.3f} s")print(f"Peak velocity: {np.degrees(max(qd)):.1f} deg/s")import numpy as npimport matplotlib.pyplot as plt
def cubic_trajectory(q0, qf, tf, dt=0.01): """Cubic polynomial rest-to-rest trajectory.""" a0 = q0 a2 = 3.0 * (qf - q0) / tf**2 a3 = -2.0 * (qf - q0) / tf**3 t = np.arange(0, tf + dt, dt) q = a0 + a2 * t**2 + a3 * t**3 qd = 2 * a2 * t + 3 * a3 * t**2 return t, q, qd
def plan_pick_and_place(): """Plan a complete pick-and-place trajectory for a 3-joint planar arm.
Phases: 1. Home -> Approach (joint-space cubic) 2. Approach -> Grasp position (task-space vertical descent, simulated as joint-space) 3. Dwell for grasping 4. Grasp position -> Lift (vertical ascent) 5. Lift -> Above conveyor (joint-space cubic, transport) 6. Above conveyor -> Place position (vertical descent) 7. Dwell for release 8. Place position -> Home (joint-space cubic) """
# Joint configurations (radians) for a 3-joint planar arm # [shoulder, elbow, wrist] q_home = np.array([0.0, 0.0, 0.0]) q_approach = np.array([0.5, -0.8, 0.3]) # above the box on shelf q_grasp = np.array([0.6, -1.0, 0.4]) # at the box (descended) q_lift = np.array([0.5, -0.8, 0.3]) # lifted above shelf q_above_cv = np.array([-0.4, -0.6, 0.2]) # above conveyor q_place = np.array([-0.35, -0.75, 0.25]) # at conveyor (descended)
# Phase durations (seconds) t_move_to_approach = 1.5 t_descend = 0.6 t_grasp_dwell = 0.3 t_lift = 0.6 t_transport = 2.0 t_lower = 0.6 t_release_dwell = 0.3 t_return = 1.5
all_t = [] all_q = [[], [], []] # one list per joint all_labels = [] t_offset = 0.0
phases = [ ("Move to approach", q_home, q_approach, t_move_to_approach), ("Descend", q_approach, q_grasp, t_descend), ("Grasp (dwell)", q_grasp, q_grasp, t_grasp_dwell), ("Lift", q_grasp, q_lift, t_lift), ("Transport", q_lift, q_above_cv, t_transport), ("Lower", q_above_cv, q_place, t_lower), ("Release (dwell)", q_place, q_place, t_release_dwell), ("Return home", q_place, q_home, t_return), ]
for name, q_start, q_end, duration in phases: for j in range(3): t_seg, q_seg, _ = cubic_trajectory(q_start[j], q_end[j], duration) if len(all_t) > 0 and j == 0: # Offset time for this phase pass all_q[j].extend(q_seg + 0 if len(all_q[j]) == 0 else q_seg.tolist())
t_seg, _, _ = cubic_trajectory(0, 0, duration) all_t.extend((t_seg + t_offset).tolist()) all_labels.append((t_offset, t_offset + duration, name)) t_offset += duration
# Rebuild cleanly all_t = [] all_q = [[], [], []] t_offset = 0.0
for name, q_start, q_end, duration in phases: t_seg = np.arange(0, duration + 0.01, 0.01) for j in range(3): _, q_seg, _ = cubic_trajectory(q_start[j], q_end[j], duration) all_q[j].extend(q_seg.tolist()) all_t.extend((t_seg + t_offset).tolist()) t_offset += duration
all_t = np.array(all_t) all_q = [np.array(q) for q in all_q]
# Plot fig, axes = plt.subplots(3, 1, figsize=(14, 10), sharex=True) joint_names = ['Shoulder', 'Elbow', 'Wrist'] colors = ['#2196F3', '#FF5722', '#4CAF50']
for j in range(3): axes[j].plot(all_t, np.degrees(all_q[j]), color=colors[j], linewidth=2) axes[j].set_ylabel(f'{joint_names[j]} (deg)') axes[j].grid(True, alpha=0.3)
# Add phase annotations phase_colors = ['#E3F2FD', '#FFF3E0', '#FFEBEE', '#E8F5E9', '#F3E5F5', '#FFF3E0', '#FFEBEE', '#E3F2FD'] t_start = 0.0 for i, (name, _, _, duration) in enumerate(phases): for j in range(3): axes[j].axvspan(t_start, t_start + duration, alpha=0.15, color=phase_colors[i % len(phase_colors)]) axes[2].text(t_start + duration / 2, axes[2].get_ylim()[0] + 2, name, ha='center', va='bottom', fontsize=7, rotation=45) t_start += duration
axes[0].set_title('Complete Pick-and-Place Joint Trajectories') axes[2].set_xlabel('Time (s)')
plt.tight_layout() plt.savefig('pick_and_place_trajectory.png', dpi=150) plt.show()
total_time = sum(d for _, _, _, d in phases) print(f"Total cycle time: {total_time:.1f} s") print(f"\nPhase breakdown:") for name, _, _, duration in phases: print(f" {name}: {duration:.1f} s")
plan_pick_and_place()import numpy as npimport matplotlib.pyplot as plt
def linear_interp(q0, qf, tf, dt=0.01): """Linear interpolation (discontinuous velocity).""" t = np.arange(0, tf + dt, dt) q = q0 + (qf - q0) * t / tf qd = np.full_like(t, (qf - q0) / tf) qdd = np.zeros_like(t) # Mark discontinuities at endpoints qdd[0] = float('inf') qdd[-1] = float('inf') return t, q, qd, qdd
def cubic_interp(q0, qf, tf, dt=0.01): """Cubic polynomial (continuous velocity, discontinuous acceleration).""" a2 = 3.0 * (qf - q0) / tf**2 a3 = -2.0 * (qf - q0) / tf**3 t = np.arange(0, tf + dt, dt) q = q0 + a2 * t**2 + a3 * t**3 qd = 2 * a2 * t + 3 * a3 * t**2 qdd = 2 * a2 + 6 * a3 * t return t, q, qd, qdd
def quintic_interp(q0, qf, tf, dt=0.01): """Quintic polynomial (continuous acceleration).""" a3 = 10.0 * (qf - q0) / tf**3 a4 = -15.0 * (qf - q0) / tf**4 a5 = 6.0 * (qf - q0) / tf**5 t = np.arange(0, tf + dt, dt) q = q0 + a3 * t**3 + a4 * t**4 + a5 * t**5 qd = 3 * a3 * t**2 + 4 * a4 * t**3 + 5 * a5 * t**4 qdd = 6 * a3 * t + 12 * a4 * t**2 + 20 * a5 * t**3 return t, q, qd, qdd
# Parametersq0 = 0.0qf = np.radians(90)tf = 2.0
methods = { 'Linear': linear_interp(q0, qf, tf), 'Cubic': cubic_interp(q0, qf, tf), 'Quintic': quintic_interp(q0, qf, tf),}
fig, axes = plt.subplots(3, 1, figsize=(12, 9), sharex=True)styles = {'Linear': ('gray', '-'), 'Cubic': ('blue', '-'), 'Quintic': ('red', '--')}
for name, (t, q, qd, qdd) in methods.items(): color, ls = styles[name] axes[0].plot(t, np.degrees(q), color=color, linestyle=ls, linewidth=2, label=name) axes[1].plot(t, np.degrees(qd), color=color, linestyle=ls, linewidth=2, label=name) if name != 'Linear': axes[2].plot(t, np.degrees(qdd), color=color, linestyle=ls, linewidth=2, label=name)
axes[0].set_ylabel('Position (deg)')axes[0].set_title('Comparison of Trajectory Interpolation Methods')axes[0].legend()axes[0].grid(True, alpha=0.3)
axes[1].set_ylabel('Velocity (deg/s)')axes[1].legend()axes[1].grid(True, alpha=0.3)
axes[2].set_ylabel('Acceleration (deg/s²)')axes[2].set_xlabel('Time (s)')axes[2].legend()axes[2].grid(True, alpha=0.3)axes[2].set_title('Acceleration (Linear omitted: infinite at endpoints)')
plt.tight_layout()plt.savefig('interpolation_comparison.png', dpi=150)plt.show()
# Print summary tableprint(f"{'Method':<10} {'Peak Vel (deg/s)':<20} {'Peak Accel (deg/s²)':<22} {'Accel at t=0':<18}")print("-" * 70)for name, (t, q, qd, qdd) in methods.items(): pv = np.degrees(np.max(np.abs(qd))) if name == 'Linear': pa = "inf (discontinuous)" a0_str = "inf" else: pa = f"{np.degrees(np.max(np.abs(qdd))):.1f}" a0_str = f"{np.degrees(qdd[0]):.1f}" print(f"{name:<10} {pv:<20.1f} {pa:<22} {a0_str:<18}")We now apply the trajectory planning tools to our warehouse mobile manipulator. The robot must pick a box from a shelf at height 1.2 m and place it on a conveyor at height 0.8 m. The arm has actuator limits of 120 deg/s maximum joint velocity and 300 deg/s² maximum joint acceleration. The target cycle time is under 8 seconds.
Trajectory strategy for each phase:
| Phase | Planning Space | Method | Rationale |
|---|---|---|---|
| Move to approach | Joint space | Trapezoidal | Time-optimal, large motion |
| Descend to object | Task space | Cubic polynomial | Smooth vertical descent, short distance |
| Grasp (dwell) | None | Hold position | Gripper closes |
| Lift | Task space | Cubic polynomial | Smooth vertical lift |
| Transport | Joint space | Trapezoidal | Time-optimal, longest segment |
| Lower to placement | Task space | Cubic polynomial | Smooth vertical descent |
| Release (dwell) | None | Hold position | Gripper opens |
| Return home | Joint space | Trapezoidal | Time-optimal return |
Cycle time budget:
| Phase | Duration |
|---|---|
| Move to approach | 1.2 s |
| Descend | 0.5 s |
| Grasp dwell | 0.3 s |
| Lift | 0.5 s |
| Transport | 2.5 s |
| Lower | 0.5 s |
| Release dwell | 0.3 s |
| Return home | 1.5 s |
| Total | 7.3 s |
This is within the 8-second target. The transport phase dominates the cycle time, so optimizing this segment (higher cruise velocity, shorter acceleration time) has the greatest impact on throughput.
Several strategies can reduce cycle time below 7 seconds:
Match Method to Phase
Use trapezoidal velocity profiles for long free-space motions where time matters. Use polynomial interpolation for short, precise segments where smoothness is critical. Use task-space planning only when the Cartesian path is important (approach, descent, straight-line segments).
Always Check Actuator Limits
After generating any trajectory, compute the required joint velocities and accelerations and verify they are within the actuator ratings. For polynomial trajectories, the peak velocity and acceleration depend on the motion distance and duration. Shorter durations produce higher peaks.
Prefer Quintic for Delicate Payloads
When carrying fragile items (electronics, glass, food products), use quintic polynomials to ensure zero jerk at segment transitions. The additional computational cost is negligible compared to the cost of dropped or damaged goods.
Test Blending Radii in Simulation
Via-point blending trades path accuracy for speed. Always simulate the blended path and verify the maximum deviation from the nominal via points. For our warehouse application, a maximum deviation of 20 mm at via points is typically acceptable; for precision assembly, it may need to be under 0.1 mm.
Trajectory Planning Fundamentals
Joint-space planning is simple and singularity-free, while task-space planning provides Cartesian path control. Industrial systems combine both: joint-space for free motions, task-space for critical approach and departure segments.
Polynomial Interpolation
Cubic polynomials satisfy four boundary conditions (position and velocity at endpoints) and are the standard for smooth point-to-point motions. Quintic polynomials add acceleration continuity for even smoother transitions.
Trapezoidal Profiles
The trapezoidal velocity profile is time-optimal under velocity and acceleration constraints. It handles both the normal trapezoidal case (cruise phase present) and the degenerate triangular case (short distances).
Pick-and-Place Sequencing
A complete pick-and-place cycle is a sequence of trajectory segments, each chosen for its specific purpose. Proper phase sequencing, dwell times, and via-point blending determine the achievable cycle time and reliability.
Next lesson: We build complete Python robot simulations with 3D visualization, then connect the kinematic and trajectory planning tools from this course to real-world applications in manufacturing, logistics, medical robotics, and agricultural automation.
Comments