planners_demo

0001 function planners_demo
0002 close all
0003 q0=0;
0004 qd0=0;
0005 qdd0=0;
0006 q1=0.5; %rad
0007 q2=0.8;
0008 t0=0;
0009 t1=10;%s
0010 t2=20;%s
0011 
0012 figure, xlabel('t (s)'), ylabel('q (rad), q_d (rad/s)'), title('PRIMER ORDEN'), hold on
0013 
0014 
0015 k=first_order([q0 q1],[t0 t1]);
0016 k=first_order([q1 q2],[t1 t2]);
0017 
0018 
0019 figure, xlabel('t (s)'), ylabel('q (rad), q_d (rad/s)'), title('SEGUNDO ORDEN'), hold on
0020 
0021 
0022 [k,qd1]=second_order([q0 q1],qd0,[t0 t1]);
0023 k=second_order([q1 q2],qd1,[t1 t2]);
0024 
0025 
0026 figure, xlabel('t (s)'), ylabel('q (rad), q_d (rad/s)'), title('TERCER ORDEN'), hold on
0027 [k,qd1,qdd1]=third_order([q0 q1],qd0,qdd0,[t0 t1]);
0028 k=third_order([q1 q2],qd1,qdd1,[t1 t2]);
0029 
0030 figure, xlabel('t (s)'), ylabel('q (rad), q_d (rad/s)'), title('TERCER ORDEN, SEGUNDA VERSIÓN'), hold on
0031 [k]=third_order_2([q0 q2],[0 0],[t0 t2]);
0032 
0033 
0034 
0035 
0036 
0037 function k=first_order(q,t)
0038 
0039 %   PRIMER ORDEN
0040 
0041 k=inv([1 t(1); 1 t(2)])*[q(1) q(2)]';
0042 
0043 t_v=t(1):0.01:t(2);
0044 q_v=k(1)+k(2)*t_v;
0045 qd_v=k(2)*ones(1,length(t_v));
0046 
0047 plot(t_v,q_v, 'r')
0048 plot(t_v,qd_v, 'g')
0049 
0050 function [k,qd1]=second_order(q,qd,t)
0051 
0052 %   SEGUNDO ORDEN
0053 k=inv([1 t(1) t(1)^2; 1 t(2) t(2)^2; 0 1 2*t(1)])*[q(1) q(2) qd(1)]';
0054 
0055 time=t(1):0.01:t(2);
0056 q_t=k(1)+k(2)*time+k(3)*time.^2;
0057 qd_t=k(2)*ones(1,length(time)) + 2*k(3)*time;
0058 qdd_t=2*k(3)*ones(1,length(time));
0059 
0060 qd1 = k(2) + 2*k(3)*t(2);
0061 
0062 plot(time,q_t, 'r')
0063 plot(time,qd_t, 'g')
0064 plot(time,qdd_t, 'b')
0065 
0066 
0067 
0068 function [k,qd1, qdd1]=third_order(q,qd,qdd,t)
0069 
0070 %   tercer orden
0071 A=[1 t(1) t(1)^2 t(1)^3;
0072    1 t(2) t(2)^2 t(2)^3;
0073    0   1  2*t(1) 3*t(1)^2;
0074    0   0   2     6*t(1)];
0075 k=inv(A)*[q(1) q(2) qd(1) qdd(1)]';
0076 
0077 time=t(1):0.01:t(2);
0078 q_t = k(1) + k(2)*time + k(3)*time.^2 + k(4)*time.^3;
0079 qd_t= k(2)*ones(1,length(time)) + 2*k(3)*time + 3*k(4)*time.^2;
0080 qdd_t=2*k(3)*ones(1,length(time)) + 6*k(4)*time;
0081 
0082 qd1 = k(2) + 2*k(3)*t(2) + 3*k(4)*t(2).^2;
0083 qdd1 = 2*k(3) + 6*k(4)*t(2);
0084 
0085 plot(time,q_t, 'r')
0086 plot(time,qd_t, 'g')
0087 plot(time,qdd_t, 'b')
0088 
0089 
0090 function [k,qd1, qdd1]=third_order_2(q,qd,t)
0091 
0092 %   tercer orden
0093 A=[1 t(1) t(1)^2 t(1)^3;
0094    1 t(2) t(2)^2 t(2)^3;
0095    0   1  2*t(1) 3*t(1)^2;
0096    0   1  2*t(2) 3*t(2)^2];
0097 k=inv(A)*[q(1) q(2) qd(1) qd(2)]';
0098 
0099 time=t(1):0.01:t(2);
0100 q_t = k(1) + k(2)*time + k(3)*time.^2 + k(4)*time.^3;
0101 qd_t= k(2)*ones(1,length(time)) + 2*k(3)*time + 3*k(4)*time.^2;
0102 qdd_t=2*k(3)*ones(1,length(time)) + 6*k(4)*time;
0103 
0104 qd1 = k(2) + 2*k(3)*t(2) + 3*k(4)*t(2).^2;
0105 qdd1 = 2*k(3) + 6*k(4)*t(2);
0106 
0107 plot(time,q_t, 'r')
0108 plot(time,qd_t, 'g')
0109 plot(time,qdd_t, 'b')