diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..5d521fa Binary files /dev/null and b/.DS_Store differ diff --git a/Team1/.DS_Store b/Team1/.DS_Store new file mode 100644 index 0000000..4377fd4 Binary files /dev/null and b/Team1/.DS_Store differ diff --git a/Team2/.DS_Store b/Team2/.DS_Store new file mode 100644 index 0000000..5008ddf Binary files /dev/null and b/Team2/.DS_Store differ diff --git a/Team2/SimulationVideo.mp4 b/Team2/SimulationVideo.mp4 new file mode 100644 index 0000000..2934b26 Binary files /dev/null and b/Team2/SimulationVideo.mp4 differ diff --git a/Team2/swarm.m b/Team2/swarm.m new file mode 100644 index 0000000..c2daba2 --- /dev/null +++ b/Team2/swarm.m @@ -0,0 +1,522 @@ +function robotSwarmFormation() +% Constants +global N dT KU_COEFF KS1_COEFF KS2_COEFF KT_COEFF + +N = 8; % number of bots +dT = .01; % timestep length (position is updated each step) + +% Control gains - This is taken from the 2004 paper. These are the gains +% for the individual abstract state variable used in the simulation of the +% paper +KU_COEFF = [2 0; 0 2]; +KS2_COEFF = 2; +KT_COEFF = 2; + +% Counter to accumulate the values of the data after each time interval +PLOT_COUNTER = 1; + + +%%%%%%%%%%%%% +% Main loop % +%%%%%%%%%%%%% +%outpath = '/Users/username/Desktop' ; +outputVideo = VideoWriter(fullfile(outpath,'SimulationVideo.mp4'),'MPEG-4'); +open(outputVideo); + +% Place the robot swarm in the space +bots = distributeBots(N); + +% Time and posPlot to accumulate values +Time(1) = 0; +KS1_COEFF = 2; +posPlot(N,1) = struct; + +for i=1:N + posPlot(i).qx(1) = bots(i).q(1) ; + posPlot(i).qy(1) = bots(i).q(2) ; + posPlot(i).uStarX(1) = 0 ; + posPlot(i).uStarY(1) = 0 ; + posPlot(i).uCapX(1) = bots(i).u(1) ; + posPlot(i).uCapY(1) = bots(i).u(2) ; +end + +PLOT_COUNTER = PLOT_COUNTER + 1; + +%Initialize the abstract space +uCentroid = [0; 0]; +theta = 0; +s1 = 0; +s2 = 0; + +%Initialize the matrices to calculate shape variables +E1 = [0 1; 1 0]; +E2 = [1 0; 0 -1]; +E3 = [0 -1; 1 0]; + + +%Initializing the desired position variables +uCentroidD = [9 ; 17]; +s1D = 0.25; +s2D = 0.15; +thetaD = 0.9 ; + +%Distance between bots - Taken from simulation of the current research +%paper +botRadius = 0.15; +botAxleLength = 0.1; +safeDist = 0.1 ; +sepDist = 2*(botRadius + botAxleLength) + safeDist; + +%Initialize the configuration +[uCentroid,theta,s1,s2] = abstractSpace(bots); + +figure; +drawElipseBoundary(bots,uCentroid,theta,s1,s2,uCentroidD,thetaD,s1D,s2D,botRadius, botAxleLength); +%Create a figure handle which we like to capture as a movie +figure; +F = getframe(gcf); + +%Initial position of the robots with the elipse +drawElipseBoundary(bots,uCentroid,theta,s1,s2,uCentroidD,thetaD,s1D,s2D,botRadius, botAxleLength); +writeVideo(outputVideo,getframe(gcf)); + +%Initialize plot variables +uPlotx(1) = uCentroidD(1) - uCentroid(1) ; +uPloty(1) = uCentroidD(1) - uCentroid(2) ; +thetaPlot(1) = thetaD - theta ; +s1Plot(1) = s1D - s1; +s2Plot(1) = s2D - s2; + +% This variable represents the complete state of the system +xPlot(1) = norm([uPlotx(1) ; uPloty(1) ; thetaPlot(1) ; s1Plot(1) ; s2Plot(1)],2); + +% step counter for every intervals of time +stepCounter = 0; +keepLooping = true; + +%Values to contain the final velocity calculated from the convex +%optimization problem +uxMax = [0.1 ; 0.1]; +kVel = [0 ; 0]; +nVel = 0; + +%Values to contain the min energy control law velocity. This is in +%accordance with the paper where the min energy control law velocity is +%having magnitude maximum of 0.05. +uxMaxCtrlLaw = 0.05; + +conditionFailure = 0 ; + +while (true == keepLooping && (350*1/dT >= stepCounter)) + + % Calculating the abstract state variables + [uCentroid,theta,s1,s2] = abstractSpace(bots); + + R = [cos(theta) -sin(theta); sin(theta) cos(theta)]; + H1 = eye(2) + R^2*E2; + H2 = eye(2) - R^2*E2; + H3 = R^2*E1 ; + g = [[R uCentroid];0 0 1]; + + %Check if the desired formation has been reached + if(isequal(round((uCentroidD(1) - uCentroid(1)),3),0) && isequal(round((uCentroidD(2) - uCentroid(2)),3),0) && isequal(round((s1D-s1),3),0) && isequal(round((s2D-s2),3),0) && isequal(round((thetaD-theta),4),0)) + break; + end + + %Calculate the error gains for each of the output + dCentroid = KU_COEFF*(uCentroidD - uCentroid); + dTheta = KT_COEFF*(thetaD - theta); + dS1 = KS1_COEFF*(s1D - s1); + dS2 = KS2_COEFF*(s2D - s2); + + for robotInd1=1:N + position = bots(robotInd1).q.'; % Current position of the robot + %Calculation of velocity using min energy control law + velocity = dCentroid + ((s1-s2)*H3*(position - uCentroid)*dTheta / (s1+s2)).... + + (H1*(position - uCentroid)*dS1 / 4*s1) + (H2*(position - uCentroid)*dS2/4*s2); + % ui* + + %Scaling the values of the min energy ctrl velocity to 0.05ms-1 + nVelCtrlLaw = max(1,norm(velocity,2)/uxMaxCtrlLaw); + + velocity = velocity / nVelCtrlLaw ; + + % Converting u to v using R and also position w.r.t moving frame + movPosition = R.'*(position - uCentroid); %pi + movVelocity = R.'*velocity; %vi* + + %Inequality constraint for asymptotic convergence + % stateTilde is the error of the state + stateTilde = [uCentroidD - uCentroid;thetaD-theta;s1D-s1;s2D-s2]; + + % Gamma is the transformation matrix from moving frame to abstract + % space + Gamma = [g zeros(3,2);zeros(2,3) eye(2)]; + + % 5 by 1 matrix used in the monotonic convergence criterian + val = [eye(2);(1/s1-s2)*movPosition.'*E1;movPosition.'*(eye(2)+E2);movPosition.'*(eye(2)-E2)]; + %Gain Matrix - 5*5 matrix + GainMat = [KU_COEFF(1,:) 0 0 0; KU_COEFF(2,:) 0 0 0; 0 0 KT_COEFF 0 0; 0 0 0 KS1_COEFF 0; 0 0 0 0 KS2_COEFF]; + + Acondition1 = stateTilde.'*GainMat*Gamma*val; + + %Inequality constraint to saturate the maximum velocity of the robot + % calculated using convex optimization to max of 0.1ms-1 + % u = Rv + Acondition2 = [1 0] * R ; + Acondition3 = [0 1] * R; + + AMatCondition =[-Acondition1 ; Acondition2 ; Acondition3]; + BMatCondition = [0; 0.1; 0.1]; + + %Check for conditions when the robots are within collision distance. + %This is when the ccollision avoidance constrained is applied for the + %robots + for robotInd2=1:N + % calculate the position of each robot and compare against + % current + if(robotInd2 ~= robotInd1) + bot1Position = R.'*(bots(robotInd1).qN.' - uCentroid); %p1 + bot2Position = R.'*(bots(robotInd2).qN.' - uCentroid); %p2 - Old + + bot2Velocity = R.'*(bots(robotInd2).uN.');%v2 - New + %the calculated delta value + delta = norm(bot2Position - bot1Position, 2); + + if(delta <= sepDist) + % Condition for the collision avoidance + Acondition4 = (bot1Position - bot2Position).'; + Bcondition4 = ((bot1Position - bot2Position).'*bot2Velocity); + AMatCondition = [AMatCondition ; -Acondition4]; + BMatCondition = [BMatCondition ; -Bcondition4]; + end + end + end +% Solving for optimal velocity based on previous condition + opts1 = optimset('display','off'); + velocityCap = lsqlin(sqrt(2)*eye(2),sqrt(2)*movVelocity,AMatCondition,BMatCondition,[],[],[],[],[],opts1); %vicap + + % There are instances in which the lsqlin function fails. This is a + % check for the failure to debug the system + if(round(AMatCondition*velocityCap,3) > round(BMatCondition,3)) + AMatCondition*velocityCap - BMatCondition + conditionFailure = conditionFailure + 1; + end + + % velocity with respect to the world frame + velWorldFrame = R*velocityCap ; %uicap + + %As per the paper we will contain the velocity at max cap velocity + %for the robot + kVel(1) = max(1, abs(velWorldFrame(1))/uxMax(1)); + kVel(2) = max(1, abs(velWorldFrame(2))/uxMax(2)); + + nVel = max(kVel(1),kVel(2)); + + velWorldFrame = velWorldFrame / nVel ; + + % Update the position of the robot based on Euler method for simulation + % Modelling the position based on the differential drive robot + % model + updateDifferentialDrivePos(velWorldFrame, botAxleLength, robotInd1); + + bots(robotInd1).uStar = velocity.' ; %ui* + end + + %Update each of the robot position + for robotInd1=1:N + + %Update the position and velocity variables of the robots + bots(robotInd1).q = bots(robotInd1).qN ; + bots(robotInd1).u = bots(robotInd1).uN; + bots(robotInd1).tr = bots(robotInd1).trN ; + + % Accumulate the plot variables + % Position of robots + posPlot(robotInd1).qx(PLOT_COUNTER) = bots(robotInd1).q(1); + posPlot(robotInd1).qy(PLOT_COUNTER) = bots(robotInd1).q(2); + % Minimum energy control velocity + posPlot(robotInd1).uStarX(PLOT_COUNTER) = bots(robotInd1).uStar(1); + posPlot(robotInd1).uStarY(PLOT_COUNTER) = bots(robotInd1).uStar(2); + % Control velocity input based on convex optimization + posPlot(robotInd1).uCapX(PLOT_COUNTER) = bots(robotInd1).u(1); + posPlot(robotInd1).uCapY(PLOT_COUNTER) = bots(robotInd1).u(2); + + end + + stepCounter = stepCounter+1; + + %Accumulate the data values for the plot + Time(PLOT_COUNTER) = (Time(1) + dT*stepCounter); + uPlotx(PLOT_COUNTER) = (uCentroidD(1) - uCentroid(1)) ; + uPloty(PLOT_COUNTER) = (uCentroidD(2) - uCentroid(2)) ; + thetaPlot(PLOT_COUNTER) = (thetaD - theta) ; + s1Plot(PLOT_COUNTER) = (s1D-s1); + s2Plot(PLOT_COUNTER) = (s2D - s2); + xPlot(PLOT_COUNTER) = norm([uPlotx(PLOT_COUNTER) ; uPloty(PLOT_COUNTER) ; thetaPlot(PLOT_COUNTER) ; s1Plot(PLOT_COUNTER) ; s2Plot(PLOT_COUNTER)],2); + + PLOT_COUNTER = PLOT_COUNTER+1; + + if(0 == isnan(s1) && 0 == isnan(s2) ) + s1 , s2, uCentroid, theta + end + + if(mod(stepCounter,20) == 0) + drawElipseBoundary(bots,uCentroid,theta,s1,s2,uCentroidD,thetaD,s1D,s2D,botRadius, botAxleLength); + writeVideo(outputVideo,getframe(gcf)); + end + +end +%Generate the data for difference in bot position +l=1; +for j=1:N + for k=(j+1):N + for plotCount=1:PLOT_COUNTER-1 + tempMatVal = [posPlot(j).qx(plotCount) - posPlot(k).qx(plotCount) ; posPlot(j).qy(plotCount) - posPlot(k).qy(plotCount)]; + posDiffPlot(l).q(plotCount) = norm(tempMatVal,2); + + end + l = l+1; + end +end + +%Generate the data for the velocity plot +for j=1:N + for plotCount=1:PLOT_COUNTER-1 + velStarPlot(j).u(plotCount) = norm([posPlot(j).uStarX(plotCount) posPlot(j).uStarY(plotCount)],2); + velCapPlot(j).u(plotCount) = norm([posPlot(j).uCapX(plotCount) posPlot(j).uCapY(plotCount)],2); + end +end + +% Draw the final elipse position +drawElipseBoundary(bots,uCentroid,theta,s1,s2,uCentroidD,thetaD,s1D,s2D,botRadius, botAxleLength); +writeVideo(outputVideo,getframe(gcf)); + +close(ouVtputVideo); + +%We start Plotting our parameters here +% Plot of the error in the state variables +figure ; + +plot1 = plot(Time,xPlot,Time, abs(uPlotx),'--',Time, abs(uPloty),'--',Time, abs(thetaPlot),'--', Time, abs(s1Plot), '--',Time, abs(s2Plot), '--','linewidth', 0.7); +plot1(1).LineWidth = 1; +plot1(1).Color = 'black'; +xlabel('Time(s)'); +ylabel('Magnitude'); +legend('||x||','||x_1||','||x_2||','||x_3||','||x_4||','||x_5||'); +title('convergencePlot'); +hold on; +figure ; +plot(Time,2*(botRadius + botAxleLength)*ones(size(Time)),'--','Color','black'); +hold on; +for i=1:l-1 + plot(Time,posDiffPlot(i).q); + hold on; +end +xlabel('Time(s)'); +ylabel('||q_i-q_j||(m)'); +title('RobotPositionComparison'); + +hold on; + +figure ; +plot(Time, velStarPlot(1).u,'--', Time, velCapPlot(1).u,'linewidth',1); +xlabel('Time(s)'); +ylabel('Magnitude(m/s)'); +title('VelocityPlot'); +legend('||u_i^{*}||','||u_i^{\^}||'); +hold on; + +figure; +drawElipseBoundary(bots,uCentroid,theta,s1,s2,uCentroidD,thetaD,s1D,s2D,botRadius,botAxleLength); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Bot initialization % +% Description - This function distributes the robots in Euclidean space as% +% a normal distribution. The separation between each robot should be % +% greater than the initial configuration % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + function bots = distributeBots(N) + + %Structure variables: + %q - holds the position w.r.t the world frame + %u - Velocity w.r.t the world frame + %qN - Holds the new position of the robots that has to be updated + bots(N,1) = struct('q',[0; 0],'u',[0; 0].','qN',[0 ; 0],'uN',[0 ; 0],'uStar',[0 ; 0],'tr',0,'trN',0); + + + % Choosing Random distribution of the robots where the robots are + % separated at a distance greater thatn the safe separation distance + % between each of them. We choose + % an arbitrary value of the mean and the standard deviation + + randLoop = true; + while(randLoop) + counter = 0; + % Standard deviation is equal to robot count / 4 + A = normrnd(12.5938, 2.3038,[2,N]); + + for robotInd1=1:N + for robotInd2=1:N + if(robotInd1 ~= robotInd2) + if(norm((A(:,robotInd1) - A(:,robotInd2)),2) < 0.6) + counter = counter+1; + end + end + end + end + + if(counter == 0) + %Print the position of the robots + A + randLoop = false; + end + end + + for robotInd=1:N + % place agent, Initializing all states of the robots + bots(robotInd).q = A(1:2,robotInd).'; + bots(robotInd).qN = A(1:2,robotInd).'; + bots(robotInd).u = [0 0]; + bots(robotInd).uN = [0 0]; + bots(robotInd).uStar = [0 0]; + bots(robotInd).tr = 0 ; + bots(robotInd).trN = 0 ; + end + end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Name - Abstract Space % +% Description - Computes the abstract state variables of the formation % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + function [aCentroid,aTheta,aS1,aS2] = abstractSpace(bots) + E1 = [0 1; 1 0]; + E2 = [1 0; 0 -1]; + E3 = [0 -1; 1 0]; + + tThetaY = 0; + tThetaX = 0; + tS1 = 0; + tS2 = 0; + tempCentroid = 0; + + %Calculate Centroid + for robotInd =1:N + position = bots(robotInd).q.'; + tempCentroid = tempCentroid + position ; + end + aCentroid = 1/N * tempCentroid ; + + %Calculate Orientation + for robotInd = 1:N + position = bots(robotInd).q.'; + tThetaY = tThetaY + (position - aCentroid).'*E1*(position - aCentroid); + tThetaX = tThetaX + (position - aCentroid).'*E2*(position - aCentroid); + end + aTheta = atan2(tThetaY , tThetaX)/2 ; + R = [cos(theta) -sin(theta); sin(theta) cos(theta)]; + H1 = eye(2) + R^2*E2; + H2 = eye(2) - R^2*E2; + H3 = R^2*E1 ; + + for robotInd=1:N + position = bots(robotInd).q.'; + tS1 = tS1 + ((position - aCentroid).'*H1*(position - aCentroid)) ; + tS2 = tS2 + ((position - aCentroid).'*H2*(position - aCentroid)) ; + end + aS1 = tS1 / (2*(N-1)) ; + aS2 = tS2 / (2*(N-1)) ; + + end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Name - drawElipseBoundary % +% Description - Draw elipse boundary around the robot formation and the % +% final desired positon % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + function drawElipseBoundary(aBots, aCentroid , aTheta, aS1, aS2 , aCentroidD, aThetaD, aS1D, aS2D, botRadius,botAxleLength) + % Plot the robot positions and the ellipsoid + X = []; + t=0:0.01:2*pi; + + for robotInd=1:N + X = [X [aBots(robotInd).q(1);aBots(robotInd).q(2)]]; + end + + if aS1>aS2 + % Multiply [acos(t); bsin(t)] by R. Taking confidence parameter as 1 + x1 = aCentroid(1) + sqrt(9.2103*aS1)*cos(t)*cos(aTheta) - sqrt(2*aS2)*sin(t)*sin(aTheta); + y1 = aCentroid(2) + sqrt(9.2103*aS2)*sin(t)*cos(aTheta) + sqrt(2*aS1)*cos(t)*sin(aTheta); + else + % Multiply [acos(t); bsin(t)] by R. Taking confidence parameter as 1 + x1 = aCentroid(1) + sqrt(9.2103*aS2)*cos(t)*cos(aTheta) - sqrt(2*aS1)*sin(t)*sin(aTheta); + y1 = aCentroid(2) + sqrt(9.2103*aS1)*sin(t)*cos(aTheta) + sqrt(2*aS2)*cos(t)*sin(aTheta); + end + + %Plot the desired elipse position + + if aS1D>aS2D + % Multiply [acos(t); bsin(t)] by R. Taking confidence parameter as 1 + x2 = aCentroidD(1) + sqrt(9.2103*aS1D)*cos(t)*cos(aThetaD) - sqrt(2*aS2D)*sin(t)*sin(aThetaD); + y2 = aCentroidD(2) + sqrt(9.2103*aS2D)*sin(t)*cos(aThetaD) + sqrt(2*aS1D)*cos(t)*sin(aThetaD); + else + % Multiply [acos(t); bsin(t)] by R. Taking confidence parameter as 1 + x2 = aCentroidD(1) + sqrt(9.2103*aS2D)*cos(t)*cos(aThetaD) - sqrt(2*aS1D)*sin(t)*sin(aThetaD); + y2 = aCentroidD(2) + sqrt(9.2103*aS1D)*sin(t)*cos(aThetaD) + sqrt(2*aS2D)*cos(t)*sin(aThetaD); + end + % labels = {'1','2','3','4','5','6','7','8','9','10'}; + plot(X(1,:),X(2,:),'o'); +% text(X(1,:),X(2,:),labels,'VerticalAlignment','bottom','HorizontalAlignment','right'); + title('Robot Configuration') + xlabel('X-axis') + ylabel('Y-axis') + hold on + plot(x1,y1,'r',x2,y2,'b'); + for robotInd=1:N + botCenter = aBots(robotInd).q; + botOrientation = aBots(robotInd).tr; + rotMat = [cos(botOrientation) -sin(botOrientation);sin(botOrientation) cos(botOrientation)]; +% rotMat = [cos(botOrientation) sin(botOrientation) botRadius*cos(botOrientation); -sin(botOrientation) cos(botOrientation) botRadius*sin(botOrientation); 0 0 1]; + lineEnd = botCenter + [(botRadius+botAxleLength)*cos(botOrientation) (botRadius+botAxleLength)*sin(botOrientation)]; +% lineEnd = lineEnd(1:2,1).'; + % Defining circles around each robot + x = aBots(robotInd).q(1)+ botAxleLength*cos(botOrientation) + (botRadius+botAxleLength)*cos(t); + y = aBots(robotInd).q(2)+ botAxleLength*sin(botOrientation) + (botRadius+botAxleLength)*sin(t); + plot(x,y,'g'); + quiver(botCenter(1,1),botCenter(1,2),lineEnd(1,1)+botAxleLength*cos(botOrientation) - botCenter(1,1), lineEnd(1,2)+botAxleLength*sin(botOrientation) - botCenter(1,2),0,'Color','red'); + + end + legend('Robot Position','Actual Formation','Desired Formation'); + hold off + end + + function updateDifferentialDrivePos(velWorldFrame, axleLength, robotInd) + linVel = 0 ; + angVel = 0 ; + + botOrientation = bots(robotInd).tr ; %New Orientation + rotMat = [cos(botOrientation) sin(botOrientation) ; -sin(botOrientation)/axleLength cos(botOrientation)/axleLength]; + + velMat = rotMat*velWorldFrame ; + + linVel = velMat(1); + angVel = velMat(2); + + %Update the kinematic position of the robots. We use the same model + %that is used in the robotic simulator toolbox to update the + %position + dx = dT * linVel * cos(botOrientation); + dy = dT * linVel * sin(botOrientation); + + dtr = dT * angVel ; + + %Update the new positions and new velocities to which the robots + %have to move and the new orientation of the robots + bots(robotInd).qN(1) = bots(robotInd).q(1) + dx ; + bots(robotInd).qN(2) = bots(robotInd).q(2) + dy; + + bots(robotInd).uN = velWorldFrame.'; + + bots(robotInd).trN = bots(robotInd).tr + dtr ; + end + +end