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{align*} \min_{{\bf{x}}[.],{{u}}[.]} \sum_{n_0}^{N-1}& h_{n}l({{u}}[n])\\ \textrm{s.t.} \quad & \dot{{\bf{x}}}(t_{c,n})=f({\bf{x}}(t_{c,n}),u(t_{c,n})), \hspace{0.2cm} \forall n \in [0,N-1] \\ & |u| \leq u_{max}\\ & {\bf{x}}[0] = {\bf{x}}_0\\ & {\bf{x}}[N] = {\bf{x}}_F \end{align*}

  • \(\bf{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 R u\): Running cost

  • R = 10: Input weight

  • \(\dot{\bf{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.,{\bf{x}}[k] = {\bf{x}}(t_k))\),in which the collocation constraints depends on the decision variables \({\bf{x}}[k], {\bf{x}}[k2, B1], u[k], u[k2, B1]\)

  • \(u_{max} = 10\): Maximum torque limit

  • \(\bf{x}_0 = [\theta = 0, \dot\theta = 0]\): Initial state constraint

  • \(\bf{x}_F = [\theta = \pi,\dot\theta = 0]\): 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/>