Project Introduction

      This project is about a Pendubot, a two-link pendulum with a motor for the first joint.

      The following image shows the Pendubot.

  System Introduction

      I set m1 = m2 = 1kg, l1 = l2 = 1m. The configuration variable X is {theta1, theta1_dot, theta2, theta2_dot}, and the control variable is u, which is the torque for the first joint.

      This project optimize and animate this system form (theta1 = Pi/2, theta2 = 0) to (theta1 = Pi, theta2 = 0).

      The following image shows the simulation procedure.

  Details

      Based on Euler-Lagrange equations (with the external force u), the dynamic functions is:

      Based on the cost function, I set Q = [[1,0,0,0],[0,0,0,0],[0,0,1,0],[0,0,0,0]], R = [[0.02]], P1 = [[0.01,0,0,0], [0,0,0,0], [0,0,0.01,0], [0,0,0,0]].

      Initial condition is X[0] = [Pi/2, 0, 0, 0], and desired configuration is theta1 = Pi and theta2 = 0. So the cost function becomes:

      For projection, I set Pr1 = P1, Qr = Q and Rr = R.

  Optimization

      T = 6.3s and final error is 0.00066.