add matlab trajectory code

lwrserv/matlab/README.md

@@ -0,0 +1,20 @@
+# README #
+
+### What is this repository for? ###
+
+* this is a robotic simulator for an LBR4+ or LBR5 capable of inverse kinematic and trajectory path planing
+* [Learn Markdown](https://bitbucket.org/tutorials/markdowndemo)
+
+### How do I get set up? ###
+
+* Dependencies: matlab has to be installed
+
+### How do I run the GUI ###
+
+    setup % setup the paths and robotic vision toolbox
+    gui_motion % start the trajectory motion gui
+
+### Who do I talk to? ###
+
+* Ivo Kuhlemann
+* Philipp Schoenberger <ph.schoenberger@googlemail.com>

lwrserv/matlab/gui/control.m

@@ -0,0 +1,90 @@
+function [ ] = control(handles )
+
+addpath('../kinematics/')
+
+global do;
+do = true;
+
+% DH Parameter
+a = [0,0,0,0,0,0,0];
+alp = [-90,90,-90,90,-90,90,0];
+d = [310.4,0,400.1,0,390,0,78];
+
+L1 = Link('d',d(1),'a',a(1),'alpha',alp(1)/180*pi);
+L1.isrevolute;
+L2 = Link('d',d(2),'a',a(2),'alpha',alp(2)/180*pi);
+L2.isrevolute;
+L3 = Link('d',d(3),'a',a(3),'alpha',alp(3)/180*pi);
+L3.isrevolute;
+L4 = Link('d',d(4),'a',a(4),'alpha',alp(4)/180*pi);
+L4.isrevolute;
+L5 = Link('d',d(5),'a',a(5),'alpha',alp(5)/180*pi);
+L5.isrevolute;
+L6 = Link('d',d(6),'a',a(6),'alpha',alp(6)/180*pi);
+L6.isrevolute;
+L7 = Link('d',d(7),'a',a(7),'alpha',alp(7)/180*pi);
+L7.isrevolute;
+bot = SerialLink([L1 L2 L3 L4 L5 L6 L7], 'name', 'Kuka LBR4 +');
+
+figure(1);
+axis equal
+
+tx_old = 0; ty_old = 0; tz_old = 0;
+x_old  = 0; y_old  = 0; z_old  = 0;
+while (do)
+    %Calculating 'M'
+    tx = get(handles.slider1,'Value');
+    ty = get(handles.slider2,'Value');
+    tz = get(handles.slider3,'Value');
+    x = get(handles.slider4,'Value')/180*pi;
+    y = get(handles.slider5,'Value')/180*pi;
+    z = get(handles.slider6,'Value')/180*pi;
+    
+    % only update if gui values have changed
+    values_unchanged = tx == tx_old && ty == ty_old && tz == tz_old && ...
+         x == x_old && y == y_old && z == z_old;
+    if (values_unchanged)
+        pause(0.01);
+        continue;
+    end
+    tx_old = tx;
+    ty_old = ty;
+    tz_old = tz;
+    x_old = x;
+    y_old = y;
+    z_old = z;
+    
+    Rx = [ 1 0 0 ; 0 cos(x) -sin(x) ; 0 sin(x) cos(x) ];
+    Ry = [ cos(y) 0 sin(y) ; 0 1 0 ; -sin(y) 0 cos(y)];
+    Rz = [ cos(z) -sin(z) 0 ; sin(z) cos(z) 0 ; 0 0 1 ];
+    
+    R = Rz*Ry*Rx;
+    
+    M = [R, [tx;ty;tz]; 0,0,0,1];
+
+    args = [get(handles.arm,'Value'),get(handles.elbow,'Value'),get(handles.flip,'Value'),get(handles.slider9,'Value')*(pi/180)];
+    for i=1:3
+        if args(i) == 0
+            args(i) = -1;
+        end
+    end
+    
+    set(handles.text8, 'String', [sprintf('%.4G',args(4)*180/pi),' °']);
+    
+    try
+        J = LBRinvKin(M,args,'LBR4+');
+        J'
+    catch err
+        err.message
+        break;
+    end
+    
+    try
+        bot.plot(J.*(pi/180));
+    catch err
+        err.message
+        break;
+    end
+
+end
+

lwrserv/matlab/gui/gui.m

@@ -0,0 +1,296 @@
+function varargout = gui(varargin)
+% GUI MATLAB code for gui.fig
+%      GUI, by itself, creates a new GUI or raises the existing
+%      singleton*.
+%
+%      H = GUI returns the handle to a new GUI or the handle to
+%      the existing singleton*.
+%
+%      GUI('CALLBACK',hObject,eventData,handles,...) calls the local
+%      function named CALLBACK in GUI.M with the given input arguments.
+%
+%      GUI('Property','Value',...) creates a new GUI or raises the
+%      existing singleton*.  Starting from the left, property value pairs are
+%      applied to the GUI before gui_OpeningFcn gets called.  An
+%      unrecognized property name or invalid value makes property application
+%      stop.  All inputs are passed to gui_OpeningFcn via varargin.
+%
+%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
+%      instance to run (singleton)".
+%
+% See also: GUIDE, GUIDATA, GUIHANDLES
+
+% Edit the above text to modify the response to help gui
+
+% Last Modified by GUIDE v2.5 29-Aug-2014 12:52:48
+
+% Begin initialization code - DO NOT EDIT
+gui_Singleton = 1;
+gui_State = struct('gui_Name',       mfilename, ...
+    'gui_Singleton',  gui_Singleton, ...
+    'gui_OpeningFcn', @gui_OpeningFcn, ...
+    'gui_OutputFcn',  @gui_OutputFcn, ...
+    'gui_LayoutFcn',  [] , ...
+    'gui_Callback',   []);
+if nargin && ischar(varargin{1})
+    gui_State.gui_Callback = str2func(varargin{1});
+end
+
+if nargout
+    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
+else
+    gui_mainfcn(gui_State, varargin{:});
+end
+% End initialization code - DO NOT EDIT
+
+
+% --- Executes just before gui is made visible.
+function gui_OpeningFcn(hObject, eventdata, handles, varargin)
+% This function has no output args, see OutputFcn.
+% hObject    handle to figure
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+% varargin   command line arguments to gui (see VARARGIN)
+
+% Choose default command line output for gui
+handles.output = hObject;
+
+% Update handles structure
+guidata(hObject, handles);
+
+% UIWAIT makes gui wait for user response (see UIRESUME)
+% uiwait(handles.figure1);
+
+
+% --- Outputs from this function are returned to the command line.
+function varargout = gui_OutputFcn(hObject, eventdata, handles)
+% varargout  cell array for returning output args (see VARARGOUT);
+% hObject    handle to figure
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Get default command line output from handles structure
+varargout{1} = handles.output;
+
+
+% --- Executes on button press in pushbutton1.
+function pushbutton1_Callback(hObject, eventdata, handles)
+    control(handles)
+end
+% --- Executes on slider movement.
+function slider1_Callback(hObject, eventdata, handles)
+% hObject    handle to slider1 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'Value') returns position of slider
+%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
+
+
+% --- Executes during object creation, after setting all properties.
+function slider1_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to slider1 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: slider controls usually have a light gray background.
+if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
+    set(hObject,'BackgroundColor',[.9 .9 .9]);
+end
+
+
+% --- Executes on slider movement.
+function slider2_Callback(hObject, eventdata, handles)
+% hObject    handle to slider2 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'Value') returns position of slider
+%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
+
+
+% --- Executes during object creation, after setting all properties.
+function slider2_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to slider2 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: slider controls usually have a light gray background.
+if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
+    set(hObject,'BackgroundColor',[.9 .9 .9]);
+end
+
+
+% --- Executes on slider movement.
+function slider3_Callback(hObject, eventdata, handles)
+% hObject    handle to slider3 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'Value') returns position of slider
+%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
+
+
+% --- Executes during object creation, after setting all properties.
+function slider3_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to slider3 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: slider controls usually have a light gray background.
+if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
+    set(hObject,'BackgroundColor',[.9 .9 .9]);
+end
+
+
+% --- Executes on slider movement.
+function slider4_Callback(hObject, eventdata, handles)
+% hObject    handle to slider4 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'Value') returns position of slider
+%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
+
+
+% --- Executes during object creation, after setting all properties.
+function slider4_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to slider4 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: slider controls usually have a light gray background.
+if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
+    set(hObject,'BackgroundColor',[.9 .9 .9]);
+end
+
+
+% --- Executes on slider movement.
+function slider5_Callback(hObject, eventdata, handles)
+% hObject    handle to slider5 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'Value') returns position of slider
+%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
+
+
+% --- Executes during object creation, after setting all properties.
+function slider5_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to slider5 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: slider controls usually have a light gray background.
+if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
+    set(hObject,'BackgroundColor',[.9 .9 .9]);
+end
+
+
+% --- Executes on slider movement.
+function slider6_Callback(hObject, eventdata, handles)
+% hObject    handle to slider6 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'Value') returns position of slider
+%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
+
+
+% --- Executes during object creation, after setting all properties.
+function slider6_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to slider6 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: slider controls usually have a light gray background.
+if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
+    set(hObject,'BackgroundColor',[.9 .9 .9]);
+end
+
+
+% --- Executes on slider movement.
+function slider7_Callback(hObject, eventdata, handles)
+% hObject    handle to slider7 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'Value') returns position of slider
+%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
+
+
+% --- Executes during object creation, after setting all properties.
+function slider7_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to slider7 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: slider controls usually have a light gray background.
+if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
+    set(hObject,'BackgroundColor',[.9 .9 .9]);
+end
+
+
+% --- Executes on button press in Stop.
+function Stop_Callback(hObject, eventdata, handles)
+% hObject    handle to Stop (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+global do;
+do = false;
+try
+    close 1;
+end
+set(0, 'DefaultFigurePosition', 'factory')
+close(gui)
+
+
+
+% --- Executes on button press in arm.
+function arm_Callback(hObject, eventdata, handles)
+% hObject    handle to arm (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hint: get(hObject,'Value') returns toggle state of arm
+
+
+% --- Executes on button press in arm.
+function elbow_Callback(hObject, eventdata, handles)
+% hObject    handle to arm (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hint: get(hObject,'Value') returns toggle state of arm
+
+
+% --- Executes on button press in flip.
+function flip_Callback(hObject, eventdata, handles)
+% hObject    handle to flip (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hint: get(hObject,'Value') returns toggle state of flip
+
+
+% --- Executes on slider movement.
+function slider9_Callback(hObject, eventdata, handles)
+% hObject    handle to slider9 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'Value') returns position of slider
+%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
+
+
+% --- Executes during object creation, after setting all properties.
+function slider9_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to slider9 (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: slider controls usually have a light gray background.
+if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
+    set(hObject,'BackgroundColor',[.9 .9 .9]);
+end
+

lwrserv/matlab/kinematics/DHjT.m

@@ -0,0 +1,9 @@
+% Calculation of joint transformation matrix DH-convention
+
+function [ T ] = DHjT( teta, a, alpha, d )
+T = [cos(teta), -sin(teta)*cos(alpha), sin(teta)*sin(alpha), a*cos(teta); ...
+        sin(teta), cos(teta)*cos(alpha), -cos(teta)*sin(alpha), a*sin(teta) ; ...
+        0           , sin(alpha)             , cos(alpha)              , d              ; ...
+        0           , 0                       , 0                        , 1                 ];
+end
+

lwrserv/matlab/kinematics/LBRforKin.m

@@ -0,0 +1,21 @@
+function [M] = LBRforKin(J,robot)
+
+%% Setting up DH-Parameter: Which robot?
+a = [0,0,0,0,0,0,0];
+alp = [-90,90,-90,90,-90,90,0].*(pi/180);
+
+if strcmp(robot,'LBR4+') % LBR 4+ 
+    d = [310.4,0,400.1,0,390,0,78]; 
+end
+if strcmp(robot,'LBR5') % LBR 5 iiwa
+    d = [340,0,400,0,400,0,111];
+end
+M01 = DHjT(J(1),a(1),alp(1),d(1));
+M12 = DHjT(J(2),a(2),alp(2),d(2));
+M23 = DHjT(J(3),a(3),alp(3),d(3));
+M34 = DHjT(J(4),a(4),alp(4),d(4));
+M56 = DHjT(J(5),a(5),alp(5),d(5));
+M67 = DHjT(J(6),a(6),alp(6),d(6));
+M78 = DHjT(J(7),a(7),alp(7),d(7));
+M08 = M01*M12*M23*M34*M56*M67*M78;
+M = M08;

lwrserv/matlab/kinematics/LBRinvKin.m

@@ -0,0 +1,113 @@
+function [J] = LBRinvKin(M,args,robot)
+
+%% Setting up DH-Parameter: Which robot?
+a = [0,0,0,0,0,0,0];
+alp = [-90,90,-90,90,-90,90,0].*(pi/180);
+
+if strcmp(robot,'LBR4+') % LBR 4+ 
+    d = [310.4,0,400.1,0,390,0,78]; 
+end
+if strcmp(robot,'LBR5') % LBR 5 iiwa
+    d = [340,0,400,0,400,0,111];
+end
+
+%Parameter
+ARM = -args(1);
+ELBOW = -args(2);
+FLIP = -args(3);
+if (ELBOW == -1)
+    delta = args(4)+pi;
+else
+    delta = args(4);
+end
+
+%Erstellen von M06
+R67 = DHjT(0,a(7),alp(7),d(7));
+M06 = M*pinv(R67);
+
+%Verdrehung der Ellipse um die z-Achse des BKS
+phi = atan2(M06(2,4),M06(1,4));
+
+%Berechnung der gew�nschten Ellenbogenposition in BKS-Koordinaten
+Rsw = getRotMat([0;0;d(1)],rotz(-phi)*M06(1:3,4));
+S = [Rsw,[0;0;d(1)];[0 0 0 1]];
+W = [Rsw,rotz(-phi)*M06(1:3,4);[0 0 0 1]];
+dsw = sqrt((W(1,4)-S(1,4))^2+(W(2,4)-S(2,4))^2+(W(3,4)-S(3,4))^2);
+alpha = acos((d(5)^2-d(3)^2-dsw^2)/(-2*d(3)*dsw));
+E0 = S*[eye(3),[sin(alpha)*d(3);0;cos(alpha)*d(3)];[0 0 0 1]];
+Ed = S*[rotz(delta),[0;0;0];[0 0 0 1]]*pinv([Rsw,[0;0;0];[0 0 0 1]])*[eye(3),E0(1:3,4)-S(1:3,4);[0 0 0 1]];
+
+% Joint 1
+Ed_phi = rotz(phi)*Ed(1:3,4);
+J(1) = atan2(ARM*Ed_phi(2),ARM*Ed_phi(1));
+M01 = DHjT(J(1),a(1),alp(1),d(1));
+% Joint 2
+J(2) = atan2(ARM*sqrt((Ed(1,4))^2+(Ed(2,4))^2),Ed(3,4)-S(3,4));
+% Joint 4
+J(4) = ARM*ELBOW*(pi-acos((dsw^2-d(5)^2-d(3)^2)/(-2*d(5)*d(3))));
+
+%% Singularit�t vorhanden?
+if (d(3)+d(5))-dsw < 1E-10
+    % Joint 3
+    J(3) = delta;
+else
+    %% Keine Singularit�t
+    % Joint 3
+    M16 = pinv(M01)*M06;
+    s3 = M16(3,4)/(sin(J(4))*d(5));
+    
+    %Floating point Fehler von Matlab
+    if 1-abs(s3) < 1E-10 
+        c3 = 0;
+    else
+        c3 = sqrt(1-s3^2);
+    end
+    %Parametrisierung der Ellipse
+    E90 = S*[rotz(pi/2),[0;0;0];[0 0 0 1]]*pinv([Rsw,[0;0;0];[0 0 0 1]])*[eye(3),E0(1:3,4)-S(1:3,4);[0 0 0 1]];
+    a_ell = E90(2,4);
+    b_ell = E0(1,4)-E90(1,4);
+    
+    if abs(b_ell)< 1E-10 % Spezialfall: Schulter und Handgelenk auf einer H�he -> Ellipse wird zur Linie
+        J(3) = atan2(s3,ELBOW*((Ed(3,4)-S(3,4))/abs(Ed(3,4)-S(3,4)))*c3);
+    else
+        if abs((-W(1,4)/2)) > abs(b_ell) %normaler Fall: Schulter au�erhalb Ellipse  %NOTE: corrected from orig: (-W(1,4)/2) < b
+            x1 = -a_ell*sqrt(-b_ell^2+(-W(1,4)/2)^2)/(-W(1,4)/2); % x0=0 gek�rzt
+            if abs(Ed(2,4))>abs(x1) || abs(Ed(1,4))>abs(-W(1,4)/2) % Tangentenschnittpunkt �berschrittten?
+                J(3) = atan2(s3,ELBOW*(b_ell/abs(b_ell))*c3);
+            else
+                J(3) = atan2(s3,-ELBOW*(b_ell/abs(b_ell))*c3); 
+            end
+        else %Schulter liegt innerhalb oder auf Ellipse -> keine Tangente m�glich
+            J(3) = atan2(s3,ELBOW*(b_ell/abs(b_ell))*c3);
+        end
+    end
+end
+
+%% Joint 5, 6 und 7
+M12 = DHjT(J(2),a(2),alp(2),d(2));
+M23 = DHjT(J(3),a(3),alp(3),d(3));
+M34 = DHjT(J(4),a(4),alp(4),d(4));
+M04 = M01*M12*M23*M34;
+M47 = pinv(M04)*M;
+
+th = 1E-10;
+if abs(abs(M47(3,3))-1) <  th
+    J(6) = 0;
+else
+    J(6) = atan2(FLIP*sqrt(1-M47(3,3)^2),M47(3,3));
+end
+
+if abs(M47(2,3)) < th && abs(M47(1,3)) < th
+    J(5) = 0;
+else
+    J(5) = atan2(FLIP*M47(2,3),FLIP*M47(1,3));
+end
+
+if abs(M47(3,2)) < th && abs(M47(3,1)) <th
+    J(7) = 0;
+else
+    J(7) = atan2(FLIP*M47(3,2),FLIP*-M47(3,1));
+end
+
+%% From rad to deg
+J = J.*(180/pi);

lwrserv/matlab/kinematics/LBRinvKinMinChange.m

@@ -0,0 +1,48 @@
+function [J,arm,elbow,flip] = LBRinvKinMinChange(M,J,delta,robot)
+
+possible_configs = 2^3;
+joints = size(J,2);
+
+J_new = zeros(possible_configs,joints);
+J_arm     = zeros(possible_configs,1);
+J_elbow   = zeros(possible_configs,1);
+J_flip    = zeros(possible_configs,1);
+
+% calculate for each config posibility
+i=1;
+for arm = [-1,1]
+    for elbow = [-1,1]
+        for flip = [-1,1]
+            
+            J_arm(i) = arm;
+            J_elbow(i) = elbow;
+            J_flip(i) = flip;
+            
+            arg = [J_arm(i),J_elbow(i),J_flip(i),delta];
+            try
+                J_new(i,:) = LBRinvKin(M,arg,robot);
+            catch
+                % config is not valid dectemten to write 
+                % at same position again
+                i = i - 1;
+            end
+            
+            i = i + 1 ;
+        end
+    end
+end
+
+% cut unused space
+J_new = J_new(1:i-1,:);
+
+% get the minimum change index
+J_changes = sum((J_new - repmat(J,possible_configs,1)).^2,2);
+[~,index] = min(J_changes);
+
+% set return values
+J = J_new(index,:);
+arm = J_arm(index);
+elbow = J_elbow(index);
+flip = J_flip(index);
+    
+    

lwrserv/matlab/kinematics/bangbang.m

@@ -0,0 +1,91 @@
+function [s,sd,sdd] = bangbang(q0, q1, t)
+
+    t0 = t;
+    if isscalar(t)
+        t = (0:t-1)';
+    else
+        t = t(:);
+    end
+
+    tf = max(t(:));
+
+    V = (q1-q0)/tf * 1.5;
+
+    % already on target
+    if q0 == q1
+        s = ones(size(t)) * q0;
+        sd = zeros(size(t));
+        sdd = zeros(size(t));
+        return
+    end
+
+    tb = t(ceil(length(t)/2)); % time intervals
+    a = V/tb;
+
+    p = zeros(length(t), 1);
+    pd = p;
+    pdd = p;
+    
+    for i = 1:length(t)
+        tt = t(i);
+
+        if tt <= tb
+            % initial blend
+            p(i) = q0 + a/2*tt^2;
+            pd(i) = a*tt;
+            pdd(i) = a;
+        else
+            % final blend
+            p(i) = q1 - a/2*tf^2 + a*tf*tt - a/2*tt^2;
+            pd(i) = a*tf - a*tt;
+            pdd(i) = -a;
+        end
+    end
+
+    switch nargout
+        case 0
+            if isscalar(t0)
+                % for scalar time steps, axis is labeled 1 .. M
+                xt = t+1;
+            else
+                % for vector time steps, axis is labeled by vector M
+                xt = t;
+            end
+
+            clf
+            subplot(311)
+            % highlight the accel, coast, decel phases with different
+            % colored markers
+            hold on
+            k = t<= tb;
+            plot(xt(k), p(k), 'r-o');
+            k = (t>=tb) & (t<= (tf-tb));
+            plot(xt(k), p(k), 'b-o');
+            k = t>= (tf-tb);
+            plot(xt(k), p(k), 'g-o');
+            grid; ylabel('s');
+            hold off
+
+            subplot(312)
+            plot(xt, pd); grid; ylabel('sd');
+            
+            subplot(313)
+            plot(xt, pdd); grid; ylabel('sdd');
+            if ~isscalar(t0)
+                xlabel('time')
+            else
+                for c=get(gcf, 'Children');
+                    set(c, 'XLim', [1 t0]);
+                end
+            end
+            shg
+        case 1
+            s = p;
+        case 2
+            s = p;
+            sd = pd;
+        case 3
+            s = p;
+            sd = pd;
+            sdd = pdd;
+    end

lwrserv/matlab/kinematics/getM.m

@@ -0,0 +1,14 @@
+function [ M ] = getM( x , y, z , tx, ty, tz)
+
+
+    Rx = [ 1 0 0 ; 0 cos(x) -sin(x) ; 0 sin(x) cos(x) ];
+    Ry = [ cos(y) 0 sin(y) ; 0 1 0 ; -sin(y) 0 cos(x)];
+    Rz = [ cos(z) -sin(z) 0 ; sin(z) cos(z) 0 ; 0 0 1 ];
+    
+    R = Rz*Ry*Rx;
+    T = [tx;ty;tz];
+    
+    M = [R, T; 0,0,0,1];
+
+end
+

lwrserv/matlab/kinematics/getRotMat.m

@@ -0,0 +1,105 @@
+function R = getRotMat(v1,v2)
+
+% x-vektor
+x = v1-v2;
+x = x/norm(x);
+
+%überprüfe vektor
+th = 0.00000001;
+ind = find(abs(x)>th);
+
+
+
+if length(ind)==3 %% normaler Fall, alles ok
+    %u-vektor
+    u = [x(3);0;0];
+    u(2) = (-(x(1)*u(1))-(x(3)*u(3)))/x(2);
+    u = u/norm(u);
+    %v-Vektor
+    v = cross(x,u);
+    
+    ct1 = cos(-pi/2);
+    st1 = sin(-pi/2);
+    ct2 = cos(pi);
+    st2 = sin(pi);
+    R = [x,u,v]*[ct1 0 st1;0 1 0;-st1 0 ct1]*[ct2 -st2 0;st2 ct2 0;0 0 1];
+    
+elseif length(ind)==2 %% Falls ein Wert Null, reagiere
+    %unterscheide fälle
+    if abs(x(1))<th
+        %u-vektor
+        u = [x(3);0;0];
+        u(2) = (-(x(1)*u(1))-(x(3)*u(3)))/x(2);
+        u = u/norm(u);
+        %v-Vektor
+        v = cross(x,u);
+        ct1 = cos(-pi/2);
+        st1 = sin(-pi/2);
+        ct2 = cos(pi);
+        st2 = sin(pi);
+        R = [x,u,v]*[ct1 0 st1;0 1 0;-st1 0 ct1]*[ct2 -st2 0;st2 ct2 0;0 0 1];
+    elseif abs(x(2))<th
+        %u-vektor
+        u = [0;abs(x(3));0];
+        u(1) = (-(x(2)*u(2))-(x(3)*u(3)))/x(1);
+        u = u/norm(u);
+        %v-Vektor
+        v = cross(x,u);
+        ct1 = cos(-pi/2);
+        st1 = sin(-pi/2);
+        ct2 = cos(pi);
+        st2 = sin(pi);
+        R = [x,u,v]*[ct1 0 st1;0 1 0;-st1 0 ct1]*[ct2 -st2 0;st2 ct2 0;0 0 1];
+    elseif abs(x(3))<th
+        %u-vektor
+        u = [0;0;x(2)];
+        u(2) = (-(x(3)*u(3))-(x(2)*u(2)))/x(1);
+        u = u/norm(u);
+        %v-Vektor
+        v = cross(x,u);
+        ct1 = cos(-pi/2);
+        st1 = sin(-pi/2);
+        ct2 = cos(-pi/2);
+        st2 = sin(-pi/2);
+        R = [x,u,v]*[ct1 0 st1;0 1 0;-st1 0 ct1]*[ct2 -st2 0;st2 ct2 0;0 0 1];
+    end
+    
+    
+elseif length(ind)==1 %% falls nur ein wert > 0  spezialfälle
+    if ind == 1
+        gamma = pi;
+        beta = pi/2;
+        if x(1)<0
+            beta = -pi/2;
+        end
+        ct1 = cos(beta);
+        st1 = sin(beta);
+        ct2 = cos(gamma);
+        st2 = sin(gamma);
+        R = [ct2 -st2 0;st2 ct2 0;0 0 1]*[ct1 0 st1;0 1 0;-st1 0 ct1];
+    elseif ind == 2
+        beta = pi/2;
+        gamma = -pi/2;
+        if x(2)<0
+            gamma = pi/2;
+        end
+        ct1 = cos(beta);
+        st1 = sin(beta);
+        ct2 = cos(gamma);
+        st2 = sin(gamma);
+        R = [ct2 -st2 0;st2 ct2 0;0 0 1]*[ct1 0 st1;0 1 0;-st1 0 ct1];
+    elseif ind == 3
+        beta = pi;
+        if x(3)<0
+            beta = -pi/2;
+        end
+%         ct1 = cos(beta);
+%         st1 = sin(beta);
+%         R = [ct1 0 st1;0 1 0;-st1 0 ct1];
+        ct2 = cos(pi);
+        st2 = sin(pi);
+        R = [ct2 -st2 0;st2 ct2 0;0 0 1];
+    end
+else
+    % Fuck off state
+end

lwrserv/matlab/motion/control_motion.m

@@ -0,0 +1,218 @@
+function [ ] = control(motion_config)
+
+global do;
+do = true;
+
+% DH Parameter
+a = [0,0,0,0,0,0,0];
+alp = [-90,90,-90,90,-90,90,0];
+d = [310.4,0,400.1,0,390,0,78];
+
+links = 7;
+L1 = Link('d',d(1),'a',a(1),'alpha',alp(1)/180*pi);
+L1.isrevolute;
+L2 = Link('d',d(2),'a',a(2),'alpha',alp(2)/180*pi);
+L2.isrevolute;
+L3 = Link('d',d(3),'a',a(3),'alpha',alp(3)/180*pi);
+L3.isrevolute;
+L4 = Link('d',d(4),'a',a(4),'alpha',alp(4)/180*pi);
+L4.isrevolute;
+L5 = Link('d',d(5),'a',a(5),'alpha',alp(5)/180*pi);
+L5.isrevolute;
+L6 = Link('d',d(6),'a',a(6),'alpha',alp(6)/180*pi);
+L6.isrevolute;
+L7 = Link('d',d(7),'a',a(7),'alpha',alp(7)/180*pi);
+L7.isrevolute;
+bot = SerialLink([L1 L2 L3 L4 L5 L6 L7], 'name', 'Kuka LBR4 +');
+
+
+axes(motion_config.robofig)
+% plot once because the axis get otherwise messed up
+bot.plot();
+
+while (motion_config.run)
+    motion_config
+    %Calculating 'M'
+    
+    % use this later for multiple points
+    point =1;
+    M_start = getM(motion_config.rx(point),   ...
+                   motion_config.ry(point),   ...
+                   motion_config.rz(point),   ...
+                   motion_config.tx(point),   ...
+                   motion_config.ty(point),   ...
+                   motion_config.tz(point));
+               
+    M_end   = getM(motion_config.rx(point+1),   ...
+                   motion_config.ry(point+1),   ...
+                   motion_config.rz(point+1),   ...
+                   motion_config.tx(point+1),   ...
+                   motion_config.ty(point+1),   ...
+                   motion_config.tz(point+1));
+    
+    args = [motion_config.arm,...
+            motion_config.elbow,...
+            motion_config.flip,...
+            motion_config.delta(point)...
+            ];
+        
+    for i=1:3
+        if args(i) == 0
+            args(i) = -1;
+        end
+    end
+    
+    animation_steps = ceil(motion_config.time/(motion_config.resolution/1000.0));
+    t = linspace(0,1,animation_steps)';
+    switch ( motion_config.type)
+        case 'joint'
+            % calculate start and end point
+            try
+                % calculate start joints 
+                J_start = LBRinvKin(M_start,args,'LBR4+');
+
+                % calculate end joints 
+                J_end = LBRinvKin(M_end,args,'LBR4+');
+
+            catch err
+                err.message
+            end
+             
+            J_animation = repmat(J_start,animation_steps,1);
+            J_motion = J_end - J_start;
+            
+            % do the interpolation
+            switch ( motion_config.core)
+                case 'linear'
+                % calculate joint movements   
+                J_animation = repmat(linspace(0,1,animation_steps)',1,links);
+                J_animation = J_animation.*repmat(J_motion,animation_steps,1); 
+                [~,J_animation_d] = gradient( J_animation,t(2));
+                [~,J_animation_dd] = gradient( J_animation_d,t(2));
+
+                case 'bang bang'                    
+                    [J_animation, J_animation_d, J_animation_dd] = mtraj(@bangbang, J_motion*0, J_motion, t);   
+                case '5poly'
+                    [J_animation, J_animation_d, J_animation_dd] = mtraj(@tpoly, J_motion*0, J_motion, t);   
+                case 'lspb'
+                    [J_animation, J_animation_d, J_animation_dd] = mtraj(@lspb, J_motion*0, J_motion, t);   
+            end
+            % add start joints
+            J_animation = J_animation+repmat(J_start,animation_steps,1); 
+            
+            % calculate motion in 3d 
+
+            
+
+        case 'carthesian'
+            switch ( motion_config.core)
+                case 'linear'
+                s = t;
+                case 'bang bang'
+                s = bangbang(0, 1, t);
+                case '5poly'
+                s = tpoly(0, 1, t);
+                case 'lspb'
+                s = lspb(0,1,t);
+            end
+            % generate a carthesian based line in space
+            M_animation = zeros(4,4,animation_steps);
+            
+            for i = 1:animation_steps
+                M_animation(:,:,i) = getM(...
+                    motion_config.rx(point)+s(i)*(motion_config.rx(point+1)-motion_config.rx(point)),   ...
+                    motion_config.ry(point)+s(i)*(motion_config.ry(point+1)-motion_config.ry(point)),   ...
+                    motion_config.rz(point)+s(i)*(motion_config.rz(point+1)-motion_config.rz(point)),   ...
+                    motion_config.tx(point)+s(i)*(motion_config.tx(point+1)-motion_config.tx(point)),   ...
+                    motion_config.ty(point)+s(i)*(motion_config.ty(point+1)-motion_config.ty(point)),   ...
+                    motion_config.tz(point)+s(i)*(motion_config.tz(point+1)-motion_config.tz(point))    ...
+                    );
+            end
+            %M_animation = ctraj(M_start,M_end,s);                    
+
+            J_animation = zeros(animation_steps,links);
+            
+            %setup the configuration of the current position
+            arm = motion_config.arm;
+            elbow = motion_config.elbow;
+            flip = motion_config.flip;
+            for i = 1:animation_steps
+                try 
+                    if (i == 1)
+                        % set the starting point
+                        J_animation(i,:) = LBRinvKin(M_animation(:,:,i),args,'LBR4+'); 
+                    else
+                        %not the starting point -> calculate by minimal
+                        %change
+                        [J_animation(i,:),new_arm,new_elbow,new_flip] = LBRinvKinMinChange(M_animation(:,:,i),J_animation(i-1,:),motion_config.delta(point),'LBR4+'); 
+                        if (new_arm ~= arm || new_elbow ~= elbow || new_flip ~= flip )
+                            
+                            arm = new_arm;
+                            elbow = new_elbow;
+                            flip = new_flip;
+                            fprintf('config change on %d  arm %d, elbow %d, flip %d \n',i,arm,elbow,flip);
+                            
+                        end
+                    end
+                        
+                catch err
+                    %err.message 
+                    %fprintf('no join angle for animation_step %d use prev value instead\n',i);
+                    if(i > 1)
+                        J_animation(i,:) =  J_animation(i-1,:);
+                    end
+                end 
+            end
+            [~,J_animation_d] = gradient( J_animation,t(2));
+            [~,J_animation_dd] = gradient( J_animation_d,t(2));
+    end
+    
+    try
+        
+        % draw the joint speed and derivitives
+        
+        % plot the joint movement
+        axes(motion_config.joinsfig)
+        plot(t, J_animation);
+        h = rotate3d;
+        set(h,'RotateStyle','box','Enable','off');
+        
+        
+        % plot the joint velocity
+        axes(motion_config.joinsdfig)
+        plot(t,J_animation_d);
+        h = rotate3d;
+        set(h,'RotateStyle','box','Enable','off');
+        
+        % plot the joint acceleration
+        axes(motion_config.joinsddfig)
+        plot(t,J_animation_dd);
+        h = rotate3d;
+        set(h,'RotateStyle','box','Enable','off');
+        
+        % do the animation
+        axes(motion_config.robofig)
+        h = rotate3d;
+        set(h,'RotateStyle','box','Enable','on');
+
+        tic % start timer
+        
+        acutual_animations = 0;
+        while motion_config.run && toc <= motion_config.time
+            i = toc / motion_config.time * animation_steps;
+            i = ceil(i);
+            if (i > animation_steps)
+                i = animation_steps;
+            end
+            bot.plot(J_animation(i,:).*(pi/180));
+            acutual_animations = acutual_animations +1 ;
+        end
+        if (acutual_animations < animation_steps)
+            display(sprintf('warning skipped %d frames',animation_steps-acutual_animations));
+        end
+    catch err
+        err.message
+    end
+
+end
+

lwrserv/matlab/motion/gui_motion.m

@@ -0,0 +1,158 @@
+function varargout = gui_motion(varargin)
+% GUI_MOTION MATLAB code for gui_motion.fig
+%      GUI_MOTION, by itself, creates a new GUI_MOTION or raises the existing
+%      singleton*.
+%
+%      H = GUI_MOTION returns the handle to a new GUI_MOTION or the handle to
+%      the existing singleton*.
+%
+%      GUI_MOTION('CALLBACK',hObject,eventData,handles,...) calls the local
+%      function named CALLBACK in GUI_MOTION.M with the given input arguments.
+%
+%      GUI_MOTION('Property','Value',...) creates a new GUI_MOTION or raises the
+%      existing singleton*.  Starting from the left, property value pairs are
+%      applied to the GUI before gui_motion_OpeningFcn gets called.  An
+%      unrecognized property name or invalid value makes property application
+%      stop.  All inputs are passed to gui_motion_OpeningFcn via varargin.
+%
+%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
+%      instance to run (singleton)".
+%
+% See also: GUIDE, GUIDATA, GUIHANDLES
+
+% Edit the above text to modify the response to help gui_motion
+
+% Last Modified by GUIDE v2.5 28-Apr-2015 14:51:33
+
+% Begin initialization code - DO NOT EDIT
+gui_Singleton = 1;
+gui_State = struct('gui_Name',       mfilename, ...
+                   'gui_Singleton',  gui_Singleton, ...
+                   'gui_OpeningFcn', @gui_motion_OpeningFcn, ...
+                   'gui_OutputFcn',  @gui_motion_OutputFcn, ...
+                   'gui_LayoutFcn',  [] , ...
+                   'gui_Callback',   []);
+if nargin && ischar(varargin{1})
+    gui_State.gui_Callback = str2func(varargin{1});
+end
+
+if nargout
+    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
+else
+    gui_mainfcn(gui_State, varargin{:});
+end
+% End initialization code - DO NOT EDIT
+
+% --- Executes just before gui_motion is made visible.
+function gui_motion_OpeningFcn(hObject, eventdata, handles, varargin)
+% This function has no output args, see OutputFcn.
+% hObject    handle to figure
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+% varargin   command line arguments to gui_motion (see VARARGIN)
+
+% Choose default command line output for gui_motion
+handles.output = hObject;
+
+% Update handles structure
+guidata(hObject, handles);
+
+update_Labels(hObject, eventdata, handles);
+eventdata; % unused
+
+% UIWAIT makes gui_motion wait for user response (see UIRESUME)
+% uiwait(handles.figure1);
+
+
+function varargout = gui_motion_OutputFcn(~, ~, handles) 
+varargout{1} = handles.output;
+
+function update_Labels(~, ~, handles)   
+    set(handles.label_delta,     'String', [sprintf('%.4G',get(handles.slider_delta,'Value')),' °']);
+
+    set(handles.label_x_start,   'String', [sprintf('%.4G',get(handles.slider_x_start,'Value')),' °']);
+    set(handles.label_x_end,     'String', [sprintf('%.4G',get(handles.slider_x_end,'Value')),' °']);
+    set(handles.label_y_start,   'String', [sprintf('%.4G',get(handles.slider_y_start,'Value')),' °']);
+    set(handles.label_y_end,     'String', [sprintf('%.4G',get(handles.slider_y_end,'Value')),' °']);
+    set(handles.label_z_start,   'String', [sprintf('%.4G',get(handles.slider_z_start,'Value')),' °']);
+    set(handles.label_z_end,     'String', [sprintf('%.4G',get(handles.slider_z_end,'Value')),' °']);
+
+    set(handles.label_tx_start,  'String', [sprintf('%.4G',get(handles.slider_tx_start,'Value')),' mm']);
+    set(handles.label_tx_end,    'String', [sprintf('%.4G',get(handles.slider_tx_end,'Value')),' mm']);
+    set(handles.label_ty_start,  'String', [sprintf('%.4G',get(handles.slider_ty_start,'Value')),' mm']);
+    set(handles.label_ty_end,    'String', [sprintf('%.4G',get(handles.slider_ty_end,'Value')),' mm']);
+    set(handles.label_tz_start,  'String', [sprintf('%.4G',get(handles.slider_tz_start,'Value')),' mm']);
+    set(handles.label_tz_end,    'String', [sprintf('%.4G',get(handles.slider_tz_end,'Value')),' mm']);
+    set(handles.label_time,      'String', [sprintf('%.4G',get(handles.slider_time,'Value')),' s']);
+    set(handles.label_resolution,'String', [sprintf('%.4G',get(handles.slider_resolution,'Value')),' ms']);
+
+    
+    m = get(handles.start_simulation,'UserData');
+    % set label of start/stop
+    if (isempty(m) || m.run == false)
+        set(handles.start_simulation,'String','Start simulation');
+    else
+        set(handles.start_simulation,'String','Stop simulation');
+        %apply new values
+        m.rx        = [ get(handles.slider_x_start,'Value')  get(handles.slider_x_end,'Value')  ]/180*pi;
+        m.ry        = [ get(handles.slider_z_start,'Value')  get(handles.slider_y_end,'Value')  ]/180*pi;
+        m.rz        = [ get(handles.slider_y_start,'Value')  get(handles.slider_z_end,'Value')  ]/180*pi;
+        m.tx        = [ get(handles.slider_tx_start,'Value') get(handles.slider_tx_end,'Value') ];
+        m.ty        = [ get(handles.slider_tz_start,'Value') get(handles.slider_ty_end,'Value') ];
+        m.tz        = [ get(handles.slider_ty_start,'Value') get(handles.slider_tz_end,'Value') ];
+        m.delta     = [ get(handles.slider_delta,'Value') get(handles.slider_delta,'Value') ]/180*pi;
+        m.arm       = get(handles.arm,'Value');
+        m.elbow     = get(handles.arm,'Value');
+        m.flip      = get(handles.arm,'Value');
+        m.time      = get(handles.slider_time,'Value');
+
+        c = get(handles.core,'Value');
+        s = get(handles.core,'String');
+        m.core      = s{c};
+
+        c = get(handles.type,'Value');
+        s = get(handles.type,'String');
+        m.type      = s{c};
+    end
+    
+    
+
+function start_simulation_Callback(~, ~, handles)
+% generate motion_config
+
+m = get(handles.start_simulation,'UserData');
+if (isempty(m))
+    m = motion_config();
+    set(handles.start_simulation,'UserData',m);
+    m.run = false;
+end
+if (m.run == true) 
+    m.run = false;
+    update_Labels([], [], handles) 
+else
+    m.run       = true;
+    update_Labels([], [], handles) 
+    
+    m.joinsfig    = handles.joins;
+    m.joinsdfig   = handles.joinsd;
+    m.joinsddfig  = handles.joinsdd;
+    m.robofig     = handles.robofig;
+    
+    c = get(handles.core,'Value')
+    s = get(handles.core,'String')
+    m.core      = s{c};
+    
+    c = get(handles.type,'Value')
+    s = get(handles.type,'String')
+    m.type      = s{c};
+    
+    m
+    control_motion(m);
+end
+
+function Exit_Callback(~, ~, ~)
+try
+    close 1;
+end
+set(0, 'DefaultFigurePosition', 'factory')
+close(gui_motion)

lwrserv/matlab/motion/motion_config.m

@@ -0,0 +1,38 @@
+classdef motion_config < handle
+    properties
+        %translation
+        tx = [0 0];
+        ty = [0 0];
+        tz = [0 0];
+        
+        %rotation
+        rx = [0 0];
+        ry = [0 0];
+        rz = [0 0];
+        
+        % delta
+        delta = [0 0];
+        
+        arm = 0;
+        elbow = 0;
+        flip = 0;
+        
+        % trajectory
+        time = 500 %ms
+        resolution = 5 %ms
+        max_acceleration = 0.5 %m/s^2
+        max_velocity = 0.03 %m/s^2
+        
+        core = 'linear'
+        type = 'joint'
+        
+        run = false;
+        
+        
+        robofig = 1;
+        joinsfig = 2;
+        joinsdfig = 3;
+        joinsddfig = 4;
+    end
+end
+

lwrserv/matlab/rvctools/common/Animate.m

@@ -0,0 +1,94 @@
+%ANIMATE Create an animation
+%
+% Helper class for creating animations.  Saves snapshots of a figture as a
+% folder of individual PNG format frames numbered 0000.png, 0001.png and so
+% on.
+%
+% Example::
+%
+%          anim = Animate('movie');
+%
+%          for i=1:100
+%              plot(...);
+%              anim.add();
+%          end
+%
+% To convert the image files to a movie you could use a tool like ffmpeg
+%           % ffmpeg -r 10 -i movie/*.png out.mp4
+
+% Copyright (C) 1993-2014, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+
+classdef Animate < handle
+    properties
+        frame
+        dir
+        resolution
+    end
+    
+    methods
+        function a = Animate(name, res)
+            %ANIMATE.ANIMATE Create an animation class
+            %
+            % A = ANIMATE(NAME, OPTIONS) initializes an animation, and creates a folder
+            % called NAME to hold the individual frames.
+            %
+            % Options::
+            % 'resolution',R    Set the resolution of the saved image to R pixels per
+            % inch.
+        
+            a.frame = 0;
+            a.dir = name;
+            mkdir(name);
+            if nargin > 1
+                a.resolution = res;
+            else
+                a.resolution = [];
+            end
+            delete( fullfile(name, '*.png') );
+            
+        end
+        
+        function add(a, fh)
+            %ANIMATE.ADD Adds current plot to the animation
+            %
+            % A.ADD() adds the current figure in PNG format to the animation
+            % folder with a unique sequential filename.
+            %
+            % A.ADD(FIG) as above but captures the figure FIG.
+            %
+            % See also print.
+
+            if nargin < 2
+                fh = gcf;
+            end
+            
+            if isempty(a.resolution)
+                print(fh, '-dpng', fullfile(a.dir, sprintf('%04d.png', a.frame)));
+            else
+                print(fh, '-dpng', sprintf('-r%d', a.resolution), fullfile(a.dir, sprintf('%04d.png', a.frame)));
+            end
+            a.frame = a.frame + 1;
+        end
+        
+        function close(a)
+        end
+    end
+end

lwrserv/matlab/rvctools/common/about.m

@@ -0,0 +1,54 @@
+%ABOUT Compact display of variable type
+%
+% ABOUT(X) displays a compact line that describes the class and dimensions of
+% X.
+%
+% ABOUT X  as above but this is the command rather than functional form
+%
+% See also WHOS.
+function about(var)
+    
+    if isstr(var)
+        % invoked without parentheses
+        w = evalin('caller', sprintf('whos(''%s'')', var));
+        varname = var;
+    else
+        w = whos('var');
+        varname = inputname(1);
+    end
+    
+    if isempty(w)
+        error(['cant find variable ' var])
+    end
+    ss = sprintf('%d', w.size(1));
+    for i=2:length(w.size)
+        ss = strcat(ss, sprintf('x%d', w.size(i)));
+    end
+    
+    % build a string to show if complex or not
+    if w.complex
+        cmplx = '+complex';
+    else
+        cmplx = '';
+    end
+    
+    % build a string to show size in convenient format
+    suffix = {'bytes', 'kB', 'MB', 'GB', 'TB'};
+    sz = w.bytes;
+    for i=1:numel(suffix)
+        if sz/1000 < 1
+            break;
+        end
+        sz = sz/1000;
+    end
+    
+    if i==1
+        size = sprintf('%d %s', sz, suffix{i});
+    else
+        size = sprintf('%.1f %s', sz, suffix{i});
+    end
+    
+    % now display the info
+    fprintf('%s [%s%s] : %s (%s)\n', ...
+        varname, w.class, cmplx, ss, size);
+

lwrserv/matlab/rvctools/common/angdiff.m

@@ -0,0 +1,47 @@
+%ANGDIFF Difference of two angles
+%
+% D = ANGDIFF(TH1, TH2) returns the difference between angles TH1 and TH2 on
+% the circle.  The result is in the interval [-pi pi).  If TH1 is a column 
+% vector, and TH2 a scalar then return a column vector where TH2 is modulo 
+% subtracted from the corresponding elements of TH1.
+%
+% D = ANGDIFF(TH) returns the equivalent angle to TH in the interval [-pi pi).
+%
+% Return the equivalent angle in the interval [-pi pi).
+
+function d = angdiff(th1, th2)
+
+    if nargin < 2
+% THIS IS A BAD IDEA, WHERE IS IT USED?
+%         if length(th1) > 1
+%             d = th1(1) - th1(2);
+%         else
+%             d = th1;
+%         end
+        d = th1;
+    else
+        d = th1 - th2;
+    end
+
+    
+    d = mod(d+pi, 2*pi) - pi;
+
+% Simplistic version of the code, easy to see what it does, but slow...
+%
+% for very negative angles keep adding 2pi
+%     while true
+%         k = find(d < -pi);
+%         if isempty(k)
+%             break;
+%         end
+%         d(k) = d(k) + 2*pi;
+%     end
+% 
+%     % for very positive angles keep subtracting 2pi
+%     while true
+%         k = find(d > pi);
+%         if isempty(k)
+%             break;
+%         end
+%         d(k) = d(k) - 2*pi;
+%     end

lwrserv/matlab/rvctools/common/arrow3.m

@@ -0,0 +1,784 @@
+function hn=arrow3(p1,p2,s,w,h,ip,alpha,beta)
+% ARROW3 (R13)
+%   ARROW3(P1,P2) draws lines from P1 to P2 with directional arrowheads.
+%   P1 and P2 are either nx2 or nx3 matrices.  Each row of P1 is an
+%   initial point, and each row of P2 is a terminal point.
+%
+%   ARROW3(P1,P2,S,W,H,IP,ALPHA,BETA) can be used to specify properties
+%   of the line, initial point marker, and arrowhead.  S is a character
+%   string made with one element from any or all of the following 3
+%   columns:
+%
+%     Color Switches      LineStyle            LineWidth
+%     ------------------  -------------------  --------------------
+%     k  blacK (default)  -  solid (default)   0.5 points (default)
+%     y  Yellow           :  dotted            0   no lines
+%     m  Magenta          -. dashdot           /   LineWidthOrder
+%     c  Cyan             -- dashed
+%     r  Red              *  LineStyleOrder            _______ __  
+%     g  Green                                       ^        |    
+%     b  Blue                                       / \       |    
+%     w  White                        Arrowhead    /   \   Height  
+%     a  Asparagus                                /     \     |    
+%     d  Dark gray                               /       \    |    
+%     e  Evergreen                              /___   ___\ __|__  
+%     f  Firebrick                             |    | |    |       
+%     h  Hot pink                              |-- Width --|       
+%     i  Indigo                                |    | |    |       
+%     j  Jade                                       | |            
+%     l  Light gray                                 | |            
+%     n  Nutbrown                                   | |            
+%     p  Pear                                       | |            
+%     q  kumQuat                      Line       -->| |<--LineWidth
+%     s  Sky blue                                   | |            
+%     t  Tawny                                      | |            
+%     u  bUrgundy                                   | |            
+%     v  Violet                                     | |            
+%     z  aZure                                      | |            
+%     x  random                       Initial      /   \           
+%     o  colorOrder                   Point    -->(     )<--IP     
+%     |  magnitude                    Marker       \_ _/           
+%
+%     -------------------------------------------------------------
+%                          Color Equivalencies
+%     -------------------------------------------------------------
+%     ColorOrder     Arrow3         |     Simulink       Arrow3
+%     ----------     ----------     |     ----------     ----------
+%     Color1         Blue           |     LightBlue      aZure
+%     Color2         Evergreen      |     DarkGreen      Asparagus
+%     Color3         Red            |     Orange         kumQuat
+%     Color4         Sky blue       |     Gray           Light gray
+%     Color5         Violet         |
+%     Color6         Pear           |
+%     Color7         Dark gray      |
+%     -------------------------------------------------------------
+%
+%   The components of S may be specified in any order.  Invalid
+%   characters in S will be ignored and replaced by default settings.
+%
+%   Prefixing the color code with '_' produces a darker shade, e.g.
+%   '_t' is dark tawny; prefixing the color code with '^' produces a
+%   lighter shade, e.g. '^q' is light kumquat.  The relative brightness
+%   of light and dark color shades is controlled by the scalar parameter
+%   BETA.  Color code prefixes do not affect black (k), white (w), or
+%   the special color switches (xo|).
+%
+%   ColorOrder may be achieved in two fashions:  The user may either
+%   set the ColorOrder property (using RGB triples) or define the
+%   global variable ColorOrder (using a string of valid color codes).
+%   If the color switch is specified with 'o', and the global variable
+%   ColorOrder is a string of color codes (color switches less 'xo|',
+%   optionally prefixed with '_' or '^'), then the ColorOrder property
+%   will be set to the sequence of colors indicated by the ColorOrder
+%   variable.  The color sequence 'bersvpd' matches the default
+%   ColorOrder property.  If the color switch is specified with 'o', and
+%   the global variable ColorOrder is empty or invalid, then the current
+%   ColorOrder property will be used.  Note that the ColorOrder variable
+%   takes precedence over the ColorOrder property.
+%
+%   The magnitude color switch is used to visualize vector magnitudes
+%   in conjunction with a colorbar.  If the color switch is specified
+%   with '|', colors are linearly interpolated from the current ColorMap
+%   according to the length of the associated line.  This option sets
+%   CLim to [MinM,MaxM], where MinM and MaxM are the minimum and maximum
+%   magnitudes, respectively.
+%
+%   The current LineStyleOrder property will be used if LineStyle is
+%   specified with '*'.  MATLAB cycles through the line styles defined
+%   by the LineStyleOrder property only after using all colors defined
+%   by the ColorOrder property.  If however, the global variable
+%   LineWidthOrder is defined, and LineWidth is specified with '/',
+%   then each line will be drawn with sequential color, linestyle, and
+%   linewidth.
+%
+%   W (default = 1) is a vector of arrowhead widths; use W = 0 for no
+%   arrowheads.  H (default = 3W) is a vector of arrowhead heights.  If
+%   vector IP is neither empty nor negative, initial point markers will
+%   be plotted with diameter IP; for default diameter W, use IP = 0.
+%   The units of W, H and IP are 1/72 of the PlotBox diagonal.
+%
+%   ALPHA (default = 1) is a vector of FaceAlpha values ranging between
+%   0 (clear) and 1 (opaque).  FaceAlpha is a surface (arrowhead and
+%   initial point marker) property and does not affect lines.  FaceAlpha
+%   is not supported for 2D rendering.
+%
+%   BETA (default = 0.4) is a scalar that controls the relative
+%   brightness of light and dark color shades, ranging between 0 (no
+%   contrast) and 1 (maximum contrast).
+%
+%   Plotting lines with a single color, linestyle, and linewidth is
+%   faster than plotting lines with multiple colors and/or linestyles.
+%   Plotting lines with multiple linewidths is slower still.  ARROW3
+%   chooses renderers that produce the best screen images; exported
+%   or printed plots may benefit from different choices.
+%
+%   ARROW3(P1,P2,S,W,H,'cone',...) will plot cones with bases centered
+%   on P1 in the direction given by P2.  In this instance, P2 is
+%   interpreted as a direction vector instead of a terminal point.
+%   Neither initial point markers nor lines are plotted with the 'cone'
+%   option.
+%
+%   HN = ARROW3(P1,P2,...) returns a vector of handles to line and
+%   surface objects created by ARROW3.
+%
+%   ARROW3 COLORS will plot a table of named colors with default
+%   brightness.  ARROW3('colors',BETA) will plot a table of named
+%   colors with brightness BETA.
+%
+%   ARROW3 attempts to preserve the appearance of existing axes.  In
+%   particular, ARROW3 will not change XYZLim, View, or CameraViewAngle.
+%   ARROW3 does not, however, support stretch-to-fill scaling.  AXIS
+%   NORMAL will restore the current axis box to full size and remove any
+%   restrictions on the scaling of units, but will likely result in
+%   distorted arrowheads and initial point markers.  See
+%   (arrow3_messes_up_my_plots.html).
+%
+%   If a particular aspect ratio or variable limit is required, use
+%   DASPECT, PBASPECT, AXIS, or XYZLIM commands before calling ARROW3.
+%   Changing limits or aspect ratios after calling ARROW3 may alter the
+%   appearance of arrowheads and initial point markers.  ARROW3 sets
+%   XYZCLimMode to manual for all plots, sets DataAspectRatioMode to
+%   manual for linear plots, and sets PlotBoxAspectRatioMode to manual
+%   for log plots and 3D plots.  CameraViewAngleMode is also set to
+%   manual for 3D plots.
+%
+%   ARROW3 UPDATE will restore the appearance of arrowheads and
+%   initial point markers that have become corrupted by changes to
+%   limits or aspect ratios.  ARROW3('update',SF) will redraw initial
+%   point markers and arrowheads with scale factor SF.  If SF has one
+%   element, SF scales W, H and IP.  If SF has two elements, SF(1)
+%   scales W and IP, and SF(2) scales H.  If SF has three elements,
+%   SF(1) scales W, SF(2) scales H, and SF(3) scales IP.  All sizes are
+%   relative to the current PlotBox diagonal.
+%
+%   ARROW3 UPDATE COLORS will update the magnitude coloring of
+%   arrowheads, initial point markers, and lines to conform to the
+%   current ColorMap.
+%
+%   HN = ARROW3('update',...) returns a vector of handles to updated
+%   objects.
+%
+%   EXAMPLES:
+%
+%     % 2D vectors
+%     arrow3([0 0],[1 3])
+%     arrow3([0 0],[1 2],'-.e')
+%     arrow3([0 0],[10 10],'--x2',1)
+%     arrow3(zeros(10,2),50*rand(10,2),'x',1,3)
+%     arrow3(zeros(10,2),[10*rand(10,1),500*rand(10,1)],'u')
+%     arrow3(10*rand(10,2),50*rand(10,2),'x',1,[],1)
+%
+%     % 3D vectors
+%     arrow3([0 0 0],[1 1 1])
+%     arrow3(zeros(20,3),50*rand(20,3),'--x1.5',2)
+%     arrow3(zeros(100,3),50*rand(100,3),'x',1,3)
+%     arrow3(zeros(10,3),[10*rand(10,1),500*rand(10,1),50*rand(10,1)],'a')
+%     arrow3(10*rand(10,3),50*rand(10,3),'x',[],[],0)
+%
+%     % Cone plot
+%     t=(pi/8:pi/8:2*pi)'; p1=[cos(t) sin(t) t]; p2=repmat([0 0 1],16,1);
+%     arrow3(p1,p2,'x',2,4,'cone'), hold on
+%     plot3(p1(:,1),p1(:,2),p1(:,3)), hold off
+%     pause % change cone size
+%     arrow3('update',[1,2])
+%
+%     % Just for fun
+%     arrow3(zeros(100,3),50*rand(100,3),'x',8,4,[],0.95)
+%     light('position',[-10 -10 -10],'style','local')
+%     light('position',[60,60,60]), lighting gouraud
+%
+%     % ColorOrder variable, color code prefixes, and Beta
+%     global ColorOrder, ColorOrder='^ui^e_hq^v';
+%     theta=[0:pi/22:pi/2]';
+%     arrow3(zeros(12,2),[cos(theta),sin(theta)],'1.5o',1.5,[],[],[],0.5)
+%
+%     % ColorOrder property, LineStyleOrder, and LineWidthOrder
+%     global ColorOrder, ColorOrder=[];
+%     set(gca,'ColorOrder',[1,0,0;0,0,1;0.25,0.75,0.25;0,0,0])
+%     set(gca,'LineStyleOrder',{'-','--','-.',':'})
+%     global LineWidthOrder, LineWidthOrder=[1,2,4,8];
+%     w=[1,2,3,4]; h=[4,6,4,2];
+%     arrow3(zeros(4,2),[10*rand(4,1),500*rand(4,1)],'o*/',w,h,0)
+%
+%     % Magnitude coloring
+%     colormap spring
+%     arrow3(20*randn(20,3),50*randn(20,3),'|',[],[],0)
+%     set(gca,'color',0.7*[1,1,1])
+%     set(gcf,'color',0.5*[1,1,1]), grid on, colorbar
+%     pause % change the ColorMap and update colors
+%     colormap hot
+%     arrow3('update','colors')
+%
+%     % LogLog plot
+%     set(gca,'xscale','log','yscale','log');
+%     axis([1e2,1e8,1e-2,1e-1]); hold on
+%     p1=repmat([1e3,2e-2],15,1);
+%     q1=[1e7,1e6,1e5,1e4,1e3,1e7,1e7,1e7,1e7,1e7,1e7,1e6,1e5,1e4,1e3];
+%     q2=1e-2*[9,9,9,9,9,7,5,4,3,2,1,1,1,1,1]; p2=[q1',q2'];
+%     global ColorOrder, ColorOrder=[];
+%     set(gca,'ColorOrder',rand(15,3))
+%     arrow3(p1,p2,'o'), grid on, hold off
+%
+%     % SemiLogX plot
+%     set(gca,'xscale','log','yscale','linear');
+%     axis([1e2,1e8,1e-2,1e-1]); hold on
+%     p1=repmat([1e3,0.05],15,1);
+%     q1=[1e7,1e6,1e5,1e4,1e3,1e7,1e7,1e7,1e7,1e7,1e7,1e6,1e5,1e4,1e3];
+%     q2=1e-2*[9,9,9,9,9,7,5,4,3,2,1,1,1,1,1]; p2=[q1',q2'];
+%     arrow3(p1,p2,'x'), grid on, hold off
+%
+%     % SemiLogY plot
+%     set(gca,'xscale','linear','yscale','log');
+%     axis([2,8,1e-2,1e-1]); hold on
+%     p1=repmat([3,2e-2],17,1);
+%     q1=[7,6,5,4,3,7,7,7,7,7,7,7,7,6,5,4,3];
+%     q2=1e-2*[9,9,9,9,9,8,7,6,5,4,3,2,1,1,1,1,1]; p2=[q1',q2'];
+%     set(gca,'LineStyleOrder',{'-','--','-.',':'})
+%     arrow3(p1,p2,'*',1,[],0), grid on, hold off
+%
+%     % Color tables
+%     arrow3('colors')           % default color table
+%     arrow3('colors',0.3)       % low contrast color table
+%     arrow3('colors',0.5)       % high contrast color table
+%
+%     % Update initial point markers and arrowheads
+%     % relative to the current PlotBox diagonal
+%     arrow3('update')           % redraw same size
+%     arrow3('update',2)         % redraw double size
+%     arrow3('update',0.5)       % redraw half size
+%     arrow3('update',[0.5,2,1]) % redraw W half size,
+%                                %        H double size, and
+%                                %        IP same size
+%
+%     See also (arrow3_examples.html), (arrow3_messes_up_my_plots.html).
+
+%   Copyright(c)2002-2008 Version 5.13
+%     Tom Davis  (tdavis@metzgerwillard.com)
+%     Jeff Chang (cpmame@hotmail.com)
+
+%   Revision History:
+%
+%     05/13/09 - Corrected spelling errors (TD)
+%     03/16/08 - Updated contact information (TD)
+%     10/23/07 - Corrected zero magnitude exclusion (TD)
+%     09/08/07 - Added cone plot option; removed adaptive grid
+%                spacing; corrected scale factor; removed "nearly"
+%                tight limits (TD)
+%     07/24/07 - Ignore zero-magnitude input (TD)
+%     07/08/07 - Modified named colors to match named Simulink
+%                colors; added light and dark shades for basic
+%                colors (ymcrgb) (TD)
+%     07/01/07 - Modified named colors to match default ColorOrder
+%                colors (TD)
+%     06/24/07 - Error checking for empty P1, P2 (TD)
+%     06/17/07 - Trim colors,W,H,IP,ALPHA to LENGTH(P1) (TD)
+%     05/27/07 - Magnitude coloring and documentation revision (TD)
+%     03/10/07 - Improved code metrics (TD)
+%     02/21/07 - Preserve existing axis appearance;
+%                use relative sizes for W, H, and IP;
+%                removed version checking; minor bug fixes (TD) 
+%     01/09/04 - Replaced calls to LINSPACE, INTERP1, and
+%                COLORMAP (TD)
+%     12/17/03 - Semilog examples, CAXIS support, magnitude
+%                coloring, and color updating; use CData instead
+%                of FaceColor; minor bug fixes (TD)
+%     07/17/03 - Changed 2D rendering from OpenGL to ZBuffer;
+%                defined HN for COLORS and UPDATE options (TD)
+%     02/27/03 - Replaced calls to RANDPERM, VIEW, REPMAT, SPHERE,
+%                and CYLINDER; added ZBuffer for log plots, RESET
+%                for CLA and CLF, and ABS for W and H (TD)
+%     02/01/03 - Added UPDATE scale factor and MATLAB version
+%                checking, replaced call to CROSS (TD)
+%     12/26/02 - Added UserData and UPDATE option (TD)
+%     11/16/02 - Added more named colors, color code prefix,
+%                global ColorOrder, ALPHA , and BETA (TD)
+%     10/12/02 - Added global LineWidthOrder,
+%                vectorized W, H and IP (TD)
+%     10/05/02 - Changed CLF to CLA for subplot support,
+%                added ColorOrder and LineStyleOrder support (TD)
+%     04/27/02 - Minor log plot revisions (TD)
+%     03/26/02 - Added log plot support (TD)
+%     03/24/02 - Adaptive grid spacing control to trade off
+%                appearance vs. speed based on size of matrix (JC)
+%     03/16/02 - Added "axis tight" for improved appearance (JC)
+%     03/12/02 - Added initial point marker (TD)
+%     03/03/02 - Added aspect ratio support (TD)
+%     03/02/02 - Enhanced program's user friendliness (JC)
+%                (lump Color, LineStyle, and LineWidth together)
+%     03/01/02 - Replaced call to ROTATE (TD)
+%     02/28/02 - Modified line plotting,
+%                added linewidth and linestyle (TD)
+%     02/27/02 - Minor enhancements on 3D appearance (JC)
+%     02/26/02 - Minor enhancements for speed (TD&JC)
+%     02/26/02 - Optimize PLOT3 and SURF for speed (TD)
+%     02/25/02 - Return handler, error handling, color effect,
+%                generalize for 2D/3D vectors (JC)
+%     02/24/02 - Optimize PLOT3 and SURF for speed (TD)
+%     02/23/02 - First release (JC&TD)
+
+%-------------------------------------------------------------------------
+% Error Checking
+global LineWidthOrder ColorOrder
+if nargin<8 || isempty(beta), beta=0.4; end
+beta=abs(beta(1)); if nargout, hn=[]; end
+if strcmpi(p1,'colors')                            % plot color table
+  if nargin>1, beta=abs(p2(1)); end
+  LocalColorTable(1,beta); return
+end
+fig=gcf; ax=gca;
+if strcmpi(p1,'update'), ud=get(ax,'UserData');    % update
+  LocalLogCheck(ax);
+  if size(ud,2)<13, error('Invalid UserData'), end
+  set(ax,'UserData',[]); sf=[1,1,1]; flag=0;
+  if nargin>1
+    if strcmpi(p2,'colors'), flag=1;               % update colors
+    elseif ~isempty(p2)                            % update surfaces
+      sf=p2(1)*sf; n=length(p2(:));
+      if n>1, sf(2)=p2(2); if n>2, sf(3)=p2(3); end, end
+    end
+  end
+  H=LocalUpdate(fig,ax,ud,sf,flag); if nargout, hn=H; end, return
+end
+InputError=['Invalid input, type HELP ',upper(mfilename),...
+  ' for usage examples'];
+if nargin<2, error(InputError), end
+[r1,c1]=size(p1); [r2,c2]=size(p2);
+if c1<2 || c1>3 || r1*r2==0, error(InputError), end
+if r1~=r2, error('P1 and P2 must have same number of rows'), end
+if c1~=c2, error('P1 and P2 must have same number of columns'), end
+p=sum(abs(p2-p1),2)~=0; cone=0;
+if nargin>5 && ~isempty(ip) && strcmpi(ip,'cone')  % cone plot
+  cone=1; p=sum(p2,2)~=0;
+  if ~any(p), error('P2 cannot equal 0'), end
+  set(ax,'tag','Arrow3ConePlot');
+elseif ~any(p), error('P1 cannot equal P2')
+end
+if ~all(p)
+  warning('Arrow3:ZeroMagnitude','Zero magnitude ignored')
+  p1=p1(p,:); p2=p2(p,:); [r1,c1]=size(p1);
+end
+n=r1; Zeros=zeros(n,1);
+if c1==2, p1=[p1,Zeros]; p2=[p2,Zeros];
+elseif ~any([p1(:,3);p2(:,3)]), c1=2; end
+L=get(ax,'LineStyleOrder'); C=get(ax,'ColorOrder');
+ST=get(ax,'DefaultSurfaceTag'); LT=get(ax,'DefaultLineTag');
+EC=get(ax,'DefaultSurfaceEdgeColor');
+if strcmp(get(ax,'nextplot'),'add') && strcmp(get(fig,'nextplot'),'add')
+  Xr=get(ax,'xlim'); Yr=get(ax,'ylim'); Zr=get(ax,'zlim');
+  [xs,ys,xys]=LocalLogCheck(ax); restore=1;
+  if xys, mode='auto';
+    if any([p1(:,3);p2(:,3)]), error('3D log plot not supported'), end
+    if (xs && ~all([p1(:,1);p2(:,1)]>0)) || ...
+       (ys && ~all([p1(:,2);p2(:,2)]>0))
+       error('Nonpositive log data not supported')
+    end
+  else mode='manual';
+    if strcmp(get(ax,'WarpToFill'),'on')
+      warning('Arrow3:WarpToFill',['Stretch-to-fill scaling not ',...
+        'supported;\nuse DASPECT or PBASPECT before calling ARROW3.']);
+    end
+  end
+  set(ax,'XLimMode',mode,'YLimMode',mode,'ZLimMode',mode,...
+    'CLimMode','manual');
+else restore=0; cla reset; xys=0; set(fig,'nextplot','add');
+  if c1==2, azel=[0,90]; else azel=[-37.5,30]; end
+  set(ax,'UserData',[],'nextplot','add','View',azel);
+end
+
+%-------------------------------------------------------------------------
+% Style Control
+[vc,cn]=LocalColorTable(0); prefix=''; OneColor=0;
+if nargin<3, [c,ls,lw]=LocalValidateCLSW;% default color, linestyle/width
+else 
+  [c,ls,lw]=LocalValidateCLSW(s);
+  if length(c)>1, if sum('_^'==c(1)), prefix=c(1); end, c=c(2); end
+  if c=='x'                              % random named color (less white)
+    [ignore,i]=sort(rand(1,23)); c=cn(i,:);        %#ok
+  elseif c=='o'                                    % ColorOrder
+    if length(ColorOrder)
+      [c,failed]=LocalColorMap(lower(ColorOrder),vc,cn,beta);
+      if failed, ColorOrderWarning=['Invalid ColorOrder ',...
+        'variable, current ColorOrder property will be used'];
+        warning('Arrow3:ColorOrder',ColorOrderWarning)
+      else C=c;
+      end
+    end, c=C;
+  elseif c=='|', map=get(fig,'colormap');          % magnitude coloring
+    M=(p1-p2); M=sqrt(sum(M.*M,2)); minM=min(M); maxM=max(M);
+    if maxM-minM<1, minM=0; end
+    set(ax,'clim',[minM,maxM]); c=LocalInterp(minM,maxM,map,M);
+  elseif ~sum(vc==c), c='k'; ColorWarning=['Invalid color switch, ',...
+    'default color (black) will be used'];
+    warning('Arrow3:Color',ColorWarning)
+  end
+end
+if length(c)==1                                    % single color
+  c=LocalColorMap([prefix,c],vc,cn,beta); OneColor=1;
+end
+set(ax,'ColorOrder',c); c=LocalRepmat(c,[ceil(n/size(c,1)),1]);
+if ls~='*', set(ax,'LineStyleOrder',ls); end       % LineStyleOrder
+if lw=='/'                                         % LineWidthOrder
+  if length(LineWidthOrder)
+    lw=LocalRepmat(LineWidthOrder(:),[ceil(n/length(LineWidthOrder)),1]);
+  else lw=0.5; LineWidthOrderWarning=['Undefined LineWidthOrder, ',...
+    'default width (0.5) will be used'];
+    warning('Arrow3:LineWidthOrder',LineWidthOrderWarning)
+  end
+end
+if nargin<4 || isempty(w), w=1; end                % width
+w=LocalRepmat(abs(w(:)),[ceil(n/length(w)),1]);
+if nargin<5 || isempty(h), h=3*w; end              % height
+h=LocalRepmat(abs(h(:)),[ceil(n/length(h)),1]);
+if nargin>5 && ~isempty(ip) && ~cone               % ip
+  ip=LocalRepmat(ip(:),[ceil(n/length(ip)),1]);
+  i=find(ip==0); ip(i)=w(i);
+else ip=-ones(n,1);
+end
+if nargin<7 || isempty(alpha), alpha=1; end
+a=LocalRepmat(alpha(:),[ceil(n/length(alpha)),1]); % FaceAlpha
+
+%-------------------------------------------------------------------------
+% Log Plot
+if xys
+  units=get(ax,'units'); set(ax,'units','points');
+  pos=get(ax,'position'); set(ax,'units',units);
+  if strcmp(get(ax,'PlotBoxAspectRatioMode'),'auto')
+    set(ax,'PlotBoxAspectRatio',[pos(3),pos(4),1]);
+  end
+  par=get(ax,'PlotBoxAspectRatio');
+  set(ax,'DataAspectRatio',[par(2),par(1),par(3)]);
+  % map coordinates onto unit square
+  q=[p1;p2]; xr=Xr; yr=Yr;
+  if xs, xr=log10(xr); q(:,1)=log10(q(:,1)); end
+  if ys, yr=log10(yr); q(:,2)=log10(q(:,2)); end
+  q=q-LocalRepmat([xr(1),yr(1),0],[2*n,1]);
+  dx=xr(2)-xr(1); dy=yr(2)-yr(1);
+  q=q*diag([1/dx,1/dy,1]);
+  q1=q(1:n,:); q2=q(n+1:end,:);
+else xs=0; ys=0; dx=0; dy=0; xr=0; yr=0;
+end
+
+%-------------------------------------------------------------------------
+% Line
+if ~cone
+  set(ax,'DefaultLineTag','arrow3');
+  if length(lw)==1
+    if lw>0
+      if OneColor && ls(end)~='*' && n>1 % single color, linestyle/width
+        P=zeros(3*n,3); i=1:n;
+        P(3*i-2,:)=p1(i,:); P(3*i-1,:)=p2(i,:); P(3*i,1)=NaN;
+        H1=plot3(P(:,1),P(:,2),P(:,3),'LineWidth',lw);
+      else                               % single linewidth
+        H1=plot3([p1(:,1),p2(:,1)]',[p1(:,2),p2(:,2)]',...
+          [p1(:,3),p2(:,3)]','LineWidth',lw);
+      end
+    else H1=[];
+    end
+  else                                   % use LineWidthOrder
+    ls=LocalRepmat(cellstr(L),[ceil(n/size(L,1)),1]);
+    H1=Zeros;
+    for i=1:n
+      H1(i)=plot3([p1(i,1),p2(i,1)],[p1(i,2),p2(i,2)],...
+        [p1(i,3),p2(i,3)],ls{i},'Color',c(i,:),'LineWidth',lw(i));
+    end
+  end
+else                                     % cone plot
+  P=zeros(3*n,3); i=1:n;
+  P(3*i-2,:)=p1(i,:); P(3*i-1,:)=p1(i,:); P(3*i,1)=NaN;
+  H1=plot3(P(:,1),P(:,2),P(:,3));
+end
+
+%-------------------------------------------------------------------------
+% Scale
+if ~restore, axis tight, end
+ar=get(ax,'DataAspectRatio'); ar=sqrt(3)*ar/norm(ar);
+set(ax,'DataAspectRatioMode','manual');
+if xys, sf=1;
+else xr=get(ax,'xlim'); yr=get(ax,'ylim'); zr=get(ax,'zlim');
+  sf=norm(diff([xr;yr;zr],1,2)./ar')/72;
+end
+
+%-------------------------------------------------------------------------
+% UserData
+c=c(1:n,:); w=w(1:n); h=h(1:n); ip=ip(1:n); a=a(1:n);
+set(ax,'UserData',[get(ax,'UserData');p1,p2,c,w,h,ip,a]);
+
+%-------------------------------------------------------------------------
+% Arrowhead
+whip=sf*[w,h,ip];
+if xys, whip=whip*sqrt(2)/72; p1=q1; p2=q2; end
+w=whip(:,1); h=whip(:,2); ip=whip(:,3);
+if cone                                            % cone plot
+  delete(H1), H1=[];
+  p2=p2./LocalRepmat(sqrt(sum(p2.*p2,2)),[1,3]);
+  p2=p1+p2.*LocalRepmat(ar,[n,1]).*LocalRepmat(h,[1,3]);
+end
+W=(p1-p2)./LocalRepmat(ar,[n,1]);
+W=W./LocalRepmat(sqrt(sum(W.*W,2)),[1,3]);         % new z direction
+U=[-W(:,2),W(:,1),Zeros];
+N=sqrt(sum(U.*U,2)); i=find(N<eps); j=length(i);
+U(i,:)=LocalRepmat([1,0,0],[j,1]); N(i)=ones(j,1);
+U=U./LocalRepmat(N,[1,3]);                         % new x direction
+V=[W(:,2).*U(:,3)-W(:,3).*U(:,2),...               % new y direction
+  W(:,3).*U(:,1)-W(:,1).*U(:,3),...
+  W(:,1).*U(:,2)-W(:,2).*U(:,1)];
+
+m=20;                               % surface grid spacing
+set(ax,'DefaultSurfaceTag','arrow3','DefaultSurfaceEdgeColor','none');
+r=[0;1]; theta=(0:m)/m*2*pi; Ones=ones(1,m+1);
+x=r*cos(theta); y=r*sin(theta); z=r*Ones;
+G=surface(x/2,y/2,z); dar=diag(ar);
+X=get(G,'XData'); Y=get(G,'YData'); Z=get(G,'ZData');
+H2=Zeros; [j,k]=size(X);
+for i=1:n                           % translate, rotate, and scale
+  H2(i)=copyobj(G,ax);
+  xyz=[w(i)*X(:),w(i)*Y(:),h(i)*Z(:)]*[U(i,:);V(i,:);W(i,:)]*dar;
+  x=reshape(xyz(:,1),j,k)+p2(i,1);
+  y=reshape(xyz(:,2),j,k)+p2(i,2);
+  z=reshape(xyz(:,3),j,k)+p2(i,3);
+  LocalSetSurface(xys,xs,ys,dx,dy,xr,yr,...
+    x,y,z,a(i),c(i,:),H2(i),2,m+1);
+end
+delete(G);
+
+%-------------------------------------------------------------------------
+% Initial Point Marker
+if any(ip>0)
+  theta=(-m:2:m)/m*pi; phi=(-m:2:m)'/m*pi/2; cosphi=cos(phi);
+  x=cosphi*cos(theta); y=cosphi*sin(theta); z=sin(phi)*Ones;
+  G=surface(x*ar(1)/2,y*ar(2)/2,z*ar(3)/2);
+  X=get(G,'XData'); Y=get(G,'YData'); Z=get(G,'ZData');
+  H3=zeros(n,1);
+  for i=1:n                                        % translate
+    if ip(i)>0
+      H3(i)=copyobj(G,ax);
+      x=p1(i,1)+X*ip(i); y=p1(i,2)+Y*ip(i); z=p1(i,3)+Z*ip(i);
+      LocalSetSurface(xys,xs,ys,dx,dy,xr,yr,...
+        x,y,z,a(i),c(i,:),H3(i),m+1,m+1);
+    end
+  end, delete(G);
+else H3=[];
+end
+
+%-------------------------------------------------------------------------
+% Finish
+if restore, xr=Xr; yr=Yr; zr=Zr;
+  if xys, set(ax,'DataAspectRatioMode','auto'); end
+else
+  axis tight
+  xr=get(ax,'xlim'); yr=get(ax,'ylim'); zr=get(ax,'zlim');
+  set(ax,'nextplot','replace');
+end
+azel=get(ax,'view');
+%if abs(azel(2))==90, renderer='ZBuffer'; else renderer='OpenGL'; c1=3; end
+%set(fig,'Renderer',renderer);
+set(ax,'LineStyleOrder',L,'ColorOrder',C,'DefaultLineTag',LT,...
+  'DefaultSurfaceTag',ST,'DefaultSurfaceEdgeColor',EC,...
+  'xlim',xr,'ylim',yr,'zlim',zr,'clim',get(ax,'CLim'));
+if c1==3, set(ax,'CameraViewAngle',get(ax,'CameraViewAngle'),...
+   'PlotBoxAspectRatio',get(ax,'PlotBoxAspectRatio'));
+end
+if nargout, hn=[H1(:);H2(:);H3(:)]; end
+
+%-------------------------------------------------------------------------
+% Local Functions
+%-------------------------------------------------------------------------
+% Update
+function H=LocalUpdate(fig,ax,ud,sf,flag)
+global ColorOrder
+p1=ud(:,1:3); p2=ud(:,4:6); c=ud(:,7:9); a=ud(:,13);
+w=sf(1)*ud(:,10); h=sf(2)*ud(:,11); ip=sf(3)*ud(:,12);
+H=get(ax,'children'); tag=get(H,'tag'); type=get(H,'type');
+delete(H(strcmp(tag,'arrow3') & strcmp(type,'surface')));
+set(fig,'nextplot','add'); set(ax,'nextplot','add'); H1=[];
+if flag, map=get(fig,'colormap');                  % update colors
+  M=(p1-p2); M=sqrt(sum(M.*M,2)); minM=min(M); maxM=max(M);
+  H1=H(strcmp(tag,'arrow3') & strcmp(type,'line'));
+  MagnitudeWarning=['Cannot perform magnitude coloring on lines ',...
+    'that\nwere drawn with a single color, linestyle, and linewidth'];
+  if length(H1)>1
+    for i=1:length(H1)                             % update line colors
+      x=get(H1(i),'xdata'); y=get(H1(i),'ydata'); z=get(H1(i),'zdata');
+      if length(x)>2                               % multiple lines
+        warning('Arrow3:Magnitude',MagnitudeWarning), continue
+      end
+      m=sqrt((x(1)-x(2))^2+(y(1)-y(2))^2+(z(1)-z(2))^2);
+      c=LocalInterp(minM,maxM,map,m); set(H1(i),'color',c);
+    end
+  elseif length(H1)==1
+    warning('Arrow3:Magnitude',MagnitudeWarning)
+  end
+  c=LocalInterp(minM,maxM,map,M);
+end
+set(ax,'ColorOrder',c);                            % update surfaces
+ColorOrder=[];
+if strcmp(get(ax,'tag'),'Arrow3ConePlot')
+     H=arrow3(p1,p2,'o' ,w,h,'cone',a);            % update cones
+else H=arrow3(p1,p2,'o0',w,h,    ip,a);
+end, H=[H1(:);H(:)];
+set(ax,'nextplot','replace');
+
+%-------------------------------------------------------------------------
+% SetSurface
+function LocalSetSurface(xys,xs,ys,dx,dy,xr,yr,x,y,z,a,c,H,n,m)
+if xys
+  x=x*dx+xr(1); y=y*dy+yr(1);
+  if xs, x=10.^x; end
+  if ys, y=10.^y; end
+end
+cd=zeros(n,m,3); cd(:,:,1)=c(1); cd(:,:,2)=c(2); cd(:,:,3)=c(3);
+set(H,'XData',x,'YData',y,'ZData',z,'CData',cd,'FaceAlpha',a);
+
+%-------------------------------------------------------------------------
+% ColorTable
+function [vc,cn]=LocalColorTable(n,beta)
+vc='kymcrgbadefhijlnpqstuvzw';                     % valid color codes
+%                k               y               m               c
+cn=[0.00,0.00,0.00; 1.00,1.00,0.00; 1.00,0.00,1.00; 0.00,1.00,1.00;
+%                r               g               b               a
+    1.00,0.00,0.00; 0.00,1.00,0.00; 0.00,0.00,1.00; 0.42,0.59,0.24;
+%                d               e               f               h
+    0.25,0.25,0.25; 0.00,0.50,0.00; 0.70,0.13,0.13; 1.00,0.41,0.71;
+%                i               j               l               n
+    0.29,0.00,0.51; 0.00,0.66,0.42; 0.50,0.50,0.50; 0.50,0.20,0.00;
+%                p               q               s               t
+    0.75,0.75,0.00; 1.00,0.50,0.00; 0.00,0.75,0.75; 0.80,0.34,0.00;
+%                u               v               z               w
+    0.50,0.00,0.13; 0.75,0.00,0.75; 0.38,0.74,0.99; 1.00,1.00,1.00];
+
+% Named Simulink Colors (zaql)
+% LightBlue = 0.38  0.74  0.99 = aZure
+% DarkGreen = 0.42  0.59  0.24 = Asparagus
+% Orange    = 1.00  0.50  0.00 = kumQuat
+% Gray      = 0.50  0.50  0.50 = Light gray
+%
+% Default ColorOrder Property Colors (bersvpd)
+% Color1    = 0.00  0.00  1.00 = Blue
+% Color2    = 0.00  0.50  0.00 = Evergreen
+% Color3    = 1.00  0.00  0.00 = Red
+% Color4    = 0.00  0.75  0.75 = Sky blue
+% Color5    = 0.75  0.00  0.75 = Violet
+% Color6    = 0.75  0.75  0.00 = Pear
+% Color7    = 0.25  0.25  0.25 = Dark gray
+
+if n, clf reset                                    % plot color table
+  name={'blacK','Yellow','Magenta','Cyan',...
+    'Red','Green','Blue','Asparagus',...
+    'Dark gray','Evergreen','Firebrick','Hot pink',...
+    'Indigo','Jade','Light gray','Nutbrown',...
+    'Pear','kumQuat','Sky blue','Tawny',...
+    'bUrgundy','Violet','aZure','White'};
+  c=['yptn';'gjae';'czsb';'hmvi';'qrfu';'wldk'];
+  set(gcf,'DefaultAxesXTick',[],'DefaultAxesYTick',[],...
+    'DefaultAxesXTickLabel',[],'DefaultAxesYTickLabel',[],...
+    'DefaultAxesXLim',[0,0.75],'DefaultAxesYLim',[0,0.75],...
+    'DefaultRectangleEdgeColor','none');
+  for i=1:24, subplot(4,6,i); box on
+    j=find(vc==c(i)); title(name{j});
+    dark=LocalBrighten(cn(j,:),-beta);
+    light=LocalBrighten(cn(j,:),beta);
+    rectangle('Position',[0,0.00,0.75,0.25],'FaceColor',dark);
+    rectangle('Position',[0,0.25,0.75,0.25],'FaceColor',cn(j,:));
+    rectangle('Position',[0,0.50,0.75,0.25],'FaceColor',light);
+    rectangle('Position',[0,0.00,0.75,0.75],'EdgeColor','k');
+    if rem(i,6)==1
+      set(gca,'YTickLabel',{'dark','normal','light'},...
+        'YTick',[0.125,0.375,0.625]);
+      if i==19
+        text(0,-0.25,['{\bf\itARROW3}  Named Color Table  ',...
+            '( \beta = ',num2str(beta),' )']);
+      end
+    end
+  end
+end
+
+%-------------------------------------------------------------------------
+% ColorMap
+function [C,failed]=LocalColorMap(c,vc,cn,beta)
+n=length(c); failed=0; C=zeros(n,3); i=1; j=1;
+while 1
+  if ~sum([vc,'_^']==c(i)), failed=1; break, end
+  if sum('_^'==c(i))
+    if i+1>n, failed=1; break, end
+    if ~sum(vc==c(i+1)), failed=1; break, end
+    cc=cn(vc==c(i+1),:); gamma=beta;
+    if c(i)=='_', gamma=-beta; end
+    C(j,:)=LocalBrighten(cc,gamma); i=i+2;
+  else C(j,:)=cn(vc==c(i),:); i=i+1;
+  end
+  if i>n, break, end, j=j+1;
+end
+if n>j, C(j+1:n,:)=[]; end
+
+%-------------------------------------------------------------------------
+% Brighten
+function C=LocalBrighten(c,beta)
+if sum([c==0,c==1])==3 && sum(c==0)<3 && sum(c==1)<3
+  if beta<0
+    C=(1+beta)*c;
+  else
+    C=c;  C(C==0)=beta;
+  end
+else
+  C=c.^((1-min(1-sqrt(eps),abs(beta)))^sign(beta));
+end
+
+%-------------------------------------------------------------------------
+% Repmat
+function B=LocalRepmat(A,siz)
+if length(A)==1, B(prod(siz))=A; B(:)=A; B=reshape(B,siz);
+else [m,n]=size(A); mind=(1:m)'; nind=(1:n)';
+  mind=mind(:,ones(1,siz(1))); nind=nind(:,ones(1,siz(2)));
+  B=A(mind,nind);
+end
+
+%-------------------------------------------------------------------------
+% Interp
+function v=LocalInterp(xmin,xmax,y,u)
+[m,n]=size(y); h=(xmax-xmin)/(m-1); p=length(u); v=zeros(p,n);
+k=min(max(1+floor((u-xmin)/h),1),m-1); s=(u-xmin)/h-k+1;
+for j=1:n, v(:,j)=y(k,j)+s.*(y(k+1,j)-y(k,j)); end
+v(v<0)=0; v(v>1)=1;
+
+%-------------------------------------------------------------------------
+% Check for supported log scales
+function [xs,ys,xys]=LocalLogCheck(ax)
+xs=strcmp(get(ax,'xscale'),'log');
+ys=strcmp(get(ax,'yscale'),'log');
+zs=strcmp(get(ax,'zscale'),'log');
+if zs, error('Z log scale not supported'), end
+xys=xs+ys;
+if xys, azel=get(ax,'view');
+  if abs(azel(2))~=90, error('3D log plot not supported'), end
+end
+
+%-------------------------------------------------------------------------
+% Generate valid value for color, linestyle and linewidth
+function [c,ls,lw]=LocalValidateCLSW(s)
+if nargin<1, c='k'; ls='-'; lw=0.5;
+else
+  % identify linestyle
+  if findstr(s,'--'), ls='--'; s=strrep(s,'--','');
+  elseif findstr(s,'-.'), ls='-.'; s=strrep(s,'-.','');
+  elseif findstr(s,'-'), ls='-'; s=strrep(s,'-','');
+  elseif findstr(s,':'), ls=':'; s=strrep(s,':','');
+  elseif findstr(s,'*'), ls='*'; s=strrep(s,'*','');
+  else ls='-';
+  end
+
+  % identify linewidth
+  tmp=double(s);
+  tmp=find(tmp>45 & tmp<58);
+  if length(tmp)
+    if any(s(tmp)=='/'), lw='/'; else lw=str2double(s(tmp)); end
+    s(tmp)='';
+  else lw=0.5;
+  end
+
+  % identify color
+  if length(s), s=lower(s);
+    if length(s)>1, c=s(1:2);
+    else c=s(1); end
+  else c='k';
+  end
+end

lwrserv/matlab/rvctools/common/bresenham.m

@@ -0,0 +1,98 @@
+%BRESENHAM Generate a line
+%
+% P = BRESENHAM(X1, Y1, X2, Y2) is a list of integer coordinates for 
+% points lying on the line segement (X1,Y1) to (X2,Y2).  Endpoints 
+% must be integer.
+%
+% P = BRESENHAM(P1, P2) as above but P1=[X1,Y1] and P2=[X2,Y2].
+%
+% See also ICANVAS.
+
+
+
+% Copyright (C) 1993-2011, by Peter I. Corke
+%
+% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
+% 
+% MVTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% MVTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
+
+function p = bresenham(x1, y1, x2, y2)
+
+    if nargin == 2
+        p1 = x1; p2 = y1;
+
+        x1 = p1(1); y1 = p1(2);
+        x2 = p2(1); y2 = p2(2);
+    elseif nargin ~= 4
+        error('expecting 2 or 4 arguments');
+    end
+
+    x = x1;
+    if x2 > x1
+        xd = x2-x1;
+        dx = 1;
+    else
+        xd = x1-x2;
+        dx = -1;
+    end
+
+    y = y1;
+    if y2 > y1
+        yd = y2-y1;
+        dy = 1;
+    else
+        yd = y1-y2;
+        dy = -1;
+    end
+
+    p = [];
+
+    if xd > yd
+      a = 2*yd;
+      b = a - xd;
+      c = b - xd;
+
+      while 1
+        p = [p; x y];
+        if all([x-x2 y-y2] == 0)
+            break
+        end
+        if  b < 0
+            b = b+a;
+            x = x+dx;
+        else
+            b = b+c;
+            x = x+dx; y = y+dy;
+        end
+      end
+    else
+      a = 2*xd;
+      b = a - yd;
+      c = b - yd;
+
+      while 1
+        p = [p; x y];
+        if all([x-x2 y-y2] == 0)
+            break
+        end
+        if  b < 0
+            b = b+a;
+            y = y+dy;
+        else
+            b = b+c;
+            x = x+dx; y = y+dy;
+        end
+      end
+    end
+end

lwrserv/matlab/rvctools/common/ccodefunctionstring.m

@@ -0,0 +1,236 @@
+%CCODEFUNCTIONSTRING Converts a symbolic expression into a C-code function
+%
+% [FUNSTR, HDRSTR] = ccodefunctionstring(SYMEXPR, ARGLIST) returns a string
+% representing a C-code implementation of a symbolic expression SYMEXPR.
+% The C-code implementation has a signature of the form:
+%
+%         void funname(double[][n_o] out, const double in1, 
+%           const double* in2, const double[][n_i] in3);
+%
+% depending on the number of inputs to the function as well as the
+% dimensionality of the inputs (n_i) and the output (n_o).
+% The whole C-code implementation is returned in FUNSTR, while HDRSTR
+% contains just the signature ending with a semi-colon (for the use in 
+% header files).
+%
+% Options::
+% 'funname',name    Specify the name of the generated C-function. If
+%                   this optional argument is omitted, the variable name 
+%                   of the first input argument is used, if possible.
+% 'output',outVar   Defines the identifier of the output variable in the C-function.
+% 'vars',varCells   The inputs to the C-code function must be defined as a cell array. The
+%                   elements of this cell array contain the symbolic variables required to
+%                   compute the output. The elements may be scalars, vectors or matrices
+%                   symbolic variables. The C-function prototype will be composed accoringly
+%                   as exemplified above.
+% 'flag',sig        Specifies if function signature only is generated, default (false).
+%
+% Example::
+%          % Create symbolic variables
+%          syms q1 q2 q3
+%
+%          Q = [q1 q2 q3];
+%          % Create symbolic expression
+%          myrot = rotz(q3)*roty(q2)*rotx(q1)
+%
+%          % Generate C-function string
+%          [funstr, hdrstr] = ccodefunctionstring(myrot,'output','foo', ...
+%          'vars',{Q},'funname','rotate_xyz')
+%
+% Notes::
+% - The function wraps around the built-in Matlab function 'ccode'. It does
+%   not check for proper C syntax. You must take care of proper
+%   dimensionality of inputs and outputs with respect to your symbolic
+%   expression on your own. Otherwise the generated C-function may not
+%   compile as desired.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also ccode, matlabFunction.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [funstr hdrstr] = ccodefunctionstring(f,varargin)
+
+
+% option defaults
+opt.funname = inputname(1);
+opt.output{1} = zeros(size(f));
+opt.outputName{1} = inputname(1);
+opt.flag = 0;
+
+if isempty(opt.outputName{1})
+    opt.outputName{1} = 'myout';
+end
+opt.vars = {};
+
+% tb_optparse is not applicable here,
+% since handling cell inputs and extracting input variable names is
+% required.
+% Thus, scan varargin manually:
+if mod(nargin,2)==0
+    error('CodeGenerator:codefunctionstring:wrongArgumentList',...
+        'Wrong number of elements in the argument list.');
+end
+for iArg = 1:2:nargin-1
+    switch lower(varargin{iArg})
+        case 'funname'
+            opt.funname = varargin{iArg+1};
+        case 'output'
+            if ~isempty(varargin{iArg+1})
+                opt.outputName{1} = varargin{iArg+1};
+            end
+        case 'vars'
+            opt.vars = varargin{iArg+1};
+        case 'flag'
+            opt.flag = varargin{iArg+1};
+        otherwise
+            error('ccodefunctionstring:unknownArgument',...
+                ['Argument ',inputname(iArg),' unknown.']);
+    end
+end
+
+nOut = numel(opt.output);
+nIn = numel(opt.vars);
+
+%% Function signature
+funstr = sprintf('void %s(', opt.funname);
+initstr = '';
+
+% outputs
+for iOut = 1:nOut
+    tmpOutName = opt.outputName{iOut};
+    tmpOut = opt.output{iOut};
+    
+    if ~isscalar(tmpOut);
+        funstr = [funstr, sprintf('double %s[][%u]', tmpOutName, size(tmpOut,1) ) ];
+        for iRow = 1:size(tmpOut,1)
+            for iCol = 1:size(tmpOut,2)
+                initstr = sprintf(' %s %s[%u][%u]=0;\n',initstr,tmpOutName,iCol-1,iRow-1);
+            end
+        end
+    else
+        funstr = [funstr, sprintf('double %s', tmpOutName ) ];
+    end
+    
+    % separate argument list by commas
+    if ( iOut ~= nOut ) || ( nIn > 0 )
+        funstr = [funstr,', '];
+    end
+    
+end
+
+% inputs
+for iIn = 1:nIn
+    tmpInName = ['input',num2str(iIn)];%opt.varsName{iIn};
+    tmpIn = opt.vars{iIn};
+    
+    % treat different dimensionality of input variables
+    if isscalar(tmpIn)
+        funstr = [funstr, sprintf('const double %s', tmpInName ) ];
+    elseif isvector(tmpIn)
+        funstr = [funstr, sprintf('const double* %s', tmpInName ) ];
+    elseif ismatrix(tmpIn)
+        funstr = [funstr, sprintf('const double %s[][%u]', tmpInName, size(tmpIn,2) ) ];
+    else
+        error('ccodefunctionstring:UnsupportedOutputType', 'Unsupported datatype for %s', tmpOutName)
+    end
+    
+    % separate argument list by commas
+    if ( iIn ~= nIn )
+        funstr = [funstr,', '];
+    end
+    
+end
+funstr = [funstr,sprintf('%s', ')')];
+
+% finalize signature for the use in header files
+if nargout > 1
+    hdrstr = [funstr,sprintf('%s', ';')];
+end
+if opt.flag
+    return;         %% STOP IF FLAG == TRUE
+end
+
+% finalize signature for use in function definition
+funstr = [funstr,sprintf('%s', '{')];
+funstr = sprintf('%s\n%s',funstr,sprintf('%s', ' ') ); % empty line
+
+%% Function body
+% input paramter expansion
+for iIn = 1:nIn
+    tmpInName = ['input',num2str(iIn)];%opt.varsName{iIn};
+    tmpIn = opt.vars{iIn};
+    
+    % for scalars
+    %   -> do nothing
+    
+    % for vectors
+    if ~isscalar(tmpIn) && isvector(tmpIn)
+        nEl = numel(tmpIn);
+        for iEl = 1:nEl
+            funstr = sprintf('%s\n%s',...
+                funstr,...
+                sprintf('  double %s = %s[%u];', char(tmpIn(iEl)), tmpInName,iEl-1 ));
+        end
+        
+        % for matrices
+    elseif ~isscalar(tmpIn) && ~isvector(tmpIn) && ismatrix(tmpIn)
+        nRow = size(tmpIn,1);
+        nCol = size(tmpIn,2);
+        for iRow = 1:nRow
+            for iCol = 1:nCol
+                
+                funstr = sprintf('%s\n%s',...
+                    funstr,...
+                    sprintf('  double %s%u%u = %s[%u][%u];', char(tmpIn(iRow,iCol)), iRow, iCol, tmpInName{iIn},iRow-1,iCol-1 ));
+            end
+        end
+    end
+    
+end
+
+funstr = sprintf('%s\n%s',...
+    funstr,...
+    sprintf('%s', ' ') );
+funstr = sprintf('%s\n%s',...
+    funstr,...
+    sprintf('%s\n\n', initstr) );
+
+% Actual code
+% use f.' here, because of column/row indexing in C
+ eval([opt.outputName{1}, ' = f.''; codestr = ccode(',opt.outputName{1},');'])
+
+if isscalar(f)
+    % in the case of scalar expressions the resulting ccode always
+    % begins with '  t0'. Replace that with the desired name.
+    codestr(1:4) = [];
+    codestr = [opt.outputName{1}, codestr];
+end
+
+funstr = sprintf('%s\n%s',...
+    funstr,...
+    codestr );
+
+funstr = sprintf('%s\n%s',...
+    funstr,sprintf('%s', '}') );
+funstr = sprintf('%s\n%s',...
+    funstr,sprintf('%s', ' ') ); % empty line

lwrserv/matlab/rvctools/common/circle.m

@@ -0,0 +1,45 @@
+%CIRCLE Compute points on a circle
+%
+% CIRCLE(C, R, OPT) plot a circle centred at C with radius R.
+%
+% X = CIRCLE(C, R, OPT) return an Nx2 matrix whose rows define the 
+% coordinates [x,y] of points around the circumferance of a circle 
+% centred at C and of radius R.
+%
+% C is normally 2x1 but if 3x1 then the circle is embedded in 3D, and X is Nx3,
+% but the circle is always in the xy-plane with a z-coordinate of C(3).
+%
+% Options::
+%  'n',N   Specify the number of points (default 50)
+function out = circle(centre, rad, varargin)
+
+	opt.n = 50;
+    
+    [opt,arglist] = tb_optparse(opt, varargin);
+
+    % compute points on circumference
+	th = [0:opt.n-1]'/ opt.n*2*pi;
+    x = rad*cos(th) + centre(1);
+    y = rad*sin(th) + centre(2);
+
+    % add extra row if z-coordinate is specified, but circle is always in xy plane
+    if length(centre) > 2
+        z = ones(size(x))*centre(3);
+        p = [x y z]';
+    else
+        p = [x y]';
+    end
+
+    if nargout > 0
+        % return now
+        out = p;
+        return;
+    else
+        % else plot the circle
+        p = [p p(:,1)]; % make it close
+        if numrows(p) > 2
+            plot3(p(1,:), p(2,:), p(3,:), arglist{:});
+        else
+            plot(p(1,:), p(2,:), arglist{:});
+        end
+    end

lwrserv/matlab/rvctools/common/colnorm.m

@@ -0,0 +1,7 @@
+%COLNORM Column-wise norm of a matrix
+%
+% CN = COLNORM(A) is an Mx1 vector of the normals of each column of the
+% matrix A which is NxM.
+function n = colnorm(a)
+
+	n = sqrt(sum(a.^2));

lwrserv/matlab/rvctools/common/colorname.m

@@ -0,0 +1,214 @@
+%COLORNAME Map between color names and RGB values
+%
+% RGB = COLORNAME(NAME) is the RGB-tristimulus value (1x3) corresponding to
+% the color specified by the string NAME.  If RGB is a cell-array (1xN) of
+% names then RGB is a matrix (Nx3) with each row being the corresponding
+% tristimulus.
+%
+% XYZ = COLORNAME(NAME, 'xyz') as above but the XYZ-tristimulus value 
+% corresponding to the color specified by the string NAME.
+%
+% XY = COLORNAME(NAME, 'xy') as above but the xy-chromaticity coordinates 
+% corresponding to the color specified by the string NAME.
+%
+% NAME = COLORNAME(RGB) is a string giving the name of the color that is 
+% closest (Euclidean) to the given RGB-tristimulus value (1x3).  If RGB is
+% a matrix (Nx3) then return a cell-array (1xN) of color names.
+%
+% NAME = COLORNAME(XYZ, 'xyz') as above but the color is the closest (Euclidean)
+% to the given XYZ-tristimulus value.
+%
+% NAME = COLORNAME(XYZ, 'xy') as above but the color is the closest (Euclidean)
+% to the given xy-chromaticity value with assumed Y=1.
+%
+% Notes::
+% - Color name may contain a wildcard, eg. "?burnt"
+% - Based on the standard X11 color database rgb.txt.
+% - Tristimulus values are in the range 0 to 1
+
+
+
+% Copyright (C) 1993-2014, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function r = colorname(a, varargin)
+
+    opt.color = {'rgb', 'xyz', 'xy'};
+    opt = tb_optparse(opt, varargin);
+
+    persistent  rgbtable;
+    
+    mvtb_present = exist('tristim2cc');
+    
+    % ensure that the database is loaded
+    if isempty(rgbtable)
+        % load mapping table from file
+        fprintf('loading rgb.txt\n');
+        f = fopen('private/rgb.txt', 'r');
+        k = 0;
+        rgb = [];
+        names = {};
+        xy = [];
+        
+        while ~feof(f),
+            line = fgets(f);
+            if line(1) == '#',
+                continue;
+            end
+            
+            [A,count,errm,next] = sscanf(line, '%d %d %d');
+            if count == 3
+                k = k + 1;
+                rgb(k,:) = A' / 255.0;
+                names{k} = lower( strtrim(line(next:end)) );
+                if mvtb_present
+                    xy = tristim2cc( colorspace('RGB->XYZ', rgb) );
+                end
+            end
+        end
+        s.rgb = rgb;
+        s.names = names;
+        if mvtb_present
+            s.xy = xy;
+        end
+        rgbtable = s;
+    end
+    
+    if isstr(a)
+        % map name to rgb/xy
+        if a(1)  == '?' 
+            % just do a wildcard lookup
+            r = namelookup(rgbtable, a(2:end), opt);
+        else
+            r = name2rgb(rgbtable, a, opt);
+        end
+    elseif iscell(a)
+        % map multiple names to rgb
+        r = [];
+        for name=a,
+            rgb = name2rgb(rgbtable, name{1}, opt.xy);
+            if isempty(rgb)
+                warning('Color %s not found', name{1});
+            end
+            r = [r; rgb];
+        end
+    else
+        % map values to strings
+        switch opt.color
+            case 'rgb'
+                if numel(a) == 3
+                    r = rgb2name(rgbtable, a(:)');
+                elseif numcols(a) ~= 3
+                    error('RGB data must have 3 columns');
+                else
+                    r = {};
+                    for i=1:numrows(a)
+                        r{i} = rgb2name(rgbtable, a(i,:));
+                    end
+                end
+                
+            case 'xyz'
+                if numel(a) == 3
+                    rgb = colorspace('XYZ->RGB', a(:)');
+                    r = rgb2name(rgbtable, rgb);
+                elseif numcols(a) ~= 3
+                    error('XYZ data must have 3 columns');
+                else
+                    rgb = colorspace('XYZ->RGB', a);
+                    
+                    r = {};
+                    for i=1:numrows(a)
+                        r{i} = rgb2name(rgbtable, rgb(i,:));
+                    end
+                end
+                
+            case 'xy'
+                if numel(a) == 2
+                    Y = 1;  XYZ = 1/a(2);
+                    X = a(1) * XYZ;
+                    Z = (1-a(1)-a(2)) * XYZ;
+                    rgb = colorspace('XYZ->RGB', [X Y Z]);
+                    r = rgb2name(rgbtable, rgb);
+                elseif numcols(a) ~= 2
+                    error('xy data must have 2 columns');
+                else
+                    Y = ones(numrows(a),1);  XYZ = 1./a(:,2);
+                    X = a(:,1) .* XYZ;
+                    Z = (1-a(:,1)-a(:,2)) .* XYZ;
+                    rgb = colorspace('XYZ->RGB', [X Y Z]);
+                    
+                    r = {};
+                    for i=1:numrows(a)
+                        r{i} = rgb2name(rgbtable, rgb(i,:));
+                    end
+                end
+        end
+         
+
+    end
+end
+    
+function r = namelookup(table, s)
+    s = lower(s);   % all matching done in lower case
+    
+    r = {};
+    count = 1;
+    for k=1:length(table.names),
+        if ~isempty( findstr(table.names{k}, s) )
+            r{count} = table.names{k};
+            count = count + 1;
+        end
+    end
+end
+
+function out = name2rgb(table, s, opt)
+
+    s = lower(s);   % all matching done in lower case
+    
+    for k=1:length(table.names),
+        if strcmp(s, table.names(k)),
+            rgb = table.rgb(k,:);
+            switch opt.color
+                case 'rgb'
+                    out = rgb;
+                case 'xy'
+                    XYZ = colorspace('RGB->XYZ', r);
+                    out = tristim2cc(XYZ);
+                case 'xyz'
+                    out = colorspace('RGB->XYZ', rgb);
+            end
+            return;
+        end
+    end
+    out = [];
+end
+
+function r = rgb2name(table, v)
+    d = table.rgb - ones(numrows(table.rgb),1) * v;
+    n = colnorm(d');
+    [z,k] = min(n);
+    r = table.names{k};
+end
+
+function r = xy2name(table, v)
+    d = table.xy - ones(numrows(table.xy),1) * v;
+    n = colnorm(d');
+    [z,k] = min(n);
+    r = table.names{k};
+end

lwrserv/matlab/rvctools/common/diff2.m

@@ -0,0 +1,10 @@
+%DIFF2 Two point difference
+%
+% D = DIFF2(V) is the 2-point difference for each point in the vector v
+% and the first element is zero.  The vector D has the same length as V.
+%
+% See also DIFF.
+function d = diff2(v)
+	[r,c] =size(v);
+
+	d = [zeros(1,c); diff(v)];

lwrserv/matlab/rvctools/common/distributeblocks.m

@@ -0,0 +1,85 @@
+%DISTRIBUTEBLOCKS Distribute blocks in Simulink block library
+%
+% distributeBlocks(MODEL) equidistantly distributes blocks in a Simulink 
+% block library named MODEL.
+%
+% Notes::
+% - The MATLAB functions to create Simulink blocks from symbolic
+%   expresssions actually place all blocks on top of each other. This
+%   function scans a simulink model and rearranges the blocks on an
+%   equidistantly spaced grid.
+% - The Simulink model must already be opened before running this
+%   function!
+%
+% Author::
+%  Joern Malzahn   
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de  
+%
+% See also symexpr2slblock, doesblockexist.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [  ] = distributeblocks( mdlName )
+ 
+%% Get a list of all first level blocks:
+blockNames = find_system(mdlName,'SearchDepth',1);
+blockNames = blockNames(2:end); % First cell contains the model name
+
+%% Determine the maximum width and height of a block
+allPosC = get_param(blockNames,'Position');
+allPosM = cell2mat(allPosC); % [left top right bottom] The maximum value for a coordinate is 32767
+
+widths  = allPosM(:,3)-allPosM(:,1);
+heights = allPosM(:,4)-allPosM(:,2);
+maxWidth = max(widths);
+maxHeight = max(heights);
+
+%% Set grid spacing
+wBase = 2*maxWidth;
+hBase = 2*maxHeight;
+
+%% Start rearranging blocks
+nBlocks = size(allPosM,1);
+nColRow = ceil(sqrt(nBlocks));
+
+for iBlocks = 1:nBlocks
+    
+    % block location on grid
+    [row,col] = ind2sub([nColRow, nColRow],iBlocks);
+    
+    % compute block coordinates
+    left = col*wBase;
+    right = left+maxWidth;
+    top = row*hBase;
+    bottom = top+maxHeight;
+    
+    % apply new coordinates
+    set_param(blockNames{iBlocks},'Position',[left top right bottom])
+    
+end
+
+
+end
+

lwrserv/matlab/rvctools/common/dockfigs.m

@@ -0,0 +1,38 @@
+%DOCKFIGS Control figure docking in the GUI
+%
+% dockfigs causes all new figures to be docked into the GUI
+%
+% dockfigs(1) as above.
+%
+% dockfigs(0) causes all new figures to be undocked from the GUI
+
+
+% Copyright (C) 1993-2014, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function dockfigs(arg)
+    if nargin == 0
+        arg = 1;
+    end
+    
+    if arg
+        set(0, 'DefaultFigureWindowStyle', 'docked')
+    else
+        set(0, 'DefaultFigureWindowStyle', 'normal')
+    end

lwrserv/matlab/rvctools/common/doesblockexist.m

@@ -0,0 +1,54 @@
+%DOESBLOCKEXIST Check existence of block in Simulink model
+%
+% RES = doesblockexist(MDLNAME, BLOCKADDRESS) is a logical result that 
+% indicates whether or not the block BLOCKADDRESS exists within the 
+% Simulink model MDLNAME.
+%
+% Author::
+%  Joern Malzahn   
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de  
+%
+% See also symexpr2slblock, distributeblocks.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [res] = doesblockexist(mdlName, blockAddress)
+ 
+wasntLoaded = 0;
+if ~bdIsLoaded(mdlName)
+    load_system(mdlName)
+    wasntLoaded = 1;
+end
+
+blockList = find_system(mdlName);
+blockList = strfind(blockList,blockAddress);
+emptyList = cellfun(@isempty,blockList);
+res = any(~emptyList);
+
+
+if wasntLoaded
+    close_system(mdlName)
+end
+

lwrserv/matlab/rvctools/common/e2h.m

@@ -0,0 +1,9 @@
+%E2H Euclidean to homogeneous
+%
+% H = E2H(E) is the homogeneous version (K+1xN) of the Euclidean 
+% points E (KxN) where each column represents one point in R^K.
+%
+% See also H2E.
+
+function h = e2h(e)
+    h = [e; ones(1,numcols(e))];

lwrserv/matlab/rvctools/common/edgelist.m

@@ -0,0 +1,119 @@
+%EDGELIST Return list of edge pixels for region
+%
+% E = EDGELIST(IM, SEED) is a list of edge pixels of a region in the
+% image IM starting at edge coordinate SEED (i,j).  The result E is a matrix,
+% each row is one edge point coordinate (x,y).  
+%
+% E = EDGELIST(IM, SEED, DIRECTION) is a list of edge pixels as above,
+% but the direction of edge following is specified.  DIRECTION == 0 (default)
+% means clockwise, non zero is counter-clockwise.  Note that direction is 
+% with respect to y-axis upward, in matrix coordinate frame, not image frame.
+%
+% [E,D] = EDGELIST(IM, SEED, DIRECTION) as above but also returns a vector
+% of edge segment directions which have values 1 to 8 representing W SW S SE E
+% NW N NW respectively.
+%
+% Notes::
+% - IM is a binary image where 0 is assumed to be background, non-zero 
+%   is an object.
+% - SEED must be a point on the edge of the region.
+% - The seed point is always the first element of the returned edgelist.
+%
+% Reference::
+% - METHODS TO ESTIMATE AREAS AND PERIMETERS OF BLOB-LIKE OBJECTS: A COMPARISON
+%   Luren Yang, Fritz Albregtsen, Tor Lgnnestad and Per Grgttum
+%   IAPR Workshop on Machine Vision Applications Dec. 13-15, 1994, Kawasaki
+%
+% See also ILABEL.
+
+function [e,d] = edgelist(im, P, direction)
+
+    % deal with direction argument
+    if nargin == 2
+        direction = 0;
+    end
+
+    if direction == 0
+        neighbours = [1:8]; % neigbours in clockwise direction
+    else
+        neighbours = [8:-1:1];  % neigbours in counter-clockwise direction
+    end
+
+    P = P(:)';
+    P0 = P;     % make a note of where we started
+    pix0 = im(P(2), P(1));  % color of pixel we start at
+
+    % find an adjacent point outside the blob
+    Q = adjacent_point(im, P, pix0);
+    if isempty(Q)
+        error('no neighbour outside the blob');
+    end
+
+    e = P;  % initialize the edge list
+    dir = []; % initialize the direction list
+
+    % these are directions of 8-neighbours in a clockwise direction
+    dirs = [-1 0; -1 1; 0 1; 1 1; 1 0; 1 -1; 0 -1; -1 -1];
+
+    while 1
+        % find which direction is Q
+        dQ = Q - P;
+        for kq=1:8
+            if all(dQ == dirs(kq,:))
+                break;
+            end
+        end
+
+
+        % now test for directions relative to Q
+        for j=neighbours
+            % get index of neighbour's direction in range [1,8]
+            k = j + kq;
+            if k > 8
+                k = k - 8;
+            end
+            dir = [dir; k];
+
+            % compute coordinate of the k'th neighbour
+            Nk = P + dirs(k,:);
+            try
+                if im(Nk(2), Nk(1)) == pix0
+                    % if this neighbour is in the blob it is the next edge pixel
+                    P = Nk;
+                    break;
+                end
+            end
+            Q = Nk;     % the (k-1)th neighbour
+        end
+
+        % check if we are back where we started
+        if all(P == P0)
+            break;
+        end
+
+        % keep going, add P to the edgelist
+        e = [e; P];
+    end
+    
+    if nargout > 1
+        d = dir;
+    end
+end
+
+function P = adjacent_point(im, seed, pix0)
+    % find an adjacent point not in the region
+    dirs = [1 0; 0 1; -1 0; 0 -1];
+    for d=dirs'
+        P = [seed(1)+d(1), seed(2)+d(2)];
+        try
+            if im(P(2), P(1)) ~= pix0
+                return;
+            end    
+        catch
+            % if we get an exception then by definition P is outside the region,
+            % since it's off the edge of the image
+            return;
+        end
+    end
+    P = [];
+end

lwrserv/matlab/rvctools/common/gauss2d.m

@@ -0,0 +1,19 @@
+%GAUSS2D    Gaussian kernel
+%
+% OUT = GAUSS2D(IM, SIGMA, C) is a unit volume Gaussian kernel rendered into
+% matrix OUT (WxH) the same size as IM (WxH). The Gaussian has a standard 
+% deviation of SIGMA.  The Gaussian is centered at C=[U,V].
+function m = gaus2d(im, sigma, c)
+
+
+    if length(sigma) == 1
+        sx = sigma(1);
+        sy = sigma(1);
+    else
+        sx = sigma(1);
+        sy = sigma(2);
+    end
+
+    [x,y] = imeshgrid(im);
+
+    m = 1/(2*pi*sx*sy) * exp( -(((x-c(1))/sx).^2 + ((y-c(2))/sy).^2)/2);

lwrserv/matlab/rvctools/common/getprofilefunctionstats.m

@@ -0,0 +1,35 @@
+function [ funstats] = getprofilefunctionstats( pstats , desfun, varargin)
+%GETPROFILEFUNCTIONSTATS Summary of this function goes here
+%   Detailed explanation goes here
+
+nEl = numel(pstats.FunctionTable);
+desfname = which(desfun);
+funstats = [];
+
+if isempty(desfname)
+    error(['Function ', desfun, ' not found!']);
+end
+
+funtype = '';
+if nargin == 3
+    funtype = lower(varargin{1});
+    
+        if ~(strcmp(funtype,'m-function') || strcmp(funtype,'mex-function'))
+            error('funtype must be either ''M-function'' or ''MEX-function''!');
+        end
+end
+
+for iEl = 1:nEl
+    curstats = pstats.FunctionTable(iEl);
+    
+    if (strcmp(curstats.FileName,desfname))
+        if strcmpi(curstats.Type,funtype) || isempty(funtype)
+            funstats = curstats;
+            return
+        end
+    end
+    
+end
+
+end
+

lwrserv/matlab/rvctools/common/h2e.m

@@ -0,0 +1,14 @@
+%H2E Homogeneous to Euclidean 
+%
+% E = H2E(H) is the Euclidean version (K-1xN) of the homogeneous 
+% points H (KxN) where each column represents one point in P^K.
+%
+% See also E2H.
+
+function e = h2e(h)
+
+    if isvector(h)
+        h = h(:);
+    end
+    e = h(1:end-1,:) ./ repmat(h(end,:), numrows(h)-1, 1);
+

lwrserv/matlab/rvctools/common/homline.m

@@ -0,0 +1,18 @@
+%HOMLINE Homogeneous line from two points
+%
+% L = HOMLINE(X1, Y1, X2, Y2) is a vector (3x1) which describes a line in
+% homogeneous form that contains the two Euclidean points (X1,Y1) and (X2,Y2).
+%
+% Homogeneous points X (3x1) on the line must satisfy L'*X = 0.
+%
+% See also PLOT_HOMLINE.
+
+% TODO, probably should be part of a HomLine class.
+
+function l = homline(x1, y1, x2, y2)
+
+    l = cross([x1 y1 1], [x2 y2 1]);
+
+    % normalize so that the result of x*l' is the pixel distance
+    % from the line
+    l = l / norm(l(1:2));

lwrserv/matlab/rvctools/common/homtrans.m

@@ -0,0 +1,52 @@
+%HOMTRANS Apply a homogeneous transformation
+%
+% P2 = HOMTRANS(T, P) applies homogeneous transformation T to the points 
+% stored columnwise in P.
+%
+% - If T is in SE(2) (3x3) and
+%   - P is 2xN (2D points) they are considered Euclidean (R^2)
+%   - P is 3xN (2D points) they are considered projective (P^2)
+% - If T is in SE(3) (4x4) and
+%   - P is 3xN (3D points) they are considered Euclidean (R^3)
+%   - P is 4xN (3D points) they are considered projective (P^3)
+%
+%  TP = HOMTRANS(T, T1) applies homogeneous transformation T to the 
+%  homogeneous transformation T1, that is TP=T*T1.  If T1 is a 3-dimensional 
+%  transformation then T is applied to each plane as defined by the first two 
+% dimensions, ie. if T = NxN and T=NxNxP then the result is NxNxP.
+%
+% See also E2H, H2E.
+
+% Copyright (C) 1995-2009, by Peter I. Corke
+%
+% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
+% 
+% MVTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% MVTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
+function pt = homtrans(T, p)
+
+    if numrows(p) == numrows(T)
+        if ndims(p) == 3
+            pt = [];
+            for i=1:size(p,3)
+                pt = cat(3, pt, T*p(:,:,i));
+            end
+        else
+            pt = T * p;
+        end
+    elseif (numrows(T)-numrows(p)) == 1
+        % second argument is Euclidean coordinate, promote to homogeneous
+        pt = h2e( T * e2h(p) );
+    else
+        error('matrices and point data do not conform')
+    end

lwrserv/matlab/rvctools/common/ishomog.m

@@ -0,0 +1,44 @@
+%ISHOMOG Test if argument is a homogeneous transformation
+%
+% ISHOMOG(T) is true (1) if the argument T is of dimension 4x4 or 4x4xN, else 
+% false (0).
+%
+% ISHOMOG(T, 'valid') as above, but also checks the validity of the rotation
+% matrix.
+%
+% Notes::
+% - The first form is a fast, but incomplete, test for a transform in SE(3)
+% - Does not work for the SE(2) case
+%
+% See also ISROT, ISVEC.
+
+
+% Copyright (C) 1993-2011, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+
+function h = ishomog(tr, rtest)
+    d = size(tr);
+    if ndims(tr) >= 2
+        h =  all(d(1:2) == [4 4]);
+
+        if h && nargin > 1
+            h = abs(det(tr(1:3,1:3)) - 1) < eps;
+        end
+
+    else
+        h = false;
+    end

lwrserv/matlab/rvctools/common/ishomog2.m

@@ -0,0 +1,46 @@
+%ISHOMOG2 Test if SE(2) homogeneous transformation
+%
+% ISHOMOG2(T) is true (1) if the argument T is of dimension 3x3 or 3x3xN, else 
+% false (0).
+%
+% ISHOMOG2(T, 'valid') as above, but also checks the validity of the rotation
+% sub-matrix.
+%
+% Notes::
+% - The first form is a fast, but incomplete, test for a transform in SE(3).
+% - Does not work for the SE(3) case.
+%
+% See also ISHOMOG, ISROT2, ISVEC.
+
+
+
+% Copyright (C) 1993-2014, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function h = ishomog2(tr, rtest)
+    d = size(tr);
+    if ndims(tr) >= 2
+        h =  all(d(1:2) == [3 3]);
+
+        if h && nargin > 1
+            h = abs(det(tr(1:2,1:2)) - 1) < eps;
+        end
+    else
+        h = false;
+    end

lwrserv/matlab/rvctools/common/isrot.m

@@ -0,0 +1,43 @@
+%ISROT Test if argument is a rotation matrix
+%
+% ISROT(R) is true (1) if the argument is of dimension 3x3 or 3x3xN, else false (0).
+%
+% ISROT(R, 'valid') as above, but also checks the validity of the rotation
+% matrix.
+%
+% Notes::
+% - A valid rotation matrix has determinant of 1.
+%
+% See also ISHOMOG, ISVEC.
+
+
+% Copyright (C) 1993-2011, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+
+function h = isrot(r, dtest)
+
+    d = size(r);
+    if ndims(r) >= 2
+        h =  all(d(1:2) == [3 3]);
+
+        if h && nargin > 1
+            h = abs(det(r) - 1) < eps;
+        end
+
+    else
+        h = false;
+    end

lwrserv/matlab/rvctools/common/isrot2.m

@@ -0,0 +1,45 @@
+%ISROT2 Test if SO(2) rotation matrix
+%
+% ISROT2(R) is true (1) if the argument is of dimension 2x2 or 2x2xN, else false (0).
+%
+% ISROT2(R, 'valid') as above, but also checks the validity of the rotation
+% matrix.
+%
+% Notes::
+% - A valid rotation matrix has determinant of 1.
+%
+% See also ISHOMOG2, ISVEC.
+
+
+
+% Copyright (C) 1993-2014, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function h = isrot2(r, dtest)
+
+    d = size(r);
+    if ndims(r) >= 2
+        h =  all(d(1:2) == [2 2]);
+
+        if h && nargin > 1
+            h = abs(det(r) - 1) < eps;
+        end
+    else
+        h = false;
+    end

lwrserv/matlab/rvctools/common/isvec.m

@@ -0,0 +1,38 @@
+%ISVEC Test if argument is a vector
+%
+% ISVEC(V) is true (1) if the argument V is a 3-vector, else false (0).
+%
+% ISVEC(V, L) is true (1) if the argument V is a vector of length L,
+% either a row- or column-vector.  Otherwise false (0).
+%
+% Notes::
+% - differs from MATLAB builtin function ISVECTOR, the latter returns true
+%   for the case of a scalar, ISVEC does not.
+%
+% See also ISHOMOG, ISROT.
+
+
+% Copyright (C) 1993-2011, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+
+function h = isvec(v, l)
+    if nargin == 1
+            l = 3;
+    end
+    d = size(v);
+    h = logical( length(d) == 2 && min(d) == 1 && numel(v) == l );
+

lwrserv/matlab/rvctools/common/multidfprintf.m

@@ -0,0 +1,66 @@
+%MULTIDFPRINTF Print formatted text to multiple streams
+%
+% COUNT = MULTIDFPRINTF(IDVEC, FORMAT, A, ...) performs formatted output
+% to multiple streams such as console and files.  FORMAT is the format string 
+% as used by sprintf and fprintf.  A is the array of elements, to which the 
+% format will be applied similar to sprintf and fprint.  
+%
+% IDVEC is a vector (1xN) of file descriptors and COUNT is a vector (1xN) of
+% the number of bytes written to each file.
+%
+% Notes::
+% - To write to the consolde use the file identifier 1.
+%
+% Example::
+%         % Create and open a new example file:
+%         fid = fopen('exampleFile.txt','w+');
+%         % Write something to the file and the console simultaneously:
+%         multidfprintf([1 FID],'% s % d % d % d!\n','This is a test!',1,2,3);
+%         % Close the file:
+%         fclose(FID);
+%
+% Authors::
+%  Joern Malzahn   
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also fprintf,sprintf.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [count] = multidfprintf(idVec,varargin)
+ 
+if isempty(idVec)
+    warning('multIDFprintf','Target ID is empty. Nothing is written.')
+    count = [];
+else
+    count = zeros(size(idVec));
+    for iID = 1:numel(idVec)
+        if idVec(iID) < 1
+            count(iID) = 0;
+        else
+            count(iID) = fprintf(idVec(iID),varargin{:});
+        end
+    end
+end

lwrserv/matlab/rvctools/common/numcols.m

@@ -0,0 +1,25 @@
+%NUMCOLS Return number of columns in matrix
+%
+% NC = NUMCOLS(M) is the number of columns in the matrix M.
+%
+% See also NUMROWS.
+
+% Copyright (C) 1993-2008, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+
+function c = numcols(m)
+	c = size(m,2);

lwrserv/matlab/rvctools/common/numrows.m

@@ -0,0 +1,27 @@
+%NUMROWS Return number of rows in matrix
+%
+% NR = NUMROWS(M) is the number of rows in the matrix M.
+%
+% See also NUMCOLS.
+
+
+% Copyright (C) 1993-2008, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+
+function r = numrows(m)
+
+	[r,x] = size(m);

lwrserv/matlab/rvctools/common/peak.m

@@ -0,0 +1,151 @@
+%PEAK Find peaks in vector
+%
+% YP = PEAK(Y, OPTIONS) are the values of the maxima in the vector Y.
+%
+% [YP,I] = PEAK(Y, OPTIONS) as above but also returns the indices of the maxima
+% in the vector Y.
+%
+% [YP,XP] = PEAK(Y, X, OPTIONS) as above but also returns the corresponding 
+% x-coordinates of the maxima in the vector Y.  X is the same length of Y
+% and contains the corresponding x-coordinates.
+%
+% Options::
+% 'npeaks',N    Number of peaks to return (default all)
+% 'scale',S     Only consider as peaks the largest value in the horizontal 
+%               range +/- S points.
+% 'interp',N    Order of interpolation polynomial (default no interpolation)
+% 'plot'        Display the interpolation polynomial overlaid on the point data
+%
+% Notes::
+% - To find minima, use PEAK(-V).
+% - The interp options fits points in the neighbourhood about the peak with
+%   an N'th order polynomial and its peak position is returned.  Typically
+%   choose N to be odd.
+%
+% See also PEAK2.
+
+
+% Copyright (C) 1993-2011, by Peter I. Corke
+%
+% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
+% 
+% MVTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% MVTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
+
+% Copyright (c) Peter Corke 1/96
+
+function [yp,xpout] = peak(y, varargin)
+
+    % process input options
+    opt.npeaks = [];
+    opt.scale = 1;
+    opt.interp = 0;
+    opt.plot = false;
+    
+    [opt,args] = tb_optparse(opt, varargin);
+    
+    
+    % if second argument is a matrix we take this as the corresponding x
+    % coordinates
+    if ~isempty(args)
+        x = args{1};
+        x = x(:);
+        if length(x) ~= length(y)
+            error('second argument must be same length as first');
+        end
+    else
+        x = [1:length(y)]';
+    end
+    
+    y = y(:);
+    
+    % find the maxima
+    if opt.scale > 1
+        % compare to a moving window max filtered version
+        k = find(y' == filt1d(y, 'max', 'width', opt.scale*2+1));
+    else
+        % take the zero crossings
+        dv = diff(y);
+        k = find( ([dv; 0]<0) & ([0; dv]>0) );
+    end
+    
+    % sort the maxima into descending magnitude
+    [m,i] = sort(y(k), 'descend');
+    k = k(i);    % indice of the maxima
+
+    if opt.npeaks
+        k = k(1:opt.npeaks);
+    end
+
+    % optionally plot the discrete data
+    if opt.plot
+        plot(x, y, '-o');      
+        hold on
+    end
+    
+
+    % interpolate the peaks if required
+    if opt.interp
+        if opt.interp < 2
+            error('interpolation polynomial must be at least second order');
+        end
+        
+        xp = [];
+        yp = [];
+        N = opt.interp;
+        N2 = round(N/2);
+
+        % for each previously identified peak x(i), y(i)
+        for i=k'
+            % fit a polynomial to the local neighbourhood
+            try
+                pp = polyfit(x(i-N2:i+N2), y(i-N2:i+N2), N);
+            catch
+                % handle situation where neighbourhood falls off the data
+                % vector
+                warning('Peak at %f too close to start or finish of data, skipping', x(i));
+                continue;
+            end
+            
+            % find the roots of the polynomial closest to the coarse peak
+            r = roots( polydiff(pp) );
+            [mm,j] = min(abs(r-x(i)));
+            xx = r(j);
+            
+            % store x, y for the refined peak
+            xp = [xp; xx];
+            yp = [y; polyval(pp, xx)];
+            
+            if opt.plot
+                % overlay the fitted polynomial and refined peak
+                xr = linspace(x(i-N2), x(i+N2), 50);
+                plot(xr, polyval(pp, xr), 'r');
+                plot(xx, polyval(pp, xx), 'rd');
+            end
+        end
+    else
+        xp = x(k);
+    end
+    
+    if opt.plot
+        grid
+        xlabel('x');
+        ylabel('y');
+        hold off
+    end
+    
+    % return values
+    yp = y(k)';
+    if nargout > 1
+        xpout = xp';
+    end

lwrserv/matlab/rvctools/common/peak2.m

@@ -0,0 +1,146 @@
+%PEAK2  Find peaks in a matrix
+%
+% ZP = PEAK2(Z, OPTIONS) are the peak values in the 2-dimensional signal Z.
+%
+% [ZP,IJ] = PEAK2(Z, OPTIONS) as above but also returns the indices of the 
+% maxima in the matrix Z.  Use SUB2IND to convert these to row and column 
+% coordinates
+%
+% Options::
+% 'npeaks',N    Number of peaks to return (default all)
+% 'scale',S     Only consider as peaks the largest value in the horizontal 
+%               and vertical range +/- S points.
+% 'interp'      Interpolate peak (default no interpolation)
+% 'plot'        Display the interpolation polynomial overlaid on the point data
+%
+% Notes::
+% - To find minima, use PEAK2(-V).
+% - The interp options fits points in the neighbourhood about the peak with
+%   a paraboloid and its peak position is returned.
+%
+% See also PEAK, SUB2IND.
+
+
+% Copyright (C) 1993-2011, by Peter I. Corke
+%
+% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
+% 
+% MVTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% MVTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
+
+% Copyright (c) Peter Corke 1/96
+
+function [zp,xypout, aout] = peak2(z, varargin)
+
+    % process input options
+    opt.npeaks = 2;
+    opt.scale = 1;
+    opt.interp = false;
+    
+    [opt,args] = tb_optparse(opt, varargin);
+    
+    
+    % create a neighbourhood mask for non-local maxima
+    % suppression
+    h = opt.scale;
+    w = 2*h+1;
+    M = ones(w,w);
+    M(h+1,h+1) = 0;
+    
+    % compute the neighbourhood maximum
+    znh = iwindow(double(z), M, 'max', 'wrap');
+    
+    % find all pixels greater than their neighbourhood
+    k = find(z > znh);
+    
+    
+    % sort these local maxima into descending order
+    [zpk,ks] = sort(z(k), 'descend');
+
+    k = k(ks);
+    
+    npks = min(length(k), opt.npeaks);
+    k = k(1:npks);
+    
+    [y,x] = ind2sub(size(z), k);
+    xy = [x y]';
+    
+
+    % interpolate the peaks if required
+    if opt.interp
+        
+        
+        xyp = [];
+        zp = [];
+        ap = [];
+               
+        % for each previously identified peak x(i), y(i)
+        for xyt=xy
+            % fit a polynomial to the local neighbourhood
+            try
+                
+                x = xyt(1); y = xyt(2);
+
+                
+                % now try to interpolate the peak over a 3x3 window
+                
+                zc = z(x,   y);
+                zn = z(x,   y-1);
+                zs = z(x,   y+1);
+                ze = z(x+1, y);
+                zw = z(x-1, y);
+                
+                dx = (ze - zw)/(2*(2*zc - ze - zw));
+                dy = (zn - zs)/(2*(zn - 2*zc + zs));
+
+                zest = zc - (ze - zw)^2/(8*(ze - 2*zc + zw)) - (zn - zs)^2/(8*(zn - 2*zc + zs));
+                
+                aest = min(abs([ze/2 - zc + zw/2, zn/2 - zc + zs/2]));
+
+                
+            catch
+                % handle situation where neighbourhood falls off the data
+                % vector
+                warning('Peak at %f too close to edge of image, skipping', x(i));
+                continue;
+            end
+            %
+            
+            % store x, y for the refined peak
+            xyp = [xyp [x+dx; y+dy]];
+            zp = [zp zest];
+            ap = [ap aest];
+
+        end
+    else
+        % no interpolation case
+        xyp = xy;
+        zp = z(k)';
+        ap = [];
+
+    end
+    
+    
+    % return values
+    if nargout > 1
+        xypout = xyp;
+    end
+    if nargout > 2
+        aout = ap;
+    end
+        
+
+
+
+    
+   

lwrserv/matlab/rvctools/common/plot2.m

@@ -0,0 +1,29 @@
+%PLOT2 Plot trajectories
+%
+% PLOT2(P) plots a line with coordinates taken from successive rows of P.  P
+% can be Nx2 or Nx3.
+%
+% If P has three dimensions, ie. Nx2xM or Nx3xM then the M trajectories are
+% overlaid in the one plot.
+%
+% PLOT2(P, LS) as above but the line style arguments LS are passed to plot.
+%
+% See also PLOT.
+function h = plot2(p1, varargin)
+
+    if ndims(p1) == 2
+        if numcols(p1) == 3,
+            hh = plot3(p1(:,1), p1(:,2), p1(:,3), varargin{:});
+        else
+            hh = plot(p1(:,1), p1(:,2), varargin{:});
+        end
+        if nargout == 1,
+            h = hh;
+        end
+    else
+        clf
+        hold on
+        for i=1:size(p1,2)
+            plot2( squeeze(p1(:,i,:))' );
+        end
+    end

lwrserv/matlab/rvctools/common/plot_arrow.m

@@ -0,0 +1,12 @@
+%PLOT_ARROW Plot arrow
+%
+% PLOT_ARROW(P, OPTIONS) draws an arrow from P1 to P2 where P=[P1; P2].
+%
+% See also ARROW3.
+function plot_arrow(p, varargin)
+    mstart = p(1:end-1,:);
+    mend = p(2:end,:);
+    %mstart = p;
+    %mend = [p(2:end,:); p(1,:)];
+
+    arrow3(mstart, mend, varargin{:});

lwrserv/matlab/rvctools/common/plot_box.m

@@ -0,0 +1,100 @@
+%PLOT_BOX	Draw a box on the current plot
+%
+% PLOT_BOX(B, LS) draws a box defined by B=[XL XR; YL YR] with optional Matlab
+% linestyle options LS.
+%
+% PLOT_BOX(X1,Y1, X2,Y2, LS) draws a box with corners at (X1,Y1) and (X2,Y2),
+% and optional Matlab linestyle options LS.
+%
+% PLOT_BOX('centre', P, 'size', W, LS) draws a box with center at P=[X,Y] and
+% with dimensions W=[WIDTH HEIGHT].
+%
+% PLOT_BOX('topleft', P, 'size', W, LS) draws a box with top-left at P=[X,Y] 
+% and with dimensions W=[WIDTH HEIGHT].
+
+% Options::
+% 'size',SZ     Specify size of box.  SZ=[WIDTH, HEIGHT] or if scalar then
+%               WIDTH=HEIGHT=SZ
+% 'topleft',P   Specify position of box by top left coordinate P
+% 'centre',P    Specify position of box by centre coordinate P
+%
+% Additional options LS are passed to PLOT.
+%
+% See also plot.
+
+% Copyright (C) 1995-2009, by Peter I. Corke
+%
+% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
+% 
+% MVTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% MVTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
+
+function plot_box(varargin)
+    opt.centre = [];
+    opt.topleft = [];
+    opt.size = [];
+
+    [opt,varargin] = tb_optparse(opt, varargin);
+
+    if ~isempty(opt.size)
+        if size(opt.size) == 1
+            w = opt.size;
+            h = opt.size;
+        else
+            w = opt.size(1);
+            h = opt.size(2);
+        end
+
+        if ~isempty(opt.centre)
+            x1 = round(opt.centre(1)-w/2);
+            y1 = round(opt.centre(2)-h/2);
+            x2 = round(opt.centre(1)+w/2);
+            y2 = round(opt.centre(2)+h/2);
+        elseif ~isempty(opt.topleft)
+            x1 = opt.topleft(1);
+            y1 = opt.topleft(2);
+            x2 = x1 + w;
+            y2 = x1 + h;
+        else
+            error('must specify top left or centre');
+        end
+    else
+        if all(size(varargin{1}) == [2 2])
+            % first arg is a box
+            b = varargin{1};
+            x1 = b(1); y1 = b(2);
+            x2 = b(3); y2 = b(4);
+            varargin = varargin(2:end);
+        else
+            % use first 4 args as x1 y1 x2 y2
+            x1 = varargin{1};
+            y1 = varargin{2};
+            x2 = varargin{3};
+            y2 = varargin{4};
+            varargin = varargin(5:end);
+        end
+    end
+    p = [	x1 y1
+            x2 y1
+            x2 y2
+            x1 y2
+            x1 y1 ];
+
+    holdon = ishold;
+    hold on
+
+    plot(p(:,1), p(:,2), varargin{:})
+
+    if holdon == 0
+        hold off
+    end

lwrserv/matlab/rvctools/common/plot_circle.m

@@ -0,0 +1,101 @@
+%PLOT_CIRCLE Draw a circle on the current plot
+%
+% PLOT_CIRCLE(C, R, options) draws a circle on the current plot with 
+% centre C=[X,Y] and radius R.  If C=[X,Y,Z] the circle is drawn in the
+% XY-plane at height Z.
+%
+% H = PLOT_CIRCLE(C, R, options) as above but return handles. For multiple
+% circles H is a vector of handles, one per circle.
+%
+% Options::
+% 'edgecolor'   the color of the circle's edge, Matlab color spec
+% 'fillcolor'   the color of the circle's interior, Matlab color spec
+% 'alpha'       transparency of the filled circle: 0=transparent, 1=solid
+% 'alter',H     alter existing circles with handle H
+%
+% For an unfilled ellipse any MATLAB LineProperty options can be given, for
+% a filled ellipse any MATLAB PatchProperty options can be given.
+%
+% See also PLOT_ELLIPSE.
+
+
+% Copyright (C) 1995-2009, by Peter I. Corke
+%
+% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
+% 
+% MVTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% MVTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
+function handles = plot_circle(centre, rad, varargin)
+
+    opt.fillcolor = [];
+    opt.alpha = 1;
+    opt.edgecolor = 'k';
+    opt.alter = [];
+
+    [opt,arglist] = tb_optparse(opt, varargin);
+    
+    if ~isempty(opt.alter) & ~ishandle(opt.alter)
+        error('RTB:plot_circle:badarg', 'argument to alter must be a valid graphic object handle');
+    end
+
+    holdon = ishold;
+    hold on
+
+	n = 50;
+	th = [0:n]'/n*2*pi;
+    
+    if length(rad) == 1
+        rad = rad*ones(numcols(centre),1);
+    end
+    if length(centre) == 2 || length(centre) == 3
+        centre = centre(:);
+    end
+
+    for i=1:numcols(centre)
+        x = rad(i)*cos(th) + centre(1,i);
+        y = rad(i)*sin(th) + centre(2,i);
+        if numrows(centre) > 2
+            % plot 3D data
+            z = ones(size(x))*centre(3,i);
+            if isempty(opt.alter)
+                h(i) = plot3(x, y, z, varargin{:});
+            else
+                set(opt.alter(i), 'xdata', x, 'ydata', y, 'zdata', z, arglist{:});
+            end
+        else
+            % plot 2D data
+            if isempty(opt.fillcolor)
+                if isempty(opt.alter)
+                    h(i) = plot(x, y, arglist{:});
+                else
+                    set(opt.alter(i), 'xdata', x, 'ydata', y, arglist{:});
+                end
+            else
+                if isempty(opt.alter)
+                    h(i) = patch(x, y, 0*y, 'FaceColor', opt.fillcolor, ...
+                        'FaceAlpha', opt.alpha, 'EdgeColor', opt.edgecolor, arglist{:});
+                else
+                    set(opt.alter(i), 'xdata', x, 'ydata', y, arglist{:});
+                end
+                
+            end
+        end
+    end
+
+    if holdon == 0
+        hold off
+    end
+    
+    if nargout > 0
+        handles = h;
+    end

lwrserv/matlab/rvctools/common/plot_ellipse.m

@@ -0,0 +1,135 @@
+%PLOT_ELLIPSE Draw an ellipse on the current plot
+%
+% PLOT_ELLIPSE(A, LS) draws an ellipse defined by X'AX = 0 on the
+% current plot, centred at the origin, with Matlab line style LS.
+%
+% PLOT_ELLIPSE(A, C, LS) as above but centred at C=[X,Y].
+% current plot.  If C=[X,Y,Z] the ellipse is parallel to the XY plane
+% but at height Z.
+%
+% H = PLOT_CIRCLE(C, R, options) as above but return handles. For multiple
+% circles H is a vector of handles, one per circle.
+%
+% Options::
+% 'edgecolor'   the color of the circle's edge, Matlab color spec
+% 'fillcolor'   the color of the circle's interior, Matlab color spec
+% 'alpha'       transparency of the filled circle: 0=transparent, 1=solid
+% 'alter',H     alter existing circles with handle H
+%
+% See also PLOT_CIRCLE.
+
+function handles = plot_ellipse(A, centre, varargin)
+    
+    if size(A,1) ~= size(A,2)
+        error('ellipse is defined by a square matrix');
+    end
+    
+    if size(A,1) > 3
+        error('can only plot ellipsoid for 2 or 3 dimenions');
+    end
+    
+    if nargin < 2
+        centre = zeros(1, size(A,1));
+    end
+    if nargin < 3
+        varargin = {};
+    end
+    
+    
+    opt.fillcolor = [];
+    opt.alpha = 1;
+    opt.edgecolor = 'k';
+    opt.alter = [];
+    
+    [opt,arglist] = tb_optparse(opt, varargin);
+    
+    if ~isempty(opt.alter) & ~ishandle(opt.alter)
+        error('RTB:plot_circle:badarg', 'argument to alter must be a valid graphic object handle');
+    end
+    
+    holdon = ishold();
+    hold on
+    
+    if size(A,1) == 3
+        %% plot an ellipsoid
+        
+        % define mesh points on the surface of a unit sphere
+        [Xs,Ys,Zs] = sphere();
+        ps = [Xs(:) Ys(:) Zs(:)]';
+        
+        % warp it into the ellipsoid
+        pe = sqrtm(A) * ps;
+        
+        % offset it to optional non-zero centre point
+        if nargin > 1
+            pe = bsxfun(@plus, centre(:), pe);
+        end
+        
+        % put back to mesh format
+        Xe = reshape(pe(1,:), size(Xs));
+        Ye = reshape(pe(2,:), size(Ys));
+        Ze = reshape(pe(3,:), size(Zs));
+        
+        % plot it
+        if isempty(opt.alter)
+            h = mesh(Xe, Ye, Ze, arglist{:});
+        else
+            set(opt.alter, 'xdata', Xe, 'ydata', Ye, 'zdata', Ze, arglist{:});
+            
+        end
+        
+    else
+        %% plot an ellipse
+        
+        
+        [V,D] = eig(A);
+        
+        % define points on a unit circle
+        th = linspace(0, 2*pi, 50);
+        pc = [cos(th);sin(th)];
+        
+        % warp it into the ellipse
+        pe = sqrtm(A)*pc;
+        
+        % offset it to optional non-zero centre point
+        centre = centre(:);
+        if nargin > 1
+            pe = bsxfun(@plus, centre(1:2), pe);
+        end
+        x = pe(1,:); y = pe(2,:);
+        
+
+        if length(centre) > 2
+            % plot 3D data
+            z = ones(size(x))*centre(3);
+            if isempty(opt.alter)
+                h = plot3(x, y, z, varargin{:});
+            else
+                set(opt.alter, 'xdata', x, 'ydata', y, 'zdata', z, arglist{:});
+            end
+        else
+            % plot 2D data
+            if isempty(opt.fillcolor)
+                if isempty(opt.alter)
+                    h = plot(x, y, arglist{:});
+                else
+                    set(opt.alter, 'xdata', x, 'ydata', y, arglist{:});
+                end
+            else
+                if isempty(opt.alter)
+                    h = patch(x, y, 0*y, 'FaceColor', opt.fillcolor, ...
+                        'FaceAlpha', opt.alpha, 'EdgeColor', opt.edgecolor, arglist{:});
+                else
+                    set(opt.alter, 'xdata', x, 'ydata', y, arglist{:});
+                end
+                
+            end
+        end
+    end
+    holdon = ishold;
+    hold on
+    
+    if nargout > 0
+        handles = h;
+    end
+end

lwrserv/matlab/rvctools/common/plot_ellipse_inv.m

@@ -0,0 +1,73 @@
+%PLOT_ELLIPSE_INV Draw an ellipse or ellipsoid
+%
+% PLOT_ELLIPSE_INV(A, OPTIONS) draws an ellipse defined by X'.inv(A).X = 0 on the
+% current plot, centred at the origin.
+%
+% PLOT_ELLIPSE_INV(A, C, OPTIONS) as above but centred at C=[X,Y].  If
+% C=[X,Y,Z] the ellipse is parallel to the XY plane but at height Z.
+%
+% H = PLOT_ELLIPSE_INV(A, C, OPTIONS) as above but return graphic handle.
+%
+% Options::
+% 'edgecolor'   the color of the circle's edge, Matlab color spec
+% 'fillcolor'   the color of the circle's interior, Matlab color spec
+% 'alpha'       transparency of the filled circle: 0=transparent, 1=solid
+% 'alter',H     alter existing circles with handle H
+%
+% Notes::
+% - For the case where the inverse of ellipse parameters are known, perhaps
+%   an inverse covariance matrix.
+% - If A (2x2) draw an ellipse, else if A(3x3) draw an ellipsoid.
+% - The ellipse is added to the current plot.
+%
+% See also PLOT_ELLIPSE, PLOT_CIRCLE, PLOT_BOX, PLOT_POLY.
+
+% Copyright (C) 1993-2014, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+% See also PLOT_ELLIPSE, PLOT_CIRCLE, PLOT_BOX, PLOT_POLY.
+
+
+% Copyright (C) 1993-2014, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function h = plot_ellipse_inv(A, xc, varargin)
+
+    if nargin == 1
+        h = plot_ellipse(inv(A));
+    elseif nargin == 2
+        h = plot_ellipse(inv(A), xc);
+    else
+        h = plot_ellipse(inv(A), xc, varargin{:});
+    end

lwrserv/matlab/rvctools/common/plot_homline.m

@@ -0,0 +1,58 @@
+%PLOT_HOMLINE Draw a line in homogeneous form
+%
+% H = PLOT_HOMLINE(L, LS) draws a line in the current figure L.X = 0.  The current
+% axis limits are used to determine the endpoints of the line.  Matlab line
+% specification LS can be set.
+%
+% The return argument is a vector of graphics handles for the lines.
+%
+% See also HOMLINE.
+
+% TODO, probably should be part of a HomLine class.
+
+% Copyright (C) 1995-2009, by Peter I. Corke
+%
+% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
+% 
+% MVTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% MVTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
+
+function handles = plot_homline(lines, varargin)
+
+	% get plot limits from current graph
+	xlim = get(gca, 'XLim');
+	ylim = get(gca, 'YLim');
+
+    ish = ishold;
+    hold on;
+
+	h = [];
+	% for all input lines (columns
+	for l=lines
+        if abs(l(2)) > abs(l(1))
+            y = (-l(3) - l(1)*xlim) / l(2);
+            hh = plot(xlim, y, varargin{:});
+        else
+            x = (-l(3) - l(2)*ylim) / l(1);
+            hh = plot(x, ylim, varargin{:});
+        end
+		h = [h; hh];
+	end
+
+    if ~ish
+        hold off
+    end
+
+	if nargout > 0,
+		handles = h;
+	end

lwrserv/matlab/rvctools/common/plot_point.m

@@ -0,0 +1,87 @@
+%PLOT_POINT	Mark point features
+%
+% PLOT_POINT(P, OPTIONS) adds point markers to a plot, where P (2xN)
+% and each column is the point coordinate.
+%
+% Options::
+%  'textcolor', colspec     Specify color of text
+%  'textsize', size         Specify size of text
+%  'bold'                   Text in bold font.
+%  'printf', {fmt, data}    Label points according to printf format
+%                           string and corresponding element of data
+%  'sequence'               Label points sequentially
+%
+% Additional options are passed through to PLOT for creating the marker.
+%
+% Examples::
+%   Simple point plot
+%        P = rand(2,4);
+%        plot_point(P);
+%
+%   Plot points with markers
+%        plot_point(P, '*');
+%
+%   Plot points with square markers and labels
+%        plot_point(P, 'sequence', 's');
+%
+%   Plot points with circles and annotations
+%        data = [1 2 4 8];
+%        plot_point(P, 'printf', {' P%d', data}, 'o');
+%
+%
+% See also PLOT, TEXT.
+
+function plot_point(p, varargin)
+
+    opt.textcolor = 'g';
+    opt.textsize = 12;
+    opt.printf = [];
+    opt.sequence = false;
+    opt.bold = false;
+
+    [opt,arglist] = tb_optparse(opt, varargin);
+
+    % default marker style
+    if isempty(arglist)
+        arglist = {'sb'};
+    end
+
+    % add stuff to pull .u and .v out of a vector of objects
+    if ~isnumeric(p) && any(strcmp('u_', properties(p)))
+        % p is an object with u_ and v_ properties
+        p = [[p.u_]; [p.v_]];
+    end
+
+    holdon = ishold();
+	hold on
+	for i=1:numcols(p)
+		plot(p(1,i), p(2,i), arglist{:});
+        if opt.sequence
+            show(p(:,i), '%d', i, opt);
+        end
+
+        if ~isempty(opt.printf)
+            show(p(:,i), opt.printf{1}, opt.printf{2}(i), opt);
+        end
+
+	end
+    if ~holdon
+        hold off
+    end
+    figure(gcf)
+end
+
+function show(p, fmt, val, opt)
+    if opt.bold
+        fw = 'bold';
+    else
+        fw = 'normal';
+    end
+    text(p(1), p(2), sprintf([' ' fmt], val), ...
+        'HorizontalAlignment', 'left', ...
+        'VerticalAlignment', 'middle', ...
+        'FontUnits', 'pixels', ...
+        'FontSize', opt.textsize, ...
+        'FontWeight', fw, ...
+        'Color', opt.textcolor);
+end

lwrserv/matlab/rvctools/common/plot_poly.m

@@ -0,0 +1,61 @@
+%PLOT_POLY Plot a polygon
+%
+% PLOTPOLY(P, OPTIONS) plot a polygon defined by columns of P which
+% can be 2xN or 3xN.
+%
+% OPTIONS::
+%  'fill'    the color of the circle's interior, Matlab color spec
+%  'alpha'   transparency of the filled circle: 0=transparent, 1=solid.
+%
+% See also PLOT, PATCH, Polygon.
+
+% TODO: options for fill, not filled, line style, labels (cell array of strings)
+
+function h_ = plot_poly(p, varargin)
+
+    if numcols(p) < 3,
+        error('too few points for a polygon');
+    end
+
+    opt.fill = [];
+    opt.alpha = 1;
+
+    [opt,arglist] = tb_optparse(opt, varargin);
+
+    % default marker style
+    if isempty(arglist)
+        arglist = {'r-'};
+    end
+
+    ish = ishold();
+	hold on
+
+    x = [p(1,:) p(1,1)];
+    y = [p(2,:) p(2,1)];
+    if numrows(p) == 2
+        % plot 2D data
+        h(1) = plot(x, y, arglist{:});
+        if ~isempty(opt.fill)
+            h(2) = patch(x', y', 0*y', 'FaceColor', opt.fill, ...
+                'FaceAlpha', opt.alpha);
+        end
+    elseif numrows(p) == 3
+        % plot 3D data
+        z = [p(3,:) p(3,1)];
+        h(1) = plot3(x, y, z, arglist{:});
+        if ~isempty(opt.fill)
+            h(2) = patch(x, y, z, 0*y, 'FaceColor', opt.fill, ...
+                'FaceAlpha', opt.alpha);
+        end
+    else
+        error('point data must have 2 or 3 rows');
+    end
+
+    if ~ish
+        hold off
+    end
+    %figure(gcf)
+    
+    if nargout > 0
+        h_ = h;
+    end

lwrserv/matlab/rvctools/common/plot_sphere.m

@@ -0,0 +1,75 @@
+%PLOT_SPHERE Plot spheres
+%
+% PLOT_SPHERE(C, R, COLOR) add spheres to the current figure.  C is the 
+% centre of the sphere and if its a 3xN matrix then N spheres are drawn 
+% with centres as per the columns.  R is the radius and COLOR is a Matlab 
+% color spec, either a letter or 3-vector.
+%
+% H = PLOT_SPHERE(C, R, COLOR) as above but returns the handle(s) for the
+% spheres.
+%
+% H = PLOT_SPHERE(C, R, COLOR, ALPHA) as above but ALPHA specifies the opacity
+% of the sphere were 0 is transparant and 1 is opaque.  The default is 1.
+%
+% Example::
+% Create four spheres
+%         plot_sphere( mkgrid(2, 1), .2, 'b')
+% and now turn on a full lighting model
+%         lighting gouraud
+%         light
+%
+% NOTES::
+% - The sphere is always added, irrespective of figure hold state.
+% - The number of vertices to draw the sphere is hardwired.
+
+% TODO
+% inconsistant call format compared to other plot_xxx functions.
+
+function h = plot_sphere(c, r, varargin)
+
+    opt.color = 'b';
+    opt.alpha = 1;
+    opt.mesh = 'none';
+
+    [opt,args] = tb_optparse(opt, varargin);
+    
+    % backward compatibility with RVC
+    if length(args) > 0
+        opt.color = args{1};
+    end
+    if length(args) > 1
+        opt.alpha = args{2};
+    end
+    
+    daspect([1 1 1])
+    hold_on = ishold
+    hold on
+    [xs,ys,zs] = sphere(40);
+
+    if isvec(c,3)
+        c = c(:);
+    end
+    if size(r) == 1
+        r = r * ones(numcols(c),1);
+    end
+
+    if nargin < 4
+        alpha = 1
+    end
+
+    % transform the sphere
+    for i=1:numcols(c)
+        x = r(i)*xs + c(1,i);
+        y = r(i)*ys + c(2,i);
+        z = r(i)*zs + c(3,i);
+                
+        % the following displays a nice smooth sphere with glint!
+        h = surf(x,y,z, 'FaceColor', opt.color, 'EdgeColor', opt.mesh, 'FaceAlpha', opt.alpha);
+        % camera patches disappear when shading interp is on
+        %h = surfl(x,y,z)
+    end
+    %lighting gouraud
+    %light
+    if ~hold_on
+        hold off
+    end

lwrserv/matlab/rvctools/common/plotp.m

@@ -0,0 +1,26 @@
+%PLOTP Plot trajectories
+%
+% PLOTP(P) plots a set of points P, which by Toolbox convention are stored
+% one per column.  P can be Nx2 or Nx3.  By default a linestyle of 'bx'
+% is used.
+%
+% PLOTP(P, LS) as above but the line style arguments LS are passed to plot.
+%
+% See also PLOT, PLOT2.
+function h = plotp(p1, varargin)
+
+    if length(varargin) == 0
+        varargin = {'bx'};
+    end
+
+    if numrows(p1) == 3,
+        hh = plot3(p1(1,:), p1(2,:), p1(3,:), varargin{:});
+        xyzlabel
+    else
+        hh = plot(p1(1,:), p1(2,:), varargin{:});
+        xlabel('x');
+        ylabel('y');
+    end
+    if nargout == 1,
+        h = hh;
+    end

lwrserv/matlab/rvctools/common/polydiff.m

@@ -0,0 +1,8 @@
+% POLYDIFF       pd = polydiff(p)
+%
+%   Return the coefficients of the derivative of polynomial p
+%
+function pd = polydiff(p)
+	n = length(p)-1;
+
+	pd = [n:-1:1] .* p(1:n);

lwrserv/matlab/rvctools/common/randinit.m

@@ -0,0 +1,11 @@
+%RANDINIT Reset random number generator
+%
+% RANDINIT reset the defaul random number stream.
+%
+% See also RandStream.
+
+function randinit(seed)
+
+    stream = RandStream.getGlobalStream;
+    stream.reset()
+

lwrserv/matlab/rvctools/common/runscript.m

@@ -0,0 +1,174 @@
+%RUNSCRIPT Run an M-file in interactive fashion
+%
+% RUNSCRIPT(FNAME, OPTIONS) runs the M-file FNAME and pauses after every
+% executable line in the file until a key is pressed.  Comment lines are shown
+% without any delay between lines.
+%
+% Options::
+% 'delay',D    Don't wait for keypress, just delay of D seconds (default 0)
+% 'cdelay',D   Pause of D seconds after each comment line (default 0)
+% 'begin'      Start executing the file after the commnent line %%begin (default true)
+% 'dock'       Cause the figures to be docked when created
+% 'path',P     Look for the file FNAME in the folder P (default .)
+%
+% Notes::
+% - If not file extension is given in FNAME, .m is assumed.
+% - If the executable statement has comments immediately afterward (no blank lines)
+%   then the pause occurs after those comments are displayed.
+% - A simple '-' prompt indicates when the script is paused, hit enter.
+%
+% See also eval.
+
+% Copyright (C) 1993-2013, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function runscript(fname, varargin)
+    
+    opt.path = [];
+    opt.delay = [];
+    opt.begin = true;
+    opt.cdelay = 0;
+    opt.dock = false;
+    
+    opt = tb_optparse(opt, varargin);
+        
+    close all
+    
+    if opt.dock
+        prevDockStatus = get(0,'DefaultFigureWindowStyle');
+        set(0,'DefaultFigureWindowStyle','docked');
+    end
+
+    
+    if ~isempty(opt.path)
+        fname = fullfile(opt.path, [fname '.m']);
+    else
+        fname = [fname '.m'];
+    end
+    
+    fp = fopen(fname, 'r');
+    
+    clc
+    fprintf('--- runscript <-- %s\n', fname);
+    
+    running = false;
+    shouldPause = false;
+    loopText = [];
+        if ~opt.begin
+        running = true;
+    end
+    
+    while 1
+        % get the next line from the file, bail if EOF
+        line = fgetl(fp);
+        if line == -1
+            break
+        end
+        
+        % logic to skip lines until we see one beginning with %%begin
+        if ~running
+            if strcmp(line, '%%begin')
+                running = true;
+            end
+            continue;
+        end;
+        
+
+        % display the line and if executable execute it
+        if length(strtrim(line)) == 0
+            % line was blank
+            fprintf(' \n');
+            if shouldPause
+                scriptwait(opt);
+                shouldPause = false;
+            end
+        elseif strfind1(strtrim(line), '%')
+            % line was a comment
+            disp(line)
+            pause(opt.cdelay)
+            
+        else
+            % line is executable
+            if shouldPause
+                scriptwait(opt);
+                
+                shouldPause = false;
+            end
+            
+            % if the start of a loop, stash the text for now
+            if strfind1(line, 'for') || strfind1(line, 'while')
+                % found a loop, don't eval it until we get to the end
+                loopText = strcat(loopText, [line ';']);
+                % display the line with a pretend MATLAB prompt
+                fprintf('>> '); disp(line)
+                continue;
+            end
+            
+            if ~isempty(loopText)
+                % we're in stashing mode
+                loopText = strcat(loopText, line);
+                % display the line with a pretend MATLAB prompt
+                disp(line);
+                
+                % if the end of a loop, unstash the text and eval it
+                if strfind1(line, 'end') && ~isempty(loopText)
+                    loopText
+                    evalin('base', loopText);
+                    shouldPause = true;
+                    loopText = [];
+                    continue;
+                end
+            else
+                            % display the line with a pretend MATLAB prompt
+            fprintf('>> '); disp(line)
+                evalin('base', line);
+                shouldPause = true;
+            end
+        end
+    end
+    fprintf('------ done --------\n');
+    if opt.dock
+        % restore the docking mode if we set it
+        set(0,'DefaultFigureWindowStyle', prevDockStatus)
+    end
+end
+
+function scriptwait(opt)
+    if isempty(opt.delay)
+        %a = input('-', 's');
+        prompt = 'continue?';
+        fprintf(prompt); pause;
+        for i=1:length(prompt)
+            fprintf('\b');
+        end
+    else
+        pause(opt.delay);
+    end
+end
+
+% test if s2 is at the start of s1
+function res = strfind1(s1, s2)
+
+    r = strfind(s1, s2);
+    res = false;
+    if ~isempty(r) && (r(1) == 1)
+        res = true;
+    end
+end
+    

lwrserv/matlab/rvctools/common/rvcpath.m

@@ -0,0 +1,25 @@
+%RVCPATH Install location of RVC tools
+%
+% p = RVCPATH is the path of the top level folder for the installed RVC
+% tools.
+
+% Copyright (C) 1993-2014, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+function p = rvcpath()
+    p = fileparts( which('startup_rvc.m') );

lwrserv/matlab/rvctools/common/simulinkext.m

@@ -0,0 +1,47 @@
+%SIMULINKEXT Return file extension of Simulink block diagrams.
+%
+% str = simulinkext() is either
+% - '.mdl' if Simulink version number is less than  8
+% - '.slx' if Simulink version numberis larger or equal to 8
+%
+% Notes::
+% The file extension for simulink block diagrams has changed from Matlab 2011b to Matlab 2012a. 
+% This function is used for backwards compatibility.
+%
+% Author::
+%  Joern Malzahn   
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de  
+%
+% See also symexpr2slblock, doesblockexist, distributeblocks.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function str = simulinkext()
+
+if verLessThan('simulink','8')
+    str = '.mdl';
+else
+    str = '.slx';
+end

lwrserv/matlab/rvctools/common/symexpr2slblock.m

@@ -0,0 +1,54 @@
+%SYMEXPR2SLBLOCK Create symbolic embedded MATLAB Function block
+%
+% symexpr2slblock(VARARGIN) creates an Embedded MATLAB Function block 
+% from a symbolic expression.% The input arguments are just as used with 
+% the functions emlBlock or matlabFunctionBlock.
+%
+% Notes::
+% - In Symbolic Toolbox versions prior to V5.7 (2011b) the function to 
+%   create Embedded Matlab Function blocks from symbolic expressions is 
+%   'emlBlock'.
+% - Since V5.7 (2011b) there is another function named 
+%   'matlabFunctionBlock' which replaces the old function. 
+% - symexpr2slblock is a wrapper around both functions, which 
+%   checks for the installed Symbolic Toolbox version and calls the 
+%   required function accordingly.
+%
+% Authors::
+%  Joern Malzahn   
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also emlBlock, matlabFunctionBlock.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [] = symexpr2slblock(varargin)
+ 
+% V5.8 (R2012a)
+if verLessThan('symbolic','5.7')                                   
+    emlBlock(varargin{:});
+else
+    matlabFunctionBlock(varargin{:});
+end

lwrserv/matlab/rvctools/common/tb_optparse.m

@@ -0,0 +1,243 @@
+%OPTPARSE Standard option parser for Toolbox functions
+%
+% [OPTOUT,ARGS] = TB_OPTPARSE(OPT, ARGLIST) is a generalized option parser for
+% Toolbox functions.  It supports options that have an assigned value, boolean 
+% or enumeration types (string or int).
+%
+% The software pattern is:
+%
+%       function(a, b, c, varargin)
+%       opt.foo = true;
+%       opt.bar = false;
+%       opt.blah = [];
+%       opt.choose = {'this', 'that', 'other'};
+%       opt.select = {'#no', '#yes'};
+%       opt = tb_optparse(opt, varargin);
+%
+% Optional arguments to the function behave as follows:
+%   'foo'           sets opt.foo <- true
+%   'nobar'         sets opt.foo <- false
+%   'blah', 3       sets opt.blah <- 3
+%   'blah', {x,y}   sets opt.blah <- {x,y}
+%   'that'          sets opt.choose <- 'that'
+%   'yes'           sets opt.select <- 2 (the second element)
+%
+% and can be given in any combination.
+%
+% If neither of 'this', 'that' or 'other' are specified then opt.choose <- 'this'.
+% Alternatively if:
+%        opt.choose = {[], 'this', 'that', 'other'};
+% then if neither of 'this', 'that' or 'other' are specified then opt.choose <- []
+%
+% If neither of 'no' or 'yes' are specified then opt.select <- 1.
+%
+% Note:
+% - That the enumerator names must be distinct from the field names.
+% - That only one value can be assigned to a field, if multiple values
+%    are required they must be converted to a cell array.
+% - To match an option that starts with a digit, prefix it with 'd_', so
+%   the field 'd_3d' matches the option '3d'.
+%
+% The allowable options are specified by the names of the fields in the
+% structure opt.  By default if an option is given that is not a field of 
+% opt an error is declared.  
+%
+% Sometimes it is useful to collect the unassigned options and this can be 
+% achieved using a second output argument
+%           [opt,arglist] = tb_optparse(opt, varargin);
+% which is a cell array of all unassigned arguments in the order given in
+% varargin.
+%
+% The return structure is automatically populated with fields: verbose and
+% debug.  The following options are automatically parsed:
+%   'verbose'           sets opt.verbose <- true
+%   'verbose=2'         sets opt.verbose <- 2 (very verbose)
+%   'verbose=3'         sets opt.verbose <- 3 (extremeley verbose)
+%   'verbose=4'         sets opt.verbose <- 4 (ridiculously verbose)
+%   'debug', N          sets opt.debug <- N
+%   'setopt', S         sets opt <- S
+%   'showopt'           displays opt and arglist
+
+% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
+%
+% Copyright (C) 1993-2011, by Peter I. Corke
+%
+% This file is part of The Robotics Toolbox for MATLAB (RTB).
+% 
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+% 
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+% 
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+% Modifications by Joern Malzahn to support classes in addition to structs
+
+function [opt,others] = tb_optparse(in, argv)
+
+    if nargin == 1
+        argv = {};
+    end
+
+    if ~iscell(argv)
+        error('RTB:tboptparse:badargs', 'input must be a cell array');
+    end
+
+    arglist = {};
+
+    argc = 1;
+    opt = in;
+    
+    opt.verbose = false;
+    opt.debug = 0;
+
+    showopt = false;
+
+    while argc <= length(argv)
+        option = argv{argc};
+        assigned = false;
+        
+        if isstr(option)
+
+            switch option
+            % look for hardwired options
+            case 'verbose'
+                opt.verbose = true;
+                assigned = true;
+            case 'verbose=2'
+                opt.verbose = 2;
+                assigned = true;
+            case 'verbose=3'
+                opt.verbose = 3;
+                assigned = true;
+            case 'verbose=4'
+                opt.verbose = 4;
+                assigned = true;
+            case 'debug'
+                opt.debug = argv{argc+1};
+                argc = argc+1;
+                assigned = true;
+            case 'setopt'
+                new = argv{argc+1};
+                argc = argc+1;
+                assigned = true;
+
+                % copy matching field names from new opt struct to current one
+                for f=fieldnames(new)'
+                    if isfield(opt, f{1})
+                        opt = setfield(opt, f{1}, getfield(new, f{1}));
+                    end
+                end
+            case 'showopt'
+                showopt = true;
+                assigned = true;
+
+            otherwise
+                % does the option match a field in the opt structure?
+%                 if isfield(opt, option) || isfield(opt, ['d_' option])
+                if any(strcmp(fieldnames(opt),option)) || any(strcmp(fieldnames(opt),['d_' option])) 
+                    
+                    if ~any(strcmp(fieldnames(opt),option))
+                        option = ['d_' option];
+                    end
+                    val = getfield(opt, option);
+                    if islogical(val)
+                        % a logical variable can only be set by an option
+                        opt = setfield(opt, option, true);
+                    else
+                        % otherwise grab its value from the next arg
+                        try
+                            opt = setfield(opt, option, argv{argc+1});
+                        catch me
+                            if strcmp(me.identifier, 'MATLAB:badsubscript')
+                                error('RTB:tboptparse:badargs', 'too few arguments provided');
+                            else
+                                rethrow(me);
+                            end
+                        end
+                        argc = argc+1;
+                    end
+                    assigned = true;
+                elseif length(option)>2 && strcmp(option(1:2), 'no') && isfield(opt, option(3:end))
+                    val = getfield(opt, option(3:end));
+                    if islogical(val)
+                        % a logical variable can only be set by an option
+                        opt = setfield(opt, option(3:end), false);
+                        assigned = true;
+                    end
+                else
+                    % the option doesnt match a field name
+                    for field=fieldnames(opt)'
+                        val = getfield(opt, field{1});
+                        if iscell(val)
+                            for i=1:length(val)
+                                if isempty(val{i})
+                                    continue;
+                                end
+                                if strcmp(option, val{i})
+                                    opt = setfield(opt, field{1}, option);
+                                    assigned = true;
+                                    break;
+                                elseif val{i}(1) == '#' && strcmp(option, val{i}(2:end))
+                                    opt = setfield(opt, field{1}, i);
+                                    assigned = true;
+                                    break;
+                                end
+                            end
+                            if assigned
+                                break;
+                            end
+                        end
+                    end
+
+
+                end
+            end % switch
+        end
+        if ~assigned
+            % non matching options are collected
+            if nargout == 2
+                arglist = [arglist argv(argc)];
+            else
+                if isstr(argv{argc})
+                    error(['unknown options: ' argv{argc}]);
+                end
+            end
+        end
+        
+        argc = argc + 1;
+    end % while
+
+    % if enumerator value not assigned, set the default value
+    if ~isempty(in)
+        for field=fieldnames(in)'
+            if iscell(getfield(in, field{1})) && iscell(getfield(opt, field{1}))
+                val = getfield(opt, field{1});
+                if isempty(val{1})
+                    opt = setfield(opt, field{1}, val{1});
+                elseif val{1}(1) == '#'
+                    opt = setfield(opt, field{1}, 1);
+                else
+                    opt = setfield(opt, field{1}, val{1});
+                end
+            end
+        end
+    end
+                        
+    if showopt
+        fprintf('Options:\n');
+        opt
+        arglist
+    end
+
+    if nargout == 2
+        others = arglist;
+    end

lwrserv/matlab/rvctools/common/xaxis.m

@@ -0,0 +1,43 @@
+%XAXIS  Set X-axis scaling
+%
+% XAXIS(MAX) set x-axis scaling from 0 to MAX.
+%
+% XAXIS(MIN, MAX) set x-axis scaling from MIN to MAX.
+%
+% XAXIS([MIN MAX]) as above.
+%
+% XAXIS restore automatic scaling for x-axis.
+
+function xaxis(varargin)
+
+    opt.all = false;
+    [opt,args] = tb_optparse(opt, varargin);
+    
+    if length(args) == 0
+        [x,y] = ginput(2);
+        mn = x(1);
+        mx = x(2);
+    elseif length(args) == 1
+        if length(args{1}) == 1
+            mn = 0;
+            mx = args{1};
+        elseif length(args{1}) == 2
+            mn = args{1}(1);
+            mx = args{1}(2);
+        end
+    elseif length(args) == 2
+        mn = args{1};
+        mx = args{2};
+    end
+
+    if opt.all
+        for a=get(gcf,'Children')',
+            if strcmp(get(a, 'Type'), 'axes') == 1,
+                set(a, 'XLimMode', 'manual', 'XLim', [mn mx])
+                set(a, 'YLimMode', 'auto')
+            end
+        end
+    else
+        set(gca, 'XLimMode', 'manual', 'XLim', [mn mx])
+        set(gca, 'YLimMode', 'auto')
+    end

lwrserv/matlab/rvctools/common/xyzlabel.m

@@ -0,0 +1,8 @@
+%XYZLABEL Label X, Y and Z axes
+%
+% XYZLABEL label the x-, y- and z-axes with 'X', 'Y', and 'Z' 
+% respectiveley
+function xyzlabel
+    xlabel('x');
+    ylabel('y');
+    zlabel('z');

lwrserv/matlab/rvctools/common/yaxis.m

@@ -0,0 +1,25 @@
+%YAYIS	set Y-axis scaling
+%
+%	YAYIS(max)
+%	YAYIS(min, max)
+%
+%	YAXIS	restore automatic scaling for this axis
+
+function yaxis(a1, a2)
+	if nargin == 0,
+		set(gca, 'YLimMode', 'auto')
+		return
+	elseif nargin == 1,
+		if length(a1) == 1,
+			mn = 0;
+			mx = a1;
+		elseif length(a1) == 2,
+			mn = a1(1);
+			mx = a1(2);
+		end
+	elseif nargin == 2,
+		mn = a1;
+		mx = a2;
+	end
+
+	set(gca, 'YLimMode', 'manual', 'YLim', [mn mx])

lwrserv/matlab/rvctools/robot/@CodeGenerator/CodeGenerator.m

@@ -0,0 +1,345 @@
+%CODEGENERATOR Class for code generation
+%
+% Objects of the CodeGenerator class automatcally generate robot specific
+% code, as either M-functions or real-time capable SerialLink blocks.
+%
+% The various methods return symbolic expressions for robot kinematic and
+% dynamic functions, and optionally support side effects such as:
+% - M-functions with symbolic robot specific model code
+% - real-time capable robot specific Simulink blocks
+% - mat-files with symbolic robot specific model expressions
+%
+% Example::
+%
+%        % load robot model
+%        mdl_twolink
+%
+%        cg = CodeGenerator(twolink);
+%        cg.geneverything();
+%
+%        % a new class has been automatically generated in the robot directory.
+%        addpath robot
+%
+%        tl = @robot();
+%        % this class is a subclass of SerialLink, and thus polymorphic with
+%        % SerialLink but its methods have been overloaded with robot-specific code,
+%        % for example
+%        T = tl.fkine([0.2 0.3]);
+%        % uses concise symbolic expressions rather than the generalized A-matrix
+%        % approach
+%
+%        % The Simulink block library containing robot-specific blocks can be
+%        % opened by
+%        open robot/robotslib.slx
+%        % and the blocks dragged into your own models.
+%
+% Methods::
+%
+%  gencoriolis     generate Coriolis/centripetal code
+%  genfdyn         generate forward dynamics code
+%  genfkine        generate forward kinematics code
+%  genfriction     generate joint frictionc code
+%  gengravload     generarte gravity load code
+%  geninertia      general inertia matrix code
+%  geninvdyn       generate forward dynamics code
+%  genjacobian     generate Jacobian code
+%  geneverything   generate code for all of the above
+%
+% Properties (read/write)::
+%
+%  basepath       basic working directory of the code generator
+%  robjpath       subdirectory for specialized MATLAB functions
+%  sympath        subdirectory for symbolic expressions
+%  slib           filename of the Simulink library
+%  slibpath       subdirectory for the Simulink library
+%  verbose        print code generation progress on console (logical)
+%  saveresult     save symbolic expressions to .mat-files (logical)
+%  logfile        print modeling progress to specified text file (string)
+%  genmfun        generate executable M-functions (logical)
+%  genslblock     generate Embedded MATLAB Function blocks (logical)
+%
+% Object properties (read only)::
+%  rob            SerialLink object to generate code for (1x1).
+%
+% Notes::
+%  - Requires the MATLAB Symbolic Toolbox
+%  - For robots with > 3 joints the symbolic expressions are massively
+%    complex, they are slow and you may run out of memory.
+%  - As much as possible the symbolic calculations are down row-wise to
+%    reduce the computation/memory burden.
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also SerialLink, Link.
+
+% Copyright (C) 1993-2012, by Peter I. Corke
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+classdef CodeGenerator
+    properties (SetAccess = private)
+        rob
+    end
+    properties
+        basepath;
+        robjpath;
+        sympath;
+        slib;
+        slibpath;
+        verbose;
+        saveresult;
+        logfile;
+        genmfun;
+        genslblock;
+    end
+    properties (GetAccess = private)
+        debug;        % just appears because of tb_optparse, so hide it from the user
+    end
+    methods
+
+        function CGen = CodeGenerator(rob,varargin)
+        %CodeGenerator.CodeGenerator Construct a code generator object
+        %
+        % cGen = CodeGenerator(ROB) is a code generator object for the SerialLink
+        % object ROB.
+        %
+        % cGen = CodeGenerator(ROB, OPTIONS) as above but with options described below.
+        %
+        % Options::
+        %
+        % The following option sets can be passed as an optional parameter:
+        %
+        %  'default'     set the options: verbose, saveResult, genMFun, genSLBlock
+        %  'debug'       set the options: verbose, saveResult, genMFun, genSLBlock 
+        %                and create a logfile named 'robModel.log' in the working directory
+        %  'silent'      set the options: saveResult, genMFun, genSLBlock
+        %  'disk'        set the options: verbose, saveResult
+        %  'workspace'   set the option: verbose; just outputs symbolic expressions to workspace
+        %  'mfun'        set the options: verbose, saveResult, genMFun
+        %  'slblock'     set the options: verbose, saveResult, genSLBlock
+        %
+        % If 'optionSet' is ommitted, then 'default' is used. The options control the code generation and user information: 
+        %
+        %  'verbose'           write code generation progress to command window
+        %  'saveResult         save results to hard disk (always enabled, when genMFun and genSLBlock are set)
+        %  'logFile',logfile   write code generation progress to specified logfile
+        %  'genMFun'           generate robot specific m-functions
+        %  'genSLBlock'        generate real-time capable robot specific Simulink blocks
+        % 
+        %  Any option may also be modified individually as optional parameter value pairs.
+        %
+        % Author::
+        %  Joern Malzahn
+        %  2012 RST, Technische Universitaet Dortmund, Germany.
+        %  http://www.rst.e-technik.tu-dortmund.de
+
+            if ~isa(rob,'SerialLink')
+                error('CodeGenerator:wrongConstructorInput','The input variable %s must be a SerialLink object.',inputname(1));
+            end
+            
+            if ~issym(rob)
+                CGen.rob = rob.sym;
+            else
+                CGen.rob = rob;
+            end
+            
+            % defaults
+            CGen.basepath = fullfile(pwd,CGen.getrobfname);
+            CGen.robjpath = fullfile(CGen.basepath,['@',CGen.getrobfname]);
+            CGen.sympath = fullfile(CGen.basepath,'symbolicexpressions');
+            CGen.slib = [CGen.getrobfname,'slib'];
+            CGen.slibpath = fullfile(CGen.basepath,CGen.slib);
+            CGen.verbose = false;
+            CGen.saveresult = false;
+            CGen.logfile = '';
+            CGen.genmfun = false;
+            CGen.genslblock = false;
+            CGen.debug = false;
+            
+            
+            if nargin < 2
+                varargin{1} = 'default';
+            end
+            
+            % Determine code generation option set
+            switch varargin{1}
+                case 'default'
+                    CGen = tb_optparse(CGen,...
+                        [{'verbose','saveresult','genmfun','genslblock'},varargin(2:end)]);
+                case 'debug'
+                    CGen = tb_optparse(CGen,...
+                        [{'verbose','saveresult','genmfun','genslblock','logfile','robModel.log'},varargin(2:end)]);
+                case 'silent'
+                    CGen = tb_optparse(CGen,...
+                        [{'saveresult','genmfun','genslblock'},varargin(2:end)]);
+                case 'disk'
+                    CGen = tb_optparse(CGen,...
+                        [{'verbose','saveresult'},varargin(2:end)]);
+                case 'workspace'
+                    CGen = tb_optparse(CGen,...
+                        [{'verbose'},varargin(2:end)]);
+                case 'mfun'
+                    CGen = tb_optparse(CGen,...
+                        [{'verbose','saveresult','genmfun'},varargin(2:end)]);
+                case 'slblock'
+                    CGen = tb_optparse(CGen,...
+                        [{'verbose','saveresult','genslblock'},varargin(2:end)]);
+                otherwise
+                    CGen = tb_optparse(CGen,varargin);
+            end
+            
+            if any([CGen.genmfun, CGen.genslblock])
+                CGen.saveresult = true;
+            end
+            
+            if ~isempty(CGen.logfile)
+                logfid = fopen(CGen.logfile,'w+'); % open or create file, discard existing contents
+                fclose(logfid);
+            end
+            CGen.logmsg([datestr(now),' +++++++++++++++++++++++++++++++++++\n']);
+            CGen.logmsg([datestr(now),'\tLog for ',CGen.getrobfname,'\n']);
+            CGen.logmsg([datestr(now),' +++++++++++++++++++++++++++++++++++\n']);
+            
+        end
+        
+        function robName = getrobfname(CGen)
+            robName = CGen.rob.name;
+            blanks = isspace(robName)==1;
+            robName(blanks)= [];
+            robName(robName=='/') = [];
+        end
+
+        function savesym(CGen,sym2save, symname, fname)
+            if ~exist(CGen.sympath,'dir')
+                mkdir(CGen.sympath)
+            end
+            
+            eval([symname,'= sym2save;']);
+            save(fullfile(CGen.sympath,fname),symname);
+        end
+
+        function [] = geneverything(CGen)
+            [t,allT] = CGen.genfkine;
+            [J0,Jn] = CGen.genjacobian;
+            [G] = CGen.gengravload;
+            [I] = CGen.geninertia;
+            [C] = CGen.gencoriolis;
+            [F] = CGen.genfriction;
+            [Iqdd] = CGen.genfdyn;
+            [tau] = CGen.geninvdyn;
+        end
+
+        function CGen = set.genmfun(CGen,value)
+            CGen.genmfun = value;
+            if value == true
+                CGen.saveresult = value;
+            end
+            
+            if ~exist(fullfile(CGen.robjpath,CGen.getrobfname),'file')
+                CGen.logmsg([datestr(now),'\tCreating ',CGen.getrobfname,' m-constructor ']);
+                CGen.createmconstructor;
+                CGen.logmsg('\t%s\n',' done!');
+            end
+        end
+
+        function CGen = set.genslblock(CGen,value)
+            CGen.genslblock = value;
+            if value == true
+                CGen.saveresult = true;
+            end
+        end
+        function [] = addpath(CGen)
+        %CodeGenerator.addpath Adds generated code to search path
+        %
+        % cGen.addpath() adds the generated m-functions and block library to the 
+        % MATLAB function search path.
+        %
+        % Author::
+        %  Joern Malzahn
+        %  2012 RST, Technische Universitaet Dortmund, Germany.
+        %  http://www.rst.e-technik.tu-dortmund.de
+        %
+        % See also addpath.
+        addpath(CGen.basepath);
+        end
+        
+        function [] = rmpath(CGen)
+        %CodeGenerator.rmpath Removes generated code from search path
+        %
+        % cGen.rmpath() removes generated m-functions and block library from the 
+        % MATLAB function search path.
+        %
+        % Author::
+        %  Joern Malzahn
+        %  2012 RST, Technische Universitaet Dortmund, Germany.
+        %  http://www.rst.e-technik.tu-dortmund.de
+        %
+        % See also rmpath.
+        rmpath(CGen.basepath);
+        end
+        
+        function [] = purge(CGen,varargin)
+        %CodeGenerator.purge  Cleanup generated files
+        %
+        % cGen.purge() deletes all generated files, first displays a question dialog to 
+        % make sure the user really wants to delete all generated files.
+        %
+        % cGen.purge(1) as above but skips the question dialog.
+        %
+        % Author::
+        %  Joern Malzahn
+        %  2012 RST, Technische Universitaet Dortmund, Germany.
+        %  http://www.rst.e-technik.tu-dortmund.de
+            dopurge = 0;
+            
+            if exist(CGen.basepath,'dir')
+                if nargin > 1
+                    dopurge = varargin{1}
+                else
+                    qstn = ['Do you really want to delete ',CGen.basepath, ' and all of it''s contents?'];
+                    tit = ['Purge: ',CGen.getrobfname];
+                    str1 = 'Yes';
+                    str2 = 'No';
+                    button = questdlg(qstn,tit,str1,str2,str1)
+                    dopurge = strcmp(button,str1);
+                end
+            end
+            
+            if dopurge
+                bdclose all
+                
+                warning off;
+                CGen.rmpath;
+                warning on;
+                
+                rmdir(CGen.basepath,'s')
+                if ~isempty(CGen.logfile) && exist(CGen.logfile,'file')
+                    delete(CGen.logfile);
+                end
+            end
+        end
+    end
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodecoriolis.m

@@ -0,0 +1,244 @@
+%CODEGENERATOR.GENCCODECORIOLIS Generate C-function for robot inertia matrix
+%
+% cGen.genccodecoriolis() generates robot-specific C-functions to compute
+% the robot coriolis matrix.
+%
+% Notes::
+% - Is called by CodeGenerator.gencoriolis if cGen has active flag genccode or
+%   genmex.
+% - The .c and .h files are generated in the directory specified by the 
+%   ccodepath property of the CodeGenerator object.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gencoriolis, CodeGenerator.genmexcoriolis.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genccodecoriolis(CGen)
+
+%%
+CGen.logmsg([datestr(now),'\tGenerating C-code for the robot coriolis matrix row' ]);
+
+
+% check for existance of C-code directories
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+if ~exist(srcDir,'dir')
+    mkdir(srcDir);
+end
+if ~exist(hdrDir,'dir')
+    mkdir(hdrDir);
+end
+
+
+[Q,QD] = CGen.rob.gencoords;
+nJoints = CGen.rob.n;
+
+%% Individual coriolis matrix rows
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['coriolis_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if exist(fname,'file')
+        tmpStruct = load(fname);
+    else
+        error ('genmfuncoriolis:SymbolicsNotFound','Save symbolic expressions to disk first!')
+    end
+    
+    funname = [CGen.getrobfname,'_',symname];
+    funfilename = [funname,'.c'];
+    hfilename = [funname,'.h'];
+    
+    % Convert symbolic expression into C-code
+    [funstr hstring] = ccodefunctionstring(tmpStruct.(symname),...
+        'funname',funname,...
+        'vars',{Q,QD},'output',['C_row_',num2str(kJoints)]);
+    
+    % Create the function description header
+    hStruct = createHeaderStructRow(CGen.rob,kJoints,funname); % create header
+    hStruct.calls = hstring;
+    hFString = CGen.constructheaderstringc(hStruct);
+    
+    %% Generate C implementation file
+    fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+    % Includes
+    fprintf(fid,'%s\n\n',...
+        ['#include "', hfilename,'"']);
+    
+    % Function
+    fprintf(fid,'%s\n\n',funstr);
+    fclose(fid);
+    
+    %% Generate C header file
+    fid = fopen(fullfile(hdrDir,hfilename),'w+');
+    
+    % Header
+    fprintf(fid,'%s\n\n',hFString);
+    
+    % Include guard
+    fprintf(fid,'%s\n%s\n\n',...
+        ['#ifndef ', upper([funname,'_h'])],...
+        ['#define ', upper([funname,'_h'])]);
+    
+    % Includes
+    fprintf(fid,'%s\n\n',...
+        '#include "math.h"');
+    
+    % Function prototype
+    fprintf(fid,'%s\n\n',hstring);
+    
+    % Include guard
+    fprintf(fid,'%s\n',...
+        ['#endif /*', upper([funname,'_h */'])]);
+    
+    fclose(fid);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+
+%% Full Coriolis matrix
+CGen.logmsg([datestr(now),'\tGenerating full coriolis matrix C-code']);
+
+symname = 'coriolis';
+
+funname = [CGen.getrobfname,'_',symname];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+outname = 'C';
+
+% Generate function prototype
+[hstring] = ccodefunctionstring(sym(zeros(nJoints)),...
+    'funname',funname,...
+    'vars',{Q,QD},'output',outname,'flag',1);
+
+% Create the function description header
+hStruct = createHeaderStructFull(CGen.rob,funname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+fprintf(fid,'%s{\n\n',hstring);
+
+fprintf(fid,'\t%s\n','/* allocate memory for individual rows */');
+for kJoints = 1:nJoints
+    fprintf(fid,'\tdouble row%d[%d][1];\n',kJoints,nJoints);
+end
+fprintf(fid,'%s\n',' '); % empty line
+
+fprintf(fid,'\t%s\n','/* call the row routines */');
+for kJoints = 1:nJoints
+    fprintf(fid,'\t%s_coriolis_row_%d(row%d, input1, input2);\n',CGen.getrobfname,kJoints,kJoints);
+end
+fprintf(fid,'%s\n',' '); % empty line\n
+
+% Copy results into output matrix
+for iJoints = 1:nJoints
+    for kJoints = 1:nJoints
+        fprintf(fid,'\t%s[%d][%d] = row%d[%d][0];\n',outname,kJoints-1,iJoints-1,iJoints,kJoints-1);
+    end
+    fprintf(fid,'%s\n',' '); % empty line\n
+end
+
+fprintf(fid,'%s\n','}');
+
+fclose(fid);
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Header
+fprintf(fid,'%s\n\n',hFString);
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([funname,'_h'])],...
+    ['#define ', upper([funname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    '#include "math.h"');
+for kJoints = 1:nJoints
+    rowstring = [CGen.getrobfname,'_coriolis_row_',num2str(kJoints)];
+    fprintf(fid,'%s\n',...
+        ['#include "',rowstring,'.h"']);
+end
+fprintf(fid,'%s\n',' '); % empty line
+
+% Function prototype
+fprintf(fid,'%s;\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([funname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+end
+
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructRow(rob,curJointIdx,fName)
+[~,hStruct.funName] = fileparts(fName);
+hStruct.shortDescription = ['Computation of the robot specific Coriolis matrix row for corresponding to joint ', num2str(curJointIdx), ' of ',num2str(rob.n),'.'];
+hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
+    ['Coriolis matrix row number ', num2str(curJointIdx),' of ',num2str(rob.n),' for ',rob.name,'. Angles have to be given in radians!']};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates'],...
+                   ['input2: ',int2str(rob.n),'-element vector of generalized velocities']};
+hStruct.outputs = {['C_row_',int2str(curJointIdx),':  [1x',int2str(rob.n),'] row of the robot Coriolis matrix']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'coriolis'};
+end
+ 
+function hStruct = createHeaderStructFull(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Coriolis matrix for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {'Given a full set of joint variables the function computes the',...
+    'Coriolis matrix of the robot. Angles have to be given in radians!'};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates'],...
+                   ['input2: ',int2str(rob.n),'-element vector of generalized velocities']};
+hStruct.outputs = {['C:  [',int2str(rob.n),'x',int2str(rob.n),'] Coriolis matrix']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'coriolis'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefdyn.m

@@ -0,0 +1,210 @@
+%CODEGENERATOR.GENCCODEFDYN Generate C-code for forward dynamics
+%
+% cGen.genccodeinvdyn() generates a robot-specific C-code to compute the
+% forward dynamics.
+%
+% Notes::
+% - Is called by CodeGenerator.genfdyn if cGen has active flag genccode or
+%   genmex.
+% - The .c and .h files are generated in the directory specified
+%   by the ccodepath property of the CodeGenerator object.
+% - The resulting C-function is composed of previously generated C-functions
+%   for the inertia matrix, Coriolis matrix, vector of gravitational load and
+%   joint friction vector. This function recombines these components to
+%   compute the forward dynamics.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genfdyn,CodeGenerator.genccodeinvdyn.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [ ] = genccodefdyn( CGen )
+
+checkexistanceofcfunctions(CGen);
+[Q, QD] = CGen.rob.gencoords;
+tau = CGen.rob.genforces;
+nJoints = CGen.rob.n;
+
+% Check for existance of C-code directories
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+if ~exist(srcDir,'dir')
+    mkdir(srcDir);
+end
+if ~exist(hdrDir,'dir')
+    mkdir(hdrDir);
+end
+
+symname = 'fdyn';
+outputname = 'QDD';
+
+funname = [CGen.getrobfname,'_accel'];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+
+CGen.logmsg([datestr(now),'\tGenerating forward dynamics C-code']);
+
+
+% Convert symbolic expression into C-code
+dummy = sym(zeros(nJoints,1));
+[funstr hstring] = ccodefunctionstring(dummy,...
+    'funname',funname,...
+    'vars',{Q, QD, tau},'output',outputname,...
+    'flag',1);
+
+% Create the function description header
+hStruct = createHeaderStruct(CGen.rob,funname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+% Function
+fprintf(fid,'%s{\n\n',funstr);
+
+
+% Allocate memory
+fprintf(fid,'\t%s\n','/* declare variables */');
+fprintf(fid,'\t%s\n','int iCol;');
+fprintf(fid,'\t%s\n',['double inertia[',num2str(nJoints),'][',num2str(nJoints),'];']);
+fprintf(fid,'\t%s\n',['double invinertia[',num2str(nJoints),'][',num2str(nJoints),'];']);
+fprintf(fid,'\t%s\n',['double coriolis[',num2str(nJoints),'][',num2str(nJoints),'];']);
+fprintf(fid,'\t%s\n',['double gravload[',num2str(nJoints),'][1];']);
+fprintf(fid,'\t%s\n',['double friction[',num2str(nJoints),'][1];']);
+fprintf(fid,'\t%s\n',['double tmpTau[1][',num2str(nJoints),'];']);
+
+fprintf(fid,'%s\n',' '); % empty line
+
+fprintf(fid,'\t%s\n','/* call the computational routines */');
+fprintf(fid,'\t%s\n',[CGen.getrobfname,'_','inertia(inertia, input1);']);
+fprintf(fid,'\t%s\n',['gaussjordan(inertia, invinertia, ',num2str(nJoints),');']);
+fprintf(fid,'\t%s\n',[CGen.getrobfname,'_','coriolis(coriolis, input1, input2);']);
+fprintf(fid,'\t%s\n',[CGen.getrobfname,'_','gravload(gravload, input1);']);
+fprintf(fid,'\t%s\n',[CGen.getrobfname,'_','friction(friction, input2);']);
+
+fprintf(fid,'%s\n',' '); % empty line
+
+fprintf(fid,'\t%s\n','/* fill temporary vector */');
+fprintf(fid,'\t%s\n',['matvecprod(tmpTau, coriolis, input2,',num2str(nJoints),',',num2str(nJoints),');']);
+fprintf(fid,'\t%s\n',['for (iCol = 0; iCol < ',num2str(nJoints),'; iCol++){']);
+fprintf(fid,'\t\t%s\n','tmpTau[0][iCol] = input3[iCol] -  tmpTau[0][iCol] - gravload[iCol][0] + friction[iCol][0];');
+fprintf(fid,'\t%s\n','}');
+
+fprintf(fid,'\t%s\n','/* compute acceleration */');
+fprintf(fid,'\t%s\n',['matvecprod(QDD, invinertia, tmpTau,',num2str(nJoints),',',num2str(nJoints),');']);
+
+fprintf(fid,'%s\n','}');
+
+fclose(fid);
+
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Function description header
+fprintf(fid,'%s\n\n',hFString);
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([funname,'_h'])],...
+    ['#define ', upper([funname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n%s\n%s\n%s\n%s\n%s\n\n',...
+    '#include "matvecprod.h"',...
+    '#include "gaussjordan.h"',...
+    ['#include "',CGen.getrobfname,'_inertia.h"'],...
+    ['#include "',CGen.getrobfname,'_coriolis.h"'],...
+    ['#include "',CGen.getrobfname,'_gravload.h"'],...
+    ['#include "',CGen.getrobfname,'_friction.h"']);
+
+
+fprintf(fid,'%s\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([funname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+function [] = checkexistanceofcfunctions(CGen)
+
+if ~(exist(fullfile(CGen.ccodepath,'src','inertia.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','inertia.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Inertia C-code not found or not complete! Generating:');
+    CGen.genccodeinertia;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','coriolis.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'coriolis','inertia.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Coriolis C-code not found or not complete! Generating:');
+    CGen.genccodecoriolis;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','gravload.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','gravload.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Gravload C-code not found or not complete! Generating:');
+    CGen.genccodegravload;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','friction.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','friction.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Friction C-code not found or not complete! Generating:');
+    CGen.genccodefriction;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','matvecprod.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','matvecprod.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Matrix-Vector product C-code not found or not complete! Generating:');
+    CGen.genmatvecprodc;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','gaussjordan.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','gaussjordan.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Gauss-Jordan matrix inversion C-code not found or not complete! Generating:');
+    CGen.gengaussjordanc;
+end
+
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStruct(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['C-implementation of the forward dynamics for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {'Given a full set of joint angles and velocities',...
+    'this function computes the joint space accelerations effected by the generalized forces. Angles have to be given in radians!'};
+hStruct.inputs = { ['input1:  ',int2str(rob.n),'-element vector of generalized coordinates'],...
+    ['input2:  ',int2str(rob.n),'-element vector of generalized velocities'],...
+    ['input3:  [',int2str(rob.n),'x1] vector of generalized forces.']};
+hStruct.outputs = {['QDD:  ',int2str(rob.n),'-element output vector of generalized accelerations.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'invdyn'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefkine.m

@@ -0,0 +1,219 @@
+%CODEGENERATOR.GENCCODEFKINE Generate C-code for the forward kinematics
+%
+% cGen.genccodefkine() generates a robot-specific C-function to compute
+% forward kinematics.
+%
+% Notes::
+% - Is called by CodeGenerator.genfkine if cGen has active flag genccode or
+%   genmex
+% - The generated .c and .h files are wirtten to the directory specified in
+%   the ccodepath property of the CodeGenerator object.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genfkine, CodeGenerator.genmexfkine.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genccodefkine(CGen)
+
+%% Check for existance symbolic expressions
+% Load symbolics
+symname = 'fkine';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfunfkine:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+%% Forward kinematics up to tool center point
+CGen.logmsg([datestr(now),'\tGenerating forward kinematics c-code up to the end-effector frame: ']);
+
+%% Prerequesites
+% check for existance of C-code directories
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+if ~exist(srcDir,'dir')
+    mkdir(srcDir);
+end
+if ~exist(hdrDir,'dir')
+    mkdir(hdrDir);
+end
+
+funname = [CGen.getrobfname,'_',symname];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+
+Q = CGen.rob.gencoords;
+
+
+% Convert symbolic expression into C-code
+[funstr hstring] = ccodefunctionstring(tmpStruct.(symname),...
+    'funname',funname,...
+    'vars',{Q},'output','T');
+
+% Create the function description header
+hStruct = createHeaderStructFkine(CGen.rob,symname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+% Function
+fprintf(fid,'%s\n\n',funstr);
+fclose(fid);
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Header
+fprintf(fid,'%s\n\n',hFString);
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([funname,'_h'])],...
+    ['#define ', upper([funname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    '#include "math.h"');
+
+% Function prototype
+fprintf(fid,'%s\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([funname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+
+
+%% Individual joint forward kinematics
+CGen.logmsg([datestr(now),'\tGenerating forward kinematics m-function up to joint: ']);
+for iJoints=1:CGen.rob.n
+    CGen.logmsg(' %i ',iJoints);
+    
+    % Load symbolics
+    symname = ['T0_',num2str(iJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    tmpStruct = struct;
+    tmpStruct = load(fname);
+    
+    funname = [CGen.getrobfname,'_',symname];
+    funfilename = [funname,'.c'];
+    hfilename = [funname,'.h'];
+    Q = CGen.rob.gencoords;
+    
+        % Convert symbolic expression into C-code
+    [funstr hstring] = ccodefunctionstring(tmpStruct.(symname),...
+        'funname',funname,...
+        'vars',{Q},'output','T');
+        
+    % Create the function description header
+    hStruct = createHeaderStruct(CGen.rob,iJoints,funname); % create header
+    hStruct.calls = hstring;
+    hFString = CGen.constructheaderstringc(hStruct);
+    
+    %% Generate C implementation file
+    fid = fopen(fullfile(srcDir,funfilename),'w+');
+    
+    % Includes
+    fprintf(fid,'%s\n\n',...
+        ['#include "', hfilename,'"']);
+    
+    % Function
+    fprintf(fid,'%s\n\n',funstr);
+    fclose(fid);
+    
+    %% Generate C header file
+    fid = fopen(fullfile(hdrDir,hfilename),'w+');
+    
+    % Description header
+    fprintf(fid,'%s\n\n',hFString);
+    
+    % Include guard
+    fprintf(fid,'%s\n%s\n\n',...
+        ['#ifndef ', upper([funname,'_h'])],...
+        ['#define ', upper([funname,'_h'])]);
+    
+    % Includes
+    fprintf(fid,'%s\n\n',...
+        '#include "math.h"');
+    
+    % Function prototype
+    fprintf(fid,'%s\n\n',hstring);
+    
+    % Include guard
+    fprintf(fid,'%s\n',...
+        ['#endif /*', upper([funname,'_h */'])]);
+    
+    fclose(fid);
+end
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStruct(rob,curBody,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['C version of the forward kinematics for the ',rob.name,' arm up to frame ',int2str(curBody),' of ',int2str(rob.n),'.'];
+hStruct.detailedDescription = {['Given a set of joint variables up to joint number ',int2str(curBody),' the function'],...
+    'computes the pose belonging to that joint with respect to the base frame.'};
+hStruct.inputs = { 'input1 Vector of generalized coordinates. Angles have to be given in radians!'};
+hStruct.outputs = {'T [4x4] Homogenous transformation matrix relating the pose of the tool for the given joint values to the base frame.'};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!<BR>',...
+    'Code generator written by:<BR>',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)<BR>'};
+hStruct.seeAlso = {rob.name};
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructFkine(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['C version of the forward kinematics solution including tool transformation for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {['Given a full set of joint variables the function'],...
+    'computes the pose belonging to that joint with respect to the base frame.'};
+hStruct.inputs = { 'input1 Vector of generalized coordinates. Angles have to be given in radians!'};
+hStruct.outputs = {'T [4x4] Homogenous transformation matrix relating the pose of the tool for the given joint values to the base frame.'};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!<BR>',...
+    'Code generator written by:<BR>',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)<BR>'};
+hStruct.seeAlso = {'jacob0'};
+end
+

lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefriction.m

@@ -0,0 +1,133 @@
+%CODEGENERATOR.GENCCODEFRICTION Generate C-code for the joint friction
+%
+% cGen.genccodefriction() generates a robot-specific C-function to compute
+% vector of friction torques/forces.
+%
+% Notes::
+% - Is called by CodeGenerator.genfriction if cGen has active flag genccode or
+%   genmex
+% - The generated .c and .h files are wirtten to the directory specified in
+%   the ccodepath property of the CodeGenerator object.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genfriction, CodeGenerator.genmexfriction.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genccodefriction(CGen)
+
+%% Check for existance symbolic expressions
+% Load symbolics
+symname = 'friction';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfunfriction:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+%% Prerequesites
+CGen.logmsg([datestr(now),'\tGenerating C-code for the vector of frictional torques/forces' ]);
+
+% Check for existance of C-code directories
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+if ~exist(srcDir,'dir')
+    mkdir(srcDir);
+end
+if ~exist(hdrDir,'dir')
+    mkdir(hdrDir);
+end
+
+funname = [CGen.getrobfname,'_',symname];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+
+[~,QD] = CGen.rob.gencoords;
+
+% Convert symbolic expression into C-code
+[funstr hstring] = ccodefunctionstring(tmpStruct.(symname),...
+    'funname',funname,...
+    'vars',{QD},'output','F');
+
+% Create the function description header
+hStruct = createHeaderStruct(CGen.rob,funname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+% Function
+fprintf(fid,'%s\n\n',funstr);
+fclose(fid);
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Description header
+fprintf(fid,'%s\n\n',hFString);
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([funname,'_h'])],...
+    ['#define ', upper([funname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    '#include "math.h"');
+
+% Function prototype
+fprintf(fid,'%s\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([funname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the function description header contents
+function hStruct = createHeaderStruct(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Joint friction for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {['Given a full set of generalized joint velocities the function'],...
+    'computes the friction forces/torques. Angles have to be given in radians!'};
+hStruct.inputs = { ['input1:  ',int2str(rob.n),'-element vector of generalized velocities.']};
+hStruct.outputs = {['F:  [',int2str(rob.n),'x1] output vector of joint friction forces/torques.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'gravload'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodegravload.m

@@ -0,0 +1,134 @@
+%CODEGENERATOR.GENCCODEGRAVLOAD Generate C-code for the vector of
+%gravitational load torques/forces
+%
+% cGen.genccodegravload() generates a robot-specific C-function to compute
+% vector of gravitational load torques/forces.
+%
+% Notes::
+% - Is called by CodeGenerator.gengravload if cGen has active flag genccode or
+%   genmex
+% - The generated .c and .h files are wirtten to the directory specified in
+%   the ccodepath property of the CodeGenerator object.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gengravload, CodeGenerator.genmexgravload.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genccodegravload(CGen)
+
+%% Check for existance symbolic expressions
+% Load symbolics
+symname = 'gravload';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfungravload:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+%% Prerequesites
+CGen.logmsg([datestr(now),'\tGenerating C-code for the vector of gravitational load torques/forces' ]);
+
+% check for existance of C-code directories
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+if ~exist(srcDir,'dir')
+    mkdir(srcDir);
+end
+if ~exist(hdrDir,'dir')
+    mkdir(hdrDir);
+end
+
+funname = [CGen.getrobfname,'_',symname];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+
+Q = CGen.rob.gencoords;
+
+% Convert symbolic expression into C-code
+[funstr hstring] = ccodefunctionstring(tmpStruct.(symname),...
+    'funname',funname,...
+    'vars',{Q},'output','G');
+
+% Create the function description header
+hStruct = createHeaderStructGravity(CGen.rob,funname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+% Function
+fprintf(fid,'%s\n\n',funstr);
+fclose(fid);
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Header
+fprintf(fid,'%s\n\n',hFString);
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([funname,'_h'])],...
+    ['#define ', upper([funname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    '#include "math.h"');
+
+% Function prototype
+fprintf(fid,'%s\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([funname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the function description header contents
+function hStruct = createHeaderStructGravity(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Computation of the configuration dependent vector of gravitational load forces/torques for ',rob.name];
+hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
+                               'configuration dependent vector of gravitational load forces/torques. Angles have to be given in radians!'};
+hStruct.inputs = { ['input1: ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['G: [',int2str(rob.n),'x1] output vector of gravitational load forces/torques.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'inertia'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodeinertia.m

@@ -0,0 +1,242 @@
+%CODEGENERATOR.GENCCODEINERTIA Generate C-function for robot inertia matrix
+%
+% cGen.genccodeinertia() generates robot-specific C-functions to compute
+% the robot inertia matrix.
+%
+% Notes::
+% - Is called by CodeGenerator.geninertia if cGen has active flag genccode or
+%   genmex.
+% - The generated .c and .h files are generated in the directory specified
+%   by the ccodepath property of the CodeGenerator object.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninertia, CodeGenerator.genmexinertia.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genccodeinertia(CGen)
+
+%%
+CGen.logmsg([datestr(now),'\tGenerating C-code for the robot inertia matrix row' ]);
+
+
+% check for existance of C-code directories
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+if ~exist(srcDir,'dir')
+    mkdir(srcDir);
+end
+if ~exist(hdrDir,'dir')
+    mkdir(hdrDir);
+end
+
+
+Q = CGen.rob.gencoords;
+nJoints = CGen.rob.n;
+
+%% Individual inertia matrix rows
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['inertia_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if exist(fname,'file')
+        tmpStruct = load(fname);
+    else
+        error ('genmfuninertia:SymbolicsNotFound','Save symbolic expressions to disk first!')
+    end
+    
+    funname = [CGen.getrobfname,'_',symname];
+    funfilename = [funname,'.c'];
+    hfilename = [funname,'.h'];
+    
+    % Convert symbolic expression into C-code
+    [funstr hstring] = ccodefunctionstring(tmpStruct.(symname),...
+        'funname',funname,...
+        'vars',{Q},'output',['I_row_',num2str(kJoints)]);
+    
+    % Create the function description header
+    hStruct = createHeaderStructRow(CGen.rob,kJoints,funname); % create header
+    hStruct.calls = hstring;
+    hFString = CGen.constructheaderstringc(hStruct);
+   
+    %% Generate C implementation file
+    fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+    % Includes
+    fprintf(fid,'%s\n\n',...
+        ['#include "', hfilename,'"']);
+    
+    % Function
+    fprintf(fid,'%s\n\n',funstr);
+    fclose(fid);
+    
+    %% Generate C header file
+    fid = fopen(fullfile(hdrDir,hfilename),'w+');
+    
+    % Header
+    fprintf(fid,'%s\n\n',hFString);
+    
+    % Include guard
+    fprintf(fid,'%s\n%s\n\n',...
+        ['#ifndef ', upper([funname,'_h'])],...
+        ['#define ', upper([funname,'_h'])]);
+    
+    % Includes
+    fprintf(fid,'%s\n\n',...
+        '#include "math.h"');
+    
+    % Function prototype
+    fprintf(fid,'%s\n\n',hstring);
+    
+    % Include guard
+    fprintf(fid,'%s\n',...
+        ['#endif /*', upper([funname,'_h */'])]);
+    
+    fclose(fid);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+
+%% Full inertia matrix
+CGen.logmsg([datestr(now),'\tGenerating full inertia matrix C-code']);
+
+symname = 'inertia';
+
+funname = [CGen.getrobfname,'_',symname];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+outname = 'I';
+
+% Generate function prototype
+[hstring] = ccodefunctionstring(sym(zeros(nJoints)),...
+    'funname',funname,...
+    'vars',{Q},'output',outname,'flag',1);
+
+% Create the function description header
+hStruct = createHeaderStructFull(CGen.rob,funname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+fprintf(fid,'%s{\n\n',hstring);
+
+fprintf(fid,'\t%s\n','/* allocate memory for individual rows */');
+for kJoints = 1:nJoints
+    fprintf(fid,'\tdouble row%d[%d][1];\n',kJoints,nJoints);
+end
+fprintf(fid,'%s\n',' '); % empty line
+
+fprintf(fid,'\t%s\n','/* call the row routines */');
+for kJoints = 1:nJoints
+    fprintf(fid,'\t%s_inertia_row_%d(row%d, input1);\n',CGen.getrobfname,kJoints,kJoints);
+end
+fprintf(fid,'%s\n',' '); % empty line\n
+
+% Copy results into output matrix
+for iJoints = 1:nJoints
+    for kJoints = 1:nJoints
+        fprintf(fid,'\t%s[%d][%d] = row%d[%d][0];\n',outname,kJoints-1,iJoints-1,iJoints,kJoints-1);
+    end
+    fprintf(fid,'%s\n',' '); % empty line\n
+end
+
+fprintf(fid,'%s\n','}');
+
+fclose(fid);
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Header
+fprintf(fid,'%s\n\n',hFString);
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([funname,'_h'])],...
+    ['#define ', upper([funname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    '#include "math.h"');
+for kJoints = 1:nJoints
+    rowstring = [CGen.getrobfname,'_inertia_row_',num2str(kJoints)];
+    fprintf(fid,'%s\n',...
+        ['#include "',rowstring,'.h"']);
+end
+fprintf(fid,'%s\n',' '); % empty line
+
+% Function prototype
+fprintf(fid,'%s;\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([funname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+end
+
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructRow(rob,curJointIdx,fName)
+[~,hStruct.funName] = fileparts(fName);
+hStruct.shortDescription = ['Computation of the robot specific inertia matrix row for corresponding to joint ', num2str(curJointIdx), ' of ',num2str(rob.n),'.'];
+hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
+    ['inertia matrix row number ', num2str(curJointIdx),' of ',num2str(rob.n),' for ',rob.name,'. Angles have to be given in radians!']};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['I_row_',int2str(curJointIdx),':  [1x',int2str(rob.n),'] output row of the robot inertia matrix.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'coriolis'};
+end
+
+function hStruct = createHeaderStructFull(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Inertia matrix for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {'Given a full set of joint variables the function computes the',...
+    'inertia Matrix of the robot. Angles have to be given in radians!'};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['I:  [',int2str(rob.n),'x',int2str(rob.n),']output inertia matrix.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'coriolis'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodeinvdyn.m

@@ -0,0 +1,212 @@
+%CODEGENERATOR.GENCCODEINVDYN Generate C-code for inverse dynamics
+% 
+% cGen.genccodeinvdyn() generates a robot-specific C-code to compute the
+% inverse dynamics.
+%
+% Notes::
+% - Is called by CodeGenerator.geninvdyn if cGen has active flag genccode or
+%   genmex.
+% - The .c and .h files are generated in the directory specified
+%   by the ccodepath property of the CodeGenerator object.
+% - The resulting C-function is composed of previously generated C-functions
+%   for the inertia matrix, coriolis matrix, vector of gravitational load and 
+%   joint friction vector. This function recombines these components to 
+%   compute the inverse dynamics.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninvdyn, CodeGenerator.genccodefdyn.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [  ] = genccodeinvdyn( CGen )
+
+checkexistanceofcfunctions(CGen);
+[Q, QD, QDD] = CGen.rob.gencoords;
+nJoints = CGen.rob.n;
+
+% Check for existance of C-code directories
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+if ~exist(srcDir,'dir')
+    mkdir(srcDir);
+end
+if ~exist(hdrDir,'dir')
+    mkdir(hdrDir);
+end
+
+symname = 'invdyn';
+outputname = 'TAU';
+
+funname = [CGen.getrobfname,'_',symname];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+
+%%
+CGen.logmsg([datestr(now),'\tGenerating inverse dynamics C-code']);
+
+% Convert symbolic expression into C-code
+dummy = sym(zeros(nJoints,1));
+[funstr hstring] = ccodefunctionstring(dummy,...
+    'funname',funname,...
+    'vars',{Q, QD, QDD},'output',outputname,...
+    'flag',1);
+
+% Create the function description header
+hStruct = createHeaderStruct(CGen.rob,funname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+% Function
+fprintf(fid,'%s{\n\n',funstr);
+
+
+% Allocate memory
+fprintf(fid,'%s\n','/* declare variables */');
+for iJoints = 1:nJoints
+	fprintf(fid,'%s\n',['double inertia_row',num2str(iJoints),'[',num2str(nJoints),'][1];']);
+end
+
+for iJoints = 1:nJoints
+    fprintf(fid,'%s\n',['double coriolis_row',num2str(iJoints),'[',num2str(nJoints),'][1];']);
+end
+
+fprintf(fid,'%s\n',['double gravload[',num2str(nJoints),'][1];']);
+
+fprintf(fid,'%s\n',['double friction[',num2str(nJoints),'][1];']);
+
+fprintf(fid,'%s\n',' '); % empty line
+fprintf(fid,'%s\n','/* call the computational routines */');
+fprintf(fid,'%s\n',[CGen.getrobfname,'_','gravload(gravload, input1);']);
+fprintf(fid,'%s\n',[CGen.getrobfname,'_','friction(friction, input2);']);
+
+fprintf(fid,'%s\n','/* rowwise routines */');
+for iJoints = 1:nJoints
+    fprintf(fid,'%s\n',[CGen.getrobfname,'_','inertia_row_',num2str(iJoints),'(inertia_row',num2str(iJoints),', input1);']);
+end
+
+for iJoints = 1:nJoints
+    fprintf(fid,'%s\n',[CGen.getrobfname,'_','coriolis_row_',num2str(iJoints),'(coriolis_row',num2str(iJoints),', input1, input2);']);
+end
+
+fprintf(fid,'%s\n',' '); % empty line
+fprintf(fid,'%s\n','/* fill output vector */');
+
+for iJoints = 1:nJoints
+    fprintf(fid,'%s\n',[outputname,'[0][',num2str(iJoints-1),'] = dotprod(inertia_row',num2str(iJoints),', input3, ',num2str(nJoints),') /* inertia */']);
+    fprintf(fid,'\t%s\n',[' + dotprod(coriolis_row',num2str(iJoints),', input2, ',num2str(nJoints),') /* coriolis */']);
+    fprintf(fid,'\t%s\n',[' + gravload[',num2str(iJoints-1),'][0]']);
+    fprintf(fid,'\t%s\n',[' - friction[',num2str(iJoints-1),'][0];']);
+end
+fprintf(fid,'%s\n',' '); % empty line
+
+fprintf(fid,'%s\n','}');
+
+fclose(fid);
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Function description header
+fprintf(fid,'%s\n\n',hFString);
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([funname,'_h'])],...
+    ['#define ', upper([funname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n%s\n%s\n%s\n%s\n\n',...
+    '#include "dotprod.h"',...
+    ['#include "',CGen.getrobfname,'_inertia.h"'],...
+    ['#include "',CGen.getrobfname,'_coriolis.h"'],...
+    ['#include "',CGen.getrobfname,'_gravload.h"'],...
+    ['#include "',CGen.getrobfname,'_friction.h"']);
+
+
+fprintf(fid,'%s\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([funname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+function [] = checkexistanceofcfunctions(CGen)
+
+if ~(exist(fullfile(CGen.ccodepath,'src','inertia.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','inertia.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Inertia C-code not found or not complete! Generating:');
+    CGen.genccodeinertia;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','coriolis.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'coriolis','inertia.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Coriolis C-code not found or not complete! Generating:');
+    CGen.genccodecoriolis;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','gravload.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','gravload.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Gravload C-code not found or not complete! Generating:');
+    CGen.genccodegravload;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','friction.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','friction.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Friction C-code not found or not complete! Generating:');
+    CGen.genccodefriction;
+end
+
+if ~(exist(fullfile(CGen.ccodepath,'src','dotprod.c'),'file') == 2) || ~(exist(fullfile(CGen.ccodepath,'include','dotprod.h'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Dot product C-code not found or not complete! Generating:');
+    CGen.gendotprodc;
+end
+
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStruct(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['C-implementation of the inverse dynamics for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {'Given a full set of joint variables and their first and second order',...
+    'temporal derivatives this function computes the joint space',...
+    'torques needed to perform the particular motion. Angles have to be given in radians!'};
+hStruct.inputs = { ['input1:  ',int2str(rob.n),'-element vector of generalized coordinates'],...
+                   ['input2:  ',int2str(rob.n),'-element vector of generalized velocities,'],...
+                   ['input3:  ',int2str(rob.n),'-element vector of generalized accelerations.']};
+hStruct.outputs = {['TAU:  [',int2str(rob.n),'x1] vector of joint forces/torques.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'fdyn'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodejacobian.m

@@ -0,0 +1,214 @@
+%CODEGENERATOR.GENCCODEJACOBIAN Generate C-functions for robot jacobians
+%
+% cGen.genccodejacobian() generates a robot-specific C-function to compute
+% the jacobians with respect to the robot base as well as the end effector.
+%
+% Notes::
+% - Is called by CodeGenerator.genjacobian if cGen has active flag genccode or
+%   genmex.
+% - The generated .c and .h files are generated in the directory specified
+%   by the ccodepath property of the CodeGenerator object.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genccodefkine, CodeGenerator.genjacobian.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genccodejacobian(CGen)
+
+%% Check for existance symbolic expressions
+% Load symbolics
+symname = 'jacob0';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfunfkine:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+%% Jacobian w.r.t. the robot base
+CGen.logmsg([datestr(now),'\tGenerating jacobian C-code with respect to the robot base frame']);
+
+% Prerequesites
+% check for existance of C-code directories
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+if ~exist(srcDir,'dir')
+    mkdir(srcDir);
+end
+if ~exist(hdrDir,'dir')
+    mkdir(hdrDir);
+end
+
+funname = [CGen.getrobfname,'_',symname];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+Q = CGen.rob.gencoords;
+
+% Convert symbolic expression into C-code
+[funstr hstring] = ccodefunctionstring(tmpStruct.(symname),...
+    'funname',funname,...
+    'vars',{Q},'output','J0');
+
+% Create the function description header
+hStruct = createHeaderStructJacob0(CGen.rob,funname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+% Function
+fprintf(fid,'%s\n\n',funstr);
+fclose(fid);
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Function description header
+fprintf(fid,'%s\n\n',hFString);
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([symname,'_h'])],...
+    ['#define ', upper([symname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    '#include "math.h"');
+
+% Function signature
+fprintf(fid,'%s\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([symname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Jacobian w.r.t. the robot end effector
+% Load symbolics
+symname = 'jacobn';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    CGen.logmsg([datestr(now),'\tGenerating jacobian C-code with respect to the robot end-effector frame']);
+    tmpStruct = load(fname);
+else
+    error ('genMFunJacobian:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funname = [CGen.getrobfname,'_',symname];
+funfilename = [funname,'.c'];
+hfilename = [funname,'.h'];
+
+% Convert symbolic expression into C-code
+[funstr hstring] = ccodefunctionstring(tmpStruct.(symname),...
+    'funname',funname,...
+    'vars',{Q},'output','Jn');
+
+% Create the function description header
+hStruct = createHeaderStructJacobn(CGen.rob,funname); % create header
+hStruct.calls = hstring;
+hFString = CGen.constructheaderstringc(hStruct);
+
+%% Generate C implementation file
+fid = fopen(fullfile(srcDir,funfilename),'w+');
+
+% Function description header
+fprintf(fid,'%s\n\n',hFString);
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    ['#include "', hfilename,'"']);
+
+% Function
+fprintf(fid,'%s\n\n',funstr);
+fclose(fid);
+
+%% Generate C header file
+fid = fopen(fullfile(hdrDir,hfilename),'w+');
+
+% Include guard
+fprintf(fid,'%s\n%s\n\n',...
+    ['#ifndef ', upper([funname,'_h'])],...
+    ['#define ', upper([funname,'_h'])]);
+
+% Includes
+fprintf(fid,'%s\n\n',...
+    '#include "math.h"');
+
+% Function signature
+fprintf(fid,'%s\n\n',hstring);
+
+% Include guard
+fprintf(fid,'%s\n',...
+    ['#endif /*', upper([funname,'_h */'])]);
+
+fclose(fid);
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the function description header contents for each generated file
+function hStruct = createHeaderStructJacob0(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['C code for the Jacobian with respect to the base coordinate frame of the ',rob.name,' arm.'];
+hStruct.detailedDescription = {['Given a full set of joint variables the function'],...
+    'computes the robot jacobian with respect to the base frame. Angles have to be given in radians!'};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['J0:  [6x',num2str(rob.n),'] Jacobian matrix']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'fkine,jacobn'};
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructJacobn(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['C code for the Jacobian with respect to the end-effector coordinate frame of the ',rob.name,' arm.'];
+hStruct.detailedDescription = {['Given a full set of joint variables the function'],...
+    'computes the robot jacobian with respect to the end-effector frame. Angles have to be given in radians!'};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['Jn:  [6x',num2str(rob.n),'] Jacobian matrix']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'fkine,jacob0'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/gencoriolis.m

@@ -0,0 +1,82 @@
+%CODEGENERATOR.GENCORIOLIS Generate code for Coriolis force
+%
+% coriolis = cGen.gencoriolis() is a symbolic matrix (NxN) of centrifugal and Coriolis
+% forces/torques.
+%
+% Notes::
+% - The Coriolis matrix is stored row by row to avoid memory issues.
+%   The generated code recombines these rows to output the full matrix.
+% - Side effects of execution depends on the cGen flags:
+%   - saveresult: the symbolic expressions are saved to
+%     disk in the directory specified by cGen.sympath
+%   - genmfun: ready to use m-functions are generated and
+%     provided via a subclass of SerialLink stored in cGen.robjpath
+%   - genslblock: a Simulink block is generated and stored in a
+%     robot specific block library cGen.slib in the directory
+%     cGen.basepath
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninertia, CodeGenerator.genfkine.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [ coriolis ] = gencoriolis( CGen )
+
+%% Derivation of symbolic expressions
+CGen.logmsg([datestr(now),'\tDeriving robot Coriolis matrix']);
+
+nJoints = CGen.rob.n;
+[q, qd] = CGen.rob.gencoords;
+coriolis = CGen.rob.coriolis(q,qd);
+
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Save symbolic expressions
+if CGen.saveresult
+    CGen.logmsg([datestr(now),'\tSaving rows of the Coriolis matrix']);
+    for kJoints = 1:nJoints
+        CGen.logmsg(' %i ',kJoints);           
+        coriolisRow = coriolis(kJoints,:);
+        symName = ['coriolis_row_',num2str(kJoints)];
+        CGen.savesym(coriolisRow,symName,[symName,'.mat']);
+    end
+    CGen.logmsg('\t%s\n',' done!');   
+end
+
+% M-Functions
+if CGen.genmfun
+    CGen.genmfuncoriolis;
+end
+
+% Embedded Matlab Function Simulink blocks
+if CGen.genslblock
+    CGen.genslblockcoriolis;
+end
+
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genfdyn.m

@@ -0,0 +1,143 @@
+%CODEGENERATOR.GENFDYN Generate code for forward dynamics
+%
+% Iqdd = cGen.genfdyn() is a symbolic vector (1xN) of joint inertial 
+% reaction forces/torques.
+%
+% Notes::
+% - Side effects of execution depends on the cGen flags:
+%   - saveresult: the symbolic expressions are saved to
+%     disk in the directory specified by cGen.sympath
+%   - genmfun: ready to use m-functions are generated and
+%     provided via a subclass of SerialLink stored in cGen.robjpath
+%   - genslblock: a Simulink block is generated and stored in a
+%     robot specific block library cGen.slib in the directory
+%     cGen.basepath
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninertia, CodeGenerator.genfkine.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [Iqdd] = genfdyn(CGen)
+[q,qd] = CGen.rob.gencoords;
+tau = CGen.rob.genforces;
+nJoints = CGen.rob.n;
+
+CGen.logmsg([datestr(now),'\tLoading required symbolic expressions\n']);
+
+%% Inertia matrix
+CGen.logmsg([datestr(now),'\tLoading inertia matrix row by row']);
+
+I = sym(zeros(nJoints));
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['inertia_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if ~exist(fname,'file')
+        CGen.logmsg(['\n',datestr(now),'\t Symbolics not found, generating...\n']);
+        CGen.geninertia;
+    end
+    tmpstruct = load(fname);
+    I(kJoints,:)=tmpstruct.(symname);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+%% Matrix of centrifugal and Coriolis forces/torques matrix
+CGen.logmsg([datestr(now),'\t\tCoriolis matrix by row']);
+
+C = sym(zeros(nJoints));
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['coriolis_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if ~exist(fname,'file')
+        CGen.logmsg(['\n',datestr(now),'\t Symbolics not found, generating...\n']);
+        CGen.gencoriolis;
+    end
+    tmpstruct = load(fname);
+    C(kJoints,:)=tmpstruct.(symname);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+%% Vector of gravitational load
+CGen.logmsg([datestr(now),'\t\tvector of gravitational forces/torques']);
+symname = 'gravload';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if ~exist(fname,'file')
+    CGen.logmsg(['\n',datestr(now),'\t Symbolics not found, generating...\n']);
+    CGen.gengravload;
+end
+tmpstruct = load(fname);
+G = tmpstruct.(symname);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Joint friction
+CGen.logmsg([datestr(now),'\t\tjoint friction vector']);
+symname = 'friction';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if ~exist(fname,'file')
+    CGen.logmsg(['\n',datestr(now),'\t Symbolics not found, generating...\n']);
+    CGen.genfriction;
+end
+tmpstruct = load(fname);
+F = tmpstruct.(symname);
+
+CGen.logmsg('\t%s\n',' done!');
+
+% Full inverse dynamics
+CGen.logmsg([datestr(now),'\tGenerating symbolic inertial reaction forces/torques expression\n']);
+Iqdd = tau.'-C*qd.' -G.' +F.';
+Iqdd = Iqdd.';
+
+%% Save symbolic expressions
+if CGen.saveresult
+    CGen.logmsg([datestr(now),'\tSaving symbolic inertial reaction forces/torques expression']);
+    
+    CGen.savesym(Iqdd,'Iqdd','Iqdd.mat')
+    
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+%% M-Functions
+if CGen.genmfun
+    CGen.genmfunfdyn;
+end
+
+%% Embedded Matlab Function Simulink blocks
+if CGen.genslblock
+    CGen.genslblockfdyn;
+end
+
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genfkine.m

@@ -0,0 +1,91 @@
+%CODEGENERATOR.GENFKINE Generate code for forward kinematics
+%
+% T = cGen.genfkine() generates a symbolic homogeneous transform matrix (4x4) representing
+% the pose of the robot end-effector in terms of the symbolic joint coordinates q1, q2, ...
+%
+% [T, ALLT] = cGen.genfkine() as above but also generates symbolic homogeneous transform 
+% matrices (4x4xN) for the poses of the individual robot joints.
+%
+% Notes::
+% - Side effects of execution depends on the cGen flags:
+%   - saveresult: the symbolic expressions are saved to
+%     disk in the directory specified by cGen.sympath
+%   - genmfun: ready to use m-functions are generated and
+%     provided via a subclass of SerialLink stored in cGen.robjpath
+%   - genslblock: a Simulink block is generated and stored in a
+%     robot specific block library cGen.slib in the directory
+%     cGen.basepath
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninvdyn, CodeGenerator.genjacobian.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [t,allT] = genfkine(CGen)
+
+%% Derivation of symbolic expressions
+CGen.logmsg([datestr(now),'\tDeriving forward kinematics']);
+
+q = CGen.rob.gencoords;
+[t, allT] = CGen.rob.fkine(q);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Save symbolic expressions
+if CGen.saveresult
+    CGen.logmsg([datestr(now),'\tSaving symbolic forward kinematics up to end-effector frame']);
+    
+    CGen.savesym(t,'fkine','fkine.mat')
+    
+    CGen.logmsg('\t%s\n',' done!');
+    
+    CGen.logmsg([datestr(now),'\tSaving symbolic forward kinematics for joint']);
+    
+    for iJoint = 1:CGen.rob.n
+        CGen.logmsg(' %s ',num2str(iJoint));
+        tName = ['T0_',num2str(iJoint)];
+        eval([tName,' = allT(:,:,',num2str(iJoint),');']);
+        CGen.savesym(eval(tName),tName,[tName,'.mat']);
+    end
+    
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+%% M-Functions
+if CGen.genmfun
+    CGen.genmfunfkine;
+end
+
+%% Embedded Matlab Function Simulink blocks
+if CGen.genslblock
+    
+    genslblockfkine(CGen);
+    
+end
+
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genfriction.m

@@ -0,0 +1,75 @@
+%CODEGENERATOR.GENFRICTION Generate code for joint friction
+%
+% F = cGen.genfriction() is the symbolic vector (1xN) of joint friction
+% forces.
+%
+% Notes::
+% - Side effects of execution depends on the cGen flags:
+%   - saveresult: the symbolic expressions are saved to
+%     disk in the directory specified by cGen.sympath
+%   - genmfun: ready to use m-functions are generated and
+%     provided via a subclass of SerialLink stored in cGen.robjpath
+%   - genslblock: a Simulink block is generated and stored in a
+%     robot specific block library cGen.slib in the directory
+%     cGen.basepath
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninvdyn, CodeGenerator.genfdyn.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [ F ] = genfriction( CGen )
+
+%% Derivation of symbolic expressions
+CGen.logmsg([datestr(now),'\tDeriving joint friction model']);
+
+[~,qd] = CGen.rob.gencoords;
+F = CGen.rob.friction(qd);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Save symbolic expressions
+if CGen.saveresult
+    CGen.logmsg([datestr(now),'\tSaving symbolic friction expression']);
+    
+    CGen.savesym(F,'friction','friction.mat')
+    
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+%% M-Functions
+if CGen.genmfun
+    CGen.genmfunfriction;
+end
+
+%% Embedded Matlab Function Simulink blocks
+if CGen.genslblock
+    CGen.genslblockfriction;
+end
+
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/gengravload.m

@@ -0,0 +1,75 @@
+%CODEGENERATOR.GENGRAVLOAD Generate code for gravitational load
+%
+%  G = cGen.gengravload() is a symbolic vector (1xN) of joint load 
+%  forces/torques due to gravity.
+%
+% Notes::
+% - Side effects of execution depends on the cGen flags:
+%   - saveresult: the symbolic expressions are saved to
+%     disk in the directory specified by cGen.sympath
+%   - genmfun: ready to use m-functions are generated and
+%     provided via a subclass of SerialLink stored in cGen.robjpath
+%   - genslblock: a Simulink block is generated and stored in a
+%     robot specific block library cGen.slib in the directory
+%     cGen.basepath
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator, CodeGenerator.geninvdyn, CodeGenerator.genfdyn.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [G] = gengravload(CGen)
+
+%% Derivation of symbolic expressions
+CGen.logmsg([datestr(now),'\tDeriving gravitational load vector']);
+
+q = CGen.rob.gencoords;
+G = CGen.rob.gravload(q);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Save symbolic expressions
+if CGen.saveresult
+    CGen.logmsg([datestr(now),'\tSaving symbolic expression for gravitational load']);
+    
+    CGen.savesym(G,'gravload','gravload.mat');
+    
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+% M-Functions
+if CGen.genmfun
+    CGen.genmfungravload;
+end
+
+% Embedded Matlab Function Simulink blocks
+if CGen.genslblock
+    CGen.genslblockgravload;
+end
+
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/geninertia.m

@@ -0,0 +1,80 @@
+%CODEGENERATOR.GENINERTIA Generate code for inertia matrix
+%
+% I = cGen.geninertia() is the symbolic robot inertia matrix (NxN).
+%
+% Notes::
+% - The inertia matrix is stored row by row to avoid memory issues.
+%   The generated code recombines these rows to output the full matrix.
+% - Side effects of execution depends on the cGen flags:
+%   - saveresult: the symbolic expressions are saved to
+%     disk in the directory specified by cGen.sympath
+%   - genmfun: ready to use m-functions are generated and
+%     provided via a subclass of SerialLink stored in cGen.robjpath
+%   - genslblock: a Simulink block is generated and stored in a
+%     robot specific block library cGen.slib in the directory
+%     cGen.basepath
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninvdyn, CodeGenerator.genfdyn.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [inertia] = geninertia(CGen)
+
+%% Derivation of symbolic expressions
+CGen.logmsg([datestr(now),'\tDeriving robot inertia matrix']);
+
+nJoints = CGen.rob.n;
+q = CGen.rob.gencoords;
+inertia = CGen.rob.inertia(q);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Save symbolic expressions
+if CGen.saveresult
+    CGen.logmsg([datestr(now),'\tSaving rows of the inertia matrix']);
+    for kJoints = 1:nJoints
+        CGen.logmsg(' %i ',kJoints);           
+        inertiaRow = inertia(kJoints,:);
+        symName = ['inertia_row_',num2str(kJoints)];
+        CGen.savesym(inertiaRow,symName,[symName,'.mat']);
+    end
+    CGen.logmsg('\t%s\n',' done!');   
+end
+
+% M-Functions
+if CGen.genmfun
+    CGen.genmfuninertia;
+end
+
+% Embedded Matlab Function Simulink blocks
+if CGen.genslblock
+    CGen.genslblockinertia;
+end
+
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/geninvdyn.m

@@ -0,0 +1,142 @@
+%CODEGENERATOR.GENINVDYN Generate code for inverse dynamics
+%
+% TAU = cGen.geninvdyn() is the symbolic vector (1xN) of joint forces/torques.
+%
+% Notes::
+% - The inverse dynamics vector is composed of the previously computed inertia matrix
+%   coriolis matrix, vector of gravitational load and joint friction for speedup.
+%   The generated code recombines these components to output the final vector.
+% - Side effects of execution depends on the cGen flags:
+%   - saveresult: the symbolic expressions are saved to
+%     disk in the directory specified by cGen.sympath
+%   - genmfun: ready to use m-functions are generated and
+%     provided via a subclass of SerialLink stored in cGen.robjpath
+%   - genslblock: a Simulink block is generated and stored in a
+%     robot specific block library cGen.slib in the directory
+%     cGen.basepath
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genfdyn, CodeGenerator.genfkine.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [ tau ] = geninvdyn( CGen )
+
+[q,qd,qdd] = CGen.rob.gencoords;
+nJoints = CGen.rob.n;
+
+%% Inertia matrix
+CGen.logmsg([datestr(now),'\tLoading inertia matrix row by row']);
+
+I = sym(zeros(nJoints));
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['inertia_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if ~exist(fname,'file')
+        CGen.logmsg(['\n',datestr(now),'\t Symbolics not found, generating...\n']);
+        CGen.geninertia;
+    end
+    tmpstruct = load(fname);
+    I(kJoints,:)=tmpstruct.(symname);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+%% Matrix of centrifugal and Coriolis forces/torques matrix
+CGen.logmsg([datestr(now),'\tLoading Coriolis matrix row by row']);
+
+C = sym(zeros(nJoints));
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['coriolis_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if ~exist(fname,'file')
+        CGen.logmsg(['\n',datestr(now),'\t Symbolics not found, generating...\n']);
+        CGen.gencoriolis;
+    end
+    tmpstruct = load(fname);
+    C(kJoints,:)=tmpstruct.(symname);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+%% Vector of gravitational load
+CGen.logmsg([datestr(now),'\tLoading vector of gravitational forces/torques']);
+symname = 'gravload';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if ~exist(fname,'file')
+    CGen.logmsg(['\n',datestr(now),'\t Symbolics not found, generating...\n']);
+    CGen.gengravload;
+end
+tmpstruct = load(fname);
+G = tmpstruct.(symname);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Joint friction
+CGen.logmsg([datestr(now),'\tLoading joint friction vector']);
+symname = 'friction';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if ~exist(fname,'file')
+    CGen.logmsg(['\n',datestr(now),'\t Symbolics not found, generating...\n']);
+    CGen.genfriction;
+end
+tmpstruct = load(fname);
+F = tmpstruct.(symname);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Full inverse dynamics
+tau = I*qdd.'+C*qd.'+ G.' - F.';
+
+
+%% Save symbolic expressions
+if CGen.saveresult
+    CGen.logmsg([datestr(now),'\tSaving symbolic inverse dynamics']);
+    
+    CGen.savesym(tau,'invdyn','invdyn.mat')
+    
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+%% M-Functions
+if CGen.genmfun
+    CGen.genmfuninvdyn;
+end
+
+%% Embedded Matlab Function Simulink blocks
+if CGen.genslblock
+    CGen.genslblockinvdyn;
+end
+
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genjacobian.m

@@ -0,0 +1,81 @@
+%CODEGENERATOR.GENJACOBIAN Generate code for robot Jacobians
+%
+% J0 = cGen.genjacobian() is the symbolic expression for the Jacobian 
+% matrix (6xN) expressed in the base coordinate frame.
+%
+% [J0, Jn] = cGen.genjacobian() as above but also returns the symbolic 
+% expression for the Jacobian matrix (6xN) expressed in the end-effector 
+% frame.
+%
+% Notes::
+% - Side effects of execution depends on the cGen flags:
+%   - saveresult: the symbolic expressions are saved to
+%     disk in the directory specified by cGen.sympath
+%   - genmfun: ready to use m-functions are generated and
+%     provided via a subclass of SerialLink stored in cGen.robjpath
+%   - genslblock: a Simulink block is generated and stored in a
+%     robot specific block library cGen.slib in the directory
+%     cGen.basepath
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genfkine.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [J0, Jn] = genjacobian(CGen)
+
+%% Derivation of symbolic expressions
+CGen.logmsg([datestr(now),'\tDeriving robot jacobians']);
+
+q = CGen.rob.gencoords;
+J0 = CGen.rob.jacob0(q);
+Jn = CGen.rob.jacobn(q);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Save symbolic expressions
+if CGen.saveresult
+    CGen.logmsg([datestr(now),'\tSaving symbolic robot jacobians']);
+    
+    CGen.savesym(J0,'jacob0','jacob0.mat');
+    CGen.savesym(Jn,'jacobn','jacobn.mat');
+    
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+% M-Functions
+if CGen.genmfun
+    CGen.genmfunjacobian;
+end
+
+% Embedded Matlab Function Simulink blocks
+if CGen.genslblock
+    CGen.genslblockjacobian;
+end
+
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexcoriolis.m

@@ -0,0 +1,138 @@
+%CODEGENERATION.GENMEXCORIOLIS Generate C-MEX-function for robot coriolis matrix
+%
+% cGen.genmexcoriolis() generates robot-specific MEX-functions to compute
+% robot coriolis matrix.
+%
+% Notes::
+% - Is called by CodeGenerator.gencoriolis if cGen has active flag genmex
+% - The MEX file uses the .c and .h files generated in the directory 
+%   specified by the ccodepath property of the CodeGenerator object.
+% - Access to generated functions is provided via subclass of SerialLink
+%   whose class definition is stored in cGen.robjpath.
+% - You will need a C compiler to use the generated MEX-functions. See the 
+%   MATLAB documentation on how to setup the compiler in MATLAB. 
+%   Nevertheless the basic C-MEX-code as such may be generated without a
+%   compiler. In this case switch the cGen flag compilemex to false.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gencoriolis.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully
+% acknowledge the financial support.
+
+function [] = genmexcoriolis(CGen)
+
+%% Individual coriolis matrix rows
+CGen.logmsg([datestr(now),'\tGenerating MEX-function for the robot coriolis matrix row' ]);
+
+[Q, QD] = CGen.rob.gencoords;
+nJoints = CGen.rob.n;
+
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['coriolis_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if exist(fname,'file')
+        tmpStruct = load(fname);
+    else
+        error ('genmfuncoriolis:SymbolicsNotFound','Save symbolic expressions to disk first!')
+    end
+    
+    funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+    
+    % Function description header
+    hStruct = createHeaderStructRow(CGen.rob,kJoints,symname);   %generate header
+    
+    % Generate and compile MEX function
+    CGen.mexfunction(tmpStruct.(symname), ...
+        'funfilename',funfilename,...
+        'funname',[CGen.getrobfname,'_',symname],...
+        'vars',{Q, QD},...
+        'output',['C_row',num2str(kJoints)],...
+        'header',hStruct);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+%% Full coriolis matrix
+CGen.logmsg([datestr(now),'\tGenerating full coriolis matrix m-function']);
+
+symname = 'coriolis';
+f = sym(zeros(nJoints)); % dummy symbolic expression
+funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+
+funname = [CGen.getrobfname,'_',symname];
+
+hStruct = createHeaderStructFull(CGen.rob,symname); % create header
+hFString = CGen.constructheaderstringc(hStruct);
+
+% Generate and compile MEX function 
+CGen.mexfunctionrowwise(f,...
+    'funfilename',funfilename,...
+    'funname',[CGen.getrobfname,'_',symname],...
+    'vars',{Q, QD},...
+    'output','C',...
+    'header',hStruct);
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructRow(rob,curJointIdx,fName)
+[~,hStruct.funName] = fileparts(fName);
+hStruct.calls = '';
+hStruct.shortDescription = ['Computation of the robot specific Coriolis matrix row for corresponding to joint ', num2str(curJointIdx), ' of ',num2str(rob.n),'.'];
+hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
+    ['Coriolis matrix row number ', num2str(curJointIdx),' of ',num2str(rob.n),' for ',rob.name,'. Angles have to be given in radians!']};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates'],...
+                   ['input2: ',int2str(rob.n),'-element vector of generalized velocities']};
+hStruct.outputs = {['C_row_',int2str(curJointIdx),':  [1x',int2str(rob.n),'] row of the robot Coriolis matrix']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'coriolis'};
+end
+ 
+function hStruct = createHeaderStructFull(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['Coriolis matrix for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {'Given a full set of joint variables the function computes the',...
+    'Coriolis matrix of the robot. Angles have to be given in radians!'};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates'],...
+                   ['input2: ',int2str(rob.n),'-element vector of generalized velocities']};
+hStruct.outputs = {['C:  [',int2str(rob.n),'x',int2str(rob.n),'] Coriolis matrix']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'coriolis'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfdyn.m

@@ -0,0 +1,126 @@
+%CODEGENERATOR.GENMEXFDYN Generate C-MEX-function for forward dynamics
+%
+% cGen.genmexfdyn() generates a robot-specific MEX-function to compute
+% the forward dynamics.
+% 
+% Notes::
+% - Is called by CodeGenerator.genfdyn if cGen has active flag genmex
+% - The MEX file uses the .c and .h files generated in the directory 
+%   specified by the ccodepath property of the CodeGenerator object.
+% - Access to generated function is provided via subclass of SerialLink
+%   whose class definition is stored in cGen.robjpath.
+% - You will need a C compiler to use the generated MEX-functions. See the 
+%   MATLAB documentation on how to setup the compiler in MATLAB. 
+%   Nevertheless the basic C-MEX-code as such may be generated without a
+%   compiler. In this case switch the cGen flag compilemex to false.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genfdyn, CodeGenerator.genmexinvdyn.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genmexfdyn(CGen)
+
+CGen.logmsg([datestr(now),'\tGenerating inverse dynamics MEX-function: ']);
+
+mexfunname = 'accel';
+mexcfilename = fullfile(CGen.robjpath,[mexfunname,'.c']);
+
+cfunname = [CGen.getrobfname,'_',mexfunname];
+cfilename = [cfunname '.c'];
+hfilename = [cfunname '.h'];
+
+
+
+[Q, QD] = CGen.rob.gencoords;
+[tau] = CGen.rob.genforces;
+
+% Function description header
+hStruct = createHeaderStruct(CGen.rob,mexfunname); 
+hFString = CGen.constructheaderstringc(hStruct);
+
+fid = fopen(mexcfilename,'w+');
+
+% Insert description header
+fprintf(fid,'%s\n',hFString);
+
+% Includes
+fprintf(fid,'%s\n%s\n\n',...
+    '#include "mex.h"',...
+    ['#include "',hfilename,'"']);
+
+dummy = sym(zeros(CGen.rob.n,1));
+% Generate the mex gateway routine
+funstr = CGen.genmexgatewaystring(dummy,'funname',cfunname, 'vars',{Q, QD, tau});
+fprintf(fid,'%s',sprintf(funstr));
+
+fclose(fid);
+
+%% Compile the MEX file
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+
+cfilelist = fullfile(srcDir,cfilename);
+for kJoints = 1:CGen.rob.n
+    cfilelist = [cfilelist, ' ',fullfile(srcDir,[CGen.getrobfname,'_inertia_row_',num2str(kJoints),'.c'])];
+end
+for kJoints = 1:CGen.rob.n
+    cfilelist = [cfilelist, ' ',fullfile(srcDir,[CGen.getrobfname,'_coriolis_row_',num2str(kJoints),'.c'])];
+end
+
+cfilelist = [cfilelist, ' ', fullfile(srcDir,[CGen.getrobfname,'_inertia.c'])];
+cfilelist = [cfilelist, ' ', fullfile(srcDir,[CGen.getrobfname,'_coriolis.c'])];
+cfilelist = [cfilelist, ' ', fullfile(srcDir,[CGen.getrobfname,'_gravload.c'])];
+cfilelist = [cfilelist, ' ', fullfile(srcDir,[CGen.getrobfname,'_friction.c'])];
+cfilelist = [cfilelist, ' ', fullfile(srcDir,'matvecprod.c')];
+cfilelist = [cfilelist, ' ', fullfile(srcDir,'gaussjordan.c')];
+
+if CGen.verbose
+    eval(['mex ',mexcfilename, ' ',cfilelist,' -I',hdrDir, ' -v -outdir ',CGen.robjpath]);   
+else
+    eval(['mex ',mexcfilename, ' ',cfilelist,' -I',hdrDir,' -outdir ',CGen.robjpath]);
+end
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStruct(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['C-implementation of the forward dynamics for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {'Given a full set of joint angles and velocities',...
+    'this function computes the joint space accelerations effected by the generalized forces. Angles have to be given in radians!'};
+hStruct.inputs = { ['input1:  ',int2str(rob.n),'-element vector of generalized coordinates'],...
+    ['input2:  ',int2str(rob.n),'-element vector of generalized velocities'],...
+    ['input3:  [',int2str(rob.n),'x1] vector of generalized forces.']};
+hStruct.outputs = {['QDD:  ',int2str(rob.n),'-element output vector of generalized accelerations.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'invdyn'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfkine.m

@@ -0,0 +1,125 @@
+%CODEGENERATOR.GENMEXFKINE Generate C-MEX-function for forward kinematics
+%
+% cGen.genmexfkine() generates a robot-specific MEX-function to compute
+% forward kinematics.
+%
+% Notes::
+% - Is called by CodeGenerator.genfkine if cGen has active flag genmex
+% - The MEX file uses the .c and .h files generated in the directory 
+%   specified by the ccodepath property of the CodeGenerator object.
+% - Access to generated function is provided via subclass of SerialLink
+%   whose class definition is stored in cGen.robjpath.
+% - You will need a C compiler to use the generated MEX-functions. See the 
+%   MATLAB documentation on how to setup the compiler in MATLAB. 
+%   Nevertheless the basic C-MEX-code as such may be generated without a
+%   compiler. In this case switch the cGen flag compilemex to false.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genfkine.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genmexfkine(CGen)
+
+%% Forward kinematics up to tool center point
+CGen.logmsg([datestr(now),'\tGenerating forward kinematics MEX-function up to the end-effector frame: ']);
+symname = 'fkine';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfunfkine:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+Q = CGen.rob.gencoords;
+
+% Function description header
+hStruct = createHeaderStructFkine(CGen.rob,symname); 
+
+% Generate and compile MEX function 
+CGen.mexfunction(tmpStruct.(symname), 'funfilename',funfilename,'funname',[CGen.getrobfname,'_',symname],'vars',{Q},'output','T','header',hStruct)
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Individual joint forward kinematics
+CGen.logmsg([datestr(now),'\tGenerating forward kinematics MEX-function up to joint: ']);
+for iJoints=1:CGen.rob.n
+    
+    CGen.logmsg(' %i ',iJoints);
+    symname = ['T0_',num2str(iJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    tmpStruct = load(fname);
+    
+    funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+    Q = CGen.rob.gencoords;
+    
+    % Function description header
+    hStruct = createHeaderStruct(CGen.rob,iJoints,symname); % create header
+    
+    % Generate and compile MEX function 
+    CGen.mexfunction(tmpStruct.(symname),'funfilename',funfilename,'funname',[CGen.getrobfname,'_',symname],'vars',{Q},'output','T','header',hStruct);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStruct(rob,curBody,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['C version of the forward kinematics for the ',rob.name,' arm up to frame ',int2str(curBody),' of ',int2str(rob.n),'.'];
+hStruct.detailedDescription = {['Given a set of joint variables up to joint number ',int2str(curBody),' the function'],...
+    'computes the pose belonging to that joint with respect to the base frame.'};
+hStruct.inputs = { 'input1 Vector of generalized coordinates. Angles have to be given in radians!'};
+hStruct.outputs = {'T [4x4] Homogenous transformation matrix relating the pose of the tool for the given joint values to the base frame.'};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!<BR>',...
+    'Code generator written by:<BR>',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)<BR>'};
+hStruct.seeAlso = {rob.name};
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructFkine(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['C version of the forward kinematics solution including tool transformation for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {['Given a full set of joint variables the function'],...
+    'computes the pose belonging to that joint with respect to the base frame.'};
+hStruct.inputs = { 'input1 Vector of generalized coordinates. Angles have to be given in radians!'};
+hStruct.outputs = {'T [4x4] Homogenous transformation matrix relating the pose of the tool for the given joint values to the base frame.'};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!<BR>',...
+    'Code generator written by:<BR>',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)<BR>'};
+hStruct.seeAlso = {'jacob0'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfriction.m

@@ -0,0 +1,90 @@
+%CODEGENERATOR.GENMEXFRICTION Generate C-MEX-function for joint friction
+%
+% cGen.genmexfriction() generates a robot-specific MEX-function to compute
+% the vector of joint friction.
+%
+% Notes::
+% - Is called by CodeGenerator.genfriction if cGen has active flag genmex
+% - The MEX file uses the .c and .h files generated in the directory 
+%   specified by the ccodepath property of the CodeGenerator object.
+% - Access to generated function is provided via subclass of SerialLink
+%   whose class definition is stored in cGen.robjpath.
+% - You will need a C compiler to use the generated MEX-functions. See the 
+%   MATLAB documentation on how to setup the compiler in MATLAB. 
+%   Nevertheless the basic C-MEX-code as such may be generated without a
+%   compiler. In this case switch the cGen flag compilemex to false.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gengravload.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genmexfriction(CGen)
+
+%% Forward kinematics up to tool center point
+CGen.logmsg([datestr(now),'\tGenerating friction vector MEX-function: ']);
+symname = 'friction';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfungravload:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+[QD] = CGen.rob.gencoords;
+
+% Function description header
+hStruct = createHeaderStruct(CGen.rob,symname); 
+
+% Generate and compile MEX function 
+CGen.mexfunction(tmpStruct.(symname),...
+    'funfilename',funfilename,...
+    'funname',[CGen.getrobfname,'_',symname],...
+    'vars',{QD},...
+    'output','F',...
+    'header',hStruct);
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the function description header contents
+function hStruct = createHeaderStruct(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['Joint friction for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {['Given a full set of generalized joint velocities the function'],...
+    'computes the friction forces/torques. Angles have to be given in radians!'};
+hStruct.inputs = { ['input1:  ',int2str(rob.n),'-element vector of generalized velocities.']};
+hStruct.outputs = {['F:  [',int2str(rob.n),'x1] output vector of joint friction forces/torques.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'gravload'};
+end
+

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexgravload.m

@@ -0,0 +1,85 @@
+%CODEGENERATOR.GENMEXGRAVLOAD Generate C-MEX-function for gravitational load
+%
+% cGen.genmexgravload() generates a robot-specific MEX-function to compute
+% gravitation load forces and torques.
+% 
+% Notes::
+% - Is called by CodeGenerator.gengravload if cGen has active flag genmex
+% - The MEX file uses the .c and .h files generated in the directory 
+%   specified by the ccodepath property of the CodeGenerator object.
+% - Access to generated function is provided via subclass of SerialLink
+%   whose class definition is stored in cGen.robjpath.
+% - You will need a C compiler to use the generated MEX-functions. See the 
+%   MATLAB documentation on how to setup the compiler in MATLAB. 
+%   Nevertheless the basic C-MEX-code as such may be generated without a
+%   compiler. In this case switch the cGen flag compilemex to false.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gengravload.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genmexgravload(CGen)
+
+%% Forward kinematics up to tool center point
+CGen.logmsg([datestr(now),'\tGenerating gravload MEX-function: ']);
+symname = 'gravload';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmexgravload:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+Q = CGen.rob.gencoords;
+
+% Function description header
+hStruct = createHeaderStructGravity(CGen.rob,symname); 
+
+% Generate and compile MEX function 
+CGen.mexfunction(tmpStruct.(symname), 'funfilename',funfilename,'funname',[CGen.getrobfname,'_',symname],'vars',{Q},'output','G','header',hStruct)
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the function description header contents
+function hStruct = createHeaderStructGravity(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['Computation of the configuration dependent vector of gravitational load forces/torques for ',rob.name];
+hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
+                               'configuration dependent vector of gravitational load forces/torques. Angles have to be given in radians!'};
+hStruct.inputs = { ['input1: ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['G: [',int2str(rob.n),'x1] output vector of gravitational load forces/torques.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'inertia'};
+end
+

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexinertia.m

@@ -0,0 +1,136 @@
+%CODEGENERATION.GENMEXINERTIA Generate C-MEX-function for robot inertia matrix
+%
+% cGen.genmexinertia() generates robot-specific MEX-functions to compute
+% robot inertia matrix.
+%
+% Notes::
+% - Is called by CodeGenerator.geninertia if cGen has active flag genmex
+% - The MEX file uses the .c and .h files generated in the directory 
+%   specified by the ccodepath property of the CodeGenerator object.
+% - Access to generated functions is provided via subclass of SerialLink
+%   whose class definition is stored in cGen.robjpath.
+% - You will need a C compiler to use the generated MEX-functions. See the 
+%   MATLAB documentation on how to setup the compiler in MATLAB. 
+%   Nevertheless the basic C-MEX-code as such may be generated without a
+%   compiler. In this case switch the cGen flag compilemex to false.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninertia.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully
+% acknowledge the financial support.
+
+function [] = genmexinertia(CGen)
+
+%% Individual inertia matrix rows
+CGen.logmsg([datestr(now),'\tGenerating MEX-function for the robot inertia matrix row' ]);
+
+Q = CGen.rob.gencoords;
+nJoints = CGen.rob.n;
+
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['inertia_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if exist(fname,'file')
+        tmpStruct = load(fname);
+    else
+        error ('genmfuninertia:SymbolicsNotFound','Save symbolic expressions to disk first!')
+    end
+    
+    funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+    
+    % Function description header
+    hStruct = createHeaderStructRow(CGen.rob,kJoints,symname);   %generate header
+    
+    % Generate and compile MEX function
+    CGen.mexfunction(tmpStruct.(symname), ...
+        'funfilename',funfilename,...
+        'funname',[CGen.getrobfname,'_',symname],...
+        'vars',{Q},...
+        'output',['I_row',num2str(kJoints)],...
+        'header',hStruct);
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+%% Full inertia matrix
+CGen.logmsg([datestr(now),'\tGenerating full inertia matrix m-function']);
+
+symname = 'inertia';
+f = sym(zeros(nJoints)); % dummy symbolic expression
+funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+
+funname = [CGen.getrobfname,'_',symname];
+
+hStruct = createHeaderStructFull(CGen.rob,symname); % create header
+hFString = CGen.constructheaderstringc(hStruct);
+
+% Generate and compile MEX function 
+CGen.mexfunctionrowwise(f,...
+    'funfilename',funfilename,...
+    'funname',[CGen.getrobfname,'_',symname],...
+    'vars',{Q},...
+    'output','I',...
+    'header',hStruct);
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructRow(rob,curJointIdx,fName)
+[~,hStruct.funName] = fileparts(fName);
+hStruct.calls = '';
+hStruct.shortDescription = ['Computation of the robot specific inertia matrix row for corresponding to joint ', num2str(curJointIdx), ' of ',num2str(rob.n),'.'];
+hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
+    ['inertia matrix row number ', num2str(curJointIdx),' of ',num2str(rob.n),' for ',rob.name,'. Angles have to be given in radians!']};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['I_row_',int2str(curJointIdx),':  [1x',int2str(rob.n),'] output row of the robot inertia matrix.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'coriolis'};
+end
+
+function hStruct = createHeaderStructFull(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['Inertia matrix for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {'Given a full set of joint variables the function computes the',...
+    'inertia Matrix of the robot. Angles have to be given in radians!'};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['I:  [',int2str(rob.n),'x',int2str(rob.n),']output inertia matrix.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'coriolis'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexinvdyn.m

@@ -0,0 +1,125 @@
+%CODEGENERATOR.GENMEXINVDYN Generate C-MEX-function for inverse dynamics
+%
+% cGen.genmexinvdyn() generates a robot-specific MEX-function to compute
+% the inverse dynamics.
+% 
+% Notes::
+% - Is called by CodeGenerator.geninvdyn if cGen has active flag genmex.
+% - The MEX file uses the .c and .h files generated in the directory 
+%   specified by the ccodepath property of the CodeGenerator object.
+% - Access to generated function is provided via subclass of SerialLink
+%   whose class definition is stored in cGen.robjpath.
+% - You will need a C compiler to use the generated MEX-functions. See the 
+%   MATLAB documentation on how to setup the compiler in MATLAB. 
+%   Nevertheless the basic C-MEX-code as such may be generated without a
+%   compiler. In this case switch the cGen flag compilemex to false.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gengravload.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genmexinvdyn(CGen)
+
+CGen.logmsg([datestr(now),'\tGenerating inverse dynamics MEX-function: ']);
+
+mexfunname = 'invdyn';
+mexcfilename = fullfile(CGen.robjpath,[mexfunname,'.c']);
+
+cfunname = [CGen.getrobfname,'_',mexfunname];
+cfilename = [cfunname '.c'];
+hfilename = [cfunname '.h'];
+
+
+
+[Q, QD, QDD] = CGen.rob.gencoords;
+
+% Function description header
+hStruct = createHeaderStruct(CGen.rob,mexfunname); 
+hFString = CGen.constructheaderstringc(hStruct);
+
+fid = fopen(mexcfilename,'w+');
+
+% Insert description header
+fprintf(fid,'%s\n',hFString);
+
+% Includes
+fprintf(fid,'%s\n%s\n\n',...
+    '#include "mex.h"',...
+    ['#include "',hfilename,'"']);
+
+dummy = sym(zeros(CGen.rob.n,1));
+% Generate the mex gateway routine
+funstr = CGen.genmexgatewaystring(dummy,'funname',cfunname, 'vars',{Q, QD, QDD});
+fprintf(fid,'%s',sprintf(funstr));
+
+fclose(fid);
+
+%% Compile the MEX file
+srcDir = fullfile(CGen.ccodepath,'src');
+hdrDir = fullfile(CGen.ccodepath,'include');
+
+cfilelist = fullfile(srcDir,cfilename);
+for kJoints = 1:CGen.rob.n
+    cfilelist = [cfilelist, ' ',fullfile(srcDir,[CGen.getrobfname,'_inertia_row_',num2str(kJoints),'.c'])];
+end
+for kJoints = 1:CGen.rob.n
+    cfilelist = [cfilelist, ' ',fullfile(srcDir,[CGen.getrobfname,'_coriolis_row_',num2str(kJoints),'.c'])];
+end
+
+cfilelist = [cfilelist, ' ', fullfile(srcDir,[CGen.getrobfname,'_gravload.c'])];
+cfilelist = [cfilelist, ' ', fullfile(srcDir,[CGen.getrobfname,'_friction.c'])];
+cfilelist = [cfilelist, ' ', fullfile(srcDir,'dotprod.c')];
+
+if CGen.verbose
+    eval(['mex ',mexcfilename, ' ',cfilelist,' -I',hdrDir, ' -v -outdir ',CGen.robjpath]);   
+else
+    eval(['mex ',mexcfilename, ' ',cfilelist,' -I',hdrDir,' -outdir ',CGen.robjpath]);
+end
+
+
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStruct(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['C-implementation of the inverse dynamics for the ',rob.name,' arm.'];
+hStruct.detailedDescription = {'Given a full set of joint variables and their first and second order',...
+    'temporal derivatives this function computes the joint space',...
+    'torques needed to perform the particular motion. Angles have to be given in radians!'};
+hStruct.inputs = { ['input1:  ',int2str(rob.n),'-element vector of generalized coordinates'],...
+                   ['input2:  ',int2str(rob.n),'-element vector of generalized velocities,'],...
+                   ['input3:  ',int2str(rob.n),'-element vector of generalized accelerations.']};
+hStruct.outputs = {['TAU:  [',int2str(rob.n),'x1] vector of joint forces/torques.']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'fdyn'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexjacobian.m

@@ -0,0 +1,126 @@
+%CODEGENERATOR.GENMEXJACOBIAN Generate C-MEX-function for the robot Jacobians
+%
+% cGen.genmexjacobian() generates robot-specific MEX-function to compute
+% the robot Jacobian with respect to the base as well as the end effector
+% frame.
+%
+% Notes::
+% - Is called by CodeGenerator.genjacobian if cGen has active flag genmex.
+% - The MEX file uses the .c and .h files generated in the directory 
+%   specified by the ccodepath property of the CodeGenerator object.
+% - Access to generated function is provided via subclass of SerialLink
+%   whose class definition is stored in cGen.robjpath.
+% - You will need a C compiler to use the generated MEX-functions. See the 
+%   MATLAB documentation on how to setup the compiler in MATLAB. 
+%   Nevertheless the basic C-MEX-code as such may be generated without a
+%   compiler. In this case switch the cGen flag compilemex to false.
+%
+% Author::
+%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genjacobian.
+
+% Copyright (C) 2012-2014, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB. If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+
+function [] = genmexjacobian(CGen)
+
+%% Jacobian w.r.t. the robot base
+symname = 'jacob0';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    CGen.logmsg([datestr(now),'\tGenerating Jacobian MEX-function with respect to the robot base frame']);
+    tmpStruct = load(fname);
+else
+    error ('genmfunjacobian:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+Q = CGen.rob.gencoords;
+
+% Function description header
+hStruct = createHeaderStructJacob0(CGen.rob,symname); % create header
+
+% Generate and compile MEX function 
+CGen.mexfunction(tmpStruct.(symname),'funfilename',funfilename,'funname',[CGen.getrobfname,'_',symname],'vars',{Q},'output','J0','header',hStruct);
+
+CGen.logmsg('\t%s\n',' done!');
+
+%% Jacobian w.r.t. the robot end effector
+symname = 'jacobn';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    CGen.logmsg([datestr(now),'\tGenerating Jacobian MEX-function with respect to the robot end-effector frame']);
+    tmpStruct = load(fname);
+else
+    error ('genMFunJacobian:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.c']);
+Q = CGen.rob.gencoords;
+
+% Function description header
+hStruct = createHeaderStructJacobn(CGen.rob,symname); % create header
+
+% Generate and compile MEX function 
+CGen.mexfunction(tmpStruct.(symname),'funfilename',funfilename,'funname',[CGen.getrobfname,'_',symname],'vars',{Q},'output','Jn','header',hStruct);
+
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the function description header contents for each generated file
+function hStruct = createHeaderStructJacob0(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['C code for the Jacobian with respect to the base coordinate frame of the ',rob.name,' arm.'];
+hStruct.detailedDescription = {['Given a full set of joint variables the function'],...
+    'computes the robot jacobian with respect to the base frame. Angles have to be given in radians!'};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['J0:  [6x',num2str(rob.n),'] Jacobian matrix']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'fkine,jacobn'};
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructJacobn(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.calls = '';
+hStruct.shortDescription = ['C code for the Jacobian with respect to the end-effector coordinate frame of the ',rob.name,' arm.'];
+hStruct.detailedDescription = {['Given a full set of joint variables the function'],...
+    'computes the robot jacobian with respect to the end-effector frame. Angles have to be given in radians!'};
+hStruct.inputs = {['input1:  ',int2str(rob.n),'-element vector of generalized coordinates.']};
+hStruct.outputs = {['Jn:  [6x',num2str(rob.n),'] Jacobian matrix']};
+hStruct.references = {'Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    'Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    'Introduction to Robotics, Mechanics and Control - Craig',...
+    'Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn (joern.malzahn@tu-dortmund.de)'};
+hStruct.seeAlso = {'fkine,jacob0'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuncoriolis.m

@@ -0,0 +1,153 @@
+%CODEGENERATOR.GENMFUNCORIOLIS Generate M-functions for Coriolis matrix
+%
+% cGen.genmfuncoriolis() generates a robot-specific M-function to compute
+% the Coriolis matrix.
+%
+% Notes::
+% - Is called by CodeGenerator.gencoriolis if cGen has active flag genmfun
+% - The Coriolis matrix is stored row by row to avoid memory issues.
+% - The generated M-function recombines the individual M-functions for each row.
+% - Access to generated function is provided via subclass of SerialLink 
+%   whose class definition is stored in cGen.robjpath.
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gencoriolis, CodeGenerator.geninertia.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [ ] = genmfuncoriolis( CGen )
+
+%% Does robot class exist?
+if ~exist(fullfile(CGen.robjpath,[CGen.getrobfname,'.m']),'file')
+    CGen.logmsg([datestr(now),'\tCreating ',CGen.getrobfname,' m-constructor ']);
+    CGen.createmconstructor;
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+%%
+CGen.logmsg([datestr(now),'\tGenerating m-function for the Coriolis matrix row' ]);
+
+[q, qd] = CGen.rob.gencoords;
+nJoints = CGen.rob.n;
+
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['coriolis_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if exist(fname,'file')
+        tmpStruct = load(fname);
+    else
+        error ('genmfuncoriolis:SymbolicsNotFound','Save symbolic expressions to disk first!')
+    end
+    
+    funfilename = fullfile(CGen.robjpath,[symname,'.m']);
+    
+    matlabFunction(tmpStruct.(symname),'file',funfilename,...              % generate function m-file
+        'outputs', {'Crow'},...
+        'vars', {'rob',q,qd});
+    hStruct = createHeaderStructRow(CGen.rob,kJoints,symname);                 % replace autogenerated function header
+    replaceheader(CGen,hStruct,funfilename);
+    
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+
+CGen.logmsg([datestr(now),'\tGenerating full Coriolis matrix m-function: ']);
+    
+    funfilename = fullfile(CGen.robjpath,'coriolis.m');
+    hStruct = createHeaderStructFull(CGen.rob,funfilename);
+    
+    fid = fopen(funfilename,'w+');
+    
+    fprintf(fid, '%s\n', ['function C = coriolis(rob,q,qd)']);                 % Function definition
+    fprintf(fid, '%s\n',constructheaderstring(CGen,hStruct));                   % Header
+   
+    fprintf(fid, '%s \n', 'C = zeros(length(q));');                        % Code
+    for iJoints = 1:nJoints
+        funCall = ['C(',num2str(iJoints),',:) = ','rob.coriolis_row_',num2str(iJoints),'(q,qd);'];
+        fprintf(fid, '%s \n', funCall);
+    end
+    
+    fclose(fid);
+    
+    CGen.logmsg('\t%s\n',' done!');           
+end
+
+function hStruct = createHeaderStructRow(rob,curJointIdx,fName)
+[~,hStruct.funName] = fileparts(fName);
+hStruct.shortDescription = ['Computation of the robot specific Coriolis matrix row for joint ', num2str(curJointIdx), ' of ',num2str(rob.n),'.'];
+hStruct.calls = {['Crow = ',hStruct.funName,'(rob,q,qd)'],...
+    ['Crow = rob.',hStruct.funName,'(q,qd)']};
+hStruct.detailedDescription = {'Given a full set of joint variables and their first order temporal derivatives this function computes the',...
+                               ['Coriolis matrix row number ', num2str(curJointIdx),' of ',num2str(rob.n),' for ',rob.name,'.']};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     coordinates';
+                   ['qd:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     velocities', ...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['Crow:  [1x',int2str(rob.n),'] row of the robot Coriolis matrix']};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function.',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+    'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'coriolis'};
+end
+
+function hStruct = createHeaderStructFull(rob,fName)
+[~,hStruct.funName] = fileparts(fName);
+hStruct.shortDescription = ['Coriolis matrix for the ',rob.name,' arm'];
+hStruct.calls = {['Crow = ',hStruct.funName,'(rob,q,qd)'],...
+    ['Crow = rob.',hStruct.funName,'(q,qd)']};
+hStruct.detailedDescription = {'Given a full set of joint variables and their first order temporal derivatives the function computes the',...
+                               'Coriolis matrix of the robot.'};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     coordinates';
+                   ['qd:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     velocities', ...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['C:  [',int2str(rob.n),'x',int2str(rob.n),'] Coriolis matrix']};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+    'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'inertia'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfdyn.m

@@ -0,0 +1,178 @@
+%CODEGENERATOR.GENMFUNFDYN Generate M-function for forward dynamics
+%
+% cGen.genmfunfdyn() generates a robot-specific M-function to compute
+% the forward dynamics.
+%
+% Notes::
+% - Is called by CodeGenerator.genfdyn if cGen has active flag genmfun
+% - The generated M-function is composed of previously generated M-functions
+%   for the inertia matrix, coriolis matrix, vector of gravitational load and 
+%   joint friction vector.  This function recombines these components to compute 
+%   the forward dynamics.
+% - Access to generated function is provided via subclass of SerialLink 
+%   whose class definition is stored in cGen.robjpath.
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninvdyn.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [] = genmfunfdyn(CGen)
+
+%% Does robot class exist?
+if ~exist(fullfile(CGen.robjpath,[CGen.getrobfname,'.m']),'file')
+    CGen.logmsg([datestr(now),'\tCreating ',CGen.getrobfname,' m-constructor ']);
+    CGen.createmconstructor;
+    CGen.logmsg('\t%s\n',' done!');
+end
+checkexistanceofmfunctions(CGen);
+
+%%
+
+CGen.logmsg([datestr(now),'\tGenerating m-function for the joint inertial reaction forces/torques' ]);
+
+symname = 'Iqdd';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfunfdyn:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.m']);
+[q qd] = CGen.rob.gencoords;
+tau = CGen.rob.genforces;
+
+matlabFunction(tmpStruct.(symname),'file',funfilename,...              % generate function m-file
+    'outputs', {'Iacc'},...
+    'vars', {'rob',q,qd,tau});
+hStruct = createHeaderStructIqdd(CGen.rob,symname);                 % replace autogenerated function header
+replaceheader(CGen,hStruct,funfilename);
+CGen.logmsg('\t%s\n',' done!');
+
+%% Generate mfunction for the acceleration
+CGen.logmsg([datestr(now),'\tGenerating joint acceleration m-function:']);
+
+funfilename = fullfile(CGen.robjpath,'accel.m');
+hStruct = createHeaderStructAccel(CGen.rob,funfilename);
+
+fid = fopen(funfilename,'w+');
+
+fprintf(fid, '%s\n', ['function qdd = accel(rob,q,qd,tau)']);                 % Function definition
+fprintf(fid, '%s\n',constructheaderstring(CGen,hStruct));                   % Header
+
+fprintf(fid, '%s \n', 'qdd = zeros(length(q),1);');                        % Code
+
+funcCall = 'qdd = rob.inertia(q) \ rob.Iqdd(q,qd,tau).'';';
+fprintf(fid, '%s \n', funcCall);
+
+fclose(fid);
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+function [] = checkexistanceofmfunctions(CGen)
+
+if ~(exist(fullfile(CGen.robjpath,'inertia.m'),'file') == 2) || ~(exist(fullfile(CGen.robjpath,'Iqdd.m'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Inertia m-function not found! Generating:');
+    CGen.genmfuninertia;
+end
+
+if ~(exist(fullfile(CGen.robjpath,'coriolis.m'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Coriolis m-function not found! Generating:');
+    CGen.genmfuncoriolis;
+end
+
+if ~(exist(fullfile(CGen.robjpath,'gravload.m'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Gravload m-function not found! Generating:');
+    CGen.genmfungravload;
+end
+
+if ~(exist(fullfile(CGen.robjpath,'friction.m'),'file') == 2)
+    CGen.logmsg('\t\t%s\n','Friction m-function not found! Generating:');
+    CGen.genmfunfriction;
+end
+
+end
+function hStruct = createHeaderStructIqdd(rob,fName)
+[~,hStruct.funName] = fileparts(fName);
+hStruct.shortDescription = ['Vector of computed inertial forces/torques for ',rob.name];
+hStruct.calls = {['Iacc = ',hStruct.funName,'(rob,q,qd,tau)'],...
+    ['Iacc = rob.',hStruct.funName,'(q,qd,tau)']};
+hStruct.detailedDescription = {'Given a full set of joint variables, their temporal derivatives and applied joint forces/torques',...
+    'this function computes the reaction inertial forces/torques due to joint acceleration.'};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     coordinates',...
+                   ['qd:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     velocities', ...
+                   ['tau:  ',int2str(rob.n),'-element vector of joint'],...
+                   '     input forces/torques',...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['Iqdd:  [1x',int2str(rob.n),'] vector of inertial reaction forces/torques']};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+    'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'accel'};
+end
+
+
+function hStruct = createHeaderStructAccel(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Computation of the joint acceleration for ',rob.name];
+hStruct.calls = {['qdd = ',hStruct.funName,'(rob,q,qd,tau)'],...
+    ['qdd = rob.',hStruct.funName,'(q,qd,tau)']};
+hStruct.detailedDescription = {'Given a full set of joint variables, their temporal derivatives and applied joint forces/torques',...
+    'this function computes the joint acceleration.'};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     coordinates',...
+                   ['qd:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     velocities', ...
+                   ['tau:  ',int2str(rob.n),'-element vector of joint'],...
+                   '     input forces/torques',...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['qdd:  [1x',int2str(rob.n),'] vector of joint accelerations']};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+    'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'Iqdd'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfkine.m

@@ -0,0 +1,144 @@
+%CODEGENERATOR.GENMFUNFKINE Generate M-function for forward kinematics
+%
+% cGen.genmfunfkine() generates a robot-specific M-function to compute
+% forward kinematics.
+%
+% Notes::
+% - Is called by CodeGenerator.genfkine if cGen has active flag genmfun
+% - Access to generated function is provided via subclass of SerialLink 
+%   whose class definition is stored in cGen.robjpath.
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.genjacobian.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [] = genmfunfkine(CGen)
+
+%% Does robot class exist?
+if ~exist(fullfile(CGen.robjpath,[CGen.getrobfname,'.m']),'file')
+    CGen.logmsg([datestr(now),'\tCreating ',CGen.getrobfname,' m-constructor ']);
+    CGen.createmconstructor;
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+%% Forward kinematics up to tool center point
+CGen.logmsg([datestr(now),'\tGenerating forward kinematics m-function up to the end-effector frame: ']);
+symname = 'fkine';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfunfkine:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.m']);
+q = CGen.rob.gencoords;
+
+matlabFunction(tmpStruct.(symname),'file',funfilename,...              % generate function m-file
+    'outputs', {symname},...
+    'vars', {'rob',[q]});
+hStruct = createHeaderStructFkine(CGen.rob,symname);                 % replace autogenerated function header
+replaceheader(CGen,hStruct,funfilename);
+CGen.logmsg('\t%s\n',' done!');
+
+
+%% Individual joint forward kinematics
+CGen.logmsg([datestr(now),'\tGenerating forward kinematics m-function up to joint: ']);
+for iJoints=1:CGen.rob.n
+    
+    CGen.logmsg(' %i ',iJoints);
+    symname = ['T0_',num2str(iJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    tmpStruct = struct;
+    tmpStruct = load(fname);
+    
+    funfilename = fullfile(CGen.robjpath,[symname,'.m']);
+    q = CGen.rob.gencoords;
+    
+    matlabFunction(tmpStruct.(symname),'file',funfilename,...              % generate function m-file
+        'outputs', {symname},...
+        'vars', {'rob',[q]});
+    hStruct = createHeaderStruct(CGen.rob,iJoints,symname);                 % replace autogenerated function header
+    CGen.replaceheader(hStruct,funfilename);
+end
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStruct(rob,curBody,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Forward kinematics for the ',rob.name,' arm up to frame ',int2str(curBody),' of ',int2str(rob.n),'.'];
+hStruct.calls = {['T = ',hStruct.funName,'(rob,q)'],...
+    ['T = rob.',hStruct.funName,'(q)']};
+hStruct.detailedDescription = {['Given a set of joint variables up to joint number ',int2str(curBody),' the function'],...
+    'computes the pose belonging to that joint with respect to the base frame.'};
+hStruct.inputs = {['q:  ',int2str(curBody),'-element vector of generalized coordinates.'],...
+    'Angles have to be given in radians!'};
+hStruct.outputs = {['T:  [4x4] Homogenous transformation matrix relating the pose of joint ',int2str(curBody),' of ',int2str(rob.n)],...
+    '          for the given joint values to the base frame.'};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+     'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {rob.name};
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStructFkine(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Forward kinematics solution including tool transformation for the ',rob.name,' arm.'];
+hStruct.calls = {['T = ',hStruct.funName,'(rob,q)'],...
+    ['T = rob.',hStruct.funName,'(q)']};
+hStruct.detailedDescription = {['Given a full set of joint variables the function'],...
+    'computes the pose belonging to that joint with respect to the base frame.'};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     coordinates',...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['T:  [4x4] Homogenous transformation matrix relating the pose of the tool'],...
+    '          for the given joint values to the base frame.'};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+     'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'jacob0'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfriction.m

@@ -0,0 +1,96 @@
+%CODEGENERATOR.GENMFUNFRICTION Generate M-function for joint friction
+%
+% cGen.genmfunfriction() generates a robot-specific M-function to compute
+% joint friction.
+%
+% Notes::
+% - Is called only if cGen has active flag genmfun
+% - Access to generated function is provided via subclass of SerialLink 
+%   whose class definition is stored in cGen.robjpath.
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gengravload.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [ F ] = genmfunfriction( CGen )
+
+%% Does robot class exist?
+if ~exist(fullfile(CGen.robjpath,[CGen.getrobfname,'.m']),'file')
+    CGen.logmsg([datestr(now),'\tCreating ',CGen.getrobfname,' m-constructor ']);
+    CGen.createmconstructor;
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+%% Generate m-function
+CGen.logmsg([datestr(now),'\tGenerating joint friction m-function']);
+symname = 'friction';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfunfriction:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.m']);
+[~,qd] = CGen.rob.gencoords;
+
+matlabFunction(tmpStruct.(symname),'file',funfilename,...              % generate function m-file
+    'outputs', {'F'},...
+    'vars', {'rob',[qd]});
+hStruct = createHeaderStruct(CGen.rob,symname);                 % replace autogenerated function header
+replaceheader(CGen,hStruct,funfilename);
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+%% Definition of the header contents for each generated file
+function hStruct = createHeaderStruct(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Joint friction for the ',rob.name,' arm.'];
+hStruct.calls = {['F = ',hStruct.funName,'(rob,qd)'],...
+    ['F = rob.',hStruct.funName,'(qd)']};
+hStruct.detailedDescription = {['Given a full set of generalized joint velocities the function'],...
+    'computes the friction forces/torques.'};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['qd:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     velocities', ...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['F:  [',int2str(rob.n),'x1] vector of joint friction forces/torques']};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+     'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'gravload'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfungravload.m

@@ -0,0 +1,94 @@
+%CODEGENERATOR.GENMFUNGRAVLOAD Generate M-functions for gravitational load
+%
+% cGen.genmfungravload() generates a robot-specific M-function to compute
+% gravitation load forces and torques.
+%
+% Notes::
+% - Is called by CodeGenerator.gengravload if cGen has active flag genmfun
+% - Access to generated function is provided via subclass of SerialLink 
+%   whose class definition is stored in cGen.robjpath.
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.geninertia.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [] = genmfungravload(CGen)
+
+%% Does robot class exist?
+if ~exist(fullfile(CGen.robjpath,[CGen.getrobfname,'.m']),'file')
+    CGen.logmsg([datestr(now),'\tCreating ',CGen.getrobfname,' m-constructor ']);
+    CGen.createmconstructor;
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+CGen.logmsg([datestr(now),'\tGenerating m-function for the vector of gravitational load torques/forces' ]);
+symname = 'gravload';
+fname = fullfile(CGen.sympath,[symname,'.mat']);
+
+if exist(fname,'file')
+    tmpStruct = load(fname);
+else
+    error ('genmfungravload:SymbolicsNotFound','Save symbolic expressions to disk first!')
+end
+
+funfilename = fullfile(CGen.robjpath,[symname,'.m']);
+q = CGen.rob.gencoords;
+
+matlabFunction(tmpStruct.(symname),'file',funfilename,...              % generate function m-file
+    'outputs', {'G'},...
+    'vars', {'rob',[q]});
+hStruct = createHeaderStructGravity(CGen.rob,symname);                 % replace autogenerated function header
+replaceheader(CGen,hStruct,funfilename);
+CGen.logmsg('\t%s\n',' done!');
+
+end
+
+function hStruct = createHeaderStructGravity(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Computation of the configuration dependent vector of gravitational load forces/torques for ',rob.name];
+hStruct.calls = {['G = ',hStruct.funName,'(rob,q)'],...
+    ['G = rob.',hStruct.funName,'(q)']};
+hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
+                               'configuration dependent vector of gravitational load forces/torques.'};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     coordinates',...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['G:  [',int2str(rob.n),'x1] vector of gravitational load forces/torques']};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+     'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'inertia'};
+end

lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuninertia.m

@@ -0,0 +1,149 @@
+%CODEGENERATION.GENMFUNINERTIA Generate M-function for robot inertia matrix
+%
+% cGen.genmfuninertia() generates a robot-specific M-function to compute
+% robot inertia matrix.
+%
+% Notes::
+% - Is called by CodeGenerator.geninertia if cGen has active flag genmfun
+% - The inertia matrix is stored row by row to avoid memory issues.
+% - The generated M-function recombines the individual M-functions for each row.
+% - Access to generated function is provided via subclass of SerialLink 
+%   whose class definition is stored in cGen.robjpath.
+%
+% Author::
+%  Joern Malzahn
+%  2012 RST, Technische Universitaet Dortmund, Germany.
+%  http://www.rst.e-technik.tu-dortmund.de
+%
+% See also CodeGenerator.CodeGenerator, CodeGenerator.gencoriolis.
+
+% Copyright (C) 2012-2013, by Joern Malzahn
+%
+% This file is part of The Robotics Toolbox for Matlab (RTB).
+%
+% RTB is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License as published by
+% the Free Software Foundation, either version 3 of the License, or
+% (at your option) any later version.
+%
+% RTB is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Leser General Public License
+% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
+%
+% http://www.petercorke.com
+%
+% The code generation module emerged during the work on a project funded by
+% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully 
+% acknowledge the financial support.
+
+function [] = genmfuninertia(CGen)
+
+%% Does robot class exist?
+if ~exist(fullfile(CGen.robjpath,[CGen.getrobfname,'.m']),'file')
+    CGen.logmsg([datestr(now),'\tCreating ',CGen.getrobfname,' m-constructor ']);
+    CGen.createmconstructor;
+    CGen.logmsg('\t%s\n',' done!');
+end
+
+%%
+CGen.logmsg([datestr(now),'\tGenerating m-function for the robot inertia matrix row' ]);
+
+q = CGen.rob.gencoords;
+nJoints = CGen.rob.n;
+
+for kJoints = 1:nJoints
+    CGen.logmsg(' %s ',num2str(kJoints));
+    symname = ['inertia_row_',num2str(kJoints)];
+    fname = fullfile(CGen.sympath,[symname,'.mat']);
+    
+    if exist(fname,'file')
+        tmpStruct = load(fname);
+    else
+        error ('genmfuninertia:SymbolicsNotFound','Save symbolic expressions to disk first!')
+    end
+    
+    funfilename = fullfile(CGen.robjpath,[symname,'.m']);
+    
+    matlabFunction(tmpStruct.(symname),'file',funfilename,...              % generate function m-file
+        'outputs', {'Irow'},...
+        'vars', {'rob',[q]});
+    hStruct = createHeaderStructRow(CGen.rob,kJoints,symname);                 % replace autogenerated function header
+    replaceheader(CGen,hStruct,funfilename);
+    
+    
+end
+CGen.logmsg('\t%s\n',' done!');
+
+
+CGen.logmsg([datestr(now),'\tGenerating full inertia matrix m-function']);
+    
+    funfilename = fullfile(CGen.robjpath,'inertia.m');
+    hStruct = createHeaderStructFullInertia(CGen.rob,funfilename);
+    
+    fid = fopen(funfilename,'w+');
+    
+    fprintf(fid, '%s\n', ['function I = inertia(rob,q)']);                 % Function definition
+    fprintf(fid, '%s\n',constructheaderstring(CGen,hStruct));                   % Header
+   
+    fprintf(fid, '%s \n', 'I = zeros(length(q));');                        % Code
+    for iJoints = 1:nJoints
+        funcCall = ['I(',num2str(iJoints),',:) = ','rob.inertia_row_',num2str(iJoints),'(q);'];
+        fprintf(fid, '%s \n', funcCall);
+    end
+    
+    fclose(fid);
+    
+    CGen.logmsg('\t%s\n',' done!');           
+end
+
+function hStruct = createHeaderStructRow(rob,curJointIdx,fName)
+[~,hStruct.funName] = fileparts(fName);
+hStruct.shortDescription = ['Computation of the robot specific inertia matrix row for corresponding to joint ', num2str(curJointIdx), ' of ',num2str(rob.n),'.'];
+hStruct.calls = {['Irow = ',hStruct.funName,'(rob,q)'],...
+    ['Irow = rob.',hStruct.funName,'(q)']};
+hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
+                               ['inertia matrix row number ', num2str(curJointIdx),' of ',num2str(rob.n),' for ',rob.name,'.']};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     coordinates',...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['Irow:  [1x',int2str(rob.n),'] row of the robot inertia matrix']};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+    'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'coriolis'};
+end
+
+function hStruct = createHeaderStructFullInertia(rob,fname)
+[~,hStruct.funName] = fileparts(fname);
+hStruct.shortDescription = ['Inertia matrix for the ',rob.name,' arm.'];
+hStruct.calls = {['I = ',hStruct.funName,'(rob,q)'],...
+    ['I = rob.',hStruct.funName,'(q)']};
+hStruct.detailedDescription = {'Given a full set of joint variables the function computes the',...
+                               'inertia Matrix of the robot.'};
+hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
+                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
+                   '     coordinates',...
+                   'Angles have to be given in radians!'};
+hStruct.outputs = {['I:  [',int2str(rob.n),'x',int2str(rob.n),'] inertia matrix']};
+hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
+    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
+    '3) Introduction to Robotics, Mechanics and Control - Craig',...
+    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
+hStruct.authors = {'This is an autogenerated function!',...
+    'Code generator written by:',...
+    'Joern Malzahn',...
+    '2012 RST, Technische Universitaet Dortmund, Germany',...
+    'http://www.rst.e-technik.tu-dortmund.de'};
+hStruct.seeAlso = {'coriolis'};
+end