Trajectory

 Trajectory constructor.

 The possible inputs are
     - Another trajectory (copy constructor)
     - A cell array with poses
     - A matrix n times k with 'n' the number of poses and 'k' the
       dimensionality of each pose.
     - A couple of trajectories: they are concatenated into the new
       trajectory
     - initial point + final point + n. To generate
       a straight line trajectory between two points discretized with 'n'
       steps.
     - initial angle + final angle + n. To generate a
       trajectory on an arc of ellipse discretized with 'n' steps.

 See also ConcatTrajectories.

0001 function T=Trajectory(varargin)
0002 % Trajectory constructor.
0003 %
0004 % The possible inputs are
0005 %     - Another trajectory (copy constructor)
0006 %     - A cell array with poses
0007 %     - A matrix n times k with 'n' the number of poses and 'k' the
0008 %       dimensionality of each pose.
0009 %     - A couple of trajectories: they are concatenated into the new
0010 %       trajectory
0011 %     - initial point + final point + n. To generate
0012 %       a straight line trajectory between two points discretized with 'n'
0013 %       steps.
0014 %     - initial angle + final angle + n. To generate a
0015 %       trajectory on an arc of ellipse discretized with 'n' steps.
0016 %
0017 % See also ConcatTrajectories.
0018 
0019   switch nargin
0020     case 1
0021       if isa(varargin{1},'Trajectory')
0022         T=varargin{1};
0023       else
0024         if iscell(varargin{1})
0025           T.type='mixed';
0026           T.data=varargin{1};
0027           T=class(T,'Trajectory');
0028         else
0029           if isa(varargin{1},'double')
0030             T.type='mixed';
0031             n=size(varargin{1},1);
0032             T.data=cell(1,n);
0033             for i=1:n
0034               T.data{i}=Pose(varargin{1}(i,:)');
0035             end
0036             T=class(T,'Trajectory');
0037           else
0038             error('Trajectory copy constructor with a wrong object');
0039           end
0040         end
0041       end
0042       
0043     case 2
0044       if isa(varargin{1},'Trajectory') && isa(varargin{2},'Trajectory')
0045         T=ConcatTrajectories(varargin{1},varargin{2});
0046       else
0047         error('Incorrect parameters type in trajectory concatenation');
0048       end
0049       
0050     case 3
0051       T.type='line';
0052       % Two poses and the number of steps
0053       T.data=StraightLine(varargin{1},varargin{2},varargin{3});
0054       T=class(T,'Trajectory');
0055       
0056     case 5
0057       T.type='ellipse';
0058       % angle range, major/minor axis, steps
0059       T.data=EllipseArc(varargin{1},varargin{2},varargin{3},varargin{4},varargin{5});
0060       T=class(T,'Trajectory');
0061       
0062     otherwise
0063       error('Incorrect number of parameters in trajectory creation');
0064   end
0065   
0066   
0067 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0068 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0069   function e=EllipseArc(aInit,aEnd,axisX,axisY,n)
0070     % Discretizes an EllipseArc between angles AInit to aEnd
0071     % The parameters of the ellipse are AxisX AxisY
0072     % The orientation of is taken along the segment connecting
0073     % two consecutive points
0074     % Approximated ellipse length (Wikipedia dixit)
0075     aRange=aEnd-aInit;
0076     step=aRange/(n-1);
0077     if aInit<aEnd
0078       s=1;
0079     else
0080       s=-1;
0081     end
0082     e=cell(1,n);
0083     for i=1:n
0084       t=aInit+(i-1)*step;
0085       ct=cos(t);
0086       st=sin(t);
0087       e{i}=Pose([axisX*ct;axisY*st;atan2(s*axisY*ct,-s*axisX*st)]);
0088     end
0089     
0090   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0091   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0092     function l=StraightLine(a,b,n)
0093       % Defines a sequence of 'n' poses interpolating points from A to B
0094       d=Absolute2Relative(a,b);
0095       initial=double(a);
0096       angle=get(d,'orientation');
0097       step=double(d)/(n-1);
0098       l=cell(1,n);
0099       for i=1:n
0100         s=initial(1:2)+step(1:2)*(i-1);
0101         l{i}=Pose([s(1);s(2);angle]);
0102       end