This assignment explores using function optimization to do trajectory optimization. I give you examples optimizing a swingup trajectory for a one link arm using Matlab, and you modify these examples to do swingup for a two link arm. The swingup problem is to swing a pendulum from the downwards stable equilibrium up to the upwards unstable equilibrium, while minimizing a tradeoff of distance to the goal state and the size of the commands.
Examples:
One link swingup based on unconstrained optimization of the history of positions:
Matlab code
One link swingup based on constrained optimization of the history of positions
and commands (torques):
Matlab code
Two link swingup adds another joint. If the base joint is the "shoulder",
the new joint is the "elbow".
In the dynamics given below, it is assumed that the elbow angle is the
relative angle between the forearm and the upper arm, with straight
down having angles shoulder=0 and elbow=0, and straight up having
angles shoulder=pi and elbow=0.
For an array of positions pos(joint,index) and commands u(joint,index),
the optimization criterion becomes:
sum_j sum_i ( state_penalty(j)*dt*(positions(j,i)-goal(j))^2 + dt*u(j,i)^2 );
j: joint index, i: time index, state_penalty = 0.1,
and the
dynamics are given by (variables not initialized are the same as the one
link case):
length = 1.0/2; mass = 1.0/2; m0 = mass; m1 = mass; l0cm = length/2; l1cm = length/2; l0 = length; l1cm_m1 = l1cm*m1; l1cm_l1cm_m1 = l1cm*l1cm_m1; l0_l1cm_m1 = l0*l1cm_m1; I0yy = mass*(length*length + width*width)/12; I1yy = mass*(length*length + width*width)/12; % applying the dynamics constraint in pend2-x-u % a0 is an array of joint angles (shoulder) % a1 is an array of joint angles (elbow) % u0 is an array of commands (shoulder) % u1 is an array of commands (elbow) idx=0; for i=1:N-1 v0 = (a0(i+1+1)-a0(i-1+1))/(2*dt); v1 = (a1(i+1+1)-a1(i-1+1))/(2*dt); acc0 = (a0(i+1+1)-2*a0(i+1)+a0(i-1+1))/(dt^2); acc1 = (a1(i+1+1)-2*a1(i+1)+a1(i-1+1))/(dt^2); sa0 = sin( a0(i+1) ); ca0 = cos( a0(i+1) ); sa1 = sin( a1(i+1) ); ca1 = cos( a1(i+1) ); l0_l1cm_m1_sa1 = l0_l1cm_m1*sa1; l0_l1cm_m1_ca1 = l0_l1cm_m1*ca1; expr1 = gravity*l1cm_m1*(ca1*sa0 + ca0*sa1); d = I1yy + l1cm_l1cm_m1; c = d + l0_l1cm_m1_ca1; f = -( expr1 + v0*v0*l0_l1cm_m1_sa1 ); b = c; a = I0yy + b + l0cm*l0cm*m0 + l0*l0*m1 + l0_l1cm_m1_ca1; e = -( expr1 + gravity*sa0*(l0cm*m0 + l0*m1) - 2*v0*v1*l0_l1cm_m1_sa1 - v1*v1*l0_l1cm_m1_sa1 ); idx=idx+1; tce(idx)= a*acc0 + b*acc1 - e - u0(i); idx=idx+1; tce(idx)= c*acc0 + d*acc1 - f - u1(i); end