function [f,df] = tumbler_grad(x,oc,t,xt0,xtf,init_pos, init_vel,obs) % globals: global delta_t; global Xg; global order; global start_const; global end_const; global Xpass; global Xvel; global Tspan; % no inequality constraints: dg = 0*x'; % do this so we can still get xf even in we stop the proceedure with cntl-c global xf; xf = x; % we need to take the free parameters contained in x and the information % contained in xt0 and xtf and construct a spline. m = length(xt0); temp = []; for i=1:m:length(x)-m+1 temp = [temp; x(i:i+m-1)]; end temp = temp'; % xa contains the full matrix of spline multipliers which define the % active joint path m1 = xt0 * ones(1,start_const); m2 = xtf * ones(1,end_const); xa = [m1 temp m2]; pth = wlpath; pth = construct(pth,t,xa,order); nj = numjoints(oc); np = numpassive(oc); na = nj - np; num_param = length(x); v0 = zeros(2*np + 1 + num_param*(2*np + 1),1); % init_pos and init_vel contain the initial position and velocity for % the passive joints v0(1:np) = init_pos; v0(np+1:2*np) = init_vel; myopt = odeset('RelTol',1e-4,'AbsTol',1e-7,'MaxStep',0.1); [T,X] = ode45('linksimgrad',[t(1):delta_t:t(length(t))],v0, ... myopt,oc,pth,obs); n = length(T); %size(X) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % f is the objective function % df is the derivative of the objective function % % To know what the index is for the df derivative terms, can use a line % like the following: % f = -X(n,1) + 20*(X(n,2) - 0)^2 + 10*X(n,5) df = -X(n,6:12) +2*20*(X(n,2) - 0)*X(n,20:26) + 10*X(n,34:40) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % store the stuff we got in some global variables so we retain this % if the optimization is stopped prematurely Xpass = X(:,1:np); Xvel = X(:,(np+1):(2*np)); % we need velocities for later analysis Tspan = T; %Xc = X; % store the integration information in a global: Xg = X; pause(.1);