sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-19 09:12:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add files via upload

Browse Source 
Added calculation for dg for "nonlcon" function; Added diff_g function to calculate dg_dz
This commit is contained in:
ppongsa
2021-11-28 12:24:44 -05:00
committed by GitHub
parent 1a20a1f4a3
commit eed2424543
1 changed files with 22 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
Main_Peter.m
View File

@@ -77,6 +77,25 @@ function cp = ontrack(pose, bl_prev, bl_next, br_prev, br_next)
cp = c1*c2; cp = c1*c2;
end end
function dg = diff_g(pose, bl_prev, bl_next, br_prev, br_next)
% syms Lx Ly Rx Ry ly lx rx ry X Y;
% g = (Lx*(Y - ly)-Ly*(X-lx))*(Rx*(Y - ry)-Ry*(X-rx));
% diff(g,X);
% diff(g,Y);
dg = [0,0];
l_vec = bl_next - bl_prev; % l = l_i+1 - l_i-1
r_vec = br_next - br_prev; % r = r_i+1 - r_i-1
pl = pose - bl_prev; % p_l = p - l_i-1
pr = pose - br_prev; % p_r = p - r_i-1
dg(1) = r_vec(2)*(l_vec(2)*pl(1) - l_vec(1)*pl(2)) + ...
l_vec(2)*(r_vec(2)*pr(1) - r_vec(1)*pr(2));
dg(2) = - r_vec(1)*(l_vec(2)*pl(1) - l_vec(1)*pl(2)) - ...
l_vec(1)*(r_vec(2)*pr(1) - r_vec(1)*pr(2));
end
function [lb, ub] = bounds(TestTrack, StepsPerPoint) function [lb, ub] = bounds(TestTrack, StepsPerPoint)
[m,nPts] = size(TestTrack.cline); [m,nPts] = size(TestTrack.cline);
@@ -165,6 +184,9 @@ function [g,h,dg,dh]=nonlcon(z, TestTrack)
g = ontrack(Pinit, bl_vec(:,1), bl_vec(:,2), br_vec(:,1), br_vec(:,2)); g = ontrack(Pinit, bl_vec(:,1), bl_vec(:,2), br_vec(:,1), br_vec(:,2));
dgi_dzj = diff_g(pose, bl_prev, bl_next, br_prev, br_next);
dg(i,numState*i-numState+1) = dgi_dzj(1);
dg(i,numState*i-numState+2) = dgi_dzj(2);
% if i==1 % if i==1
% h(1:3) = z(1:3,:) ; % h(1:3) = z(1:3,:) ;
@@ -175,8 +197,6 @@ function [g,h,dg,dh]=nonlcon(z, TestTrack)
% dh(3*i-2:3*i,3*i-5:3*i) = [-eye(3)-dt*statepart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)),eye(3)] ; % dh(3*i-2:3*i,3*i-5:3*i) = [-eye(3)-dt*statepart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)),eye(3)] ;
% dh(3*i-2:3*i,3*nsteps+2*i-3:3*nsteps+2*i-2) = -dt*inputpart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)) ; % dh(3*i-2:3*i,3*nsteps+2*i-3:3*nsteps+2*i-2) = -dt*inputpart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)) ;
% end % end
% dg(i,3*i-2:3*i-1) = -2*[z(3*i-2)-3.5, z(3*i-1)+0.5] ;
end end
end end