Trajectory optimization using direct collocation

Note

Type: Trajectory Optimization

State/action space constraints: Yes

Optimal: Yes

Versatility: Swingup and stabilization

Theory

Direct collocation is an approach from collocation methods , which transforms the optimal control problem into a mathematical programming problem. The numerical solution can be achieved directly by solving the new problem using sequential quadratic programming [1] [2]. The formulation of the optimization problem at the collocation points is as follows:

$$\begin{array}{rl}\underset{\mathbf{x}[.],u[.]}{min}\sum _{n_0}^{N-1}& h_{n}l(u[n])\\ \text{s.t.}& \dot{\mathbf{x}}(t_{c,n})=f(\mathbf{x}(t_{c,n}),u(t_{c,n})),\mathrm{\forall }n\in [0,N-1]\\ & |u|\le u_{max}\\ & \mathbf{x}[0]={\mathbf{x}}_0\\ & \mathbf{x}[N]={\mathbf{x}}_F\end{array}$$

• $\mathbf{x}$ = $[\theta (.),\dot{\theta }(.){]}^T$>: Angular position and velocity are the states of the system

• u: Input torque of the system applied by motor

• N = 21: Number of break points in the trajectory

• $h_k=t_k-t_k$: Time interval between two breaking points

• $l(u)=u^{T}Ru$: Running cost

• R = 10: Input weight

• $\dot{\mathbf{x}}(t_{c,n})$: Nonlinear dynamics of the pendulum considered as equality constraint at collocation point

• $t_{c,k}=\frac{1}{2}\left(t_k{t}_{k1}\right)$: A collocation point at time instant $k,(i.e.,\mathbf{x}[k]=\mathbf{x}(t_k))$,in which the collocation constraints depends on the decision variables $\mathbf{x}[k],\mathbf{x}[k2,B1],u[k],u[k2,B1]$

• $u_{max}=10$: Maximum torque limit

• ${\mathbf{x}}_{\mathbf{0}}\mathbf{=}\mathbf{[}\theta \mathbf{=}\mathbf{0}\mathbf{,}\dot{\theta }\mathbf{=}\mathbf{0}\mathbf{]}$: Initial state constraint

• ${\mathbf{x}}_{\mathbf{F}}\mathbf{=}\mathbf{[}\theta \mathbf{=}\pi \mathbf{,}\dot{\theta }\mathbf{=}\mathbf{0}\mathbf{]}$: Terminal state constraint

Minimum and a maximum spacing between sample times set to 0.05 and 0.5`s`. It assumes a first-order hold on the input trajectory, in which the signal is reconstructed as a piecewise linear approximation to the original sampled signal.

API

The direct collocation algorithm explained above can be executed by using the DirectCollocationCalculator class:

dircal = DirectCollocationCalculator()

To parse the pendulum parameters to the calculator, do:

dircal.init_pendulum(mass=0.5,
                     length=0.5,
                     damping=0.1,
                     gravity=9.81,
                     torque_limit=1.5)

The optimal trajectory can be computed with:

x_trajectory, dircol, result = dircal.compute_trajectory(N=21,
                                                         max_dt=0.5,
                                                         start_state=[0.0, 0.0],
                                                         goal_state=[3.14, 0.0])

The method returns three pydrake objects containing the optimization results. The trajectory as numpy arrays can be extracted from these objects with:

T, X, XD, U = dircal.extract_trajectory(x_trajectory, dircol, result, N=1000)

where T is the time, X the position, XD the velocity and U the control trajectory. The parameter N determines here how with how many steps the trajectories are sampled.

The phase space of the trajectory can be plotted with:

dircal.plot_phase_space_trajectory(x_trajectory, save_to="None")

If a string with a path to a file location is parsed with ‘save_to’ the plot is saved there.

Dependencies

The trajectory optimization using direct collocation for the pendulum swing-up is accomplished by taking advantage of Drake toolbox [3].

References

[1] Hargraves, Charles R., and Stephen W. Paris. “Direct trajectory optimization using nonlinear programming and collocation.” Journal of guidance, control, and dynamics 10.4 (1987): 338-342

[2] Russ Tedrake. Underactuated Robotics: Algorithms for Walking, Running, Swimming, Flying, and Manipulation (Course Notes for MIT 6.832).

[3] Model-Based Design and Verification for Robotics <https://drake.mit.edu/>