Hey friend
I was watching a humanoid robot like Tesla Optimus or Figure 01 the day. It was moving its arm to pick up an object. The motion looked so smooth and natural that it almost felt like a human was doing it.
I know that behind that smooth movement is some serious math. If you just send joint angles from point A to point B the robot will jerk and shake like a cheap toy car. That is why every good humanoid robot needs something called trajectory planning.
Today I want to show you two methods: Cubic Polynomials and Minimum-Jerk Trajectories. I will explain them in a way with easy to understand code that you can try.
Why Trajectory Planning Matters
A trajectory is not about moving from one point to another. It is about how the robot moves over time including its position, velocity and acceleration at every moment.
Good trajectories give you motion, like a human. They also help the robot use energy and they are easier on the motors and gearboxes. This is especially important when the robot is walking because it needs to stay balanced and stable.
Bad trajectories make the robot look robotic. They waste battery power. They can even cause the robot to slip or fall.
1. Cubic Polynomials, A Way to Move
The simplest way to plan a trajectory is to use something called a cubic polynomial.
A cubic polynomial for position is like this:
q(t) = a₀ + a₁t + a₂t² + a₃t³
We use four numbers (a₀ to a₃) to control the starting position, ending position starting velocity and ending velocity. This gives the robot acceleration and it prevents sudden jerks.
Here is a simple Python example for moving one joint:
import numpy as np
import matplotlib.pyplot as plt
def cubic_trajectory(t_start, t_end, q_start, q_end, v_start=0.0, v_end=0.0):
T = t_end - t_start
t = np.linspace(0, T, 100)
# Cubic coefficients
a0 = q_start
a1 = v_start
a2 = (3*(q_end - q_start) - 2*v_start*T - v_end*T) / T2
a3 = (2*(q_start - q_end) + v_start*T + v_end*T) / T3
position = a0 + a1*t + a2*t2 + a3*t3
velocity = a1 + 2*a2*t + 3*a3*t2
acceleration = 2*a2 + 6*a3*t
return t + t_start, position, velocity, acceleration
# Example: Move elbow from 0° to 90° in 2 seconds
t, pos, vel, acc = cubic_trajectory(0, 2, 0, 90, v_start=0, v_end=0)
plt.figure(figsize=(10, 6))
plt.subplot(3,1,1); plt.plot(t, pos); plt.ylabel('Position (deg)')
plt.subplot(3,1,2); plt.plot(t, vel); plt.ylabel('Velocity (deg/s)')
plt.subplot(3,1,3); plt.plot(t, acc); plt.ylabel('Acceleration (deg/s²)')
plt.xlabel('Time (s)')
plt.suptitle('Cubic Polynomial Trajectory')
plt.tight_layout()
plt.show()This kind of trajectory is used in many things from simple arm movements in Tesla Optimus to basic walking motions in Figure 01.
2. Minimum-Jerk Trajectories, The Most Human-Like Motion
Humans move in a way that’s smooth and comfortable. Our brains seem to optimize for this kind of motion.
The minimum-jerk trajectory is a way to plan a motion that’s even smoother than the cubic polynomial. It is like a 5th-order polynomial. It sets the starting and ending acceleration to zero. This produces smooth movements with zero initial and final jerk.
Minimum-jerk trajectories are especially popular for things like arm movements and walking. They require solving for six numbers. The result feels noticeably more natural than cubic trajectories.
In modern humanoid robots including research versions of Figure 01 and Tesla Optimus engineers combine cubic polynomials for foot placement with minimum-jerk for the upper body and swing leg.
Practical Tips for Humanoid Robots
- Use polynomials for simple point-to-point joint movements and basic walking patterns.
- Use minimum-jerk trajectories when you want the motion to feel human-like and gentle.
- Always match velocity and acceleration at trajectory boundaries. You will get sudden jerks when switching between segments.
- For walking plan the swing foot trajectory using minimum-jerk in space then convert to joint space with inverse kinematics.
- Combine trajectory planning with PID control and ZMP checking for stable smooth walking.
My Personal Take
Trajectory planning is one of those things that doesn’t get attention in flashy robot videos but it makes a huge difference in how natural a robot feels.
When I see Tesla Optimus reach for an object with a gentle arc instead of a straight line I know good trajectory planning is, at work. The same goes for the swing leg during walking. A planned minimum-jerk trajectory makes the gait look confident and energy-efficient.
Understanding cubic and minimum-jerk trajectories also helps you appreciate why we care much about compliant actuators, good torque-speed curves and Lagrangian dynamics. All these pieces work together to turn trajectories into beautiful physical motion.