diff --git a/lwrserv/matlab/README.md b/lwrserv/matlab/README.md new file mode 100644 index 0000000..379ad3d --- /dev/null +++ b/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 \ No newline at end of file diff --git a/lwrserv/matlab/gui/control.m b/lwrserv/matlab/gui/control.m new file mode 100644 index 0000000..97c8ae9 --- /dev/null +++ b/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 + diff --git a/lwrserv/matlab/gui/gui.fig b/lwrserv/matlab/gui/gui.fig new file mode 100644 index 0000000..58f553b Binary files /dev/null and b/lwrserv/matlab/gui/gui.fig differ diff --git a/lwrserv/matlab/gui/gui.m b/lwrserv/matlab/gui/gui.m new file mode 100644 index 0000000..73bba96 --- /dev/null +++ b/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 + diff --git a/lwrserv/matlab/kinematics/DHjT.m b/lwrserv/matlab/kinematics/DHjT.m new file mode 100644 index 0000000..d6e39d2 --- /dev/null +++ b/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 + diff --git a/lwrserv/matlab/kinematics/LBRforKin.m b/lwrserv/matlab/kinematics/LBRforKin.m new file mode 100644 index 0000000..65e90d3 --- /dev/null +++ b/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; diff --git a/lwrserv/matlab/kinematics/LBRinvKin.m b/lwrserv/matlab/kinematics/LBRinvKin.m new file mode 100644 index 0000000..ff321e5 --- /dev/null +++ b/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)) =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 diff --git a/lwrserv/matlab/kinematics/getM.m b/lwrserv/matlab/kinematics/getM.m new file mode 100644 index 0000000..90fb415 --- /dev/null +++ b/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 + diff --git a/lwrserv/matlab/kinematics/getRotMat.m b/lwrserv/matlab/kinematics/getRotMat.m new file mode 100644 index 0000000..309820f --- /dev/null +++ b/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)) 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 \ No newline at end of file diff --git a/lwrserv/matlab/lspb_test.m b/lwrserv/matlab/lspb_test.m new file mode 100644 index 0000000..e69de29 diff --git a/lwrserv/matlab/motion/control_motion.m b/lwrserv/matlab/motion/control_motion.m new file mode 100644 index 0000000..a8fc019 --- /dev/null +++ b/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 + diff --git a/lwrserv/matlab/motion/gui_motion.fig b/lwrserv/matlab/motion/gui_motion.fig new file mode 100644 index 0000000..9f4f0d1 Binary files /dev/null and b/lwrserv/matlab/motion/gui_motion.fig differ diff --git a/lwrserv/matlab/motion/gui_motion.m b/lwrserv/matlab/motion/gui_motion.m new file mode 100644 index 0000000..1a9fdab --- /dev/null +++ b/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) diff --git a/lwrserv/matlab/motion/motion_config.m b/lwrserv/matlab/motion/motion_config.m new file mode 100644 index 0000000..89b1451 --- /dev/null +++ b/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 + diff --git a/lwrserv/matlab/rvctools/common/Animate.m b/lwrserv/matlab/rvctools/common/Animate.m new file mode 100644 index 0000000..240e406 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/common/PGraph.m b/lwrserv/matlab/rvctools/common/PGraph.m new file mode 100644 index 0000000..580cc0f --- /dev/null +++ b/lwrserv/matlab/rvctools/common/PGraph.m @@ -0,0 +1,1055 @@ +%PGraph Graph class +% +% g = PGraph() create a 2D, planar, undirected graph +% g = PGraph(n) create an n-d, undirected graph +% +% Provides support for graphs that: +% - are directed +% - are embedded in coordinate system +% - have symmetric cost edges (A to B is same cost as B to A) +% - have no loops (edges from A to A) +% - have vertices are represented by integers vid +% - have edges are represented by integers, eid +% +% Methods:: +% +% Constructing the graph:: +% g.add_node(coord) add vertex, return vid +% g.add_edge(v1, v2) add edge from v1 to v2, return eid +% g.setcost(e, c) set cost for edge e +% g.setdata(v, u) set user data for vertex v +% g.data(v) get user data for vertex v +% g.clear() remove all vertices and edges from the graph +% +% Information from graph:: +% g.edges(v) list of edges for vertex v +% g.cost(e) cost of edge e +% g.neighbours(v) neighbours of vertex v +% g.component(v) component id for vertex v +% g.connectivity() number of edges for all vertices +% +% Display:: +% +% g.plot() set goal vertex for path planning +% g.highlight_node(v) highlight vertex v +% g.highlight_edge(e) highlight edge e +% g.highlight_component(c) highlight all nodes in component c +% g.highlight_path(p) highlight nodes and edge along path p +% +% g.pick(coord) vertex closest to coord +% +% g.char() convert graph to string +% g.display() display summary of graph +% +% Matrix representations:: +% g.adjacency() adjacency matrix +% g.incidence() incidence matrix +% g.degree() degree matrix +% g.laplacian() Laplacian matrix +% +% Planning paths through the graph:: +% g.Astar(s, g) shortest path from s to g +% g.goal(v) set goal vertex, and plan paths +% g.path(v) list of vertices from v to goal +% +% Graph and world points:: +% g.coord(v) coordinate of vertex v +% g.distance(v1, v2) distance between v1 and v2 +% g.distances(coord) return sorted distances from coord to all vertices +% g.closest(coord) vertex closest to coord +% +% Object properties (read only):: +% g.n number of vertices +% g.ne number of edges +% g.nc number of components +% +% Notes:: +% - Graph connectivity is maintained by a labeling algorithm and this +% is updated every time an edge is added. +% - Nodes and edges cannot be deleted. +% - Support for edge direction is rudimentary. + +% Peter Corke 8/2009. + +% TODO: +% be able to delete nodes, must update connectivity + +classdef PGraph < handle + + properties (SetAccess=private, GetAccess=private) + vertexlist % vertex coordinates, columnwise, vertex number is the column number + edgelist % 2xNe matrix, each column is vertex index of edge start and end + edgelen % length (cost) of this edge + + curLabel % current label + ncomponents % number of components + labels % label of each vertex (1xN) + labelset % set of all labels (1xNc) + + goaldist % distance from goal, after planning + + userdata % per vertex data, cell array + ndims % number of coordinate dimensions, height of vertices matrix + verbose + measure % distance measure: 'Euclidean', 'SE2' + end + + properties (Dependent) + n % number of nodes/vertices + ne % number of edges + nc % number of components + end + + methods + + function g = PGraph(ndims, varargin) + %PGraph.PGraph Graph class constructor + % + % G=PGraph(D, OPTIONS) is a graph object embedded in D dimensions. + % + % Options:: + % 'distance',M Use the distance metric M for path planning which is either + % 'Euclidean' (default) or 'SE2'. + % 'verbose' Specify verbose operation + % + % Note:: + % - Number of dimensions is not limited to 2 or 3. + % - The distance metric 'SE2' is the sum of the squares of the difference + % in position and angle modulo 2pi. + % - To use a different distance metric create a subclass of PGraph and + % override the method distance_metric(). + + if nargin < 1 + ndims = 2; % planar by default + end + g.ndims = ndims; + opt.distance = 'Euclidean'; + opt = tb_optparse(opt, varargin); + + g.clear(); + g.verbose = opt.verbose; + g.measure = opt.distance; + g.userdata = {}; + end + + function n = get.n(g) + %Pgraph.n Number of vertices + % + % G.n is the number of vertices in the graph. + % + % See also PGraph.ne. + n = numcols(g.vertexlist); + end + + function ne = get.ne(g) + %Pgraph.ne Number of edges + % + % G.ne is the number of edges in the graph. + % + % See also PGraph.n. + ne = numcols(g.edgelist); + end + + function ne = get.nc(g) + %Pgraph.nc Number of components + % + % G.nc is the number of components in the graph. + % + % See also PGraph.component. + + ne = g.ncomponents; + end + + function clear(g) + %PGraph.clear Clear the graph + % + % G.clear() removes all vertices, edges and components. + + g.labelset = zeros(1, 0); + g.labels = zeros(1, 0); + g.edgelist = zeros(2, 0); + g.edgelen = zeros(1, 0); + g.vertexlist = zeros(g.ndims, 0); + + g.ncomponents = 0; + g.curLabel = 0; + end + + function v = add_node(g, coord, varargin) + %PGraph.add_node Add a node + % + % V = G.add_node(X) adds a node/vertex with coordinate X (Dx1) and + % returns the integer node id V. + % + % V = G.add_node(X, V2) as above but connected by a directed edge from vertex V + % to vertex V2 with cost equal to the distance between the vertices. + % + % V = G.add_node(X, V2, C) as above but the added edge has cost C. + % + % See also PGraph.add_edge, PGraph.data, PGraph.getdata. + + if length(coord) ~= g.ndims + error('coordinate length different to graph coordinate dimensions'); + end + + % append the coordinate as a column in the vertex matrix + g.vertexlist = [g.vertexlist coord(:)]; + v = numcols(g.vertexlist); + g.labels(v) = g.newlabel(); + + if g.verbose + fprintf('add node (%d) = ', v); + fprintf('%f ', coord); + fprintf('\n'); + end + + % optionally add an edge + if nargin > 2 + g.add_edge(v, varargin{:}); + end + end + + function u = setdata(g, v, u) + %PGraph.setdata Set user data for node + % + % G.setdata(V, U) sets the user data of vertex V to U which can be of any + % type such as number, struct, object or cell array. + % + % See also PGraph.data. + + g.userdata{v} = u; + end + + function u = data(g, v) + %PGraph.data Get user data for node + % + % U = G.data(V) gets the user data of vertex V which can be of any + % type such as number, struct, object or cell array. + % + % See also PGraph.setdata. + u = g.userdata{v}; + end + + + function add_edge(g, v1, v2, d) + %PGraph.add_edge Add an edge + % + % E = G.add_edge(V1, V2) adds a directed edge from vertex id V1 to vertex id V2, and + % returns the edge id E. The edge cost is the distance between the vertices. + % + % E = G.add_edge(V1, V2, C) as above but the edge cost is C. + % cost C. + % + % Note:: + % - Graph connectivity is maintained by a labeling algorithm and this + % is updated every time an edge is added. + % + % See also PGraph.add_node, PGraph.edgedir. + if g.verbose + fprintf('add edge %d -> %d\n', v1, v2); + end + for vv=v2(:)' + g.edgelist = [g.edgelist [v1; vv]]; + if (nargin < 4) || isempty(d) + d = g.distance(v1, vv); + end + g.edgelen = [g.edgelen d]; + if g.labels(vv) ~= g.labels(v1) + g.merge(g.labels(vv), g.labels(v1)); + end + end + end + + function c = component(g, v) + %PGraph.component Graph component + % + % C = G.component(V) is the id of the graph component + c = []; + for vv=v + tf = ismember(g.labelset, g.labels(vv)); + c = [c find(tf)]; + end + end + + % which edges contain v + % elist = g.edges(v) + function e = edges(g, v) + %PGraph.edges Find edges given vertex + % + % E = G.edges(V) is a vector containing the id of all edges from vertex id V. + % + % See also PGraph.edgedir. + e = [find(g.edgelist(1,:) == v) find(g.edgelist(2,:) == v)]; + end + + + function dir = edgedir(g, v1, v2) + %PGraph.edgedir Find edge direction + % + % D = G.edgedir(V1, V2) is the direction of the edge from vertex id V1 + % to vertex id V2. + % + % If we add an edge from vertex 3 to vertex 4 + % g.add_edge(3, 4) + % then + % g.edgedir(3, 4) + % is positive, and + % g.edgedir(4, 3) + % is negative. + % + % See also PGraph.add_node, PGraph.add_edge. + n = g.edges(v1); + if any(ismember( g.edgelist(2, n), v2)) + dir = 1; + elseif any(ismember( g.edgelist(1, n), v2)) + dir = -1; + else + dir = 0; + end + end + + function v = vertices(g, e) + %PGraph.vertices Find vertices given edge + % + % V = G.vertices(E) return the id of the vertices that define edge E. + v = g.edgelist(:,e); + end + + + function [n,c] = neighbours(g, v) + %PGraph.neighbours Neighbours of a vertex + % + % N = G.neighbours(V) is a vector of ids for all vertices which are + % directly connected neighbours of vertex V. + % + % [N,C] = G.neighbours(V) as above but also returns a vector C whose elements + % are the edge costs of the paths corresponding to the vertex ids in N. + e = g.edges(v); + n = g.edgelist(:,e); + n = n(:)'; + n(n==v) = []; % remove references to self + if nargout > 1 + c = g.cost(e); + end + end + + + + function [n,c] = neighbours_d(g, v) + %PGraph.neighbours_d Directed neighbours of a vertex + % + % N = G.neighbours_d(V) is a vector of ids for all vertices which are + % directly connected neighbours of vertex V. Elements are positive + % if there is a link from V to the node, and negative if the link + % is from the node to V. + % + % [N,C] = G.neighbours_d(V) as above but also returns a vector C whose elements + % are the edge costs of the paths corresponding to the vertex ids in N. + e = g.edges(v); + n = [-g.edgelist(1,e) g.edgelist(2,e)]; + n(abs(n)==v) = []; % remove references to self + if nargout > 1 + c = g.cost(e); + end + end + + function d = cost(g, e) + %PGraph.cost Cost of edge + % + % C = G.cost(E) is the cost of edge id E. + d = g.edgelen(e); + end + + function d = setcost(g, e, c) + %PGraph.cost Set cost of edge + % + % G.setcost(E, C) set cost of edge id E to C. + g.edgelen(e) = c; + end + + function p = coord(g, v) + %PGraph.coord Coordinate of node + % + % X = G.coord(V) is the coordinate vector (Dx1) of vertex id V. + + p = g.vertexlist(:,v); + end + + + function c = connectivity(g) + %PGraph.connectivity Graph connectivity + % + % C = G.connectivity() is a vector (Nx1) with the number of edges per + % vertex. + % + % The average vertex connectivity is + % mean(g.connectivity()) + % + % and the minimum vertex connectivity is + % min(g.connectivity()) + + for k=1:g.n + c(k) = length(g.edges(k)); + end + end + + function plot(g, varargin) + %PGraph.plot Plot the graph + % + % G.plot(OPT) plots the graph in the current figure. Nodes + % are shown as colored circles. + % + % Options:: + % 'labels' Display vertex id (default false) + % 'edges' Display edges (default true) + % 'edgelabels' Display edge id (default false) + % 'NodeSize',S Size of vertex circle (default 8) + % 'NodeFaceColor',C Node circle color (default blue) + % 'NodeEdgeColor',C Node circle edge color (default blue) + % 'NodeLabelSize',S Node label text sizer (default 16) + % 'NodeLabelColor',C Node label text color (default blue) + % 'EdgeColor',C Edge color (default black) + % 'EdgeLabelSize',S Edge label text size (default black) + % 'EdgeLabelColor',C Edge label text color (default black) + % 'componentcolor' Node color is a function of graph component + + colorlist = 'bgmyc'; + + % show vertices + holdon = ishold; + hold on + + % parse options + opt.componentcolor = false; + opt.labels = false; + opt.edges = true; + opt.edgelabels = false; + opt.NodeSize = 8; + opt.NodeFaceColor = 'b'; + opt.NodeEdgeColor = 'b'; + opt.NodeLabelSize = 16; + opt.NodeLabelColor = 'b'; + opt.EdgeColor = 'k'; + opt.EdgeLabelSize = 8; + opt.EdgeLabelColor = 'k'; + + [opt,args] = tb_optparse(opt, varargin); + + % set default color if none specified + if ~isempty(args) + mcolor = args{1}; + else + mcolor = 'b'; + end + + % show the vertices as filled circles + for i=1:g.n + % for each node + if opt.componentcolor + j = mod( g.component(i)-1, length(colorlist) ) + 1; + c = colorlist(j); + else + c = mcolor; + end + args = {'LineStyle', 'None', ... + 'Marker', 'o', ... + 'MarkerFaceColor', opt.NodeFaceColor, ... + 'MarkerSize', opt.NodeSize, ... + 'MarkerEdgeColor', opt.NodeEdgeColor }; + if g.ndims == 3 + plot3(g.vertexlist(1,i), g.vertexlist(2,i), g.vertexlist(3,i), args{:}); + else + plot(g.vertexlist(1,i), g.vertexlist(2,i), args{:}); + end + end + % show edges + if opt.edges + for e=g.edgelist + v1 = g.vertexlist(:,e(1)); + v2 = g.vertexlist(:,e(2)); + if g.ndims == 3 + plot3([v1(1) v2(1)], [v1(2) v2(2)], [v1(3) v2(3)], ... + 'Color', opt.EdgeColor); + else + plot([v1(1) v2(1)], [v1(2) v2(2)], ... + 'Color', opt.EdgeColor); + end + end + end + % show the edge labels + if opt.edgelabels + for i=1:numcols(g.edgelist) + e = g.edgelist(:,i); + v1 = g.vertexlist(:,e(1)); + v2 = g.vertexlist(:,e(2)); + + text('String', sprintf(' %g', g.cost(i)), ... + 'Position', (v1 + v2)/2, ... + 'HorizontalAlignment', 'left', ... + 'VerticalAlignment', 'middle', ... + 'FontUnits', 'pixels', ... + 'FontSize', opt.EdgeLabelSize, ... + 'Color', opt.EdgeLabelColor); + end + end + % show the labels + if opt.labels + for i=1:numcols(g.vertexlist) + text('String', sprintf(' %d', i), ... + 'Position', g.vertexlist(:,i), ... + 'HorizontalAlignment', 'left', ... + 'VerticalAlignment', 'middle', ... + 'FontUnits', 'pixels', ... + 'FontSize', opt.NodeLabelSize, ... + 'Color', opt.NodeLabelColor); + end + end + if ~holdon + hold off + end + end + + function v = pick(g) + %PGraph.pick Graphically select a vertex + % + % V = G.pick() is the id of the vertex closest to the point clicked + % by the user on a plot of the graph. + % + % See also PGraph.plot. + [x,y] = ginput(1); + v = g.closest([x; y]); + end + + function goal(g, vg) + %PGraph.goal Set goal node + % + % G.goal(VG) computes the cost of reaching every vertex in the graph connected + % to the goal vertex VG. + % + % Notes:: + % - Combined with G.path performs a breadth-first search for paths to the goal. + % + % See also PGraph.path, PGraph.Astar. + + % cost is total distance from goal + g.goaldist = Inf*ones(1, numcols(g.vertexlist)); + + g.goaldist(vg) = 0; + g.descend(vg); + end + + + function p = path(g, v) + %PGraph.path Find path to goal node + % + % P = G.path(VS) is a vector of vertex ids that form a path from + % the starting vertex VS to the previously specified goal. The path + % includes the start and goal vertex id. + % + % To compute path to goal vertex 5 + % g.goal(5); + % then the path, starting from vertex 1 is + % p1 = g.path(1); + % and the path starting from vertex 2 is + % p2 = g.path(2); + % + % Notes:: + % - Pgraph.goal must have been invoked first. + % - Can be used repeatedly to find paths from different starting points + % to the goal specified to Pgraph.goal(). + % + % See also PGraph.goal, PGraph.Astar. + p = [v]; + + while g.goaldist(v) ~= 0 + v = g.next(v); + p = [p v]; + end + end + + + function d = distance(g, v1, v2) + %PGraph.distance Distance between vertices + % + % D = G.distance(V1, V2) is the geometric distance between + % the vertices V1 and V2. + % + % See also PGraph.distances. + + d = g.distance_metric( g.vertexlist(:,v1), g.vertexlist(:,v2)); + + end + + function [d,k] = distances(g, p) + %PGraph.distances Distances from point to vertices + % + % D = G.distances(X) is a vector (1xN) of geometric distance from the point + % X (Dx1) to every other vertex sorted into increasing order. + % + % [D,W] = G.distances(P) as above but also returns W (1xN) with the + % corresponding vertex id. + % + % See also PGraph.closest. + + d = g.distance_metric(p(:), g.vertexlist); + [d,k] = sort(d, 'ascend'); + end + + function [c,dn] = closest(g, p) + %PGraph.closest Find closest vertex + % + % V = G.closest(X) is the vertex geometrically closest to coordinate X. + % + % [V,D] = G.closest(X) as above but also returns the distance D. + % + % See also PGraph.distances. + d = g.distance_metric(p(:), g.vertexlist); + [mn,c] = min(d); + + if nargin > 1 + dn = mn; + end + end + + function display(g) + %PGraph.display Display graph + % + % G.display() displays a compact human readable representation of the + % state of the graph including the number of vertices, edges and components. + % + % See also PGraph.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(g) ); + end % display() + + function s = char(g) + %PGraph.char Convert graph to string + % + % S = G.char() is a compact human readable representation of the + % state of the graph including the number of vertices, edges and components. + + s = ''; + s = strvcat(s, sprintf(' %d dimensions', g.ndims)); + s = strvcat(s, sprintf(' %d vertices', g.n)); + s = strvcat(s, sprintf(' %d edges', numcols(g.edgelist))); + s = strvcat(s, sprintf(' %d components', g.ncomponents)); + end + + %% convert graphs to matrix representations + + function L = laplacian(g) + %Pgraph.laplacian Laplacian matrix of graph + % + % L = G.laplacian() is the Laplacian matrix (NxN) of the graph. + % + % Notes:: + % - L is always positive-semidefinite. + % - L has at least one zero eigenvalue. + % - The number of zero eigenvalues is the number of connected components + % in the graph. + % + % See also PGraph.adjacency, PGraph.incidence, PGraph.degree. + + L = g.degree() - (g.adjacency() > 0); + end + + function D = degree(g) + %Pgraph.degree Degree matrix of graph + % + % D = G.degree() is a diagonal matrix (NxN) where element D(i,i) is the number + % of edges connected to vertex id i. + % + % See also PGraph.adjacency, PGraph.incidence, PGraph.laplacian. + + D = diag( g.connectivity() ); + end + + function A = adjacency(g) + %Pgraph.adjacency Adjacency matrix of graph + % + % A = G.adjacency() is a matrix (NxN) where element A(i,j) is the cost + % of moving from vertex i to vertex j. + % + % Notes:: + % - Matrix is symmetric. + % - Eigenvalues of A are real and are known as the spectrum of the graph. + % - The element A(I,J) can be considered the number of walks of one + % edge from vertex I to vertex J (either zero or one). The element (I,J) + % of A^N are the number of walks of length N from vertex I to vertex J. + % + % See also PGraph.degree, PGraph.incidence, PGraph.laplacian. + + A = zeros(g.n, g.n); + for i=1:g.n + [n,c] = g.neighbours(i); + for j=1:numel(n) + A(i,n(j)) = c(j); + A(n(j),i) = c(j); + end + end + end + + function I = incidence(g) + %Pgraph.degree Incidence matrix of graph + % + % IN = G.incidence() is a matrix (NxNE) where element IN(i,j) is + % non-zero if vertex id i is connected to edge id j. + % + % See also PGraph.adjacency, PGraph.degree, PGraph.laplacian. + I = zeros(g.n, numcols(g.edgelist)); + for i=1:g.n + for n=g.edges(i) + I(i,n) = 1; + end + end + end + + %% these are problematic, dont advertise them + % + % removing an edge may divide the graph into 2 components, this is expensive + % to check and currently not implemented + function delete_edge(g, e) + g.edgelist(:,e) = []; + % really need to check if the two halves are connected, is expensive + % could use path planner + end + + function delete_node(g, v) + el = g.edges(v); + el + % make the column invalid, really should remove it but this + % requires changing all the edgelist entries, and the vertex + % numbers will change... + g.vertexlist(:,v) = [NaN; NaN]; + g.delete_edge(el); + g.n = g.n - 1; + end + + + function highlight_node(g, verts, varargin) + %PGraph.highlight_node Highlight a node + % + % G.highlight_node(V, OPTIONS) highlights the vertex V with a yellow marker. + % If V is a list of vertices then all are highlighted. + % + % Options:: + % 'NodeSize',S Size of vertex circle (default 12) + % 'NodeFaceColor',C Node circle color (default yellow) + % 'NodeEdgeColor',C Node circle edge color (default blue) + % + % See also PGraph.highlight_edge, PGraph.highlight_path, PGraph.highlight_component. + + hold on + + % parse options + opt.NodeSize = 12; + opt.NodeFaceColor = 'y'; + opt.NodeEdgeColor = 'b'; + + [opt,args] = tb_optparse(opt, varargin); + markerprops = {'LineStyle', 'None', ... + 'Marker', 'o', ... + 'MarkerFaceColor', opt.NodeFaceColor, ... + 'MarkerSize', opt.NodeSize, ... + 'MarkerEdgeColor', opt.NodeEdgeColor }; + + for v=verts + if g.ndims == 3 + plot3(g.vertexlist(1,v), g.vertexlist(2,v), g.vertexlist(3,v), ... + markerprops{:}); + else + plot(g.vertexlist(1,v), g.vertexlist(2,v), markerprops{:}); + end + end + end + + function highlight_component(g, c, varargin) + %PGraph.highlight_component Highlight a graph component + % + % G.highlight_component(C, OPTIONS) highlights the vertices that belong to + % graph component C. + % + % Options:: + % 'NodeSize',S Size of vertex circle (default 12) + % 'NodeFaceColor',C Node circle color (default yellow) + % 'NodeEdgeColor',C Node circle edge color (default blue) + % + % See also PGraph.highlight_node, PGraph.highlight_edge, PGraph.highlight_component. + nodes = find(g.labels == g.labelset(c)); + for v=nodes + g.highlight_node(v, varargin{:}); + end + end + + function highlight_edge(g, e, varargin) + %PGraph.highlight_node Highlight a node + % + % G.highlight_edge(V1, V2) highlights the edge between vertices V1 and V2. + % + % G.highlight_edge(E) highlights the edge with id E. + % + % Options:: + % 'EdgeColor',C Edge edge color (default black) + % 'EdgeThickness',T Edge thickness (default 1.5) + % + % See also PGraph.highlight_node, PGraph.highlight_path, PGraph.highlight_component. + + % parse options + opt.EdgeColor = 'k'; + opt.EdgeThickness = 1.5; + + [opt,args] = tb_optparse(opt, varargin); + + hold on + if (length(args) > 0) && isnumeric(args{1}) + % highlight_edge(V1, V2) + v1 = e; + v2 = args{1}; + + v1 = g.vertexlist(:,v1); + v2 = g.vertexlist(:,v2); + else + % highlight_edge(E) + e = g.edgelist(:,e); + v1 = g.vertexlist(:,e(1)); + v2 = g.vertexlist(:,e(2)); + end + + % create the line properties for the edges + lineprops = { + 'Color', opt.EdgeColor, ... + 'LineWidth', opt.EdgeThickness }; + + if g.ndims == 3 + plot3([v1(1) v2(1)], [v1(2) v2(2)], [v1(3) v2(3)], lineprops{:}); + else + plot([v1(1) v2(1)], [v1(2) v2(2)], lineprops{:}); + end + end + + function highlight_path(g, path) + %PGraph.highlight_path Highlight path + % + % G.highlight_path(P, OPTIONS) highlights the path defined by vector P + % which is a list of vertices comprising the path. + % + % Options:: + % 'NodeSize',S Size of vertex circle (default 12) + % 'NodeFaceColor',C Node circle color (default yellow) + % 'NodeEdgeColor',C Node circle edge color (default blue) + % 'EdgeColor',C Node circle edge color (default black) + % + % See also PGraph.highlight_node, PGraph.highlight_edge, PGraph.highlight_component. + g.highlight_node(path); + + % highlight the edges + for i=1:numel(path)-1 + v1 = path(i); + v2 = path(i+1); + g.highlight_edge(v1, v2); + end + end + + function [path,cost] = Astar(g, vstart, vgoal) + %PGraph.Astar path finding + % + % PATH = G.Astar(V1, V2) is the lowest cost path from vertex V1 to + % vertex V2. PATH is a list of vertices starting with V1 and ending + % V2. + % + % [PATH,C] = G.Astar(V1, V2) as above but also returns the total cost + % of traversing PATH. + % + % Notes:: + % - Uses the efficient A* search algorithm. + % + % References:: + % - Correction to "A Formal Basis for the Heuristic Determination of Minimum Cost Paths". + % Hart, P. E.; Nilsson, N. J.; Raphael, B. + % SIGART Newsletter 37: 28-29, 1972. + % + % See also PGraph.goal, PGraph.path. + + % The set of vertices already evaluated. + closedset = []; + + % The set of tentative vertices to be evaluated, initially containing the start node + openset = [vstart]; + came_from = []; % The map of navigated vertices. + + g_score(vstart) = 0; % Cost from start along best known path. + h_score(vstart) = g.distance(vstart, vgoal); + % Estimated total cost from start to goal through y. + f_score(vstart) = g_score(vstart) + h_score(vstart); + + while ~isempty(openset) + % current := the node in openset having the lowest f_score[] value + [mn,k] = min(f_score(openset)); + vcurrent = openset(k); + + if vcurrent == vgoal + path = []; + p = vgoal; + while true + path = [p path]; + p = came_from(p); + if p == 0 + break; + end + end + if nargout > 1 + cost = f_score(vgoal); + end + return + end + + %remove current from openset + openset = setdiff(openset, vcurrent); + %add current to closedset + closedset = union(closedset, vcurrent); + + for neighbour = g.neighbours(vcurrent) + if ismember(neighbour, closedset) + continue; + end + tentative_g_score = g_score(vcurrent) + ... + g.distance(vcurrent,neighbour); + + if ~ismember(neighbour, openset) + %add neighbor to openset + openset = union(openset, neighbour); + h_score(neighbour) = g.distance(neighbour, vgoal); + tentative_is_better = true; + elseif tentative_g_score < g_score(neighbour) + tentative_is_better = true; + else + tentative_is_better = false; + end + if tentative_is_better + came_from(neighbour) = vcurrent; + g_score(neighbour) = tentative_g_score; + f_score(neighbour) = g_score(neighbour) + h_score(neighbour); + end + end + end + path = []; + end + + + end % method + + methods (Access='protected') + % private methods + + % depth first + function descend(g, vg) + + % get neighbours and their distance + for nc = g.neighbours2(vg); + vn = nc(1); + d = nc(2); + newcost = g.goaldist(vg) + d; + if isinf(g.goaldist(vn)) + % no cost yet assigned, give it this one + g.goaldist(vn) = newcost; + %fprintf('1: cost %d <- %f\n', vn, newcost); + descend(g, vn); + else + % it already has a cost + if g.goaldist(vn) <= newcost + continue; + else + g.goaldist(vn) = newcost; + %fprintf('2: cost %d <- %f\n', vn, newcost); + descend(g, vn); + end + end + end + end + + % breadth first + function descend2(g, vg) + + % get neighbours and their distance + for vn = g.neighbours2(vg); + vn = nc(1); + d = nc(2); + newcost = g.goaldist(vg) + d; + if isinf(g.goaldist(vn)) + % no cost yet assigned, give it this one + g.goaldist(vn) = newcost; + fprintf('1: cost %d <- %f\n', vn, newcost); + descend(g, vn); + elseif g.goaldist(vn) > newcost + % it already has a cost + g.goaldist(vn) = newcost; + end + end + for vn = g.neighbours(vg); + descend(g, vn); + end + end + + function l = newlabel(g) + g.curLabel = g.curLabel + 1; + l = g.curLabel; + g.ncomponents = g.ncomponents + 1; + g.labelset = union(g.labelset, l); + end + + % merge label1 and label2, lowest label dominates + function merge(g, l1, l2) + % get the dominant and submissive labels + ldom = min(l1, l2); + lsub = max(l1, l2); + + % change all instances of submissive label to dominant one + g.labels(g.labels==lsub) = ldom; + + % reduce the number of components + g.ncomponents = g.ncomponents - 1; + % and remove the submissive label from the set of all labels + g.labelset = setdiff(g.labelset, lsub); + end + + function nc = neighbours2(g, v) + e = g.edges(v); + n = g.edgelist(:,e); + n = n(:)'; + n(n==v) = []; % remove references to self + c = g.cost(e); + nc = [n; c]; + end + + function d = distance_metric(g, x1, x2) + + % distance between coordinates x1 and x2 using the relevant metric + % x2 can be multiple points represented by multiple columns + switch g.measure + case 'Euclidean' + d = colnorm( bsxfun(@minus, x1, x2) ); + + case 'SE2' + d = bsxfun(@minus, x1, x2); + d(3,:) = angdiff(d(3,:)); + d = colnorm( d ); + otherwise + error('unknown distance measure', g.measure); + end + end + + function vn = next(g, v) + + % V = G.next(VS) return the id of a node connected to node id VS + % that is closer to the goal. + n = g.neighbours(v); + [mn,k] = min( g.goaldist(n) ); + vn = n(k); + end + + end % private methods +end % classdef diff --git a/lwrserv/matlab/rvctools/common/Polygon.m b/lwrserv/matlab/rvctools/common/Polygon.m new file mode 100644 index 0000000..4490a96 --- /dev/null +++ b/lwrserv/matlab/rvctools/common/Polygon.m @@ -0,0 +1,1328 @@ +%POLYGON Polygon class +% +% A general class for manipulating polygons and vectors of polygons. +% +% Methods:: +% plot Plot polygon +% area Area of polygon +% moments Moments of polygon +% centroid Centroid of polygon +% perimeter Perimter of polygon +% transform Transform polygon +% inside Test if points are inside polygon +% intersection Intersection of two polygons +% difference Difference of two polygons +% union Union of two polygons +% xor Exclusive or of two polygons +% display print the polygon in human readable form +% char convert the polgyon to human readable string +% +% Properties:: +% vertices List of polygon vertices, one per column +% extent Bounding box [minx maxx; miny maxy] +% n Number of vertices +% +% Notes:: +% - This is reference class object +% - Polygon objects can be used in vectors and arrays +% +% Acknowledgement:: +% +% The methods inside, intersection, difference, union, and xor are based on code +% written by: +% Kirill K. Pankratov, kirill@plume.mit.edu, +% http://puddle.mit.edu/~glenn/kirill/saga.html +% and require a licence. However the author does not respond to email regarding +% the licence, so use with care, and modify with acknowledgement. + +% TODO +% split the code in two. Simple polygon functions in Polgon class, subclass with +% Pankratov code to Polygon2. + +classdef Polygon < handle + + properties + vertices + extent + end + + properties (Dependent=true) + n + x + y + end + + methods + + function p = Polygon(v, wh) + %Polygon.Polygon Polygon class constructor + % + % P = Polygon(V) is a polygon with vertices given by V, one column per + % vertex. + % + % P = Polygon(C, WH) is a rectangle centred at C with dimensions + % WH=[WIDTH, HEIGHT]. + + if nargin == 0 + p.n = 0; + p.vertices = []; + return; + end + + if nargin < 2 + if numrows(v) ~= 2 + error('vertices must have two rows'); + end + p.vertices = v; + end + if nargin == 2 + if length(v) ~= 2 + error('first argument must be polygon centre'); + end + if length(wh) ~= 2 + error('second arugment must be width height'); + end + + p.vertices = [ + v(1)-wh(1)/2 v(1)+wh(1)/2 v(1)+wh(1)/2 v(1)-wh(1)/2 + v(2)-wh(2)/2 v(2)-wh(2)/2 v(2)+wh(2)/2 v(2)+wh(2)/2 ]; + end + + % compute the extent + p.extent(1,1) = min(p.x); + p.extent(1,2) = max(p.x); + p.extent(2,1) = min(p.y); + p.extent(2,2) = max(p.y); + end + + function r = get.n(p) + r = numcols(p.vertices); + end + + function r = get.x(p) + r = p.vertices(1,:)'; + end + + function r = get.y(p) + r = p.vertices(2,:)'; + end + + function r = set.n(p) + error('cant set property'); + end + function r = set.x(p) + error('cant set property'); + end + function r = set.y(p) + error('cant set property'); + end + + function s = char(p) + %Polygon.char String representation + % + % S = P.char() is a compact representation of the polgyon in human + % readable form. + s = sprintf('%d vertices', p.n); + end + + function display(p) + %Polygon.display Display polygon + % + % P.display() displays the polygon in a compact human readable form. + % + % See also Polygon.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + if loose + disp(' '); + end + disp(char(p)) + if loose + disp(' '); + end + end + + function plot(plist, varargin) + %Polygon.plot Plot polygon + % + % P.plot() plot the polygon. + % + % P.plot(LS) as above but pass the arguments LS to plot. + + opt.fill = []; + [opt,args] = tb_optparse(opt, varargin); + + ish = ishold + hold all + + for p=plist + % for every polygon in the list + + % get the vertices + X = p.vertices(1,:)'; + Y = p.vertices(2,:)'; + + while true + + % look for NaNs which indicate disjoint vertex sets + k = find(isnan(X)); + + if length(k) > 0 + % if a NaN chop out the segment before and after + k = k(1); + x = X(1:k-1); + y = Y(1:k-1); + X = X(k+1:end); + Y = Y(k+1:end); + else + x = X; y = Y; + end + + % close the polygon + x = [x; x(1)]; + y = [y; y(1)]; + + if opt.fill + patch(x, y, opt.fill); + else + plot(x, y, args{:}); + end + + if length(k) == 0 + break; + end + end + end + hold(ish) + end + + function a = area(p) + %Polygon.area Area of polygon + % + % A = P.area() is the area of the polygon. + a = p.moments(0, 0); + end + + function m = moments(p, mp, mq) + %Polygon.moments Moments of polygon + % + % A = P.moments(p, q) is the pq'th moment of the polygon. + % + % See also mpq_poly. + m = mpq_poly(p.vertices, mp, mq); + end + + function q = transform(p, T) + %Polygon.transform Transformation of polygon vertices + % + % P2 = P.transform(T) is a new Polygon object whose vertices have + % been transfored by the 3x3 homgoeneous transformation T. + if length(T) == 3 + T = se2(T); + end + q = Polygon( homtrans(T, p.vertices) ); + end + + function f = inside(p, points) + %Polygon.inside Test if points are inside polygon + % + % IN = P.inside(P) tests if points given by columns of P are + % inside the polygon. The corresponding elements of IN are + % either true or false. + IN = inpolygon(points(1,:), points(2,:), p.x, p.y) + end + + function c = centroid(p) + %Polygon.centroid Centroid of polygon + % + % X = P.centroid() is the centroid of the polygon. + xc = p.moments(1,1) / p.area(); + end + + function r = perimeter(p) + %Polygon.perimeter Perimeter of polygon + % + % L = P.perimeter() is the perimeter of the polygon. + p = sum(sqrt(diff(p.x).^2+diff(p.y).^2)); + end + + function f = intersect(p, plist) + %Polygon.intersect Intersection of polygon with list of polygons + % + % I = P.intersect(PLIST) indicates whether or not the Polygon P + % intersects with + % + % i(j) = 1 if p intersects polylist(j), else 0. + + % Based on ISINTPL + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 06/20/95, 08/25/95 + f = []; + for q=plist + f = [f isintpl(p.x, p.y, q.x, q.y)]; + end + end + + function f = intersect_line(p, l) + %Polygon.intersect_line Intersection of polygon and line segment + % + % I = P.intersect_line(L) is the intersection points of a polygon P + % with the line segment L=[x1 x2; y1 y2]. I is an Nx2 matrix with + % one column per intersection, each column is [x y]'. + + f = []; + % find intersections + for i=1:p.n + in = mod(i, p.n)+1; + + xv = [p.x(i); p.x(in)]; + yv = [p.y(i); p.y(in)]; + intsec = iscross(xv, yv, l(1,:)', l(2,:)'); + if intsec + [x,y] = intsecl(xv, yv, l(1,:)', l(2,:)'); + f = [f [x;y]]; + end + end + end + + function r = difference(p, q) + %Polygon.difference Difference of polygons + % + % D = P.difference(Q) is polygon P minus polygon Q. + % + % Notes:: + % - If polygons P and Q are not intersecting, returns + % coordinates of P. + % - If the result D is not simply connected or consists of + % several polygons, resulting vertex list will contain NaNs. + + % POLYDIFF Difference of 2 polygons. + % [XO,YO] = POLYDIFF(X1,Y1,X2,Y2) Calculates polygon(s) P + % of difference of polygons P1 and P1 with coordinates + % X1, Y1 and X2, Y2. + % The resulting polygon(s) is a set of all points which belong + % to P1 but not to to P2: P = P1 & ~P2. + % The input polygons must be non-self-intersecting and + % simply connected. + % + % If polygons P1, P2 are not intersecting, returns + % coordinates of the first polygon X1, X2. + % If the result P is not simply connected or consists of several + % polygons, resulting boundary consists of concatenated + % coordinates of these polygons, separated by NaN. + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 06/25/95 + + % Call POLYBOOL with flag=3 + [xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 3); + + r = Polygon([xo(:) yo(:)]'); + end + + function r = intersection(p, q) + %Polygon.intersection Intersection of polygons + % + % I = P.intersection(Q) is a Polygon representing the + % intersection of polygons P and Q. + % + % Notes:: + % - If these polygons are not intersecting, returns empty polygon. + % - If intersection consist of several disjoint polygons + % (for non-convex P or Q) then vertices of I is the concatenation + % of the vertices of these polygons. + + + % POLYINTS Intersection of 2 polygons. + % [XO,YO] = POLYINTS(X1,Y1,X2,Y2) Calculates polygon(s) + % if intersection of polygons with coordinates X1, Y1 + % and X2, Y2. + % The resulting polygon(s) is a set of all points which + % belong to both P1 and P2: P = P1 & P2. + % These polygons must be non-self-intersecting and + % simply connected. + % + % If these polygons are not intersecting, returns empty. + % If intersection consist of several disjoint polygons + % (for non-convex P1 or P2) output vectors XO, YO consist + % of concatenated cooddinates of these polygons, + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 06/25/95 + + + % Call POLYBOOL with flag=1 + [xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 1); + + + r = Polygon([xo(:) yo(:)]'); + end + + function r = union(p, q) + %Polygon.union Union of polygons + % + % I = P.union(Q) is a Polygon representing the + % union of polygons P and Q. + % + % Notes:: + % - If these polygons are not intersecting, returns a polygon with + % vertices of both polygons separated by NaNs. + % - If the result P is not simply connected (such as a polygon + % with a "hole") the resulting contour consist of counter- + % clockwise "outer boundary" and one or more clock-wise + % "inner boundaries" around "holes". + + % POLYUNI Union of 2 polygons. + % [XO,YO] = POLYINT(X1,Y1,X2,Y2) Calculates polygon(s) P + % which is (are) union of polygons P1 and P2 with coordinates + % X1, Y1 and X2, Y2. + % The resulting polygon(s) is a set of all points which belong + % either to P1 or to P2: P = P1 | P2. + % The input polygons must be non-self-intersecting and + % simply connected. + % + % If polygons P1, P2 are not intersecting, returns + % coordinates of the both polygons separated by NaN. + % If both P1 and P2 are convex, their boundaries can have no + % more than 2 intersections. The result is also a convex + % polygon. + % If the result P is not simply connected (such as a polygon + % with a "hole") the resulting contour consist of counter- + % clockwise "outer boundary" and one or more clock-wise + % "inner boundaries" around "holes". + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 06/25/95 + + + % Call POLYBOOL with flag=2 .......... + [xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 2); + + + r = Polygon([xo(:) yo(:)]'); + end + + function r = xor(p, q) + %Polygon.xor Exclusive or of polygons + % + % I = P.union(Q) is a Polygon representing the + % union of polygons P and Q. + % + % Notes:: + % - If these polygons are not intersecting, returns a polygon with + % vertices of both polygons separated by NaNs. + % - If the result P is not simply connected (such as a polygon + % with a "hole") the resulting contour consist of counter- + % clockwise "outer boundary" and one or more clock-wise + % "inner boundaries" around "holes". + + % POLYXOR Exclusive OR of 2 polygons. + % [XO,YO] = POLYXOR(X1,Y1,X2,Y2) Calculates polygon(s) P + % of difference of polygons P1 and P1 with coordinates + % X1, Y1 and X2, Y2. + % The resulting polygon(s) is a set of all points which belong + % either to P1 or to P2 but not to both: + % P = (P1 & ~P2) | (P2 & ~P1). + % The input polygons must be non-self-intersecting and + % simply connected. + % + % If polygons P1, P2 are not intersecting, returns + % coordinates of the both polygons separated by NaN. + % If the result P is not simply connected or consists of several + % polygons, resulting boundary consists of concatenated + % coordinates of these polygons, separated by NaN. + + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 06/25/95 + + + % Call POLYBOOL twice with flag=3 + [xx,yy,ind] = polybool(p.x, p.y, q.x, q.y, 3); + + xo = [xx; NaN]; yo = [yy; NaN]; + [xx,yy,ind] = polybool(q.x, q.y, p.x, p.y, 3); + + xo = [xo; xx]; yo = [yo; yy]; + + r = Polygon([xo(:) yo(:)]'); + + + end + end % methods +end % classdef + + +function [is,in,un] = interval(x1,x2) + + % Intersection and union of 2 intervals. + % [IS,IN,UN] = INTERVAL(X1,X2) calculates pair-wise + % intersection IN and union UN of N pairs of + % intervals with coordinates X1 and X2 (both are + % 2 by N vectors). Returns 1 by N boolean vector IS + % equal to 1 if intervals have non-empty intersection + % and 0 if they don't. + + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 08/24/95 + + % Handle input ........................... + if nargin==0, help interval, return, end + if nargin==1 + un = x1; + else + un = [x1; x2]; + end + + [in,un] = sort(un); % Sort both intervals together + un = un(1:2,:)-1; + is = sum(floor(un/2)); % Check for [0 0 1 1] or [1 1 0 0] + is = (is==1); + ii = find(in(2,:)==in(3,:)); + is(ii) = .5*ones(size(ii)); + + % Extract intersection and union from sorted coordinates + if nargout>1 + un = in([1 4],:); + in = in(2:3,:); + in(:,~is) = flipud(in(:,~is)); + end +end + +function [is,S] = iscross(x1,y1,x2,y2,tol) + + % ISCROSS Finds whether pairs of lines cross each other + % [IS,S] = ISCROSS(X1,Y1,X2,Y2) where arguments X1, Y1, + % X2, Y2 are all 2 by N matrices are coordinates of + % ends of the pairs of line segments. + % Returns vector IS (1 by N) consisting of ones if + % corresponding pairs cross each other, zeros if they + % don't and .5 if an end of one line segment lies on + % another segment. + % Also returns a matrix S (4 by N) with each row + % consisting of cross products (double areas of + % corresponding triangles) built on the following points: + % (X2(1,:),Y2(1,:)),(X1(1,:),Y1(1,:)),(X2(2,:),Y2(2,:)), + % (X2(1,:),Y2(1,:)),(X1(2,:),Y1(2,:)),(X2(2,:),Y2(2,:)) + % (X1(1,:),Y1(1,:)),(X2(1,:),Y2(1,:)),(X1(2,:),Y1(2,:)) + % (X1(1,:),Y1(1,:)),(X2(2,:),Y2(2,:)),(X1(2,:),Y1(2,:)) + % The signs of these 4 areas can be used to determine + % whether these lines and their continuations cross each + % other. + % [IS,S] = ISCROSS(X1,Y1,X2,Y2,TOL) uses tolerance TOL + % for detecting the crossings (default is 0). + + % Copyright (c) 1995 by Kirill K. Pankratov + % kirill@plume.mit.edu + % 08/14/94, 05/18/95, 08/25/95 + + % Defaults and parameters ....................... + tol_dflt = 0; % Tolerance for area calculation + is_chk = 1; % Check input arguments + + % Handle input .................................. + if nargin==0, help iscross, return, end + if nargin<4 % Check if all 4 entered + error(' Not enough input arguments') + end + if nargin<5, tol = tol_dflt; end + if tol < 0, is_chk = 0; tol = 0; end + + % Check the format of arguments ................. + if is_chk + [x1,y1,x2,y2] = linechk(x1,y1,x2,y2); + end + + len = size(x1,2); + o2 = ones(2,1); + + % Find if the ranges of pairs of segments intersect + [isx,S,A] = interval(x1,x2); + scx = diff(A); + [isy,S,A] = interval(y1,y2); + scy = diff(A); + is = isx & isy; + + % If S values are not needed, extract only those pairs + % which have intersecting ranges .............. + if nargout < 2 + isx = find(is); % Indices of pairs to be checked + % further + x1 = x1(:,isx); + x2 = x2(:,isx); + y1 = y1(:,isx); + y2 = y2(:,isx); + is = is(isx); + if isempty(is), is = zeros(1,len); return, end + scx = scx(isx); + scy = scy(isx); + end + + % Rescale by ranges ........................... + x1 = x1.*scx(o2,:); + x2 = x2.*scx(o2,:); + y1 = y1.*scy(o2,:); + y2 = y2.*scy(o2,:); + + + % Calculate areas ............................. + S = zeros(4,length(scx)); + S(1,:) = (x2(1,:)-x1(1,:)).*(y2(2,:)-y1(1,:)); + S(1,:) = S(1,:)-(x2(2,:)-x1(1,:)).*(y2(1,:)-y1(1,:)); + + S(2,:) = (x2(1,:)-x1(2,:)).*(y2(2,:)-y1(2,:)); + S(2,:) = S(2,:)-(x2(2,:)-x1(2,:)).*(y2(1,:)-y1(2,:)); + + S(3,:) = (x1(1,:)-x2(1,:)).*(y1(2,:)-y2(1,:)); + S(3,:) = S(3,:)-(x1(2,:)-x2(1,:)).*(y1(1,:)-y2(1,:)); + + S(4,:) = (x1(1,:)-x2(2,:)).*(y1(2,:)-y2(2,:)); + S(4,:) = S(4,:)-(x1(2,:)-x2(2,:)).*(y1(1,:)-y2(2,:)); + + + % Find if they cross each other ............... + is = (S(1,:).*S(2,:)<=0)&(S(3,:).*S(4,:)<=0); + + + % Find very close to intersection + isy = min(abs(S)); + ii = find(isy<=tol & is); + is(ii) = .5*ones(size(ii)); + + % Output + if nargout < 2 + isy = zeros(1,len); + isy(isx) = is; + is = isy; + + else + isy = scx.*scy; + ii = find(~isy); + isy(ii) = ones(size(ii)); + S = S./isy(ones(4,1),:); + + end + +end + +function [xo,yo,ind] = polybool(x1,y1,x2,y2,flag) + + % [XO,YO] = POLYBOOL(X1,Y1,X2,Y2,FLAG) + % calulates results of Boolean operations on + % a pair of polygons. + % FLAG Specifies the type of the operation: + % 1 - Intersection (P1 & P2) + % 2 - Union (P1 | P2) + % 3 - Difference (P1 & ~P2) + + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 06/25/95, 09/07/95 + + % This program calls the following functions: + % AREA, ISINTPL, ISCROSS, INTSECL. + + % Algorithm: + % 1. Check boundary contour directions (area). + % For intersection and union make all + % counter-clockwise. For difference make the second + % contour clock-wise. + % 2. Calculate matrix of intersections (function ISINTPL). + % Quick exit if no intersections. + % 3. For intersecting segments calculate intersection + % coordinates (function INTSECL). + % 4. Sort intersections along both contours. + % 5. Calculate sign of cross-product between intersectiong + % segments. This will give which contour goes "in" and + % "out" at intersections. + % + % 6. Start with first intersection: + % Determine direction to go ("in" for intersection, + % "out" for union). + % Move until next intersection, switch polygons at each + % intersection until coming to the initial point. + % If not all intersections are encountered, the + % resulting polygon is disjoint. Separate output + % coordinates by NaN and repeat procedure until all + % intersections are counted. + + % Default for flag + flag_dflt = 1; % 1- intersec., 2-union, 3 - diff. + + % Handle input + if nargin==0, help polybool, return, end + if nargin < 4 + error(' Not enough input arguments') + end + if nargin<5, flag = flag_dflt; end + + x1 = x1(:); y1 = y1(:); + x2 = x2(:); y2 = y2(:); + l1 = length(x1); + l2 = length(x2); + + % Check areas and reverse if negative + nn1 = area(x1,y1); + if nn1<0, x1 = flipud(x1); y1 = flipud(y1); end + nn2 = area(x2,y2); + if (nn2<0 & flag<3) | (nn2>0 & flag==3) + x2 = flipud(x2); y2 = flipud(y2); + end + + % If both polygons are identical ........ + if l1==l2 + if all(x1==x2) & all(y1==y2) + if flag<3, xo = x1; yo = y1; ind = 1:l1; + else, xo = []; yo = []; ind = []; end + return + end + end + + % Calculate matrix of intersections ..... + [is,C] = isintpl(x1,y1,x2,y2); + is = any(any(C)); + + % Quick exit if no intersections ........ + if ~is + if flag==1 % Intersection + xo=[]; yo = []; + elseif flag==2 % Union + xo = [x1; nan; x2]; + yo = [y1; nan; y2]; + elseif flag==3 % Difference + xo = x1; yo = y1; + end + return + end + + % Mark intersections with unique numbers + i1 = find(C); + ni = length(i1); + C(i1) = 1:ni; + + % Close polygon contours + x1 = [x1; x1(1)]; y1 = [y1; y1(1)]; + x2 = [x2; x2(1)]; y2 = [y2; y2(1)]; + l1 = length(x1); l2 = length(x2); + + % Calculate intersections themselves + [i1,i2,id] = find(C); + xs1 = [x1(i1) x1(i1+1)]'; ys1 = [y1(i1) y1(i1+1)]'; + xs2 = [x2(i2) x2(i2+1)]'; ys2 = [y2(i2) y2(i2+1)]'; + + % Call INTSECL ............................ + [xint,yint] = intsecl(xs1,ys1,xs2,ys2); + + % For sements belonging to the same line + % find interval of intersection ........... + ii = find(xint==inf); + if ~isempty(ii) + [is,inx] = interval(xs1(:,ii),xs2(:,ii)); + [is,iny] = interval(ys1(:,ii),ys2(:,ii)); + xint(ii) = mean(inx); + yint(ii) = mean(iny); + end + + % Coordinate differences of intersecting segments + xs1 = diff(xs1); ys1 = diff(ys1); + xs2 = diff(xs2); ys2 = diff(ys2); + + % Calculate cross-products + cp = xs1.*ys2-xs2.*ys1; + cp = cp>0; + if flag==2, cp=~cp; end % Reverse if union + cp(ii) = 2*ones(size(ii)); + + % Sort intersections along the contours + ind = (xint-x1(i1)').^2+(yint-y1(i1)').^2; + ind = ind./(xs1.^2+ys1.^2); + cnd = min(ind(ind>0)); + ind = ind+i1'+i2'/(ni+1)*cnd*0; + [xo,ii] = sort(ind); + xs1 = id(ii); + [xo,ind] = sort(xs1); + ind = rem(ind,ni)+1; + xs1 = xs1(ind); + + ind = (xint-x2(i2)').^2+(yint-y2(i2)').^2; + ind = ind./(xs2.^2+ys2.^2); + cnd = min(ind(ind>0)); + [xo,ii] = sort(i2'+ind+i1'/(ni+1)*cnd*0); + xs2 = id(ii); + [xo,ind] = sort(xs2); + ind = rem(ind,ni)+1; + xs2 = xs2(ind); + + % Combine coordinates in one vector + x1 = [x1; x2]; y1 = [y1; y2]; + + % Find max. possible length of a chain + xo = find(any(C')); + xo = diff([xo xo(1)+l1]); + mlen(1) = max(xo); + xo = find(any(C)); + xo = diff([xo xo(1)+l2]); + mlen(2) = max(xo); + + % Check if multiple intersections in one segment + xo = diff([i1 i2]); + is_1 = ~all(all(xo)); + + % Begin counting intersections ********************* + + % Initialization .................. + int = zeros(size(xint)); + nn = 1; % First intersection + nn1 = i1(nn); nn2 = i2(nn); + b = cp(nn); + is2 = b==2; + xo = []; yo = []; ind = []; + closed = 0; + + % Proceed until all intersections are counted + while ~closed % begin counting `````````````````````0 + + % If contour closes, find new starting point + if int(nn) & ~all(int) + ii = find(int); + C(id(ii)) = zeros(size(ii)); + nn = min(find(~int)); % Next intersection + nn1 = i1(nn); + nn2 = i2(nn); + xo = [xo; nan]; % Separate by NaN + yo = [yo; nan]; + ind = [ind; nan]; + % Choose direction ...... + b = cp(nn); + end + + % Add current intersection ...... + xo = [xo; xint(nn)]; + yo = [yo; yint(nn)]; + ind = [ind; 0]; + int(nn) = 1; + closed = all(int); + + % Find next segment + % Indices for next intersection + if ~b, nn = xs1(nn); + else, nn = xs2(nn); + end + if ~b, pt0 = nn1; else, pt0 = nn2; end + + nn1 = i1(nn); + nn2 = i2(nn); + + if b, pt = nn2; else, pt = nn1; end + + if b, pt0 = pt0+l1; pt = pt+l1; end + ii = (pt0+1:pt); + + + % Go through the beginning .............. + cnd = pt1); + if cnd + if ~b, ii = [pt0+1:l1 1:pt]; + else, ii = [pt0+1:l1+l2 l1+1:pt]; + end + end + len = length(ii); + cnd = b & len>mlen(2); + cnd = cnd | (~b & len>mlen(1)); + if is2 | cnd, ii=[]; end + + + % Add new segment + xo = [xo; x1(ii)]; + yo = [yo; y1(ii)]; + ind = [ind; ii']; + + % Switch direction + if cp(nn)==2, b = ~b; is2 = 1; + else, b = cp(nn); is2 = 0; + end + + end % End while (all intersections) '''''''''''''''0 + + % Remove coincident successive points + ii = find(~diff(xo) & ~diff(yo)); + xo(ii) = []; yo(ii) = []; ind(ii) = []; + + % Remove points which are + ii = find(isnan(xo)); + if ~isempty(ii) + i2 = ones(size(xo)); + ii = [ii; length(xo)+1]; + + i1 = find(diff(ii)==3); + i1 = ii(i1); + i1 = [i1; i1+1; i1+2]; + i2(i1) = zeros(size(i1)); + + i1 = find(diff(ii)==2); + i1 = ii(i1); + i1 = [i1; i1+1]; + i2(i1) = zeros(size(i1)); + + xo = xo(i2); yo = yo(i2); ind = ind(i2); + end +end + +function [xo,yo] = intsecpl(xv,yv,xl,yl,trace) + + % INTSECPL Intersection of a polygon and a line. + % [XI,YI] = INTSECPL(XV,YV,XL,YL) calculates + % intersections XI, YI of a polygon with vertices XV, + % YV and a line specified by pairs of end coordinates + % XL = [XL0 XL1], YL = [YL0 YL1]. Line is assumed to + % continue beyond the range of end points. + % INTSECPL(XV,YV,[A B]) uses another specification for + % a line: Y = A*X+B. + % + % If a line does not intersect polygon, returns empty + % XI, YI. + % For convex polygon maximum number of intersections is + % 2, for non-convex polygons multiple intersections are + % possible. + % + % INTSECPL(XV,YV,XL,YL) by itself or + % [XI,YI] = INTSECPL(XV,YV,XL,YL,1) plots polygon, + % a line segment and intersection segment(s) + % (part(s) of the same line inside the polygon). + + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 06/25/95, 08/27/95, 09/27/95 + + % Calls ISCROSS, INTSECL programs. + + + % Defaults and parameters ................................. + tol = 1e-14; % Tolerance + marg = tol; % Margins for polygon frame + is_ab = 0; % Default A*X+B mode + + % Handle input ............................................ + if nargin==0, help intsecpl, return, end + if nargin < 3 + error(' Not enough input arguments') + end + if nargin<5, trace = 0; end + if nargin==4 % Check if 4-th arg is trace + if max(size(yl))==1, trace = yl; is_ab = 1; end + end + if nargin==3, is_ab = 1; end + trace = trace | nargin<2; + if length(xv)~=length(yv) + error(' Vectors X, Y must have the same size') + end + + % Auxillary ........... + xv = [xv(:); xv(1)]; + yv = [yv(:); yv(1)]; + ii = find(abs(diff(xv))1 & 0 % Do not execute + xx = [xo yo]; + yy = diff(xx)'; + ii = [1 find(any(abs(yy)>tol))+1]; + xo = xx(ii,1); yo = xx(ii,2); + oi = ones(size(xo)); + end + + + % Plotting ................................................ + if trace + oi(3:2:length(oi)) = oi(3:2:length(oi))+1; + oi = cumsum(oi); + len = max(oi); + xp = nan*ones(len,1); yp = xp; + xp(oi) = xo; + yp(oi) = yo; + + % Intersection with polygon frame + [xl,yl] = intsecpl(lim([1 2 2 1]),lim([3 3 4 4]),xl,yl); + + plot(xv,yv,xl,yl,xp,yp) % Plotting itself + end +end + +function [is,S] = isintpl(x1,y1,x2,y2) + + % ISINTPL Check for intersection of polygons. + % [IS,S] = ISINTPL(X1,Y1,X2,Y2) Accepts coordinates + % X1,Y1 and X2, Y2 of two polygons. Returns scalar + % IS equal to 1 if these polygons intersect each other + % and 0 if they do not. + % Also returns Boolean matrix S of the size length(X1) + % by length(X2) so that {ij} element of which is 1 if + % line segments i to i+1 of the first polygon and + % j to j+1 of the second polygon intersect each other, + % 0 if they don't and .5 if they "touch" each other. + + % Copyright (c) 1995 by Kirill K. Pankratov, + % kirill@plume.mit.edu. + % 06/20/95, 08/25/95 + + + % Handle input ................................... + if nargin==0, help isintpl, return, end + if nargin<4 + error(' Not enough input arguments ') + end + + % Make column vectors and check sizes ............ + x1 = x1(:); y1 = y1(:); + x2 = x2(:); y2 = y2(:); + l1 = length(x1); + l2 = length(x2); + if length(y1)~=l1 | length(y2)~=l2 + error('(X1,Y1) and (X2,Y2) must pair-wise have the same length.') + end + + % Quick exit if empty + if l1<1 | l2<1, is = []; S = []; return, end + + % Check if their ranges are intersecting ......... + lim1 = [min(x1) max(x1)]'; + lim2 = [min(x2) max(x2)]'; + isx = interval(lim1,lim2); % X-ranges + lim1 = [min(y1) max(y1)]'; + lim2 = [min(y2) max(y2)]'; + isy = interval(lim1,lim2); % Y-ranges + is = isx & isy; + S = zeros(l2,l1); + + if ~is, return, end % Early exit if disparate limits + + % Indexing ....................................... + [i11,i12] = meshgrid(1:l1,1:l2); + [i21,i22] = meshgrid([2:l1 1],[2:l2 1]); + i11 = i11(:); i12 = i12(:); + i21 = i21(:); i22 = i22(:); + + % Calculate matrix of intersections .............. + S(:) = iscross([x1(i11) x1(i21)]',[y1(i11) y1(i21)]',... + [x2(i12) x2(i22)]',[y2(i12) y2(i22)]')'; + + S = S'; + is = any(any(S)); +end + +function [xo,yo] = intsecl(x1,y1,x2,y2,tol) + + % INTSECL Intersection coordinates of two line segments. + % [XI,YI] = INTSECL(X1,Y1,X2,Y2) where all 4 + % arguments are 2 by N matrices with coordinates + % of ends of N pairs of line segments (so that + % the command such as PLOT(X1,Y1,X2,Y2) will plot + % these pairs of lines). + % Returns 1 by N vectors XI and YI consisting of + % coordinates of intersection points of each of N + % pairs of lines. + % + % Special cases: + % When a line segment is degenerate into a point + % and does not lie on line through the other segment + % of a pair returns XI=NaN while YI has the following + % values: 1 - when the first segment in a pair is + % degenerate, 2 - second segment, 0 - both segments + % are degenerate. + % When a pair of line segments is parallel, returns + % XI = Inf while YI is 1 for coincident lines, + % 0 - for parallel non-coincident ones. + % INTSECL(X1,Y1,X2,Y2,TOL) also specifies tolerance + % in detecting coincident points in different line + % segments. + + % Copyright (c) 1995 by Kirill K. Pankratov + % kirill@plume.mit.edu + % 04/15/94, 08/14/94, 05/10/95, 08/23/95 + + + % Defaults and parameters ......................... + tol_dflt = 0; % Tolerance for coincident points + is_chk = 1; % Check input arguments + + % Handle input .................................... + if nargin==0, help intsecl, return, end + if nargin<4 % Check if all 4 entered + error(' Not enough input arguments') + end + if nargin<5, tol = tol_dflt; end + if tol < 0, is_chk = 0; tol = 0; end + + % Check the format of arguments ....... + if is_chk + [x1,y1,x2,y2] = linechk(x1,y1,x2,y2); + end + + + % Auxillary + o2 = ones(2,1); + i_pt1 = []; i_pt2 = []; i_pt12 = []; + + % Make first points origins ........... + xo = x1(1,:); + yo = y1(1,:); + x2 = x2-xo(o2,:); + y2 = y2-yo(o2,:); + + % Differences of first segments ....... + a = x1(2,:)-x1(1,:); + b = y1(2,:)-y1(1,:); + s = sqrt(a.^2+b.^2); % Lengths of first segments + i_pt1 = find(~s); + s(i_pt1) = ones(size(i_pt1)); + rr = rand(size(i_pt1)); + a(i_pt1) = cos(rr); + b(i_pt1) = sin(rr); + + % Normalize by length ................. + a = a./s; b = b./s; + + % Rotate coordinates of the second segment + tmp = x2.*a(o2,:)+y2.*b(o2,:); + y2 = -x2.*b(o2,:)+y2.*a(o2,:); + x2 = tmp; + + % Calculate differences in second segments + s = x2(2,:)-x2(1,:); + tmp = y2(2,:)-y2(1,:); + cc = tmp(i_pt1); + + % Find some degenerate cases ....................... + + % Find zeros in differences + izy2 = find(~tmp); + tmp(izy2) = ones(size(izy2)); + + % Find degenerate and parallel segments + bool = ~s(izy2); + i_par = izy2(~bool); + i_pt2 = izy2(bool); + + bool = abs(y2(1,i_pt2))<=tol; + i_pt2_off = i_pt2(~bool); + i_pt2_on = i_pt2(bool); + + if ~isempty(i_par) + bool = abs(y2(1,i_par))<=tol; + i_par_off = i_par(~bool); + i_par_on = i_par(bool); + end + + % Calculate intercept with rotated x-axis .......... + tmp = s./tmp; % Slope + tmp = x2(1,:)-y2(1,:).*tmp; + + + % Rotate and translate back to original coordinates + xo = tmp.*a+xo; + yo = tmp.*b+yo; + + % Mark special cases ................................... + % First segments are degenerate to points + if ~isempty(i_pt1) + bool = ~s(i_pt1) & ~cc; + i_pt12 = i_pt1(bool); + i_pt1 = i_pt1(~bool); + + bool = abs(tmp(i_pt1))<=tol; + i_pt1_on = i_pt1(bool); + i_pt1_off = i_pt1(~bool); + + xo(i_pt1_on) = x1(1,i_pt1_on); + yo(i_pt1_on) = y1(1,i_pt1_on); + + oo = ones(size(i_pt1_off)); + xo(i_pt1_off) = nan*oo; + yo(i_pt1_off) = oo; + end + + % Second segments are degenerate to points ... + if ~isempty(i_pt2) + oo = ones(size(i_pt2_off)); + xo(i_pt2_off) = nan*oo; + yo(i_pt2_off) = 2*oo; + end + + % Both segments are degenerate ............... + if ~isempty(i_pt12) + bool = x1(i_pt12)==xo(i_pt12); + i_pt12_on = i_pt12(bool); + i_pt12_off = i_pt12(~bool); + + xo(i_pt12_on) = x1(1,i_pt12_on); + yo(i_pt12_on) = y1(1,i_pt12_on); + + oo = ones(size(i_pt12_off)); + xo(i_pt12_off) = nan*oo; + yo(i_pt12_off) = 0*oo; + end + + % Parallel segments ......................... + if ~isempty(i_par) + oo = ones(size(i_par_on)); + xo(i_par_on) = inf*oo; + yo(i_par_on) = oo; + + oo = ones(size(i_par_off)); + xo(i_par_off) = inf*oo; + yo(i_par_off) = 0*oo; + end + + + +end + +function [x1,y1,x2,y2] = linechk(x1,y1,x2,y2) + % LINECHK Input checking for line segments. + + % Copyright (c) 1995 by Kirill K. Pankratov + % kirill@plume.mit.edu + % 08/22/95, + + % String for transposing + str = ['x1=x1'';'; 'y1=y1'';'; 'x2=x2'';'; 'y2=y2'';']; + + % Sizes + sz = [size(x1); size(y1); size(x2); size(y2)]'; + psz = prod(sz); + + % Check x1, y1 + if psz(1)~=psz(2) + error(' Arguments x1 and y1 must have the same size') + end + + % Check x2, y2 + if psz(3)~=psz(3) + error(' Arguments x2 and y2 must have the same size') + end + + % Check if any arguments are less than 2 by 1 + if any(max(sz)<2) + error(' Arguments x1, y1, x2, y2 must be at least 2 by 1 vectors') + end + + % Check if no size is equal to 2 + if any(all(sz~=2)) + error(' Arguments x1, y1, x2, y2 must be 2 by 1 vectors') + end + + % Find aruments to be transposed ............................. + ii = find(sz(1,:)~=2); + for jj = 1:length(ii) + eval(str(ii(jj),:)); % Transpose if neccessary + end + sz(:,ii) = flipud(sz(:,ii)); + + % If vectors, extend to 2 by n matrices ...................... + n = max(sz(2,:)); + on = ones(1,n); + if sz(2,1) 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 diff --git a/lwrserv/matlab/rvctools/common/arrow3.m b/lwrserv/matlab/rvctools/common/arrow3.m new file mode 100644 index 0000000..7eadbf5 --- /dev/null +++ b/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(N0) + 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 diff --git a/lwrserv/matlab/rvctools/common/bresenham.m b/lwrserv/matlab/rvctools/common/bresenham.m new file mode 100644 index 0000000..adf09be --- /dev/null +++ b/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 . + +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 diff --git a/lwrserv/matlab/rvctools/common/ccodefunctionstring.m b/lwrserv/matlab/rvctools/common/ccodefunctionstring.m new file mode 100644 index 0000000..2c83990 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/common/circle.m b/lwrserv/matlab/rvctools/common/circle.m new file mode 100644 index 0000000..3045ea3 --- /dev/null +++ b/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 diff --git a/lwrserv/matlab/rvctools/common/colnorm.m b/lwrserv/matlab/rvctools/common/colnorm.m new file mode 100644 index 0000000..6686dbc --- /dev/null +++ b/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)); diff --git a/lwrserv/matlab/rvctools/common/colorname.m b/lwrserv/matlab/rvctools/common/colorname.m new file mode 100644 index 0000000..87b75c8 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/common/diff2.m b/lwrserv/matlab/rvctools/common/diff2.m new file mode 100644 index 0000000..8a7716d --- /dev/null +++ b/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)]; diff --git a/lwrserv/matlab/rvctools/common/distributeblocks.m b/lwrserv/matlab/rvctools/common/distributeblocks.m new file mode 100644 index 0000000..0f596a2 --- /dev/null +++ b/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.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 + diff --git a/lwrserv/matlab/rvctools/common/dockfigs.m b/lwrserv/matlab/rvctools/common/dockfigs.m new file mode 100644 index 0000000..31a0b4c --- /dev/null +++ b/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.petercorke.com + +function dockfigs(arg) + if nargin == 0 + arg = 1; + end + + if arg + set(0, 'DefaultFigureWindowStyle', 'docked') + else + set(0, 'DefaultFigureWindowStyle', 'normal') + end diff --git a/lwrserv/matlab/rvctools/common/doesblockexist.m b/lwrserv/matlab/rvctools/common/doesblockexist.m new file mode 100644 index 0000000..096805f --- /dev/null +++ b/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.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 + diff --git a/lwrserv/matlab/rvctools/common/e2h.m b/lwrserv/matlab/rvctools/common/e2h.m new file mode 100644 index 0000000..76bc3b6 --- /dev/null +++ b/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))]; diff --git a/lwrserv/matlab/rvctools/common/edgelist.m b/lwrserv/matlab/rvctools/common/edgelist.m new file mode 100644 index 0000000..bcc78c7 --- /dev/null +++ b/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 diff --git a/lwrserv/matlab/rvctools/common/gauss2d.m b/lwrserv/matlab/rvctools/common/gauss2d.m new file mode 100644 index 0000000..03c429e --- /dev/null +++ b/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); diff --git a/lwrserv/matlab/rvctools/common/getprofilefunctionstats.m b/lwrserv/matlab/rvctools/common/getprofilefunctionstats.m new file mode 100644 index 0000000..574c949 --- /dev/null +++ b/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 + diff --git a/lwrserv/matlab/rvctools/common/h2e.m b/lwrserv/matlab/rvctools/common/h2e.m new file mode 100644 index 0000000..04b3c91 --- /dev/null +++ b/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); + diff --git a/lwrserv/matlab/rvctools/common/homline.m b/lwrserv/matlab/rvctools/common/homline.m new file mode 100644 index 0000000..e185ee0 --- /dev/null +++ b/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)); diff --git a/lwrserv/matlab/rvctools/common/homtrans.m b/lwrserv/matlab/rvctools/common/homtrans.m new file mode 100644 index 0000000..89cb43a --- /dev/null +++ b/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 . +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 diff --git a/lwrserv/matlab/rvctools/common/ishomog.m b/lwrserv/matlab/rvctools/common/ishomog.m new file mode 100644 index 0000000..689bd29 --- /dev/null +++ b/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 . + +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 diff --git a/lwrserv/matlab/rvctools/common/ishomog2.m b/lwrserv/matlab/rvctools/common/ishomog2.m new file mode 100644 index 0000000..4ff8f70 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/common/isrot.m b/lwrserv/matlab/rvctools/common/isrot.m new file mode 100644 index 0000000..d6db200 --- /dev/null +++ b/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 . + +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 diff --git a/lwrserv/matlab/rvctools/common/isrot2.m b/lwrserv/matlab/rvctools/common/isrot2.m new file mode 100644 index 0000000..c415f0e --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/common/isvec.m b/lwrserv/matlab/rvctools/common/isvec.m new file mode 100644 index 0000000..8e96111 --- /dev/null +++ b/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 . + +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 ); + diff --git a/lwrserv/matlab/rvctools/common/multidfprintf.m b/lwrserv/matlab/rvctools/common/multidfprintf.m new file mode 100644 index 0000000..19b8a45 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/common/numcols.m b/lwrserv/matlab/rvctools/common/numcols.m new file mode 100644 index 0000000..cedeebf --- /dev/null +++ b/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 . + +function c = numcols(m) + c = size(m,2); diff --git a/lwrserv/matlab/rvctools/common/numrows.m b/lwrserv/matlab/rvctools/common/numrows.m new file mode 100644 index 0000000..2107a1c --- /dev/null +++ b/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 . + +function r = numrows(m) + + [r,x] = size(m); diff --git a/lwrserv/matlab/rvctools/common/peak.m b/lwrserv/matlab/rvctools/common/peak.m new file mode 100644 index 0000000..28c66f8 --- /dev/null +++ b/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 . + +% 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 diff --git a/lwrserv/matlab/rvctools/common/peak2.m b/lwrserv/matlab/rvctools/common/peak2.m new file mode 100644 index 0000000..75774e0 --- /dev/null +++ b/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 . + +% 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 + + + + + + diff --git a/lwrserv/matlab/rvctools/common/plot2.m b/lwrserv/matlab/rvctools/common/plot2.m new file mode 100644 index 0000000..5817b6a --- /dev/null +++ b/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 diff --git a/lwrserv/matlab/rvctools/common/plot_arrow.m b/lwrserv/matlab/rvctools/common/plot_arrow.m new file mode 100644 index 0000000..f6a6470 --- /dev/null +++ b/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{:}); diff --git a/lwrserv/matlab/rvctools/common/plot_box.m b/lwrserv/matlab/rvctools/common/plot_box.m new file mode 100644 index 0000000..426dba4 --- /dev/null +++ b/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 . + +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 diff --git a/lwrserv/matlab/rvctools/common/plot_circle.m b/lwrserv/matlab/rvctools/common/plot_circle.m new file mode 100644 index 0000000..428be53 --- /dev/null +++ b/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 . +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 diff --git a/lwrserv/matlab/rvctools/common/plot_ellipse.m b/lwrserv/matlab/rvctools/common/plot_ellipse.m new file mode 100644 index 0000000..23e8435 --- /dev/null +++ b/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 diff --git a/lwrserv/matlab/rvctools/common/plot_ellipse_inv.m b/lwrserv/matlab/rvctools/common/plot_ellipse_inv.m new file mode 100644 index 0000000..780002d --- /dev/null +++ b/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.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.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 diff --git a/lwrserv/matlab/rvctools/common/plot_homline.m b/lwrserv/matlab/rvctools/common/plot_homline.m new file mode 100644 index 0000000..56374b6 --- /dev/null +++ b/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 . + +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 diff --git a/lwrserv/matlab/rvctools/common/plot_point.m b/lwrserv/matlab/rvctools/common/plot_point.m new file mode 100644 index 0000000..90de3f0 --- /dev/null +++ b/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 diff --git a/lwrserv/matlab/rvctools/common/plot_poly.m b/lwrserv/matlab/rvctools/common/plot_poly.m new file mode 100644 index 0000000..82aa516 --- /dev/null +++ b/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 diff --git a/lwrserv/matlab/rvctools/common/plot_sphere.m b/lwrserv/matlab/rvctools/common/plot_sphere.m new file mode 100644 index 0000000..835c64f --- /dev/null +++ b/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 diff --git a/lwrserv/matlab/rvctools/common/plotp.m b/lwrserv/matlab/rvctools/common/plotp.m new file mode 100644 index 0000000..cf34c0d --- /dev/null +++ b/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 diff --git a/lwrserv/matlab/rvctools/common/polydiff.m b/lwrserv/matlab/rvctools/common/polydiff.m new file mode 100644 index 0000000..d3cadf1 --- /dev/null +++ b/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); diff --git a/lwrserv/matlab/rvctools/common/randinit.m b/lwrserv/matlab/rvctools/common/randinit.m new file mode 100644 index 0000000..56424ac --- /dev/null +++ b/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() + diff --git a/lwrserv/matlab/rvctools/common/runscript.m b/lwrserv/matlab/rvctools/common/runscript.m new file mode 100644 index 0000000..5085fa6 --- /dev/null +++ b/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.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 + diff --git a/lwrserv/matlab/rvctools/common/rvcpath.m b/lwrserv/matlab/rvctools/common/rvcpath.m new file mode 100644 index 0000000..dd42bdb --- /dev/null +++ b/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.petercorke.com +function p = rvcpath() + p = fileparts( which('startup_rvc.m') ); diff --git a/lwrserv/matlab/rvctools/common/simulinkext.m b/lwrserv/matlab/rvctools/common/simulinkext.m new file mode 100644 index 0000000..fdd0a93 --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/common/symexpr2slblock.m b/lwrserv/matlab/rvctools/common/symexpr2slblock.m new file mode 100644 index 0000000..cc978dd --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/common/tb_optparse.m b/lwrserv/matlab/rvctools/common/tb_optparse.m new file mode 100644 index 0000000..ea4127d --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/common/xaxis.m b/lwrserv/matlab/rvctools/common/xaxis.m new file mode 100644 index 0000000..0a1c29f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/common/xyzlabel.m b/lwrserv/matlab/rvctools/common/xyzlabel.m new file mode 100644 index 0000000..6aa263b --- /dev/null +++ b/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'); diff --git a/lwrserv/matlab/rvctools/common/yaxis.m b/lwrserv/matlab/rvctools/common/yaxis.m new file mode 100644 index 0000000..8b61d5a --- /dev/null +++ b/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]) diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/CodeGenerator.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/CodeGenerator.m new file mode 100644 index 0000000..d76def7 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodecoriolis.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodecoriolis.m new file mode 100644 index 0000000..6f04c46 --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefdyn.m new file mode 100644 index 0000000..9fd9b57 --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefkine.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefkine.m new file mode 100644 index 0000000..7d81532 --- /dev/null +++ b/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.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!
',... + 'Code generator written by:
',... + 'Joern Malzahn (joern.malzahn@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 = ['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!
',... + 'Code generator written by:
',... + 'Joern Malzahn (joern.malzahn@tu-dortmund.de)
'}; +hStruct.seeAlso = {'jacob0'}; +end + diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefriction.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodefriction.m new file mode 100644 index 0000000..c3eec40 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodegravload.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodegravload.m new file mode 100644 index 0000000..539cf25 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodeinertia.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodeinertia.m new file mode 100644 index 0000000..99e4ef7 --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodeinvdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodeinvdyn.m new file mode 100644 index 0000000..9b71893 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodejacobian.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genccodejacobian.m new file mode 100644 index 0000000..fac873f --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/gencoriolis.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/gencoriolis.m new file mode 100644 index 0000000..5cbe268 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genfdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genfdyn.m new file mode 100644 index 0000000..fee849e --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genfkine.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genfkine.m new file mode 100644 index 0000000..04d1fe5 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genfriction.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genfriction.m new file mode 100644 index 0000000..4830c3d --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/gengravload.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/gengravload.m new file mode 100644 index 0000000..aa57bfb --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/geninertia.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/geninertia.m new file mode 100644 index 0000000..29efaaf --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/geninvdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/geninvdyn.m new file mode 100644 index 0000000..0ee830d --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genjacobian.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genjacobian.m new file mode 100644 index 0000000..60d695b --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexcoriolis.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexcoriolis.m new file mode 100644 index 0000000..dc8ba27 --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfdyn.m new file mode 100644 index 0000000..b29a082 --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfkine.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfkine.m new file mode 100644 index 0000000..d061cc2 --- /dev/null +++ b/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.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!
',... + 'Code generator written by:
',... + 'Joern Malzahn (joern.malzahn@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.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!
',... + 'Code generator written by:
',... + 'Joern Malzahn (joern.malzahn@tu-dortmund.de)
'}; +hStruct.seeAlso = {'jacob0'}; +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfriction.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexfriction.m new file mode 100644 index 0000000..4c55f47 --- /dev/null +++ b/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.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 + diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexgravload.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexgravload.m new file mode 100644 index 0000000..2d5a963 --- /dev/null +++ b/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.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 + diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexinertia.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexinertia.m new file mode 100644 index 0000000..9b91636 --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexinvdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexinvdyn.m new file mode 100644 index 0000000..18f1d2f --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexjacobian.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmexjacobian.m new file mode 100644 index 0000000..39a57fd --- /dev/null +++ b/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.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 \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuncoriolis.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuncoriolis.m new file mode 100644 index 0000000..66cca3b --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfdyn.m new file mode 100644 index 0000000..f127311 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfkine.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfkine.m new file mode 100644 index 0000000..8d903ad --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfriction.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunfriction.m new file mode 100644 index 0000000..4f6cc86 --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfungravload.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfungravload.m new file mode 100644 index 0000000..0d980ab --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuninertia.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuninertia.m new file mode 100644 index 0000000..6e00abe --- /dev/null +++ b/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.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 diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuninvdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuninvdyn.m new file mode 100644 index 0000000..97159ea --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfuninvdyn.m @@ -0,0 +1,132 @@ +%CODEGENERATOR.GENMFUNINVDYN Generate M-functions for inverse dynamics +% +% cGen.genmfuninvdyn() generates a robot-specific M-function to compute +% inverse dynamics. +% +% Notes:: +% - Is called by CodeGenerator.geninvdyn if cGen has active flag genmMfun +% - 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.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 [ ] = genmfuninvdyn( 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 inverse dynamics m-function']); + +funfilename = fullfile(CGen.robjpath,'invdyn.m'); +hStruct = createHeaderStruct(CGen.rob,funfilename); + +fid = fopen(funfilename,'w+'); + +fprintf(fid, '%s\n', ['function tau = invdyn(rob,q,qd,qdd)']); % Function definition +fprintf(fid, '%s\n',constructheaderstring(CGen,hStruct)); % Header + +fprintf(fid, '%s \n', 'tau = zeros(length(q),1);'); % Code + +funcCall = ['tau = rob.inertia(q)*qdd(:) + ',... + 'rob.coriolis(q,qd)*qd(:) + ',... + 'rob.gravload(q).'' - ', ... + 'rob.friction(qd).'';']; +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) + 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 + +%% Definition of the header contents for each generated file +function hStruct = createHeaderStruct(rob,fname) +[~,hStruct.funName] = fileparts(fname); +hStruct.shortDescription = ['Inverse dynamics for the',rob.name,' arm.']; +hStruct.calls = {['tau = ',hStruct.funName,'(rob,q,qd,qdd)'],... + ['tau = rob.',hStruct.funName,'(q,qd,qdd)']}; +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.'}; +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', ... + ['qdd: ',int2str(rob.n),'-element vector of generalized'],... + ' accelerations',... + 'Angles have to be given in radians!'}; +hStruct.outputs = {['tau: [',int2str(rob.n),'x1] vector of joint 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 = {'fdyn'}; +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunjacobian.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunjacobian.m new file mode 100644 index 0000000..d3aa211 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genmfunjacobian.m @@ -0,0 +1,140 @@ +%CODEGENERATOR.GENMFUNJACOBIAN Generate M-functions for robot Jacobian +% +% cGen.genmfunjacobian() generates a robot-specific M-function to compute +% robot Jacobian. +% +% 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.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.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 [] = genmfunjacobian(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 +symname = 'jacob0'; +fname = fullfile(CGen.sympath,[symname,'.mat']); + +if exist(fname,'file') + CGen.logmsg([datestr(now),'\tGenerating jacobian m-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,'.m']); +q = CGen.rob.gencoords; + +matlabFunction(tmpStruct.(symname),'file',funfilename,... % generate function m-file + 'outputs', {symname},... + 'vars', {'rob',[q]}); +hStruct = createHeaderStructJacob0(CGen.rob,symname); % replace autogenerated function header +replaceheader(CGen,hStruct,funfilename); +CGen.logmsg('\t%s\n',' done!'); + + +symname = 'jacobn'; +fname = fullfile(CGen.sympath,[symname,'.mat']); + +if exist(fname,'file') + CGen.logmsg([datestr(now),'\tGenerating jacobian m-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,'.m']); +q = CGen.rob.gencoords; + +matlabFunction(tmpStruct.(symname),'file',funfilename,... % generate function m-file + 'outputs', {symname},... + 'vars', {'rob',[q]}); +hStruct = createHeaderStructJacobn(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 = createHeaderStructJacob0(rob,fname) +[~,hStruct.funName] = fileparts(fname); +hStruct.shortDescription = ['Jacobian with respect to the base coordinate frame of the ',rob.name,' arm.']; +hStruct.calls = {['J0 = ',hStruct.funName,'(rob,q)'],... + ['J0 = rob.',hStruct.funName,'(q)']}; +hStruct.detailedDescription = {['Given a full set of joint variables the function'],... + 'computes the robot jacobian with respect to the base frame.'}; +hStruct.inputs = {['q: ',int2str(rob.n),'-element vector of generalized coordinates.'],... + 'Angles have to be given in radians!'}; +hStruct.outputs = {['J0: [6x',num2str(rob.n),'] Jacobian 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 = {'fkine,jacobn'}; +end + +%% Definition of the header contents for each generated file +function hStruct = createHeaderStructJacobn(rob,fname) +[~,hStruct.funName] = fileparts(fname); +hStruct.shortDescription = ['Jacobian with respect to the end-effector coordinate frame of the ',rob.name,' arm.']; +hStruct.calls = {['Jn = ',hStruct.funName,'(rob,q)'],... + ['Jn = rob.',hStruct.funName,'(q)']}; +hStruct.detailedDescription = {['Given a full set of joint variables the function'],... + 'computes the robot jacobian with respect to the end-effector frame.'}; +hStruct.inputs = {['q: ',int2str(rob.n),'-element vector of generalized coordinates.'],... + 'Angles have to be given in radians!'}; +hStruct.outputs = {['Jn: [6x',num2str(rob.n),'] Jacobian 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 = {'fkine,jacob0'}; +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockcoriolis.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockcoriolis.m new file mode 100644 index 0000000..f95fdcb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockcoriolis.m @@ -0,0 +1,150 @@ +%CODEGENERATOR.GENSLBLOCKCORIOLIS Generat Simulink block for Coriolis matrix +% +% cGen.genslblockcoriolis() generates a robot-specific Simulink block to compute +% Coriolis/centripetal matrix. +% +% Notes:: +% - Is called by CodeGenerator.gencoriolis if cGen has active flag genslblock +% - The Coriolis matrix is stored row by row to avoid memory issues. +% - The Simulink block recombines the the individual blocks 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.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 [ ] = genslblockcoriolis( CGen ) + +%% Open or create block library +bdclose('all') % avoid problems with previously loaded libraries +load_system('simulink'); +if ~(exist([CGen.slibpath,simulinkext]) == 2) % Create new block library if none exists + CGen.createnewblocklibrary; +end +open_system(CGen.slibpath); +set_param(CGen.slib,'lock','off'); + +[q,qd] = CGen.rob.gencoords; + +%% Generate Coriolis Block +CGen.logmsg([datestr(now),'\tGenerating Simulink Block for the robot Coriolis matrix\n']); +nJoints = CGen.rob.n; +symname = 'coriolis'; + +CGen.logmsg([datestr(now),'\t\t... enclosing subsystem ']); +CoriolisBlock = [CGen.slib,'/',symname]; +if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated block + delete_block(CoriolisBlock) + save_system; +end + +% Subsystem in which individual rows are concatenated +add_block('built-in/SubSystem',CoriolisBlock); % Add new inertia matrix block +add_block('Simulink/Math Operations/Matrix Concatenate'... + , [CoriolisBlock,'/coriolis']... + , 'NumInputs',num2str(nJoints)... + , 'ConcatenateDimension','1'); +add_block('Simulink/Sinks/Out1',[CoriolisBlock,'/out']); +add_block('Simulink/Sources/In1',[CoriolisBlock,'/q']); +add_block('Simulink/Sources/In1',[CoriolisBlock,'/qd']); +add_line(CoriolisBlock,'coriolis/1','out/1'); +CGen.logmsg('\t%s\n',' done!'); + +for kJoints = 1:nJoints + CGen.logmsg([datestr(now),'\t\t... Embedded Matlab Function Block for joint ',num2str(kJoints),': ']); + + % Generate Embedded Matlab Function block for each row + symname = ['coriolis_row_',num2str(kJoints)]; + fname = fullfile(CGen.sympath,[symname,'.mat']); + + if exist(fname,'file') + tmpStruct = load(fname); + else + error ('genslblockcoriolis:SymbolicsNotFound','Save symbolic expressions to disk first!') + end + + blockaddress = [CoriolisBlock,'/',symname]; + if doesblockexist(CGen.slib,symname) + delete_block(blockaddress); + save_system; + end + + CGen.logmsg('%s',' block creation'); + symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{q,qd}); + + + % connect output + CGen.logmsg('%s',', output wiring'); + if ( verLessThan('matlab','7.11.0.584') ) && ( isequal(tmpStruct.(symname),zeros(1,nJoints)) ) + % There is a bug in earlier Matlab versions. If the symbolic + % vector is a zero vector, then the Simulink Embedded Matlab + % Function block outputs a scalar zero. We need to concatenate + % a row vector of zeros here, which we have to construct on our + % own. + add_block('Simulink/Math Operations/Matrix Concatenate'... % Use a matrix concatenation block ... + , [CoriolisBlock,'/DimCorrection',num2str(kJoints)]... % ... named with the current row number ... + , 'NumInputs',num2str(nJoints),'ConcatenateDimension','2'); % ... intended to concatenate zero values for each joint ... + % ... columnwise. This will circumvent the bug. + + for iJoints = 1:nJoints % Connect signal lines from the created block (which outputs + add_line(CoriolisBlock... % a scalar zero in this case) with the bugfix block. + , [symname,'/1']... + , ['DimCorrection',num2str(kJoints),'/', num2str(iJoints)]); + end + + add_line(CoriolisBlock,['DimCorrection',num2str(kJoints)... % Connect the fixed row with other rows. + , '/1'],['coriolis/', num2str(kJoints)]); + + else + add_line(CoriolisBlock,[symname,'/1']... % In case that no bug occurs, we can just connect the rows. + , ['coriolis/', num2str(kJoints)]); + end + + % Vector generalized joint values + add_line(CoriolisBlock,'q/1',[symname,'/1']); + % Vector generalized joint velocities + add_line(CoriolisBlock,'qd/1',[symname,'/2']); + + CGen.logmsg('\t%s\n','row complete!'); +end +addterms(CoriolisBlock); % Add terminators where needed +distributeblocks(CoriolisBlock); +CGen.logmsg([datestr(now),'\tCoriolis matrix block complete\n']); + + +%% Cleanup +% Arrange blocks +distributeblocks(CGen.slib); + +% Lock, save and close library +set_param(CGen.slib,'lock','on'); +save_system(CGen.slib,CGen.slibpath); +close_system(CGen.slib); + +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfdyn.m new file mode 100644 index 0000000..c23e845 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfdyn.m @@ -0,0 +1,150 @@ +%CODEGENERATOR.GENSLBLOCKFDYN Generate Simulink block for forward dynamics +% +% cGen.genslblockfdyn() generates a robot-specific Simulink block to compute +% forward dynamics. +% +% Notes:: +% - Is called by CodeGenerator.genfdyn if cGen has active flag genslblock +% - The generated Simulink block is composed of previously generated blocks +% for the inertia matrix, coriolis matrix, vector of gravitational load and +% joint friction vector. The block 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.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.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 [] = genslblockfdyn(CGen) + +%% Open or create block library +bdclose('all') % avoid problems with previously loaded libraries +load_system('simulink'); +if ~(exist([CGen.slibpath,simulinkext]) == 2) % Create new block library if none exists + CGen.createnewblocklibrary; +end +checkexistanceofblocks(CGen); +open_system(CGen.slibpath); +set_param(CGen.slib,'lock','off'); + +%% Generate forward dynamics block +CGen.logmsg([datestr(now),'\tGenerating Simulink Block for the forward dynamics\n']); +nJoints = CGen.rob.n; + +CGen.logmsg([datestr(now),'\t\t... enclosing subsystem ']); +symname = 'fdyn'; +forwDynamicsBlock = [CGen.slib,'/',symname]; +if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated block + delete_block(forwDynamicsBlock) + save_system; +end +CGen.logmsg('\t%s\n',' done!'); + +% add required blocks +CGen.logmsg([datestr(now),'\t\t... adding Simulink blocks for all components']); +add_block('built-in/SubSystem',forwDynamicsBlock); +add_block([CGen.slib,'/invinertia'],[forwDynamicsBlock,'/invinertia']); +add_block([CGen.slib,'/coriolis'],[forwDynamicsBlock,'/coriolis']); +add_block([CGen.slib,'/gravload'],[forwDynamicsBlock,'/gravload']); +add_block([CGen.slib,'/friction'],[forwDynamicsBlock,'/friction']); +add_block('Simulink/Sinks/Out1',[forwDynamicsBlock,'/q']); +add_block('Simulink/Sinks/Out1',[forwDynamicsBlock,'/qDot']); +add_block('Simulink/Sinks/Out1',[forwDynamicsBlock,'/qDDot']); +add_block('Simulink/Sources/In1',[forwDynamicsBlock,'/tau']); +add_block('built-in/Sum',[forwDynamicsBlock,'/Sum'],'inputs','|+---'); +add_block('built-in/Product',[forwDynamicsBlock,'/invInertiaMultiply'],'multiplication','Matrix(*)'); +add_block('built-in/Product',[forwDynamicsBlock,'/coriolisMultiply'],'multiplication','Matrix(*)'); +add_block('built-in/Integrator',[forwDynamicsBlock,'/Integrator1']); +add_block('built-in/Integrator',[forwDynamicsBlock,'/Integrator2']); +CGen.logmsg('\t%s\n',' done!'); + +% connect simulink blocks +CGen.logmsg([datestr(now),'\t\t... wiring']); +add_line(forwDynamicsBlock,'tau/1','Sum/1'); +add_line(forwDynamicsBlock,'invInertiaMultiply/1','qDDot/1'); +add_line(forwDynamicsBlock,'invInertiaMultiply/1','Integrator1/1'); +add_line(forwDynamicsBlock,'Integrator1/1','Integrator2/1'); +add_line(forwDynamicsBlock,'Integrator1/1','qDot/1'); +add_line(forwDynamicsBlock,'Integrator2/1','q/1'); +add_line(forwDynamicsBlock,'Integrator1/1','coriolis/2'); +add_line(forwDynamicsBlock,'Integrator1/1','coriolisMultiply/2'); +add_line(forwDynamicsBlock,'Integrator1/1','friction/1'); +add_line(forwDynamicsBlock,'Integrator2/1','coriolis/1'); +add_line(forwDynamicsBlock,'Integrator2/1','gravload/1'); +add_line(forwDynamicsBlock,'Integrator2/1','invinertia/1'); +add_line(forwDynamicsBlock,'invinertia/1','invInertiaMultiply/1'); +add_line(forwDynamicsBlock,'Sum/1','invInertiaMultiply/2'); +add_line(forwDynamicsBlock,'coriolisMultiply/1','Sum/2'); +add_line(forwDynamicsBlock,'gravload/1','Sum/3'); +add_line(forwDynamicsBlock,'friction/1','Sum/4'); +add_line(forwDynamicsBlock,'coriolis/1','coriolisMultiply/1'); +distributeblocks(forwDynamicsBlock); +CGen.logmsg('\t%s\n',' done!'); + +CGen.logmsg([datestr(now),'\tForward dynamics block complete\n']); + +%% Cleanup +% Arrange blocks +distributeblocks(CGen.slib); + +% Lock, save and close library +set_param(CGen.slib,'lock','on'); +save_system(CGen.slib,CGen.slibpath); +close_system(CGen.slib); + +end + +function [] = checkexistanceofblocks(CGen) +open_system(CGen.slibpath); +if isempty(find_system(CGen.slib,'SearchDepth',1,'Name','inertia')) || isempty(find_system(CGen.slib,'SearchDepth',1,'Name','invinertia')) + CGen.logmsg('\t\t%s\n','Inertia block not found! Generating:'); + CGen.genslblockinertia; + open_system(CGen.slibpath); +end + +if isempty(find_system(CGen.slib,'SearchDepth',1,'Name','coriolis')) + CGen.logmsg('\t\t%s\n','Coriolis block not found! Generating:'); + CGen.genslblockcoriolis; + open_system(CGen.slibpath); +end + +if isempty(find_system(CGen.slib,'SearchDepth',1,'Name','gravload')) + CGen.logmsg('\t\t%s\n','Gravload block not found! Generating:'); + CGen.genslblockgravload; + open_system(CGen.slibpath); +end + +if isempty(find_system(CGen.slib,'SearchDepth',1,'Name','friction')) + CGen.logmsg('\t\t%s\n','Friction block not found! Generating:'); + CGen.genslblockfriction; + open_system(CGen.slibpath); +end + +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfkine.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfkine.m new file mode 100644 index 0000000..cbe9b9c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfkine.m @@ -0,0 +1,111 @@ +%CODEGENERATOR.GENSLBLOCKFKINE Generate Simulink block for forward kinematics +% +% cGen.genslblockfkine() generates a robot-specific Simulink block to compute +% forward kinematics. +% +% Notes:: +% - Is called by CodeGenerator.genfkine if cGen has active flag genslblock. +% - The Simulink blocks are generated and stored in a robot specific block +% library cGen.slib in the directory cGen.basepath. +% - Blocks are created for intermediate transforms T0, T1 etc. as well. +% +% 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.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 genslblockfkine(CGen) + +%% Open or create block library +bdclose('all') % avoid problems with previously loaded libraries +load_system('simulink'); +if ~(exist([CGen.slibpath,simulinkext]) == 2) % Create new block library if none exists + CGen.createnewblocklibrary; +end +open_system(CGen.slibpath); +set_param(CGen.slib,'lock','off'); + +q = CGen.rob.gencoords; + +%% Forward kinematics up to tool center point +CGen.logmsg([datestr(now),'\tGenerating forward kinematics Simulink block up to the end-effector frame']); +symname = 'fkine'; +fname = fullfile(CGen.sympath,[symname,'.mat']); + +if exist(fname,'file') + tmpStruct = load(fname); +else + error ('genslblockfkine:SymbolicsNotFound','Save symbolic expressions to disk first!') +end + +blockaddress = [CGen.slib,'/',symname]; % treat intermediate transformations separately +if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated block + delete_block(blockaddress); + save_system; +end + +symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{q}); + +CGen.logmsg('\t%s\n',' done!'); + +%% Individual joint forward kinematics +CGen.logmsg([datestr(now),'\tGenerating forward kinematics Simulink block 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; + + + blockaddress = [CGen.slib,'/',symname]; % treat intermediate transformations separately + if doesblockexist(CGen.slib,symname) + delete_block(blockaddress); + save_system; + end + + symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{q}); + +end +CGen.logmsg('\t%s\n',' done!'); + +%% Cleanup +% Arrange blocks +distributeblocks(CGen.slib); + +% Lock, save and close library +set_param(CGen.slib,'lock','on'); +save_system(CGen.slib,CGen.slibpath); +close_system(CGen.slib); + +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfriction.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfriction.m new file mode 100644 index 0000000..524df52 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockfriction.m @@ -0,0 +1,84 @@ +%CODEGENERATOR.GENSLBLOCKFRICTION Generate Simulink block for joint friction +% +% cGen.genslblockfriction() generates a robot-specific Simulink block to compute +% the joint friction model. +% +% Notes:: +% - Is called by CodeGenerator.genfriction if cGen has active flag genslblock +% - The Simulink blocks are 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.genfriction. + +% 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.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 ] = genslblockfriction( CGen ) + +%% Open or create block library +bdclose('all') % avoid problems with previously loaded libraries +load_system('simulink'); +if ~(exist([CGen.slibpath,simulinkext]) == 2) % Create new block library if none exists + CGen.createnewblocklibrary; +end +open_system(CGen.slibpath); +set_param(CGen.slib,'lock','off'); + +[~,qd] = CGen.rob.gencoords; + +%% Generate block +CGen.logmsg([datestr(now),'\tGenerating joint friction Embedded Matlab Function Block:']); +symname = 'friction'; +fname = fullfile(CGen.sympath,[symname,'.mat']); + +if exist(fname,'file') + tmpStruct = load(fname); +else + error ('genslblockgfriction:SymbolicsNotFound','Save symbolic expressions to disk first!') +end + +blockaddress = [CGen.slib,'/',symname]; % treat intermediate transformations separately +if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated block + delete_block(blockaddress) + save_system; +end + +symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{qd}); + +CGen.logmsg('\t%s\n',' done!'); + +%% Cleanup +% Arrange blocks +distributeblocks(CGen.slib); + +% Lock, save and close library +set_param(CGen.slib,'lock','on'); +save_system(CGen.slib,CGen.slibpath); +close_system(CGen.slib); + +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockgravload.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockgravload.m new file mode 100644 index 0000000..eb66a95 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockgravload.m @@ -0,0 +1,84 @@ +%CODEGENERATOR.GENSLBLOCKGRAVLOAD Generate Simulink block for gravitational load +% +% cGen.genslblockgravload() generates a robot-specific Simulink block to compute +% gravitational load. +% +% Notes:: +% - Is called by CodeGenerator.gengravload if cGen has active flag genslblock +% - The Simulink blocks are 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.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.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 genslblockgravload(CGen) + +%% Open or create block library +bdclose('all') % avoid problems with previously loaded libraries +load_system('simulink'); +if ~(exist([CGen.slibpath,simulinkext]) == 2) % Create new block library if none exists + CGen.createnewblocklibrary; +end +open_system(CGen.slibpath); +set_param(CGen.slib,'lock','off'); + +q = CGen.rob.gencoords; + +%% Generate Block +CGen.logmsg([datestr(now),'\tGenerating Embedded Matlab Function Block for the vector of gravitational load forces/torques']); +symname = 'gravload'; +fname = fullfile(CGen.sympath,[symname,'.mat']); + +if exist(fname,'file') + tmpStruct = load(fname); +else + error ('genslblockgravload:SymbolicsNotFound','Save symbolic expressions to disk first!') +end + +blockaddress = [CGen.slib,'/',symname]; +if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated block + delete_block(blockaddress) + save_system; +end + +symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{q}); + +CGen.logmsg('\t%s\n',' done!'); + +%% Cleanup +% Arrange blocks +distributeblocks(CGen.slib); + +% Lock, save and close library +set_param(CGen.slib,'lock','on'); +save_system(CGen.slib,CGen.slibpath); +close_system(CGen.slib); + +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockinertia.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockinertia.m new file mode 100644 index 0000000..671c607 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockinertia.m @@ -0,0 +1,187 @@ +%CODEGENERATOR.GENSLBLOCKINERTIA Generate Simulink block for inertia matrix +% +% cGen.genslbgenslblockinertia() generates a robot-specific Simulink block to compute +% robot inertia matrix. +% +% Notes:: +% - Is called by CodeGenerator.geninertia if cGen has active flag genslblock +% - The Inertia matrix is stored row by row to avoid memory issues. +% - The Simulink block recombines the the individual blocks for each row. +% - The Simulink blocks are 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. + +% 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.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 genslblockinertia(CGen) + +%% Open or create block library +bdclose('all') % avoid problems with previously loaded libraries +load_system('simulink'); +if ~(exist([CGen.slibpath,simulinkext]) == 2) % Create new block library if none exists + CGen.createnewblocklibrary; +end +open_system(CGen.slibpath); +set_param(CGen.slib,'lock','off'); + +q = CGen.rob.gencoords; + +%% Generate Inertia Block +CGen.logmsg([datestr(now),'\tGenerating Simulink Block for the robot inertia matrix\n']); +nJoints = CGen.rob.n; + +CGen.logmsg([datestr(now),'\t\t... enclosing subsystem ']); +symname = 'inertia'; +InertiaBlock = [CGen.slib,'/',symname]; + if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated inertia matrix block + delete_block(InertiaBlock); + save_system; + end +% Subsystem in which individual rows are concatenated +add_block('built-in/SubSystem',InertiaBlock); % Add new inertia matrix block +add_block('Simulink/Math Operations/Matrix Concatenate'... + , [InertiaBlock,'/inertia']... + , 'NumInputs',num2str(nJoints)... + , 'ConcatenateDimension','1'); +add_block('Simulink/Sinks/Out1',[InertiaBlock,'/out']); +add_block('Simulink/Sources/In1',[InertiaBlock,'/q']); +add_line(InertiaBlock,'inertia/1','out/1'); +CGen.logmsg('\t%s\n',' done!'); + +for kJoints = 1:nJoints + CGen.logmsg([datestr(now),'\t\t... Embedded Matlab Function Block for joint ',num2str(kJoints),': ']); + + % Generate Embedded Matlab Function block for each row + symname = ['inertia_row_',num2str(kJoints)]; + fname = fullfile(CGen.sympath,[symname,'.mat']); + + if exist(fname,'file') + tmpStruct = load(fname); + else + error ('genslblockinertia:SymbolicsNotFound','Save symbolic expressions to disk first!') + end + + blockaddress = [InertiaBlock,'/',symname]; + if doesblockexist(CGen.slib,symname) + delete_block(blockaddress); + save_system; + end + + CGen.logmsg('%s',' block creation'); + symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{q}); + + + % connect output + CGen.logmsg('%s',', output wiring'); + if ( verLessThan('matlab','7.11.0.584') ) && ( isequal(tmpStruct.(symname),zeros(1,nJoints)) ) + % There is a bug in earlier Matlab versions. If the symbolic + % vector is a zero vector, then the Simulink Embedded Matlab + % Function block outputs a scalar zero. We need to concatenate + % a row vector of zeros here, which we have to construct on our + % own. + add_block('Simulink/Math Operations/Matrix Concatenate'... % Use a matrix concatenation block ... + , [InertiaBlock,'/DimCorrection',num2str(kJoints)]... % ... named with the current row number ... + , 'NumInputs',num2str(nJoints),'ConcatenateDimension','2'); % ... intended to concatenate zero values for each joint ... + % ... columnwise. This will circumvent the bug. + + for iJoints = 1:nJoints % Connect signal lines from the created block (which outputs + add_line(InertiaBlock... % a scalar zero in this case) with the bugfix block. + , [symname,'/1']... + , ['DimCorrection',num2str(kJoints),'/', num2str(iJoints)]); + end + + add_line(InertiaBlock,['DimCorrection',num2str(kJoints)... % Connect the fixed row with other rows. + , '/1'],['inertia/', num2str(kJoints)]); + + else + add_line(InertiaBlock,[symname,'/1']... % In case that no bug occurs, we can just connect the rows. + , ['inertia/', num2str(kJoints)]); + end + + % Connect inputs + CGen.logmsg('%s',', input wiring'); + add_line(InertiaBlock,'q/1',[symname,'/1']); + CGen.logmsg('\t%s\n','row complete!'); +end +addterms(InertiaBlock); % Add terminators where needed +distributeblocks(InertiaBlock); +CGen.logmsg([datestr(now),'\tInertia matrix block complete\n']); + + +%% Built inverse Inertia matrix block +CGen.logmsg([datestr(now),'\tGenerating Simulink Block for the inverse robot inertia matrix\n']); +CGen.logmsg([datestr(now),'\t\t... enclosing subsystem ']); +% block address +symname = 'invinertia'; +invInertiaBlock = [CGen.slib,'/',symname]; +% remove any existing blocks + if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated block + delete_block(invInertiaBlock); + save_system; + end +add_block('built-in/SubSystem',invInertiaBlock); +CGen.logmsg('\t%s\n',' done!'); + +% matrix inversion +CGen.logmsg([datestr(now),'\t\t... matrix inversion block ']); +add_block('Simulink/Math Operations/Product',[invInertiaBlock,'/inverse']); % Use a product block... +set_param([invInertiaBlock,'/inverse'],'Inputs','/'); % ... with single input '/'... +set_param([invInertiaBlock,'/inverse'],'Multiplication','Matrix(*)'); % ... and matrix multiplication +CGen.logmsg('\t%s\n',' done!'); + +% wire the input and output +CGen.logmsg([datestr(now),'\t\t... input and output ']); +add_block(InertiaBlock,[invInertiaBlock,'/inertiaMatrix']); +add_block('Simulink/Sources/In1',[invInertiaBlock,'/q']); +add_block('Simulink/Sinks/Out1',[invInertiaBlock,'/out']); + + +% wire the blocks among each other +CGen.logmsg('and internal wiring'); +add_line(invInertiaBlock,'q/1','inertiaMatrix/1'); +add_line(invInertiaBlock,'inertiaMatrix/1','inverse/1'); +add_line(invInertiaBlock,'inverse/1','out/1'); +CGen.logmsg('\t%s\n',' done!'); + +% add terminators where necessary +addterms(invInertiaBlock); +distributeblocks(invInertiaBlock); +CGen.logmsg([datestr(now),'\tInverse inertia matrix block complete.\n']); + +%% Cleanup +% Arrange blocks +distributeblocks(CGen.slib); + +% Lock, save and close library +set_param(CGen.slib,'lock','on'); +save_system(CGen.slib,CGen.slibpath); +close_system(CGen.slib); + +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockinvdyn.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockinvdyn.m new file mode 100644 index 0000000..dac3263 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockinvdyn.m @@ -0,0 +1,144 @@ +%CODEGENERATOR.GENSLBLOCKINVDYN Generate Simulink block for inverse dynamics +% +% cGen.genslblockinvdyn() generates a robot-specific Simulink block to compute +% inverse dynamics. +% +% Notes:: +% - Is called by CodeGenerator.geninvdyn if cGen has active flag genslblock +% - The generated Simulink block is composed of previously generated blocks +% for the inertia matrix, coriolis matrix, vector of gravitational load and +% joint friction vector.% The block recombines these components to compute +% the forward dynamics. +% - The Simulink blocks are 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. + +% 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.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 [ output_args ] = genslblockinvdyn( CGen ) + +%% Open or create block library +bdclose('all') % avoid problems with previously loaded libraries +load_system('simulink'); +if ~(exist([CGen.slibpath,simulinkext]) == 2) % Create new block library if none exists + CGen.createnewblocklibrary; +end +checkexistanceofblocks(CGen); +open_system(CGen.slibpath); +set_param(CGen.slib,'lock','off'); + + + +%% Generate Inertia Block +CGen.logmsg([datestr(now),'\tGenerating Simulink Block for the inverse robot dynamics:\n']); +nJoints = CGen.rob.n; + +CGen.logmsg([datestr(now),'\t\t... enclosing subsystem ']); +InvDynBlock = [CGen.slib,'/invdyn']; +if ~isempty(find_system(CGen.slib,'Name','invdyn')) % Delete previously generated inertia matrix block + delete_block(InvDynBlock) +end +CGen.logmsg('\t%s\n',' done!'); + +% Subsystem in which individual rows are concatenated +CGen.logmsg([datestr(now),'\t\t... adding Simulink blocks for all components']); +add_block('built-in/SubSystem',InvDynBlock); % Add new inertia matrix block +add_block([CGen.slib,'/inertia'],[InvDynBlock,'/inertia']); +add_block([CGen.slib,'/coriolis'],[InvDynBlock,'/coriolis']); +add_block([CGen.slib,'/gravload'],[InvDynBlock,'/gravload']); +add_block([CGen.slib,'/friction'],[InvDynBlock,'/friction']) +add_block('Simulink/Sources/In1',[InvDynBlock,'/q']); +add_block('Simulink/Sources/In1',[InvDynBlock,'/qd']); +add_block('Simulink/Sources/In1',[InvDynBlock,'/qdd']); +add_block('Simulink/Sinks/Out1',[InvDynBlock,'/tau']); +add_block('built-in/Product',[InvDynBlock,'/inertiaTorque'],'multiplication','Matrix(*)'); +add_block('built-in/Product',[InvDynBlock,'/coriolisTorque'],'multiplication','Matrix(*)'); +add_block('built-in/Sum',[InvDynBlock,'/Sum'],'inputs','++++'); +CGen.logmsg('\t%s\n',' done!'); + +% Connect individual Simulink blocks +CGen.logmsg([datestr(now),'\t\t... wiring']); +add_line(InvDynBlock,'q/1','inertia/1'); +add_line(InvDynBlock,'q/1','coriolis/1'); +add_line(InvDynBlock,'q/1','gravload/1'); +add_line(InvDynBlock,'qd/1','coriolis/2'); +add_line(InvDynBlock,'qd/1','friction/1'); +add_line(InvDynBlock,'inertia/1','inertiaTorque/1'); +add_line(InvDynBlock,'qdd/1','inertiaTorque/2'); +add_line(InvDynBlock,'coriolis/1','coriolisTorque/1'); +add_line(InvDynBlock,'qd/1','coriolisTorque/2'); +add_line(InvDynBlock,'inertiaTorque/1','Sum/1'); +add_line(InvDynBlock,'coriolisTorque/1','Sum/2'); +add_line(InvDynBlock,'gravload/1','Sum/3'); +add_line(InvDynBlock,'friction/1','Sum/4'); +add_line(InvDynBlock,'Sum/1','tau/1'); +distributeblocks(InvDynBlock); +CGen.logmsg('\t%s\n',' done!'); + +CGen.logmsg([datestr(now),'\tInverse dynamics block complete.\n']); + +%% Cleanup +% Arrange blocks +distributeblocks(CGen.slib); + +% Lock, save and close library +set_param(CGen.slib,'lock','on'); +save_system(CGen.slib,CGen.slibpath); +close_system(CGen.slib); + +end + +function [] = checkexistanceofblocks(CGen) +open_system(CGen.slibpath); +if isempty(find_system(CGen.slib,'SearchDepth',1,'Name','inertia')) + CGen.logmsg('\t\t%s\n','Inertia block not found! Generating:'); + CGen.genslblockinertia; + open_system(CGen.slibpath); +end + +if isempty(find_system(CGen.slib,'SearchDepth',1,'Name','coriolis')) + CGen.logmsg('\t\t%s\n','Coriolis block not found! Generating:'); + CGen.genslblockcoriolis; + open_system(CGen.slibpath); +end + +if isempty(find_system(CGen.slib,'SearchDepth',1,'Name','gravload')) + CGen.logmsg('\t\t%s\n','Gravload block not found! Generating:'); + CGen.genslblockgravload; + open_system(CGen.slibpath); +end + +if isempty(find_system(CGen.slib,'SearchDepth',1,'Name','friction')) + CGen.logmsg('\t\t%s\n','Friction block not found! Generating:'); + CGen.genslblockfriction; + open_system(CGen.slibpath); +end + +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockjacobian.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockjacobian.m new file mode 100644 index 0000000..1a3b662 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/genslblockjacobian.m @@ -0,0 +1,104 @@ +%CODEGENERATOR.GENSLBLOCKJACOBIAN Generate Simulink block for robot Jacobians +% +% cGen.genslblockjacobian() generates a robot-specific Simulink block to compute +% robot Jacobians (world and tool frame). +% +% Notes:: +% - Is called by CodeGenerator.genjacobian if cGen has active flag genslblock +% - The Simulink blocks are 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.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.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 genslblockjacobian(CGen) + +%% Open or create block library +bdclose('all') % avoid problems with previously loaded libraries +load_system('simulink'); +if ~(exist([CGen.slibpath,simulinkext]) == 2) % Create new block library if none exists + CGen.createnewblocklibrary; +end +open_system(CGen.slibpath); +set_param(CGen.slib,'lock','off'); + +q = CGen.rob.gencoords; +%% Jacobian0 +CGen.logmsg([datestr(now),'\tGenerating jacobian Embedded Matlab Function Block with respect to the robot base frame']); +% [datestr(now),'\tGenerating jacobian Embedded Matlab Function Block with respect to the end-effector frame']); +symname = 'jacob0'; +fname = fullfile(CGen.sympath,[symname,'.mat']); + +if exist(fname,'file') + tmpStruct = load(fname); +else + error ('genSLBlockFkine:SymbolicsNotFound','Save symbolic expressions to disk first!') +end + +blockaddress = [CGen.slib,'/',symname]; % treat intermediate transformations separately +if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated block + delete_block(blockaddress); + save_system; +end + +symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{q}); + +CGen.logmsg('\t%s\n',' done!'); + +%% Jacobn +CGen.logmsg([datestr(now),'\tGenerating jacobian Embedded Matlab Function Block with respect to the end-effector frame']); +symname = 'jacobn'; +fname = fullfile(CGen.sympath,[symname,'.mat']); + +if exist(fname,'file') + tmpStruct = load(fname); +else + error ('genSLBlockFkine:SymbolicsNotFound','Save symbolic expressions to disk first!') +end + +blockaddress = [CGen.slib,'/',symname]; % treat intermediate transformations separately +if ~isempty(find_system(CGen.slib,'SearchDepth',1,'Name',symname)) % Delete previously generated block + delete_block(blockaddress); + save_system; +end + +symexpr2slblock(blockaddress,tmpStruct.(symname),'vars',{q}); +CGen.logmsg('\t%s\n',' done!'); + +%% Cleanup +% Arrange blocks +distributeblocks(CGen.slib); + +% Lock, save and close library +set_param(CGen.slib,'lock','on'); +save_system(CGen.slib,CGen.slibpath); +close_system(CGen.slib); + +end diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/logmsg.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/logmsg.m new file mode 100644 index 0000000..c48117a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/logmsg.m @@ -0,0 +1,59 @@ +%CODEGENERATOR.LOGMSG Print CodeGenerator logs. +% +% count = CGen.logmsg( FORMAT, A, ...) is the number of characters written to the CGen.logfile. +% For the additional arguments see fprintf. +% +% Note:: +% Matlab ships with a function for writing formatted strings into a text +% file or to the console (fprintf). The function works with single +% target identifiers (file, console, string). This function uses the +% same syntax as for the fprintf function to output log messages to +% either the Matlab console, a log file or both. +% +% Authors:: +% Joern Malzahn +% 2012 RST, Technische Universitaet Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% +% See also multidfprintf,fprintf,sprintf. + +% 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.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 [cnt] = logmsg(CGen, varargin) + +% Output to logfile? +if ~isempty(CGen.logfile) + logfid = fopen(CGen.logfile,'a+'); +else + logfid = []; +end + +% write message to multiple destinations +cnt = multidfprintf([CGen.verbose, logfid],varargin{:}); + +% Logfile to close? +if ~isempty(CGen.logfile) + logfid = fclose(logfid); +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/constructheaderstring.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/constructheaderstring.m new file mode 100644 index 0000000..1cbed0a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/constructheaderstring.m @@ -0,0 +1,206 @@ +%CODEGENERATOR.CONSTRUCTHEADERSTRING Creates common toolbox header string. +% +% HFSTRING = CGen.constructheaderstring(HSTRUCT) is the formatted header +% string. +% HSTRUCT is the geader content struct +% +% Notes:: +% The contents of the header are coded in a struct that is the input +% parameter and has the following self-explaining fields: +% Fieldname Datatype +% - funName 'string' +% - shortDescription 'string' +% - detailedDescription {'line1','line2',...} +% - inputs {'input1: description','input2: description',...} +% - outputs {'output1: description','output2: description',...} +% - examples {'line1','line2',...} +% - knownBugs {'line1','line2',...} +% - toDO {'line1','line2',...} +% - references {'line1','line2',...} +% - authors {'line1','line2',...} +% - seeAlso {'function1','function2',...} +% +% Example:: +% hStruct.funName = 'myFirstFunction'; +% hStruct.shortDescription = ['Just an example!']; +% hStruct.calls = {'result = myFirstFunction(A,B)'}; +% constructheaderstring(hStruct) +% +% Authors:: +% Joern Malzahn +% 2012 RST, Technische Universitaet Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% +% See also CodeGenerator.replaceheader, sprintf. + +% Copyright (C) 1993-2012, by Peter I. Corke +% Copyright (C) 2012, 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.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 [hFString] = constructheaderstring(CGen,hStruct) + +if isfield(hStruct,'funName') && isfield(hStruct,'shortDescription') + hFString = []; + hFString = [hFString, sprintf('%s \n',['%% ',upper(hStruct.funName),' - ', hStruct.shortDescription, ''])]; + hFString = [hFString, sprintf('%s \n', '% =========================================================================')]; + hFString = [hFString, sprintf('%s \n', '% ')]; +else + error('Header must include the function name and provide a short description!') +end + +%% Possible Function calls +if isfield(hStruct,'calls') + nCalls = length(hStruct.calls); + for iCall = 1:nCalls + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.calls{iCall}])]; + end +else + error('Header must provide function call information!') +end +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% Detailed description +hFString = [hFString, sprintf('%s \n', '% Description::')]; +if isfield(hStruct,'detailedDescription') + nDescription = length(hStruct.detailedDescription); + for iDescription = 1:nDescription + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.detailedDescription{iDescription}])]; + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% Explanation of the Inputs +hFString = [hFString, sprintf('%s \n', '% Input::')]; +if isfield(hStruct,'inputs') + nInputs = length(hStruct.inputs); + for iInputs = 1:nInputs + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.inputs{iInputs}])]; + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end + +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% Explanation of the Outputs +hFString = [hFString, sprintf('%s \n', '% Output::')]; +if isfield(hStruct,'outputs') + nOutputs = length(hStruct.outputs); + for iOutputs = 1:nOutputs + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.outputs{iOutputs}])]; + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end + +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% Examples +hFString = [hFString, sprintf('%s \n', '% Example::')]; +if isfield(hStruct,'example') + nExamples = length(hStruct.example); + for iExamples = 1:nExamples + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.example{iExamples}])]; + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end + +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% Known Bugs +hFString = [hFString, sprintf('%s \n', '% Known Bugs::')]; +if isfield(hStruct,'knownBugs') + nBugs = length(hStruct.knownBugs); + for iBugs = 1:nBugs + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.knownBugs{iBugs}])]; + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end + +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% TODO list +hFString = [hFString, sprintf('%s \n', '% TODO::')]; +if isfield(hStruct,'toDo') + nToDo = length(hStruct.toDo); + for iToDo = 1:nToDo + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.toDo{iToDo}])]; + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end + +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% References +hFString = [hFString, sprintf('%s \n', '% References::')]; +if isfield(hStruct,'references') + nRef = length(hStruct.references); + for iRef = 1:nRef + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.references{iRef}])]; + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end + +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% Authors +hFString = [hFString, sprintf('%s \n', '% Authors::')]; +if isfield(hStruct,'authors') + nAuthors = length(hStruct.authors); + for iAuthors = 1:nAuthors + hFString = [hFString, sprintf('%s \n', ['% ',hStruct.authors{iAuthors}])]; + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end + +hFString = [hFString, sprintf('%s \n', '% ')]; + +%% See also +hFString = [hFString, sprintf('%s ', '% See also')]; +if isfield(hStruct,'seeAlso') + nAlso = length(hStruct.seeAlso); + for iAlso = 1:nAlso + hFString = [hFString, sprintf('%s', hStruct.seeAlso{iAlso})]; + if iAlso < nAlso + hFString = [hFString, sprintf('%s', ', ')]; + else + hFString = [hFString, sprintf('%s\n', '.')]; + end + end +else + hFString = [hFString, sprintf('%s \n', '% ---')]; +end + +hFString = [hFString, sprintf('%s \n', '% ')]; + +hFString = [hFString, sprintf('%s \n', ' ')]; + +%% Copyright note +hFString = [hFString,CGen.generatecopyrightnote]; \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/constructheaderstringc.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/constructheaderstringc.m new file mode 100644 index 0000000..4b72eb1 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/constructheaderstringc.m @@ -0,0 +1,245 @@ +%CODEGENERATOR.CONSTRUCTHEADERSTRINGC Creates common toolbox header string. +% +% HFSTRING = CGen.constructheaderstringc(HSTRUCT) is the formatted header +% string. +% HSTRUCT is the header content struct +% +% Notes:: +% The contents of the header are coded in a struct that is the input +% parameter and has the following self-explaining fields: +% Fieldname Datatype +% - funName 'string' +% - shortDescription 'string' +% - detailedDescription {'line1','line2',...} +% - inputs {'input1: description','input2: description',...} +% - outputs {'output1: description','output2: description',...} +% - examples {'line1','line2',...} +% - knownBugs {'line1','line2',...} +% - toDO {'line1','line2',...} +% - references {'line1','line2',...} +% - authors {'line1','line2',...} +% - seeAlso {'function1','function2',...} +% +% Example:: +% hStruct.funName = 'myFirstFunction'; +% hStruct.shortDescription = ['Just an example!']; +% hStruct.calls = {'result = myFirstFunction(A,B)'}; +% constructheaderstringc(hStruct) +% +% Authors:: +% Joern Malzahn (joern.malzahn@tu-dortmund.de) +% +% See also CodeGenerator.constructheaderstring, sprintf. + +% Copyright (C) 1993-2014, by Peter I. Corke +% Copyright (C) 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.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 [hFString] = constructheaderstringc(CGen,hStruct) +% +% % reuse existing header construction routine, +% hFString = CGen.constructheaderstring(hStruct); +% +% % replace comment characters for C-compliance +% hFString = regexprep(hFString, '%', '//'); + +%CODEGENERATOR.CONSTRUCTHEADERSTRING Creates common toolbox header string. +% +% HFSTRING = CGen.constructheaderstring(HSTRUCT) is the formatted header +% string. +% HSTRUCT is the geader content struct +% +% Notes:: +% The contents of the header are coded in a struct that is the input +% parameter and has the following self-explaining fields: +% Fieldname Datatype +% - funName 'string' +% - shortDescription 'string' +% - detailedDescription {'line1','line2',...} +% - inputs {'input1: description','input2: description',...} +% - outputs {'output1: description','output2: description',...} +% - examples {'line1','line2',...} +% - knownBugs {'line1','line2',...} +% - toDO {'line1','line2',...} +% - references {'line1','line2',...} +% - authors {'line1','line2',...} +% - seeAlso {'function1','function2',...} +% +% Example:: +% hStruct.funName = 'myFirstFunction'; +% hStruct.shortDescription = ['Just an example!']; +% hStruct.calls = {'result = myFirstFunction(A,B)'}; +% constructheaderstring(hStruct) +% +% Authors:: +% Joern Malzahn (joern.malzahn@tu-dortmund.de) +% +% See also CodeGenerator.replaceheader, sprintf. + +% Copyright (C) 1993-2014, by Peter I. Corke +% Copyright (C) 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.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 [hFString] = constructheaderstringc(CGen,hStruct) + +hFString = []; +hFString = [hFString,sprintf('%s\n',['/*! \file ',hStruct.funName,'.h'])]; + +if isfield(hStruct,'funName') && isfield(hStruct,'shortDescription') + + hFString = [hFString, sprintf('%s \n',['\brief ', hStruct.shortDescription, ''])]; + hFString = [hFString, sprintf('%s \n', ' ')]; % Empty line +else + error('Header must include the function name and provide a short description!') +end + +%% Detailed description +hFString = [hFString, sprintf('%s \n', ' ')]; +if isfield(hStruct,'detailedDescription') + nDescription = length(hStruct.detailedDescription); + for iDescription = 1:nDescription + hFString = [hFString, sprintf('%s \n', [hStruct.detailedDescription{iDescription}])]; + end +else + hFString = [hFString, sprintf('%s \n', ' ')]; +end +hFString = [hFString, sprintf('%s \n', ' ')]; % Empty line + +%% Examples +hFString = [hFString, sprintf('%s \n', '__Example__:
')]; +if isfield(hStruct,'example') + nExamples = length(hStruct.example); + for iExamples = 1:nExamples + hFString = [hFString, sprintf('%s \n', [hStruct.example{iExamples}])]; + end +else + hFString = [hFString, sprintf('%s \n', ' ')]; +end + +hFString = [hFString, sprintf('%s \n', ' ')]; + +%% Known Bugs +hFString = [hFString, sprintf('%s \n', '__Known Bugs__:
')]; +if isfield(hStruct,'knownBugs') + nBugs = length(hStruct.knownBugs); + for iBugs = 1:nBugs + hFString = [hFString, sprintf('%s \n', [hStruct.knownBugs{iBugs}])]; + end +else + hFString = [hFString, sprintf('%s \n', ' ')]; +end + +hFString = [hFString, sprintf('%s \n', ' ')]; + +%% TODO list +hFString = [hFString, sprintf('%s \n', '__TODO__:
')]; +if isfield(hStruct,'toDo') + nToDo = length(hStruct.toDo); + for iToDo = 1:nToDo + hFString = [hFString, sprintf('%s \n', [hStruct.toDo{iToDo}])]; + end +else + hFString = [hFString, sprintf('%s \n', ' ')]; +end + +hFString = [hFString, sprintf('%s \n', ' ')]; + +%% References +hFString = [hFString, sprintf('%s \n', '__References__:
')]; +if isfield(hStruct,'references') + nRef = length(hStruct.references); + for iRef = 1:nRef + hFString = [hFString, sprintf('+ %s \n', [hStruct.references{iRef}])]; + end +else + hFString = [hFString, sprintf('%s \n', ' ')]; +end + +hFString = [hFString, sprintf('%s \n', ' ')]; + +%% Authors +hFString = [hFString, sprintf('%s \n', '__Authors__:
')]; +if isfield(hStruct,'authors') + nAuthors = length(hStruct.authors); + for iAuthors = 1:nAuthors + hFString = [hFString, sprintf('%s', [hStruct.authors{iAuthors}])]; + end +else + hFString = [hFString, sprintf('%s \n', ' ')]; +end + +hFString = [hFString, sprintf('%s \n', ' ')]; +hFString = [hFString, sprintf('%s \n', '

')]; + +%% Copyright note +hFString = [hFString, sprintf('%s \n', '__Copyright Note__:')]; +cprNote = CGen.generatecopyrightnote; +cprNote = regexprep(cprNote, '%', ' '); +cprNote = regexprep(cprNote, 'Copyright', '
Copyright '); + +hFString = [hFString,cprNote]; + +hFString = [hFString,sprintf('%s\n','*/')]; +hFString = [hFString, sprintf('%s \n', ' ')]; % Empty line +hFString = [hFString,sprintf('%s\n',['/*!\fn ',hStruct.calls])]; + +%% Explanation of the Inputs +if isfield(hStruct,'inputs') + nInputs = length(hStruct.inputs); + for iInputs = 1:nInputs + hFString = [hFString, sprintf('\\param %s\n', hStruct.inputs{iInputs})]; + end +else + hFString = [hFString, sprintf('%s \n', '')]; +end +%% Explanation of the Outputs +if isfield(hStruct,'outputs') + nOutputs = length(hStruct.outputs); + for iOutputs = 1:nOutputs + hFString = [hFString, sprintf('\\param %s\n', hStruct.outputs{iOutputs})]; + end +else + hFString = [hFString, sprintf('%s \n', ' ')]; +end + +hFString = [hFString,sprintf('%s\n','*/')]; \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/createmconstructor.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/createmconstructor.m new file mode 100644 index 0000000..68c94cc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/createmconstructor.m @@ -0,0 +1,71 @@ +%CODEGENERATOR.CREATEMCONSTRUCTOR Creates the constructor of the specialized robot class collecting the generated m-function code. +% +% cGen.createmconstructor() +% +% Authors:: +% Joern Malzahn +% 2012 RST, Technische Universitaet Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% +% See also CodeGenerator.genfkine, CodeGenerator.genmfunfkine. + +% 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.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 [] = createmconstructor(CGen) + +objdir = CGen.robjpath; +robname = CGen.getrobfname; + +if ~exist(objdir,'dir') + mkdir(objdir) +end + +sr = CGen.rob; +save(fullfile(objdir,['mat',robname]), 'sr'); + +fid = fopen(fullfile(objdir,[robname,'.m']),'w+'); + +fprintf(fid,'%s\n',['classdef ',robname,' < SerialLink']); +fprintf(fid,'%s\n',' '); +fprintf(fid,'\t%s\n', 'properties'); +fprintf(fid,'\t%s\n','end'); +fprintf(fid,'%s\n',' '); +fprintf(fid,'\t%s\n','methods'); +fprintf(fid,'\t\t%s\n',['function ro = ',robname,'()']); +fprintf(fid,'\t\t\t%s\n',['objdir = which(''',robname,''');']); +fprintf(fid,'\t\t\t%s\n','idx = find(objdir == filesep,2,''last'');'); +fprintf(fid,'\t\t\t%s\n','objdir = objdir(1:idx(1));'); +fprintf(fid,'\t\t\t%s\n',' '); +fprintf(fid,'\t\t\t%s\n',['tmp = load(fullfile(objdir,',['''@',robname],''',''mat',robname,'.mat''));']); +fprintf(fid,'\t\t\t%s\n',' '); +fprintf(fid,'\t\t\t%s\n','ro = ro@SerialLink(tmp.sr);'); +fprintf(fid,'\t\t\t%s\n', ' '); +fprintf(fid,'\t\t\t%s\n',' '); +fprintf(fid,'\t\t%s\n','end'); +fprintf(fid,'\t%s\n','end'); +fprintf(fid,'\t%s\n',' '); +fprintf(fid,'%s\n','end'); + +fclose(fid); \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/createnewblocklibrary.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/createnewblocklibrary.m new file mode 100644 index 0000000..f4017a9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/createnewblocklibrary.m @@ -0,0 +1,68 @@ +function [] = createnewblocklibrary(CGen) +%CodeGenerator.createnewblocklibrary +% +% Creates a new Simulink library file along with its slblocks.m. +% +% Authors:: +% 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.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. + +warning off; +CGen.rmpath; +warning on; + +%% Create Simulink library file +new_system(CGen.slib,'Library', 'ErrorIfShadowed'); +open_system(CGen.slib); +save_system(CGen.slib,CGen.slibpath); + +%% Create slblock.m + +fid = fopen(fullfile(CGen.basepath,'slblocks.m'),'w+'); + +fprintf(fid,'%s\n',['% This file is the slblocks file for the ', CGen.getrobfname,' Simulink library.']); +fprintf(fid,'%s\n',['% It has been automatically generated using The Robotics Toolbox for Matlab (RTB).']); +fprintf(fid,'%s\n',' '); +fprintf(fid,'%s\n\n',CGen.generatecopyrightnote); + +fprintf(fid,'%s\n','function blkStruct = slblocks'); +fprintf(fid,'%s\n',' '); +fprintf(fid,'%s\n', ['Browser.Library = ''',CGen.slib,''';']); +fprintf(fid,'%s\n', ['Browser.Name = ''',CGen.getrobfname,''';']); +fprintf(fid,'%s\n', ['Browser.IsFlat = 1;']); +fprintf(fid,'%s\n',' '); + +fprintf(fid,'%s\n', ['blkStruct.Name = [''',CGen.getrobfname,''', sprintf(''\n''), ''Library''];']); +fprintf(fid,'%s\n', ['blkStruct.OpenFcn= ''',CGen.slib,''';']); +fprintf(fid,'%s\n', ['blkStruct.MaskDisplay = ''''',';']); +fprintf(fid,'%s\n', ['blkStruct.Bworser = Browser;']); +fprintf(fid,'%s\n',' '); + +fclose(fid); diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/ffindreplace.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/ffindreplace.m new file mode 100644 index 0000000..a4d26cd --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/ffindreplace.m @@ -0,0 +1,82 @@ +%CODEGENERATOR.FFINDREPLACE Find and replace all occurrences of string in a file. +% +% CGen.ffindreplace(fName, oText, nText, varargin) +% FNAME is the absolute or relative path to the file to replace the text in. +% OTEXT is the text passage to replace. +% NTEXT is the new text. +% +% Notes:: +% The function opens and sweeps the text file FNAME. All occurrences of +% OTEXT are replaced by NTEXT. +% +% Authors:: +% Joern Malzahn +% 2012 RST, Technische Universitaet Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% +% See also CodeGenerator.finsertfront. + +% 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.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 [ ] = ffindreplace(CGen, fName, oText, nText, varargin) + +fid = fopen(fName,'r'); % open the file + +% determine number of lines in the file +nLines = getNLines(fid); + +% read all lines to cell +oldLines = cell(1,nLines); +newLines = {1,nLines}; +for iLines = 1:nLines + oldLines{iLines} = fgets(fid); +end + +% close the file again +fclose(fid); + +% replace all occurrences of oText by nText in each line +for iLines = 1:nLines + newLines{iLines}= strrep(oldLines{iLines}, oText, nText); +end + +% rewrite the file +fid = fopen(fName,'w'); +for iLines = 1:nLines + fprintf(fid,'%s',newLines{iLines}); +end +fclose(fid); + + +end + +function [nLines] = getNLines(fid) +nLines = 0; +while (~feof(fid)) % go through each line until eof is reached + fgets(fid); + nLines = nLines +1; % increment line counter +end +frewind(fid); % set file indicator back to the beginning of the file +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/finsertfront.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/finsertfront.m new file mode 100644 index 0000000..a0f3ccc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/finsertfront.m @@ -0,0 +1,78 @@ +%CODEGENERATOR.FINSERTFRONT Insert a string at the beginning of a textfile. +% +% CGen.finsertfront(FNAME,NTEXT) +% FNAME is the full or relative path to the text file. +% NTEXT is the string containing the text to be inserted at the beginning +% of the file. +% +% Notes:: +% MatLab ships with functions for reading, overwriting and appending text +% to files. This function is used to insert text at the beginning of a +% text file. +% +% Authors:: +% Joern Malzahn +% 2012 RST, Technische Universitaet Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% +% See also CodeGenerator.ffindreplace. + +% 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.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 [ ] = finsertfront(CGen, fName,nText) + +%% analyse the file +fid = fopen(fName,'r'); % open the file +% determine number of lines in the file +nLines = getNLines(fid); +% read all lines to cell +oldLines = cell(1,nLines); +for iLines = 1:nLines + oldLines{iLines} = fgets(fid); +end +% close the file again +fclose(fid); + +%% rewrite the file +fid = fopen(fName,'w'); +fprintf(fid,'%s',nText); + +for iLines = 1:nLines + fprintf(fid,'%s',oldLines{iLines}); +end +fclose(fid); + + +end + +%% Determine the number of lines in a file +function [nLines] = getNLines(fid) +nLines = 0; +while (~feof(fid)) % go through each line until eof is reached + fgets(fid); + nLines = nLines +1; % increment line counter +end +frewind(fid); % set file indicator back to the beginning of the file +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/gendotprodc.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/gendotprodc.m new file mode 100644 index 0000000..a86f9bc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/gendotprodc.m @@ -0,0 +1,119 @@ +%CODEGENERATOR.GENDOTPRODC Generates a dot product C-implementation. +% +% cGen.gendotprodc generates a .h and a .c file in the directory +% specified by ccodepath. +% +% Notes:: +% - Is called by geninvdyn if cGen has active flag genmex or genccode. +% +% Authors:: +% Joern Malzahn (joern.malzahn@tu-dortmund.de) +% +% See also CodeGenerator, CodeGenerator.gengaussjordanc. + +% Copyright (C) 1993-2014, by Peter I. Corke +% Copyright (C) 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 Lesser General Public License +% along with RTB. If not, see . +% +% http://www.petercorke.com +function [] = gendotprodc(CGen) + +funname = 'dotprod'; +funfilename = [funname,'.c']; +hfilename = [funname,'.h']; +srcDir = fullfile(CGen.ccodepath,'src'); +hdrDir = fullfile(CGen.ccodepath,'include'); + +% Check for existance of dotProd C-files +if exist(fullfile(srcDir,funfilename),'file') && exist(fullfile(srcDir,funfilename),'file') + return; +end + +% Check for existance of C-code directories +if ~exist(srcDir,'dir') + mkdir(srcDir); +end +if ~exist(hdrDir,'dir') + mkdir(hdrDir); +end + +fSignature = ['double ',funname,'(const double *input1, const double *input2, int nEl)']; +% Create the function description header +hStruct = createHeaderStruct(funname); % create header +hStruct.calls = fSignature; +if ~isempty(hStruct) + hFString = CGen.constructheaderstringc(hStruct); +end + +%% Generate C implementation file +fid = fopen(fullfile(srcDir,funfilename),'w+'); + +% Includes +fprintf(fid,'%s\n\n',... + ['#include "', hfilename,'"']); +% Function +fprintf(fid,'%s\n',[fSignature,'{']); +fprintf(fid,'\t%s\n','double res = 0;'); +fprintf(fid,'\t%s\n\n','int iEl = 0;'); +fprintf(fid,'\t%s\n','for (iEl = 0; iEl < nEl; iEl++){'); +fprintf(fid,'\t\t%s\n','res += input1[iEl] * input2[iEl];'); +fprintf(fid,'\t%s\n\n','}'); +fprintf(fid,'\t%s\n','return res;'); +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'])]); + +% Function prototype +fprintf(fid,'%s\n\n',['double ',funname,'(const double *input1, const double *input2, int nEl);']); + +% 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(fname) +[~,hStruct.funName] = fileparts(fname); +hStruct.shortDescription = ['Compute the dot product of two vectors']; +hStruct.detailedDescription = {['Given two vectors of length nEl in the input arrays'],... + 'input1 and input 2 the function computes the dot product (or scalar product) of both vectors.'}; +hStruct.inputs = { ['input1: nEl element array of doubles,'],... + ['input2: nEl element array of doubles,'],... + 'nEl: dimension of the two arrays.'}; +hStruct.outputs = {}; +hStruct.references = {'The C Book: http://publications.gbdirect.co.uk/c_book/'}; +hStruct.authors = {'This is an autogenerated function!',... + 'Code generator written by:',... + 'Joern Malzahn (joern.malzahn@tu-dortmund.de)'}; +hStruct.seeAlso = {'CodeGenerator'}; +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/generatecopyrightnote.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/generatecopyrightnote.m new file mode 100644 index 0000000..ff6ed72 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/generatecopyrightnote.m @@ -0,0 +1,70 @@ +function [ outstring ] = generatecopyrightnote( CGen ) +%GENERATECOPYRIGHTNOTE Creates copyright information for autogenerated +%files. +% +% outstring = CGen.generatecopyrightnote is the formatted string containing +% the copyright note. +% +% Authors:: +% Joern Malzahn +% 2012 RST, Technische Universitaet Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% +% See also CodeGenerator.CodeGenerator, CodeGenerator.genjacobian + +% 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.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. + +outstring = []; + +%% Copyright note +outstring = [outstring, sprintf('%s \n', '% Copyright (C) 1993-2013, by Peter I. Corke')]; +outstring = [outstring, sprintf('%s \n', '% Copyright (C) 2012-2013, by Joern Malzahn')]; +outstring = [outstring, sprintf('%s \n', '%')]; +outstring = [outstring, sprintf('%s \n', '% This file has been automatically generated with The Robotics Toolbox for Matlab (RTB).')]; +outstring = [outstring, sprintf('%s \n', '%')]; +outstring = [outstring, sprintf('%s \n', '% RTB and code generated with RTB is free software: you can redistribute it and/or modify')]; +outstring = [outstring, sprintf('%s \n', '% it under the terms of the GNU Lesser General Public License as published by')]; +outstring = [outstring, sprintf('%s \n', '% the Free Software Foundation, either version 3 of the License, or')]; +outstring = [outstring, sprintf('%s \n', '% (at your option) any later version.')]; +outstring = [outstring, sprintf('%s \n', '% ')]; +outstring = [outstring, sprintf('%s \n', '% RTB is distributed in the hope that it will be useful,')]; +outstring = [outstring, sprintf('%s \n', '% but WITHOUT ANY WARRANTY; without even the implied warranty of')]; +outstring = [outstring, sprintf('%s \n', '% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the')]; +outstring = [outstring, sprintf('%s \n', '% GNU Lesser General Public License for more details.')]; +outstring = [outstring, sprintf('%s \n', '% ')]; +outstring = [outstring, sprintf('%s \n', '% You should have received a copy of the GNU Leser General Public License')]; +outstring = [outstring, sprintf('%s \n', '% along with RTB. If not, see .')]; +outstring = [outstring, sprintf('%s \n', '%')]; +outstring = [outstring, sprintf('%s \n', '% http://www.petercorke.com');]; +outstring = [outstring, sprintf('%s \n', '%')]; +outstring = [outstring, sprintf('%s \n', '% The code generation module emerged during the work on a project funded by')]; +outstring = [outstring, sprintf('%s \n', '% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully ')]; +outstring = [outstring, sprintf('%s \n', '% acknowledge the financial support.')]; + + + + +end + diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/gengaussjordanc.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/gengaussjordanc.m new file mode 100644 index 0000000..f58aa8c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/gengaussjordanc.m @@ -0,0 +1,180 @@ +%CODEGENERATOR.GENGAUSSJORDANC Generates a Gauss-Jordan C-implementation. +% +% cGen.gengaussjordanc generates a .h and a .c file in the directory +% specified by ccodepath. +% +% Notes:: +% - Is called by genfdyn if cGen has active flag genmex or genccode. +% +% Authors:: +% Joern Malzahn (joern.malzahn@tu-dortmund.de) +% +% See also CodeGenerator, CodeGenerator.gendotprodc. + +% Copyright (C) 1993-2014, by Peter I. Corke +% Copyright (C) 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 Lesser General Public License +% along with RTB. If not, see . +% +% http://www.petercorke.com + +function [] = gengaussjordanc(CGen) + +funname = 'gaussjordan'; +funfilename = [funname,'.c']; +hfilename = [funname,'.h']; +srcDir = fullfile(CGen.ccodepath,'src'); +hdrDir = fullfile(CGen.ccodepath,'include'); + +% Check for already existing gauss-jordan C-files +if exist(fullfile(srcDir,funfilename),'file') && exist(fullfile(srcDir,funfilename),'file') + return; +end + +% Check for existance of C-code directories +if ~exist(srcDir,'dir') + mkdir(srcDir); +end +if ~exist(hdrDir,'dir') + mkdir(hdrDir); +end + +% Create the function description header +hStruct = createHeaderStruct(funname); % create header +if ~isempty(hStruct) + hFString = CGen.constructheaderstringc(hStruct); +end + +%% Generate C implementation file +fid = fopen(fullfile(srcDir,funfilename),'w+'); + +% Includes +fprintf(fid,'%s\n\n',... + ['#include "', hfilename,'"']); + +% Start actual function implementation +fprintf(fid,'%s\n',['void ',funname,'(const double* inMatrix, double* outMatrix, int dim){']); + +fprintf(fid,'%s\n',' '); % empty line + +% variable declarations +fprintf(fid,'\t%s\n','int iRow, iCol, diagIndex;'); +fprintf(fid,'\t%s\n','double diagFactor, tmpFactor;'); +fprintf(fid,'\t%s\n','double* inMatrixCopy = (double*) malloc(dim*dim*sizeof(double));'); + +fprintf(fid,'%s\n',' '); % empty line + +% input initialization +fprintf(fid,'\t%s\n','/* make deep copy of input matrix */'); +fprintf(fid,'\t%s\n','for(iRow = 0; iRow < dim; iRow++ ){'); +fprintf(fid,'\t\t%s\n','for (iCol = 0; iCol < dim; iCol++){'); +fprintf(fid,'\t\t\t%s\n','inMatrixCopy[dim*iCol+iRow] = inMatrix[dim*iCol+iRow];'); +fprintf(fid,'\t\t%s\n','}'); + +fprintf(fid,'\t%s\n','}'); +fprintf(fid,'\t%s\n','/* Initialize output matrix as identity matrix. */'); +fprintf(fid,'\t%s\n','for (iRow = 0; iRow < dim; iRow++ ){'); +fprintf(fid,'\t\t%s\n','for (iCol = 0; iCol < dim; iCol++ ){'); +fprintf(fid,'\t\t\t%s\n','if (iCol == iRow){'); +fprintf(fid,'\t\t\t\t%s\n','outMatrix[dim*iCol+iRow] = 1;'); +fprintf(fid,'\t\t\t%s\n','}'); +fprintf(fid,'\t\t\t%s\n','else{'); +fprintf(fid,'\t\t\t\t%s\n','outMatrix[dim*iCol+iRow] = 0;'); +fprintf(fid,'\t\t\t%s\n','}'); +fprintf(fid,'\t\t%s\n','}'); +fprintf(fid,'\t%s\n','}'); + +fprintf(fid,'%s\n',' '); % empty line + +% actual elimination +fprintf(fid,'\t%s\n','for (diagIndex = 0; diagIndex < dim; diagIndex++ )'); +fprintf(fid,'\t%s\n','{'); +fprintf(fid,'\t\t%s\n','/* determine diagonal factor */'); +fprintf(fid,'\t\t%s\n','diagFactor = inMatrixCopy[dim*diagIndex+diagIndex];'); + +fprintf(fid,'%s\n',' '); % empty line + +fprintf(fid,'\t\t%s\n','/* divide column entries by diagonal factor */'); +fprintf(fid,'\t\t%s\n','for (iCol = 0; iCol < dim; iCol++){'); +fprintf(fid,'\t\t\t%s\n','inMatrixCopy[dim*iCol+diagIndex] /= diagFactor;'); +fprintf(fid,'\t\t\t%s\n','outMatrix[dim*iCol+diagIndex] /= diagFactor;'); +fprintf(fid,'\t\t%s\n','}'); + +fprintf(fid,'%s\n',' '); % empty line + +fprintf(fid,'\t\t%s\n','/* perform line-by-line elimination */'); +fprintf(fid,'\t\t%s\n','for (iRow = 0; iRow < dim; iRow++){'); +fprintf(fid,'\t\t\t%s\n','if (iRow != diagIndex){'); +fprintf(fid,'\t\t\t\t%s\n','tmpFactor = inMatrixCopy[dim*diagIndex+iRow];'); + +fprintf(fid,'%s\n',' '); % empty line + +fprintf(fid,'\t\t\t\t%s\n','for(iCol = 0; iCol < dim; iCol++){'); +fprintf(fid,'\t\t\t\t%s\n','inMatrixCopy[dim*iCol+iRow] -= inMatrixCopy[dim*iCol+diagIndex]*tmpFactor;'); +fprintf(fid,'\t\t\t\t%s\n','outMatrix[dim*iCol+iRow] -= outMatrix[dim*iCol+diagIndex]*tmpFactor;'); +fprintf(fid,'\t\t\t\t%s\n','}'); +fprintf(fid,'\t\t\t%s\n','}'); +fprintf(fid,'\t\t%s\n','} /* line-by-line elimination */'); + +fprintf(fid,'%s\n',' '); % empty line + +fprintf(fid,'\t%s\n','}'); +fprintf(fid,'\t%s\n','free(inMatrixCopy);'); +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'])]); + +% Function prototype +fprintf(fid,'%s\n\n',['void ',funname,'(const double *inMatrix, double *outMatrix, int dim);']); + +% 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(fname) +[~,hStruct.funName] = fileparts(fname); +hStruct.shortDescription = ['Compute the inverse of a positive definite square matrix']; +hStruct.calls = [hStruct.funName,'(const double* inMatrix, double* outMatrix, int dim)']; +hStruct.detailedDescription = {'Given a positive definite square matrix of dimension dim in inMatrix,',... + 'the function returns its inverse outMatrix. The inverse is computed using Gauss-Jordan elimination ',... + 'without pivoting.'}; +hStruct.inputs = { ['inMatrix: array of doubles storing [dim x dim] elements of the input square matrix column by column,'],... + 'dim: dimension of the square matrix.'}; +hStruct.outputs = {['outMatrix: array of doubles storing [dim x dim] elements of the output square matrix column by column.']}; +hStruct.references = {'The C Book: http://publications.gbdirect.co.uk/c_book/'}; +hStruct.authors = {'This is an autogenerated function!',... + 'Code generator written by:',... + 'Joern Malzahn (joern.malzahn@tu-dortmund.de)'}; +hStruct.seeAlso = {'CodeGenerator'}; +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/genmatvecprodc.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/genmatvecprodc.m new file mode 100644 index 0000000..633112f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/genmatvecprodc.m @@ -0,0 +1,125 @@ +%CODEGENERATOR.GENMATVECPRODC Generates a matrix-vector product C-implementation. +% +% cGen.gendotprodc generates a .h and a .c file in the directory +% specified by ccodepath. +% +% Notes:: +% - Is called by geninvdyn and genfdyn if cGen has active flag genmex or genccode. +% +% Authors:: +% Joern Malzahn (joern.malzahn@tu-dortmund.de) +% +% See also CodeGenerator, CodeGenerator.gengaussjordanc. + +% Copyright (C) 1993-2014, by Peter I. Corke +% Copyright (C) 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 Lesser General Public License +% along with RTB. If not, see . +% +% http://www.petercorke.com +function [] = genmatvecprodc(CGen) + +funname = 'matvecprod'; +funfilename = [funname,'.c']; +hfilename = [funname,'.h']; +srcDir = fullfile(CGen.ccodepath,'src'); +hdrDir = fullfile(CGen.ccodepath,'include'); + +% Check for existance of dotProd C-files +if exist(fullfile(srcDir,funfilename),'file') && exist(fullfile(srcDir,funfilename),'file') + return; +end + +% Check for existance of C-code directories +if ~exist(srcDir,'dir') + mkdir(srcDir); +end +if ~exist(hdrDir,'dir') + mkdir(hdrDir); +end + +% Create the function description header +hStruct = createHeaderStruct(funname); % create header +if ~isempty(hStruct) + hFString = CGen.constructheaderstringc(hStruct); +end + +%% 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',['void ', funname, '(double *outVector, const double *inMatrix, const double *inVector, int nRow, int nCol){']); +fprintf(fid,'%s\n',' '); % empty line +fprintf(fid,'\t%s\n','int iRow, iCol = 0;'); +fprintf(fid,'%s\n',' '); % empty line +fprintf(fid,'\t%s\n','for (iRow = 0; iRow < nRow; iRow++){'); +fprintf(fid,'\t\t%s\n','for (iCol = 0; iCol < nCol; iCol++){'); +fprintf(fid,'\t\t\t%s\n','outVector[iCol] += inMatrix[nRow*iRow+iCol] * inVector[iRow];'); +fprintf(fid,'\t\t%s\n','}'); +fprintf(fid,'\t%s\n','} '); +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'])]); + +% Function prototype +fprintf(fid,'%s\n\n',['void ',funname,'(double *outVector, const double *inMatrix, const double *inVector, int nRow, int nCol);']); + +% 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(fname) +[~,hStruct.funName] = fileparts(fname); +hStruct.shortDescription = ['Compute the product of a matrix and a vector']; +hStruct.calls = ['void ',hStruct.funName,'(double *outVector, const double *inMatrix, const double *inVector, int nRow, int nCol)']; +hStruct.detailedDescription = {['Given an [nRow x nCol] inMatrix and a nCol-element inVector, the function'],... + 'computes the nRow-element outVector as: outVector = inMatrix*inVector.'}; +hStruct.inputs = { ['inMatrix: [nRow x nCol] input matrix,'],... + ['inVector: nCol-element input vector,'],... + ['nRow: number of rows of inMatrix,'],... + 'nCol: number of columns of inMatrix and elements of inVector.'}; +hStruct.outputs = {['outVector: nRow-element output vector.']}; +hStruct.references = {'The C Book: http://publications.gbdirect.co.uk/c_book/'}; +hStruct.authors = {'This is an autogenerated function!',... + 'Code generator written by:',... + 'Joern Malzahn (joern.malzahn@tu-dortmund.de)'}; +hStruct.seeAlso = {'CodeGenerator'}; +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/genmexgatewaystring.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/genmexgatewaystring.m new file mode 100644 index 0000000..b2d0cd7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/genmexgatewaystring.m @@ -0,0 +1,171 @@ +%GENMEXGATEWAYSTRING Generates a mex gateway function +% +% [FUNSTR] = genmexgatewaystring(SYMEXPR, ARGLIST) returns a string +% representing a C-code implementation of a mex gateway function. +% +% The argumentlist ARGLIST may contain the following property-value pairs +% PROPERTY, VALUE +% - 'funname', 'name_string' +% 'name_string' is the identifier of the actual computational C-function +% called by the gateway routine. If this optional argument is omitted, +% the identifier 'myfun' is used +% +% - 'vars', {symVar1, symVar2,...} +% The inputs to the actual computational C-code function called by the +% gateway routine 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. +% +% 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] = genmexgatewaystring(myrot,'vars',{Q},'funname','rotate_xyz') +% +% Notes:: +% +% +% Author:: +% Joern Malzahn, (joern.malzahn@tu-dortmund.de) +% +% See also ccode, ccodefunctionstring. + +% 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.petercorke.com + + +function [ gwstring ] = genmexgatewaystring(CGen, f, varargin ) + +%% Read parameters +% option defaults +opt.funname = 'myfun'; +opt.output{1} = zeros(size(f)); +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:genmexgatewaystring:wrongArgumentList',... + 'Wrong number of elements in the argument list.'); +end + +for iArg = 1:2:nargin-2 + switch lower(varargin{iArg}) + case 'funname' + if ~isempty(varargin{iArg+1}) + opt.funname = varargin{iArg+1}; + end + case 'vars' + opt.vars = varargin{iArg+1}; + otherwise + error('genmexgatewaystring:unknownArgument',... + ['Argument ',inputname(iArg),' unknown.']); + end +end + +nIn = numel(opt.vars); + +%% begin to write the function string +% gwstring = sprintf('\n\n\n'); +gwstring = sprintf('\n'); + +gwstring = [gwstring, sprintf('%s\n','/* The gateway function */')]; +gwstring = [gwstring, sprintf('%s\n','void mexFunction( int nlhs, mxArray *plhs[],')]; +gwstring = [gwstring, sprintf('%s\n',' int nrhs, const mxArray *prhs[])')]; +gwstring = [gwstring, sprintf('%s\n','{')]; + +%% variable declaration +gwstring = [gwstring, sprintf('\t%s\n','/* variable declarations */')]; +% output +gwstring = [gwstring, sprintf('\t%s\n','double* outMatrix; /* output matrix */')]; + +% inputs +for iIn = 1:nIn + tmpInName = ['input',num2str(iIn)]; + tmpIn = opt.vars{iIn}; + gwstring = [gwstring, sprintf('\tdouble* %s;\n', tmpInName ) ]; +end + +gwstring = [gwstring, sprintf('%s\n',' ')]; % Empty line + +%% argument checks +% number of input arguments +gwstring = [gwstring, sprintf('\t%s\n','/* check for proper number of arguments */')]; +gwstring = [gwstring, sprintf('\t%s%u%s\n','if(nrhs!=',nIn+1,') {')]; +gwstring = [gwstring, sprintf('\t\t%s%s%s\n','mexErrMsgIdAndTxt("',opt.funname,':nrhs",')]; +gwstring = [gwstring, sprintf('\t\t%s%u%s\n',' "',nIn,' inputs required.");')]; +gwstring = [gwstring, sprintf('\t%s\n','}')]; + +% dimensions of input arguments + +% number of output arguments +gwstring = [gwstring, sprintf('\t%s%u%s\n','if(nlhs>',1,') {')]; +gwstring = [gwstring, sprintf('\t\t%s%s%s\n','mexErrMsgIdAndTxt("',opt.funname,':nlhs",')]; +gwstring = [gwstring, sprintf('\t\t%s\n',' "Only single output allowed.");')]; +gwstring = [gwstring, sprintf('\t%s\n','}')]; + +gwstring = [gwstring, sprintf('%s\n',' ')]; % Empty line + +%% pointer initialization for... +% the output +gwstring = [gwstring, sprintf('\t%s\n','/* allocate memory for the output matrix */')]; +gwstring = [gwstring, sprintf('\t%s\n',['plhs[0] = mxCreateDoubleMatrix(',num2str(size(f,1)),',',num2str(size(f,2)),', mxREAL);'])]; +gwstring = [gwstring, sprintf('\t%s\n','/* get a pointer to the real data in the output matrix */')]; +gwstring = [gwstring, sprintf('\t%s\n','outMatrix = mxGetPr(plhs[0]);')]; +gwstring = [gwstring, sprintf('%s\n',' ')]; % Empty line + +% inputs +gwstring = [gwstring, sprintf('\t%s\n','/* get a pointers to the real data in the input matrices */')]; +for iIn = 1:nIn + tmpInName = ['input',num2str(iIn)];%opt.varsName{iIn}; + tmpIn = opt.vars{iIn}; + + gwstring = [gwstring, sprintf('\t%s\n',[tmpInName,' = mxGetPr(prhs[',num2str(iIn),']);'])]; % first input argument is the robot object, thus count one-based here +end + +gwstring = [gwstring, sprintf('%s\n',' ')]; % Empty line + +%% call computational routine +gwstring = [gwstring, sprintf('\t%s\n','/* call the computational routine */')]; +gwstring = [gwstring, sprintf('\t%s',[opt.funname,'(','outMatrix, '])]; + +% comma separated list of input arguments +for iIn = 1:nIn + tmpInName = ['input',num2str(iIn)]; + + gwstring = [gwstring, sprintf('%s',tmpInName)]; + + % separate argument list by commas + if ( iIn ~= nIn ) + gwstring = [gwstring,', ']; + else + gwstring = [gwstring,sprintf('%s\n',');')]; + end +end + +%% finalize +gwstring = [gwstring, sprintf('%s\n','}')]; + diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/getpibugfixstring.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/getpibugfixstring.m new file mode 100644 index 0000000..926793b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/getpibugfixstring.m @@ -0,0 +1,50 @@ +%CODEGENERATOR.GETPIBUGFIXSTRING Returns a string to fix PI-Bug in auto genereated functions. +% +% bfixString = CGen.getPiBugfixString() Is the string with explanation comment +% and variable declaration as described below. +% +% Notes:: +% In some versions the symbolic toolbox writes the constant $pi$ in +% capital letters. This way autogenerated functions might not work properly. +% To fix this issue a local variable is introduced: +% PI = pi +% +% Authors:: +% Joern Malzahn +% 2012 RST, Technische Universitaet Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% +% See also CodeGenerator.constructheaderstring, CodeGenerator.replaceheader. + +% 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.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 bfixString = getpibugfixstring(CGen) +bfixString = []; +bfixString = [bfixString, sprintf('%s\n','%% Bugfix')]; +bfixString = [bfixString, sprintf('%s\n','% In some versions the symbolic toolbox writes the constant $pi$ in')]; +bfixString = [bfixString, sprintf('%s\n','% capital letters. This way autogenerated functions might not work properly.')]; +bfixString = [bfixString, sprintf('%s\n','% To fix this issue a local variable is introduced:')]; +bfixString = [bfixString, sprintf('%s\n','PI = pi;')]; +bfixString = [bfixString, sprintf('%s\n',' ')]; \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/mexfunction.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/mexfunction.m new file mode 100644 index 0000000..6e31865 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/mexfunction.m @@ -0,0 +1,147 @@ +%MEXFUNCTION Converts a symbolic expression into a MEX-function +% +% [] = cGen.mexfunction(SYMEXPR, ARGLIST) translates a symbolic expression +% into C-code and joins it with a MEX gateway routine. The resulting C-file +% is ready to be compiled using the matlab built-in mex command. +% +% The argumentlist ARGLIST may contain the following property-value pairs +% PROPERTY, VALUE +% - 'funname', 'name_string' +% 'name_string' is the actual identifier of the obtained C-function. +% Default: 'myfun' +% +% - 'funfilename', 'file_name_string' +% 'file_name_string' is the name of the generated C-file including +% relativepath or absolute path information. +% Default: 'myfunfilename' +% +% - 'output', 'output_name' +% Defines the identifier of the output variable in the C-function. +% Default: 'out' +% +% - 'vars', {symVar1, symVar2,...} +% 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 accordingly +% as exemplified above. +% +% - 'header', hStruct +% Struct containing header information. See constructheaderstring +% Default: [] +% +% Example:: +% +% +% Notes:: +% +% Author:: +% Joern Malzahn, (joern.malzahn@tu-dortmund.de) +% +% See also mex, 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.petercorke.com + +function [] = mexfunction(CGen, f, varargin ) + +%% Read parameters +% option defaults +opt.funname = 'myfun'; +opt.vars = {}; +opt.output = 'out'; +opt.funfilename = 'myfunfilename'; +opt.hStruct = []; + +% 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:mexfunction:wrongArgumentList',... + 'Wrong number of elements in the argument list.'); +end + +for iArg = 1:2:nargin-2 + switch lower(varargin{iArg}) + case 'funname' + if ~isempty(varargin{iArg+1}) + opt.funname = varargin{iArg+1}; + end + case 'funfilename' + if ~isempty(varargin{iArg+1}) + opt.funfilename = varargin{iArg+1}; + end + case 'output' + if ~isempty(varargin{iArg+1}) + opt.outputName{1} = varargin{iArg+1}; + end + case 'vars' + opt.vars = varargin{iArg+1}; + case 'header' + opt.hStruct = varargin{iArg+1}; + otherwise + error('genmexgatewaystring:unknownArgument',... + ['Argument ',inputname(iArg),' unknown.']); + end +end + +%% Create Copyright Note +cprNote = CGen.generatecopyrightnote; +cprNote = regexprep(cprNote, '%', '//'); + +%% Create Compilation Command +srcDir = fullfile(CGen.ccodepath,'src'); +hdrDir = fullfile(CGen.ccodepath,'include'); +if CGen.verbose + mexCompCmnd = ['mex ',opt.funfilename, ' ',fullfile(srcDir,[opt.funname,'.c']),' -I',hdrDir, ' -v -outdir ',CGen.robjpath]; +else + mexCompCmnd = ['mex ',opt.funfilename, ' ',fullfile(srcDir,[opt.funname,'.c']),' -I',hdrDir,' -outdir ',CGen.robjpath]; +end + +%% Generate C code +fid = fopen(opt.funfilename,'w+'); + +% Add compilation note +fprintf(fid,'/* %s\n',[upper(opt.funname) ,' - This file contains auto generated C-code for a MATLAB MEX function.']); +fprintf(fid,'// %s\n',['For details on how to use the complied MEX function see the documentation provided in ',opt.funname,'.m']); +fprintf(fid,'// %s\n//\n',['The compiled MEX function replaces this .m-function with identical usage but substantial execution speedup.']); +fprintf(fid,'// %s\n//\n',['For compilation of this C-code using MATLAB please run:']); +fprintf(fid,'// \t\t%s\n//\n',['''',mexCompCmnd,'''']); +fprintf(fid,'// %s\n',['Make sure you have a C-compiler installed and your MATLAB MEX environment readily configured.']); +fprintf(fid,'// %s\n//\n',['Type ''doc mex'' for additional help.']); + +% Insert Copyright Note +fprintf(fid,'// %s\n','__Copyright Note__:'); +fprintf(fid,'%s */\n',cprNote); + +% Includes +fprintf(fid,'%s\n%s\n\n',... + '#include "mex.h"',... + ['#include "',[opt.funname,'.h'],'"']); + +% Generate the mex gateway routine +funstr = CGen.genmexgatewaystring(f,'funname',opt.funname, 'vars',opt.vars); +fprintf(fid,'%s',sprintf(funstr)); + +fclose(fid); + +%% Compile the MEX file +if CGen.compilemex + eval(mexCompCmnd) +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/mexfunctionrowwise.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/mexfunctionrowwise.m new file mode 100644 index 0000000..4e5c293 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/mexfunctionrowwise.m @@ -0,0 +1,154 @@ +%MEXFUNCTION Converts a symbolic expression into a MEX-function +% +% [] = cGen.mexfunction(SYMEXPR, ARGLIST) translates a symbolic expression +% into C-code and joins it with a MEX gateway routine. The resulting C-file +% is ready to be compiled using the matlab built-in mex command. +% +% The argumentlist ARGLIST may contain the following property-value pairs +% PROPERTY, VALUE +% - 'funname', 'name_string' +% 'name_string' is the actual identifier of the obtained C-function. +% Default: 'myfun' +% +% - 'funfilename', 'file_name_string' +% 'file_name_string' is the name of the generated C-file including +% relativepath or absolute path information. +% Default: 'myfunfilename' +% +% - 'output', 'output_name' +% Defines the identifier of the output variable in the C-function. +% Default: 'out' +% +% - 'vars', {symVar1, symVar2,...} +% 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 accordingly +% as exemplified above. +% +% - 'header', hStruct +% Struct containing header information. See constructheaderstring +% Default: [] +% +% Example:: +% +% +% Notes:: +% +% Author:: +% Joern Malzahn, (joern.malzahn@tu-dortmund.de) +% +% See also mex, 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.petercorke.com + +function [] = mexfunctionrowwise(CGen, f, varargin ) + +%% Read parameters +% option defaults +opt.funname = 'myfun'; +opt.vars = {}; +opt.output = 'out'; +opt.funfilename = 'myfunfilename'; +opt.hStruct = []; + +% 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:mexfunction:wrongArgumentList',... + 'Wrong number of elements in the argument list.'); +end + +for iArg = 1:2:nargin-2 + switch lower(varargin{iArg}) + case 'funname' + if ~isempty(varargin{iArg+1}) + opt.funname = varargin{iArg+1}; + end + case 'funfilename' + if ~isempty(varargin{iArg+1}) + opt.funfilename = varargin{iArg+1}; + end + case 'output' + if ~isempty(varargin{iArg+1}) + opt.outputName{1} = varargin{iArg+1}; + end + case 'vars' + opt.vars = varargin{iArg+1}; + case 'header' + opt.hStruct = varargin{iArg+1}; + otherwise + error('genmexgatewaystring:unknownArgument',... + ['Argument ',inputname(iArg),' unknown.']); + end +end + +%% Copyright Note +cprNote = CGen.generatecopyrightnote; +cprNote = regexprep(cprNote, '%', '//'); + +%% Create compilation command +srcDir = fullfile(CGen.ccodepath,'src'); +hdrDir = fullfile(CGen.ccodepath,'include'); +cfilelist = fullfile(srcDir,[opt.funname,'.c']); +for kJoints = 1:CGen.rob.n + cfilelist = [cfilelist, ' ',fullfile(srcDir,[opt.funname,'_row_',num2str(kJoints),'.c'])]; +end +if CGen.verbose + mexCompCmnd = ['mex ',opt.funfilename, ' ',cfilelist,' -I',hdrDir, ' -v -outdir ',CGen.robjpath]; +else + mexCompCmnd = ['mex ',opt.funfilename, ' ',cfilelist,' -I',hdrDir,' -outdir ',CGen.robjpath]; +end + +%% Generate C code +fid = fopen(opt.funfilename,'w+'); + +% Add compilation note +fprintf(fid,'/* %s\n',[upper(opt.funname) ,' - This file contains auto generated C-code for a MATLAB MEX function.']); +fprintf(fid,'// %s\n',['For details on how to use the complied MEX function see the documentation provided in ',opt.funname,'.m']); +fprintf(fid,'// %s\n//\n',['The compiled MEX function replaces this .m-function with identical usage but substantial execution speedup.']); +fprintf(fid,'// %s\n//\n',['For compilation of this C-code using MATLAB please run:']); +fprintf(fid,'// \t\t%s\n//\n',['''',mexCompCmnd,'''']); +fprintf(fid,'// %s\n',['Make sure you have a C-compiler installed and your MATLAB MEX environment readily configured.']); +fprintf(fid,'// %s\n//\n',['Type ''doc mex'' for additional help.']); + +% Insert Copyright Note +fprintf(fid,'// %s\n','__Copyright Note__:'); +fprintf(fid,'%s */\n',cprNote); + +% Includes +fprintf(fid,'%s\n%s\n\n',... + '#include "mex.h"',... + ['#include "',[opt.funname,'.h'],'"']); + +% Generate the mex gateway routine +funstr = CGen.genmexgatewaystring(f,'funname',opt.funname, 'vars',opt.vars); +fprintf(fid,'%s',sprintf(funstr)); + +fclose(fid); + +%% Compile the MEX file +if CGen.compilemex + eval(mexCompCmnd) +end + +CGen.logmsg('\t%s\n',' done!'); + diff --git a/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/replaceheader.m b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/replaceheader.m new file mode 100644 index 0000000..2392715 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@CodeGenerator/private/replaceheader.m @@ -0,0 +1,80 @@ +%CODEGENERATOR.REPLACEHEADER Replace autogenerated function headers by Toolbox-headers. +% +% CGen.replaceheader(HSTRUCT,FILENAME) +% HSTRUCT is the struct defining the contents of the header. +% FILENAME is the relative or full path to the file that has an autogenerated +% header to be replaced. +% +% Notes:: +% The MatLab built-in function "matlabFunction" generates a standard +% header stub of 3 lines. This header is extended by the common +% toolbox-header. The contents of the header are coded in a struct that is +% the first input parameter and has the following self-explaining fields: +% +% Fieldname Datatype +% - funName 'string' +% - shortDescription 'string' +% - detailedDescription {'line1','line2',...} +% - inputs {'input1: description','input2: description',...} +% - outputs {'output1: description','output2: description',...} +% - examples {'line1','line2',...} +% - knownBugs {'line1','line2',...} +% - toDO {'line1','line2',...} +% - references {'line1','line2',...} +% - authors {'line1','line2',...} +% - seeAlso {'function1','function2',...} +% +% Authors:: +% Joern Malzahn +% 2012 RST, Technische Universitaet Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% +% See also matlabFunction, CodeGenerator.constructheaderstring. + +% 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.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 [] = replaceheader(CGen,hStruct,fileName) + +%% Get the first three lines of the original file +threeLines = cell(1,3); % innitialize storage cell +fid = fopen(fileName,'r'); % open the file + +threeLines{1} = fgetl(fid); % read the three lines +threeLines{2} = fgetl(fid); +threeLines{3} = fgetl(fid); + +fclose(fid); % close the file + +%% Delete old header +CGen.ffindreplace(fileName,threeLines{1},''); % Erase first line +CGen.ffindreplace(fileName,threeLines{2},''); % Erase second line +CGen.ffindreplace(fileName,threeLines{3},''); % Erase third line + +%% Insert new header +hFString = CGen.constructheaderstring(hStruct); +CGen.finsertfront(fileName, sprintf([threeLines{1},'\n%s\n%s'],hFString,CGen.getpibugfixstring)); + + +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/SerialLink.m b/lwrserv/matlab/rvctools/robot/@SerialLink/SerialLink.m new file mode 100644 index 0000000..f58fd98 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/SerialLink.m @@ -0,0 +1,586 @@ +%SerialLink Serial-link robot class +% +% A concrete class that represents a serial-link arm-type robot. The +% mechanism is described using Denavit-Hartenberg parameters, one set +% per joint. +% +% Methods:: +% +% plot display graphical representation of robot +% teach drive the graphical robot +% isspherical test if robot has spherical wrist +% islimit test if robot at joint limit +% +% fkine forward kinematics +% ikine6s inverse kinematics for 6-axis spherical wrist revolute robot +% ikine3 inverse kinematics for 3-axis revolute robot +% ikine inverse kinematics using iterative method +% jacob0 Jacobian matrix in world frame +% jacobn Jacobian matrix in tool frame +% maniplty manipulability +% +% jtraj a joint space trajectory +% +% accel joint acceleration +% coriolis Coriolis joint force +% dyn show dynamic properties of links +% fdyn joint motion +% friction friction force +% gravload gravity joint force +% inertia joint inertia matrix +% nofriction set friction parameters to zero +% rne joint torque/force +% payload add a payload in end-effector frame +% perturb randomly perturb link dynamic parameters +% +% Properties (read/write):: +% +% links vector of Link objects (1xN) +% gravity direction of gravity [gx gy gz] +% base pose of robot's base (4x4 homog xform) +% tool robot's tool transform, T6 to tool tip (4x4 homog xform) +% qlim joint limits, [qmin qmax] (Nx2) +% offset kinematic joint coordinate offsets (Nx1) +% name name of robot, used for graphical display +% manuf annotation, manufacturer's name +% comment annotation, general comment +% plotopt options for plot() method (cell array) +% +% Object properties (read only):: +% +% n number of joints +% config joint configuration string, eg. 'RRRRRR' +% mdh kinematic convention boolean (0=DH, 1=MDH) +% +% Note:: +% - SerialLink is a reference object. +% - SerialLink objects can be used in vectors and arrays +% +% Reference:: +% - Robotics, Vision & Control, Chaps 7-9, +% P. Corke, Springer 2011. +% - Robot, Modeling & Control, +% M.Spong, S. Hutchinson & M. Vidyasagar, Wiley 2006. +% +% See also Link, DHFactor. + +% 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.petercorke.com + +classdef SerialLink < handle + + properties + name + gravity + base + tool + manuf + comment + + plotopt + lineopt + shadowopt + + qteach + + fast_rne % mex version of rne detected + + end + + events + Moved + end + + properties (SetAccess = private) + n + links + mdh + T + end + + properties (Dependent = true, SetAccess = private) + config + end + + properties (Dependent = true) + offset + qlim + end + + methods + function r = SerialLink(L, varargin) + %SerialLink Create a SerialLink robot object + % + % R = SerialLink(LINKS, OPTIONS) is a robot object defined by a vector + % of Link objects. + % + % R = SerialLink(DH, OPTIONS) is a robot object with kinematics defined + % by the matrix DH which has one row per joint and each row is + % [theta d a alpha] and joints are assumed revolute. An optional + % fifth column sigma indicate revolute (sigma=0, default) or + % prismatic (sigma=1). + % + % R = SerialLink(OPTIONS) is a null robot object with no links. + % + % R = SerialLink([R1 R2 ...], OPTIONS) concatenate robots, the base of + % R2 is attached to the tip of R1. + % + % R = SerialLink(R1, options) is a deep copy of the robot object R1, + % with all the same properties. + % + % Options:: + % + % 'name', name set robot name property + % 'comment', comment set robot comment property + % 'manufacturer', manuf set robot manufacturer property + % 'base', base set base transformation matrix property + % 'tool', tool set tool transformation matrix property + % 'gravity', g set gravity vector property + % 'plotopt', po set plotting options property + % + % Examples:: + % + % Create a 2-link robot + % L(1) = Link([ 0 0 a1 0], 'standard'); + % L(2) = Link([ 0 0 a2 0], 'standard'); + % twolink = SerialLink(L, 'name', 'two link'); + % + % Robot objects can be concatenated in two ways + % R = R1 * R2; + % R = SerialLink([R1 R2]); + % + % Note:: + % - SerialLink is a reference object, a subclass of Handle object. + % - SerialLink objects can be used in vectors and arrays + % - When robots are concatenated (either syntax) the intermediate base and + % tool transforms are removed since general constant transforms cannot + % be represented in Denavit-Hartenberg notation. + % + % See also Link, SerialLink.plot. + + r.name = 'noname'; + r.manuf = ''; + r.comment = ''; + r.links = []; + r.n = 0; + r.mdh = 0; + r.gravity = [0; 0; 9.81]; + r.base = eye(4,4); + r.tool = eye(4,4); + + r.lineopt = {'Color', 'black', 'Linewidth', 4}; + r.shadowopt = {'Color', 0.7*[1 1 1], 'Linewidth', 3}; + r.plotopt = {}; + + if exist('frne') == 3 + r.fast_rne = true; + end + + if nargin == 0 + % zero argument constructor, sets default values + return; + else + % at least one argument, either a robot or link array + + if isa(L, 'SerialLink') + if length(L) == 1 + % clone the passed robot + for j=1:L.n + newlinks(j) = copy(L.links(j)); + end + r.links = newlinks; + else + % compound the robots in the vector + r = L(1); + for k=2:length(L) + r = r * L(k); + end + end + elseif isa(L, 'Link') + r.links = L; % attach the links + elseif isa(L, 'double') + % legacy matrix + dh_dyn = L; + clear L + for j=1:numrows(dh_dyn) + L(j) = Link(); + L(j).theta = dh_dyn(j,1); + L(j).d = dh_dyn(j,2); + L(j).a = dh_dyn(j,3); + L(j).alpha = dh_dyn(j,4); + + if numcols(dh_dyn) > 4 + L(j).sigma = dh_dyn(j,5); + end + end + r.links = L; + else + error('unknown type passed to robot'); + end + r.n = length(r.links); + end + + % process the rest of the arguments in key, value pairs + opt.name = 'robot'; + opt.comment = []; + opt.manufacturer = []; + opt.base = []; + opt.tool = []; + opt.offset = []; + opt.qlim = []; + opt.plotopt = []; + + [opt,out] = tb_optparse(opt, varargin); + if ~isempty(out) + error('unknown option <%s>', out{1}); + end + + % copy the properties to robot object + p = properties(r); + for i=1:length(p) + if isfield(opt, p{i}) && ~isempty(getfield(opt, p{i})) + r.(p{i}) = getfield(opt, p{i}); + end + end + + % set the robot object mdh status flag + mdh = [r.links.mdh]; + if all(mdh == 0) + r.mdh = mdh(1); + elseif all (mdh == 1) + r.mdh = mdh(1); + else + error('robot has mixed D&H links conventions'); + end + end + + %{ + function delete(r) + disp('in destructor'); + if ~isempty(r.teachfig) + delete(r.teachfig); + end + rh = findobj('Tag', r.name); + for f=rh + delete(f); + end + end + %} + + function r2 = mtimes(r, l) + %SerialLink.mtimes Concatenate robots + % + % R = R1 * R2 is a robot object that is equivalent to mechanically attaching + % robot R2 to the end of robot R1. + % + % Notes:: + % - If R1 has a tool transform or R2 has a base transform these are + % discarded since DH convention does not allow for arbitrary intermediate + % transformations. + if isa(l, 'SerialLink') + r2 = SerialLink(r); + r2.links = [r2.links l.links]; + r2.base = r.base; + r2.n = length(r2.links); + elseif isa(l, 'Link') + r2 = SerialLink(r); + r2.links = [r2.links l]; + r2.n = length(r2.links); + end + end + + function display(r) + %SerialLink.display Display parameters + % + % R.display() displays the robot parameters in human-readable form. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a SerialLink object and the command has no trailing + % semicolon. + % + % See also SerialLink.char, SerialLink.dyn. + + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + if loose + disp(' '); + end + disp(char(r)) + if loose + disp(' '); + end + end + + function s = char(robot) + %SerialLink.char Convert to string + % + % S = R.char() is a string representation of the robot's kinematic parameters, + % showing DH parameters, joint structure, comments, gravity vector, base and + % tool transform. + + s = ''; + for j=1:length(robot) + r = robot(j); + + % informational line + if r.mdh + convention = 'modDH'; + else + convention = 'stdDH'; + end + s = sprintf('%s (%d axis, %s, %s)', r.name, r.n, r.config, convention); + + % comment and other info + line = ''; + if ~isempty(r.manuf) + line = strcat(line, sprintf(' %s;', r.manuf)); + end + if ~isempty(r.comment) + line = strcat(line, sprintf(' %s;', r.comment)); + end + s = char(s, line); + + % link parameters + s = char(s, '+---+-----------+-----------+-----------+-----------+'); + s = char(s, '| j | theta | d | a | alpha |'); + s = char(s, '+---+-----------+-----------+-----------+-----------+'); + s = char(s, char(r.links, true)); + s = char(s, '+---+-----------+-----------+-----------+-----------+'); + + % gravity, base, tool + s_grav = horzcat(char('grav = ', ' ', ' '), mat2str(r.gravity)); + s_grav = char(s_grav, ' '); + s_base = horzcat(char(' base = ',' ',' ', ' '), mat2str(r.base)); + + s_tool = horzcat(char(' tool = ',' ',' ', ' '), mat2str(r.tool)); + + line = horzcat(s_grav, s_base, s_tool); + + s = char(s, ' ', line); + if j ~= length(robot) + s = char(s, ' '); + end + end + end + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % set/get methods + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + function set.tool(r, v) + if isempty(v) + r.base = eye(4,4); + elseif ~ishomog(v) + error('tool must be a homogeneous transform'); + else + r.tool = v; + end + end + + function set.base(r, v) + if isempty(v) + r.base = eye(4,4); + elseif ~ishomog(v) + error('base must be a homogeneous transform'); + else + r.base = v; + end + end + + function set.offset(r, v) + if length(v) ~= length(v) + error('offset vector length must equal number DOF'); + end + L = r.links; + for i=1:r.n + L(i).offset = v(i); + end + r.links = L; + end + + function v = get.offset(r) + v = [r.links.offset]; + end + + function set.qlim(r, v) + if numrows(v) ~= r.n + error('insufficient rows in joint limit matrix'); + end + L = r.links; + for i=1:r.n + L(i).qlim = v(i,:); + end + r.links = L; + end + + function v = get.qlim(r) + L = r.links; + v = zeros(r.n, 2); + for i=1:r.n + if isempty(L(i).qlim) + if L(i).isrevolute + v(i,:) = [-pi pi]; + else + v(i,:) = [-Inf Inf]; + end + else + v(i,:) = L(i).qlim; + end + end + end + + function set.gravity(r, v) + if isvec(v, 3) + r.gravity = v; + else + error('gravity must be a 3-vector'); + end + end + + function v = get.config(r) + v = ''; + for i=1:r.n + v(i) = r.links(i).RP; + end + end + + % general methods + + function v = islimit(r,q) + %SerialLink.islimit Joint limit test + % + % V = R.islimit(Q) is a vector of boolean values, one per joint, + % false (0) if Q(i) is within the joint limits, else true (1). + % + % Notes:: + % - Joint limits are purely advisory and are not used in any + % other function. Just seemed like a useful thing to include... + % + % See also Link.islimit. + L = r.links; + if length(q) ~= r.n + error('argument for islimit method is wrong length'); + end + v = zeros(r.n, 2); + for i=1:r.n + v(i,:) = L(i).islimit(q(i)); + end + end + + function v = isspherical(r) + %SerialLink.isspherical Test for spherical wrist + % + % R.isspherical() is true if the robot has a spherical wrist, that is, the + % last 3 axes are revolute and their axes intersect at a point. + % + % See also SerialLink.ikine6s. + L = r.links(end-2:end); + + v = false; + % first test that all lengths are zero + if ~all( [L(1).a L(2).a L(3).a L(2).d L(3).d] == 0 ) + return + end + + if (abs(L(1).alpha) == pi/2) && (abs(L(1).alpha + L(2).alpha) < eps) + v = true; + return; + end + end + + function payload(r, m, p) + %SerialLink.payload Add payload mass + % + % R.payload(M, P) adds a payload with point mass M at position P + % in the end-effector coordinate frame. + % + % See also SerialLink.rne, SerialLink.gravload. + lastlink = r.links(r.n); + lastlink.m = m; + lastlink.r = p; + end + + function jt = jtraj(r, T1, T2, t, varargin) + %SerialLink.jtraj Joint space trajectory + % + % Q = R.jtraj(T1, T2, K) is a joint space trajectory (KxN) where the joint + % coordinates reflect motion from end-effector pose T1 to T2 in K steps with + % default zero boundary conditions for velocity and acceleration. + % The trajectory Q has one row per time step, and one column per joint, + % where N is the number of robot joints. + % + % Note:: + % - Requires solution of inverse kinematics. R.ikine6s() is used if + % appropriate, else R.ikine(). Additional trailing arguments to R.jtraj() + % are passed as trailing arugments to these functions. + % + % See also jtraj, SerialLink.ikine, SerialLink.ikine6s. + if r.isspherical && (r.n == 6) + q1 = r.ikine6s(T1, varargin{:}); + q2 = r.ikine6s(T2, varargin{:}); + else + q1 = r.ikine(T1, varargin{:}); + q2 = r.ikine(T2, varargin{:}); + end + + jt = jtraj(q1, q2, t); + end + + + function dyn(r, j) + %SerialLink.dyn Display inertial properties + % + % R.dyn() displays the inertial properties of the SerialLink object in a multi-line + % format. The properties shown are mass, centre of mass, inertia, gear ratio, + % motor inertia and motor friction. + % + % R.dyn(J) as above but display parameters for joint J only. + % + % See also Link.dyn. + if nargin == 2 + r.links(j).dyn() + else + r.links.dyn(); + end + end + + function record(r, q) + r.qteach = [r.qteach; q]; + end + + function sr = sym(r) + sr = SerialLink(r); + for i=1:r.n + sr.links(i) = r.links(i).sym; + end + end + end % methods + +end % classdef + +% utility function +function s = mat2str(m) + m(abs(m). +% +% http://www.petercorke.com + + +function qdd = accel(robot, Q, qd, torque) + + n = robot.n; + + if nargin == 2 + if length(Q) ~= (3*robot.n) + error('RTB:accel:badarg', 'Input vector X is length %d, should be %d (q, qd, tau)', length(Q), 3*robot.n); + end + % accel(X) + Q = Q(:)'; % make it a row vector + q = Q(1:n); + qd = Q(n+1:2*n); + torque = Q(2*n+1:3*n); + elseif nargin == 4 + % accel(Q, qd, torque) + + if numrows(Q) > 1 + % handle trajectory by recursion + if numrows(Q) ~= numrows(qd) + error('for trajectory q and qd must have same number of rows'); + end + if numrows(Q) ~= numrows(torque) + error('for trajectory q and torque must have same number of rows'); + end + qdd = []; + for i=1:numrows(Q) + qdd = cat(1, qdd, robot.accel(Q(i,:), qd(i,:), torque(i,:))'); + end + return + else + q = Q'; + if length(q) == n + q = q(:)'; + qd = qd(:)'; + end + if numcols(Q) ~= n + error('q must have %d columns', n); + end + if numcols(qd) ~= robot.n + error('qd must have %d columns', n); + end + if numcols(torque) ~= robot.n + error('torque must have %d columns', n); + end + end + else + error('RTB:accel:badargs', 'insufficient arguments'); + end + + + % compute current manipulator inertia + % torques resulting from unit acceleration of each joint with + % no gravity. + M = rne(robot, ones(n,1)*q, zeros(n,n), eye(n), [0;0;0]); + + % compute gravity and coriolis torque + % torques resulting from zero acceleration at given velocity & + % with gravity acting. + tau = rne(robot, q, qd, zeros(1,n)); + + qdd = M \ (torque - tau)'; \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/animate.m b/lwrserv/matlab/rvctools/robot/@SerialLink/animate.m new file mode 100644 index 0000000..616a46e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/animate.m @@ -0,0 +1,128 @@ +%SerialLink.animate Update a robot animation +% +% R.animate(q) updates an existing animation for the robot R. This will have +% been created using R.plot(). +% +% Updates graphical instances of this robot in all figures. +% +% Notes:: +% - Not a general purpose method, used for Simulink robot animation. +% +% See also SerialLink.plot. + +function animate(robot, q, handles) + + if nargin < 3 + handles = findobj('Tag', robot.name); + end + + for handle=handles' % for each graphical robot instance + h = get(handle, 'UserData'); + animate2( h, q); + end + + +function animate2(h, q) + + robot = h.robot; + n = robot.n; + L = robot.links; + + mag = h.mag; + + % compute the link transforms, and record the origin of each frame + % for the animation. + t = robot.base; + Tn = t; + x = zeros(1,n); + y = zeros(1,n); + z = zeros(1,n); + xs = zeros(1,n); + ys = zeros(1,n); + zs = zeros(1,n); + + % add first point to the link/shadow line, the base + x(1) = t(1,4); + y(1) = t(2,4); + z(1) = t(3,4); + xs(1) = t(1,4); + ys(1) = t(2,4); + zs(1) = h.zmin; + + % add subsequent points + for j=1:n + Tn(:,:,j) = t; + + t = t * L(j).A(q(j)); + + x(j+1) = t(1,4); + y(j+1) = t(2,4); + z(j+1) = t(3,4); + xs(j+1) = t(1,4); + ys(j+1) = t(2,4); + zs(j+1) = h.zmin; + end + t = t *robot.tool; + + % + % draw the robot stick figure and the shadow + % + set(h.links,'xdata', x, 'ydata', y, 'zdata', z); + if isfield(h, 'shadow') + set(h.shadow,'xdata', xs, 'ydata', ys, 'zdata', zs); + end + + + % + % display the joints as cylinders with rotation axes + % + if isfield(h, 'joint') + xyz_line = [0 0; 0 0; -2*mag 2*mag; 1 1]; + + for j=1:n + % get coordinate data from the cylinder + xyz = get(h.joint(j), 'UserData'); + xyz = Tn(:,:,j) * xyz; + ncols = numcols(xyz)/2; + xc = reshape(xyz(1,:), 2, ncols); + yc = reshape(xyz(2,:), 2, ncols); + zc = reshape(xyz(3,:), 2, ncols); + + set(h.joint(j), 'Xdata', xc, 'Ydata', yc, ... + 'Zdata', zc); + + xyzl = Tn(:,:,j) * xyz_line; + if isfield(h, 'jointaxis') + set(h.jointaxis(j), 'Xdata', xyzl(1,:), ... + 'Ydata', xyzl(2,:), ... + 'Zdata', xyzl(3,:)); + set(h.jointlabel(j), 'Position', xyzl(1:3,1)); + end + end + end + + % + % display the wrist axes and labels + % + if isfield(h, 'x') + % + % compute the wrist axes, based on final link transformation + % plus the tool transformation. + % + xv = t*[mag;0;0;1]; + yv = t*[0;mag;0;1]; + zv = t*[0;0;mag;1]; + + % + % update the line segments, wrist axis and links + % + set(h.x,'xdata',[t(1,4) xv(1)], 'ydata', [t(2,4) xv(2)], ... + 'zdata', [t(3,4) xv(3)]); + set(h.y,'xdata',[t(1,4) yv(1)], 'ydata', [t(2,4) yv(2)], ... + 'zdata', [t(3,4) yv(3)]); + set(h.z,'xdata',[t(1,4) zv(1)], 'ydata', [t(2,4) zv(2)], ... + 'zdata', [t(3,4) zv(3)]); + set(h.xt, 'Position', xv(1:3)); + set(h.yt, 'Position', yv(1:3)); + set(h.zt, 'Position', zv(1:3)); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/cinertia.m b/lwrserv/matlab/rvctools/robot/@SerialLink/cinertia.m new file mode 100644 index 0000000..05924ec --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/cinertia.m @@ -0,0 +1,39 @@ +%SerialLink.cinertia Cartesian inertia matrix +% +% M = R.cinertia(Q) is the NxN Cartesian (operational space) inertia matrix which relates +% Cartesian force/torque to Cartesian acceleration at the joint configuration Q, and N +% is the number of robot joints. +% +% See also SerialLink.inertia, SerialLink.rne. + +% MOD HISTORY +% 4/99 add object support +% $Log: not supported by cvs2svn $ +% $Revision: 1.2 $ + + + +% 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.petercorke.com + +function Mx = cinertia(robot, q) + J = jacob0(robot, q); + Ji = inv(J); %#ok<*MINV> + M = inertia(robot, q); + Mx = Ji' * M * Ji; diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/collisions.m b/lwrserv/matlab/rvctools/robot/@SerialLink/collisions.m new file mode 100644 index 0000000..6875ebc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/collisions.m @@ -0,0 +1,148 @@ +%SerialLink.COLLISIONS Perform collision checking +% +% C = R.collisions(Q, MODEL) is true if the SerialLink object R at +% pose Q (1xN) intersects the solid model MODEL which belongs to the +% class CollisionModel. The model comprises a number of geometric +% primitives and associate pose. +% +% C = R.collisions(Q, MODEL, DYNMODEL, TDYN) as above but also checks +% dynamic collision model DYNMODEL whose elements are at pose TDYN. +% TDYN is an array of transformation matrices (4x4xP), where +% P = length(DYNMODEL.primitives). The P'th plane of TDYN premultiplies the +% pose of the P'th primitive of DYNMODEL. +% +% C = R.collisions(Q, MODEL, DYNMODEL) as above but assumes TDYN is the +% robot's tool frame. +% +% Trajectory operation:: +% +% If Q is MxN it is taken as a pose sequence and C is Mx1 and the collision +% value applies to the pose of the corresponding row of Q. TDYN is 4x4xMxP. +% +% Notes:: +% - Requires the pHRIWARE package which defines CollisionModel class. +% Available from: https://code.google.com/p/phriware/ . +% - The robot is defined by a point cloud, given by its points property. +% - The function does not currently check the base of the SerialLink +% object. +% - If MODEL is [] then no static objects are assumed. +% +% Author:: +% Bryan Moutrie +% +% See also CollisionModel, SerialLink. + +% Copyright (C) Bryan Moutrie, 2013-2015 +% Licensed under the GNU Lesser General Public License +% see full file for full statement +% +% LICENSE STATEMENT: +% +% This file is part of pHRIWARE. +% +% pHRIWARE 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. +% +% pHRIWARE 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 General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public +% License along with pHRIWARE. If not, see . + +function c = collisions(robot, q, cmdl, dyn_cmdl, dyn_T) + + if ~exist('pHRIWARE') + error('rtb:collisions:nosupport', 'You need to install pHRIWARE in order to use this functionality'); + end + + % VERSION WITH BASE CHECKING + % pts = robot.points; + pts = robot.points(end-robot.n+1:end); + for i = length(pts): -1: 1 + numPts(i) = size(pts{i},1); + pts{i}(:,4) = 1; + pts{i} = pts{i}'; + end + + if isempty(cmdl), checkfuns = []; else checkfuns = cmdl.checkFuns; end + if nargin > 3 + dyn_checkfuns = dyn_cmdl.checkFuns; + if nargin == 4 || isempty(dyn_T) + tool = robot.tool; + dyn_T = []; + end + else + dyn_checkfuns = []; + end + + % VERSION WITH BASE CHECKING + % base = length(pts) - robot.n; + % switch base + % case 0 + % % No base + % case 1 + % T = robot.base; + % trPts = T * pts{1}; + % points = trPts (1:3,:)'; + % if any(checkfuns{1}(points(:,1),points(:,2),points(:,3))) + % C(:) = 1; + % display('Base is colliding'); + % return; + % end + % otherwise + % error('robot has missing or extra points'); + % end + + poses = size(q,1); + c = false(poses, 1); + + nan = any(isnan(q),2); + c(nan) = true; + notnan = find(~nan)'; + + T0 = robot.base; + + for p = notnan + T = T0; + prevPts = 0; + for i = 1: robot.n; + T = T * robot.links(i).A(q(p,i)); + if numPts(i) % Allows some links to be STLless + % VERSION WITH BASE CHECKING + % nextPts = prevPts+numPts(i+base); + % trPts(:,prevPts+1:nextPts) = T * pts{i+base}; + nextPts = prevPts+numPts(i); + trPts(:,prevPts+1:nextPts) = T * pts{i}; + prevPts = nextPts; + end + end + + % Does same thing as cmdl.collision(trPoints(1:3,:)'), but + % Does not have to access object every time - quicker + for i = 1: length(checkfuns) + if any(checkfuns{i}(trPts(1,:), trPts(2,:), trPts(3,:))) + c(p) = true; + break; + end + end + + % Then check the dynamic collision models, if any + for i = 1: length(dyn_checkfuns) + if isempty(dyn_T) + dyn_trPts = T*tool \ trPts; + else + dyn_trPts = dyn_T(:,:,p,i) \ trPts; + end + if any(dyn_checkfuns{i}(dyn_trPts(1,:), dyn_trPts(2,:), ... + dyn_trPts(3,:))) + c(p) = true; + break; + end + end + + end +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/coriolis.m b/lwrserv/matlab/rvctools/robot/@SerialLink/coriolis.m new file mode 100644 index 0000000..5a7acae --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/coriolis.m @@ -0,0 +1,122 @@ +%SerialLink.coriolis Coriolis matrix +% +% C = R.coriolis(Q, QD) is the Coriolis/centripetal matrix (NxN) for +% the robot in configuration Q and velocity QD, where N is the number of +% joints. The product C*QD is the vector of joint force/torque due to velocity +% coupling. The diagonal elements are due to centripetal effects and the +% off-diagonal elements are due to Coriolis effects. This matrix is also +% known as the velocity coupling matrix, since gives the disturbance forces +% on all joints due to velocity of any joint. +% +% If Q and QD are matrices (KxN), each row is interpretted as a joint state +% vector, and the result (NxNxK) is a 3d-matrix where each plane corresponds +% to a row of Q and QD. +% +% C = R.coriolis( QQD) as above but the matrix QQD (1x2N) is [Q QD]. +% +% Notes:: +% - Joint friction is also a joint force proportional to velocity but it is +% eliminated in the computation of this value. +% - Computationally slow, involves N^2/2 invocations of RNE. +% +% See also SerialLink.rne. + + + + +% 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.petercorke.com + +function C = coriolis(robot, q, qd) + + n = robot.n; + + if nargin == 2 + % coriolis( [q qd] ) + if numcols(q) ~= 2*n + error('RTB:coriolis:badarg', 'arg must have %d columns', 2*n); + end + qd = q(:,n+1:end); + q = q(:,1:n); + else + if numcols(q) ~= n + error('RTB:coriolis:badarg', 'Cq must have %d columns', n); + end + if numcols(qd) ~= n + error('RTB:coriolis:badarg', 'qd must have %d columns', n); + end + end + + % we need to create a clone robot with no friciton, since friction + % is also proportional to joint velocity + robot2 = robot.nofriction('all'); + + if numrows(q) > 1 + if numrows(q) ~= numrows(qd) + error('RTB:coriolis:badarg', 'for trajectory q and qd must have same number of rows'); + end + C = []; + for i=1:numrows(q) + C = cat(3, C, robot2.coriolis(q(i,:), qd(i,:))); + end + return + end + + N = robot2.n; + + if isa(q, 'sym') + C(N,N) = sym(); + Csq(N,N) = sym(); + else + + C = zeros(N,N); + Csq = zeros(N,N); + end + + + % find the torques that depend on a single finite joint speed, + % these are due to the squared (centripetal) terms + % + % set QD = [1 0 0 ...] then resulting torque is due to qd_1^2 + for j=1:N + QD = zeros(1,N); + QD(j) = 1; + tau = robot2.rne(q, QD, zeros(size(q)), [0 0 0]'); + Csq(:,j) = Csq(:,j) + tau.'; + end + + % find the torques that depend on a pair of finite joint speeds, + % these are due to the product (Coridolis) terms + % set QD = [1 1 0 ...] then resulting torque is due to + % qd_1 qd_2 + qd_1^2 + qd_2^2 + for j=1:N + for k=j+1:N + % find a product term qd_j * qd_k + QD = zeros(1,N); + QD(j) = 1; + QD(k) = 1; + tau = robot2.rne(q, QD, zeros(size(q)), [0 0 0]'); + C(:,k) = C(:,k) + (tau.' - Csq(:,k) - Csq(:,j)) * qd(j); + end + end + C = C + Csq * diag(qd); + + if isa(q, 'sym') + C = simplify(C); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockCGJacobianDH.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockCGJacobianDH.m new file mode 100644 index 0000000..a2342ce --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockCGJacobianDH.m @@ -0,0 +1,102 @@ +function [status] = createBlockCGJacobianDH( rob, varargin ) +%% CREATEBLOCKCGJACOBIANDH Create Embedded Matlab Function block for robot link centre of gravity jacobians. +% ========================================================================= +% +% [status] = createBlockCGJacobianDH( rob ) +% [status] = createBlockCGJacobianDH( rob, robOpts ) +% +% +% Input: +% rob : Robot definition struct. +% robOpts: Optional robot model generation options struct. +% +% Output: +% status: 1 If block has been created. +% 0 If block has NOT been created. +% +% Example: +% rob = stanfordarm3; +% cgJacobian = createBlockCGJacobianDH(rob) +% +% Known Bugs: +% --- +% +% TODO: +% --- +% +% References: +% --- +% +% Authors: +% Jörn Malzahn +% +% See also createCGJacobianDH,createBlockDirKinematicDH. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +%% Handle robot modeling options +if nargin > 1 + robOpts = completeRobOpts(varargin{1}); +else + robOpts = getRobotModelOpts('default'); +end + +% Output to logfile? +if ~isempty(robOpts.logFile) + logFid = fopen(robOpts.logFile,'a'); +else + logFid = []; +end + +%% Prerequesites +status = 0; +curPath = [pwd '\',rob.name,'\']; % compose path to output directory +if (~exist(curPath,'dir')) % check whether output directory exists, otherwise no blocks can be created + error('Nothing to create a library from. Create files first.') +else + addpath(curPath); % if the output directory exists, make sure it is on the search path +end + +%% Gather information about the robot +mdlName = ['mdl',rob.name]; +nJoints = size(rob.DH,1); + +%% Open or create block library +load_system('simulink'); +if exist([curPath,mdlName],'file') % open existing block library if it already exists + open_system(mdlName) +else + new_system(mdlName,'Library', 'ErrorIfShadowed'); % create new block library if none exists + open_system(mdlName) +end +set_param(mdlName,'lock','off') % unlock library to modify contents + +%% Load Jacobian +fName = [curPath,'cgJacobianDH.mat']; % read precomputed cgJacobians +if exist(fName,'file') + tmpStruct = load(fName); + + for iJoints = 1:nJoints % acutal block creation + blockAddress = [mdlName,'/cgJaconian','0',num2str(iJoints)]; + symExpr2SLBlock(blockAddress,tmpStruct.cgJacobians{iJoints}); + end + + status = 1; % block successfully created, change return status +end + + + +%% Tidy up, relock and close library +distributeBlocks( mdlName ); +set_param(mdlName,'lock','on'); +save_system(mdlName, [curPath,mdlName]); +close_system(mdlName); +end + + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockDirKinematicDH.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockDirKinematicDH.m new file mode 100644 index 0000000..ad2dc35 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockDirKinematicDH.m @@ -0,0 +1,105 @@ +function [status] = createBlockDirKinematicDH( rob, varargin ) +%% CREATEBLOCKDIRKINEMATICDH Create Embedded Matlab Function block for the analytic forward kinematics. +% ========================================================================= +% +% [status] = createBlockDirKinematicDH( rob ) +% [status] = createBlockDirKinematicDH( rob, robOpts) +% +% +% Input: +% rob : Robot definition struct. +% robOpts: Optional robot model generation options struct. +% +% Output: +% status: 1 If block has been created. +% 0 If block has NOT been created. +% +% Example: +% rob = stanfordarm3; +% status = createBlockDirKinematicDH(rob); +% +% Known Bugs: +% --- +% +% TODO: +% --- +% +% References: +% --- +% +% Authors: +% Jörn Malzahn +% +% See also createDirKinematicDH,createBlockJacobianDH. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +%% Handle robot modeling options +if nargin > 1 + robOpts = completeRobOpts(varargin{1}); +else + robOpts = getRobotModelOpts('default'); +end + +% Output to logfile? +if ~isempty(robOpts.logFile) + logFid = fopen(robOpts.logFile,'a'); +else + logFid = []; +end + +%% Prerequesites +status = 0; +curPath = fullfile(pwd, rob.name); % compose path to output directory + +if (~exist(curPath,'dir')) % check whether output directory exists, otherwise no blocks can be created + error('Nothing to create a library from. Create files first.') +else + addpath(curPath); % if the output directory exists, make sure it is on the search path +end + +%% Gather information about the robot +nJoints = size(rob.DH,1); +mdlName = ['mdl',rob.name]; + +%% Open or create block library +load_system('simulink'); +if exist(fullfile(curPath,mdlName),'file') % open existing block library if it already exists + open_system(mdlName) +else + new_system(mdlName,'Library'); % create new block library if none exists + open_system(mdlName) +end +set_param(mdlName,'lock','off'); % unlock library to modify contents + +%% Load Forward Kinematics +fName = fullfile(curPath,'DirKinematicDH.mat'); +if exist(fName,'file') + tmpStruct = load(fName); % read precomputed kinematics + + for iJoints = 1:nJoints % acutal block creation + if iJoints ~= nJoints + blockAddress = [mdlName,'/dirKin',num2str(iJoints),'0']; % treat intermediate transformations separately + symExpr2SLBlock(blockAddress,tmpStruct.dirKinC{iJoints}); + else + blockAddress = [mdlName,'/dirKin']; + symExpr2SLBlock(blockAddress,tmpStruct.dirKinC{iJoints}); % full forward kinematics + end + end + + status = 1; % blocks successfully created, change return status +end + +%% Tidy up, relock and close library +distributeBlocks( mdlName ); +set_param(mdlName,'lock','on'); +save_system(mdlName, fullfile(curPath,mdlName)); +close_system(mdlName); +end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockJacobianDH.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockJacobianDH.m new file mode 100644 index 0000000..3653c38 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createBlockJacobianDH.m @@ -0,0 +1,102 @@ +function [status] = createBlockJacobianDH( rob, varargin) +%% CREATEBLOCKJACOBIANDH Create Embedded Matlab Function block for the geometric robot joint jacobian. +% ========================================================================= +% +% [status] = createBlockJacobianDH( rob ) +% [status] = createBlockJacobianDH( rob, robOpts ) +% +% Description: +% enter the detailed description here +% +% Input: +% rob : Robot definition struct. +% robOpts: Optional robot model generation options struct. +% +% Output: +% status: 1 If block has been created. +% 0 If block has NOT been created. +% +% Example: +% rob = stanfordarm3; +% cgJacobian = createBlockJacobianDH(rob) +% +% Known Bugs: +% --- +% +% TODO: +% --- +% +% References: +% --- +% +% Authors: +% Jörn Malzahn +% +% See also createJacobianDH,createBlockDirKinematicDH. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +%% Handle robot modeling options +if nargin > 1 + robOpts = completeRobOpts(varargin{1}); +else + robOpts = getRobotModelOpts('default'); +end + +% Output to logfile? +if ~isempty(robOpts.logFile) + logFid = fopen(robOpts.logFile,'a'); +else + logFid = []; +end + +%% Prerequesites +status = 0; +curPath = [pwd '\',rob.name,'\']; % compose path to output directory + +if (~exist(curPath,'dir')) % check whether output directory exists, otherwise no blocks can be created + error('Nothing to create a library from. Create files first.') +else + addpath(curPath); % if the output directory exists, make sure it is on the search path +end + +%% Gather information about the robot +mdlName = ['mdl',rob.name]; + +%% Open or create block library +load_system('simulink'); + +if exist([curPath,mdlName],'file') % open existing block library if it already exists + open_system(mdlName) +else + new_system(mdlName,'Library', 'ErrorIfShadowed'); % create new block library if none exists + open_system(mdlName) +end +set_param(mdlName,'lock','off'); % unlock library to modify contents + +%% Load Jacobian +fName = [curPath,'eeJacobian.mat']; % read precomputed kinematics + +if exist(fName,'file') + tmpStruct = load(fName); + + blockAddress = [mdlName,'/eeJacobian']; % acutal block creation + symExpr2SLBlock(blockAddress,tmpStruct.eeJacobian); + + status = 1; % blocks successfully created, change return status +end + + +%% Tidy up, relock and close library +distributeBlocks( mdlName ); +set_param(mdlName,'lock','on'); +save_system(mdlName, [curPath,mdlName]); +close_system(mdlName); +end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createCGJacobianDH.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createCGJacobianDH.m new file mode 100644 index 0000000..7b49542 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createCGJacobianDH.m @@ -0,0 +1,217 @@ +function [cgJacobians] = createCGJacobianDH(rob,varargin) +%% CREATECGJACOBIANDH Computes the symbolic geometric robot link centre of gravity jacobians for open chain robot arms. +% ========================================================================= +% +% [cgJacobians] = createCGJacobianDH(rob) +% [cgJacobians] = createCGJacobianDH(rob,robOpts) +% +% Description: +% The function computes the symbolic expression of the geometric +% per link centre of gravity jacobians for the robot defined in the +% input structure rob. The optional robOpts structure allows to specify +% the automatic generation of m-functions and real-time capable simulink +% blocks. +% +% Up to now only serial rigid robot arms are supported. Joints may be +% revolute or prismatic. +% +% If the createMFun flag is set in the robOpts structure, the function +% creates an m-file for the geometric centre of gravity jacobians. The +% m-function takes a vector of generalized joint values as well as a +% vector of generalized joint velocities as input and outputs the +% numerical jacobian matrix for these joint values. The m-functions are +% stored in a subdirectory named after the robot. +% +% If the createSLBlock flag is set in the robOpts structure, a Simulink +% Embedded Matlab Function Block is automatically generated for the +% geometric centre of gravity jacobians. The blocks take a vector of +% generalized joint values as input and output the numerical jacobian. +% The blocks are added to a block library named "mdlrobotname" in the +% subdirectory named after the robot. +% +% Input: +% rob : Robot definition structure +% robOpts: Optional robot model generation options structure. +% +% Output: +% cgJacobians: {1xn} including the geometrical [6xn] centre of garvity +% jacobian for each link +% +% Example: +% rob = stanfordarm3; +% cgJacobian = createCGJacobianDH(rob) +% +% Known Bugs: +% +% 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 +% +% Authors: +% Jörn Malzahn +% +% +% See also createDirKinematicDH,stanfordarm3,getRobModelOpts. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +%% Handle robot modeling options +if nargin > 1 + robOpts = completeRobOpts(varargin{1}); +else + robOpts = getRobotModelOpts('default'); +end + +% Output to logfile? +if ~isempty(robOpts.logFile) + logFid = fopen(robOpts.logFile,'a'); +else + logFid = []; +end + +%% Load existing or invoke computation of forward kinematics +nJoints = size(rob.DH,1); % Number of joints +q = createGenCoordinates(rob); + +curPath = [pwd '\',rob.name,'\']; % compose path to output directory +if (~exist(curPath,'dir')) % create output directory if it does not already exist + dirKinC = createDirKinematicDH(rob,robOpts); +else + tmp = load([curPath,'\DirKinematicDH.mat']); + dirKinC = tmp.dirKinC; +end + +multIDFprintf([robOpts.verbose, logFid],... + '%s: - Creating geometric centre of gravity jacobians ...\n',datestr(now)); + +%% Compute the linear motion jacobians for all centres of gravity +multIDFprintf([robOpts.verbose, logFid],... + '\tProcessing translation part for joint: '); +posJacobians = cell(1,nJoints); +for iJoints = 1:nJoints + multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints); + posJacobians{iJoints} = ... + simplify(jacobian(dirKinC{iJoints}(1:3,4)+dirKinC{iJoints}(1:3,1:3)*rob.COG(:,iJoints),q )); +end +multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + +% Compute the rotary motion jacobians for all centres of gravity +rotJacobians = cell(1,nJoints); % Initialize jacobian container +z0 = [0 0 1].'; % default rotary axis. +rotJacobians{1} = sym(zeros(3,nJoints)); +multIDFprintf([robOpts.verbose, logFid],... + '\tProcessing rotational part for joint: '); +for iJoints = 1:nJoints + multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints); + if rob.joints(iJoints) ~= 'r' + if iJoints ==1 + % do nothing for the first joint + else + rotJacobians{iJoints} = rotJacobians{iJoints-1}; % copy old jacobian + end + + else + if iJoints ==1 + rotJacobians{iJoints}(:,1) = z0; + else + rotJacobians{iJoints} = rotJacobians{iJoints-1}; % copy old jacobian + rotJacobians{iJoints}(:,iJoints) = dirKinC{iJoints-1}(1:3,1:3)*z0; % insert new axis of rotation expressed in base frame + end + end +end +multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + +cgJacobians = cell(1,nJoints); +multIDFprintf([robOpts.verbose, logFid],'\t%s ','Now simplifying jacobian for joint...'); +for iJoints = 1:nJoints + multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints); + cgJacobians{iJoints} = simplify([posJacobians{iJoints};rotJacobians{iJoints}]); +end +multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + + +if robOpts.saveResult + multIDFprintf([robOpts.verbose, logFid],... + '\tSaving computed jacobians: '); + % cosmetics to the symbolic equations + save([curPath,'cgJacobianDH'],'cgJacobians'); + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); +end + +%% Create m-functions +if (robOpts.createMFun) + multIDFprintf([robOpts.verbose, logFid],... + '\tGenerating m-function for joint: '); + + % create per joint centre of gravity jacobian m-functions + for iJoints=1:nJoints + multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints); + + funName = ['CGJacobian_0_',num2str(iJoints)]; + fileName = [curPath,rob.name,funName,'.m']; % intermediate joint forward kinematics + + + matlabFunction(cgJacobians{iJoints},'file',fileName,... % generate function m-file + 'outputs', {'cgJ'},... + 'vars', {q(1:end)}); + + hStruct = createHeaderStruct(rob,iJoints,nJoints,funName); % replace autogenerated function header + replaceHeader(hStruct,fileName); + + end + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); +end + +%% Create Simulink Embedded Matlab Function Block +if robOpts.createSLBlock + multIDFprintf([robOpts.verbose, logFid],... + '\tGenerating Simulink Block... '); + status = createBlockCGJacobianDH( rob ); + if (status == 1) + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + else + multIDFprintf([robOpts.verbose*2, logFid],'\t%s\n',' failed!'); + end +end + +if ~isempty(robOpts.logFile) + fclose(logFid); +end + +%% Are the results correct? +if robOpts.selfCheck +% [testResult.cgJacobian, maxErr ] = checkCGJacobian(rob,robOpts); +end + +end % of main function + +%% Definition of the RST-header contents for each generated file +function hStruct = createHeaderStruct(rob,curBody,nBodies,fName) +hStruct.funName = fName; +hStruct.shortDescription = ['Centre of gravity jacobians for the ',rob.name,' arm up to link ',int2str(curBody),' of ',int2str(nBodies),'.']; +hStruct.calls= {['cgJ = ',fName,'(q)']}; +hStruct.detailedDescription = {['Given a full set of ',int2str(nBodies),' joint variables the function'],... + 'computes the geometric centre of gravity jacobian with respect to the',... + ['base frame for the link subsequent to joint ',int2str(curBody),'.']}; +hStruct.inputs = {['q: ',int2str(nBodies),'-element vector of generalized coordinates.'],... + ' Angles have to be given in radians!'}; +hStruct.outputs = {['cgJ: [6x',num2str(nBodies),'] geometric jacobian relating the Cartesian'],... + [' velocity of the centre of gravity belonging to link ',int2str(curBody),' of ',int2str(nBodies)],... + 'expressed in the robot 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!',... + 'Generator createCGJacobianDH written by:',... + 'Jörn Malzahn '}; +hStruct.seeAlso = {rob.name,'createCGJacobianDH'}; +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createDirKinematicDH.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createDirKinematicDH.m new file mode 100644 index 0000000..2770da8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createDirKinematicDH.m @@ -0,0 +1,198 @@ +function [dirKinC varargout] = createDirKinematicDH(rob,varargin) +%% CREATEDIRKINEMATICDH Computes the symbolic forward kinematics. +% ========================================================================= +% +% [dirKinC] = createDirKinematicDH(rob) +% [dirKinC] = createDirKinematicDH(rob,robOpts) +% +% Description: +% The function outputs the symbolic representation of the per joint +% analytic forward kinematics for the robot specified by rob. The +% optional robOpts structure allows to specify the automatic generation +% of m-functions and real-time capable simulink blocks. +% +% The computation of the forward kinematics expression is based on the +% Denavit-Hartenberg (DH) formalism. Up to now only serial rigid robot +% arms are supported. Joints may be revolute or prismatic. +% +% If the createMFun flag is set in the robOpts structure, m-files are +% generated for the forward kinematics of each joint as well as the +% end-effector. The generated m-functions take a vector of generalized +% joint values as input and output the numerical homogenous +% transformation. The m-functions are stored in a subdirectory named +% after the robot. +% +% If the createSLBlock flag is set in the robOpts structure, Simulink +% Embedded Matlab Function Blocks are automatically generated for the +% forward kinematics of each joint as well as the end-effector. The +% Embedded Matlab Function Blocks take a vector of generalized joint +% values as input and output the numerical homogenous transformation. +% The blocks are added to a block library named "mdlrobotname" in the +% subdirectory named after the robot. +% +% Input: +% rob : Robot definition structure +% robOpts: Optional robot model generation options structure. +% +% Output: +% dirKinC: {1xn} containing [4x4] symbolic homogenous transforms for the +% direct kinematics for each of the n bodies. +% +% Example: +% rob = stanfordarm3; +% dirKinC = createDirKinematicDH(rob) +% +% Known Bugs: +% +% 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 +% +% Authors: +% Jörn Malzahn +% +% +% See also createJacobianDH,stanfordarm3,getRobModelOpts. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +%% Handle robot modeling options +% Read user specified options +if nargin > 1 + robOpts = completeRobOpts(varargin{1}); +else + robOpts = getRobotModelOpts('default'); +end + +% Output to logfile? +if ~isempty(robOpts.logFile) + logFid = fopen(robOpts.logFile,'a'); +else + logFid = []; +end + +multIDFprintf([robOpts.verbose, logFid],... + '%s: - Creating forward kinematics ...\n',datestr(now)); + +%% Gather information about the robot and prepare temporary variables +nBodies = rob.n; +[ q phie dPhiedx] = createGenCoordinates(rob); % create symbolic variables for all generalized coordinates +if (robOpts.createMFun) + curPath = fullfile(pwd, rob.name); % compose path to output directory + if (~exist(curPath,'dir')) % create output directory if it does not already exist + mkdir(curPath); + end +end + +%% Create the direct kinematics symbolic expression +dirKin = eye(4); % initialize containers for speed +dirKinC = cell(1,nBodies); +stateOffset = 0; + +multIDFprintf([robOpts.verbose, logFid],'\tProcessing joint: '); +for iBodies = 1:nBodies + multIDFprintf([robOpts.verbose, logFid],' %i ',iBodies); + + T = rob.links(iBodies).sym.A(q(iBodies)); + + % Append the current homogenous transformation to the kinematic chain + dirKin = dirKin*T; + dirKinC{1,iBodies} = dirKin; +end +multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + +%{ +%% Save results +if (robOpts.saveResult) + savePath = fullfile(curPath,'DirKinematicDH'); + multIDFprintf([robOpts.verbose, logFid],... + '\tSaving computed kinematics: '); + % cosmetics to the symbolic direct kinematics equation + dirKin = simple(dirKin); + save(savePath,'dirKinC'); + + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); +end + +%% Create m-functions +if (robOpts.createMFun) + multIDFprintf([robOpts.verbose, logFid],... + '\tGenerating m-function for joint: '); + + % create per joint direct kinematics m-functions + for iBodies=1:nBodies + multIDFprintf([robOpts.verbose, logFid],' %i ',iBodies); + + if (iBodies ~= nBodies) + funName = ['DirKinDH_0_',num2str(iBodies)]; + fileName = fullfile(curPath,rob.name,... % intermediate joint direct kinematics + [funName,'.m']); + else + funName = 'DirKinDH'; + fileName = fullfile(curPath,rob.name, [funName,'.m']); % end-effector direct kinematics + end + + matlabFunction(dirKinC{1,iBodies},'file',fileName,... % generate function m-file + 'outputs', {'T'},... + 'vars', {[q(1:end), findsym(phie),findsym(dPhiedx)]}); + + hStruct = createHeaderStruct(rob,iBodies,nBodies,funName); % replace autogenerated function header + replaceHeader(hStruct,fileName); + + end + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); +end + +%% Create Simulink Embedded Matlab Function Block +if robOpts.createSLBlock + multIDFprintf([robOpts.verbose, logFid],... + '\tGenerating Simulink Block... '); + status = createBlockDirKinematicDH( rob ); + if (status == 1) + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + else + multIDFprintf([robOpts.verbose*2, logFid],'\t%s\n',' failed!'); + end +end + +if ~isempty(robOpts.logFile) % close log file if necessary + fclose(logFid); +end + +%% Are the results correct? +if robOpts.selfCheck + [testResult.directKinematics, maxErr ] = checkDirKinematics(rob,robOpts); +end + +end % of main function + + +%% Definition of the RST-header contents for each generated file +function hStruct = createHeaderStruct(rob,curBody,nBodies,fName) +hStruct.funName = fName; +hStruct.shortDescription = ['Forward kinematics solution for the ',rob.name,' arm up to frame ',int2str(curBody),' of ',int2str(nBodies),'.']; +hStruct.calls= {['T = ',fName,'(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(nBodies)],... + ' 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!',... + 'Generator createDirKinematicDH written by:',... + 'Jörn Malzahn '}; +hStruct.seeAlso = {rob.name,'createDirKinematicDH'}; +end +%} diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createGenAccelerations.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createGenAccelerations.m new file mode 100644 index 0000000..db4357c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createGenAccelerations.m @@ -0,0 +1,48 @@ +function [ QDDot ] = createGenAccelerations( rob ) +%% CREATEGENACCELERATIONS Create symbolic vector of second order joint generalized coordinate derivatives. +% ========================================================================= +% +% [ QDDot ] = createGenAccelerations( rob ) +% +% Input: +% rob: Robot definition structure. +% +% Output: +% QDDot: [nx1] Symbolic vector of second order derivatives of the +% generalized robot coordinates. +% +% Example: +% createGenAccelerations( stanfordarm3 ) +% +% Known Bugs: +% --- +% +% TODO: +% --- +% +% References: +% --- +% +% Authors: +% Jörn Malzahn +% +% See also createGenCoordinates,createGenForces,createGenVelocities. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +nCoords = size(rob.DH,1); + +evalString = []; +for iCoords = 1:nCoords + evalString = [evalString, 'qDDot', num2str(iCoords), ' ']; +end +eval(['syms ', evalString]); +QDDot = eval([ '[', evalString,']' ] ); + +QDDot=QDDot.'; \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createGenCoordinates.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createGenCoordinates.m new file mode 100644 index 0000000..23586e3 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createGenCoordinates.m @@ -0,0 +1,77 @@ +function [ Q phie dPhiedx] = createGenCoordinates( rob ) +%% CREATEGENCOORDINATES Create symbolic vector of generalized robot joint coordinates. +% ========================================================================= +% +% [ Q ] = createGenCoordinates( rob ) +% +% Input: +% rob: Robot definition structure. +% +% Output: +% Q: [nx1] Symbolic vector of the generalized robot coordinates. +% +% Example: +% createGenCoordinates( stanfordarm3 ) +% +% Known Bugs: +% --- +% +% TODO: +% --- +% +% References: +% --- +% +% Authors: +% Jörn Malzahn +% +% See also createGenForces,createGenVelocities,createGenAccelerations. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +nCoords = rob.n; + +evalString = []; + +phie = sym([]); +dPhiedx = sym([]); +indOffset = 0; + +for iCoords = 1:nCoords + % Generalized coordinates: Joint values... + evalString = [evalString, 'q', num2str(iCoords), ' ']; + + if isfield(rob, 'links') % If there is no link definition field, the robot must be rigid + if rob.links(iCoords) == 'f' + % ... and flexible time dependent mode amplitudes + evalString = [evalString, 'del', num2str(iCoords), '1 ']; + evalString = [evalString, 'del', num2str(iCoords), '2 ']; + + % Additionally store symbolic variables for the mode shape + % functions at the link ends in separate containers + tmpEvalString = ['syms phie', num2str(iCoords),'1; phie(', num2str(iCoords+indOffset+1),') = phie', num2str(iCoords),'1;']; + eval(tmpEvalString); + tmpEvalString = ['syms phie', num2str(iCoords),'2; phie(', num2str(iCoords+indOffset+2),') = phie', num2str(iCoords),'2;']; + eval(tmpEvalString); + tmpEvalString = ['syms dPhiedx', num2str(iCoords),'1; dPhiedx(', num2str(iCoords+indOffset+1),') = dPhiedx', num2str(iCoords),'1;']; + eval(tmpEvalString); + tmpEvalString = ['syms dPhiedx', num2str(iCoords),'2; dPhiedx(', num2str(iCoords+indOffset+2),') = dPhiedx', num2str(iCoords),'2;']; + eval(tmpEvalString); + indOffset = indOffset +2; + else + + end + else + end + +end +eval(['syms ', evalString]); +Q = eval([ '[', evalString,']' ] ); + +Q = Q.'; diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createGenForces.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createGenForces.m new file mode 100644 index 0000000..de95754 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createGenForces.m @@ -0,0 +1,47 @@ +function [ tau ] = createGenForces( rob ) +%% CREATEGENFORCES Create symbolic vector of generalized robot joint forces. +% ========================================================================= +% +% [ tau ] = createGenForces( rob ) +% +% Input: +% rob: Robot definition structure. +% +% Output: +% tau: [nx1] Symbolic vector of the generalized robot joint forces. +% +% Example: +% createGenForces( stanfordarm3 ) +% +% Known Bugs: +% --- +% +% TODO: +% --- +% +% References: +% --- +% +% Authors: +% Jörn Malzahn +% +% See also createGenCoordinates,createGenVelocities,createGenAccelerations. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +nCoords = size(rob.DH,1); + +evalString = []; +for iCoords = 1:nCoords + evalString = [evalString, 'tau', num2str(iCoords), ' ']; +end +eval(['syms ', evalString]); +tau = eval([ '[', evalString,']' ] ); + +tau=tau.'; \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createGenVelocities.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createGenVelocities.m new file mode 100644 index 0000000..7b29a99 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createGenVelocities.m @@ -0,0 +1,46 @@ +function [ QDot ] = createGenVelocities( rob ) +%% CREATEGENVELOCITIES Create symbolic vector of generalized robot joint velocities. +% ========================================================================= +% +% [ QDot ] = createGenForces( rob ) +% +% Input: +% rob: Robot definition structure. +% +% Output: +% QDot: [nx1] Symbolic vector of the generalized robot joint forces. +% +% Example: +% createGenVelocities( stanfordarm3 ) +% +% Known Bugs: +% --- +% +% TODO: +% --- +% +% References: +% --- +% +% Authors: +% Jörn Malzahn +% +% See also createGenCoordinates,createGenVelocities,createGenAccelerations. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +nCoords = size(rob.DH,1); + +evalString = []; +for iCoords = 1:nCoords + evalString = [evalString, 'qDot', num2str(iCoords), ' ']; +end +eval(['syms ', evalString]); +QDot = eval([ '[', evalString,']' ] ); +QDot = QDot.'; \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/createJacobianDH.m b/lwrserv/matlab/rvctools/robot/@SerialLink/createJacobianDH.m new file mode 100644 index 0000000..b41bd72 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/createJacobianDH.m @@ -0,0 +1,242 @@ +function [eeJacobian] = createJacobianDH(rob,varargin) +%% CREATEJACOBIANDH Compute the symbolic geometric robot joint jacobian. +% ========================================================================= +% +% [eeJacobian] = createJacobianDH(rob) +% [eeJacobian] = createJacobianDH(rob,robOpts) +% +% Description: +% The function computes the symbolic expression of the geometric +% end-effector jacobian for the robot defined in the input structure +% rob. The optional robOpts structure allows to specify the automatic +% generation of m-functions and real-time capable simulink blocks. +% +% Up to now only serial rigid robot arms are supported. Joints may be +% revolute or prismatic. +% +% If the createMFun flag is set in the robOpts structure, the function +% creates an m-file for the geometric end-effector jacobian. The +% m-function takes a vector of generalized joint values as well as a +% vector of generalized joint velocities as input and outputs the +% numerical jacobian matrix for these joint values. The m-functions are +% stored in a subdirectory named after the robot. +% +% If the createSLBlock flag is set in the robOpts structure, a Simulink +% Embedded Matlab Function Block is automatically generated for the +% geometric end-effector jacobian. The block takes a vector of +% generalized joint values as input and outputs the numerical jacobian. +% The blocks are added to a block library named "mdlrobotname" in the +% subdirectory named after the robot. +% +% Input: +% rob : Robot definition structure +% robOpts: Optional robot model generation options structure. +% +% Output: +% eeJacobian: [6xn] representing the geometric end-effector jacobian +% +% Example: +% rob = stanfordarm3; +% eeJacobian = createJacobianDH(rob) +% +% Known Bugs: +% --- +% +% TODO: +% --- +% +% 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 +% +% Authors: +% Jörn Malzahn +% +% See also createDirKinematicDH,stanfordarm3,getRobModelOpts. +% +% This software may be used under the terms of CC BY-SA 3.0 license +% < http://creativecommons.org/licenses/by-sa/3.0/ > +% +% 2012 RST, Technische Universität Dortmund, Germany +% < http://www.rst.e-technik.tu-dortmund.de > +% +% ======================================================================== + +srob = SerialLink(rob); +srob +for i=1:rob.n + srob.links(i) = srob.links(i).sym; +end + +q = createGenCoordinates(rob); +eeJacobian = srob.jacobn(q); +eeJacobian = simplify(eeJacobian); % simplify to the shortest form of the result.- + + +%{ +%% Handle robot modeling options +if nargin > 1 + robOpts = completeRobOpts(varargin{1}); +else + robOpts = getRobotModelOpts('default'); +end + +% Output to logfile? +if ~isempty(robOpts.logFile) + logFid = fopen(robOpts.logFile,'a'); +else + logFid = []; +end + +%% Load existing or invoke computation of forward kinematics +curPath = [pwd '\',rob.name,'\']; % compose path to output directory +if (~exist(curPath,'dir')) % create output directory if it does not already exist + dirKinC = createDirKinematicDH(rob,robOpts); +else + tmp = load([curPath,'\DirKinematicDH.mat']); + dirKinC = tmp.dirKinC; +end + +multIDFprintf([robOpts.verbose, logFid],... + '%s: - Creating geometric endeffector jacobian ...\n',datestr(now)); + +%% Gather information about the robot +nBodies = size(rob.DH,1); +if ~isfield(rob,'links') % if not specified, the robot considered to have revolute joints only + warning('Joint variable definition not found. Treating all joints as revolute!'); + rob.links = repmat(['r'],1,nBodies); +end + +[ q ] = createGenCoordinates(rob); % joint generalized variables +nGenCoord = length(q); +eeJacobian = sym(zeros(6,nGenCoord)); + +%% Compute the translation part of the jacobian +multIDFprintf([robOpts.verbose, logFid],... + '\tProcessing translation part for joint: '); +for iGenCoord = 1:nGenCoord + multIDFprintf([robOpts.verbose, logFid],' %i ',iGenCoord); + t = dirKinC{nBodies}(1:3,4); % extract the translation part of the homogenous forward kinematics transformations + for kDim = 1:3 + eeJacobian(kDim,iGenCoord) = diff(t(kDim),q(iGenCoord)); % fill the columns of the translation part + end +end +multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + +%% Compute the rotation part of the jacobian +indOffset = 0; +multIDFprintf([robOpts.verbose, logFid],... + '\tProcessing rotational part for joint: '); +for iBodies = 1:nBodies + multIDFprintf([robOpts.verbose, logFid],' %i ',iBodies); + Z = [0,0,1].'; % axis of rotation in each joint frame is the z-axis. + + if rob.links(iBodies) == 'f' % case of flexible links (future releases) + %% + if iBodies ==1 + R = sym(eye(3)); % treat the first link seperatley because zero indices are not allowed. + else + R = dirKinC{iBodies-1}(1:3,1:3); % all other links + end + + if rob.joints(iBodies)== 'r' % only revolute joints contribute to angular velocity + omega = R*Z; % describe the axis of rotation with respect to the base frame. + eeJacobian(4:6,iBodies+indOffset:iBodies+indOffset+2) = repmat(omega,1,3); % this rotation vector is a column of the rotation part of the jacobian. + else + R1 = zeros(3); + omega = [R1*Z,R*Z,R*Z]; % describe the axis of rotation with respect to the base frame. + eeJacobian(4:6,iBodies+indOffset:iBodies+indOffset+2) = omega;% this rotation vector is a column of the rotation part of the jacobian. + end + indOffset = indOffset+2; + + else + %% + if rob.joints(iBodies)== 'r' % only revolute joints contribute to angular velocity + if iBodies ==1 + R = sym(eye(3)); % treat the first link seperatley because zero indices are not allowed. + else + R = dirKinC{iBodies-1}(1:3,1:3); % all other links + end + else + R = zeros(3); + end + omega = R*Z; % Describe the axis of rotation with respect to the base frame. + eeJacobian(4:6,iBodies+indOffset) = omega; % This rotation vector is a column of the rotation part of the jacobian. + end +end +multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + +multIDFprintf([robOpts.verbose, logFid],'\t%s ','Now simplifying...'); +eeJacobian = simplify(eeJacobian); % simplify to the shortest form of the result.- +multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + + +if (robOpts.saveResult) + multIDFprintf([robOpts.verbose, logFid],... + '\tSaving computed jacobian: '); + save([curPath,'eeJacobian'],'eeJacobian'); + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); +end + +%% Create m-functions +if robOpts.createMFun + % create per joint jacobian m-functions + multIDFprintf([robOpts.verbose, logFid],... + '\tGenerating m-function ... '); + + funName = 'EEJacobianDH'; + hStruct = createHeaderStruct(rob,nBodies,funName); + + fileName = [curPath,rob.name,funName,'.m']; + matlabFunction(eeJacobian,'file',fileName,... % generate function m-file + 'outputs', {'Je'},... + 'vars', {q(1:end)}); + replaceHeader(hStruct,fileName); % replace autogenerated function header + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); +end + +%% Create Simulink Embedded Matlab Function Block +if robOpts.createSLBlock + multIDFprintf([robOpts.verbose, logFid],... + '\tGenerating Simulink Block... '); + status = createBlockJacobianDH(rob); + if (status == 1) + multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!'); + else + multIDFprintf([robOpts.verbose*2, logFid],'\t%s\n',' failed!'); + end +end + +if ~isempty(robOpts.logFile) + fclose(logFid); +end + +%% Are the results correct? +if robOpts.selfCheck + [testResult.endeffectorJacobian, maxErr ] = checkJacobian(rob,robOpts); +end + +end % of main function + +%% Definition of the RST-header contents for each generated file +function hStruct = createHeaderStruct(rob,nBodies,fName) +hStruct.funName = fName; +hStruct.shortDescription = ['Geometric end-effector jacobian for the',rob.name,' arm.']; +hStruct.calls= {['Je = ',fName,'(q)']}; +hStruct.detailedDescription = {'Given a full set of joint variables and their time derivatives this function',... + 'computes the geometric end-effector velocities expressed in the robot base frame.'}; +hStruct.inputs = {['q: ',int2str(nBodies),'-element vector of generalized coordinates.'],... + ' Angles have to be given in radians!'}; +hStruct.outputs = {['Je: [6x,',int2str(nBodies),'] geometric end-effector Jacobian.']}; +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!',... + 'Generator createJacobianDH written by:',... + 'Jörn Malzahn '}; +hStruct.seeAlso = {rob.name,'createJacobianDH'}; +end +%} \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/edit.m b/lwrserv/matlab/rvctools/robot/@SerialLink/edit.m new file mode 100644 index 0000000..36917c8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/edit.m @@ -0,0 +1,115 @@ + %SerialLink.edit Edit kinematic and dynamic parameters of a seriallink manipulator + % + % R.edit displays the kinematic parameters of the robot as an editable + % table in a new figure. + % + % R.edit('dyn') as above but also displays the dynamic parameters. + % + % Notes:: + % - The 'Save' button copies the values from the table to the SerialLink + % manipulator object. + % - To exit the editor without updating the object just + % kill the figure window. + function edit(r, dyn) + + isdyn = nargin > 1 && strcmp(dyn, 'dyn') == 1; + + f = figure('Position',[100 100 600 220], ... + 'Menubar', 'none', ... + 'Name', r.name); + dh = zeros(r.n,0); + for j=1:r.n + L = r.links(j); + + %dh(j,1) = L.theta; + dh(j,2) = L.d; + dh(j,3) = L.a; + dh(j,4) = L.alpha; + dh(j,5) = L.sigma; + dh(j,6) = L.offset; + if ~isempty(L.qlim) + dh(j,7) = L.qlim(1); + dh(j,8) = L.qlim(2); + else + dh(j,7) = -Inf; + dh(j,8) = Inf; + end + if isdyn + dh(j,9) = L.m; + dh(j,10) = L.Jm; + dh(j,11) = L.B; + dh(j,12) = L.G; + dh(j,13) = L.Tc(1); + dh(j,14) = L.Tc(2); + dh(j,15) = L.r(1); + dh(j,16) = L.r(2); + dh(j,17) = L.r(3); + dh(j,18) = L.I(1,1); + dh(j,19) = L.I(2,2); + dh(j,20) = L.I(3,3); + dh(j,21) = L.I(1,2); + dh(j,22) = L.I(2,3); + dh(j,23) = L.I(3,1); + end + end + headings = {'theta', 'd', 'a', 'alpha', 'sigma', 'offset', 'q_min', 'q_max'}; + if isdyn + headings = [headings 'mass', 'Jm', 'B', 'G', 'Tc+', 'Tc-', 'rx', 'ry', 'rz', 'Ixx', 'Iyy', 'Izz', 'Ixy', 'Iyz', 'Ixz']; + end + t = uitable(f, ... + 'ColumnEditable', true, ... + 'ColumnName', headings,... + 'Rowstriping', 'on', ... + 'Data', dh, ... + 'UserData', r); + % Set width and height + f.Position(3) = t.Extent(3)+50; + f.Position(4) = t.Extent(4)+50; + t.Position(3) = t.Extent(3)+50; + t.Position(4) = t.Extent(4)+50; + t.Position(1) = 0; + t.Position(2) = 0; + b = uicontrol('Style', 'pushbutton', ... + 'Position', [10 10 50 20], ... + 'String', 'Save', ... + 'Callback', {@edit_save, t}); + end + + function edit_save(button, event, table) + + dh = get(table, 'Data'); + r = get(table, 'UserData'); + for j=1:r.n + L = r.links(j); + + %dh(j,1) = L.theta; + L.d = dh(j,2); + L.a = dh(j,3); + L.alpha = dh(j,4); + L.sigma = dh(j,5); + L.offset = dh(j,6); + L.qlim(1) = dh(j,7); + L.qlim(2) = dh(j,8); + if numcols(dh) > 6 + L.m = dh(j,9); + L.Jm = dh(j,10); + L.B = dh(j,11); + L.G = dh(j,12); + L.Tc(1) = dh(j,13); + L.Tc(2) = dh(j,14); + L.r(1) = dh(j,15); + L.r(2) = dh(j,16); + L.r(3) = dh(j,17); + L.I(1,1) = dh(j,18); + L.I(2,2) = dh(j,19); + L.I(3,3) = dh(j,20); + L.I(1,2) = dh(j,21); + L.I(2,3) = dh(j,22); + L.I(3,1) = dh(j,23); + L.I(2,1) = dh(j,21); + L.I(3,2) = dh(j,22); + L.I(1,3) = dh(j,23); + end + end + end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/fdyn.m b/lwrserv/matlab/rvctools/robot/@SerialLink/fdyn.m new file mode 100644 index 0000000..cd31591 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/fdyn.m @@ -0,0 +1,111 @@ +%SerialLink.fdyn Integrate forward dynamics +% +% [T,Q,QD] = R.fdyn(T1, TORQFUN) integrates the dynamics of the robot over +% the time interval 0 to T and returns vectors of time TI, joint position Q +% and joint velocity QD. The initial joint position and velocity are zero. +% The torque applied to the joints is computed by the user function TORQFUN: +% +% [TI,Q,QD] = R.fdyn(T, TORQFUN, Q0, QD0) as above but allows the initial +% joint position and velocity to be specified. +% +% The control torque is computed by a user defined function +% +% TAU = TORQFUN(T, Q, QD, ARG1, ARG2, ...) +% +% where Q and QD are the manipulator joint coordinate and velocity state +% respectively, and T is the current time. +% +% [T,Q,QD] = R.fdyn(T1, TORQFUN, Q0, QD0, ARG1, ARG2, ...) allows optional +% arguments to be passed through to the user function. +% +% Note:: +% - This function performs poorly with non-linear joint friction, such as +% Coulomb friction. The R.nofriction() method can be used to set this +% friction to zero. +% - If TORQFUN is not specified, or is given as 0 or [], then zero torque +% is applied to the manipulator joints. +% - The builtin integration function ode45() is used. +% +% See also SerialLink.accel, SerialLink.nofriction, SerialLink.rne, ode45. + + + +% 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.petercorke.com + +function [t, q, qd] = fdyn(robot, t1, torqfun, q0, qd0, varargin) + + % check the Matlab version, since ode45 syntax has changed + if verLessThan('matlab', '7') + error('fdyn now requires Matlab version >= 7'); + end + + n = robot.n; + if nargin == 2 + torqfun = 0; + q0 = zeros(1,n); + qd0 = zeros(1,n); + elseif nargin == 3 + q0 = zeros(1,n); + qd0 = zeros(1,n); + elseif nargin == 4 + qd0 = zeros(1,n); + end + + % concatenate q and qd into the initial state vector + q0 = [q0(:); qd0(:)]; + + [t,y] = ode45(@fdyn2, [0 t1], q0, [], robot, torqfun, varargin{:}); + q = y(:,1:n); + qd = y(:,n+1:2*n); + +end + + +%FDYN2 private function called by FDYN +% +% XDD = FDYN2(T, X, FLAG, ROBOT, TORQUEFUN) +% +% Called by FDYN to evaluate the robot velocity and acceleration for +% forward dynamics. T is the current time, X = [Q QD] is the state vector, +% ROBOT is the object being integrated, and TORQUEFUN is the string name of +% the function to compute joint torques and called as +% +% TAU = TORQUEFUN(T, X) +% +% if not given zero joint torques are assumed. +% +% The result is XDD = [QD QDD]. +function xd = fdyn2(t, x, robot, torqfun, varargin) + + n = robot.n; + + q = x(1:n)'; + qd = x(n+1:2*n)'; + + % evaluate the torque function if one is given + if isa(torqfun, 'function_handle') + tau = torqfun(robot, t, q, qd, varargin{:}); + else + tau = zeros(1,n); + end + + qdd = robot.accel(x(1:n,1)', x(n+1:2*n,1)', tau); + xd = [x(n+1:2*n,1); qdd]; +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/fellipse.m b/lwrserv/matlab/rvctools/robot/@SerialLink/fellipse.m new file mode 100644 index 0000000..0f96057 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/fellipse.m @@ -0,0 +1,68 @@ +function fellipse(robot, q, varargin) + %SerialLink.fellipse Force ellipsoid for seriallink manipulator + % + % R.fellipse(Q, OPTIONS) displays the force ellipsoid for the + % robot R at pose Q. The ellipsoid is centered at the tool tip position. + % + % Options:: + % '2d' Ellipse for translational xy motion, for planar manipulator + % 'trans' Ellipsoid for translational motion (default) + % 'rot' Ellipsoid for rotational motion + % + % Display options as per plot_ellipse to control ellipsoid face and edge + % color and transparency. + % + % Example:: + % To interactively update the force ellipsoid while using sliders + % to change the robot's pose: + % robot.teach('callback', @(r,q) r.fellipse(q)) + % + % Notes:: + % - The ellipsoid is tagged with the name of the robot prepended to + % ".fellipse". + % - Calling the function with a different pose will update the ellipsoid. + % + % See also SerialLink.jacob0, SerialLink.vellipse, plot_ellipse. + + name = [robot.name '.fellipse']; + + e = findobj('Tag', name); + + if isempty(q) + delete(e); + return; + end + + opt.mode = {'trans', 'rot', '2d'}; + [opt,args] = tb_optparse(opt, varargin); + + J = robot.jacob0(q); + + switch opt.mode + case'2d' + J = J(1:2,1:2); + case 'trans' + J = J(1:3,:); + case 'rot' + J = J(4:6,:); + end + + N = inv(J*J'); + + t = transl(robot.fkine(q)); + + switch opt.mode + case '2d' + if isempty(e) + h = plot_ellipse(N, t(1:2), 'edgecolor', 'r', 'Tag', name, args{:}); + else + plot_ellipse(N, t(1:2), 'alter', e); + end + otherwise + if isempty(e) + h = plot_ellipse(N, t(1:3), 'edgecolor', 'k', 'fillcolor', 'r', 'alpha', 0.5, 'Tag', name, args{:}); + else + plot_ellipse(N, t(1:3), 'alter', e); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/fkine.m b/lwrserv/matlab/rvctools/robot/@SerialLink/fkine.m new file mode 100644 index 0000000..10144ce --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/fkine.m @@ -0,0 +1,85 @@ +%SerialLink.fkine Forward kinematics +% +% T = R.fkine(Q) is the pose (4x4) of the robot end-effector as a homogeneous +% transformation for the joint configuration Q (1xN). +% +% If Q is a matrix (KxN) the rows are interpretted as the generalized +% joint coordinates for a sequence of points along a trajectory. Q(i,j) is +% the j'th joint parameter for the i'th trajectory point. In this case +% T is a 3d matrix (4x4xK) where the last subscript is the index along the path. +% +% Note:: +% - The robot's base or tool transform, if present, are incorporated into the +% result. +% - Joint offsets, if defined, are added to Q before the forward kinematics are +% computed. +% +% See also SerialLink.ikine, SerialLink.ikine6s. + +% 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.petercorke.com + +% Modifications by Joern Malzahn to support CodeGenerator functionality + +function [t allt] = fkine(robot, q) +% +% evaluate fkine for each point on a trajectory of +% theta_i or q_i data +% + +n = robot.n; + +if nargout > 1 + allt = zeros(4,4,n); + if isa(q,'sym') + allt = sym(allt); + end +end + +L = robot.links; +if numel(q) == n + t = robot.base; + for i=1:n + t = t * L(i).A(q(i)); + + if nargout > 1 + allt(:,:,i) = t; % intermediate transformations + end + end + t = t * robot.tool; +else + if numcols(q) ~= n + error('q must have %d columns', n) + end + t = zeros(4,4,0); + for qv=q', % for each trajectory point + tt = robot.base; + for i=1:n + tt = tt * L(i).A(qv(i)); + end + t = cat(3, t, tt * robot.tool); + end +end + +if isa(t, 'sym') + t = simplify(t); +end + +%robot.T = t; +%robot.notify('Moved'); diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/friction.m b/lwrserv/matlab/rvctools/robot/@SerialLink/friction.m new file mode 100644 index 0000000..064f6e8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/friction.m @@ -0,0 +1,46 @@ +%SerialLink.friction Friction force +% +% TAU = R.friction(QD) is the vector of joint friction forces/torques for the +% robot moving with joint velocities QD. +% +% The friction model includes: +% - viscous friction which is linear with velocity; +% - Coulomb friction which is proportional to sign(QD). +% +% See also Link.friction. + + + +% 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.petercorke.com + +function tau = friction(robot, qd) + + L = robot.links; + + tau = zeros(1,robot.n); + if robot.issym + tau = sym(tau); + end + + + for j=1:robot.n + tau(j) = L(j).friction(qd(j)); + end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/frne.mexmaci64 b/lwrserv/matlab/rvctools/robot/@SerialLink/frne.mexmaci64 new file mode 100644 index 0000000..81cfecf Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/@SerialLink/frne.mexmaci64 differ diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/frne.mexw32 b/lwrserv/matlab/rvctools/robot/@SerialLink/frne.mexw32 new file mode 100644 index 0000000..683e63b Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/@SerialLink/frne.mexw32 differ diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/gencoords.m b/lwrserv/matlab/rvctools/robot/@SerialLink/gencoords.m new file mode 100644 index 0000000..265db33 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/gencoords.m @@ -0,0 +1,30 @@ +%GENCOORDS Vector of symbolic generalized coordinates +% +% Q = R.GENCOORDS is a vector (1xN) of symbols [q1 q2 ... qN]. +% +% [Q,QD] = R.GENCOORDS as above but QD is a vector (1xN) of +% symbols [qd1 qd2 ... qdN]. +% +% [Q,QD,QDD] = R.GENCOORDS as above but QDD is a vector (1xN) of +% symbols [qdd1 qdd2 ... qddN]. +% +% +function [q,qd,qdd] = gencoords(r) + + if nargout > 0 + for j=1:r.n + q(j) = sym( sprintf('q%d', j), 'real' ); + end + end + + if nargout > 1 + for j=1:r.n + qd(j) = sym( sprintf('qd%d', j), 'real' ); + end + end + + if nargout > 2 + for j=1:r.n + qdd(j) = sym( sprintf('qdd%d', j), 'real' ); + end + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/genforces.m b/lwrserv/matlab/rvctools/robot/@SerialLink/genforces.m new file mode 100644 index 0000000..a68f414 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/genforces.m @@ -0,0 +1,5 @@ +function tau = genforces(r) + + for j=1:r.n + tau(j) = sym( sprintf('tau%d', j), 'real' ); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/gravjac.m b/lwrserv/matlab/rvctools/robot/@SerialLink/gravjac.m new file mode 100644 index 0000000..962ade9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/gravjac.m @@ -0,0 +1,135 @@ +%SerialLink.GRAVJAC Fast gravity load and Jacobian +% +% [TAU,JAC0] = R.gravjac(Q) is the generalised joint force/torques due to +% gravity (1xN) and the manipulator Jacobian in the base frame (6xN) for +% robot pose Q (1xN), where N is the number of robot joints. +% +% [TAU,JAC0] = R.gravjac(Q,GRAV) as above but gravity is given explicitly +% by GRAV (3x1). +% +% Trajectory operation:: +% +% If Q is MxN where N is the number of robot joints then a trajectory is +% assumed where each row of Q corresponds to a pose. TAU (MxN) is the +% generalised joint torque, each row corresponding to an input pose, and +% JAC0 (6xNxM) where each plane is a Jacobian corresponding to an input pose. +% +% Notes:: +% - The gravity vector is defined by the SerialLink property if not explicitly given. +% - Does not use inverse dynamics function RNE. +% - Faster than computing gravity and Jacobian separately. +% +% Author:: +% Bryan Moutrie +% +% See also SerialLink.pay, SerialLink, SerialLink.gravload, SerialLink.jacob0. + +% Copyright (C) Bryan Moutrie, 2013-2015 +% Licensed under the GNU Lesser General Public License +% see full file for full statement +% +% LICENSE STATEMENT: +% +% This file is part of pHRIWARE. +% +% pHRIWARE 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. +% +% pHRIWARE 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 General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public +% License along with pHRIWARE. If not, see . +% + +function [tauB, J] = gravjac(robot, q, grav) + + n = robot.n; + revolute = [robot.links(:).isrevolute]; + if ~robot.mdh + baseAxis = robot.base(1:3,3); + baseOrigin = robot.base(1:3,4); + end + + poses = size(q, 1); + tauB = zeros(poses, n); + if nargout == 2, J = zeros(6, robot.n, poses); end + + % Forces + force = zeros(3,n); + if nargin < 3, grav = robot.gravity; end + for joint = 1: n + force(:,joint) = robot.links(joint).m * grav; + end + + % Centre of masses (local frames) + r = zeros(4,n); + for joint = 1: n + r(:,joint) = [robot.links(joint).r'; 1]; + end + com_arr = zeros(3, n); + + for pose = 1: poses + + [Te, T] = robot.fkine(q(pose,:)); + + jointOrigins = squeeze(T(1:3,4,:)); + jointAxes = squeeze(T(1:3,3,:)); + + if ~robot.mdh + jointOrigins = [baseOrigin, jointOrigins(:,1:end-1)]; + jointAxes = [baseAxis, jointAxes(:,1:end-1)]; + end + + % Backwards recursion + for joint = n: -1: 1 + + com = T(:,:,joint) * r(:,joint); % C.o.M. in world frame, homog + com_arr(:,joint) = com(1:3); % Add it to the distal others + + t = 0; + for link = joint: n % for all links distal to it + if revolute(joint) + d = com_arr(:,link) - jointOrigins(:,joint); + t = t + cross3(d, force(:,link)); + % Though r x F would give the applied torque and not the + % reaction torque, the gravity vector is nominally in the + % positive z direction, not negative, hence the force is + % the reaction force + else + t = t + force(:,link); %force on prismatic joint + end + end + + tauB(pose,joint) = t' * jointAxes(:,joint); + end + + if nargout == 2 + J(:,:,pose) = makeJ(jointOrigins,jointAxes,Te(1:3,4),revolute); + end + + end + + +end + +function J = makeJ(O,A,e,r) + J(4:6,:) = A; + for j = 1:length(r) + if r(j) + J(1:3,j) = cross3(A(:,j),e-O(:,j)); + else + J(:,j) = J([4 5 6 1 2 3],j); %J(1:3,:) = 0; + end + end +end + +function c = cross3(a,b) + c(3,1) = a(1)*b(2) - a(2)*b(1); + c(1,1) = a(2)*b(3) - a(3)*b(2); + c(2,1) = a(3)*b(1) - a(1)*b(3); +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/gravload.m b/lwrserv/matlab/rvctools/robot/@SerialLink/gravload.m new file mode 100644 index 0000000..2b091ff --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/gravload.m @@ -0,0 +1,45 @@ +%SerialLink.gravload Gravity loading +% +% TAUG = R.gravload(Q) is the joint gravity loading for the robot in the +% joint configuration Q. Gravitational acceleration is a property of the +% robot object. +% +% If Q is a row vector, the result is a row vector of joint torques. If +% Q is a matrix, each row is interpreted as a joint configuration vector, +% and the result is a matrix each row being the corresponding joint torques. +% +% TAUG = R.gravload(Q, GRAV) is as above but the gravitational +% acceleration vector GRAV is given explicitly. +% +% See also SerialLink.rne, SerialLink.itorque, SerialLink.coriolis. + + + +% 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.petercorke.com + +function tg = gravload(robot, q, grav) + if numcols(q) ~= robot.n + error('Insufficient columns in q') + end + if nargin == 2 + tg = rne(robot, q, zeros(size(q)), zeros(size(q))); + elseif nargin == 3 + tg = rne(robot, q, zeros(size(q)), zeros(size(q)), grav); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/ikcon.m b/lwrserv/matlab/rvctools/robot/@SerialLink/ikcon.m new file mode 100644 index 0000000..a5a3207 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/ikcon.m @@ -0,0 +1,121 @@ +%SerialLink.IKCON Numerical inverse kinematics with joint limits +% +% Q = R.ikcon(T) are the joint coordinates (1xN) corresponding to the robot +% end-effector pose T (4x4) which is a homogenenous transform. +% +% [Q,ERR] = robot.ikcon(T) as above but also returns ERR which is the +% scalar final value of the objective function. +% +% [Q,ERR,EXITFLAG] = robot.ikcon(T) as above but also returns the +% status EXITFLAG from fmincon. +% +% [Q,ERR,EXITFLAG] = robot.ikcon(T, Q0) as above but specify the +% initial joint coordinates Q0 used for the minimisation. +% +% [Q,ERR,EXITFLAG] = robot.ikcon(T, Q0, options) as above but specify the +% options for fmincon to use. +% +% Trajectory operation:: +% +% In all cases if T is 4x4xM it is taken as a homogeneous transform +% sequence and R.ikcon() returns the joint coordinates corresponding to +% each of the transforms in the sequence. Q is MxN where N is the number +% of robot joints. The initial estimate of Q for each time step is taken as +% the solution from the previous time step. +% +% ERR and EXITFLAG are also Mx1 and indicate the results of optimisation +% for the corresponding trajectory step. +% +% Notes:: +% - Requires fmincon from the Optimization Toolbox. +% - Joint limits are considered in this solution. +% - Can be used for robots with arbitrary degrees of freedom. +% - In the case of multiple feasible solutions, the solution returned +% depends on the initial choice of Q0. +% - Works by minimizing the error between the forward kinematics of the +% joint angle solution and the end-effector frame as an optimisation. +% The objective function (error) is described as: +% sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega ) +% Where omega is some gain matrix, currently not modifiable. +% +% Author:: +% Bryan Moutrie +% +% See also SerialLink.ikunc, fmincon, SerialLink.ikine, SerialLink.fkine. + +% Copyright (C) Bryan Moutrie, 2013-2015 +% Licensed under the GNU Lesser General Public License +% see full file for full statement +% +% LICENSE STATEMENT: +% +% This file is part of pHRIWARE. +% +% pHRIWARE 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. +% +% pHRIWARE 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 General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public +% License along with pHRIWARE. If not, see . + + +function [qstar, error, exitflag] = ikcon(robot, T, q0, options) + + % check if Optimization Toolbox exists, we need it + if ~exist('fmincon') + error('rtb:ikcon:nosupport', 'Optimization Toolbox required'); + end + + % create output variables + T_sz = size(T, 3); + qstar = zeros(T_sz, robot.n); + error = zeros(T_sz, 1); + exitflag = zeros(T_sz, 1); + + problem.x0 = zeros(1, robot.n); + problem.options = optimoptions('fmincon', ... + 'Algorithm', 'active-set', ... + 'Display', 'off'); % default options for ikcon + + if nargin > 2 + % user passed initial joint coordinates + problem.x0 = q0; + end + if nargin > 3 + % if given, add optional argument to the list of optimiser options + problem.options = optimset(problem.options, options); + end + + % set the joint limit bounds + problem.lb = robot.qlim(:,1); + problem.ub = robot.qlim(:,2); + problem.solver = 'fmincon'; + + reach = sum(abs([robot.a, robot.d])); + omega = diag([1 1 1 3/reach]); + + for t = 1: T_sz + problem.objective = ... + @(x) sumsqr(((T(:,:,t) \ robot.fkine(x)) - eye(4)) * omega); + + [q_t, err_t, ef_t] = fmincon(problem); + + qstar(t,:) = q_t; + error(t) = err_t; + exitflag(t) = ef_t; + + problem.x0 = q_t; + end + +end + +function s = sumsqr(A) + s = sum(A(:).^2); +end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/ikine.m b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine.m new file mode 100644 index 0000000..4b8f2ff --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine.m @@ -0,0 +1,285 @@ +%SerialLink.IKINE Inverse manipulator kinematics +% +% Q = R.ikine(T) is the joint coordinates corresponding to the robot +% end-effector pose T which is a homogenenous transform. +% +% Q = R.ikine(T, Q0, OPTIONS) specifies the initial estimate of the joint +% coordinates. +% +% Q = R.ikine(T, Q0, M, OPTIONS) specifies the initial estimate of the joint +% coordinates and a mask matrix. For the case where the manipulator +% has fewer than 6 DOF the solution space has more dimensions than can +% be spanned by the manipulator joint coordinates. In this case +% the mask matrix M specifies the Cartesian DOF (in the wrist coordinate +% frame) that will be ignored in reaching a solution. The mask matrix +% has six elements that correspond to translation in X, Y and Z, and rotation +% about X, Y and Z respectively. The value should be 0 (for ignore) or 1. +% The number of non-zero elements should equal the number of manipulator DOF. +% +% For example when using a 5 DOF manipulator rotation about the wrist z-axis +% might be unimportant in which case M = [1 1 1 1 1 0]. +% +% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence +% and R.ikine() returns the joint coordinates corresponding to each of the +% transforms in the sequence. Q is MxN where N is the number of robot joints. +% The initial estimate of Q for each time step is taken as the solution +% from the previous time step. +% +% Options:: +% 'pinv' use pseudo-inverse instead of Jacobian transpose +% 'ilimit',L set the maximum iteration count (default 1000) +% 'tol',T set the tolerance on error norm (default 1e-6) +% 'alpha',A set step size gain (default 1) +% 'novarstep' disable variable step size +% 'verbose' show number of iterations for each point +% 'verbose=2' show state at each iteration +% 'plot' plot iteration state versus time +% +% Notes:: +% - Solution is computed iteratively. +% - Solution is sensitive to choice of initial gain. The variable +% step size logic (enabled by default) does its best to find a balance +% between speed of convergence and divergence. +% - Some experimentation might be required to find the right values of +% tol, ilimit and alpha. +% - The pinv option sometimes leads to much faster convergence. +% - The tolerance is computed on the norm of the error between current +% and desired tool pose. This norm is computed from distances +% and angles without any kind of weighting. +% - The inverse kinematic solution is generally not unique, and +% depends on the initial guess Q0 (defaults to 0). +% - The default value of Q0 is zero which is a poor choice for most +% manipulators (eg. puma560, twolink) since it corresponds to a kinematic +% singularity. +% - Such a solution is completely general, though much less efficient +% than specific inverse kinematic solutions derived symbolically, like +% ikine6s or ikine3. +% - This approach allows a solution to obtained at a singularity, but +% the joint angles within the null space are arbitrarily assigned. +% - Joint offsets, if defined, are added to the inverse kinematics to +% generate Q. +% +% See also SerialLink.fkine, tr2delta, SerialLink.jacob0, SerialLink.ikine6s. + + +% 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.petercorke.com + +function [qt,histout] = ikine(robot, tr, varargin) + % set default parameters for solution + opt.ilimit = 1000; + opt.tol = 1e-6; + opt.alpha = 1; + opt.plot = false; + opt.pinv = false; + opt.varstep = true; + + [opt,args] = tb_optparse(opt, varargin); + + n = robot.n; + + % robot.ikine(tr, q) + if ~isempty(args) + q = args{1}; + if isempty(q) + q = zeros(1, n); + else + q = q(:)'; + end + else + q = zeros(1, n); + end + + % robot.ikine(tr, q, m) + if length(args) > 1 + m = args{2}; + m = m(:); + if numel(m) ~= 6 + error('Mask matrix should have 6 elements'); + end + if numel(find(m)) ~= robot.n + error('Mask matrix must have same number of 1s as robot DOF') + end + else + if n < 6 + error('For a manipulator with fewer than 6DOF a mask matrix argument must be specified'); + end + m = ones(6, 1); + end + % make this a logical array so we can index with it + m = logical(m); + + npoints = size(tr,3); % number of points + qt = zeros(npoints, n); % preallocate space for results + tcount = 0; % total iteration count + + if ~ishomog(tr) + error('RTB:ikine:badarg', 'T is not a homog xform'); + end + + J0 = jacob0(robot, q); +% J0 = J0(m, m); + J0 = J0(m, :); + if cond(J0) > 100 + warning('RTB:ikine:singular', 'Initial joint angles results in near-singular configuration, this may slow convergence'); + end + + history = []; + failed = false; + for i=1:npoints + T = tr(:,:,i); + + nm = Inf; + % initialize state for the ikine loop + eprev = Inf; + save.e = [Inf Inf Inf Inf Inf Inf]; + save.q = []; + count = 0; + + while true + % update the count and test against iteration limit + count = count + 1; + if count > opt.ilimit + warning('ikine: iteration limit %d exceeded (row %d), final err %f', ... + opt.ilimit, i, nm); + failed = true; + q = NaN*ones(1,n); + break + end + + % compute the error + e = tr2delta( robot.fkine(q'), T); + + % optionally adjust the step size + if opt.varstep + % test against last best error, only consider the DOF of + % interest + if norm(e(m)) < norm(save.e(m)) + % error reduced, + % let's save current state of solution and rack up the step size + save.q = q; + save.e = e; + opt.alpha = opt.alpha * (2.0^(1.0/8)); + if opt.verbose > 1 + fprintf('raise alpha to %f\n', opt.alpha); + end + else + % rats! error got worse, + % restore to last good solution and reduce step size + q = save.q; + e = save.e; + opt.alpha = opt.alpha * 0.5; + if opt.verbose > 1 + fprintf('drop alpha to %f\n', opt.alpha); + end + end + end + + % compute the Jacobian + J = jacob0(robot, q); + + % compute change in joint angles to reduce the error, + % based on the square sub-Jacobian + if opt.pinv + dq = opt.alpha * pinv( J(m,:) ) * e(m); + else + dq = J(m,:)' * e(m); + dq = opt.alpha * dq; + end + + % diagnostic stuff + if opt.verbose > 1 + fprintf('%d:%d: |e| = %f\n', i, count, nm); + fprintf(' e = '); disp(e'); + fprintf(' dq = '); disp(dq'); + end + if opt.plot + h.q = q'; + h.dq = dq; + h.e = e; + h.ne = nm; + h.alpha = opt.alpha; + history = [history; h]; %#ok<*AGROW> + end + + % update the estimated solution + % IVO modification + dq(3,1)=0; + %%%%% + q = q + dq'; + nm = norm(e(m)); + + if norm(e) > 1.5*norm(eprev) + warning('RTB:ikine:diverged', 'solution diverging, try reducing alpha'); + end + eprev = e; + + if nm <= opt.tol + break + end + + end % end ikine solution for tr(:,:,i) + qt(i,:) = q'; + tcount = tcount + count; + if opt.verbose + fprintf('%d iterations\n', count); + end + end + + if opt.verbose && npoints > 1 + fprintf('TOTAL %d iterations\n', tcount); + end + + % plot evolution of variables + if opt.plot + figure(1); + plot([history.q]'); + xlabel('iteration'); + ylabel('q'); + grid + + figure(2); + plot([history.dq]'); + xlabel('iteration'); + ylabel('dq'); + grid + + figure(3); + plot([history.e]'); + xlabel('iteration'); + ylabel('e'); + grid + + figure(4); + semilogy([history.ne]); + xlabel('iteration'); + ylabel('|e|'); + grid + + figure(5); + plot([history.alpha]); + xlabel('iteration'); + ylabel('\alpha'); + grid + + if nargout > 1 + histout = history; + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/ikine.m-new b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine.m-new new file mode 100644 index 0000000..da2bbc4 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine.m-new @@ -0,0 +1,290 @@ +%SerialLink.IKINE Inverse manipulator kinematics +% +% Q = R.ikine(T) is the joint coordinates corresponding to the robot +% end-effector pose T which is a homogenenous transform. +% +% Q = R.ikine(T, Q0, OPTIONS) specifies the initial estimate of the joint +% coordinates. +% +% Q = R.ikine(T, Q0, M, OPTIONS) specifies the initial estimate of the joint +% coordinates and a mask matrix. For the case where the manipulator +% has fewer than 6 DOF the solution space has more dimensions than can +% be spanned by the manipulator joint coordinates. In this case +% the mask matrix M specifies the Cartesian DOF (in the wrist coordinate +% frame) that will be ignored in reaching a solution. The mask matrix +% has six elements that correspond to translation in X, Y and Z, and rotation +% about X, Y and Z respectively. The value should be 0 (for ignore) or 1. +% The number of non-zero elements should equal the number of manipulator DOF. +% +% For example when using a 5 DOF manipulator rotation about the wrist z-axis +% might be unimportant in which case M = [1 1 1 1 1 0]. +% +% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence +% and R.ikine() returns the joint coordinates corresponding to each of the +% transforms in the sequence. Q is MxN where N is the number of robot joints. +% The initial estimate of Q for each time step is taken as the solution +% from the previous time step. +% +% Options:: +% 'pinv' use pseudo-inverse instead of Jacobian transpose +% 'ilimit',L set the maximum iteration count (default 1000) +% 'tol',T set the tolerance on error norm (default 1e-6) +% 'alpha',A set step size gain (default 1) +% 'novarstep' disable variable step size +% 'verbose' show number of iterations for each point +% 'verbose=2' show state at each iteration +% 'plot' plot iteration state versus time +% +% Notes:: +% - Solution is computed iteratively. +% - Solution is sensitive to choice of initial gain. The variable +% step size logic (enabled by default) does its best to find a balance +% between speed of convergence and divergence. +% - Some experimentation might be required to find the right values of +% tol, ilimit and alpha. +% - The pinv option sometimes leads to much faster convergence. +% - The tolerance is computed on the norm of the error between current +% and desired tool pose. This norm is computed from distances +% and angles without any kind of weighting. +% - The inverse kinematic solution is generally not unique, and +% depends on the initial guess Q0 (defaults to 0). +% - The default value of Q0 is zero which is a poor choice for most +% manipulators (eg. puma560, twolink) since it corresponds to a kinematic +% singularity. +% - Such a solution is completely general, though much less efficient +% than specific inverse kinematic solutions derived symbolically, like +% ikine6s or ikine3. +% - This approach allows a solution to obtained at a singularity, but +% the joint angles within the null space are arbitrarily assigned. +% - Joint offsets, if defined, are added to the inverse kinematics to +% generate Q. +% +% See also SerialLink.fkine, tr2delta, SerialLink.jacob0, SerialLink.ikine6s. + + +% 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.petercorke.com + +function [qt,histout] = ikine(robot, tr, varargin) + % set default parameters for solution + opt.ilimit = 1000; + opt.tol = 1e-6; + opt.alpha = 1; + opt.plot = false; + opt.pinv = false; + opt.varstep = true; + + [opt,args] = tb_optparse(opt, varargin); + + n = robot.n; + + % robot.ikine(tr, q) + if ~isempty(args) + q = args{1}; + if isempty(q) + q = zeros(1, n); + else + q = q(:)'; + end + else + q = zeros(1, n); + end + + % robot.ikine(tr, q, m) + if length(args) > 1 + m = args{2}; + m = m(:); + if numel(m) ~= 6 + error('Mask matrix should have 6 elements'); + end + if numel(find(m)) > robot.n + error('RTB:ikine:badarg', 'Number of robot DOF must be >= the same number of 1s in the mask matrix') + end + else + if n < 6 + error('RTB:ikine:badarg', 'For a manipulator with fewer than 6DOF a mask matrix argument must be specified'); + end + m = ones(6, 1); + end + % make this a logical array so we can index with it + m = logical(m); + + npoints = size(tr,3); % number of points + qt = zeros(npoints, n); % preallocate space for results + tcount = 0; % total iteration count + + if ~ishomog(tr) + error('RTB:ikine:badarg', 'T is not a homog xform'); + end + + J0 = jacob0(robot, q); + J0 = J0(m, m); + if cond(J0) > 100 + warning('RTB:ikine:singular', 'Initial joint angles results in near-singular configuration, this may slow convergence'); + end + %e = zeros(6,1); + + history = []; + failed = false; + for i=1:npoints + T = tr(:,:,i); + + nm = Inf; + % initialize state for the ikine loop + eprev = Inf; + save.e = [Inf Inf Inf Inf Inf Inf]; + save.q = []; + count = 0; + + while true + % update the count and test against iteration limit + count = count + 1; + if count > opt.ilimit + warning('ikine: iteration limit %d exceeded (row %d), final err %f', ... + opt.ilimit, i, nm); + failed = true; + q = NaN*ones(1,n); + break + end + + % compute the error T* - fkine + e = tr2delta( robot.fkine(q'), T); + % e = tr2delta( robot.fkine(q'), T); % T - robot.fkine(q) + %Tq = robot.fkine(q'); + %e = tr2delta( Tq, T); + %e(1:3) = transl(T - Tq); + %Rq = t2r(Tq); + %[th,n] = tr2angvec(t2r(T)*Rq'); + %[th,n] = tr2angvec(Rq'); + %e(4:6) = th*n; + + % optionally adjust the step size + if opt.varstep + % test against last best error, only consider the DOF of + % interest + if norm(e(m)) < norm(save.e(m)) + % error reduced, + % let's save current state of solution and rack up the step size + save.q = q; + save.e = e; + opt.alpha = opt.alpha * (2.0^(1.0/8)); + if opt.verbose > 1 + fprintf('raise alpha to %f\n', opt.alpha); + end + else + % rats! error got worse, + % restore to last good solution and reduce step size + q = save.q; + e = save.e; + opt.alpha = opt.alpha * 0.5; + if opt.verbose > 1 + fprintf('drop alpha to %f\n', opt.alpha); + end + end + end + + % compute the Jacobian + J = jacob0(robot, q); + + % compute change in joint angles to reduce the error, + % based on the square sub-Jacobian + if opt.pinv + dq = opt.alpha * pinv( J(m,:) ) * e(m); + else + dq = J(m,:)' * e(m); + dq = opt.alpha * dq; + end + + % diagnostic stuff + if opt.verbose > 1 + fprintf('%d/%d: |e| = %f\n', i, count, nm); + fprintf(' e = '); disp(e'); + fprintf(' dq = '); disp(dq'); + end + if opt.plot + h.q = q'; + h.dq = dq; + h.e = e; + h.ne = nm; + h.alpha = opt.alpha; + history = [history; h]; %#ok<*AGROW> + end + + % update the estimated solution + q = q + dq'; + nm = norm(e(m)); + + if norm(e) > 1.5*norm(eprev) + warning('RTB:ikine:diverged', 'solution diverging, try reducing alpha'); + end + eprev = e; + + if nm <= opt.tol + break + end + + end % end ikine solution for tr(:,:,i) + qt(i,:) = q'; + tcount = tcount + count; + if opt.verbose + fprintf('%d iterations\n', count); + end + end + + if opt.verbose && npoints > 1 + fprintf('TOTAL %d iterations\n', tcount); + end + + % plot evolution of variables + if opt.plot + figure(1); + plot([history.q]'); + xlabel('iteration'); + ylabel('q'); + grid + + figure(2); + plot([history.dq]'); + xlabel('iteration'); + ylabel('dq'); + grid + + figure(3); + plot([history.e]'); + xlabel('iteration'); + ylabel('e'); + grid + + figure(4); + semilogy([history.ne]); + xlabel('iteration'); + ylabel('|e|'); + grid + + figure(5); + plot([history.alpha]); + xlabel('iteration'); + ylabel('\alpha'); + grid + + if nargout > 1 + histout = history; + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/ikine3.m b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine3.m new file mode 100644 index 0000000..f24c043 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine3.m @@ -0,0 +1,157 @@ +%SerialLink.ikine3 Inverse kinematics for 3-axis robot with no wrist +% +% Q = R.ikine3(T) is the joint coordinates corresponding to the robot +% end-effector pose T represented by the homogenenous transform. This +% is a analytic solution for a 3-axis robot (such as the first three joints +% of a robot like the Puma 560). +% +% Q = R.ikine3(T, CONFIG) as above but specifies the configuration of the arm in +% the form of a string containing one or more of the configuration codes: +% +% 'l' arm to the left (default) +% 'r' arm to the right +% 'u' elbow up (default) +% 'd' elbow down +% +% Notes:: +% - The same as IKINE6S without the wrist. +% - The inverse kinematic solution is generally not unique, and +% depends on the configuration string. +% - Joint offsets, if defined, are added to the inverse kinematics to +% generate Q. +% +% Reference:: +% +% Inverse kinematics for a PUMA 560 based on the equations by Paul and Zhang +% From The International Journal of Robotics Research +% Vol. 5, No. 2, Summer 1986, p. 32-44 +% +% +% Author:: +% Robert Biro with Gary Von McMurray, +% GTRI/ATRP/IIMB, +% Georgia Institute of Technology +% 2/13/95 +% +% See also SerialLink.FKINE, SerialLink.IKINE. + +function theta = ikine3(robot, T, varargin) + + if ~strncmp(robot.config, 'RRR', 3) + error('Solution only applicable for 6DOF all-revolute manipulator'); + end + + if robot.mdh ~= 0 + error('Solution only applicable for standard DH conventions'); + end + + if ndims(T) == 3 + theta = zeros(size(T,3),robot.n); + for k=1:size(T,3) + theta(k,:) = ikine3(robot, T(:,:,k), varargin{:}); + end + return; + end + L = robot.links; + a2 = L(2).a; + a3 = L(3).a; + + d3 = L(3).d; + + if ~ishomog(T) + error('T is not a homog xform'); + end + + % undo base transformation + T = robot.base \ T; + + % The following parameters are extracted from the Homogeneous + % Transformation as defined in equation 1, p. 34 + + Px = T(1,4); + Py = T(2,4); + Pz = T(3,4); + + % The configuration parameter determines what n1,n2 values are used + % and how many solutions are determined which have values of -1 or +1. + + if nargin < 3 + configuration = ''; + else + configuration = lower(varargin{1}); + end + + % default configuration + + n1 = -1; % L + n2 = -1; % U + if ~isempty(strfind(configuration, 'l')) + n1 = -1; + end + if ~isempty(strfind(configuration, 'r')) + n1 = 1; + end + if ~isempty(strfind(configuration, 'u')) + if n1 == 1 + n2 = 1; + else + n2 = -1; + end + end + if ~isempty(strfind(configuration, 'd')) + if n1 == 1 + n2 = -1; + else + n2 = 1; + end + end + + % + % Solve for theta(1) + % + % r is defined in equation 38, p. 39. + % theta(1) uses equations 40 and 41, p.39, + % based on the configuration parameter n1 + % + + r=sqrt(Px^2 + Py^2); + if (n1 == 1) + theta(1)= atan2(Py,Px) + asin(d3/r); + else + theta(1)= atan2(Py,Px) + pi - asin(d3/r); + end + + % + % Solve for theta(2) + % + % V114 is defined in equation 43, p.39. + % r is defined in equation 47, p.39. + % Psi is defined in equation 49, p.40. + % theta(2) uses equations 50 and 51, p.40, based on the configuration + % parameter n2 + % + + V114= Px*cos(theta(1)) + Py*sin(theta(1)); + r=sqrt(V114^2 + Pz^2); + Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r)); + if ~isreal(Psi) + warning('RTB:ikine3:notreachable', 'point not reachable'); + theta = [NaN NaN NaN NaN NaN NaN]; + return + end + theta(2) = atan2(Pz,V114) + n2*Psi; + + % + % Solve for theta(3) + % + % theta(3) uses equation 57, p. 40. + % + + num = cos(theta(2))*V114+sin(theta(2))*Pz-a2; + den = cos(theta(2))*Pz - sin(theta(2))*V114; + theta(3) = atan2(a3,d4) - atan2(num, den); + + % remove the link offset angles + for i=1:robot.n %#ok<*AGROW> + theta(i) = theta(i) - L(i).offset; + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/ikine6s.m b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine6s.m new file mode 100644 index 0000000..0773a7d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine6s.m @@ -0,0 +1,233 @@ +%SerialLink.ikine6s Inverse kinematics for 6-axis robot with spherical wrist +% +% Q = R.ikine6s(T) is the joint coordinates corresponding to the robot +% end-effector pose T represented by the homogenenous transform. This +% is a analytic solution for a 6-axis robot with a spherical wrist (such as +% the Puma 560). +% +% Q = R.IKINE6S(T, CONFIG) as above but specifies the configuration of the arm in +% the form of a string containing one or more of the configuration codes: +% +% 'l' arm to the left (default) +% 'r' arm to the right +% 'u' elbow up (default) +% 'd' elbow down +% 'n' wrist not flipped (default) +% 'f' wrist flipped (rotated by 180 deg) +% +% Notes:: +% - Only applicable for an all revolute 6-axis robot RRRRRR. +% - The inverse kinematic solution is generally not unique, and +% depends on the configuration string. +% - Joint offsets, if defined, are added to the inverse kinematics to +% generate Q. +% +% Reference:: +% - Inverse kinematics for a PUMA 560, +% Paul and Zhang, +% The International Journal of Robotics Research, +% Vol. 5, No. 2, Summer 1986, p. 32-44 +% +% Author:: +% Robert Biro with Gary Von McMurray, +% GTRI/ATRP/IIMB, +% Georgia Institute of Technology +% 2/13/95 +% +% See also SerialLink.FKINE, SerialLink.IKINE. + +function theta = ikine6s(robot, T, varargin) + + if ~strcmp(robot.config, 'RRRRRR') + error('Solution only applicable for 6DOF all-revolute manipulator'); + end + + if robot.mdh ~= 0 + error('Solution only applicable for standard DH conventions'); + end + + if ndims(T) == 3 + theta = zeros(size(T,3),robot.n); + for k=1:size(T,3) + theta(k,:) = ikine6s(robot, T(:,:,k), varargin{:}); + end + return; + end + L = robot.links; + a2 = L(2).a; + a3 = L(3).a; + + if ~robot.isspherical() + error('wrist is not spherical') + end + + d3 = L(3).d; + d4 = L(4).d; + + if ~ishomog(T) + error('RTB:ikine:badarg', 'T is not a homog xform'); + end + + % undo base and tool transformations + T = inv(robot.base) * T; + T = T * inv(robot.tool); + + % The following parameters are extracted from the Homogeneous + % Transformation as defined in equation 1, p. 34 + + Ox = T(1,2); + Oy = T(2,2); + Oz = T(3,2); + + Ax = T(1,3); + Ay = T(2,3); + Az = T(3,3); + + Px = T(1,4); + Py = T(2,4); + Pz = T(3,4); + + % The configuration parameter determines what n1,n2,n4 values are used + % and how many solutions are determined which have values of -1 or +1. + + if nargin < 3 + configuration = ''; + else + configuration = lower(varargin{1}); + end + + % default configuration + + n1 = -1; % L + n2 = -1; % U + n4 = -1; % N + if ~isempty(strfind(configuration, 'l')) + n1 = -1; + end + if ~isempty(strfind(configuration, 'r')) + n1 = 1; + end + if ~isempty(strfind(configuration, 'u')) + if n1 == 1 + n2 = 1; + else + n2 = -1; + end + end + if ~isempty(strfind(configuration, 'd')) + if n1 == 1 + n2 = -1; + else + n2 = 1; + end + end + if ~isempty(strfind(configuration, 'n')) + n4 = 1; + end + if ~isempty(strfind(configuration, 'f')) + n4 = -1; + end + + + % + % Solve for theta(1) + % + % r is defined in equation 38, p. 39. + % theta(1) uses equations 40 and 41, p.39, + % based on the configuration parameter n1 + % + + r=sqrt(Px^2 + Py^2); + if (n1 == 1) + theta(1)= atan2(Py,Px) + asin(d3/r); + else + theta(1)= atan2(Py,Px) + pi - asin(d3/r); + end + + % + % Solve for theta(2) + % + % V114 is defined in equation 43, p.39. + % r is defined in equation 47, p.39. + % Psi is defined in equation 49, p.40. + % theta(2) uses equations 50 and 51, p.40, based on the configuration + % parameter n2 + % + + V114= Px*cos(theta(1)) + Py*sin(theta(1)); + r=sqrt(V114^2 + Pz^2); + Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r)); + if ~isreal(Psi) + warning('RTB:ikine6s:notreachable', 'point not reachable'); + theta = [NaN NaN NaN NaN NaN NaN]; + return + end + theta(2) = atan2(Pz,V114) + n2*Psi; + + % + % Solve for theta(3) + % + % theta(3) uses equation 57, p. 40. + % + + num = cos(theta(2))*V114+sin(theta(2))*Pz-a2; + den = cos(theta(2))*Pz - sin(theta(2))*V114; + theta(3) = atan2(a3,d4) - atan2(num, den); + + % + % Solve for theta(4) + % + % V113 is defined in equation 62, p. 41. + % V323 is defined in equation 62, p. 41. + % V313 is defined in equation 62, p. 41. + % theta(4) uses equation 61, p.40, based on the configuration + % parameter n4 + % + + V113 = cos(theta(1))*Ax + sin(theta(1))*Ay; + V323 = cos(theta(1))*Ay - sin(theta(1))*Ax; + V313 = cos(theta(2)+theta(3))*V113 + sin(theta(2)+theta(3))*Az; + theta(4) = atan2((n4*V323),(n4*V313)); + %[(n4*V323),(n4*V313)] + + % + % Solve for theta(5) + % + % num is defined in equation 65, p. 41. + % den is defined in equation 65, p. 41. + % theta(5) uses equation 66, p. 41. + % + + num = -cos(theta(4))*V313 - V323*sin(theta(4)); + den = -V113*sin(theta(2)+theta(3)) + Az*cos(theta(2)+theta(3)); + theta(5) = atan2(num,den); + %[num den] + + % + % Solve for theta(6) + % + % V112 is defined in equation 69, p. 41. + % V122 is defined in equation 69, p. 41. + % V312 is defined in equation 69, p. 41. + % V332 is defined in equation 69, p. 41. + % V412 is defined in equation 69, p. 41. + % V432 is defined in equation 69, p. 41. + % num is defined in equation 68, p. 41. + % den is defined in equation 68, p. 41. + % theta(6) uses equation 70, p. 41. + % + + V112 = cos(theta(1))*Ox + sin(theta(1))*Oy; + V132 = sin(theta(1))*Ox - cos(theta(1))*Oy; + V312 = V112*cos(theta(2)+theta(3)) + Oz*sin(theta(2)+theta(3)); + V332 = -V112*sin(theta(2)+theta(3)) + Oz*cos(theta(2)+theta(3)); + V412 = V312*cos(theta(4)) - V132*sin(theta(4)); + V432 = V312*sin(theta(4)) + V132*cos(theta(4)); + num = -V412*cos(theta(5)) - V332*sin(theta(5)); + den = - V432; + theta(6) = atan2(num,den); + + % remove the link offset angles + for i=1:robot.n %#ok<*AGROW> + theta(i) = theta(i) - L(i).offset; + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/ikine_sym.m b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine_sym.m new file mode 100644 index 0000000..3c9356a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/ikine_sym.m @@ -0,0 +1,484 @@ +%IKINE_SYM Symbolic inverse kinematics +% +% Q = R.IKINE_SYM(K, OPTIONS) is a cell array (Cx1) of inverse kinematic +% solutions of the SerialLink object ROBOT. The cells of Q represent the +% different possible configurations. Each cell of Q is a vector (Nx1), and +% element J is the symbolic expressions for the J'th joint angle. The +% solution is in terms of the desired end-point pose of the robot which is +% represented by the symbolic matrix (3x4) with elements +% nx ox ax tx +% ny oy ay ty +% nz oz az tz +% where the first three columns specify orientation and the last column +% specifies translation. +% +% K <= N can have only specific values: +% - 2 solve for translation tx and ty +% - 3 solve for translation tx, ty and tz +% - 6 solve for translation and orientation +% +% Options:: +% +% 'file',F Write the solution to an m-file named F +% +% Example:: +% +% mdl_planar2 +% sol = p2.ikine_sym(2); +% length(sol) +% ans = +% 2 % there are 2 solutions +% s1 = sol{1} % is one solution +% q1 = s1(1); % the expression for q1 +% q2 = s1(2); % the expression for q2 +% +% Notes:: +% - Requires the Symbolic Toolbox for MATLAB. +% - This code is experimental and has a lot of diagnostic prints. +% - Based on the classical approach using Pieper's method. + +% Copyright (C) 1993-2015, 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.petercorke.com + +function out = ikine_sym(srobot, N, varargin) + + % + % Given a robot model the following steps are performed: + % 1. Convert model to symbolic form + % 2. Find relevant trig equations and solve them for joint angles + % 3. Write an M-file to implement the solution + % xikine(T) + % xikine(T, S) where S is a 3 vector with elements 1 or 2 to select + % the first or second solution for the corresponding joint. + % + % TODO: + % - handle the wrist joints, only first 3 joints so far + % - handle base and tool transforms + + opt.file = []; + opt = tb_optparse(opt, varargin); + + % make a symbolic representation of the passed robot + srobot = sym(srobot); + q = srobot.gencoords(); + + % test N DOF has an allowable value + switch N + case 2 + case 3 + case 6 + otherwise + error('RTB:ikine_sym:badarg', 'Can only solve for 2,3,6 DOF'); + end + + % define symbolic elements of the homogeneous transform + syms nx ox ax tx + syms ny oy ay ty + syms nz oz az tz + syms d3 + + % inits + Q = {}; + trigsubOld = []; + trigsubNew = []; + + % loop over each joint variable + for j=1:N + fprintf('----- solving for joint %d\n', j); + + % create some equations to sift through + [left,right] = pieper(srobot, j, 'left'); + + % decide which equations to look at + if j <= 3 + % for first three joints only focus on translational part + left = left(1:3, 4); left = left(:); + right = right(1:3, 4); right = right(:); + else + % for last three joints only focus on rotational part + left = left(1:3, 1:3); left = left(:); + right = right(1:3, 1:3); right = right(:); + end + + % substitute sin/cos for preceding joint as S/C, essentially removes + % the joint variables from the equations and treats them as constants. + if ~isempty(trigsubOld) + left = subs(left, trigsubOld, trigsubNew); + right = subs(right, trigsubOld, trigsubNew); + end + + % then simplify the LHS + % do it after the substitution to prevent sum of angle terms being introduced + left = simplify(left); + + % search for a solveable equation: + % function of current joint variable on the LHS + % constant element on the RHS + k = NaN; + for i=1:length(left) + if hasonly(left(i), j) && isconstant(right(i)) + k = i; + break; + end + end + + eq = []; + + if ~isnan(k) + % create the equation to solve: LHS-RHS == 0 + eq = left(k) - right(k); + else + % ok, we weren't lucky, try another strategy + + % find all equations: + % function of current joint variable on the LHS + + k = []; + for i=1:length(left) + % has qj on the left and constant on the right + if hasonly(left(i), j) + k = [k i]; + end + end + + % hopefully we found two of them + if length(k) < 2 + continue; + end + + % we did, lets see if the sum square RHS is constant + rhs = simplify(right(k(1))^2 + right(k(2))^2); % was simple + if isconstant( rhs ) + % it is, let's sum and square the LHS + fprintf('lets square and add %d %d\n', k); + + eq = simplify( expand( left(k(1))^2 + left(k(2))^2 ) ) - rhs; % was simple + end + end + + % expand the list of joint variable subsitutions + fprintf('subs sin/cos q%d for S/C\n', j); + trigsubOld = [trigsubOld mvar('sin(q%d)', j) mvar('cos(q%d)', j)]; + trigsubNew = [trigsubNew mvar('S%d', j) mvar('C%d', j)]; + + if isempty(eq) + fprintf('cant solve this equation'); + k + left(k)==right(k) + error('cant solve'); + end + % now solve the equation + if srobot.links(j).isrevolute() + % for revolute joint it will be a trig equation, do we know how to solve it? + Q{j} = solve_joint(eq, j ); + if isempty(Q) + warning('cant solve this kind of equation'); + end + else + fprintf('prismatic case\n') + q = sym( sprintf('q%d', j) ); + Q{j} = solve( eq == 0, q); + end + end + + % final simplification + % get rid of C^2+S^2 and C^4, S^4 terms + fprintf('**final simplification pass\n') + + % create a list of simplifications + % substitute S^2 = 1-C^2, S^4=(1-C^2)^2 + tsubOld = []; + tsubNew = []; + for j=1:N + tsubOld = [tsubOld mvar('S%d', j)^2 mvar('S%d', j)^4]; + tsubNew = [tsubNew 1-mvar('C%d', j)^2 (1-mvar('C%d', j)^2)^2]; + end + + for j=1:N + for k=1:5 + % seem to need to iterate this, not quite sure why + Q{j} = simplify( expand( subs(Q{j}, tsubOld, tsubNew) ) ); + end + end + + % Q is a cell array of equations for joint variables + if nargout > 0 + out = Q; + end + + if ~isempty(opt.file) + fprintf('**generate MATLAB code\n') + gencode(Q); + end +end + +%PIEPER Return a set of equations using Pieper's method +% +% [L,R] = pieper(robot, n, which) +% +% If robot has link matrix A1 A2 A3 A4 then returns 12 equations from equating the coefficients of +% +% A1' T = A2 A3 A4 n=1, which='left' +% A2' A1' T = A3 A4 n=2, which='left' +% A3' A2' A1' T = A4 n=3, which='left' +% +% T A4' = A1 A2 A3 n=1, which='right' +% T A4' A3' = A1 A2 n=2, which='right' +% T A4' A3' A2' = A1 n=3, which='right' +% +% Judicious choice of the equations can lead to joint solutions + +function [L,R] = pieper(robot, n, which) + + if nargin < 3 + which = 'left'; + end + + syms nx ox ax tx real + syms ny oy ay ty real + syms nz oz az tz real + + T = [nx ox ax tx + ny oy ay ty + nx oz az tz + 0 0 0 1 ]; + + T = inv(robot.base) * T * inv(robot.tool); + + q = robot.gencoords(); + + + % Create the symbolic A matrices + for j=1:robot.n + A{j} = robot.links(j).A(q(j)); + end + + switch which + case 'left' + left = T; + for j=1:n + left = inv(A{j}) * left ; + end + + right = eye(4,4); + for j=n+1:robot.n + right = right * A{j}; + end + + case 'right' + left = T; + for j=1:n + left = left * inv(A{robot.n-j+1}); + end + + right = eye(4,4); + for j=1:(robot.n-n) + right = right * A{j}; + end + end + + % left = simple(left); + % right = simple(right); + + if nargout == 0 + left == right + elseif nargout == 1 + L = left; + elseif nargout == 2 + L = left; + R = right; + end +end + +%SOLVE_JOINT Solve a trigonometric equation +% +% S = SOLVE_JOINT(EQ, J) solves the equation EQ=0 for the joint variable qJ. +% The result is a cell array of solutions. +% +% The equations must be of the form: +% A cos(qJ) + B sin(qJ) = 0 +% A cos(qJ) + B sin(qJ) = C +% +% where A, B, C are arbitrarily complex expressions. qJ can be the only +% joint variable in the expression. + +function s = solve_joint(eq, j) + + sinj = mvar('sin(q%d)', j); + cosj = mvar('cos(q%d)', j); + + A = getcoef(eq, cosj); + B = getcoef(eq, sinj); + + if isempty(A) || isempty(B) + warning('don''t know how to solve this kind of equation'); + end + + C = -simplify(eq - A*cosj - B*sinj); % was simple + + if C == 0 + % A cos(q) + B sin(q) = 0 + s(2) = atan2(A, -B); + s(1) = atan2(-A, B); + else + % A cos(q) + B sin(q) = C + r = sqrt(A^2 + B^2 - C^2); + phi = atan2(A, B); + + s(2) = atan2(C, r) - phi; + s(1) = atan2(C, -r) - phi; + end + if nargout == 0 + try + eval(s) + catch + s + end + end +end + +%MVAR Create a symbolic variable +% +% V = MVAR(FMT, ARGS) is a symbolic variable created using SPRINTF +% +% eg. mvar('q%d', j) +% +% The symbolic is explicitly declared to be real. + +function v = mvar(fmt, varargin) + + if isempty(strfind(fmt, '(')) + % not a function + v = sym( sprintf(fmt, varargin{:}), 'real' ); + else + v = sym( sprintf(fmt, varargin{:}) ); + + end +end + +%HASONLY Determine if an expression contains only certain joint variables +% +% S = HASONLY(E L) is true if the joint variables (q1, q2 etc.) in the expression E +% are listed in the vector L. +% +% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1 2 3]) -> true +% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1]) -> false + +function s = hasonly(eq, j) + + q = findq(eq); + if isempty(q) + s = false; + else + s = all(ismember(j, findq(eq))); + end +end + +%ISCONSTANT Determine if an expression is free of joint variables +% +% S = ISCONSTANT(E) is true if the expression E contains no joint variables such +% q1, q2 etc. + +function s = isconstant(eq) + s = isempty(findq(eq)); +end + +%FINDQ Find the joint variables in expression +% +% Q = FINDQ(E) returns a list of integers indicating the joint variables found +% in the expression E. For instance an instance of 'q1' would cause a 1 to be +% returned and so on. +% +% Eg. findq('sin(q1)*cos(q2)+S3') -> [1 2] + +function q = findq(s) + + q = []; + + for var=symvar(s) + if isempty(var) + break + end + varname = char(var); + if varname(1) == 'q' + q = [q str2num(varname(2:end))]; + end + end +end + +function coef = getcoef(eq, trig) + z = children( collect(eq, trig) ); + z = children( z(1) ); + coef = z(1); +end + +% Output a joint expression to a file + +function s = gencode(Q, filename) + + function s = G(s, fmt, varargin) + s = strvcat(s, sprintf(fmt, varargin{:})); + end + + s = 'function q = xikine(T, sol)'; + s = G(s, ' if nargin < 2; sol = ones(1, %d); end', length(Q)); + s = G(s, ' px = T(1,4); py = T(2,4); pz = T(3,4);'); + + for j=1:3 + Qj = Q{j}; % cast it to subclass + if length(Qj) == 1 + s = G(s, ' q(%d) = %s', j, matgen2(Qj)); + elseif length(Qj) == 2 + s = G(s, ' if sol(%d) == 1', j); + s = G(s, ' q(%d) = %s', j, matgen2(Qj(1))); + s = G(s, ' else'); + s = G(s, ' q(%d) = %s', j, matgen2(Qj(2))); + s = G(s, ' end'); + + + end + + + s = G(s, ' S%d = sin(q(%d));', j, j); + s = G(s, ' C%d = cos(q(%d));', j, j); + s = G(s, ' '); + + + end + s = G(s, 'end'); + + fp = fopen(filename, 'w'); + for i=1:numrows(s) + fprintf(fp, '%s\n', deblank(s(i,:))); + end + fclose(fp); + +end + +% Generate MATLAB code from an expression +% +% Requires a bit of a hack, a subclass of sym (sym2) to do this + +function s = matgen2(e) + + s = matgen(sym2(e)); + + k = strfind(s, '='); + s = deblank( s(k+2:end) ); +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/ikinem.m b/lwrserv/matlab/rvctools/robot/@SerialLink/ikinem.m new file mode 100644 index 0000000..47bb5e1 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/ikinem.m @@ -0,0 +1,157 @@ +%SerialLink.IKINEM Numerical inverse kinematics by minimization +% +% Q = R.ikinem(T) is the joint coordinates corresponding to the robot +% end-effector pose T which is a homogenenous transform. +% +% Q = R.ikinem(T, Q0, OPTIONS) specifies the initial estimate of the joint +% coordinates. +% +% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence +% and R.ikinem() returns the joint coordinates corresponding to each of the +% transforms in the sequence. Q is MxN where N is the number of robot joints. +% The initial estimate of Q for each time step is taken as the solution +% from the previous time step. +% +% Options:: +% 'pweight',P weighting on position error norm compared to rotation +% error (default 1) +% 'stiffness',S Stiffness used to impose a smoothness contraint on joint +% angles, useful when N is large (default 0) +% 'qlimits' Enforce joint limits +% 'ilimit',L Iteration limit (default 1000) +% 'nolm' Disable Levenberg-Marquadt +% +% Notes:: +% - PROTOTYPE CODE UNDER DEVELOPMENT, intended to do numerical inverse kinematics +% with joint limits +% - The inverse kinematic solution is generally not unique, and +% depends on the initial guess Q0 (defaults to 0). +% - The function to be minimized is highly nonlinear and the solution is +% often trapped in a local minimum, adjust Q0 if this happens. +% - The default value of Q0 is zero which is a poor choice for most +% manipulators (eg. puma560, twolink) since it corresponds to a kinematic +% singularity. +% - Such a solution is completely general, though much less efficient +% than specific inverse kinematic solutions derived symbolically, like +% ikine6s or ikine3.% - Uses Levenberg-Marquadt minimizer LMFsolve if it can be found, +% if 'nolm' is not given, and 'qlimits' false +% - The error function to be minimized is computed on the norm of the error +% between current and desired tool pose. This norm is computed from distances +% and angles and 'pweight' can be used to scale the position error norm to +% be congruent with rotation error norm. +% - This approach allows a solution to obtained at a singularity, but +% the joint angles within the null space are arbitrarily assigned. +% - Joint offsets, if defined, are added to the inverse kinematics to +% generate Q. +% - Joint limits become explicit contraints if 'qlimits' is set. +% +% See also fminsearch, fmincon, SerialLink.fkine, SerialLink.ikine, tr2angvec. + + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function qt = ikinem(robot, tr, varargin) + + opt.pweight = 1; + opt.stiffness = 0; + opt.qlimits = false; + opt.ilimit = 1000; + opt.lm = true; + opt.col = 2; + + [opt,args] = tb_optparse(opt, varargin); + + % check if optional argument is a valid q + q0 = args{1}; + if numel(q0) ~= robot.n + error('q0 length must match number of joints in robot'); + end + + + for i=1:size(tr,3) + T = tr(:,:,i); + + if opt.qlimits + % constrained optimization to handle joint limits + options = optimset('MaxIter', opt.ilimit); + qlim = robot.qlim; + + [q, ef, exflag, output] = fmincon( @(x) costfun(x, robot, T, opt), q0, ... + [], [], [], [], ... + qlim(:,1), qlim(:,2), ... + [], options); + + if opt.verbose + fprintf('final error %f, %d iterations, %d evalations\n', ... + ef, output.iterations, output.funcCount); + end + + else + % no joint limits, unconstrained optimization + if exist('LMFsolve') == 2 && opt.lm + [q, ef, count] = LMFsolve( @(x) costfun(x, robot, T, opt), q0, 'MaxIter', opt.ilimit); + q = q'; + if opt.verbose + fprintf('final error %f, %d iterations\n', ... + ef, count); + end + else + options = optimset('MaxIter', opt.ilimit); + [q, ef, exflag, output] = fminsearch( @(x) costfun(x, robot, T, opt), q0, options); + + if opt.verbose + fprintf('final error %f, %d iterations, %d evalations\n', ... + ef, output.iterations, output.funcCount); + end + end + end + + qt(i,:) = q; + end + + if opt.verbose + robot.fkine(qt) + end +end + +% The cost function, this is the value to be minimized +function E = costfun(q, robot, T, opt) + + Tq = robot.fkine(q); + % find the pose error in SE(3) + dT = transl(T) - transl(Tq); + + % translation error + E = norm(dT) * opt.pweight; + + % rotation error + % find dot product of + dd = dot(T(1:3,opt.col), Tq(1:3,opt.col)); + %E = E + (1 - dd)^2*100000 ; + E = E + acos(dd)^2*1000 ; + + + if opt.stiffness > 0 + % enforce a continuity constraint on joints, minimum bend + E = E + sum( diff(q).^2 ) * opt.stiffness; + end +end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/ikunc.m b/lwrserv/matlab/rvctools/robot/@SerialLink/ikunc.m new file mode 100644 index 0000000..361561d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/ikunc.m @@ -0,0 +1,116 @@ +%SerialLink.IKUNC Numerical inverse manipulator without joint limits +% +% Q = R.ikunc(T) are the joint coordinates (1xN) corresponding to the robot +% end-effector pose T (4x4) which is a homogenenous transform, and N is the +% number of robot joints. +% +% [Q,ERR] = robot.ikunc(T) as above but also returns ERR which is the +% scalar final value of the objective function. +% +% [Q,ERR,EXITFLAG] = robot.ikunc(T) as above but also returns the +% status EXITFLAG from fminunc. +% +% [Q,ERR,EXITFLAG] = robot.ikunc(T, Q0) as above but specify the +% initial joint coordinates Q0 used for the minimisation. +% +% [Q,ERR,EXITFLAG] = robot.ikunc(T, Q0, options) as above but specify the +% options for fminunc to use. +% +% Trajectory operation:: +% +% In all cases if T is 4x4xM it is taken as a homogeneous transform +% sequence and R.ikunc() returns the joint coordinates corresponding to +% each of the transforms in the sequence. Q is MxN where N is the number +% of robot joints. The initial estimate of Q for each time step is taken as +% the solution from the previous time step. +% +% ERR and EXITFLAG are also Mx1 and indicate the results of optimisation +% for the corresponding trajectory step. +% +% Notes:: +% - Requires fminunc from the Optimization Toolbox. +% - Joint limits are not considered in this solution. +% - Can be used for robots with arbitrary degrees of freedom. +% - In the case of multiple feasible solutions, the solution returned +% depends on the initial choice of Q0 +% - Works by minimizing the error between the forward kinematics of the +% joint angle solution and the end-effector frame as an optimisation. +% The objective function (error) is described as: +% sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega ) +% Where omega is some gain matrix, currently not modifiable. +% +% Author:: +% Bryan Moutrie +% +% See also SerialLink.ikcon, fmincon, SerialLink.ikine, SerialLink.fkine. + +% Copyright (C) Bryan Moutrie, 2013-2015 +% Licensed under the GNU Lesser General Public License +% see full file for full statement +% +% LICENSE STATEMENT: +% +% This file is part of pHRIWARE. +% +% pHRIWARE 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. +% +% pHRIWARE 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 General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public +% License along with pHRIWARE. If not, see . + + +function [qstar, error, exitflag] = ikunc(robot, T, q0, options) + + % check if Optimization Toolbox exists, we need it + if ~exist('fminunc') + error('rtb:ikunc:nosupport', 'Optimization Toolbox required'); + end + + % create output variables + T_sz = size(T,3); + qstar = zeros(T_sz,robot.n); + error = zeros(T_sz,1); + exitflag = zeros(T_sz,1); + + problem.solver = 'fminunc'; + problem.x0 = zeros(1, robot.n); + problem.options = optimoptions('fminunc', ... + 'Algorithm', 'quasi-newton', ... + 'Display', 'off'); % default options for ikunc + + if nargin > 2 + problem.x0 = q0; + end + if nargin > 3 + problem.options = optimset(problem.options, options); + end + + reach = sum(abs([robot.a, robot.d])); + omega = diag([1 1 1 3/reach]); + + for t = 1:T_sz + problem.objective = ... + @(x) sumsqr(((T(:,:,t) \ robot.fkine(x)) - eye(4)) * omega); + + [q_t, err_t, ef_t] = fminunc(problem); + + qstar(t,:) = q_t; + error(t) = err_t; + exitflag(t) = ef_t; + + problem.x0 = q_t; + end + +end + +function s = sumsqr(A) + s = sum(A(:).^2); +end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/inertia.m b/lwrserv/matlab/rvctools/robot/@SerialLink/inertia.m new file mode 100644 index 0000000..992c5f6 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/inertia.m @@ -0,0 +1,62 @@ +%SerialLink.INERTIA Manipulator inertia matrix +% +% I = R.inertia(Q) is the symmetric joint inertia matrix (NxN) which relates +% joint torque to joint acceleration for the robot at joint configuration Q. +% +% If Q is a matrix (KxN), each row is interpretted as a joint state +% vector, and the result is a 3d-matrix (NxNxK) where each plane corresponds +% to the inertia for the corresponding row of Q. +% +% Notes:: +% - The diagonal elements I(J,J) are the inertia seen by joint actuator J. +% - The off-diagonal elements I(J,K) are coupling inertias that relate +% acceleration on joint J to force/torque on joint K. +% - The diagonal terms include the motor inertia reflected through the gear +% ratio. +% +% See also SerialLink.RNE, SerialLink.CINERTIA, SerialLink.ITORQUE. + + +% 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.petercorke.com + +function M = inertia(robot, q) + if numcols(q) ~= robot.n + error('q must have %d columns', robot.n); + end + + if numrows(q) > 1 + M = []; + for i=1:numrows(q) + M = cat(3, M, robot.inertia(q(i,:))); + end + return + end + + n = robot.n; + + if numel(q) == robot.n + q = q(:).'; + end + + M = zeros(n,n,0); + for Q = q.' + m = rne(robot, ones(n,1)*Q.', zeros(n,n), eye(n), [0;0;0]); + M = cat(3, M, m); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/issym.m b/lwrserv/matlab/rvctools/robot/@SerialLink/issym.m new file mode 100644 index 0000000..4d0cab8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/issym.m @@ -0,0 +1,20 @@ +function res = issym(l) +%% ISSYM Checks wether Link or SerialLink object is a symbolic model. +% +% res = issym(l) +% +% Input:: +% l: Link or SerialLink class object (1x1) +% +% Output:: +% res: true if l has symbolic variables as parameters. +% +% Authors:: +% Jörn Malzahn +% 2012 RST, Technische Universität Dortmund, Germany +% http://www.rst.e-technik.tu-dortmund.de +% + + res = issym(l.links(1)); + +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/itorque.m b/lwrserv/matlab/rvctools/robot/@SerialLink/itorque.m new file mode 100644 index 0000000..9c81029 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/itorque.m @@ -0,0 +1,39 @@ +%SerialLink.itorque Inertia torque +% +% TAUI = R.itorque(Q, QDD) is the inertia force/torque vector (1xN) at the +% specified joint configuration Q (1xN) and acceleration QDD (1xN), that is, +% TAUI = INERTIA(Q)*QDD. +% +% If Q and QDD are matrices (KxN), each row is interpretted as a joint state +% vector, and the result is a matrix (KxN) where each row is the corresponding +% joint torques. +% +% Note:: +% - If the robot model contains non-zero motor inertia then this will +% included in the result. +% +% See also SerialLink.rne, SerialLink.inertia. + + + +% 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.petercorke.com + +function it = itorque(robot, q, qdd) + it = rne(robot, q, zeros(size(q)), qdd, [0;0;0]); diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/jacob0.m b/lwrserv/matlab/rvctools/robot/@SerialLink/jacob0.m new file mode 100644 index 0000000..391e8be --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/jacob0.m @@ -0,0 +1,84 @@ +%SerialLink.JACOB0 Jacobian in world coordinates +% +% J0 = R.jacob0(Q, OPTIONS) is the Jacobian matrix (6xN) for the robot in +% pose Q (1xN). The manipulator Jacobian matrix maps joint velocity to +% end-effector spatial velocity V = J0*QD expressed in the world-coordinate +% frame. +% +% Options:: +% 'rpy' Compute analytical Jacobian with rotation rate in terms of +% roll-pitch-yaw angles +% 'eul' Compute analytical Jacobian with rotation rates in terms of +% Euler angles +% 'trans' Return translational submatrix of Jacobian +% 'rot' Return rotational submatrix of Jacobian +% +% Note:: +% - The Jacobian is computed in the world frame and transformed to the +% end-effector frame. +% - The default Jacobian returned is often referred to as the geometric +% Jacobian, as opposed to the analytical Jacobian. +% +% See also SerialLink.jacobn, jsingu, deltatr, tr2delta, jsingu. + +% 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.petercorke.com + +function J0 = jacob0(robot, q, varargin) + + opt.rpy = false; + opt.eul = false; + opt.trans = false; + opt.rot = false; + + opt = tb_optparse(opt, varargin); + + % + % dX_tn = Jn dq + % + Jn = jacobn(robot, q); % Jacobian from joint to wrist space + + % + % convert to Jacobian in base coordinates + % + Tn = fkine(robot, q); % end-effector transformation + R = t2r(Tn); + J0 = [R zeros(3,3); zeros(3,3) R] * Jn; + + if opt.rpy + rpy = tr2rpy( fkine(robot, q) ); + B = rpy2jac(rpy); + if rcond(B) < eps + error('Representational singularity'); + end + J0 = blkdiag( eye(3,3), inv(B) ) * J0; + elseif opt.eul + eul = tr2eul( fkine(robot, q) ); + B = eul2jac(eul); + if rcond(B) < eps + error('Representational singularity'); + end + J0 = blkdiag( eye(3,3), inv(B) ) * J0; + end + + if opt.trans + J0 = J0(1:3,:); + elseif opt.rot + J0 = J0(4:6,:); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/jacob_dot.m b/lwrserv/matlab/rvctools/robot/@SerialLink/jacob_dot.m new file mode 100644 index 0000000..1ed801c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/jacob_dot.m @@ -0,0 +1,99 @@ +%SerialLink.jacob_dot Derivative of Jacobian +% +% JDQ = R.jacob_dot(Q, QD) is the product (6x1) of the derivative of the +% Jacobian (in the world frame) and the joint rates. +% +% Notes:: +% - Useful for operational space control XDD = J(Q)QDD + JDOT(Q)QD +% - Written as per the text and not very efficient. +% +% References:: +% - Fundamentals of Robotics Mechanical Systems (2nd ed) +% J. Angleles, Springer 2003. +% +% See also: SerialLink.jacob0, diff2tr, tr2diff. + +% 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.petercorke.com + +function Jdot = jacob_dot(robot, q, qd) + + n = robot.n; + links = robot.links; + + % Using the notation of Angeles: + % [Q,a] ~ [R,t] the per link transformation + % P ~ R the cumulative rotation t2r(Tj) in world frame + % e the last column of P, the local frame z axis in world coordinates + % w angular velocity in base frame + % ed deriv of e + % r is distance from final frame + % rd deriv of r + % ud ?? + + for i=1:n + T = links(i).A(q(i)); + Q{i} = t2r(T); + a{i} = transl(T); + end + + P{1} = Q{1}; + e{1} = [0 0 1]'; + for i=2:n + P{i} = P{i-1}*Q{i}; + e{i} = P{i}(:,3); + end + + % step 1 + w{1} = qd(1)*e{1}; + for i=1:(n-1) + w{i+1} = qd(i+1)*[0 0 1]' + Q{i}'*w{i}; + end + + % step 2 + ed{1} = [0 0 0]'; + for i=2:n + ed{i} = cross(w{i}, e{i}); + end + + % step 3 + rd{n} = cross( w{n}, a{n}); + for i=(n-1):-1:1 + rd{i} = cross(w{i}, a{i}) + Q{i}*rd{i+1}; + end + + r{n} = a{n}; + for i=(n-1):-1:1 + r{i} = a{i} + Q{i}*r{i+1}; + end + + ud{1} = cross(e{1}, rd{1}); + for i=2:n + ud{i} = cross(ed{i}, r{i}) + cross(e{i}, rd{i}); + end + + % step 4 + % swap ud and ed + v{n} = qd(n)*[ud{n}; ed{n}]; + for i=(n-1):-1:1 + Ui = blkdiag(Q{i}, Q{i}); + v{i} = qd(i)*[ud{i}; ed{i}] + Ui*v{i+1}; + end + + Jdot = v{1}; diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/jacobn.m b/lwrserv/matlab/rvctools/robot/@SerialLink/jacobn.m new file mode 100644 index 0000000..8eeee2b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/jacobn.m @@ -0,0 +1,90 @@ +%SerialLink.JACOBN Jacobian in end-effector frame +% +% JN = R.jacobn(Q, options) is the Jacobian matrix (6xN) for the robot in +% pose Q. The manipulator Jacobian matrix maps joint velocity to +% end-effector spatial velocity V = JN*QD in the end-effector frame. +% +% Options:: +% 'trans' Return translational submatrix of Jacobian +% 'rot' Return rotational submatrix of Jacobian +% +% Notes:: +% - This Jacobian is often referred to as the geometric Jacobian. +% +% Reference:: +% Differential Kinematic Control Equations for Simple Manipulators, +% Paul, Shimano, Mayer, +% IEEE SMC 11(6) 1981, +% pp. 456-460 +% +% See also SerialLink.jacob0, jsingu, delta2tr, tr2delta. + + + +% 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.petercorke.com + +function J = jacobn(robot, q, varargin) + + opt.trans = false; + opt.rot = false; + + opt = tb_optparse(opt, varargin); + + n = robot.n; + L = robot.links; % get the links + + if isa(q, 'sym') + tau(6, robot.n) = sym(); + else + J = zeros(6, robot.n); + end + U = robot.tool; + for j=n:-1:1 + if robot.mdh == 0 + % standard DH convention + U = L(j).A(q(j)) * U; + end + if L(j).RP == 'R' + % revolute axis + d = [ -U(1,1)*U(2,4)+U(2,1)*U(1,4) + -U(1,2)*U(2,4)+U(2,2)*U(1,4) + -U(1,3)*U(2,4)+U(2,3)*U(1,4)]; + delta = U(3,1:3)'; % nz oz az + else + % prismatic axis + d = U(3,1:3)'; % nz oz az + delta = zeros(3,1); % 0 0 0 + end + J(:,j) = [d; delta]; + + if robot.mdh ~= 0 + % modified DH convention + U = L(j).A(q(j)) * U; + end + end + if opt.trans + J = J(1:3,:); + elseif opt.rot + J = J(4:6,:); + end + + if isa(J, 'sym') + J = simplify(J); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/maniplty.m b/lwrserv/matlab/rvctools/robot/@SerialLink/maniplty.m new file mode 100644 index 0000000..7b32d5b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/maniplty.m @@ -0,0 +1,150 @@ +%SerialLink.MANIPLTY Manipulability measure +% +% M = R.maniplty(Q, OPTIONS) is the manipulability index measure for the robot +% at the joint configuration Q. It indicates dexterity, that is, how isotropic +% the robot's motion is with respect to the 6 degrees of Cartesian motion. +% The measure is high when the manipulator is capable of equal motion in all +% directions and low when the manipulator is close to a singularity. +% +% If Q is a matrix (MxN) then M (Mx1) is a vector of manipulability +% indices for each pose specified by a row of Q. +% +% [M,CI] = R.maniplty(Q, OPTIONS) as above, but for the case of the Asada +% measure returns the Cartesian inertia matrix CI. +% +% Two measures can be selected: +% - Yoshikawa's manipulability measure is based on the shape of the velocity +% ellipsoid and depends only on kinematic parameters. +% - Asada's manipulability measure is based on the shape of the acceleration +% ellipsoid which in turn is a function of the Cartesian inertia matrix and +% the dynamic parameters. The scalar measure computed here is the ratio of +% the smallest/largest ellipsoid axis. Ideally the ellipsoid would be +% spherical, giving a ratio of 1, but in practice will be less than 1. +% +% Options:: +% 'T' manipulability for transational motion only (default) +% 'R' manipulability for rotational motion only +% 'all' manipulability for all motions +% 'dof',D D is a vector (1x6) with non-zero elements if the +% corresponding DOF is to be included for manipulability +% 'yoshikawa' use Yoshikawa algorithm (default) +% 'asada' use Asada algorithm +% +% Notes:: +% - By default the measure includes rotational and translational dexterity, but +% this involves adding different units. It can be more useful to look at the +% translational and rotational manipulability separately. +% +% References:: +% +% - Analysis and control of robot manipulators with redundancy, +% T. Yoshikawa, +% Robotics Research: The First International Symposium (M. Brady and R. Paul, eds.), +% pp. 735-747, The MIT press, 1984. +% - A geometrical representation of manipulator dynamics and its application to +% arm design, +% H. Asada, +% Journal of Dynamic Systems, Measurement, and Control, +% vol. 105, p. 131, 1983. +% +% See also SerialLink.inertia, SerialLink.jacob0. + + + +% 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.petercorke.com + +%TODO +% return the ellipsoid? + +function [w,mx] = maniplty(robot, q, varargin) + + opt.method = {'yoshikawa', 'asada'}; + opt.axes = {'T', 'all', 'R'}; + opt.dof = []; + + opt = tb_optparse(opt, varargin); + + if isempty(opt.dof) + switch opt.axes + case 'T' + dof = [1 1 1 0 0 0]; + case 'R' + dof = [0 0 0 1 1 1]; + case 'all' + dof = [1 1 1 1 1 1]; + end + else + dof = opt.dof; + end + + opt.dof = logical(dof); + + if strcmp(opt.method, 'yoshikawa') + w = zeros(numrows(q),1); + for i=1:numrows(q) + w(i) = yoshi(robot, q(i,:), opt); + end + elseif strcmp(opt.method, 'asada') + w = zeros(numrows(q),1); + if nargout > 1 + dof = sum(opt.dof); + MX = zeros(dof,dof,numrows(q)); + for i=1:numrows(q) + [ww,mm] = asada(robot, q(i,:), opt); + w(i) = ww; + MX(:,:,i) = mm; + end + else + for i=1:numrows(q) + w(i) = asada(robot, q(i,:), opt); + end + end + end + + if nargout > 1 + mx = MX; + end + +function m = yoshi(robot, q, opt) + J = robot.jacob0(q); + + J = J(opt.dof,:); + m = sqrt(det(J * J')); + +function [m, mx] = asada(robot, q, opt) + J = robot.jacob0(q); + + if rank(J) < 6 + warning('robot is in degenerate configuration') + m = 0; + return; + end + + Ji = pinv(J); + M = robot.inertia(q); + Mx = Ji' * M * Ji; + d = find(opt.dof); + Mx = Mx(d,d); + e = eig(Mx); + m = min(e) / max(e); + + if nargout > 1 + mx = Mx; + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/nofriction.m b/lwrserv/matlab/rvctools/robot/@SerialLink/nofriction.m new file mode 100644 index 0000000..adf2820 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/nofriction.m @@ -0,0 +1,47 @@ +%SerialLink.nofriction Remove friction +% +% RNF = R.nofriction() is a robot object with the same parameters as R but +% with non-linear (Coulomb) friction coefficients set to zero. +% +% RNF = R.nofriction('all') as above but all friction coefficients set to zero. +% +% RNF = R.nofriction('viscous') as above but only viscous friction coefficients +% are set to zero. +% +% Notes:: +% - Non-linear (Coulomb) friction can cause numerical problems when integrating +% the equations of motion (R.fdyn). +% - The resulting robot object has its name string prefixed with 'NF/'. +% +% See also SerialLink.fdyn, Link.nofriction. + + + +% 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.petercorke.com + +function r2 = nofriction(r, varargin) + + r2 = SerialLink(r); % make a copy + + for j=1:r2.n + r2.links(j) = r.links(j).nofriction(varargin{:}); + end + + r2.name = ['NF/' r.name]; diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/old/animate2.m b/lwrserv/matlab/rvctools/robot/@SerialLink/old/animate2.m new file mode 100644 index 0000000..4715de5 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/old/animate2.m @@ -0,0 +1,172 @@ +%SerialLink.animate Update a robot animation +% +% R.animate(q) updates an existing animation for the robot R. This will have +% been created using R.plot(). +% +% Updates graphical instances of this robot in all figures. +% +% Notes:: +% - Not a general purpose method, used for Simulink robot animation. +% +% See also SerialLink.plot. + +function animate(robot, q, handles) + + if nargin < 3 + handles = findobj('Tag', robot.name); + end + + for handle=handles' % for each graphical robot instance + h = get(handle, 'UserData'); + animate2( h, q); + end + + +function animate2(h, q) + + robot = h.robot; + n = robot.n; + L = robot.links; + + mag = h.mag; + + % compute the link transforms, and record the origin of each frame + % for the animation. + + + % points on the multiline, skeleton of the arm + x = zeros(1,n); + y = zeros(1,n); + z = zeros(1,n); + + % points on the multiline, skeleton of the shadow + xs = zeros(1,n); + ys = zeros(1,n); + zs = zeros(1,n); + + % add first point to the link/shadow line, the base + + t = robot.base; + + x(1) = t(1,4); + y(1) = t(2,4); + z(1) = t(3,4); + xs(1) = t(1,4); + ys(1) = t(2,4); + zs(1) = h.zmin; + + Tn = t; % robot base + + if robot.mdh == 0 + % standard DH parameterization + + % Tn(:,:,j) is pose of joint j + for j=1:n + Tn(:,:,j) = t; + + t = t * L(j).A(q(j)); + + % add point to arm and shadow skeleton + % first point is the base + x(j+1) = t(1,4); + y(j+1) = t(2,4); + z(j+1) = t(3,4); + xs(j+1) = t(1,4); + ys(j+1) = t(2,4); + zs(j+1) = h.zmin; + end + t = t *robot.tool; + else + % modified DH parameterization + for j=1:n + x(j+1) = t(1,4); + y(j+1) = t(2,4); + z(j+1) = t(3,4); + xs(j+1) = t(1,4); + ys(j+1) = t(2,4); + zs(j+1) = h.zmin; + + t = t * L(j).A(q(j)); + Tn(:,:,j) = t; + + + % add point to arm and shadow skeleton + % first point is the base + + end + t = t *robot.tool; + + end + + % + % draw the robot stick figure and the shadow + % + set(h.links,'xdata', x, 'ydata', y, 'zdata', z); + if isfield(h, 'shadow') + set(h.shadow,'xdata', xs, 'ydata', ys, 'zdata', zs); + end + + + % + % display the joints as cylinders with rotation axes + % + if isfield(h, 'joint') + xyz_line = [0 0; 0 0; -2*mag 2*mag; 1 1]; + + for j=1:n + % get coordinate data from the cylinder + xyz = get(h.joint(j), 'UserData'); + + % transform to current joint frame + xyz = Tn(:,:,j) * xyz; + + % convert to 1-vector + ncols = numcols(xyz)/2; + xc = reshape(xyz(1,:), 2, ncols); + yc = reshape(xyz(2,:), 2, ncols); + zc = reshape(xyz(3,:), 2, ncols); + + % and update the coordinates of the graphical model + set(h.joint(j), 'Xdata', xc, 'Ydata', yc, ... + 'Zdata', zc); + + if isfield(h, 'jointaxis') + % optionally draw the joint axis + + % transform line endpoints to current joint frame + xyzl = Tn(:,:,j) * xyz_line; + + % and update the graphical line + set(h.jointaxis(j), 'Xdata', xyzl(1,:), ... + 'Ydata', xyzl(2,:), ... + 'Zdata', xyzl(3,:)); + set(h.jointlabel(j), 'Position', xyzl(1:3,1)); + end + end + end + + % + % display the wrist axes and labels + % + if isfield(h, 'x') + % + % compute the wrist axes, based on final link transformation + % plus the tool transformation. + % + xv = t*[mag;0;0;1]; + yv = t*[0;mag;0;1]; + zv = t*[0;0;mag;1]; + + % + % update the line segments, wrist axis and links + % + set(h.x,'xdata',[t(1,4) xv(1)], 'ydata', [t(2,4) xv(2)], ... + 'zdata', [t(3,4) xv(3)]); + set(h.y,'xdata',[t(1,4) yv(1)], 'ydata', [t(2,4) yv(2)], ... + 'zdata', [t(3,4) yv(3)]); + set(h.z,'xdata',[t(1,4) zv(1)], 'ydata', [t(2,4) zv(2)], ... + 'zdata', [t(3,4) zv(3)]); + set(h.xt, 'Position', xv(1:3)); + set(h.yt, 'Position', yv(1:3)); + set(h.zt, 'Position', zv(1:3)); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/old/ikine2.m b/lwrserv/matlab/rvctools/robot/@SerialLink/old/ikine2.m new file mode 100644 index 0000000..aafba25 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/old/ikine2.m @@ -0,0 +1,186 @@ + +function [qt,histout] = ikine2(robot, tr, varargin) + % set default parameters for solution + opt.ilimit = 1000; + opt.tol = 1e-6; + opt.alpha = 1; + opt.plot = false; + opt.pinv = true; + opt.varstep = false; + + tau = 1e-2; + v = 2; + + [opt,args] = tb_optparse(opt, varargin); + + n = robot.n; + + % robot.ikine(tr, q) + if ~isempty(args) + q = args{1}; + if isempty(q) + q = zeros(1, n); + else + q = q(:)'; + end + else + q = zeros(1, n); + end + + % robot.ikine(tr, q, m) + if length(args) > 1 + m = args{2}; + m = m(:); + if numel(m) ~= 6 + error('RTB:ikine:badarg', 'Mask matrix should have 6 elements'); + end + if numel(find(m)) > robot.n + error('RTB:ikine:badarg', 'Number of robot DOF must be >= the same number of 1s in the mask matrix') + end + else + if n < 6 + error('RTB:ikine:badarg', 'For a manipulator with fewer than 6DOF a mask matrix argument must be specified'); + end + m = ones(6, 1); + end + % make this a logical array so we can index with it + m = logical(m); + + npoints = size(tr,3); % number of points + qt = zeros(npoints, n); % preallocate space for results + tcount = 0; % total iteration count + + if ~ishomog(tr) + error('RTB:ikine:badarg', 'T is not a homog xform'); + end + + J0 = jacob0(robot, q); + J0 = J0(m, :); + if cond(J0) > 100 + warning('RTB:ikine:singular', 'Initial joint angles results in near-singular configuration, this may slow convergence'); + end + + history = []; + failed = false; + e = zeros(6,1); + + revolutes = [robot.links.sigma] == 0; + + for i=1:npoints % for each transform on trajectory + T = tr(:,:,i); + + nm = Inf; + % initialize state for the ikine loop + eprev = Inf; + save.e = [Inf Inf Inf Inf Inf Inf]; + save.q = []; + count = 0; + + while true % iterate the solution + % update the count and test against iteration limit + count = count + 1; + if count > opt.ilimit + warning('ikine: iteration limit %d exceeded (row %d), final err %f', ... + opt.ilimit, i, nm); + failed = true; + %q = NaN*ones(1,n); + break + end + + % compute the error + [en,e] = errfun(robot, T, q, m); + + if en <= opt.tol + break + end + + % compute the Jacobian + J = jacob0(robot, q); + + % compute change in joint angles to reduce the error, + % based on the square sub-Jacobian + + J = J(m,:); + + JJ = J'*J; + lambda = tau *diag(JJ); + + + while true % the Levenberg-Marquadt iteration + + dq1 = inv(JJ + diag(lambda))*J' * e(m)'; + en1 = errfun(robot, T, q-dq1', m); + +% dq2 = inv(JJ + diag(lambda/v))*J' * e(m)'; +% en2 = errfun(robot, T, q-dq2', m); + + if en1 > en + disp('increase lambda'); + lambda = lambda * v; + else + q = q - dq1'; + break; % dq1 is good + end + + [en en1] + +% if en2 > en +% if en1 > en +% disp('increase lambda'); +% lambda = lambda * v; +% else +% break; % dq1 is good +% end +% else +% disp('decrease lambda'); +% +% lambda = lambda / v; +% end + end + + + disp(' done'); + + % update the estimated solution + %q = q + dq1'; + + % wrap angles for revolute joints + k = (q > pi) & revolutes; + q(k) = q(k) - 2*pi; + + k = (q < -pi) & revolutes; + q(k) = q(k) + 2*pi; + + + if norm(e) > 1.5*norm(eprev) + warning('RTB:ikine:diverged', 'solution diverging, try reducing alpha'); + end + eprev = e; + + + end % end ikine solution for tr(:,:,i) + qt(i,:) = q'; + tcount = tcount + count; + if opt.verbose + fprintf('%d iterations\n', count); + end + end + + if opt.verbose && npoints > 1 + fprintf('TOTAL %d iterations\n', tcount); + end + + +end + +function [nm,e] = errfun(robot, T, q, m) + Tq = robot.fkine(q'); + + e(1:3) = transl(T - Tq); + Rq = t2r(Tq); + [th,n] = tr2angvec(t2r(T)*Rq'); + e(4:6) = th*n; + + nm = norm(e(m)); + +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/old/ikine3.m b/lwrserv/matlab/rvctools/robot/@SerialLink/old/ikine3.m new file mode 100644 index 0000000..f24c043 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/old/ikine3.m @@ -0,0 +1,157 @@ +%SerialLink.ikine3 Inverse kinematics for 3-axis robot with no wrist +% +% Q = R.ikine3(T) is the joint coordinates corresponding to the robot +% end-effector pose T represented by the homogenenous transform. This +% is a analytic solution for a 3-axis robot (such as the first three joints +% of a robot like the Puma 560). +% +% Q = R.ikine3(T, CONFIG) as above but specifies the configuration of the arm in +% the form of a string containing one or more of the configuration codes: +% +% 'l' arm to the left (default) +% 'r' arm to the right +% 'u' elbow up (default) +% 'd' elbow down +% +% Notes:: +% - The same as IKINE6S without the wrist. +% - The inverse kinematic solution is generally not unique, and +% depends on the configuration string. +% - Joint offsets, if defined, are added to the inverse kinematics to +% generate Q. +% +% Reference:: +% +% Inverse kinematics for a PUMA 560 based on the equations by Paul and Zhang +% From The International Journal of Robotics Research +% Vol. 5, No. 2, Summer 1986, p. 32-44 +% +% +% Author:: +% Robert Biro with Gary Von McMurray, +% GTRI/ATRP/IIMB, +% Georgia Institute of Technology +% 2/13/95 +% +% See also SerialLink.FKINE, SerialLink.IKINE. + +function theta = ikine3(robot, T, varargin) + + if ~strncmp(robot.config, 'RRR', 3) + error('Solution only applicable for 6DOF all-revolute manipulator'); + end + + if robot.mdh ~= 0 + error('Solution only applicable for standard DH conventions'); + end + + if ndims(T) == 3 + theta = zeros(size(T,3),robot.n); + for k=1:size(T,3) + theta(k,:) = ikine3(robot, T(:,:,k), varargin{:}); + end + return; + end + L = robot.links; + a2 = L(2).a; + a3 = L(3).a; + + d3 = L(3).d; + + if ~ishomog(T) + error('T is not a homog xform'); + end + + % undo base transformation + T = robot.base \ T; + + % The following parameters are extracted from the Homogeneous + % Transformation as defined in equation 1, p. 34 + + Px = T(1,4); + Py = T(2,4); + Pz = T(3,4); + + % The configuration parameter determines what n1,n2 values are used + % and how many solutions are determined which have values of -1 or +1. + + if nargin < 3 + configuration = ''; + else + configuration = lower(varargin{1}); + end + + % default configuration + + n1 = -1; % L + n2 = -1; % U + if ~isempty(strfind(configuration, 'l')) + n1 = -1; + end + if ~isempty(strfind(configuration, 'r')) + n1 = 1; + end + if ~isempty(strfind(configuration, 'u')) + if n1 == 1 + n2 = 1; + else + n2 = -1; + end + end + if ~isempty(strfind(configuration, 'd')) + if n1 == 1 + n2 = -1; + else + n2 = 1; + end + end + + % + % Solve for theta(1) + % + % r is defined in equation 38, p. 39. + % theta(1) uses equations 40 and 41, p.39, + % based on the configuration parameter n1 + % + + r=sqrt(Px^2 + Py^2); + if (n1 == 1) + theta(1)= atan2(Py,Px) + asin(d3/r); + else + theta(1)= atan2(Py,Px) + pi - asin(d3/r); + end + + % + % Solve for theta(2) + % + % V114 is defined in equation 43, p.39. + % r is defined in equation 47, p.39. + % Psi is defined in equation 49, p.40. + % theta(2) uses equations 50 and 51, p.40, based on the configuration + % parameter n2 + % + + V114= Px*cos(theta(1)) + Py*sin(theta(1)); + r=sqrt(V114^2 + Pz^2); + Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r)); + if ~isreal(Psi) + warning('RTB:ikine3:notreachable', 'point not reachable'); + theta = [NaN NaN NaN NaN NaN NaN]; + return + end + theta(2) = atan2(Pz,V114) + n2*Psi; + + % + % Solve for theta(3) + % + % theta(3) uses equation 57, p. 40. + % + + num = cos(theta(2))*V114+sin(theta(2))*Pz-a2; + den = cos(theta(2))*Pz - sin(theta(2))*V114; + theta(3) = atan2(a3,d4) - atan2(num, den); + + % remove the link offset angles + for i=1:robot.n %#ok<*AGROW> + theta(i) = theta(i) - L(i).offset; + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/old/plot2.m b/lwrserv/matlab/rvctools/robot/@SerialLink/old/plot2.m new file mode 100644 index 0000000..37eebf9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/old/plot2.m @@ -0,0 +1,521 @@ +%SerialLink.plot Graphical display and animation +% +% R.plot(Q, options) displays a graphical animation of a robot based on +% the kinematic model. A stick figure polyline joins the origins of +% the link coordinate frames. The robot is displayed at the joint angle Q (1xN), or +% if a matrix (MxN) it is animated as the robot moves along the M-point trajectory. +% +% Options:: +% 'workspace', W size of robot 3D workspace, W = [xmn, xmx ymn ymx zmn zmx] +% 'delay', d delay betwen frames for animation (s) +% 'fps',fps set number of frames per second for display +% '[no]loop' loop over the trajectory forever +% 'mag', scale annotation scale factor +% 'cylinder', C color for joint cylinders, C=[r g b] +% 'ortho' orthogonal camera view (default) +% 'perspective' perspective camera view +% 'xyz' wrist axis label is XYZ +% 'noa' wrist axis label is NOA +% '[no]raise' autoraise the figure (very slow). +% '[no]render' controls shaded rendering after drawing +% '[no]base' controls display of base 'pedestal' +% '[no]wrist' controls display of wrist +% '[no]shadow' controls display of shadow +% '[no]name' display the robot's name +% '[no]jaxes' control display of joint axes +% '[no]joints' controls display of joints +% 'movie',M save frames as files in the folder M +% +% +% The options come from 3 sources and are processed in order: +% - Cell array of options returned by the function PLOTBOTOPT (if it exists) +% - Cell array of options given by the 'plotopt' option when creating the +% SerialLink object. +% - List of arguments in the command line. +% +% Many boolean options can be enabled or disabled with the 'no' prefix. The +% various option sources can toggle an option, the last value is taken. +% +% To save the effort of processing options on each call they can be preprocessed by +% opts = robot.plot({'opt1', 'opt2', ... }); +% and the resulting object can be passed in to subsequent calls instead of text-based +% options, for example: +% robot.plot(q, opts); +% +% Graphical annotations and options:: +% +% The robot is displayed as a basic stick figure robot with annotations +% such as: +% - shadow on the floor +% - XYZ wrist axes and labels +% - joint cylinders and axes +% which are controlled by options. +% +% The size of the annotations is determined using a simple heuristic from +% the workspace dimensions. This dimension can be changed by setting the +% multiplicative scale factor using the 'mag' option. +% +% Figure behaviour:: +% +% - If no figure exists one will be created and teh robot drawn in it. +% - If no robot of this name is currently displayed then a robot will +% be drawn in the current figure. If hold is enabled (hold on) then the +% robot will be added to the current figure. +% - If the robot already exists then that graphical model will be found +% and moved. +% +% Multiple views of the same robot:: +% +% If one or more plots of this robot already exist then these will all +% be moved according to the argument Q. All robots in all windows with +% the same name will be moved. +% +% Create a robot in figure 1 +% figure(1) +% p560.plot(qz); +% Create a robot in figure 2 +% figure(2) +% p560.plot(qz); +% Now move both robots +% p560.plot(qn) +% +% Multiple robots in the same figure:: +% +% Multiple robots can be displayed in the same plot, by using "hold on" +% before calls to robot.plot(). +% +% Create a robot in figure 1 +% figure(1) +% p560.plot(qz); +% Make a clone of the robot named bob +% bob = SerialLink(p560, 'name', 'bob'); +% Draw bob in this figure +% hold on +% bob.plot(qn) +% +% To animate both robots so they move together: +% qtg = jtraj(qr, qz, 100); +% for q=qtg' +% p560.plot(q'); +% bob.plot(q'); +% end +% +% Making an animation movie:: +% - The 'movie' options saves frames as files NNNN.png. +% - When using 'movie' option ensure that the window is fully visible. +% - To convert frames to a movie use a command like: +% ffmpeg -r 10 -i %04d.png out.avi +% +% Notes:: +% - The skeleton line does not neccessarily represent the links of the robot, it +% comprises straight line segments that join the origins of successive link +% coordinate frames. +% - Delay betwen frames can be eliminated by setting option 'delay', 0 or +% 'fps', Inf. +% - By default a quite detailed plot is generated, but turning off labels, +% axes, shadows etc. will speed things up. +% - Each graphical robot object is tagged by the robot's name and has UserData +% that holds graphical handles and the handle of the robot object. +% - The graphical state holds the last joint configuration which can be retrieved +% using q = robot.plot(). +% - The size of the plot volume is determined by a heuristic for an all-revolute +% robot. If a prismatic joint is present the 'workspace' option is required. +% +% See also plotbotopt, SerialLink.animate, SerialLink.fkine. + + +% HANDLES: +% +% A robot comprises a bunch of individual graphical elements and these are +% kept in a structure: +% +% h.link the robot stick figure +% h.shadow the robot's shadow +% h.x wrist vectors +% h.y +% h.z +% h.xt wrist vector labels +% h.yt +% h.zt +% +% h.q the last set of joint coordinates +% h.robot pointer to the robot object +% h.opt the final options structure +% +% The h.link graphical element is tagged with the robot's name and has this +% struct as its UserData. +% +% h.links -> h -> robot +% +% This enables us to find all robots with a given name, in all figures, +% and update them. + + +% 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.petercorke.com + +function retval = plot(robot, tg, varargin) + + % opts = PLOT(robot, options) + % just convert options list to an options struct + if (nargin == 2) && iscell(tg) + retval = plot_options(robot, tg); + return; + end + + % + % q = PLOT(robot) + % return joint coordinates from a graphical robot of given name + % + %TODO should be robot.get_q() + if nargin == 1 + handles = findobj('Tag', robot.name); + if ~isempty(handles) + h = get(handles(1), 'UserData'); + retval = h.q; + end + return + end + + % process options + if (nargin > 2) && isstruct(varargin{1}) + % options is a struct + opt = varargin{1}; + else + % options is a list of options + opt = plot_options(robot, varargin); + end + + % + % robot2 = ROBOT(robot, q, varargin) + % + np = numrows(tg); + n = robot.n; + + if numcols(tg) ~= n + error('Insufficient columns in q') + end + +% if ~ishandle(robot.handle), +% %disp('has handles') +% % handles provided, animate just that robot +% count = opt.repeat; +% while count > 0 +% for p=1:np, +% animate( robot, tg(p,:)); +% pause(opt.delay) +% end +% count = count - 1; +% end +% return; +% end + + % get handle of any existing graphical robot of same name + handles = findobj('Tag', robot.name); + + if isempty(handles) + % no robot of this name exists in any figure, create one + + if ishold + % hold is on, add the robot to this figure + + if isempty( findobj(gca, 'Tag', robot.name)) + % if no robot of this name in current axes, create one + handle = create_new_robot(robot, opt); + else + handle = findobj(gca, 'Tag', robot.name); + end + + else + % hold not on, create a robot in this or new plot + + newplot(); + handle = create_new_robot(robot, opt); + + end + + % tag one of the graphical handles with the robot name and hang + % the handle structure off it + set(handle.links, 'Tag', robot.name); + set(handle.links, 'UserData', handle); + + handles = handle.links; + end + + +% if isempty( get(gcf, 'Children')) + + if opt.raise + % note this is a very time consuming operation + figure(gcf); + end + + if ~isempty(opt.movie) + mkdir(opt.movie); + framenum = 1; + end + + while true + for p=1:np % for each point on path + robot.animate(tg(p,:), handles); + %drawnow + + if ~isempty(opt.movie) + % write the frame to the movie folder + print( '-dpng', fullfile(opt.movie, sprintf('%04d.png', framenum)) ); + framenum = framenum+1; + end + + if opt.delay > 0 + pause(opt.delay); + drawnow + end + end + if ~opt.loop + break; + end + end + + % save the last joint angles away in the graphical robot + for handle=handles' + h = get(handle, 'UserData'); + h.q = tg(end,:); + set(handle, 'UserData', h); + end +end + +%PLOT_OPTIONS +% +% o = PLOT_OPTIONS(robot, options) +% +% Returns an options structure + +function o = plot_options(robot, optin) + % process a cell array of options and return a struct + + % define all possible options and their default values + o.erasemode = 'normal'; + o.joints = true; + o.wrist = true; + o.loop = false; + o.shadow = true; + o.wrist = true; + o.jaxes = true; + o.base = true; + o.wristlabel = 'xyz'; + o.perspective = true; + o.magscale = 1; + o.name = true; + o.delay = 0.1; + o.fps = []; + o.raise = true; + o.cylinder = [0 0 0.7]; + o.workspace = []; + o.movie = []; + o.render = true; + o.ortho = true; + o.perspective = false; + + + % build a list of options from all sources + % 1. the M-file plotbotopt if it exists + % 2. robot.plotopt + % 3. command line arguments + if exist('plotbotopt', 'file') == 2 + options = [plotbotopt robot.plotopt optin]; + else + options = [robot.plotopt optin]; + end + + % parse the options + [o,args] = tb_optparse(o, options); + if ~isempty(args) + error(['unknown option: ' args{1}]); + end + + if isempty(o.workspace) + % + % simple heuristic to figure the maximum reach of the robot + % + L = robot.links; + if any(L.isprismatic) + error('Prismatic joint(s) present: requires the ''workspace'' option'); + end + reach = 0; + for i=1:robot.n + reach = reach + abs(L(i).a) + abs(L(i).d); + end + o.workspace = [-reach reach -reach reach -reach reach]; + o.mag = reach/10; + else + reach = min(abs(o.workspace)); + end + o.mag = o.magscale * reach/10; + + if ~isempty(o.fps) + o.delay = 1/o.fps; + end + +end + +%CREATE_NEW_ROBOT +% +% h = CREATE_NEW_ROBOT(robot, opt) +% +% Using data from robot object and options create a graphical robot in +% the current figure. +% +% Returns a structure of handles to graphical objects. +% +% If current figure is empty, draw robot in it +% If current figure has hold on, add robot to it +% Otherwise, create new figure and draw robot in it. +% + +% h.mag +% h.zmin +% h.links the line segment that is the robot +% h.shadow the robot's shadow +% h.x the line segment that is T6 x-axis +% h.y the line segment that is T6 x-axis +% h.z the line segment that is T6 x-axis +% h.xt text for T6 x-axis +% h.yt text for T6 y-axis +% h.zt text for T6 z-axis +% h.joint(i) the joint i cylinder or box +% h.jointaxis(i) the line segment that is joint i axis +% h.jointlabel(i) text for joint i label + +function h = create_new_robot(robot, opt) + h.opt = opt; % the options + h.robot = robot; % pointer to robot + + h.mag = opt.mag; + % + % setup an axis in which to animate the robot + % + % handles not provided, create graphics + %disp('in creat_new_robot') + if ~ishold + % if hold is off, set the axis dimensions + axis(opt.workspace); + end + xlabel('X') + ylabel('Y') + zlabel('Z') + %set(gca, 'drawmode', 'fast'); + grid on + + zlim = get(gca, 'ZLim'); + h.zmin = zlim(1); + + if opt.base + b = transl(robot.base); + line('xdata', [b(1);b(1)], ... + 'ydata', [b(2);b(2)], ... + 'zdata', [h.zmin;b(3)], ... + 'LineWidth', 4, ... + 'color', 'red'); + end + + if opt.name + b = transl(robot.base); + text(b(1), b(2)-opt.mag, [' ' robot.name], 'FontAngle', 'italic', 'FontWeight', 'bold') + end + % create a line which we will + % subsequently modify. Set erase mode to xor for fast + % update + % + h.links = line(robot.lineopt{:}); + + if opt.shadow + h.shadow = line(robot.shadowopt{:}, ... + 'Erasemode', opt.erasemode); + end + + if opt.wrist + h.x = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'red'); + h.y = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'green'); + h.z = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'blue'); + h.xt = text(0, 0, opt.wristlabel(1), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + h.yt = text(0, 0, opt.wristlabel(2), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + h.zt = text(0, 0, opt.wristlabel(3), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + + end + + % + % display cylinders (revolute) or boxes (pristmatic) at + % each joint, as well as axis centerline. + % + L = robot.links; + for i=1:robot.n + + if opt.joints + + % cylinder or box to represent the joint + if L(i).sigma == 0 + N = 16; + else + N = 4; + end + % define the vertices of the cylinder + [xc,yc,zc] = cylinder(opt.mag/4, N); + zc(zc==0) = -opt.mag/2; + zc(zc==1) = opt.mag/2; + + % create vertex color data + cdata = zeros(size(xc)); + for j=1:3 + cdata(:,:,j) = opt.cylinder(j); + end + % render the surface + h.joint(i) = surface(xc,yc,zc,cdata); + + % set the surfaces to be smoothed and translucent + set(h.joint(i), 'FaceColor', 'interp'); + set(h.joint(i), 'EdgeColor', 'none'); + set(h.joint(i), 'FaceAlpha', 0.7); + + % build a matrix of coordinates so we + % can transform the cylinder in animate() + % and hang it off the cylinder + xyz = [xc(:)'; yc(:)'; zc(:)'; ones(1,2*N+2)]; + set(h.joint(i), 'UserData', xyz); + end + + if opt.jaxes + % add a dashed line along the axis + h.jointaxis(i) = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'blue', ... + 'linestyle', ':'); + h.jointlabel(i) = text(0, 0, 0, num2str(i), 'HorizontalAlignment', 'Center'); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/old/teach2.m b/lwrserv/matlab/rvctools/robot/@SerialLink/old/teach2.m new file mode 100644 index 0000000..0b4654c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/old/teach2.m @@ -0,0 +1,432 @@ +%SerialLink.teach Graphical teach pendant +% +% R.teach(OPTIONS) drive a graphical robot by means of a graphical slider panel. +% If no graphical robot exists one is created in a new window. Otherwise +% all current instances of the graphical robot are driven. +% +% h = R.teach(OPTIONS) as above but returns a handle for the teach window. Can +% be used to programmatically delete the teach window. +% +% Options:: +% 'eul' Display tool orientation in Euler angles +% 'rpy' Display tool orientation in roll/pitch/yaw angles +% 'approach' Display tool orientation as approach vector (z-axis) +% 'degrees' Display angles in degrees (default radians) +% 'q0',q Set initial joint coordinates +% 'callback',C Set a callback function +% +% GUI:: +% +% - The record button adds the current joint coordinates as a row to the robot's +% qteach property. +% - The Quit button destroys the teach window. +% +% Notes:: +% - The slider limits are derived from the joint limit properties. If not +% set then for +% - a revolute joint they are assumed to be [-pi, +pi] +% - a prismatic joint they are assumed unknown and an error occurs. +% +% See also SerialLink.plot. + +% 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.petercorke.com + +%% TODO: +%% make the sliders change the animation while moving +%% http://www.mathworks.com/matlabcentral/newsreader/view_thread/159374 +%% 1. download FINDJOBJ from the file exchange: http://www.mathworks.com/matlabcentral/fileexchange/14317 +%% 2. hSlider = uicontrol('style','slider', ...); %create the slider, get its Matlab handle +%% 3. jSlider = findjobj(hSlider,'nomenu'); %get handle of the underlying java object +%% 4. jSlider.AdjustmentValueChangedCallback = @myMatlabFunction; %set callback +%% +%% Note: you can also use the familiar format: +%% set(jSlider,'AdjustmentValueChangedCallback',@myMatlabFunction) +%% +%% Feel free to explore many other properties (and ~30 callbacks) +%% available in jSlider but not in its Matlab front-end interface hSlider. +%% +%% Warning: FINDJOBJ relies on undocumented and unsupported Matlab/Java +%% functionality. + +function handle = teach(r, varargin) + bgcol = [135 206 250]/255; + + opt.degrees = false; + opt.q0 = []; + opt.orientation = {'approach', 'eul', 'rpy'}; + opt.callback = []; +% TODO: options for rpy, or eul angle display + + opt = tb_optparse(opt, varargin); + + % drivebot(r, q) + % drivebot(r, 'deg') + + n = r.n; + width = 300; + height = 40; + + qlim = r.qlim; + if any(isinf(qlim)) + error('for prismatic axes must define joint coordinate limits, set qlim properties for prismatic Links'); + end + + if isempty(opt.q0) + q = zeros(1,n); + else + q = opt.q0; + end + + % set up scale factor, from actual limits in radians/metres to display units + qscale = ones(r.n,1); + for i=1:r.n + L=r.links(i); + if opt.degrees && L.isrevolute + qscale(i) = 180/pi; + end + end + + handles.qscale = qscale; + handles.orientation = opt.orientation; + handles.callback = opt.callback; + + T6 = r.fkine(q); + + fig = figure('Units', 'pixels', ... + 'Position', [0 -height width height*(n+2)+40], ... + 'BusyAction', 'cancel', ... + 'HandleVisibility', 'off', ... + 'Color', bgcol); + set(fig,'MenuBar','none') + delete( get(fig, 'Children') ) + + % first we check to see if there are any graphical robots of + % this name, if so we use them, otherwise create a robot plot. + + rhandles = findobj('Tag', r.name); + + % attempt to get current joint config of graphical robot + if ~isempty(rhandles) + h = get(rhandles(1), 'UserData'); + if ~isempty(h.q) + q = h.q; + end + end + + % now make the sliders + for i=1:n + % slider label + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n-i)+40 width*0.1 height*0.4], ... + 'String', sprintf('q%d', i)); + + % slider itself + q(i) = max( qlim(i,1), min( qlim(i,2), q(i) ) ); % clip to range + handles.slider(i) = uicontrol(fig, 'Style', 'slider', ... + 'Units', 'pixels', ... + 'Position', [width*0.1 height*(n-i)+40 width*0.7 height*0.4], ... + 'Min', qlim(i,1), ... + 'Max', qlim(i,2), ... + 'Value', q(i), ... + 'Tag', sprintf('Slider%d', i)); + + % text box showing slider value, also editable + handles.edit(i) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [width*0.8 height*(n-i-0.1)+40 width*0.2 height*0.7], ... + 'String', num2str(qscale(i)*q(i)), ... + 'Tag', sprintf('Edit%d', i)); + end + + set(handles.slider(1)) + + + % robot name text box + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'FontSize', 20, ... + 'HorizontalAlignment', 'left', ... + 'Position', [0 height*(n+1.2)+40 0.8*width 0.8*height], ... + 'BackgroundColor', 'white', ... + 'String', r.name); + + % X + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n+0.5)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'x:'); + + handles.t6.t(1) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.06*width height*(n+0.5)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(1,4)), ... + 'Tag', 'T6'); + + % Y + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.26*width height*(n+0.5)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'y:'); + + handles.t6.t(2) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.32*width height*(n+0.5)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(2,4))); + + % Z + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.52*width height*(n+0.5)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'z:'); + + handles.t6.t(3) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.58*width height*(n+0.5)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(3,4))); + + % Orientation + switch opt.orientation + case 'approach' + labels = {'ax:', 'ay:', 'az:'}; + case 'eul' + labels = {'\phi:', '\theta:', '\psi:'}; + case'rpy' + labels = {'R:', 'P:', 'Y:'}; + end + % AX + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', labels(1)); + + handles.t6.r(1) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.06*width height*(n)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(1,3))); + + % AY + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.26*width height*(n)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', labels(2)); + + handles.t6.r(2) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.32*width height*(n)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(2,3))); + + % AZ + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.52*width height*(n)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', labels(2)); + + handles.t6.r(3) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.58*width height*(n)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(3,3))); + + % add buttons + uicontrol(fig, 'Style', 'pushbutton', ... + 'Units', 'pixels', ... + 'FontSize', 16, ... + 'Position', [0.8*width height*n+40 0.2*width 2*height], ... + 'CallBack', @(src,event) delete(fig), ... + 'BackgroundColor', 'red', ... + 'String', 'Quit'); + % the record button + handles.record = []; + uicontrol(fig, 'Style', 'pushbutton', ... + 'Units', 'pixels', ... + 'FontSize', 15, ... + 'Position', [0.05*width 8 0.9*width 0.7*height], ... + 'CallBack', @(src,event) record_callback(r, handles), ... + 'BackgroundColor', 'blue', ... + 'ForegroundColor', 'white', ... + 'String', 'record'); + + + % now assign the callbacks + for i=1:n + % text edit box + set(handles.edit(i), ... + 'Interruptible', 'off', ... + 'Callback', @(src,event)teach_callback(src, r.name, i, handles)); + + % slider + set(handles.slider(i), ... + 'Interruptible', 'off', ... + 'BusyAction', 'queue', ... + 'Callback', @(src,event)teach_callback(src, r.name, i, handles)); + + % if findjobj exists use it, since it lets us get continous callbacks while + % a slider moves + if (exist('findjobj', 'file')>0) && verLessThan('matlab', '8') && ~ispc + disp('using findjobj'); + pause(0.1); + drawnow + jh = findjobj(handles.slider(i), 'nomenu'); + %jh.AdjustmentValueChangedCallback = {@sliderCallbackFunc, r.name, i}; + jh.AdjustmentValueChangedCallback = @(src,event)sliderCallbackFunc(src, handles.slider(i), r.name, i, handles); + end + end + + if isempty(rhandles) + figure + r.plot(q); + end + + if nargout > 0 + handle = fig; + end +end + +function teach_callback(src, name, j, handles) +% called on changes to a slider or to the edit box showing joint coordinate +% +% src the object that caused the event +% name name of the robot +% j the joint index concerned (1..N) +% slider true if the + + qscale = handles.qscale; + + switch get(src, 'Style') + case 'slider' + % slider changed, get value and reflect it to edit box + newval = get(src, 'Value'); + set(handles.edit(j), 'String', num2str(qscale(j)*newval)); + case 'edit' + % edit box changed, get value and reflect it to slider + newval = str2double(get(src, 'String')) / qscale(j); + set(handles.slider(j), 'Value', newval); + end + %fprintf('newval %d %f\n', j, newval); + + + % find all graphical objects tagged with the robot name, this is the + % instancs of that robot across all figures + rhandles = findobj('Tag', name); + + for rhandle=rhandles' + % for every graphical robot instance + + h = get(rhandle, 'UserData'); % get the robot object + robot = h.robot; + q = h.q; % get its current joint angles + if isempty(q) + q = zeros(1,robot.n); + end + q(j) = newval; % update the joint angle + + % update the robot object + %robot.q = q; + %set(r, 'UserData', robot); + + robot.plot(q); % plot it + drawnow + end + + % compute the robot tool pose + T6 = robot.fkine(q); + + % convert orientation to desired format + switch handles.orientation + case 'approach' + orient = T6(:,3); % approach vector + case 'eul' + orient = tr2eul(T6); + case'rpy' + orient = tr2rpy(T6); + end + + % update the display in the teach window + for i=1:3 + set(handles.t6.t(i), 'String', sprintf('%.3f', T6(i,4))); + set(handles.t6.r(i), 'String', sprintf('%.3f', orient(i))); + end + + if ~isempty(handles.callback) + handles.callback(q); + end + + robot.T = T6; + robot.notify('Moved'); + %drawnow +end + +function record_callback(robot, handles) + rhandles = findobj('Tag', robot.name); + + if ~isempty(rhandles) + h = get(rhandles(1), 'UserData'); % get the plot graphics + h.q + robot.record(h.q); + end +end + +function sliderCallbackFunc(src, h, name, joint, handles) + persistent busy + + % this is really ugly but it works + if busy + return + end + + if get(src,'ValueIsAdjusting') == 1 + busy = true; + try + teach_callback(h, name, joint, handles); + catch me + fprintf('*******\n') + busy = false; + return + end + end + busy = false; + + +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/pay.m b/lwrserv/matlab/rvctools/robot/@SerialLink/pay.m new file mode 100644 index 0000000..d577d68 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/pay.m @@ -0,0 +1,77 @@ +%SerialLink.PAY Joint forces due to payload +% +% TAU = R.PAY(W, J) returns the generalised joint force/torques due to a +% payload wrench W (1x6) and where the manipulator Jacobian is J (6xN), and +% N is the number of robot joints. +% +% TAU = R.PAY(Q, W, F) as above but the Jacobian is calculated at pose Q +% (1xN) in the frame given by F which is '0' for world frame, 'n' for +% end-effector frame. +% +% Uses the formula TAU = J'W, where W is a wrench vector applied at the end +% effector, W = [Fx Fy Fz Mx My Mz]'. +% +% Trajectory operation:: +% +% In the case Q is MxN or J is 6xNxM then TAU is MxN where each row is the +% generalised force/torque at the pose given by corresponding row of Q. +% +% Notes:: +% - Wrench vector and Jacobian must be from the same reference frame. +% - Tool transforms are taken into consideration when F = 'n'. +% - Must have a constant wrench - no trajectory support for this yet. +% +% Author:: +% Bryan Moutrie +% +% See also SerialLink.paycap, SerialLink.jacob0, SerialLink.jacobn. + +% Copyright (C) Bryan Moutrie, 2013-2015 +% Licensed under the GNU Lesser General Public License +% see full file for full statement +% +% LICENSE STATEMENT: +% +% This file is part of pHRIWARE. +% +% pHRIWARE 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. +% +% pHRIWARE 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 General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public +% License along with pHRIWARE. If not, see . + +function tauP = pay(robot, varargin) + + if length(varargin) == 2 + w = varargin{1}; + J = varargin{2}; + n = size(J,2); + elseif length(varargin) == 3 + q = varargin{1}; + w = varargin{2}; + f = varargin{3}; + n = robot.n; + J = zeros(6,n,size(q,1)); + if f == '0' + for i= 1: size(q,1) + J(:,:,i) = robot.jacob0(q(i,:)); + end + elseif f == 'n' + for i= 1: size(q,1) + J(:,:,i) = robot.jacobn(q(i,:)); + end + end + end + + if ~isequal(size(w),[6 1]), error(pHRIWARE('error', 'inputSize')); end + tauP = -reshape(J(:,:)'*w,n,[])'; + +end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/paycap.m b/lwrserv/matlab/rvctools/robot/@SerialLink/paycap.m new file mode 100644 index 0000000..6f12691 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/paycap.m @@ -0,0 +1,81 @@ +%SerialLink.PAYCAP Static payload capacity of a robot +% +% [WMAX,J] = R.paycap(Q, W, F, TLIM) returns the maximum permissible +% payload wrench WMAX (1x6) applied at the end-effector, and the index of +% the joint J which hits its force/torque limit at that wrench. Q (1xN) is +% the manipulator pose, W the payload wrench (1x6), F the wrench reference +% frame (either '0' or 'n') and TLIM (2xN) is a matrix of joint +% forces/torques (first row is maximum, second row minimum). +% +% Trajectory operation:: +% +% In the case Q is MxN then WMAX is Mx6 and J is Mx1 where the rows are the +% results at the pose given by corresponding row of Q. +% +% Notes:: +% - Wrench vector and Jacobian must be from the same reference frame +% - Tool transforms are taken into consideration for F = 'n'. +% +% Author:: +% Bryan Moutrie +% +% See also SerialLink.pay, SerialLink.gravjac, SerialLink.gravload. + +% Copyright (C) Bryan Moutrie, 2013-2015 +% Licensed under the GNU Lesser General Public License +% see full file for full statement +% +% LICENSE STATEMENT: +% +% This file is part of pHRIWARE. +% +% pHRIWARE 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. +% +% pHRIWARE 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 General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public +% License along with pHRIWARE. If not, see . + +function [wM, j] = paycap(robot, q, w, f, tauR) + + if robot.fast + tauB = robot.gravload(q); + tauP = robot.rne(q, zeros(size(q)), zeros(size(q)), [0; 0; 0], unit(w)); + elseif f == '0' + [tauB, J] = gravjac(robot, q); + tauP = robot.pay(unit(w), J); + elseif f == 'n' + tauB = gravjac(robot, q); + tauP = robot.pay(unit(w), q, 'n'); + end + + M = tauP > 0; + m = ~M; + + TAUm = ones(size(tauB)); + TAUM = ones(size(tauB)); + for c = 1:robot.n + TAUM(:,c) = tauR(1,c); + TAUm(:,c) = tauR(2,c); + end + + WM = zeros(size(tauB)); + WM(M) = (TAUM(M) - tauB(M)) ./ tauP(M); + WM(m) = (TAUm(m) - tauB(m)) ./ tauP(m); + + WM(isinf(WM)) = Inf; % Makes -Inf values Inf + + [wM, j] = min(WM,[],2); + +end + +function u = unit(v) + u = v/norm(v); +end + diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/perturb.m b/lwrserv/matlab/rvctools/robot/@SerialLink/perturb.m new file mode 100644 index 0000000..c8c2e66 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/perturb.m @@ -0,0 +1,48 @@ +%SerialLink.perturb Perturb robot parameters +% +% RP = R.perturb(P) is a new robot object in which the dynamic parameters (link +% mass and inertia) have been perturbed. The perturbation is multiplicative so +% that values are multiplied by random numbers in the interval (1-P) to (1+P). +% The name string of the perturbed robot is prefixed by 'P/'. +% +% Useful for investigating the robustness of various model-based control +% schemes. For example to vary parameters in the range +/- 10 percent is: +% r2 = p560.perturb(0.1); + +% 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.petercorke.com + +function r2 = perturb(r, p) + + if nargin == 1 + p = 0.1; % 10 percent disturb by default + end + + r2 = SerialLink(r); + + links = r2.links; + for i=1:r.n + s = (2*rand-1)*p + 1; + links(i).m = links(i).m * s; + + s = (2*rand-1)*p + 1; + links(i).I = links(i).I * s; + end + + r2.name = ['P/' r.name]; diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/plot.m b/lwrserv/matlab/rvctools/robot/@SerialLink/plot.m new file mode 100644 index 0000000..a9ae660 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/plot.m @@ -0,0 +1,509 @@ +%SerialLink.plot Graphical display and animation +% +% R.plot(Q, options) displays a graphical animation of a robot based on +% the kinematic model. A stick figure polyline joins the origins of +% the link coordinate frames. The robot is displayed at the joint angle Q (1xN), or +% if a matrix (MxN) it is animated as the robot moves along the M-point trajectory. +% +% Options:: +% 'workspace', W size of robot 3D workspace, W = [xmn, xmx ymn ymx zmn zmx] +% 'delay', d delay betwen frames for animation (s) +% 'fps',fps set number of frames per second for display +% '[no]loop' loop over the trajectory forever +% 'mag', scale annotation scale factor +% 'cylinder', C color for joint cylinders, C=[r g b] +% 'ortho' orthogonal camera view (default) +% 'perspective' perspective camera view +% 'xyz' wrist axis label is XYZ +% 'noa' wrist axis label is NOA +% '[no]raise' autoraise the figure (very slow). +% '[no]render' controls shaded rendering after drawing +% '[no]base' controls display of base 'pedestal' +% '[no]wrist' controls display of wrist +% '[no]shadow' controls display of shadow +% '[no]name' display the robot's name +% '[no]jaxes' control display of joint axes +% '[no]joints' controls display of joints +% 'movie',M save frames as files in the folder M +% +% +% The options come from 3 sources and are processed in order: +% - Cell array of options returned by the function PLOTBOTOPT (if it exists) +% - Cell array of options given by the 'plotopt' option when creating the +% SerialLink object. +% - List of arguments in the command line. +% +% Many boolean options can be enabled or disabled with the 'no' prefix. The +% various option sources can toggle an option, the last value is taken. +% +% To save the effort of processing options on each call they can be preprocessed by +% opts = robot.plot({'opt1', 'opt2', ... }); +% and the resulting object can be passed in to subsequent calls instead of text-based +% options, for example: +% robot.plot(q, opts); +% +% Graphical annotations and options:: +% +% The robot is displayed as a basic stick figure robot with annotations +% such as: +% - shadow on the floor +% - XYZ wrist axes and labels +% - joint cylinders and axes +% which are controlled by options. +% +% The size of the annotations is determined using a simple heuristic from +% the workspace dimensions. This dimension can be changed by setting the +% multiplicative scale factor using the 'mag' option. +% +% Figure behaviour:: +% +% - If no figure exists one will be created and teh robot drawn in it. +% - If no robot of this name is currently displayed then a robot will +% be drawn in the current figure. If hold is enabled (hold on) then the +% robot will be added to the current figure. +% - If the robot already exists then that graphical model will be found +% and moved. +% +% Multiple views of the same robot:: +% +% If one or more plots of this robot already exist then these will all +% be moved according to the argument Q. All robots in all windows with +% the same name will be moved. +% +% Create a robot in figure 1 +% figure(1) +% p560.plot(qz); +% Create a robot in figure 2 +% figure(2) +% p560.plot(qz); +% Now move both robots +% p560.plot(qn) +% +% Multiple robots in the same figure:: +% +% Multiple robots can be displayed in the same plot, by using "hold on" +% before calls to robot.plot(). +% +% Create a robot in figure 1 +% figure(1) +% p560.plot(qz); +% Make a clone of the robot named bob +% bob = SerialLink(p560, 'name', 'bob'); +% Draw bob in this figure +% hold on +% bob.plot(qn) +% +% To animate both robots so they move together: +% qtg = jtraj(qr, qz, 100); +% for q=qtg' +% p560.plot(q'); +% bob.plot(q'); +% end +% +% Making an animation movie:: +% - The 'movie' options saves frames as files NNNN.png. +% - When using 'movie' option ensure that the window is fully visible. +% - To convert frames to a movie use a command like: +% ffmpeg -r 10 -i %04d.png out.avi +% +% Notes:: +% - Delay betwen frames can be eliminated by setting option 'delay', 0 or +% 'fps', Inf. +% - By default a quite detailed plot is generated, but turning off labels, +% axes, shadows etc. will speed things up. +% - Each graphical robot object is tagged by the robot's name and has UserData +% that holds graphical handles and the handle of the robot object. +% - The graphical state holds the last joint configuration which can be retrieved +% using q = robot.plot(). +% +% See also plotbotopt, SerialLink.animate, SerialLink.fkine. + + +% HANDLES: +% +% A robot comprises a bunch of individual graphical elements and these are +% kept in a structure: +% +% h.link the robot stick figure +% h.shadow the robot's shadow +% h.x wrist vectors +% h.y +% h.z +% h.xt wrist vector labels +% h.yt +% h.zt +% +% h.q the last set of joint coordinates +% h.robot pointer to the robot object +% h.opt the final options structure +% +% The h.link graphical element is tagged with the robot's name and has this +% struct as its UserData. +% +% h.links -> h -> robot +% +% This enables us to find all robots with a given name, in all figures, +% and update them. + + +% 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.petercorke.com + +function retval = plot(robot, tg, varargin) + + % opts = PLOT(robot, options) + % just convert options list to an options struct + if (nargin == 2) && iscell(tg) + retval = plot_options(robot, tg); + return; + end + + % + % q = PLOT(robot) + % return joint coordinates from a graphical robot of given name + % + %TODO should be robot.get_q() + if nargin == 1 + handles = findobj('Tag', robot.name); + if ~isempty(handles) + h = get(handles(1), 'UserData'); + retval = h.q; + end + return + end + + % process options + if (nargin > 2) && isstruct(varargin{1}) + % options is a struct + opt = varargin{1}; + else + % options is a list of options + opt = plot_options(robot, varargin); + end + + % + % robot2 = ROBOT(robot, q, varargin) + % + np = numrows(tg); + n = robot.n; + + if numcols(tg) ~= n + error('Insufficient columns in q') + end + +% if ~ishandle(robot.handle), +% %disp('has handles') +% % handles provided, animate just that robot +% count = opt.repeat; +% while count > 0 +% for p=1:np, +% animate( robot, tg(p,:)); +% pause(opt.delay) +% end +% count = count - 1; +% end +% return; +% end + + % get handle of any existing graphical robot of same name + handles = findobj('Tag', robot.name); + + if isempty(handles) || isempty( get(gcf, 'Children')) + % no robot of this name exists + + % create one + newplot(); + handle = create_new_robot(robot, opt); + + % tag one of the graphical handles with the robot name and hang + % the handle structure off it + set(handle.links, 'Tag', robot.name); + set(handle.links, 'UserData', handle); + + handles = handle.links; + end + + if ishold && isempty( findobj(gca, 'Tag', robot.name)) + % if hold is on and no robot of this name in current axes + h = create_new_robot(robot, opt); + + % tag one of the graphical handles with the robot name and hang + % the handle structure off it + set(handle.links, 'Tag', robot.name); + set(handle.links, 'UserData', handle); + + handles = handle.links; + end + + if opt.raise + % note this is a very time consuming operation + figure(gcf); + end + + if ~isempty(opt.movie) + mkdir(opt.movie); + framenum = 1; + end + + while true + for p=1:np % for each point on path + robot.animate(tg(p,:), handles); + %drawnow + + if ~isempty(opt.movie) + % write the frame to the movie folder + print( '-dpng', fullfile(opt.movie, sprintf('%04d.png', framenum)) ); + framenum = framenum+1; + end + + if opt.delay > 0 + pause(opt.delay); + drawnow + end + end + if ~opt.loop + break; + end + end + + % save the last joint angles away in the graphical robot + for handle=handles' + h = get(handle, 'UserData'); + h.q = tg(end,:); + set(handle, 'UserData', h); + end +end + +%PLOT_OPTIONS +% +% o = PLOT_OPTIONS(robot, options) +% +% Returns an options structure + +function o = plot_options(robot, optin) + % process a cell array of options and return a struct + + % define all possible options and their default values + o.erasemode = 'normal'; + o.joints = true; + o.wrist = true; + o.loop = false; + o.shadow = true; + o.wrist = true; + o.jaxes = true; + o.base = true; + o.wristlabel = 'xyz'; + o.perspective = true; + o.magscale = 1; + o.name = true; + o.delay = 0.1; + o.fps = []; + o.raise = true; + o.cylinder = [0 0 0.7]; + o.workspace = []; + o.movie = []; + o.render = true; + o.ortho = true; + o.perspective = false; + + + % build a list of options from all sources + % 1. the M-file plotbotopt if it exists + % 2. robot.plotopt + % 3. command line arguments + if exist('plotbotopt', 'file') == 2 + options = [plotbotopt robot.plotopt optin]; + else + options = [robot.plotopt optin]; + end + + % parse the options + [o,args] = tb_optparse(o, options); + if ~isempty(args) + error(['unknown option: ' args{1}]); + end + + if isempty(o.workspace) + % + % simple heuristic to figure the maximum reach of the robot + % + L = robot.links; + reach = 0; + for i=1:robot.n + reach = reach + abs(L(i).a) + abs(L(i).d); + end + o.workspace = [-reach reach -reach reach -reach reach]; + o.mag = reach/10; + else + reach = min(abs(o.workspace)); + end + o.mag = o.magscale * reach/10; + + if ~isempty(o.fps) + o.delay = 1/o.fps; + end + +end + +%CREATE_NEW_ROBOT +% +% h = CREATE_NEW_ROBOT(robot, opt) +% +% Using data from robot object and options create a graphical robot in +% the current figure. +% +% Returns a structure of handles to graphical objects. +% +% If current figure is empty, draw robot in it +% If current figure has hold on, add robot to it +% Otherwise, create new figure and draw robot in it. +% + +% h.mag +% h.zmin +% h.links the line segment that is the robot +% h.shadow the robot's shadow +% h.x the line segment that is T6 x-axis +% h.y the line segment that is T6 x-axis +% h.z the line segment that is T6 x-axis +% h.xt text for T6 x-axis +% h.yt text for T6 y-axis +% h.zt text for T6 z-axis +% h.joint(i) the joint i cylinder or box +% h.jointaxis(i) the line segment that is joint i axis +% h.jointlabel(i) text for joint i label + +function h = create_new_robot(robot, opt) + h.opt = opt; % the options + h.robot = robot; % pointer to robot + + h.mag = opt.mag; + % + % setup an axis in which to animate the robot + % + % handles not provided, create graphics + %disp('in creat_new_robot') + if ~ishold + % if current figure has hold on, then draw robot here + % otherwise, create a new figure + axis(opt.workspace); + end + xlabel('X') + ylabel('Y') + zlabel('Z') + %set(gca, 'drawmode', 'fast'); + grid on + + zlim = get(gca, 'ZLim'); + h.zmin = zlim(1); + + if opt.base + b = transl(robot.base); + line('xdata', [b(1);b(1)], ... + 'ydata', [b(2);b(2)], ... + 'zdata', [h.zmin;b(3)], ... + 'LineWidth', 4, ... + 'color', 'red'); + end + + if opt.name + b = transl(robot.base); + text(b(1), b(2)-opt.mag, [' ' robot.name], 'FontAngle', 'italic', 'FontWeight', 'bold') + end + % create a line which we will + % subsequently modify. Set erase mode to xor for fast + % update + % + h.links = line(robot.lineopt{:}); + + if opt.shadow + h.shadow = line(robot.shadowopt{:}, ... + 'Erasemode', opt.erasemode); + end + + if opt.wrist, + h.x = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'red'); + h.y = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'green'); + h.z = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'blue'); + h.xt = text(0, 0, opt.wristlabel(1), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + h.yt = text(0, 0, opt.wristlabel(2), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + h.zt = text(0, 0, opt.wristlabel(3), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + + end + + % + % display cylinders (revolute) or boxes (pristmatic) at + % each joint, as well as axis centerline. + % + L = robot.links; + for i=1:robot.n + + if opt.joints + + % cylinder or box to represent the joint + if L(i).sigma == 0 + N = 16; + else + N = 4; + end + % define the vertices of the cylinder + [xc,yc,zc] = cylinder(opt.mag/4, N); + zc(zc==0) = -opt.mag/2; + zc(zc==1) = opt.mag/2; + + % create vertex color data + cdata = zeros(size(xc)); + for j=1:3 + cdata(:,:,j) = opt.cylinder(j); + end + % render the surface + h.joint(i) = surface(xc,yc,zc,cdata); + + % set the surfaces to be smoothed and translucent + set(h.joint(i), 'FaceColor', 'interp'); + set(h.joint(i), 'EdgeColor', 'none'); + set(h.joint(i), 'FaceAlpha', 0.7); + + % build a matrix of coordinates so we + % can transform the cylinder in animate() + % and hang it off the cylinder + xyz = [xc(:)'; yc(:)'; zc(:)'; ones(1,2*N+2)]; + set(h.joint(i), 'UserData', xyz); + end + + if opt.jaxes + % add a dashed line along the axis + h.jointaxis(i) = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'blue', ... + 'linestyle', ':'); + h.jointlabel(i) = text(0, 0, 0, num2str(i), 'HorizontalAlignment', 'Center'); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/plot3d.m b/lwrserv/matlab/rvctools/robot/@SerialLink/plot3d.m new file mode 100644 index 0000000..c7f32eb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/plot3d.m @@ -0,0 +1,499 @@ +%SerialLink.plot3d Graphical display and animation of solid model robot +% +% R.plot3d(Q, options) displays and animates a solid model of the robot. +% The robot is displayed at the joint angle Q (1xN), or +% if a matrix (MxN) it is animated as the robot moves along the M-point trajectory. +% +% Options:: +% +% 'color',C A cell array of color names, one per link. These are +% mapped to RGB using colorname(). If not given, colors +% come from the axis ColorOrder property. +% 'alpha',A Set alpha for all links, 0 is transparant, 1 is opaque +% (default 1) +% 'path',P Overide path to folder containing STL model files +% 'workspace', W Size of robot 3D workspace, W = [xmn, xmx ymn ymx zmn zmx] +% 'floorlevel',L Z-coordinate of floor (default -1) +%- +% 'delay',D Delay betwen frames for animation (s) +% 'fps',fps Number of frames per second for display, inverse of 'delay' option +% '[no]loop' Loop over the trajectory forever +% '[no]raise' Autoraise the figure +% 'movie',M Save frames as files in the folder M +%- +% 'scale',S Annotation scale factor +% 'ortho' Orthographic view (default) +% 'perspective' Perspective view +% 'view',V Specify view V='x', 'y', 'top' or [az el] for side elevations, +% plan view, or general view by azimuth and elevation +% angle. +%- +% '[no]wrist' Enable display of wrist coordinate frame +% 'xyz' Wrist axis label is XYZ +% 'noa' Wrist axis label is NOA +% '[no]arrow' Display wrist frame with 3D arrows +%- +% '[no]tiles' Enable tiled floor (default true) +% 'tilesize',S Side length of square tiles on the floor (default 0.2) +% 'tile1color',C Color of even tiles [r g b] (default [0.5 1 0.5] light green) +% 'tile2color',C Color of odd tiles [r g b] (default [1 1 1] white) +%- +% '[no]jaxes' Enable display of joint axes (default true) +% '[no]joints' Enable display of joints +%- +% '[no]base' Enable display of base shape +% +% Notes:: +% - Solid models of the robot links are required as STL ascii format files, +% with extensions .stl +% - Suitable STL files can be found in the package ARTE: A ROBOTICS TOOLBOX +% FOR EDUCATION by Arturo Gil, https://arvc.umh.es/arte +% - The root of the solid models is an installation of ARTE with an empty +% file called arte.m at the top level +% - Each STL model is called 'linkN'.stl where N is the link number 0 to N +% - The specific folder to use comes from the SerialLink.model3d property +% - The path of the folder containing the STL files can be specified using +% the 'path' option +% - The height of the floor is set in decreasing priority order by: +% - 'workspace' option, the fifth element of the passed vector +% - 'floorlevel' option +% - the lowest z-coordinate in the link1.stl object +% +% Authors:: +% - Peter Corke, based on existing code for plot() +% - Bryan Moutrie, demo code on the Google Group for connecting ARTE and RTB +% - Don Riley, function rndread() extracted from cad2matdemo (MATLAB +% File Exchange) +% +% See also SerialLink.plot, plotbotopt3d, SerialLink.animate, SerialLink.teach, SerialLink.fkine. + +% Copyright (C) 1993-2015, 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.petercorke.com + + +function plot3d(robot, q, varargin) + + + if robot.mdh + error('RTB:plot3d:badmodel', '3D models are defined for standard, not modified, DH parameters'); + end + + clf + opt = plot_options(robot, varargin); + + %-- load the shape if need be + + nshapes = robot.n+1; + + if isempty(robot.faces) + % no 3d model defined, let's try to load one + + if isempty(opt.path) + % first find the path to the models + pth = which('arte.m'); + if ~pth + error('RTB:plot3d:nomodel', 'no 3D model found, install the RTB contrib zip file'); + end + + % find the path to this specific model + pth = fullfile(fileparts(pth), 'robots', robot.model3d); + else + pth = opt.path; + end + + % now load the STL files + robot.points = cell(1, robot.n+1); + robot.faces = cell(1, robot.n+1); + for i=1:nshapes + [F,P] = rndread( fullfile(pth, sprintf('link%d.stl', i-1)) ); + robot.points{i} = P; + robot.faces{i} = F; + end + end + + % if a base is specified set the floor height to this + if isempty(opt.workspace) + % workspace not provided, fall through the options for setting floor level + if ~isempty(opt.floorlevel) + opt.ws(5) = opt.floorlevel; + elseif opt.base + mn = min( robot.points{1} ); + opt.ws(5) = mn(3); + end + end + opt.floorlevel = opt.ws(5); + +% TODO +% should test if the plot exists, pinch the logic from plot() + + %-- set up to plot + % create an axis + ish = ishold(); + if ~ishold + % if hold is off, set the axis dimensions + axis(opt.ws); + set(gca, 'ZLimMode', 'manual'); + axis(opt.ws); + hold on + end + + + if opt.raise + % note this is a very time consuming operation + figure(gcf); + end + + if strcmp(opt.projection, 'perspective') + set(gca, 'Projection', 'perspective'); + end + + grid on + xlabel('X'); ylabel('Y'); zlabel('Z'); + + %--- create floor if required + if opt.tiles + create_tiled_floor(opt); + end + + %--- configure view and lighting + if isstr(opt.view) + switch opt.view + case 'top' + view(0, 90); + case 'x' + view(0, 0); + case 'y' + view(90, 0) + otherwise + error('rtb:plot3d:badarg', 'view must be: x, y, top') + end + elseif isnumeric(opt.view) && length(opt.view) == 2 + view(opt.view) + else + campos([2 2 1]); + end + + daspect([1 1 1]); + light('Position', [0 0 opt.reach*2]); + light('Position', [1 0.5 1]); + + + %-- figure the colors for each shape + if isempty(opt.color) + % if not given, use the axis color order + C = get(gca,'ColorOrder'); + else + C = []; + for c=opt.color + C = [C; colorname(c{1})]; + end + end + + + %--- create the robot + % one patch per shape, use hgtransform to animate them later + group = hggroup('Tag', robot.name); + ncolors = numrows(C); + + h = []; + for link=0:robot.n + if link == 0 + if ~opt.base + continue; + end + + patch('Faces', robot.faces{link+1}, 'Vertices', robot.points{link+1}, ... + 'FaceColor', C(mod(link,ncolors)+1,:), 'EdgeAlpha', 0, 'FaceAlpha', opt.alpha); + else + h.link(link) = hgtransform('Tag', sprintf('link%d', link), 'Parent', group); + + patch('Faces', robot.faces{link+1}, 'Vertices', robot.points{link+1}, ... + 'FaceColor', C(mod(link,ncolors)+1,:), 'EdgeAlpha', 0, 'FaceAlpha', opt.alpha, ... + 'Parent', h.link(link)); + end + end + + % enable mouse-based 3D rotation + rotate3d on + + h.wrist = []; % HACK, should be trplot + h.robot = robot; + h.link = [0 h.link]; + set(group, 'UserData', h); + + robot.animate(q); + + if ~ish + hold off + end +end + +function opt = plot_options(robot, optin) + opt.color = []; + opt.path = []; % override path + opt.alpha = 1; + + % timing/looping + opt.delay = 0.1; + opt.fps = []; + opt.loop = false; + + opt.raise = false; + + % general appearance + opt.scale = 1; + + opt.workspace = []; + opt.floorlevel = []; + + opt.name = true; + opt.projection = {'ortho', 'perspective'}; + opt.view = []; + + + % tiled floor + opt.tiles = true; + opt.tile1color = [0.5 1 0.5]; % light green + opt.tile2color = [1 1 1]; % white + opt.tilesize = 0.2; + + + % the base or pedestal + opt.base = true; + + % wrist + opt.wrist = true; + opt.wristlabel = {'xyz', 'noa'}; + opt.arrow = true; + + % joint rotation axes + opt.jaxes = false; + + + % misc + opt.movie = []; + + % build a list of options from all sources + % 1. the M-file plotbotopt if it exists + % 2. robot.plotopt + % 3. command line arguments + if exist('plotbotopt3d', 'file') == 2 + options = [plotbotopt3d robot.plotopt3d optin]; + else + options = [robot.plotopt3d optin]; + end + + % parse the options + [opt,args] = tb_optparse(opt, options); + if ~isempty(args) + error(['unknown option: ' args{1}]); + end + + if ~isempty(opt.fps) + opt.delay = 1/opt.fps; + end + % figure the size of the figure + + if isempty(opt.workspace) + % + % simple heuristic to figure the maximum reach of the robot + % + L = robot.links; + if any(L.isprismatic) + error('Prismatic joint(s) present: requires the ''workspace'' option'); + end + reach = 0; + for i=1:robot.n + reach = reach + abs(L(i).a) + abs(L(i).d); + end + + % if we have a floor, quantize the reach to a tile size + if opt.tiles + reach = opt.tilesize * ceil(reach/opt.tilesize); + end + + % now create a 3D volume based on this reach + opt.ws = [-reach reach -reach reach -reach reach]; + + % if a floorlevel has been given, ammend the 3D volume + if ~isempty(opt.floorlevel) + opt.ws(5) = opt.floorlevel; + end + else + % workspace is provided + reach = min(abs(opt.workspace)); + if opt.tiles + % set xy limits to be integer multiple of tilesize + opt.ws(1:4) = opt.tilesize * round(opt.workspace(1:4)/opt.tilesize); + opt.ws(5:6) = opt.workspace(5:6); + end + end + + opt.reach = reach; + + % update the fundamental scale factor (given by the user as a multiplier) by a length derived from + % the overall workspace dimension + % we need that a lot when creating the robot model + opt.scale = opt.scale * reach/40; + + % deal with a few options that need to be stashed in the SerialLink object + % movie mode has not already been flagged + if opt.movie + robot.framenum = 0; + robot.moviepath = opt.movie; + else + robot.framenum = []; + end + + if ~isempty(opt.movie) + mkdir(opt.movie); + framenum = 1; + end + + robot.delay = opt.delay; + robot.loop = opt.loop; +end + +% Extracted from cad2matdemo +% http://www.mathworks.com/matlabcentral/fileexchange/3642-cad2matdemo-m/content/cad2matR2.zip +% +% Copyright (c) 2003, Don Riley +% All rights reserved. +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are +% met: +% +% * Redistributions of source code must retain the above copyright +% notice, this list of conditions and the following disclaimer. +% * Redistributions in binary form must reproduce the above copyright +% notice, this list of conditions and the following disclaimer in +% the documentation and/or other materials provided with the distribution +% * Neither the name of the Walla Walla University nor the names +% of its contributors may be used to endorse or promote products derived +% from this software without specific prior written permission. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE. + +function [fout, vout, cout] = rndread(filename) + + % Reads CAD STL ASCII files, which most CAD programs can export. + % Used to create Matlab patches of CAD 3D data. + % Returns a vertex list and face list, for Matlab patch command. + % + % filename = 'hook.stl'; % Example file. + % + fid=fopen(filename, 'r'); %Open the file, assumes STL ASCII format. + if fid == -1 + error('File could not be opened, check name or path.') + end + % + % Render files take the form: + % + %solid BLOCK + % color 1.000 1.000 1.000 + % facet + % normal 0.000000e+00 0.000000e+00 -1.000000e+00 + % normal 0.000000e+00 0.000000e+00 -1.000000e+00 + % normal 0.000000e+00 0.000000e+00 -1.000000e+00 + % outer loop + % vertex 5.000000e-01 -5.000000e-01 -5.000000e-01 + % vertex -5.000000e-01 -5.000000e-01 -5.000000e-01 + % vertex -5.000000e-01 5.000000e-01 -5.000000e-01 + % endloop + % endfacet + % + % The first line is object name, then comes multiple facet and vertex lines. + % A color specifier is next, followed by those faces of that color, until + % next color line. + % + CAD_object_name = sscanf(fgetl(fid), '%*s %s'); %CAD object name, if needed. + % %Some STLs have it, some don't. + vnum=0; %Vertex number counter. + report_num=0; %Report the status as we go. + VColor = 0; + % + while feof(fid) == 0 % test for end of file, if not then do stuff + tline = fgetl(fid); % reads a line of data from file. + fword = sscanf(tline, '%s '); % make the line a character string + % Check for color + if strncmpi(fword, 'c',1) == 1; % Checking if a "C"olor line, as "C" is 1st char. + VColor = sscanf(tline, '%*s %f %f %f'); % & if a C, get the RGB color data of the face. + end % Keep this color, until the next color is used. + if strncmpi(fword, 'v',1) == 1; % Checking if a "V"ertex line, as "V" is 1st char. + vnum = vnum + 1; % If a V we count the # of V's + report_num = report_num + 1; % Report a counter, so long files show status + if report_num > 249; + %disp(sprintf('Reading vertix num: %d.',vnum)); + report_num = 0; + end + v(:,vnum) = sscanf(tline, '%*s %f %f %f'); % & if a V, get the XYZ data of it. + c(:,vnum) = VColor; % A color for each vertex, which will color the faces. + end % we "*s" skip the name "color" and get the data. + end + % Build face list; The vertices are in order, so just number them. + % + fnum = vnum/3; %Number of faces, vnum is number of vertices. STL is triangles. + flist = 1:vnum; %Face list of vertices, all in order. + F = reshape(flist, 3,fnum); %Make a "3 by fnum" matrix of face list data. + % + % Return the faces and vertexs. + % + fout = F'; %Orients the array for direct use in patch. + vout = v'; % " + cout = c'; + % + fclose(fid); +end + +% draw a tiled floor in the current axes +function create_tiled_floor(opt) + + xmin = opt.ws(1); + xmax = opt.ws(2); + ymin = opt.ws(3); + ymax = opt.ws(4); + + % create a colored tiled floor + xt = xmin:opt.tilesize:xmax; + yt = ymin:opt.tilesize:ymax; + Z = opt.floorlevel*ones( numel(yt), numel(xt)); + C = zeros(size(Z)); + [r,c] = ind2sub(size(C), 1:numel(C)); + C = bitand(r+c,1); + C = reshape(C, size(Z)); + C = cat(3, opt.tile1color(1)*C+opt.tile2color(1)*(1-C), ... + opt.tile1color(2)*C+opt.tile2color(2)*(1-C), ... + opt.tile1color(3)*C+opt.tile2color(3)*(1-C)); + [X,Y] = meshgrid(xt, yt); + surface(X, Y, Z, C, ... + 'FaceColor','texturemap',... + 'EdgeColor','none',... + 'CDataMapping','direct'); +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/private/findjobj.m b/lwrserv/matlab/rvctools/robot/@SerialLink/private/findjobj.m new file mode 100644 index 0000000..bcd5286 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/private/findjobj.m @@ -0,0 +1,2547 @@ +function [handles,levels,parentIdx,listing] = findjobj(container,varargin) +%findjobj Find java objects contained within a specified java container or Matlab GUI handle +% +% Syntax: +% [handles, levels, parentIds, listing] = findjobj(container, 'PropName',PropValue(s), ...) +% +% Input parameters: +% container - optional handle to java container uipanel or figure. If unsupplied then current figure will be used +% 'PropName',PropValue - optional list of property pairs (case insensitive). PropName may also be named -PropName +% 'position' - filter results based on those elements that contain the specified X,Y position or a java element +% Note: specify a Matlab position (X,Y = pixels from bottom left corner), not a java one +% 'size' - filter results based on those elements that have the specified W,H (in pixels) +% 'class' - filter results based on those elements that contain the substring (or java class) PropValue +% Note1: filtering is case insensitive and relies on regexp, so you can pass wildcards etc. +% Note2: '-class' is an undocumented findobj PropName, but only works on Matlab (not java) classes +% 'property' - filter results based on those elements that possess the specified case-insensitive property string +% Note1: passing a property value is possible if the argument following 'property' is a cell in the +% format of {'propName','propValue'}. Example: FINDJOBJ(...,'property',{'Text','click me'}) +% Note2: partial property names (e.g. 'Tex') are accepted, as long as they're not ambiguous +% 'depth' - filter results based on specified depth. 0=top-level, Inf=all levels (default=Inf) +% 'flat' - same as specifying: 'depth',0 +% 'not' - negates the following filter: 'not','class','c' returns all elements EXCEPT those with class 'c' +% 'persist' - persist figure components information, allowing much faster results for subsequent invocations +% 'print' - display all java elements in a hierarchical list, indented appropriately +% Note1: optional PropValue of element index or handle to java container +% Note2: normally this option would be placed last, after all filtering is complete. Placing this +% option before some filters enables debug print-outs of interim filtering results. +% Note3: output is to the Matlab command window unless the 'listing' (4th) output arg is requested +% 'list' - same as 'print' +% 'nomenu' - skip menu processing, for "lean" list of handles & much faster processing; +% This option is the default for HG containers but not for figure, Java or no container +% 'debug' - list found component positions in the Command Window +% +% Output parameters: +% handles - list of handles to java elements +% levels - list of corresponding hierarchy level of the java elements (top=0) +% parentIds - list of indexes (in unfiltered handles) of the parent container of the corresponding java element +% listing - results of 'print'/'list' options (empty if these options were not specified) +% +% Note: If no output parameter is specified, then an interactive window will be displayed with a +% ^^^^ tree view of all container components, their properties and callbacks. +% +% Examples: +% findjobj; % display list of all javaelements of currrent figure in an interactive GUI +% handles = findjobj; % get list of all java elements of current figure (inc. menus, toolbars etc.) +% findjobj('print'); % list all java elements in current figure +% findjobj('print',6); % list all java elements in current figure, contained within its 6th element +% handles = findjobj(hButton); % hButton is a matlab button +% handles = findjobj(gcf,'position',getpixelposition(hButton,1)); % same as above but also return hButton's panel +% handles = findjobj(hButton,'persist'); % same as above, persist info for future reuse +% handles = findjobj('class','pushbutton'); % get all pushbuttons in current figure +% handles = findjobj('class','pushbutton','position',123,456); % get all pushbuttons at the specified position +% handles = findjobj(gcf,'class','pushbutton','size',23,15); % get all pushbuttons with the specified size +% handles = findjobj('property','Text','not','class','button'); % get all non-button elements with 'text' property +% handles = findjobj('-property',{'Text','click me'}); % get all elements with 'text' property = 'click me' +% +% Sample usage: +% hButton = uicontrol('string','click me'); +% jButton = findjobj(hButton,'nomenu'); +% % or: jButton = findjobj('property',{'Text','click me'}); +% jButton.setFlyOverAppearance(1); +% jButton.setCursor(java.awt.Cursor.getPredefinedCursor(java.awt.Cursor.HAND_CURSOR)); +% set(jButton,'FocusGainedCallback',@myMatlabFunction); % some 30 callback points available... +% jButton.get; % list all changeable properties... +% +% hEditbox = uicontrol('style',edit'); +% jEditbox = findjobj(hEditbox,'nomenu'); +% jEditbox.setCaretColor(java.awt.Color.red); +% jEditbox.KeyTypedCallback = @myCallbackFunc; % many more callbacks where this came from... +% jEdit.requestFocus; +% +% Known issues/limitations: +% - Cannot currently process multiple container objects - just one at a time +% - Initial processing is a bit slow when the figure is laden with many UI components (so better use 'persist') +% - Passing a simple container Matlab handle is currently filtered by its position+size: should find a better way to do this +% - Matlab uipanels are not implemented as simple java panels, and so they can't be found using this utility +% - Labels have a write-only text property in java, so they can't be found using the 'property',{'Text','string'} notation +% +% Warning: +% This code heavily relies on undocumented and unsupported Matlab functionality. +% It works on Matlab 7+, but use at your own risk! +% +% Bugs and suggestions: +% Please send to Yair Altman (altmany at gmail dot com) +% +% Change log: +% 2009-05-24: Added support for future Matlab versions that will not support JavaFrame +% 2009-05-15: Added sanity checks for axes items +% 2009-04-28: Added 'debug' input arg; increased size tolerance 1px => 2px +% 2009-04-23: Fixed location of popupmenus (always 20px high despite what's reported by Matlab...); fixed uiinspect processing issues; added blog link; narrower action buttons +% 2009-04-09: Automatic 'nomenu' for uicontrol inputs; significant performance improvement +% 2009-03-31: Fixed position of some Java components; fixed properties tooltip; fixed node visibility indication +% 2009-02-26: Indicated components visibility (& auto-collapse non-visible containers); auto-highlight selected component; fixes in node icons, figure title & tree refresh; improved error handling; display FindJObj version update description if available +% 2009-02-24: Fixed update check; added dedicated labels icon +% 2009-02-18: Fixed compatibility with old Matlab versions +% 2009-02-08: Callbacks table fixes; use uiinspect if available; fix update check according to new FEX website +% 2008-12-17: R2008b compatibility +% 2008-09-10: Fixed minor bug as per Johnny Smith +% 2007-11-14: Fixed edge case problem with class properties tooltip; used existing object icon if available; added checkbox option to hide standard callbacks +% 2007-08-15: Fixed object naming relative property priorities; added sanity check for illegal container arg; enabled desktop (0) container; cleaned up warnings about special class objects +% 2007-08-03: Fixed minor tagging problems with a few Java sub-classes; displayed UIClassID if text/name/tag is unavailable +% 2007-06-15: Fixed problems finding HG components found by J. Wagberg +% 2007-05-22: Added 'nomenu' option for improved performance; fixed 'export handles' bug; fixed handle-finding/display bugs; "cleaner" error handling +% 2007-04-23: HTMLized classname tooltip; returned top-level figure Frame handle for figure container; fixed callbacks table; auto-checked newer version; fixed Matlab 7.2 compatibility issue; added HG objects tree +% 2007-04-19: Fixed edge case of missing figure; displayed tree hierarchy in interactive GUI if no output args; workaround for figure sub-menus invisible unless clicked +% 2007-04-04: Improved performance; returned full listing results in 4th output arg; enabled partial property names & property values; automatically filtered out container panels if children also returned; fixed finding sub-menu items +% 2007-03-20: First version posted on the MathWorks file exchange: http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=14317 +% +% See also: +% java, handle, findobj, findall, javaGetHandles, uiinspect (on the File Exchange) + +% License to use and modify this code is granted freely to all interested, as long as the original author is +% referenced and attributed as such. The original author maintains the right to be solely associated with this work. + +% Programmed and Copyright by Yair M. Altman: altmany(at)gmail.com +% $Revision: 1.21 $ $Date: 2009/05/24 00:47:55 $ + + % Ensure Java AWT is enabled + error(javachk('awt')); + + persistent pContainer pHandles pLevels pParentIdx pPositions + + try + % Initialize + handles = handle([]); + levels = []; + parentIdx = []; + positions = []; % Java positions start at the top-left corner + %sizes = []; + listing = ''; + hg_levels = []; + hg_handles = handle([]); % HG handles are double + hg_parentIdx = []; + nomenu = false; + menuBarFoundFlag = false; + + % Default container is the current figure's root panel + if nargin + if isempty(container) % empty container - bail out + return; + elseif ischar(container) % container skipped - this is part of the args list... + varargin = {container, varargin{:}}; + origContainer = getCurrentFigure; + [container,contentSize] = getRootPanel(origContainer); + elseif isequal(container,0) % root + origContainer = handle(container); + container = com.mathworks.mde.desk.MLDesktop.getInstance.getMainFrame; + contentSize = [container.getWidth, container.getHeight]; + elseif ishghandle(container) % && ~isa(container,'java.awt.Container') + container = container(1); % another current limitation... + hFig = ancestor(container,'figure'); + origContainer = handle(container); + if isa(origContainer,'uimenu') + % getpixelposition doesn't work for menus... - damn! + varargin = {'class','MenuPeer', 'property',{'Label',strrep(get(container,'Label'),'&','')}, varargin{:}}; + elseif ~isa(origContainer, 'figure') && ~isempty(hFig) + % See limitations section above: should find a better way to directly refer to the element's java container + try + % Note: 'PixelBounds' is undocumented and unsupported, but much faster than getpixelposition! + % ^^^^ unfortunately, its Y position is inaccurate in some cases - damn! + %size = get(container,'PixelBounds'); + pos = fix(getpixelposition(container,1)); + %varargin = {'position',pos(1:2), 'size',pos(3:4), 'not','class','java.awt.Panel', varargin{:}}; + catch + try + figName = get(hFig,'name'); + if strcmpi(get(hFig,'number'),'on') + figName = regexprep(['Figure ' num2str(hFig) ': ' figName],': $',''); + end + mde = com.mathworks.mde.desk.MLDesktop.getInstance; + jFig = mde.getClient(figName); + if isempty(jFig), error('dummy'); end + catch + warning('off','MATLAB:HandleGraphics:ObsoletedProperty:JavaFrame'); % R2008b compatibility + jFig = get(get(hFig,'JavaFrame'),'FigurePanelContainer'); + end + pos = []; + try + pxsize = get(container,'PixelBounds'); + pos = [pxsize(1)+5, jFig.getHeight - (pxsize(4)-5)]; + catch + % never mind... + end + end + if size(pos,2) == 2 + pos(:,3:4) = 0; + end + if ~isempty(pos) + if isa(handle(container),'uicontrol') && strcmp(get(container,'style'),'popupmenu') + % popupmenus (combo-box dropdowns) are always 20px high + pos(2) = pos(2) + pos(4) - 20; + pos(4) = 20; + end + %varargin = {'position',pos(1:2), 'size',size(3:4)-size(1:2)-10, 'not','class','java.awt.Panel', varargin{:}}; + varargin = {'position',pos(1:2)+[0,pos(4)], 'size',pos(3:4), 'not','class','java.awt.Panel', 'nomenu', varargin{:}}; + end + elseif isempty(hFig) + hFig = handle(container); + end + [container,contentSize] = getRootPanel(hFig); + elseif isjava(container) + % Maybe a java container obj (will crash otherwise) + origContainer = container; + contentSize = [container.getWidth, container.getHeight]; + else + error('YMA:findjobj:IllegalContainer','Input arg does not appear to be a valid GUI object'); + end + else + % Default container = current figure + origContainer = getCurrentFigure; + [container,contentSize] = getRootPanel(origContainer); + end + + % Check persistency + if isequal(pContainer,container) + % persistency requested and the same container is reused, so reuse the hierarchy information + [handles,levels,parentIdx,positions] = deal(pHandles, pLevels, pParentIdx, pPositions); + else + % Pre-allocate space of complex data containers for improved performance + handles = repmat(handles,1,1000); + positions = zeros(1000,2); + + % Check whether to skip menu processing + nomenu = paramSupplied(varargin,'nomenu'); + + % Traverse the container hierarchy and extract the elements within + traverseContainer(container,0,1); + + % Remove unnecessary pre-allocated elements + dataLen = length(levels); + handles (dataLen+1:end) = []; + positions(dataLen+1:end,:) = []; + end + + % Process persistency check before any filtering is done + if paramSupplied(varargin,'persist') + [pContainer, pHandles, pLevels, pParentIdx, pPositions] = deal(container,handles,levels,parentIdx,positions); + end + + % Save data for possible future use in presentObjectTree() below + allHandles = handles; + allLevels = levels; + allParents = parentIdx; + selectedIdx = 1:length(handles); + %[positions(:,1)-container.getX, container.getHeight - positions(:,2)] + + % Debug-list all found compponents and their positions + if paramSupplied(varargin,'debug') + for handleIdx = 1 : length(allHandles) + pos = sprintf('%d,%d %dx%d',[positions(handleIdx,:) getXY(handles(handleIdx))]); + disp([repmat(' ',1,levels(handleIdx)) '[' pos(1:end-1) '] ' char(toString(handles(handleIdx)))]); + end + end + + % Process optional args + % Note: positions is NOT returned since it's based on java coord system (origin = top-left): will confuse Matlab users + processArgs(varargin{:}); %#ok + + % De-cell and trim listing, if only one element was found (no use for indented listing in this case) + if iscell(listing) && length(listing)==1 + listing = strtrim(listing{1}); + end + + % If no output args and no listing requested, present the FINDJOBJ interactive GUI + if nargout == 0 && isempty(listing) + presentObjectTree(); + + % Display the listing, if this was specifically requested yet no relevant output arg was specified + elseif nargout < 4 && ~isempty(listing) + if ~iscell(listing) + disp(listing); + else + for listingIdx = 1 : length(listing) + disp(listing{listingIdx}); + end + end + end + + return; %debug point + + catch + % 'Cleaner' error handling - strip the stack info etc. + err = lasterror; + err.message = regexprep(err.message,'Error using ==> [^\n]+\n',''); + if isempty(findstr(mfilename,err.message)) + % Indicate error origin, if not already stated within the error message + err.message = [mfilename ': ' err.message]; + end + rethrow(err); + end + + +%% Check existence of a (case-insensitive) optional parameter in the params list + function [flag,idx] = paramSupplied(paramsList,paramName) + idx = find(~cellfun('isempty',regexpi(paramsList(cellfun(@ischar,paramsList)),['^-?' paramName]))); + flag = any(idx); + end + +%% Get current figure (even if its 'HandleVisibility' property is 'off') + function curFig = getCurrentFigure + oldShowHidden = get(0,'ShowHiddenHandles'); + set(0,'ShowHiddenHandles','on'); % minor fix per Johnny Smith + curFig = gcf; + set(0,'ShowHiddenHandles',oldShowHidden); + end + +%% Get Java reference to top-level (root) panel - actually, a reference to the java figure + function [jRootPane,contentSize] = getRootPanel(hFig) + try + contentSize = [0,0]; % initialize + jRootPane = hFig; + figName = get(hFig,'name'); + if strcmpi(get(hFig,'number'),'on') + figName = regexprep(['Figure ' num2str(hFig) ': ' figName],': $',''); + end + mde = com.mathworks.mde.desk.MLDesktop.getInstance; + jFigPanel = mde.getClient(figName); + jRootPane = jFigPanel; + jRootPane = jFigPanel.getRootPane; + catch + try + jFrame = get(hFig,'JavaFrame'); + jFigPanel = get(jFrame,'FigurePanelContainer'); + jRootPane = jFigPanel; + jRootPane = jFigPanel.getComponent(0).getRootPane; + catch + % Never mind + end + end + try + % If invalid RootPane, retry up to N times + tries = 10; + while isempty(jRootPane) && tries>0 % might happen if figure is still undergoing rendering... + drawnow; pause(0.001); + tries = tries - 1; + jRootPane = jFigPanel.getComponent(0).getRootPane; + end + + % If still invalid, use FigurePanelContainer which is good enough in 99% of cases... (menu/tool bars won't be accessible, though) + if isempty(jRootPane) + jRootPane = jFigPanel; + end + contentSize = [jRootPane.getWidth, jRootPane.getHeight]; + + % Try to get the ancestor FigureFrame + jRootPane = jRootPane.getTopLevelAncestor; + catch + % Never mind - FigurePanelContainer is good enough in 99% of cases... (menu/tool bars won't be accessible, though) + end + end + +%% Traverse the container hierarchy and extract the elements within + function traverseContainer(jcontainer,level,parent) + persistent figureComponentFound menuRootFound + + % Record the data for this node + %disp([repmat(' ',1,level) '<= ' char(jcontainer.toString)]) + thisIdx = length(levels) + 1; + levels(thisIdx) = level; + parentIdx(thisIdx) = parent; + handles(thisIdx) = handle(jcontainer,'callbackproperties'); + try + positions(thisIdx,:) = getXY(jcontainer); + %sizes(thisIdx,:) = [jcontainer.getWidth, jcontainer.getHeight]; + catch + positions(thisIdx,:) = [0,0]; + %sizes(thisIdx,:) = [0,0]; + end + if level>0 + positions(thisIdx,:) = positions(thisIdx,:) + positions(parent,:); + if ~figureComponentFound && ... + strcmp(jcontainer.getName,'fComponentContainer') && ... + isa(jcontainer,'com.mathworks.hg.peer.FigureComponentContainer') % there are 2 FigureComponentContainers - only process one... + + % restart coordinate system, to exclude menu & toolbar areas + positions(thisIdx,:) = positions(thisIdx,:) - [jcontainer.getRootPane.getX, jcontainer.getRootPane.getY]; + figureComponentFound = true; + end + elseif level==1 + positions(thisIdx,:) = positions(thisIdx,:) + positions(parent,:); + else + % level 0 - initialize flags used later + figureComponentFound = false; + menuRootFound = false; + end + parentId = length(parentIdx); + + % Traverse Menu items, unless the 'nomenu' option was requested + if ~nomenu + try + for child = 1 : getNumMenuComponents(jcontainer) + traverseContainer(jcontainer.getMenuComponent(child-1),level+1,parentId); + end + catch + % Probably not a Menu container, but maybe a top-level JMenu, so discard duplicates + %if isa(handles(end).java,'javax.swing.JMenuBar') + if ~menuRootFound && strcmp(handles(end).java.class,'javax.swing.JMenuBar') %faster... + if removeDuplicateNode(thisIdx) + menuRootFound = true; + return; + end + end + end + end + + % Now recursively process all this node's children (if any), except menu items if so requested + %if isa(jcontainer,'java.awt.Container') + try % try-catch is faster than checking isa(jcontainer,'java.awt.Container')... + %if jcontainer.getComponentCount, jcontainer.getComponents, end + if ~nomenu || menuBarFoundFlag || isempty(strfind(jcontainer.class,'FigureMenuBar')) + lastChildComponent = java.lang.Object; + child = 0; + while (child < jcontainer.getComponentCount) + childComponent = jcontainer.getComponent(child); + % Looping over menus sometimes causes jcontainer to get mixed up (probably a JITC bug), so identify & fix + if isequal(childComponent,lastChildComponent) + child = child + 1; + childComponent = jcontainer.getComponent(child); + end + lastChildComponent = childComponent; + %disp([repmat(' ',1,level) '=> ' num2str(child) ': ' char(childComponent.class)]) + traverseContainer(childComponent,level+1,parentId); + child = child + 1; + end + else + menuBarFoundFlag = true; % use this flag to skip further testing for FigureMenuBar + end + catch + % do nothing - probably not a container + %dispError + end + + % ...and yet another type of child traversal... + try + for child = 1 : jcontainer.java.getChildCount + traverseContainer(jcontainer.java.getChildAt(child-1),level+1,parentId); + end + catch + % do nothing - probably not a container + %dispError + end + + % TODO: Add axis (plot) component handles + end + +%% Get the XY location of a Java component + function xy = getXY(jcontainer) + % Note: getX/getY are better than get(..,'X') (mem leaks), + % ^^^^ but sometimes they fail and revert to getx.m ... + % Note2: try awtinvoke() catch is faster than checking ismethod()... + % Note3: using AWTUtilities.invokeAndWait() directly is even faster than awtinvoke()... + try %if ismethod(jcontainer,'getX') + %positions(thisIdx,:) = [jcontainer.getX, jcontainer.getY]; + cls = getClass(jcontainer); + location = com.mathworks.jmi.AWTUtilities.invokeAndWait(jcontainer,getMethod(cls,'getLocation',[]),[]); + x = location.getX; + y = location.getY; + catch %else + try + x = com.mathworks.jmi.AWTUtilities.invokeAndWait(jcontainer,getMethod(cls,'getX',[]),[]); + y = com.mathworks.jmi.AWTUtilities.invokeAndWait(jcontainer,getMethod(cls,'getY',[]),[]); + catch + try + x = awtinvoke(jcontainer,'getX()'); + y = awtinvoke(jcontainer,'getY()'); + catch + x = get(jcontainer,'X'); + y = get(jcontainer,'Y'); + end + end + end + %positions(thisIdx,:) = [x, y]; + xy = [x,y]; + end + +%% Get the number of menu sub-elements + function numMenuComponents = getNumMenuComponents(jcontainer) + + % The following line will raise an Exception for anything except menus + numMenuComponents = jcontainer.getMenuComponentCount; + + % No error so far, so this must be a menu container... + % Note: Menu subitems are not visible until the top-level (root) menu gets initial focus... + % Try several alternatives, until we get a non-empty menu (or not...) + % TODO: Improve performance - this takes WAY too long... + if jcontainer.isTopLevelMenu && (numMenuComponents==0) + jcontainer.requestFocus; + numMenuComponents = jcontainer.getMenuComponentCount; + if (numMenuComponents == 0) + drawnow; pause(0.001); + numMenuComponents = jcontainer.getMenuComponentCount; + if (numMenuComponents == 0) + jcontainer.setSelected(true); + numMenuComponents = jcontainer.getMenuComponentCount; + if (numMenuComponents == 0) + drawnow; pause(0.001); + numMenuComponents = jcontainer.getMenuComponentCount; + if (numMenuComponents == 0) + jcontainer.doClick; % needed in order to populate the sub-menu components + numMenuComponents = jcontainer.getMenuComponentCount; + if (numMenuComponents == 0) + drawnow; %pause(0.001); + numMenuComponents = jcontainer.getMenuComponentCount; + jcontainer.doClick; % close menu by re-clicking... + if (numMenuComponents == 0) + drawnow; %pause(0.001); + numMenuComponents = jcontainer.getMenuComponentCount; + end + else + % ok - found sub-items + % Note: no need to close menu since this will be done when focus moves to the FindJObj window + %jcontainer.doClick; % close menu by re-clicking... + end + end + end + jcontainer.setSelected(false); % de-select the menu + end + end + end + end + +%% Remove a specific tree node's data + function nodeRemovedFlag = removeDuplicateNode(thisIdx) + nodeRemovedFlag = false; + for idx = 1 : thisIdx-1 + if isequal(handles(idx),handles(thisIdx)) + levels(thisIdx) = []; + parentIdx(thisIdx) = []; + handles(thisIdx) = []; + positions(thisIdx,:) = []; + %sizes(thisIdx,:) = []; + nodeRemovedFlag = true; + return; + end + end + end + +%% Process optional args + function processArgs(varargin) + + % Initialize + invertFlag = false; + listing = ''; + + % Loop over all optional args + while ~isempty(varargin) && ~isempty(handles) + + % Process the arg (and all its params) + foundIdx = 1 : length(handles); + if iscell(varargin{1}), varargin{1} = varargin{1}{1}; end + if ~isempty(varargin{1}) && varargin{1}(1)=='-' + varargin{1}(1) = []; + end + switch lower(varargin{1}) + case 'not' + invertFlag = true; + case 'position' + [varargin,foundIdx] = processPositionArgs(varargin{:}); + if invertFlag, foundIdx = ~foundIdx; invertFlag = false; end + case 'size' + [varargin,foundIdx] = processSizeArgs(varargin{:}); + if invertFlag, foundIdx = ~foundIdx; invertFlag = false; end + case 'class' + [varargin,foundIdx] = processClassArgs(varargin{:}); + if invertFlag, foundIdx = ~foundIdx; invertFlag = false; end + case 'property' + [varargin,foundIdx] = processPropertyArgs(varargin{:}); + if invertFlag, foundIdx = ~foundIdx; invertFlag = false; end + case 'depth' + [varargin,foundIdx] = processDepthArgs(varargin{:}); + if invertFlag, foundIdx = ~foundIdx; invertFlag = false; end + case 'flat' + varargin = {'depth',0, varargin{min(2:end):end}}; + [varargin,foundIdx] = processDepthArgs(varargin{:}); + if invertFlag, foundIdx = ~foundIdx; invertFlag = false; end + case {'print','list'} + [varargin,listing] = processPrintArgs(varargin{:}); + case {'persist','nomenu','debug'} + % ignore - already handled in main function above + otherwise + error('YMA:findjobj:IllegalOption',['Option ' num2str(varargin{1}) ' is not a valid option. Type ''help ' mfilename ''' for the full options list.']); + end + + % If only parent-child pairs found + foundIdx = find(foundIdx); + if ~isempty(foundIdx) && isequal(parentIdx(foundIdx(2:2:end)),foundIdx(1:2:end)) + % Return just the children (the parent panels are uninteresting) + foundIdx(1:2:end) = []; + end + + % Filter the results + selectedIdx = selectedIdx(foundIdx); + handles = handles(foundIdx); + levels = levels(foundIdx); + parentIdx = parentIdx(foundIdx); + positions = positions(foundIdx,:); + + % Remove this arg and proceed to the next one + varargin(1) = []; + + end % Loop over all args + end + +%% Process 'print' option + function [varargin,listing] = processPrintArgs(varargin) + if length(varargin)<2 || ischar(varargin{2}) + % No second arg given, so use the first available element + listingContainer = handles(1); %#ok - used in evalc below + else + % Get the element to print from the specified second arg + if isnumeric(varargin{2}) && (varargin{2} == fix(varargin{2})) % isinteger doesn't work on doubles... + if (varargin{2} > 0) && (varargin{2} <= length(handles)) + listingContainer = handles(varargin{2}); %#ok - used in evalc below + elseif varargin{2} > 0 + error('YMA:findjobj:IllegalPrintFilter','Print filter index %g > number of available elements (%d)',varargin{2},length(handles)); + else + error('YMA:findjobj:IllegalPrintFilter','Print filter must be a java handle or positive numeric index into handles'); + end + elseif ismethod(varargin{2},'list') + listingContainer = varargin{2}; %#ok - used in evalc below + else + error('YMA:findjobj:IllegalPrintFilter','Print filter must be a java handle or numeric index into handles'); + end + varargin(2) = []; + end + + % use evalc() to capture output into a Matlab variable + %listing = evalc('listingContainer.list'); + + % Better solution: loop over all handles and process them one by one + listing = cell(length(handles),1); + for componentIdx = 1 : length(handles) + listing{componentIdx} = [repmat(' ',1,levels(componentIdx)) char(handles(componentIdx).toString)]; + end + end + +%% Process 'position' option + function [varargin,foundIdx] = processPositionArgs(varargin) + if length(varargin)>1 + positionFilter = varargin{2}; + %if (isjava(positionFilter) || iscom(positionFilter)) && ismethod(positionFilter,'getLocation') + try % try-catch is faster... + % Java/COM object passed - get its position + positionFilter = positionFilter.getLocation; + filterXY = [positionFilter.getX, positionFilter.getY]; + catch + if ~isscalar(positionFilter) + % position vector passed + if (length(positionFilter)>=2) && isnumeric(positionFilter) + % Remember that java coordinates start at top-left corner, Matlab coords start at bottom left... + %positionFilter = java.awt.Point(positionFilter(1), container.getHeight - positionFilter(2)); + filterXY = [container.getX + positionFilter(1), container.getY + contentSize(2) - positionFilter(2)]; + + % Check for full Matlab position vector (x,y,w,h) + %if (length(positionFilter)==4) + % varargin{end+1} = 'size'; + % varargin{end+1} = fix(positionFilter(3:4)); + %end + else + error('YMA:findjobj:IllegalPositionFilter','Position filter must be a java UI component, or X,Y pair'); + end + elseif length(varargin)>2 + % x,y passed as separate arg values + if isnumeric(positionFilter) && isnumeric(varargin{3}) + % Remember that java coordinates start at top-left corner, Matlab coords start at bottom left... + %positionFilter = java.awt.Point(positionFilter, container.getHeight - varargin{3}); + filterXY = [container.getX + positionFilter, container.getY + contentSize(2) - varargin{3}]; + varargin(3) = []; + else + error('YMA:findjobj:IllegalPositionFilter','Position filter must be a java UI component, or X,Y pair'); + end + else + error('YMA:findjobj:IllegalPositionFilter','Position filter must be a java UI component, or X,Y pair'); + end + end + + % Compute the required element positions in order to be eligible for a more detailed examination + % Note: based on the following constraints: 0 <= abs(elementX-filterX) + abs(elementY+elementH-filterY) < 7 + baseDeltas = [positions(:,1)-filterXY(1), positions(:,2)-filterXY(2)]; % faster than repmat()... + %baseHeight = - baseDeltas(:,2);% -abs(baseDeltas(:,1)); + %minHeight = baseHeight - 7; + %maxHeight = baseHeight + 7; + %foundIdx = ~arrayfun(@(b)(invoke(b,'contains',positionFilter)),handles); % ARGH! - disallowed by Matlab! + %foundIdx = repmat(false,1,length(handles)); + %foundIdx(length(handles)) = false; % faster than repmat()... + foundIdx = (abs(baseDeltas(:,1)) < 7) & (abs(baseDeltas(:,2)) < 7); % & (minHeight >= 0); + %fi = find(foundIdx); + %for componentIdx = 1 : length(fi) + %foundIdx(componentIdx) = handles(componentIdx).getBounds.contains(positionFilter); + + % Search for a point no farther than 7 pixels away (prevents rounding errors) + %foundIdx(componentIdx) = handles(componentIdx).getLocationOnScreen.distanceSq(positionFilter) < 50; % fails for invisible components... + + %p = java.awt.Point(positions(componentIdx,1), positions(componentIdx,2) + handles(componentIdx).getHeight); + %foundIdx(componentIdx) = p.distanceSq(positionFilter) < 50; + + %foundIdx(componentIdx) = sum(([baseDeltas(componentIdx,1),baseDeltas(componentIdx,2)+handles(componentIdx).getHeight]).^2) < 50; + + % Following is the fastest method found to date: only eligible elements are checked in detailed + % elementHeight = handles(fi(componentIdx)).getHeight; + % foundIdx(fi(componentIdx)) = elementHeight > minHeight(fi(componentIdx)) && ... + % elementHeight < maxHeight(fi(componentIdx)); + %disp([componentIdx,elementHeight,minHeight(fi(componentIdx)),maxHeight(fi(componentIdx)),foundIdx(fi(componentIdx))]) + %end + + varargin(2) = []; + else + foundIdx = []; + end + end + +%% Process 'size' option + function [varargin,foundIdx] = processSizeArgs(varargin) + if length(varargin)>1 + sizeFilter = lower(varargin{2}); + %if (isjava(sizeFilter) || iscom(sizeFilter)) && ismethod(sizeFilter,'getSize') + try % try-catch is faster... + % Java/COM object passed - get its size + sizeFilter = sizeFilter.getSize; + filterWidth = sizeFilter.getWidth; + filterHeight = sizeFilter.getHeight; + catch + if ~isscalar(sizeFilter) + % size vector passed + if (length(sizeFilter)>=2) && isnumeric(sizeFilter) + %sizeFilter = java.awt.Dimension(sizeFilter(1),sizeFilter(2)); + filterWidth = sizeFilter(1); + filterHeight = sizeFilter(2); + else + error('YMA:findjobj:IllegalSizeFilter','Size filter must be a java UI component, or W,H pair'); + end + elseif length(varargin)>2 + % w,h passed as separate arg values + if isnumeric(sizeFilter) && isnumeric(varargin{3}) + %sizeFilter = java.awt.Dimension(sizeFilter,varargin{3}); + filterWidth = sizeFilter; + filterHeight = varargin{3}; + varargin(3) = []; + else + error('YMA:findjobj:IllegalSizeFilter','Size filter must be a java UI component, or W,H pair'); + end + else + error('YMA:findjobj:IllegalSizeFilter','Size filter must be a java UI component, or W,H pair'); + end + end + %foundIdx = ~arrayfun(@(b)(invoke(b,'contains',sizeFilter)),handles); % ARGH! - disallowed by Matlab! + foundIdx(length(handles)) = false; % faster than repmat()... + for componentIdx = 1 : length(handles) + %foundIdx(componentIdx) = handles(componentIdx).getSize.equals(sizeFilter); + % Allow a 2-pixel tollerance to account for non-integer pixel sizes + foundIdx(componentIdx) = abs(handles(componentIdx).getWidth - filterWidth) <= 2 && ... % faster than getSize.equals() + abs(handles(componentIdx).getHeight - filterHeight) <= 2; + end + varargin(2) = []; + else + foundIdx = []; + end + end + +%% Process 'class' option + function [varargin,foundIdx] = processClassArgs(varargin) + if length(varargin)>1 + classFilter = varargin{2}; + %if ismethod(classFilter,'getClass') + try % try-catch is faster... + classFilter = char(classFilter.getClass); + catch + if ~ischar(classFilter) + error('YMA:findjobj:IllegalClassFilter','Class filter must be a java object, class or string'); + end + end + + % Now convert all java classes to java.lang.Strings and compare to the requested filter string + try + foundIdx(length(handles)) = false; % faster than repmat()... + jClassFilter = java.lang.String(classFilter).toLowerCase; + for componentIdx = 1 : length(handles) + % Note: JVM 1.5's String.contains() appears slightly slower and is available only since Matlab 7.2 + foundIdx(componentIdx) = handles(componentIdx).getClass.toString.toLowerCase.indexOf(jClassFilter) >= 0; + end + catch + % Simple processing: slower since it does extra processing within opaque.char() + for componentIdx = 1 : length(handles) + % Note: using @toChar is faster but returns java String, not a Matlab char + foundIdx(componentIdx) = ~isempty(regexpi(char(handles(componentIdx).getClass),classFilter)); + end + end + + varargin(2) = []; + else + foundIdx = []; + end + end + +%% Process 'property' option + function [varargin,foundIdx] = processPropertyArgs(varargin) + if length(varargin)>1 + propertyName = varargin{2}; + if iscell(propertyName) + if length(propertyName) == 2 + propertyVal = propertyName{2}; + propertyName = propertyName{1}; + elseif length(propertyName) == 1 + propertyName = propertyName{1}; + else + error('YMA:findjobj:IllegalPropertyFilter','Property filter must be a string (case insensitive name of property) or cell array {propName,propValue}'); + end + end + if ~ischar(propertyName) + error('YMA:findjobj:IllegalPropertyFilter','Property filter must be a string (case insensitive name of property) or cell array {propName,propValue}'); + end + propertyName = lower(propertyName); + %foundIdx = arrayfun(@(h)isprop(h,propertyName),handles); % ARGH! - disallowed by Matlab! + foundIdx(length(handles)) = false; % faster than repmat()... + + % Split processing depending on whether a specific property value was requested (ugly but faster...) + if exist('propertyVal','var') + for componentIdx = 1 : length(handles) + try + % Find out whether this element has the specified property + % Note: findprop() and its return value schema.prop are undocumented and unsupported! + prop = findprop(handles(componentIdx),propertyName); % faster than isprop() & enables partial property names + + % If found, compare it to the actual element's property value + foundIdx(componentIdx) = ~isempty(prop) && isequal(get(handles(componentIdx),prop.Name),propertyVal); + catch + % Some Java classes have a write-only property (like LabelPeer with 'Text'), so we end up here + % In these cases, simply assume that the property value doesn't match and continue + foundIdx(componentIdx) = false; + end + end + else + for componentIdx = 1 : length(handles) + try + % Find out whether this element has the specified property + % Note: findprop() and its return value schema.prop are undocumented and unsupported! + foundIdx(componentIdx) = ~isempty(findprop(handles(componentIdx),propertyName)); + catch + foundIdx(componentIdx) = false; + end + end + end + varargin(2) = []; + else + foundIdx = []; + end + end + +%% Process 'depth' option + function [varargin,foundIdx] = processDepthArgs(varargin) + if length(varargin)>1 + level = varargin{2}; + if ~isnumeric(level) + error('YMA:findjobj:IllegalDepthFilter','Depth filter must be a number (=maximal element depth)'); + end + foundIdx = (levels <= level); + varargin(2) = []; + else + foundIdx = []; + end + end + +%% Convert property data into a string + function data = charizeData(data) + if isa(data,'com.mathworks.hg.types.HGCallback') + data = get(data,'Callback'); + end + if ~ischar(data) + newData = strtrim(evalc('disp(data)')); + try + newData = regexprep(newData,' +',' '); + newData = regexprep(newData,'Columns \d+ through \d+\s',''); + newData = regexprep(newData,'Column \d+\s',''); + catch + %never mind... + end + if iscell(data) + newData = ['{ ' newData ' }']; + elseif isempty(data) + newData = ''; + elseif isnumeric(data) || islogical(data) || any(ishandle(data)) || numel(data) > 1 %&& ~isscalar(data) + newData = ['[' newData ']']; + end + data = newData; + elseif ~isempty(data) + data = ['''' data '''']; + end + end % charizeData + +%% Get callbacks table data + function [cbData, cbHeaders, cbTableEnabled] = getCbsData(obj, stripStdCbsFlag) + classHdl = classhandle(handle(obj)); + cbNames = get(classHdl.Events,'Name'); + if ~isempty(cbNames) && ~iscom(obj) %only java-based please... + cbNames = strcat(cbNames,'Callback'); + end + propNames = get(classHdl.Properties,'Name'); + propCbIdx = []; + if ~isempty(propNames) + propCbIdx = find(~cellfun(@isempty,regexp(propNames,'(Fcn|Callback)$'))); + cbNames = unique([cbNames; propNames(propCbIdx)]); %#ok logical is faster but less debuggable... + end + if ~isempty(cbNames) + if stripStdCbsFlag + cbNames = stripStdCbs(cbNames); + end + if iscell(cbNames) + cbNames = sort(cbNames); + end + hgHandleFlag = 0; try hgHandleFlag = ishghandle(obj); catch, end %#ok + try + obj = handle(obj,'CallbackProperties'); + catch + hgHandleFlag = 1; + end + if hgHandleFlag + % HG handles don't allow CallbackProperties - search only for *Fcn + cbNames = propNames(propCbIdx); + end + if iscom(obj) + cbs = obj.eventlisteners; + if ~isempty(cbs) + cbNamesRegistered = cbs(:,1); + cbData = setdiff(cbNames,cbNamesRegistered); + %cbData = charizeData(cbData); + if size(cbData,2) > size(cbData(1)) + cbData = cbData'; + end + cbData = [cbData, cellstr(repmat(' ',length(cbData),1))]; + cbData = [cbData; cbs]; + [sortedNames, sortedIdx] = sort(cbData(:,1)); + sortedCbs = cellfun(@charizeData,cbData(sortedIdx,2),'un',0); + cbData = [sortedNames, sortedCbs]; + else + cbData = [cbNames, cellstr(repmat(' ',length(cbNames),1))]; + end + elseif iscell(cbNames) + cbNames = sort(cbNames); + %cbData = [cbNames, get(obj,cbNames)']; + cbData = cbNames; + for idx = 1 : length(cbNames) + try + cbData{idx,2} = charizeData(get(obj,cbNames{idx})); + catch + cbData{idx,2} = '(callback value inaccessible)'; + end + end + else % only one event callback + %cbData = {cbNames, get(obj,cbNames)'}; + %cbData{1,2} = charizeData(cbData{1,2}); + try + cbData = {cbNames, charizeData(get(obj,cbNames))}; + catch + cbData = {cbNames, '(callback value inaccessible)'}; + end + end + cbHeaders = {'Callback name','Callback value'}; + cbTableEnabled = true; + else + cbData = {'(no callbacks)'}; + cbHeaders = {'Callback name'}; + cbTableEnabled = false; + end + end % getCbsData + +%% Get relative (0.0-1.0) divider location + function divLocation = getRalativeDivlocation(jDiv) + divLocation = jDiv.getDividerLocation; + if divLocation > 1 % i.e. [pixels] + visibleRect = jDiv.getVisibleRect; + if jDiv.getOrientation == 0 % vertical + start = visibleRect.getY; + extent = visibleRect.getHeight - start; + else + start = visibleRect.getX; + extent = visibleRect.getWidth - start; + end + divLocation = (divLocation - start) / extent; + end + end % getRalativeDivlocation + +%% Try to set a treenode icon based on a container's icon + function setTreeNodeIcon(treenode,container) + try + iconImage = []; + iconImage = container.getIcon; + if ~isempty(findprop(handle(iconImage),'Image')) % get(iconImage,'Image') is easier but leaks memory... + iconImage = iconImage.getImage; + else + a=b; %#ok cause an error + end + catch + try + iconImage = container.getIconImage; + catch + try + if ~isempty(iconImage) + ge = java.awt.GraphicsEnvironment.getLocalGraphicsEnvironment; + gd = ge.getDefaultScreenDevice; + gc = gd.getDefaultConfiguration; + image = gc.createCompatibleImage(iconImage.getIconWidth, iconImage.getIconHeight); % a BufferedImage object + g = image.createGraphics; + iconImage.paintIcon([], g, 0, 0); + g.dispose; + iconImage = image; + end + catch + % never mind... + end + end + end + if ~isempty(iconImage) + iconImage = setIconSize(iconImage); + treenode.setIcon(iconImage); + end + end % setTreeNodeIcon + +%% Present the object hierarchy tree + function presentObjectTree() + import java.awt.* + import javax.swing.* + hTreeFig = findall(0,'tag','findjobjFig'); + iconpath = [matlabroot, '/toolbox/matlab/icons/']; + cbHideStd = 0; % Initial state of the cbHideStdCbs checkbox + if isempty(hTreeFig) + % Prepare the figure + hTreeFig = figure('tag','findjobjFig','menuBar','none','toolBar','none','Name','FindJObj','NumberTitle','off','handleVisibility','off','IntegerHandle','off'); + figIcon = ImageIcon([iconpath 'tool_legend.gif']); + drawnow; + try + mde = com.mathworks.mde.desk.MLDesktop.getInstance; + jTreeFig = mde.getClient('FindJObj').getTopLevelAncestor; + jTreeFig.setIcon(figIcon); + catch + jTreeFig = get(hTreeFig,'JavaFrame'); + jTreeFig.setFigureIcon(figIcon); + end + vsplitPaneLocation = 0.8; + hsplitPaneLocation = 0.5; + else + % Remember cbHideStdCbs checkbox & dividers state for later + userdata = get(hTreeFig, 'userdata'); + try cbHideStd = userdata.cbHideStdCbs.isSelected; catch, end %#ok + vsplitPaneLocation = getRalativeDivlocation(userdata.vsplitPane); + hsplitPaneLocation = getRalativeDivlocation(userdata.hsplitPane); + + % Clear the figure and redraw + clf(hTreeFig); + figure(hTreeFig); % bring to front + end + + % Traverse all HG children, if root container was a HG handle + if ishghandle(origContainer) %&& ~isequal(origContainer,container) + traverseHGContainer(origContainer,0,0); + end + + % Prepare the tree pane + warning('off','MATLAB:uitreenode:MigratingFunction'); % R2008b compatibility + tree_h = com.mathworks.hg.peer.UITreePeer; + hasChildren = sum(allParents==1) > 1; + icon = [iconpath 'upfolder.gif']; + [rootName, rootTitle] = getNodeName(container); + try + root = uitreenode('v0', handle(container), rootName, icon, ~hasChildren); + catch % old matlab version don't have the 'v0' option + root = uitreenode(handle(container), rootName, icon, ~hasChildren); + end + setTreeNodeIcon(root,container); % constructor must accept a char icon unfortunately, so need to do this afterwards... + if ~isempty(rootTitle) + set(hTreeFig, 'Name',['FindJObj - ' char(rootTitle)]); + end + nodedata.idx = 1; + nodedata.obj = container; + set(root,'userdata',nodedata); + root.setUserObject(container); + setappdata(root,'childHandle',container); + tree_h.setRoot(root); + treePane = tree_h.getScrollPane; + treePane.setMinimumSize(Dimension(50,50)); + jTreeObj = treePane.getViewport.getComponent(0); + jTreeObj.setShowsRootHandles(true) + jTreeObj.getSelectionModel.setSelectionMode(javax.swing.tree.TreeSelectionModel.DISCONTIGUOUS_TREE_SELECTION); + %jTreeObj.setVisible(0); + %jTreeObj.getCellRenderer.setLeafIcon([]); + %jTreeObj.getCellRenderer.setOpenIcon(figIcon); + %jTreeObj.getCellRenderer.setClosedIcon([]); + treePanel = JPanel(BorderLayout); + treePanel.add(treePane, BorderLayout.CENTER); + progressBar = JProgressBar(0); + progressBar.setMaximum(length(allHandles) + length(hg_handles)); % = # of all nodes + treePanel.add(progressBar, BorderLayout.SOUTH); + + % Prepare the image pane +%disable for now, until we get it working... +%{ + try + hFig = ancestor(origContainer,'figure'); + [cdata, cm] = getframe(hFig); %#ok cm unused + tempfname = [tempname '.png']; + imwrite(cdata,tempfname); % don't know how to pass directly to BufferedImage, so use disk... + jImg = javax.imageio.ImageIO.read(java.io.File(tempfname)); + try delete(tempfname); catch end + imgPanel = JPanel(); + leftPanel = JSplitPane(JSplitPane.VERTICAL_SPLIT, treePanel, imgPanel); + leftPanel.setOneTouchExpandable(true); + leftPanel.setContinuousLayout(true); + leftPanel.setResizeWeight(0.8); + catch + leftPanel = treePanel; + end +%} + leftPanel = treePanel; + + % Prepare the inspector pane + classNameLabel = JLabel([' ' char(container.class)]); + classNameLabel.setForeground(Color.red); + updateNodeTooltip(container, classNameLabel); + inspectorPanel = JPanel(BorderLayout); + inspectorPanel.add(classNameLabel, BorderLayout.NORTH); + % TODO: Maybe uncomment the following when we add the HG tree - in the meantime it's unused (java properties are un-groupable) + %objReg = com.mathworks.services.ObjectRegistry.getLayoutRegistry; + %toolBar = awtinvoke('com.mathworks.mlwidgets.inspector.PropertyView$ToolBarStyle','valueOf(Ljava.lang.String;)','GROUPTOOLBAR'); + %inspectorPane = com.mathworks.mlwidgets.inspector.PropertyView(objReg, toolBar); + inspectorPane = com.mathworks.mlwidgets.inspector.PropertyView; + identifiers = disableDbstopError; %#ok "dbstop if error" causes inspect.m to croak due to a bug - so workaround + inspectorPane.setObject(container); + inspectorPane.setAutoUpdate(true); + % TODO: Add property listeners + % TODO: Display additional props + inspectorTable = inspectorPane; + while ~isa(inspectorTable,'javax.swing.JTable') + inspectorTable = inspectorTable.getComponent(0); + end + toolTipText = 'hover mouse over the red classname above to see the full list of properties'; + inspectorTable.setToolTipText(toolTipText); + jideTableUtils = []; + try + % Try JIDE features - see http://www.jidesoft.com/products/JIDE_Grids_Developer_Guide.pdf + com.mathworks.mwswing.MJUtilities.initJIDE; + jideTableUtils = eval('com.jidesoft.grid.TableUtils;'); % prevent JIDE alert by run-time (not load-time) evaluation + jideTableUtils.autoResizeAllColumns(inspectorTable); + inspectorTable.setRowAutoResizes(true); + inspectorTable.getModel.setShowExpert(1); + catch + % JIDE is probably unavailable - never mind... + end + inspectorPanel.add(inspectorPane, BorderLayout.CENTER); + % TODO: Add data update listeners + + % Prepare the callbacks pane + callbacksPanel = JPanel(BorderLayout); + classHdl = classhandle(handle(container)); + eventNames = get(classHdl.Events,'Name'); + if ~isempty(eventNames) + cbNames = sort(strcat(eventNames,'Callback')); + cbData = [cbNames, get(container,cbNames)']; + cbTableEnabled = true; + else + cbData = {'(no callbacks)',''}; + cbTableEnabled = false; + end + cbHeaders = {'Callback name','Callback value'}; + try + % Use JideTable if available on this system + %callbacksTableModel = javax.swing.table.DefaultTableModel(cbData,cbHeaders); %#ok + %callbacksTable = eval('com.jidesoft.grid.PropertyTable(callbacksTableModel);'); % prevent JIDE alert by run-time (not load-time) evaluation + callbacksTable = eval('com.jidesoft.grid.TreeTable(cbData,cbHeaders);'); % prevent JIDE alert by run-time (not load-time) evaluation + callbacksTable.setRowAutoResizes(true); + callbacksTable.setColumnAutoResizable(true); + callbacksTable.setColumnResizable(true); + jideTableUtils.autoResizeAllColumns(callbacksTable); + callbacksTable.setTableHeader([]); % hide the column headers since now we can resize columns with the gridline + callbacksLabel = JLabel(' Callbacks:'); % The column headers are replaced with a header label + %callbacksPanel.add(callbacksLabel, BorderLayout.NORTH); + + % Add checkbox to show/hide standard callbacks + callbacksTopPanel = JPanel; + callbacksTopPanel.setLayout(BoxLayout(callbacksTopPanel, BoxLayout.LINE_AXIS)); + callbacksTopPanel.add(callbacksLabel); + callbacksTopPanel.add(Box.createHorizontalGlue); + jcb = JCheckBox('Hide standard callbacks', cbHideStd); + set(jcb, 'ActionPerformedCallback',@cbHideStdCbs_Callback, 'userdata',callbacksTable, 'tooltip','Hide standard Swing callbacks - only component-specific callbacks will be displayed'); + callbacksTopPanel.add(jcb); + callbacksPanel.add(callbacksTopPanel, BorderLayout.NORTH); + catch + % Otherwise, use a standard Swing JTable (keep the headers to enable resizing) + callbacksTable = JTable(cbData,cbHeaders); + end + cbToolTipText = 'Callbacks may be ''strings'', @funcHandle or {@funcHandle,arg1,...}'; + callbacksTable.setToolTipText(cbToolTipText); + callbacksTable.setGridColor(inspectorTable.getGridColor); + cbNameTextField = JTextField; + cbNameTextField.setEditable(false); % ensure that the callback names are not modified... + cbNameCellEditor = DefaultCellEditor(cbNameTextField); + cbNameCellEditor.setClickCountToStart(intmax); % i.e, never enter edit mode... + callbacksTable.getColumnModel.getColumn(0).setCellEditor(cbNameCellEditor); + if ~cbTableEnabled + callbacksTable.getColumnModel.getColumn(1).setCellEditor(cbNameCellEditor); + end + set(callbacksTable.getModel, 'TableChangedCallback',@tbCallbacksChanged, 'UserData',container); + cbScrollPane = JScrollPane(callbacksTable); + cbScrollPane.setVerticalScrollBarPolicy(cbScrollPane.VERTICAL_SCROLLBAR_AS_NEEDED); + callbacksPanel.add(cbScrollPane, BorderLayout.CENTER); + callbacksPanel.setToolTipText(cbToolTipText); + + % Prepare the top-bottom JSplitPanes + vsplitPane = JSplitPane(JSplitPane.VERTICAL_SPLIT, inspectorPanel, callbacksPanel); + vsplitPane.setOneTouchExpandable(true); + vsplitPane.setContinuousLayout(true); + vsplitPane.setResizeWeight(0.8); + + % Prepare the left-right JSplitPane + hsplitPane = JSplitPane(JSplitPane.HORIZONTAL_SPLIT, leftPanel, vsplitPane); + hsplitPane.setOneTouchExpandable(true); + hsplitPane.setContinuousLayout(true); + hsplitPane.setResizeWeight(0.6); + pos = getpixelposition(hTreeFig); + + % Prepare the bottom pane with all buttons + lowerPanel = JPanel(FlowLayout); + blogUrlLabel = 'Undocumented
Matlab.com'; + jWebsite = createJButton(blogUrlLabel, @btWebsite_Callback, 'Visit the UndocumentedMatlab.com blog'); + jWebsite.setContentAreaFilled(0); + lowerPanel.add(jWebsite); + lowerPanel.add(createJButton('Refresh
tree', {@btRefresh_Callback, origContainer, hTreeFig}, 'Rescan the component tree, from the root down')); + lowerPanel.add(createJButton('Export to
workspace', {@btExport_Callback, jTreeObj, classNameLabel}, 'Export the selected component handles to workspace variable findjobj_hdls')); + lowerPanel.add(createJButton('Request
focus', {@btFocus_Callback, jTreeObj, root}, 'Set the focus on the first selected component')); + lowerPanel.add(createJButton('Inspect
object', {@btInspect_Callback, jTreeObj, root}, 'View the signature of all methods supported by the first selected component')); + lowerPanel.add(createJButton('Check for
updates', {@btCheckFex_Callback}, 'Check the MathWorks FileExchange for the latest version of FindJObj')); + + % Display everything on-screen + globalPanel = JPanel(BorderLayout); + globalPanel.add(hsplitPane, BorderLayout.CENTER); + globalPanel.add(lowerPanel, BorderLayout.SOUTH); + [obj, hcontainer] = javacomponent(globalPanel, [0,0,pos(3:4)], hTreeFig); + set(hcontainer,'units','normalized'); + drawnow; + hsplitPane.setDividerLocation(hsplitPaneLocation); % this only works after the JSplitPane is displayed... + vsplitPane.setDividerLocation(vsplitPaneLocation); % this only works after the JSplitPane is displayed... + %restoreDbstopError(identifiers); + + % Refresh & resize the screenshot thumbnail +%disable for now, until we get it working... +%{ + try + hAx = axes('Parent',hTreeFig, 'units','pixels', 'position',[10,10,250,150], 'visible','off'); + axis(hAx,'image'); + image(cdata,'Parent',hAx); + axis(hAx,'off'); + set(hAx,'UserData',cdata); + set(imgPanel, 'ComponentResizedCallback',{@resizeImg, hAx}, 'UserData',lowerPanel); + imgPanel.getGraphics.drawImage(jImg, 0, 0, []); + catch + % Never mind... + end +%} + % If all handles were selected (i.e., none were filtered) then only select the first + if (length(selectedIdx) == length(allHandles)) && ~isempty(selectedIdx) + selectedIdx = 1; + end + + % Store handles for callback use + userdata.handles = allHandles; + userdata.levels = allLevels; + userdata.parents = allParents; + userdata.hg_handles = hg_handles; + userdata.hg_levels = hg_levels; + userdata.hg_parents = hg_parentIdx; + userdata.initialIdx = selectedIdx; + userdata.userSelected = false; % Indicates the user has modified the initial selections + userdata.inInit = true; + userdata.jTree = jTreeObj; + userdata.jTreePeer = tree_h; + userdata.vsplitPane = vsplitPane; + userdata.hsplitPane = hsplitPane; + userdata.classNameLabel = classNameLabel; + userdata.inspectorPane = inspectorPane; + userdata.callbacksTable = callbacksTable; + userdata.jideTableUtils = jideTableUtils; + userdata.cbHideStdCbs = jcb; + + set(tree_h, 'userdata',userdata); + set(hTreeFig, 'userdata',userdata); + set(callbacksTable,'userdata',userdata); + + % Select the root node if requested + % Note: we must do so here since all other nodes except the root are processed by expandNode + if any(selectedIdx==1) + tree_h.setSelectedNode(root); + end + + % Set the initial cbHideStdCbs state + try + if jcb.isSelected + drawnow; + evd.getSource.isSelected = jcb.isSelected; + cbHideStdCbs_Callback(jcb,evd); + end + catch + % never mind... + end + + % Set the callback functions + set(tree_h, 'NodeExpandedCallback', {@nodeExpanded, tree_h}); + set(tree_h, 'NodeSelectedCallback', {@nodeSelected, tree_h}); + + % Pre-expand all rows + expandNode(progressBar, jTreeObj, tree_h, root, 0); + %jTreeObj.setVisible(1); + + % Hide the progressbar now that we've finished expanding all rows + try + hsplitPane.getLeftComponent.setTopComponent(treePane); + catch + % Probably not a vSplitPane on the left... + hsplitPane.setLeftComponent(treePane); + end + hsplitPane.setDividerLocation(hsplitPaneLocation); % need to do it again... + + % Update userdata + userdata.inInit = false; + set(tree_h, 'userdata',userdata); + set(hTreeFig,'userdata',userdata); + + % Set keyboard focus on the tree + jTreeObj.requestFocus; + drawnow; + + % Check for a newer version + checkVersion(); + + % Reset the last error + lasterr(''); + end + +%% Rresize image pane + function resizeImg(varargin) %#ok - unused (TODO: waiting for img placement fix...) + try + hPanel = varargin{1}; + hAx = varargin{3}; + lowerPanel = get(hPanel,'UserData'); + newJPos = cell2mat(get(hPanel,{'X','Y','Width','Height'})); + newMPos = [1,get(lowerPanel,'Height'),newJPos(3:4)]; + set(hAx, 'units','pixels', 'position',newMPos, 'Visible','on'); + uistack(hAx,'top'); % no good... + set(hPanel,'Opaque','off'); % also no good... + catch + % Never mind... + dispError + end + return; + end + +%% "dbstop if error" causes inspect.m to croak due to a bug - so workaround by temporarily disabling this dbstop + function identifiers = disableDbstopError + dbStat = dbstatus; + idx = find(strcmp({dbStat.cond},'error')); + identifiers = [dbStat(idx).identifier]; + if ~isempty(idx) + dbclear if error; + msgbox('''dbstop if error'' had to be disabled due to a Matlab bug that would have caused Matlab to crash.', 'FindJObj', 'warn'); + end + end + +%% Restore any previous "dbstop if error" + function restoreDbstopError(identifiers) %#ok + for itemIdx = 1 : length(identifiers) + eval(['dbstop if error ' identifiers{itemIdx}]); + end + end + +%% Recursively expand all nodes (except toolbar/menubar) in startup + function expandNode(progressBar, tree, tree_h, parentNode, parentRow) + try + if nargin < 5 + parentPath = javax.swing.tree.TreePath(parentNode.getPath); + parentRow = tree.getRowForPath(parentPath); + end + tree.expandRow(parentRow); + progressBar.setValue(progressBar.getValue+1); + numChildren = parentNode.getChildCount; + if (numChildren == 0) + pause(0.0002); % as short as possible... + drawnow; + end + nodesToUnExpand = {'FigureMenuBar','MLMenuBar','MJToolBar','Box','uimenu','uitoolbar','ScrollBar'}; + numChildren = parentNode.getChildCount; + for childIdx = 0 : numChildren-1 + childNode = parentNode.getChildAt(childIdx); + + % Expand child node if not leaf & not toolbar/menubar + if childNode.isLeafNode + progressBar.setValue(progressBar.getValue+1); + + % Pre-select the node based upon the user's FINDJOBJ filters + try + nodedata = get(childNode, 'userdata'); + userdata = get(tree_h, 'userdata'); + if ~ishghandle(nodedata.obj) && ~userdata.userSelected && any(userdata.initialIdx == nodedata.idx) + pause(0.0002); % as short as possible... + drawnow; + if isempty(tree_h.getSelectedNodes) + tree_h.setSelectedNode(childNode); + else + newSelectedNodes = [tree_h.getSelectedNodes, childNode]; + tree_h.setSelectedNodes(newSelectedNodes); + end + end + catch + % never mind... + dispError + end + + else + % Expand all non-leaves + expandNode(progressBar, tree, tree_h, childNode); + + % Re-collapse toolbar/menubar etc., and also invisible containers + % Note: if we simply did nothing, progressbar would not have been updated... + try + childHandle = getappdata(childNode,'childHandle'); %=childNode.getUserObject + visible = childHandle.isVisible; + catch + visible = 1; + end + %if any(strcmp(childNode.getName,nodesToUnExpand)) + %name = char(childNode.getName); + if any(cellfun(@(s)~isempty(strmatch(s,char(childNode.getName))),nodesToUnExpand)) || ~visible + childPath = javax.swing.tree.TreePath(childNode.getPath); + childRow = tree.getRowForPath(childPath); + tree.collapseRow(childRow); + end + end + end + catch + % never mind... + dispError + end + end + +%% Create utility buttons + function hButton = createJButton(nameStr, handler, toolTipText) + try + jButton = javax.swing.JButton(['
' nameStr]); + jButton.setCursor(java.awt.Cursor.getPredefinedCursor(java.awt.Cursor.HAND_CURSOR)); + jButton.setToolTipText(toolTipText); + minSize = jButton.getMinimumSize; + jButton.setMinimumSize(java.awt.Dimension(minSize.getWidth,35)); + hButton = handle(jButton,'CallbackProperties'); + set(hButton,'ActionPerformedCallback',handler); + catch + % Never mind... + end + end + +%% Flash a component off/on for the specified duration +% note: starts with 'on'; if numTimes is odd then ends with 'on', otherwise with 'off' + function flashComponent(jComps,delaySecs,numTimes) + persistent redBorder redBorderPanels + try + if isempty(redBorder) % reuse if possible + redBorder = javax.swing.border.LineBorder(java.awt.Color.red,2,0); + end + for compIdx = 1 : length(jComps) + try + oldBorder{compIdx} = jComps(compIdx).getBorder; %#ok grow + catch + oldBorder{compIdx} = []; %#ok grow + end + isSettable(compIdx) = ismethod(jComps(compIdx),'setBorder'); %#ok grow + if isSettable(compIdx) + try + % some components prevent border modification: + oldBorderFlag = jComps(compIdx).isBorderPainted; + if ~oldBorderFlag + jComps(compIdx).setBorderPainted(1); + isSettable(compIdx) = jComps(compIdx).isBorderPainted; %#ok grow + jComps(compIdx).setBorderPainted(oldBorderFlag); + end + catch + % do nothing... + end + end + if compIdx > length(redBorderPanels) + redBorderPanels{compIdx} = javax.swing.JPanel; + redBorderPanels{compIdx}.setBorder(redBorder); + redBorderPanels{compIdx}.setOpaque(0); % transparent interior, red border + end + try + redBorderPanels{compIdx}.setBounds(jComps(compIdx).getBounds); + catch + % never mind - might be an HG handle + end + end + for idx = 1 : 2*numTimes + if idx>1, pause(delaySecs); end % don't pause at start + visible = mod(idx,2); + for compIdx = 1 : length(jComps) + try + jComp = jComps(compIdx); + + % Prevent Matlab crash (java buffer overflow...) + if jComp.isa('com.mathworks.mwswing.desk.DTSplitPane') || ... + jComp.isa('com.mathworks.mwswing.MJSplitPane') + continue; + end + + % HG handles are highlighted by setting their 'Selected' property + if isa(jComp,'uimenu') + if visible + oldColor = get(jComp,'ForegroundColor'); + setappdata(jComp,'findjobj_oldColor',oldColor); + set(jComp,'ForegroundColor','red'); + else + oldColor = getappdata(jComp,'findjobj_oldColor'); + set(jComp,'ForegroundColor',oldColor); + rmappdata(jComp,'ForegroundColor'); + end + + elseif ishghandle(jComp) + if visible + set(jComp,'Selected','on'); + else + set(jComp,'Selected','off'); + end + + else %if isjava(jComp) + + jParent = jComps(compIdx).getParent; + + % Most Java components allow modifying their borders + if isSettable(compIdx) + if visible + jComp.setBorder(redBorder); + try jComp.setBorderPainted(1); catch, end %#ok + else %if ~isempty(oldBorder{compIdx}) + jComp.setBorder(oldBorder{compIdx}); + end + jComp.repaint; + + % The other Java components are highlighted by a transparent red-border + % panel that is placed on top of them in their parent's space + elseif ~isempty(jParent) + if visible + jParent.add(redBorderPanels{compIdx}); + jParent.setComponentZOrder(redBorderPanels{compIdx},0); + else + jParent.remove(redBorderPanels{compIdx}); + end + jParent.repaint + end + end + catch + % never mind - try the next component (if any) + end + end + drawnow; + end + catch + % never mind... + dispError; + end + return; % debug point + end % flashComponent + +%% Select tree node + function nodeSelected(src, evd, tree) %#ok + try + %nodeHandle = evd.getCurrentNode.getUserObject; + nodedata = get(evd.getCurrentNode,'userdata'); + nodeHandle = nodedata.obj; + userdata = get(src,'userdata'); + if ~isempty(nodeHandle) && ~isempty(userdata) + numSelections = userdata.jTree.getSelectionCount; + selectionPaths = userdata.jTree.getSelectionPaths; + if (numSelections == 1) + % Indicate that the user has modified the initial selection (except if this was an initial auto-selected node) + if ~userdata.inInit + userdata.userSelected = true; + set(src,'userdata',userdata); + end + + % Update the fully-qualified class name label + numInitialIdx = length(userdata.initialIdx); + thisHandle = nodeHandle; + try + if ~ishghandle(thisHandle) + thisHandle = java(nodeHandle); + end + catch + % never mind... + end + if ~userdata.inInit || (numInitialIdx == 1) + userdata.classNameLabel.setText([' ' char(thisHandle.class)]); + else + userdata.classNameLabel.setText([' ' num2str(numInitialIdx) 'x handles (some handles hidden by unexpanded tree nodes)']); + end + if ishghandle(thisHandle) + userdata.classNameLabel.setText(userdata.classNameLabel.getText.concat(' (HG handle)')); + end + userdata.inspectorPane.dispose; % remove props listeners - doesn't work... + updateNodeTooltip(nodeHandle, userdata.classNameLabel); + + % Update the data properties inspector pane + % Note: we can't simply use the evd nodeHandle, because this node might have been DE-selected with only one other node left selected... + %nodeHandle = selectionPaths(1).getLastPathComponent.getUserObject; + nodedata = get(selectionPaths(1).getLastPathComponent,'userdata'); + nodeHandle = nodedata.obj; + %identifiers = disableDbstopError; % "dbstop if error" causes inspect.m to croak due to a bug - so workaround + userdata.inspectorPane.setObject(thisHandle); + + % Update the callbacks table + try + stripStdCbsFlag = getappdata(userdata.callbacksTable,'hideStdCbs'); + [cbData, cbHeaders, cbTableEnabled] = getCbsData(nodeHandle, stripStdCbsFlag); %#ok cbTableEnabled unused + callbacksTableModel = javax.swing.table.DefaultTableModel(cbData,cbHeaders); + set(callbacksTableModel, 'TableChangedCallback',@tbCallbacksChanged, 'UserData',nodeHandle); + userdata.callbacksTable.setModel(callbacksTableModel) + userdata.callbacksTable.setRowAutoResizes(true); + userdata.jideTableUtils.autoResizeAllColumns(userdata.callbacksTable); + catch + % never mind... + dispError + end + pause(0.005); + drawnow; + %restoreDbstopError(identifiers); + + % Highlight the selected object (if visible) + flashComponent(nodeHandle,0.2,3); + + elseif (numSelections > 1) % Multiple selections + + % Get the list of all selected nodes + jArray = javaArray('java.lang.Object', numSelections); + toolTipStr = ''; + sameClassFlag = true; + for idx = 1 : numSelections + %jArray(idx) = selectionPaths(idx).getLastPathComponent.getUserObject; + nodedata = get(selectionPaths(idx).getLastPathComponent,'userdata'); + try + jArray(idx) = java(nodedata.obj); + catch + jArray(idx) = nodedata.obj; + end + toolTipStr = [toolTipStr ' ' jArray(idx).class ' ']; %#ok grow + if (idx < numSelections), toolTipStr = [toolTipStr '
']; end %#ok grow + if (idx > 1) && sameClassFlag && ~isequal(jArray(idx).getClass,jArray(1).getClass) + sameClassFlag = false; + end + end + toolTipStr = [toolTipStr '']; + + % Update the fully-qualified class name label + if sameClassFlag + classNameStr = jArray(1).class; + else + classNameStr = 'handle'; + end + if all(ishghandle(jArray)) + if strcmp(classNameStr,'handle') + classNameStr = 'HG handles'; + else + classNameStr = [classNameStr ' (HG handles)']; + end + end + classNameStr = [' ' num2str(numSelections) 'x ' classNameStr]; + userdata.classNameLabel.setText(classNameStr); + userdata.classNameLabel.setToolTipText(toolTipStr); + + % Update the data properties inspector pane + %identifiers = disableDbstopError; % "dbstop if error" causes inspect.m to croak due to a bug - so workaround + userdata.inspectorPane.getRegistry.setSelected(jArray, true); + + % Update the callbacks table + try + % Get intersecting callback names & values + stripStdCbsFlag = getappdata(userdata.callbacksTable,'hideStdCbs'); + [cbData, cbHeaders, cbTableEnabled] = getCbsData(jArray(1), stripStdCbsFlag); %#ok cbHeaders & cbTableEnabled unused + if ~isempty(cbData) + cbNames = cbData(:,1); + for idx = 2 : length(jArray) + [cbData2, cbHeaders2] = getCbsData(jArray(idx), stripStdCbsFlag); %#ok cbHeaders2 unused + if ~isempty(cbData2) + newCbNames = cbData2(:,1); + [cbNames, cbIdx, cb2Idx] = intersect(cbNames,newCbNames); %#ok cb2Idx unused + cbData = cbData(cbIdx,:); + for cbIdx = 1 : length(cbNames) + newIdx = find(strcmp(cbNames{cbIdx},newCbNames)); + if ~isequal(cbData2{newIdx,2}, cbData{cbIdx,2}) + cbData{cbIdx,2} = ''; + end + end + else + cbData = cbData([],:); %=empty cell array + end + if isempty(cbData) + break; + end + end + end + cbHeaders = {'Callback name','Callback value'}; + callbacksTableModel = javax.swing.table.DefaultTableModel(cbData,cbHeaders); + set(callbacksTableModel, 'TableChangedCallback',@tbCallbacksChanged, 'UserData',jArray); + userdata.callbacksTable.setModel(callbacksTableModel) + userdata.callbacksTable.setRowAutoResizes(true); + userdata.jideTableUtils.autoResizeAllColumns(userdata.callbacksTable); + catch + % never mind... + dispError + end + + pause(0.005); + drawnow; + %restoreDbstopError(identifiers); + + % Highlight the selected objects (if visible) + flashComponent(jArray,0.2,3); + end + + % TODO: Auto-highlight selected object (?) + %nodeHandle.requestFocus; + end + catch + dispError + end + end + +%% IFF utility function for annonymous cellfun funcs + function result = iff(test,trueVal,falseVal) %#ok + try + if test + result = trueVal; + else + result = falseVal; + end + catch + result = false; + end + end + +%% Get an HTML representation of the object's properties + function dataFieldsStr = getPropsHtml(nodeHandle, dataFields) + try + % Get a text representation of the fieldnames & values + dataFieldsStr = ''; % just in case the following croaks... + if isempty(dataFields) + return; + end + dataFieldsStr = evalc('disp(dataFields)'); + if dataFieldsStr(end)==char(10), dataFieldsStr=dataFieldsStr(1:end-1); end + + % Strip out callbacks + dataFieldsStr = regexprep(dataFieldsStr,'^\s*\w*Callback(Data)?:[^\n]*$','','lineanchors'); + dataFieldsStr = regexprep(dataFieldsStr,'\n\n','\n'); + + % HTMLize tooltip data + % First, set the fields' font based on its read-write status + nodeHandle = handle(nodeHandle); % ensure this is a Matlab handle, not a java object + fieldNames = fieldnames(dataFields); + undefinedStr = ''; + for fieldIdx = 1 : length(fieldNames) + thisFieldName = fieldNames{fieldIdx}; + accessFlags = get(findprop(nodeHandle,thisFieldName),'AccessFlags'); + if isfield(accessFlags,'PublicSet') && strcmp(accessFlags.PublicSet,'on') + % Bolden read/write fields + thisFieldFormat = ['' thisFieldName ':$2']; + elseif ~isfield(accessFlags,'PublicSet') + % Undefined - probably a Matlab-defined field of com.mathworks.hg.peer.FigureFrameProxy... + thisFieldFormat = ['' thisFieldName ':$2']; + undefinedStr = ', undefined'; + else % PublicSet=='off' + % Gray-out & italicize any read-only fields + thisFieldFormat = ['' thisFieldName ':$2']; + end + dataFieldsStr = regexprep(dataFieldsStr, ['([\s\n])' thisFieldName ':([^\n]*)'], ['$1' thisFieldFormat]); + end + catch + % never mind... - probably an ambiguous property name + %dispError + end + + % Method 1: simple
list + %dataFieldsStr = strrep(dataFieldsStr,char(10),' 
  '); + + % Method 2: 2x2-column + dataFieldsStr = regexprep(dataFieldsStr, '^\s*([^:]+:)([^\n]*)\n^\s*([^:]+:)([^\n]*)$', '', 'lineanchors'); + dataFieldsStr = regexprep(dataFieldsStr, '^[^<]\s*([^:]+:)([^\n]*)$', '', 'lineanchors'); + dataFieldsStr = ['(modifiable' undefinedStr ' & read-only fields)

  

 $1 $2    $3 $4 
 $1 $2  
' dataFieldsStr '
']; + end + +%% Update tooltip string with a node's data + function updateNodeTooltip(nodeHandle, uiObject) + try + toolTipStr = nodeHandle.class; + dataFieldsStr = ''; + + % Add HG annotation if relevant + if ishghandle(nodeHandle) + hgStr = ' HG Handle'; + else + hgStr = ''; + end + + % Note: don't bulk-get because (1) not all properties are returned & (2) some properties cause a Java exception + % Note2: the classhandle approach does not enable access to user-defined schema.props + ch = classhandle(handle(nodeHandle)); + dataFields = []; + [sortedNames, sortedIdx] = sort(get(ch.Properties,'Name')); + for idx = 1 : length(sortedIdx) + sp = ch.Properties(sortedIdx(idx)); + % TODO: some fields (see EOL comment below) generate a Java Exception from: com.mathworks.mlwidgets.inspector.PropertyRootNode$PropertyListener$1$1.run + if strcmp(sp.AccessFlags.PublicGet,'on') % && ~any(strcmp(sp.Name,{'FixedColors','ListboxTop','Extent'})) + try + dataFields.(sp.Name) = get(nodeHandle, sp.Name); + catch + dataFields.(sp.Name) = 'Error!'; + end + else + dataFields.(sp.Name) = '(no public getter method)'; + end + end + dataFieldsStr = getPropsHtml(nodeHandle, dataFields); + catch + % Probably a non-HG java object + try + % Note: the bulk-get approach enables access to user-defined schema-props, but not to some original classhandle Properties... + dataFields = get(nodeHandle); + dataFieldsStr = getPropsHtml(nodeHandle, dataFields); + catch + % Probably a missing property getter implementation + try + % Inform the user - bail out on error + err = lasterror; + dataFieldsStr = ['

' strrep(err.message, char(10), '
')]; + catch + % forget it... + end + end + end + + % Set the object tooltip + if ~isempty(dataFieldsStr) + toolTipStr = [' ' char(toolTipStr) '' hgStr ': ' dataFieldsStr '']; + end + uiObject.setToolTipText(toolTipStr); + end + +%% Expand tree node + function nodeExpanded(src, evd, tree) %#ok + % tree = handle(src); + % evdsrc = evd.getSource; + evdnode = evd.getCurrentNode; + + if ~tree.isLoaded(evdnode) + + % Get the list of children TreeNodes + childnodes = getChildrenNodes(tree, evdnode); + + % Add the HG sub-tree (unless already included in the first tree) + childHandle = getappdata(evdnode.handle,'childHandle'); %=evdnode.getUserObject + if evdnode.isRoot && ~isempty(hg_handles) && ~isequal(hg_handles(1).java, childHandle) + childnodes = [childnodes, getChildrenNodes(tree, evdnode, true)]; + end + + % If we have a single child handle, wrap it within a javaArray for tree.add() to "swallow" + if (length(childnodes) == 1) + chnodes = childnodes; + childnodes = javaArray('com.mathworks.hg.peer.UITreeNode', 1); + childnodes(1) = java(chnodes); + end + + % Add child nodes to the current node + tree.add(evdnode, childnodes); + tree.setLoaded(evdnode, true); + end + end + +%% Get an icon image no larger than 16x16 pixels + function iconImage = setIconSize(iconImage) + try + iconWidth = iconImage.getWidth; + iconHeight = iconImage.getHeight; + if iconWidth > 16 + newHeight = fix(iconHeight * 16 / iconWidth); + iconImage = iconImage.getScaledInstance(16,newHeight,iconImage.SCALE_SMOOTH); + elseif iconHeight > 16 + newWidth = fix(iconWidth * 16 / iconHeight); + iconImage = iconImage.getScaledInstance(newWidth,16,iconImage.SCALE_SMOOTH); + end + catch + % never mind... - return original icon + end + end % setIconSize + +%% Get list of children nodes + function nodes = getChildrenNodes(tree, parentNode, isRootHGNode) + try + iconpath = [matlabroot, '/toolbox/matlab/icons/']; + nodes = handle([]); + userdata = get(tree,'userData'); + hdls = userdata.handles; + nodedata = get(parentNode,'userdata'); + if nargin < 3 + %isJavaNode = ~ishghandle(parentNode.getUserObject); + isJavaNode = ~ishghandle(nodedata.obj); + isRootHGNode = false; + else + isJavaNode = ~isRootHGNode; + end + + % Search for this parent node in the list of all nodes + parents = userdata.parents; + nodeIdx = nodedata.idx; + + if isJavaNode && isempty(nodeIdx) % Failback, in case userdata doesn't work for some reason... + for hIdx = 1 : length(hdls) + %if isequal(handle(parentNode.getUserObject), hdls(hIdx)) + if isequal(handle(nodedata.obj), hdls(hIdx)) + nodeIdx = hIdx; + break; + end + end + end + if ~isJavaNode + if isRootHGNode % =root HG node + thisChildHandle = userdata.hg_handles(1); + childName = getNodeName(thisChildHandle); + hasGrandChildren = any(parents==1); + icon = []; + if hasGrandChildren && length(hg_handles)>1 + childName = childName.concat(' - HG root container'); + icon = [iconpath 'figureicon.gif']; + end + try + nodes = uitreenode('v0', thisChildHandle, childName, icon, ~hasGrandChildren); + catch % old matlab version don't have the 'v0' option + try + nodes = uitreenode(thisChildHandle, childName, icon, ~hasGrandChildren); + catch + % probably an invalid handle - ignore... + end + end + + % Add the handler to the node's internal data + % Note: could also use 'userdata', but setUserObject() is recommended for TreeNodes + % Note2: however, setUserObject() sets a java *BeenAdapter object for HG handles instead of the required original class, so use setappdata + %nodes.setUserObject(thisChildHandle); + setappdata(nodes,'childHandle',thisChildHandle); + nodedata.idx = 1; + nodedata.obj = thisChildHandle; + set(nodes,'userdata',nodedata); + return; + else % non-root HG node + parents = userdata.hg_parents; + hdls = userdata.hg_handles; + end % if isRootHGNode + end % if ~isJavaNode + + % If this node was found, get the list of its children + if ~isempty(nodeIdx) + %childIdx = setdiff(find(parents==nodeIdx),nodeIdx); + childIdx = find(parents==nodeIdx); + childIdx(childIdx==nodeIdx) = []; % faster... + numChildren = length(childIdx); + for cIdx = 1 : numChildren + thisChildIdx = childIdx(cIdx); + thisChildHandle = hdls(thisChildIdx); + childName = getNodeName(thisChildHandle); + try + visible = thisChildHandle.Visible; + if visible + try visible = thisChildHandle.Width > 0; catch, end %#ok + end + if ~visible + childName = ['' char(childName) '']; %#ok grow + end + catch + % never mind... + end + hasGrandChildren = any(parents==thisChildIdx); + try + isaLabel = isa(thisChildHandle.java,'javax.swing.JLabel'); + catch + isaLabel = 0; + end + if hasGrandChildren && ~any(strcmp(thisChildHandle.class,{'axes'})) + icon = [iconpath 'foldericon.gif']; + elseif isaLabel + icon = [iconpath 'tool_text.gif']; + else + icon = []; + end + try + nodes(cIdx) = uitreenode('v0', thisChildHandle, childName, icon, ~hasGrandChildren); + catch % old matlab version don't have the 'v0' option + try + nodes(cIdx) = uitreenode(thisChildHandle, childName, icon, ~hasGrandChildren); + catch + % probably an invalid handle - ignore... + end + end + + % Use existing object icon, if available + try + setTreeNodeIcon(nodes(cIdx),thisChildHandle); + catch + % probably an invalid handle - ignore... + end + + % Pre-select the node based upon the user's FINDJOBJ filters + try + if isJavaNode && ~userdata.userSelected && any(userdata.initialIdx == thisChildIdx) + pause(0.0002); % as short as possible... + drawnow; + if isempty(tree.getSelectedNodes) + tree.setSelectedNode(nodes(cIdx)); + else + newSelectedNodes = [tree.getSelectedNodes, nodes(cIdx).java]; + tree.setSelectedNodes(newSelectedNodes); + end + end + catch + % never mind... + end + + % Add the handler to the node's internal data + % Note: could also use 'userdata', but setUserObject() is recommended for TreeNodes + % Note2: however, setUserObject() sets a java *BeenAdapter object for HG handles instead of the required original class, so use setappdata + % Note3: the following will error if invalid handle - ignore + try + if isJavaNode + thisChildHandle = thisChildHandle.java; + end + %nodes(cIdx).setUserObject(thisChildHandle); + setappdata(nodes(cIdx),'childHandle',thisChildHandle); + nodedata.idx = thisChildIdx; + nodedata.obj = thisChildHandle; + set(nodes(cIdx),'userdata',nodedata); + catch + % never mind (probably an invalid handle) - leave unchanged (like a leaf) + end + end + end + catch + % Never mind - leave unchanged (like a leaf) + %error('YMA:findjobj:UnknownNodeType', 'Error expanding component tree node'); + dispError + end + end + +%% Get a node's name + function [nodeName, nodeTitle] = getNodeName(hndl) + try + % Initialize (just in case one of the succeding lines croaks) + nodeName = ''; + if ~ismethod(hndl,'getClass') + try + nodeName = hndl.class; + catch + nodeName = hndl.type; % last-ditch try... + end + else + nodeName = hndl.getClass.getSimpleName; + end + + % Strip away the package name, leaving only the regular classname + if ~isempty(nodeName) && ischar(nodeName) + nodeName = java.lang.String(nodeName); + nodeName = nodeName.substring(nodeName.lastIndexOf('.')+1); + end + if (nodeName.length == 0) + % fix case of anonymous internal classes, that do not have SimpleNames + try + nodeName = hndl.getClass.getName; + nodeName = nodeName.substring(nodeName.lastIndexOf('.')+1); + catch + % never mind - leave unchanged... + end + end + + % Get any unique identifying string (if available in one of several fields) + labelsToCheck = {'label','title','text','string','displayname','toolTipText','TooltipString','actionCommand','name','Tag','style','UIClassID'}; + nodeTitle = ''; + strField = ''; %#ok - used for debugging + while ((~isa(nodeTitle,'java.lang.String') && ~ischar(nodeTitle)) || isempty(nodeTitle)) && ~isempty(labelsToCheck) + try + nodeTitle = get(hndl,labelsToCheck{1}); + strField = labelsToCheck{1}; %#ok - used for debugging + catch + % never mind - probably missing prop, so skip to next one + end + labelsToCheck(1) = []; + end + if length(nodeTitle) ~= numel(nodeTitle) + % Multi-line - convert to a long single line + nodeTitle = nodeTitle'; + nodeTitle = nodeTitle(:)'; + end + extraStr = regexprep(nodeTitle,{sprintf('(.{25,%d}).*',min(25,length(nodeTitle)-1)),' +'},{'$1...',' '},'once'); + if ~isempty(extraStr) + if ischar(extraStr) + nodeName = nodeName.concat(' (''').concat(extraStr).concat(''')'); + else + nodeName = nodeName.concat(' (').concat(num2str(extraStr)).concat(')'); + end + %nodeName = nodeName.concat(strField); + end + catch + % Never mind - use whatever we have so far + %dispError + end + end + +%% Strip standard Swing callbacks from a list of events + function evNames = stripStdCbs(evNames) + try + stdEvents = {'AncestorAdded', 'AncestorMoved', 'AncestorRemoved', 'AncestorResized', ... + 'CaretPositionChanged', 'ComponentAdded', 'ComponentRemoved', ... + 'ComponentHidden', 'ComponentMoved', 'ComponentResized', 'ComponentShown', ... + 'PropertyChange', 'FocusGained', 'FocusLost', ... + 'HierarchyChanged', 'InputMethodTextChanged', ... + 'KeyPressed', 'KeyReleased', 'KeyTyped', ... + 'MouseClicked', 'MouseDragged', 'MouseEntered', 'MouseExited', ... + 'MouseMoved', 'MousePressed', 'MouseReleased', 'MouseWheelMoved', ... + 'VetoableChange'}; + stdEvents = [stdEvents, strcat(stdEvents,'Callback'), strcat(stdEvents,'Fcn')]; + evNames = setdiff(evNames,stdEvents)'; + catch + % Never mind... + dispError + end + end + +%% Callback function for checkbox + function cbHideStdCbs_Callback(src, evd, varargin) %#ok + try + % Store the current checkbox value for later use + callbacksTable = get(src,'userdata'); + if evd.getSource.isSelected + setappdata(callbacksTable,'hideStdCbs',1); + else + setappdata(callbacksTable,'hideStdCbs',[]); + end + + % Rescan the current node + userdata = get(callbacksTable,'userdata'); + ed.getCurrentNode = userdata.jTree.getSelectionModel.getSelectionPath.getLastPathComponent; + nodeSelected(userdata.jTreePeer,ed,[]); + catch + % Never mind... + dispError + end + end + +%% Callback function for button + function btWebsite_Callback(src, evd, varargin) %#ok + try + web('http://UndocumentedMatlab.com/'); + catch + % Never mind... + dispError + end + end + +%% Callback function for button + function btRefresh_Callback(src, evd, varargin) %#ok + try + % Set cursor shape to hourglass until we're done + hTreeFig = varargin{2}; + set(hTreeFig,'Pointer','watch'); + drawnow; + object = varargin{1}; + + % Re-invoke this utility to re-scan the container for all children + findjobj(object); + catch + % Never mind... + end + + % Restore default cursor shape + set(hTreeFig,'Pointer','arrow'); + end + +%% Callback function for button + function btExport_Callback(src, evd, varargin) %#ok + try + % Get the list of all selected nodes + jTree = varargin{1}; + numSelections = jTree.getSelectionCount; + selectionPaths = jTree.getSelectionPaths; + hdls = handle([]); + for idx = 1 : numSelections + %hdls(idx) = handle(selectionPaths(idx).getLastPathComponent.getUserObject); + nodedata = get(selectionPaths(idx).getLastPathComponent,'userdata'); + hdls(idx) = handle(nodedata.obj); + end + + % Assign the handles in the base workspace & inform user + assignin('base','findjobj_hdls',hdls); + classNameLabel = varargin{2}; + msg = ['Exported ' char(classNameLabel.getText.trim) ' to base workspace variable findjobj_hdls']; + msgbox(msg,'FindJObj','help'); + catch + % Never mind... + dispError + end + end + +%% Callback function for button + function btFocus_Callback(src, evd, varargin) %#ok + try + % Request focus for the specified object + object = getTopSelectedObject(varargin{:}); + object.requestFocus; + catch + try + object = object.java.getPeer.requestFocus; + object.requestFocus; + catch + % Never mind... + %dispError + end + end + end + +%% Callback function for button + function btInspect_Callback(src, evd, varargin) %#ok + try + % Inspect the specified object + object = getTopSelectedObject(varargin{:}); + if isempty(which('uiinspect')) + + % If the user has not indicated NOT to be informed about UIInspect + if ~ispref('FindJObj','dontCheckUIInspect') + + % Ask the user whether to download UIINSPECT (YES, no, no & don't ask again) + answer = questdlg({'The object inspector requires UIINSPECT from the MathWorks File Exchange. UIINSPECT was created by Yair Altman, like this FindJObj utility.','','Download & install UIINSPECT?'},'UIInspect','Yes','No','No & never ask again','Yes'); + switch answer + case 'Yes' % => Yes: download & install + try + % Download UIINSPECT + baseUrl = 'http://www.mathworks.com/matlabcentral/fileexchange/17935'; + fileUrl = [baseUrl '?controller=file_infos&download=true']; + file = urlread(fileUrl); + file = regexprep(file,[char(13),char(10)],'\n'); %convert to OS-dependent EOL + + % Install... + newPath = fullfile(fileparts(which(mfilename)),'uiinspect.m'); + fid = fopen(newPath,'wt'); + fprintf(fid,'%s',file); + fclose(fid); + catch + % Error downloading: inform the user + msgbox(['Error in downloading: ' lasterr], 'UIInspect', 'warn'); + web(baseUrl); + end + + % ...and now run it... + %pause(0.1); + drawnow; + dummy = which('uiinspect'); %#ok used only to load into memory + uiinspect(object); + return; + + case 'No & never ask again' % => No & don't ask again + setpref('FindJObj','dontCheckUIInspect',1); + + otherwise + % forget it... + end + end + drawnow; + + % No UIINSPECT available - run the good-ol' METHODSVIEW()... + methodsview(object); + else + uiinspect(object); + end + catch + try + if isjava(object) + methodsview(object) + else + methodsview(object.java); + end + catch + % Never mind... + dispError + end + end + end + +%% Callback function for button + function btCheckFex_Callback(src, evd, varargin) %#ok + try + % Check the FileExchange for the latest version + web('http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=14317'); + catch + % Never mind... + dispError + end + end + +%% Check for existence of a newer version + function checkVersion() + try + % If the user has not indicated NOT to be informed + if ~ispref('FindJObj','dontCheckNewerVersion') + + % Get the latest version date from the File Exchange webpage + baseUrl = 'http://www.mathworks.com/matlabcentral/fileexchange/'; + webUrl = [baseUrl '14317']; % 'loadFile.do?objectId=14317']; + webPage = urlread(webUrl); + modIdx = strfind(webPage,'>Updates<'); + if ~isempty(modIdx) + webPage = webPage(modIdx:end); + % Note: regexp hangs if substr not found, so use strfind instead... + %latestWebVersion = regexprep(webPage,'.*?>(20[\d-]+).*','$1'); + dateIdx = strfind(webPage,'class="date">'); + if ~isempty(dateIdx) + latestDate = webPage(dateIdx(end)+13 : dateIdx(end)+23); + try + startIdx = dateIdx(end)+27; + descStartIdx = startIdx + strfind(webPage(startIdx:startIdx+999),''); + descEndIdx = startIdx + strfind(webPage(startIdx:startIdx+999),''); + descStr = webPage(descStartIdx(1)+3 : descEndIdx(1)-2); + catch + descStr = ''; + end + + % Get this file's latest date + thisFileName = which(mfilename); %#ok + try + thisFileData = dir(thisFileName); + try + thisFileDatenum = thisFileData.datenum; + catch % old ML versions... + thisFileDatenum = datenum(thisFileData.date); + end + catch + thisFileText = evalc('type(thisFileName)'); + thisFileLatestDate = regexprep(thisFileText,'.*Change log:[\s%]+([\d-]+).*','$1'); + thisFileDatenum = datenum(thisFileLatestDate,'yyyy-mm-dd'); + end + + % If there's a newer version on the File Exchange webpage (allow 2 days grace period) + if (thisFileDatenum < datenum(latestDate,'dd mmm yyyy')-2) + + % Ask the user whether to download the newer version (YES, no, no & don't ask again) + msg = {['A newer version (' latestDate ') of FindJObj is available on the MathWorks File Exchange:'], '', ... + ['\color{blue}' descStr '\color{black}'], '', ... + 'Download & install the new version?'}; + createStruct.Interpreter = 'tex'; + createStruct.Default = 'Yes'; + answer = questdlg(msg,'FindJObj','Yes','No','No & never ask again',createStruct); + switch answer + case 'Yes' % => Yes: download & install newer file + try + %fileUrl = [baseUrl '/download.do?objectId=14317&fn=findjobj&fe=.m']; + fileUrl = [baseUrl '/14317?controller=file_infos&download=true']; + file = urlread(fileUrl); + file = regexprep(file,[char(13),char(10)],'\n'); %convert to OS-dependent EOL + fid = fopen(thisFileName,'wt'); + fprintf(fid,'%s',file); + fclose(fid); + catch + % Error downloading: inform the user + msgbox(['Error in downloading: ' lasterr], 'FindJObj', 'warn'); + web(webUrl); + end + case 'No & never ask again' % => No & don't ask again + setpref('FindJObj','dontCheckNewerVersion',1); + otherwise + % forget it... + end + end + end + else + % Maybe webpage not fully loaded or changed format - bail out... + end + end + catch + % Never mind... + end + end + +%% Get the first selected object (might not be the top one - depends on selection order) + function object = getTopSelectedObject(jTree, root) + try + object = []; + numSelections = jTree.getSelectionCount; + if numSelections > 0 + % Get the first object specified + %object = jTree.getSelectionPath.getLastPathComponent.getUserObject; + nodedata = get(jTree.getSelectionPath.getLastPathComponent,'userdata'); + else + % Get the root object (container) + %object = root.getUserObject; + nodedata = get(root,'userdata'); + end + object = nodedata.obj; + catch + % Never mind... + dispError + end + end + +%% Update component callback upon callbacksTable data change + function tbCallbacksChanged(src, evd) + try + % exit if invalid handle or already in Callback + if ~ishandle(src) || ~isempty(getappdata(src,'inCallback')) % || length(dbstack)>1 %exit also if not called from user action + return; + end + setappdata(src,'inCallback',1); % used to prevent endless recursion + + % Update the object's callback with the modified value + modifiedColIdx = evd.getColumn; + modifiedRowIdx = evd.getFirstRow; + if modifiedColIdx==1 && modifiedRowIdx>=0 %sanity check - should always be true + table = evd.getSource; + object = get(src,'userdata'); + cbName = strtrim(table.getValueAt(modifiedRowIdx,0)); + try + cbValue = strtrim(char(table.getValueAt(modifiedRowIdx,1))); + if ~isempty(cbValue) && ismember(cbValue(1),'{[@''') + cbValue = eval(cbValue); + end + if (~ischar(cbValue) && ~isa(cbValue, 'function_handle') && (iscom(object(1)) || iscell(cbValue))) + revertCbTableModification(table, modifiedRowIdx, modifiedColIdx, cbName, object, ''); + else + for objIdx = 1 : length(object) + if ~iscom(object(objIdx)) + set(object(objIdx), cbName, cbValue); + else + cbs = object(objIdx).eventlisteners; + if ~isempty(cbs) + cbs = cbs(strcmpi(cbs(:,1),cbName),:); + object(objIdx).unregisterevent(cbs); + end + if ~isempty(cbValue) + object(objIdx).registerevent({cbName, cbValue}); + end + end + end + end + catch + revertCbTableModification(table, modifiedRowIdx, modifiedColIdx, cbName, object, lasterr) + end + end + catch + % never mind... + end + setappdata(src,'inCallback',[]); % used to prevent endless recursion + end + +%% Revert Callback table modification + function revertCbTableModification(table, modifiedRowIdx, modifiedColIdx, cbName, object, errMsg) %#ok + try + % Display a notification MsgBox + msg = 'Callbacks must be a ''string'', or a @function handle'; + if ~iscom(object(1)), msg = [msg ' or a {@func,args...} construct']; end + if ~isempty(errMsg), msg = {errMsg, '', msg}; end + msgbox(msg, ['Error setting ' cbName ' callback'], 'warn'); + + % Revert to the current value + curValue = ''; + try + if ~iscom(object(1)) + curValue = charizeData(get(object(1),cbName)); + else + cbs = object(1).eventlisteners; + if ~isempty(cbs) + cbs = cbs(strcmpi(cbs(:,1),cbName),:); + curValue = charizeData(cbs(1,2)); + end + end + catch + % never mind... - clear the current value + end + table.setValueAt(curValue, modifiedRowIdx, modifiedColIdx); + catch + % never mind... + end + end % revertCbTableModification + +%% Traverse an HG container hierarchy and extract the HG elements within + function traverseHGContainer(hcontainer,level,parent) + try + % Record the data for this node + thisIdx = length(hg_levels) + 1; + hg_levels(thisIdx) = level; + hg_parentIdx(thisIdx) = parent; + hg_handles(thisIdx) = handle(hcontainer); + parentId = length(hg_parentIdx); + + % Now recursively process all this node's children (if any) + %if ishghandle(hcontainer) + try % try-catch is faster than checking ishghandle(hcontainer)... + allChildren = allchild(handle(hcontainer)); + for childIdx = 1 : length(allChildren) + traverseHGContainer(allChildren(childIdx),level+1,parentId); + end + catch + % do nothing - probably not a container + %dispError + end + + % TODO: Add axis (plot) component handles + catch + % forget it... + end + end + +%% Debuggable "quiet" error-handling + function dispError + err = lasterror; + msg = err.message; + for idx = 1 : length(err.stack) + filename = err.stack(idx).file; + if ~isempty(regexpi(filename,mfilename)) + funcname = err.stack(idx).name; + line = num2str(err.stack(idx).line); + msg = [msg ' at ' funcname ' line #' line '']; %#ok grow + break; + end + end + disp(msg); + return; % debug point + end + +end % FINDJOBJ + + +%% TODO TODO TODO +%{ +- Enh: Improve performance - esp. expandNode() (performance solved in non-nteractive mode) +- Enh: Add property listeners - same problem in MathWork's inspect.m +- Enh: Display additional properties - same problem in MathWork's inspect.m +- Enh: Add axis (plot, Graphics) component handles +- Enh: Group callbacks according to the first word (up to 2nd cap letter) +- Enh: Add figure thumbnail image below the java tree (& indicate corresponding jObject when selected) +- Enh: scroll initially-selected node into view (problem because treenode has no pixel location) +- Fix: java exceptions when getting some fields (com.mathworks.mlwidgets.inspector.PropertyRootNode$PropertyListener$1$1.run) +- Fix: use EDT if available (especially in flashComponent) +%} \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/qmincon.m b/lwrserv/matlab/rvctools/robot/@SerialLink/qmincon.m new file mode 100644 index 0000000..6331863 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/qmincon.m @@ -0,0 +1,102 @@ +%SerialLink.QMINCON Use redundancy to avoid joint limits +% +% QS = R.qmincon(Q) exploits null space motion and returns a set of joint +% angles QS (1xN) that result in the same end-effector pose but are away +% from the joint coordinate limits. N is the number of robot joints. +% +% [Q,ERR] = R.qmincon(Q) as above but also returns ERR which is the +% scalar final value of the objective function. +% +% [Q,ERR,EXITFLAG] = R.qmincon(Q) as above but also returns the +% status EXITFLAG from fmincon. +% +% Trajectory operation:: +% +% In all cases if Q is MxN it is taken as a pose sequence and R.qmincon() +% returns the adjusted joint coordinates (MxN) corresponding to each of the +% poses in the sequence. +% +% ERR and EXITFLAG are also Mx1 and indicate the results of optimisation +% for the corresponding trajectory step. +% +% Notes:: +% - Requires fmincon from the Optimization Toolbox. +% - Robot must be redundant. +% +% Author:: +% Bryan Moutrie +% +% See also SerialLink.ikcon, SerialLink.ikunc, SerialLink.jacob0. + +% Copyright (C) Bryan Moutrie, 2013-2015 +% Licensed under the GNU Lesser General Public License +% see full file for full statement +% +% LICENSE STATEMENT: +% +% This file is part of pHRIWARE. +% +% pHRIWARE 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. +% +% pHRIWARE 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 General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public +% License along with pHRIWARE. If not, see . + +function [qstar, error, exitflag] = qmincon(robot, q) + + % check if Optimization Toolbox exists, we need it + if ~exist('fmincon') + error('rtb:qmincon:nosupport', 'Optimization Toolbox required'); + end + M = size(q,1); + n = robot.n; + + qstar = zeros(M,n); + error = zeros(M,1); + exitflag = zeros(M,1); + + opt = optimoptions('fmincon', ... + 'Algorithm', 'active-set', ... + 'Display', 'off'); + + lb = robot.qlim(:,1); + ub = robot.qlim(:,2); + + x_m = 0; % Little trick for setting x0 in first iteration of loop + + for m = 1:M + q_m = q(m,:); + + J = robot.jacobn(q(m,:)); + N = null(J); + + if isempty(N) + error('rtb:qmincon:badarg', 'pHRIWARE:Robot is not redundant'); + end + + f = @(x) sumsqr((2*(N*x + q_m') - ub - lb)./(ub-lb)); + + x0 = zeros(size(N,2), 1) + x_m; + + A = [N; -N]; + b = [ub-q_m'; q_m'-lb]; + + [x_m, err_m, ef_m] = fmincon(f,x0,A,b,[],[],[],[],[],opt); + + qstar(m,:) = q(m,:) + (N*x_m)'; + error(m) = err_m; + exitflag(m) = ef_m; + end + +end + +function s = sumsqr(A) + s = sum(A.^2); +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/rne.m b/lwrserv/matlab/rvctools/robot/@SerialLink/rne.m new file mode 100644 index 0000000..4f7cef4 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/rne.m @@ -0,0 +1,79 @@ +%SerialLink.rne Inverse dynamics +% +% TAU = R.rne(Q, QD, QDD) is the joint torque required for the robot R +% to achieve the specified joint position Q, velocity QD and acceleration QDD. +% +% TAU = R.rne(Q, QD, QDD, GRAV) as above but overriding the gravitational +% acceleration vector in the robot object R. +% +% TAU = R.rne(Q, QD, QDD, GRAV, FEXT) as above but specifying a wrench +% acting on the end of the manipulator which is a 6-vector [Fx Fy Fz Mx My Mz]. +% +% TAU = R.rne(X) as above where X=[Q,QD,QDD]. +% +% TAU = R.rne(X, GRAV) as above but overriding the gravitational +% acceleration vector in the robot object R. +% +% TAU = R.rne(X, GRAV, FEXT) as above but specifying a wrench +% acting on the end of the manipulator which is a 6-vector [Fx Fy Fz Mx My Mz]. +% +% [TAU,WBASE] = R.rne(X, GRAV, FEXT) as above but the extra output is the +% wrench on the base. +% +% If Q,QD and QDD (MxN), or X (Mx3N) are matrices with M rows representing a +% trajectory then TAU (MxN) is a matrix with rows corresponding to each trajectory +% step. +% +% Notes:: +% - The robot base transform is ignored. +% - The torque computed contains a contribution due to armature +% inertia and joint friction. +% - RNE can be either an M-file or a MEX-file. +% - See the README file in the mex folder for details on how to configure +% MEX-file operation. +% - The M-file is a wrapper which calls either RNE_DH or RNE_MDH depending on +% the kinematic conventions used by the robot object. +% - Currently the MEX-file version does not compute WBASE. +% +% See also SerialLink.accel, SerialLink.gravload, SerialLink.inertia. + +% TODO: +% should use tb_optparse +% +% verified against MAPLE code, which is verified by examples +% + + +% 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.petercorke.com + + +function varargout = rne(robot, varargin) + + if exist('frne') && ~robot.issym() + % use the MEX-file implementation + [varargout{1:nargout}] = frne(robot, varargin{:}); + else + % use the M-file implementation + if robot.mdh == 0 + [varargout{1:nargout}] = rne_dh(robot, varargin{:}); + else + [varargout{1:nargout}] = rne_mdh(robot, varargin{:}); + end + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/rne.mexmaci64 b/lwrserv/matlab/rvctools/robot/@SerialLink/rne.mexmaci64 new file mode 100644 index 0000000..81cfecf Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/@SerialLink/rne.mexmaci64 differ diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/rne.mexw32 b/lwrserv/matlab/rvctools/robot/@SerialLink/rne.mexw32 new file mode 100644 index 0000000..683e63b Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/@SerialLink/rne.mexw32 differ diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/rne_dh.m b/lwrserv/matlab/rvctools/robot/@SerialLink/rne_dh.m new file mode 100644 index 0000000..f86df0e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/rne_dh.m @@ -0,0 +1,232 @@ + +%SERIALLINK.RNE_DH Compute inverse dynamics via recursive Newton-Euler formulation +% +% Recursive Newton-Euler for standard Denavit-Hartenberg notation. Is invoked by +% R.RNE(). +% +% See also SERIALLINK.RNE. + +% +% verified against MAPLE code, which is verified by examples +% + + + +% 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.petercorke.com +function [tau,wbase] = rne_dh(robot, a1, a2, a3, a4, a5) + + z0 = [0;0;1]; + grav = robot.gravity; % default gravity from the object + fext = zeros(6, 1); + n = robot.n; + + % check that robot object has dynamic parameters for each link + for j=1:n + link = robot.links(j); + if isempty(link.r) || isempty(link.I) || isempty(link.m) + error('dynamic parameters (m, r, I) not set in link %d', j); + end + end + + % Set debug to: + % 0 no messages + % 1 display results of forward and backward recursions + % 2 display print R and p* + debug = 0; + + if numcols(a1) == 3*n + Q = a1(:,1:n); + Qd = a1(:,n+1:2*n); + Qdd = a1(:,2*n+1:3*n); + np = numrows(Q); + if nargin >= 3, + grav = a2(:); + end + if nargin == 4 + fext = a3; + end + else + np = numrows(a1); + Q = a1; + Qd = a2; + Qdd = a3; + if numcols(a1) ~= n || numcols(Qd) ~= n || numcols(Qdd) ~= n || ... + numrows(Qd) ~= np || numrows(Qdd) ~= np + error('bad data'); + end + if nargin >= 5, + grav = a4(:); + end + if nargin == 6 + fext = a5; + end + end + + if isa(Q, 'sym') + tau(np, n) = sym(); + else + tau = zeros(np,n); + end + + for p=1:np + q = Q(p,:).'; + qd = Qd(p,:).'; + qdd = Qdd(p,:)'; + + Fm = []; + Nm = []; + pstarm = []; + Rm = []; + w = zeros(3,1); + wd = zeros(3,1); + vd = grav(:); + + % + % init some variables, compute the link rotation matrices + % + for j=1:n + link = robot.links(j); + Tj = link.A(q(j)); + if link.RP == 'R' + d = link.d; + else + d = q(j); + end + alpha = link.alpha; + pstar = [link.a; d*sin(alpha); d*cos(alpha)]; + if j == 1 + pstar = t2r(robot.base) * pstar; + Tj = robot.base * Tj; + end + pstarm(:,j) = pstar; + Rm{j} = t2r(Tj); + if debug>1 + Rm{j} + Pstarm(:,j).' + end + end + + % + % the forward recursion + % + for j=1:n + link = robot.links(j); + + Rt = Rm{j}.'; % transpose!! + pstar = pstarm(:,j); + r = link.r; + + % + % statement order is important here + % + if link.RP == 'R' + % revolute axis + wd = Rt*(wd + z0*qdd(j) + ... + cross(w,z0*qd(j))); + w = Rt*(w + z0*qd(j)); + %v = cross(w,pstar) + Rt*v; + vd = cross(wd,pstar) + ... + cross(w, cross(w,pstar)) +Rt*vd; + + else + % prismatic axis + w = Rt*w; + wd = Rt*wd; + vd = Rt*(z0*qdd(j)+vd) + ... + cross(wd,pstar) + ... + 2*cross(w,Rt*z0*qd(j)) +... + cross(w, cross(w,pstar)); + end + + %whos + vhat = cross(wd,r.') + ... + cross(w,cross(w,r.')) + vd; + F = link.m*vhat; + N = link.I*wd + cross(w,link.I*w); + Fm = [Fm F]; + Nm = [Nm N]; + + if debug + fprintf('w: '); fprintf('%.3f ', w) + fprintf('\nwd: '); fprintf('%.3f ', wd) + fprintf('\nvd: '); fprintf('%.3f ', vd) + fprintf('\nvdbar: '); fprintf('%.3f ', vhat) + fprintf('\n'); + end + end + + % + % the backward recursion + % + + fext = fext(:); + f = fext(1:3); % force/moments on end of arm + nn = fext(4:6); + + for j=n:-1:1 + link = robot.links(j); + pstar = pstarm(:,j); + + % + % order of these statements is important, since both + % nn and f are functions of previous f. + % + if j == n + R = eye(3,3); + else + R = Rm{j+1}; + end + r = link.r; + nn = R*(nn + cross(R.'*pstar,f)) + ... + cross(pstar+r.',Fm(:,j)) + ... + Nm(:,j); + f = R*f + Fm(:,j); + if debug + fprintf('f: '); fprintf('%.3f ', f) + fprintf('\nn: '); fprintf('%.3f ', nn) + fprintf('\n'); + end + + R = Rm{j}; + if link.RP == 'R' + % revolute + %tau(p,j) = nn.'*(R.'*z0) + ... + t = nn.'*(R.'*z0) + ... + link.G^2 * link.Jm*qdd(j) - ... + link.friction(qd(j)); + tau(p,j) = t; + else + % prismatic + t = f.'*(R.'*z0) + ... + link.G^2 * link.Jm*qdd(j) - ... + link.friction(qd(j)); + tau(p,j) = t; + end + end + % this last bit needs work/testing + R = Rm{1}; + nn = R*(nn); + f = R*f; + wbase = [f; nn]; + end + + if isa(tau, 'sym') + tau = simplify(tau); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/rne_mdh.m b/lwrserv/matlab/rvctools/robot/@SerialLink/rne_mdh.m new file mode 100644 index 0000000..43d2feb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/rne_mdh.m @@ -0,0 +1,210 @@ + +%SERIALLINK.RNE_MDH Compute inverse dynamics via recursive Newton-Euler formulation +% +% Recursive Newton-Euler for modified Denavit-Hartenberg notation. Is invoked by +% R.RNE(). +% +% See also SERIALLINK.RNE. + + + +% 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.petercorke.com + +function tau = rne_mdh(robot, a1, a2, a3, a4, a5) + + z0 = [0;0;1]; + grav = robot.gravity; % default gravity from the object + fext = zeros(6, 1); + + % Set debug to: + % 0 no messages + % 1 display results of forward and backward recursions + % 2 display print R and p* + debug = 0; + + n = robot.n; + if numcols(a1) == 3*n + Q = a1(:,1:n); + Qd = a1(:,n+1:2*n); + Qdd = a1(:,2*n+1:3*n); + np = numrows(Q); + if nargin >= 3, + grav = a2(:); + end + if nargin == 4 + fext = a3; + end + else + np = numrows(a1); + Q = a1; + Qd = a2; + Qdd = a3; + if numcols(a1) ~= n || numcols(Qd) ~= n || numcols(Qdd) ~= n || ... + numrows(Qd) ~= np || numrows(Qdd) ~= np + error('bad data'); + end + if nargin >= 5, + grav = a4(:); + end + if nargin == 6 + fext = a5; + end + end + + tau = zeros(np,n); + + for p=1:np + q = Q(p,:).'; + qd = Qd(p,:).'; + qdd = Qdd(p,:).'; + + Fm = []; + Nm = []; + pstarm = []; + Rm = []; + w = zeros(3,1); + wd = zeros(3,1); + vd = grav(:); + + % + % init some variables, compute the link rotation matrices + % + for j=1:n + link = robot.links(j); + Tj = link.A(q(j)); + if link.RP == 'R' + D = link.d; + else + D = q(j); + end + alpha = link.alpha; + pm = [link.a; -D*sin(alpha); D*cos(alpha)]; % (i-1) P i + if j == 1 + pm = t2r(robot.base) * pm; + Tj = robot.base * Tj; + end + Pm(:,j) = pm; + Rm{j} = t2r(Tj); + if debug>1 + Rm{j} + Pm(:,j).' + end + end + + % + % the forward recursion + % + for j=1:n + link = robot.links(j); + + R = Rm{j}.'; % transpose!! + P = Pm(:,j); + Pc = link.r; + + % + % trailing underscore means new value + % + if link.RP == 'R' + % revolute axis + w_ = R*w + z0*qd(j); + wd_ = R*wd + cross(R*w,z0*qd(j)) + z0*qdd(j); + %v = cross(w,P) + R*v; + vd_ = R * (cross(wd,P) + ... + cross(w, cross(w,P)) + vd); + + else + % prismatic axis + w_ = R*w; + wd_ = R*wd; + %v = R*(z0*qd(j) + v) + cross(w,P); + vd_ = R*(cross(wd,P) + ... + cross(w, cross(w,P)) + vd ... + ) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j); + end + % update variables + w = w_; + wd = wd_; + vd = vd_; + + vdC = cross(wd,Pc).' + ... + cross(w,cross(w,Pc)).' + vd; + F = link.m*vdC; + N = link.I*wd + cross(w,link.I*w); + Fm = [Fm F]; + Nm = [Nm N]; + if debug + fprintf('w: '); fprintf('%.3f ', w) + fprintf('\nwd: '); fprintf('%.3f ', wd) + fprintf('\nvd: '); fprintf('%.3f ', vd) + fprintf('\nvdbar: '); fprintf('%.3f ', vdC) + fprintf('\n'); + end + end + + % + % the backward recursion + % + + fext = fext(:); + f = fext(1:3); % force/moments on end of arm + nn = fext(4:6); + + for j=n:-1:1 + + % + % order of these statements is important, since both + % nn and f are functions of previous f. + % + link = robot.links(j); + + if j == n + R = eye(3,3); + P = [0;0;0]; + else + R = Rm{j+1}; + P = Pm(:,j+1); % i/P/(i+1) + end + Pc = link.r; + + f_ = R*f + Fm(:,j); + nn_ = Nm(:,j) + R*nn + cross(Pc,Fm(:,j)).' + ... + cross(P,R*f); + + f = f_; + nn = nn_; + + if debug + fprintf('f: '); fprintf('%.3f ', f) + fprintf('\nn: '); fprintf('%.3f ', nn) + fprintf('\n'); + end + if link.RP == 'R' + % revolute + tau(p,j) = nn.'*z0 + ... + link.G^2 * link.Jm*qdd(j) - ... + friction(link, qd(j)); + else + % prismatic + tau(p,j) = f.'*z0 + ... + link.G^2 * link.Jm*qdd(j) - ... + friction(link, qd(j)); + end + end + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/slink.m b/lwrserv/matlab/rvctools/robot/@SerialLink/slink.m new file mode 100644 index 0000000..61d5da4 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/slink.m @@ -0,0 +1,31 @@ +function sl = slink(l) + + sl = Link(l); % clone the link + + if sl.alpha == pi/2 + sl.alpha = sym('pi/2'); + end + if sl.alpha == -pi/2 + sl.alpha = sym('-pi/2'); + end + if sl.alpha == pi + sl.alpha = sym('pi'); + end + if sl.alpha == -pi + sl.alpha = sym('-pi'); + end + + if l.isprismatic + if sl.alpha == pi/2 + sl.alpha = sym('pi/2'); + end + if sl.alpha == -pi/2 + sl.alpha = sym('-pi/2'); + end + if sl.alpha == pi + sl.alpha = sym('pi'); + end + if sl.alpha == -pi + sl.alpha = sym('-pi'); + end + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/teach.m b/lwrserv/matlab/rvctools/robot/@SerialLink/teach.m new file mode 100644 index 0000000..9f6bfb5 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/teach.m @@ -0,0 +1,425 @@ +%SerialLink.teach Graphical teach pendant +% +% R.teach(OPTIONS) drive a graphical robot by means of a graphical slider panel. +% If no graphical robot exists one is created in a new window. Otherwise +% all current instances of the graphical robot are driven. +% +% h = R.teach(OPTIONS) as above but returns a handle for the teach window. Can +% be used to programmatically delete the teach window. +% +% Options:: +% 'eul' Display tool orientation in Euler angles +% 'rpy' Display tool orientation in roll/pitch/yaw angles +% 'approach' Display tool orientation as approach vector (z-axis) +% 'degrees' Display angles in degrees (default radians) +% 'q0',q Set initial joint coordinates +% +% GUI:: +% +% - The record button adds the current joint coordinates as a row to the robot's +% qteach property. +% - The Quit button destroys the teach window. +% +% Notes:: +% - The slider limits are derived from the joint limit properties. If not +% set then for +% - a revolute joint they are assumed to be [-pi, +pi] +% - a prismatic joint they are assumed unknown and an error occurs. +% +% See also SerialLink.plot. + +% 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.petercorke.com + +%% TODO: +%% make the sliders change the animation while moving +%% http://www.mathworks.com/matlabcentral/newsreader/view_thread/159374 +%% 1. download FINDJOBJ from the file exchange: http://www.mathworks.com/matlabcentral/fileexchange/14317 +%% 2. hSlider = uicontrol('style','slider', ...); %create the slider, get its Matlab handle +%% 3. jSlider = findjobj(hSlider,'nomenu'); %get handle of the underlying java object +%% 4. jSlider.AdjustmentValueChangedCallback = @myMatlabFunction; %set callback +%% +%% Note: you can also use the familiar format: +%% set(jSlider,'AdjustmentValueChangedCallback',@myMatlabFunction) +%% +%% Feel free to explore many other properties (and ~30 callbacks) +%% available in jSlider but not in its Matlab front-end interface hSlider. +%% +%% Warning: FINDJOBJ relies on undocumented and unsupported Matlab/Java +%% functionality. + +function handle = teach(r, varargin) + bgcol = [135 206 250]/255; + + opt.degrees = false; + opt.q0 = []; + opt.orientation = {'approach', 'eul', 'rpy'}; +% TODO: options for rpy, or eul angle display + + opt = tb_optparse(opt, varargin); + + % drivebot(r, q) + % drivebot(r, 'deg') + + n = r.n; + width = 300; + height = 40; + + qlim = r.qlim; + if any(isinf(qlim)) + error('for prismatic axes must define joint coordinate limits, set qlim properties for prismatic Links'); + end + + if isempty(opt.q0) + q = zeros(1,n); + else + q = opt.q0; + end + + % set up scale factor, from actual limits in radians/metres to display units + qscale = ones(r.n,1); + for i=1:r.n + L=r.links(i); + if opt.degrees && L.isrevolute + qscale(i) = 180/pi; + end + end + + handles.qscale = qscale; + handles.orientation = opt.orientation; + + T6 = r.fkine(q); + + fig = figure('Units', 'pixels', ... + 'Position', [0 -height width height*(n+2)+40], ... + 'BusyAction', 'cancel', ... + 'HandleVisibility', 'off', ... + 'Color', bgcol); + set(fig,'MenuBar','none') + delete( get(fig, 'Children') ) + + % first we check to see if there are any graphical robots of + % this name, if so we use them, otherwise create a robot plot. + + rhandles = findobj('Tag', r.name); + + % attempt to get current joint config of graphical robot + if ~isempty(rhandles) + h = get(rhandles(1), 'UserData'); + if ~isempty(h.q) + q = h.q; + end + end + + % now make the sliders + for i=1:n + % slider label + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n-i)+40 width*0.1 height*0.4], ... + 'String', sprintf('q%d', i)); + + % slider itself + q(i) = max( qlim(i,1), min( qlim(i,2), q(i) ) ); % clip to range + handles.slider(i) = uicontrol(fig, 'Style', 'slider', ... + 'Units', 'pixels', ... + 'Position', [width*0.1 height*(n-i)+40 width*0.7 height*0.4], ... + 'Min', qlim(i,1), ... + 'Max', qlim(i,2), ... + 'Value', q(i), ... + 'Tag', sprintf('Slider%d', i)); + + % text box showing slider value, also editable + handles.edit(i) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [width*0.8 height*(n-i-0.1)+40 width*0.2 height*0.7], ... + 'String', num2str(qscale(i)*q(i)), ... + 'Tag', sprintf('Edit%d', i)); + end + + set(handles.slider(1)) + + + % robot name text box + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'FontSize', 20, ... + 'HorizontalAlignment', 'left', ... + 'Position', [0 height*(n+1.2)+40 0.8*width 0.8*height], ... + 'BackgroundColor', 'white', ... + 'String', r.name); + + % X + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n+0.5)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'x:'); + + handles.t6.t(1) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.06*width height*(n+0.5)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(1,4)), ... + 'Tag', 'T6'); + + % Y + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.26*width height*(n+0.5)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'y:'); + + handles.t6.t(2) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.32*width height*(n+0.5)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(2,4))); + + % Z + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.52*width height*(n+0.5)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'z:'); + + handles.t6.t(3) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.58*width height*(n+0.5)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(3,4))); + + % Orientation + switch opt.orientation + case 'approach' + labels = {'ax:', 'ay:', 'az:'}; + case 'eul' + labels = {'\phi:', '\theta:', '\psi:'}; + case'rpy' + labels = {'R:', 'P:', 'Y:'}; + end + % AX + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', labels(1)); + + handles.t6.r(1) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.06*width height*(n)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(1,3))); + + % AY + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.26*width height*(n)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', labels(2)); + + handles.t6.r(2) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.32*width height*(n)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(2,3))); + + % AZ + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.52*width height*(n)+40 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', labels(2)); + + handles.t6.r(3) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.58*width height*(n)+40 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(3,3))); + + % add buttons + uicontrol(fig, 'Style', 'pushbutton', ... + 'Units', 'pixels', ... + 'FontSize', 16, ... + 'Position', [0.8*width height*n+40 0.2*width 2*height], ... + 'CallBack', @(src,event) delete(fig), ... + 'BackgroundColor', 'red', ... + 'String', 'Quit'); + % the record button + handles.record = []; + uicontrol(fig, 'Style', 'pushbutton', ... + 'Units', 'pixels', ... + 'FontSize', 15, ... + 'Position', [0.05*width 8 0.9*width 0.7*height], ... + 'CallBack', @(src,event) record_callback(r, handles), ... + 'BackgroundColor', 'blue', ... + 'ForegroundColor', 'white', ... + 'String', 'record'); + + + % now assign the callbacks + for i=1:n + % text edit box + set(handles.edit(i), ... + 'Interruptible', 'off', ... + 'Callback', @(src,event)teach_callback(src, r.name, i, handles)); + + % slider + set(handles.slider(i), ... + 'Interruptible', 'off', ... + 'BusyAction', 'queue', ... + 'Callback', @(src,event)teach_callback(src, r.name, i, handles)); + + % if findjobj exists use it, since it lets us get continous callbacks while + % a slider moves + if (exist('findjobj', 'file')>0) && verLessThan('matlab', '8') && ~ispc + disp('using findjobj'); + pause(0.1); + drawnow + jh = findjobj(handles.slider(i), 'nomenu'); + %jh.AdjustmentValueChangedCallback = {@sliderCallbackFunc, r.name, i}; + jh.AdjustmentValueChangedCallback = @(src,event)sliderCallbackFunc(src, handles.slider(i), r.name, i, handles); + end + end + + if isempty(rhandles) + figure + r.plot(q); + end + + if nargout > 0 + handle = fig; + end +end + +function teach_callback(src, name, j, handles) +% called on changes to a slider or to the edit box showing joint coordinate +% +% src the object that caused the event +% name name of the robot +% j the joint index concerned (1..N) +% slider true if the + + qscale = handles.qscale; + + switch get(src, 'Style') + case 'slider' + % slider changed, get value and reflect it to edit box + newval = get(src, 'Value'); + set(handles.edit(j), 'String', num2str(qscale(j)*newval)); + case 'edit' + % edit box changed, get value and reflect it to slider + newval = str2double(get(src, 'String')) / qscale(j); + set(handles.slider(j), 'Value', newval); + end + %fprintf('newval %d %f\n', j, newval); + + + % find all graphical objects tagged with the robot name, this is the + % instancs of that robot across all figures + rhandles = findobj('Tag', name); + + for rhandle=rhandles' + % for every graphical robot instance + + h = get(rhandle, 'UserData'); % get the robot object + robot = h.robot; + q = h.q; % get its current joint angles + if isempty(q) + q = zeros(1,robot.n); + end + q(j) = newval; % update the joint angle + + % update the robot object + %robot.q = q; + %set(r, 'UserData', robot); + + robot.plot(q); % plot it + drawnow + end + + % compute the robot tool pose + T6 = robot.fkine(q); + + % convert orientation to desired format + switch handles.orientation + case 'approach' + orient = T6(:,3); % approach vector + case 'eul' + orient = tr2eul(T6); + case'rpy' + orient = tr2rpy(T6); + end + + % update the display in the teach window + for i=1:3 + set(handles.t6.t(i), 'String', sprintf('%.3f', T6(i,4))); + set(handles.t6.r(i), 'String', sprintf('%.3f', orient(i))); + end + + robot.T = T6; + robot.notify('Moved'); + %drawnow +end + +function record_callback(robot, handles) + rhandles = findobj('Tag', robot.name); + + if ~isempty(rhandles) + h = get(rhandles(1), 'UserData'); % get the plot graphics + h.q + robot.record(h.q); + end +end + +function sliderCallbackFunc(src, h, name, joint, handles) + persistent busy + + % this is really ugly but it works + if busy + return + end + + if get(src,'ValueIsAdjusting') == 1 + busy = true; + try + teach_callback(h, name, joint, handles); + catch me + fprintf('*******\n') + busy = false; + return + end + end + busy = false; + + +end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/trchain.m b/lwrserv/matlab/rvctools/robot/@SerialLink/trchain.m new file mode 100644 index 0000000..d6040b3 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/trchain.m @@ -0,0 +1,93 @@ +%SERIALLINK.TRCHAIN Convert to elementary transform sequence +% +% S = R.TRCHAIN(OPTIONS) is a sequence of elementary transforms that describe the +% kinematics of the serial link robot arm. The string S comprises a number +% of tokens of the form X(ARG) where X is one of Tx, Ty, Tz, Rx, Ry, or Rz. +% ARG is a joint variable, or a constant angle or length dimension. +% +% For example: +% >> mdl_puma560 +% >> p560.trchain +% ans = +% Rz(q1)Rx(90)Rz(q2)Tx(0.431800)Rz(q3)Tz(0.150050)Tx(0.020300)Rx(-90) +% Rz(q4)Tz(0.431800)Rx(90)Rz(q5)Rx(-90)Rz(q6) +% +% Options:: +% '[no]deg' Express angles in degrees rather than radians (default deg) +% 'sym' Replace length parameters by symbolic values L1, L2 etc. +% +% See also trchain, trotx, troty, trotz, transl. + +% Copyright (C) 1993-2015, 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.petercorke.com + +function s = trchain(robot, varargin) + + opt.sym = false; + opt.deg = true; + opt = tb_optparse(opt, varargin); + + if opt.deg + conv = 180/pi; + else + conv = 1; + end + + s = ''; + varcount = 1; + + for j=1:robot.n + L = robot.links(j); + + if L.isrevolute() + s = append(s, 'Rz(q%d)', j); + if L.d ~= 0 + if opt.sym + s = append(s, 'Tz(L%d)', varcount); + varcount = varcount+1; + else + s = append(s, 'Tz(%g)', L.d); + end + end + else + if L.theta ~= 0 + s = append(s, 'Rz(%g)', (L.alpha*conv)); + end + s = append(s, 'Tz(q%d)', j); + end + + if L.a ~= 0 + if opt.sym + s = append(s, 'Tx(L%d)', varcount); + varcount = varcount+1; + else + s = append(s, 'Tx(%g)', L.a); + end + + end + if L.alpha ~= 0 + s = append(s, 'Rx(%g)', (L.alpha*conv)); + end + end +end + + + function s = append(s, fmt, j) + s = strcat(s, sprintf(fmt, j)); + end diff --git a/lwrserv/matlab/rvctools/robot/@SerialLink/vellipse.m b/lwrserv/matlab/rvctools/robot/@SerialLink/vellipse.m new file mode 100644 index 0000000..639f1f2 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/@SerialLink/vellipse.m @@ -0,0 +1,72 @@ +function vellipse(robot, q, varargin) + %SerialLink.vellipse Velocity ellipsoid for seriallink manipulator + % + % R.vellipse(Q, OPTIONS) displays the velocity ellipsoid for the + % robot R at pose Q. The ellipsoid is centered at the tool tip position. + % + % Options:: + % '2d' Ellipse for translational xy motion, for planar manipulator + % 'trans' Ellipsoid for translational motion (default) + % 'rot' Ellipsoid for rotational motion + % + % Display options as per plot_ellipse to control ellipsoid face and edge + % color and transparency. + % + % Example:: + % To interactively update the velocity ellipsoid while using sliders + % to change the robot's pose: + % robot.teach('callback', @(r,q) r.vellipse(q)) + % + % Notes:: + % - The ellipsoid is tagged with the name of the robot prepended to + % ".vellipse". + % - Calling the function with a different pose will update the ellipsoid. + % + % See also SerialLink.jacob0, SerialLink.fellipse, plot_ellipse. + + name = [robot.name '.vellipse']; + + e = findobj('Tag', name); + + if isempty(q) + delete(e); + return; + end + + opt.mode = {'trans', 'rot', '2d'}; + [opt,args] = tb_optparse(opt, varargin); + + if robot.n == 2 + opt.mode = '2d'; + end + + J = robot.jacob0(q); + + switch opt.mode + case'2d' + J = J(1:2,1:2); + case 'trans' + J = J(1:3,:); + case 'rot' + J = J(4:6,:); + end + + N = (J*J'); + + t = transl(robot.fkine(q)); + + switch opt.mode + case '2d' + if isempty(e) + h = plot_ellipse(N, t(1:2), 'edgecolor', 'r', 'Tag', name, args{:}); + else + plot_ellipse(N, t(1:2), 'alter', e); + end + otherwise + if isempty(e) + h = plot_ellipse(N, t(1:3), 'edgecolor', 'k', 'fillcolor', 'r', 'alpha', 0.5, 'Tag', name, args{:}); + else + plot_ellipse(N, t(1:3), 'alter', e); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/Bug2.m b/lwrserv/matlab/rvctools/robot/Bug2.m new file mode 100644 index 0000000..797fe35 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Bug2.m @@ -0,0 +1,180 @@ +%BUG2 Bug navigation class +% +% A concrete subclass of the Navigation class that implements the bug2 +% navigation algorithm. This is a simple automaton that performs local +% planning, that is, it can only sense the immediate presence of an obstacle. +% +% Methods:: +% path Compute a path from start to goal +% visualize Display the obstacle map (deprecated) +% plot Display the obstacle map +% display Display state/parameters in human readable form +% char Convert to string +% +% Example:: +% load map1 % load the map +% bug = Bug2(map); % create navigation object +% bug.goal = [50, 35]; % set the goal +% bug.path([20, 10]); % animate path to (20,10) +% +% Reference:: +% - Dynamic path planning for a mobile automaton with limited information on the environment,, +% V. Lumelsky and A. Stepanov, +% IEEE Transactions on Automatic Control, vol. 31, pp. 1058-1063, Nov. 1986. +% - Robotics, Vision & Control, Sec 5.1.2, +% Peter Corke, Springer, 2011. +% +% See also Navigation, DXform, Dstar, PRM. + +% 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 . + +classdef Bug2 < Navigation + + properties(Access=protected) + H % hit points + j % number of hit points + mline % line from starting position to goal + step % state, in step 1 or step 2 of algorithm + edge % edge list + k % edge index + end + + methods + + function bug = Bug2(world, goal) + %Bug2.Bug2 bug2 navigation object constructor + % + % B = Bug2(MAP) is a bug2 navigation + % object, and MAP is an occupancy grid, a representation of a + % planar world as a matrix whose elements are 0 (free space) or 1 + % (occupied). + % + % Options:: + % 'goal',G Specify the goal point (1x2) + % 'inflate',K Inflate all obstacles by K cells. + % + % See also Navigation.Navigation. + + % invoke the superclass constructor + bug = bug@Navigation(world); + + bug.H = []; + bug.j = 1; + bug.step = 1; + end + + % null planning for the bug! + function plan(bug, goal) + bug.goal = goal; + end + + function navigate_init(bug, robot) + + if isempty(bug.goal) + error('RTB:bug2:nogoal', 'no goal set, cant compute path'); + end + % parameters of the M-line, direct from initial position to goal + % as a vector mline, such that [robot 1]*mline = 0 + dims = axis; + xmin = dims(1); xmax = dims(2); + ymin = dims(3); ymax = dims(4); + + % create homogeneous representation of the line + % line*[x y 1]' = 0 + bug.mline = homline(robot(1), robot(2), ... + bug.goal(1), bug.goal(2)); + bug.mline = bug.mline / norm(bug.mline(1:2)); + if bug.mline(2) == 0 + % handle the case that the line is vertical + plot([robot(1) robot(1)], [ymin ymax], 'k--'); + else + x = [xmin xmax]'; + y = -[x [1;1]] * [bug.mline(1); bug.mline(3)] / bug.mline(2); + plot(x, y, 'k--'); + end + end + + % this should be a protected function, but can't make this callable + % from superclass when it's instantiation of an abstract method + % (R2010a). + + function n = next(bug, robot) + + n = []; + % these are coordinates (x,y) + + if bug.step == 1 + % Step 1. Move along the M-line toward the goal + + if colnorm(bug.goal - robot) == 0 % are we there yet? + return + end + + % motion on line toward goal + d = bug.goal-robot; + dx = sign(d(1)); + dy = sign(d(2)); + + % detect if next step is an obstacle + if bug.occgrid(robot(2)+dy, robot(1)+dx) + bug.message('(%d,%d) obstacle!', n); + bug.H(bug.j,:) = robot; % define hit point + bug.step = 2; + % get a list of all the points around the obstacle + bug.edge = edgelist(bug.occgrid==0, robot); + bug.k = 2; % skip the first edge point, we are already there + else + n = robot + [dx; dy]; + end + end % step 1 + + if bug.step == 2 + % Step 2. Move around the obstacle until we reach a point + % on the M-line closer than when we started. + if colnorm(bug.goal-robot) == 0 % are we there yet? + return + end + + if bug.k <= numrows(bug.edge) + n = bug.edge(bug.k,:)'; % next edge point + else + % we are at the end of the list of edge points, we + % are back where we started. Step 2.c test. + error('robot is trapped') + return; + end + + % are we on the M-line now ? + if abs( [robot' 1]*bug.mline') <= 0.5 + bug.message('(%d,%d) moving along the M-line', n); + % are closer than when we encountered the obstacle? + if colnorm(robot-bug.goal) < colnorm(bug.H(bug.j,:)'-bug.goal) + % back to moving along the M-line + bug.j = bug.j + 1; + bug.step = 1; + return; + end + end + % no, keep going around + bug.message('(%d,%d) keep moving around obstacle', n) + bug.k = bug.k+1; + end % step 2 + end % next + + end % methods +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/CHANGES b/lwrserv/matlab/rvctools/robot/CHANGES new file mode 100644 index 0000000..6a93dec --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/CHANGES @@ -0,0 +1,1002 @@ +CHANGES FOR RTB 9.8 +======================================= + +CodeGenerator +---------------- + +A new class CodeGenerator allows you to create efficient robot-specific +models from any SerialLink manipulator object. It writes m-files or +creates Simulink blocks which can be used in real-time systems. + +Requires Symbolics Toolbox. Does not yet support inverse kinematics. + +Demo +---------------- + +Now has 20 demonstrations that include mobile robotics, CodeGeneration +and symbolic operations. The MATLAB code now runs interactively using +the runscript module. + +Link +---------------- +New subclasses Revolute, Prismatic are polymorphic with Link but +have implicit joint type, no need to specify sigma: + +L(1) = Revolute('d', 0, 'a', 0, 'alpha', pi/2, ... + 'I', [0, 0.35, 0, 0, 0, 0], ... + 'r', [0, 0, 0], ... + 'm', 0, ... + 'Jm', 200e-6, ... + 'G', -62.6111, ... + 'B', 1.48e-3, ... + 'Tc', [0.395 -0.435] ); + +New files +---------------- +tripleangle, demo of RPY/Euler angles + + +Simulink +---------------- +Most Simulink models now for MATLAB 2012b. +pinv option for ikine block. + + +CHANGES FOR RTB 9.7 +======================================= + +Link +---------------- + +New way to specify link parameters + +L(1) = Link('d', 0, 'a', 0, 'alpha', pi/2, ... + 'I', [0, 0.35, 0, 0, 0, 0], ... + 'r', [0, 0, 0], ... + 'm', 0, ... + 'Jm', 200e-6, ... + 'G', -62.6111, ... + 'B', 1.48e-3, ... + 'Tc', [0.395 -0.435] ); + +There is no need to give a value for theta in this case, since it is +the joint variable. + +The old syntax is still valid. + +New models +---------------- + + mdl_puma560_3 first 3 links of the Puma560 + mdl_ball a 50 link robot that folds into a ball + mdl_coil a 50 link robot that forms a helix + +Symbolics +---------------- + +all the kinematic and dynamics functions can now generate symbolic expressions. You need +to have the MATLAB Symbolic Toolbox. + +You need to create symbolic links + +L(1) = Link('d', 0, 'a', 0, 'alpha', pi/2, 'sym'); + +and build them into a robot + +Alternatively you can create a symbolic version of an existing robot + +symbot = sym(twolink); + +where all numerical parameters are converted to symbolic versions. Numerical +values like pi/2 become the exact symbolic fraction pi/2. + +To get a vector symbolic joint angles or rates use the new method + +[q,qd] = symbot.gencoords(); + +then the following functions will give symbolic expressions in terms of the joint coordinates +and robot parameters: + +T = symbot.fkine(q) +g = symbot.gravload(q) +C = symbot.coriolis(q, qd) +J = symbot.jacobn(q) + + +------------------------------------------------------------------------ +------------------------------------------------------------------------ +r1061 | peter.i.corke@gmail.com | 2013-01-26 16:25:36 +1000 (Sat, 26 Jan 2013) | 2 lines +Changed paths: + A /matlab/robot/trunk/tripleangle.fig + A /matlab/robot/trunk/tripleangle.m + +New interactive demo to show 3 axis rotations + +------------------------------------------------------------------------ +r1060 | malzahnandroid | 2013-01-25 20:13:41 +1000 (Fri, 25 Jan 2013) | 1 line +Changed paths: + M /matlab/common/tb_optparse.m + M /matlab/robot/trunk/@SerialLink/fkine.m + +modification hint +------------------------------------------------------------------------ +r1058 | peter.i.corke@gmail.com | 2013-01-24 16:09:29 +1000 (Thu, 24 Jan 2013) | 2 lines +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m + M /matlab/robot/trunk/@CodeGenerator/gencoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genfkine.m + M /matlab/robot/trunk/@CodeGenerator/genfriction.m + M /matlab/robot/trunk/@CodeGenerator/gengravload.m + M /matlab/robot/trunk/@CodeGenerator/geninertia.m + M /matlab/robot/trunk/@CodeGenerator/geninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genmfuncoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfkine.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@CodeGenerator/genmfungravload.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninertia.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfkine.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@CodeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockjacobian.m + +Documentation tidyup. + +------------------------------------------------------------------------ +r1057 | malzahnandroid | 2013-01-24 00:25:38 +1000 (Thu, 24 Jan 2013) | 2 lines +Changed paths: + M /matlab/common/distributeblocks.m + M /matlab/common/doesblockexist.m + M /matlab/common/multidfprintf.m + M /matlab/common/symexpr2slblock.m + M /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m + M /matlab/robot/trunk/@CodeGenerator/gencoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genfkine.m + M /matlab/robot/trunk/@CodeGenerator/genfriction.m + M /matlab/robot/trunk/@CodeGenerator/gengravload.m + M /matlab/robot/trunk/@CodeGenerator/geninertia.m + M /matlab/robot/trunk/@CodeGenerator/geninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genmfuncoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfkine.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@CodeGenerator/genmfungravload.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninertia.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfkine.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@CodeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockjacobian.m + M /matlab/robot/trunk/@CodeGenerator/logmsg.m + M /matlab/robot/trunk/@CodeGenerator/private/createmconstructor.m + M /matlab/robot/trunk/@CodeGenerator/private/createnewblocklibrary.m + M /matlab/robot/trunk/@CodeGenerator/private/ffindreplace.m + M /matlab/robot/trunk/@CodeGenerator/private/finsertfront.m + M /matlab/robot/trunk/@CodeGenerator/private/generatecopyrightnote.m + M /matlab/robot/trunk/@CodeGenerator/private/getpibugfixstring.m + M /matlab/robot/trunk/@CodeGenerator/private/replaceheader.m + ++ fixed umlaut issue in headers ++ acknowledgement in copyright note +------------------------------------------------------------------------ +r1056 | malzahnandroid | 2013-01-24 00:08:12 +1000 (Thu, 24 Jan 2013) | 3 lines +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m + M /matlab/robot/trunk/@CodeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfkine.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@CodeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockjacobian.m + M /matlab/robot/trunk/@CodeGenerator/private/constructheaderstring.m + A /matlab/robot/trunk/@CodeGenerator/private/createnewblocklibrary.m + A /matlab/robot/trunk/@CodeGenerator/private/generatecopyrightnote.m + ++ addpath and rmpath methods for CodeGenerator ++ createnewblocklibrary.m collects multiple lines which would otherwhise be copied in each genslblock*-file. It also creates an slblock.m for the library so we find the autogenerated libraries in the Simulink browser now ++ generatecopyrightnote.m for easier maintenance +------------------------------------------------------------------------ +r1055 | malzahnandroid | 2013-01-22 23:55:32 +1000 (Tue, 22 Jan 2013) | 2 lines +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfkine.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@CodeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockjacobian.m + ++ Simulink block generators now check if required blocks are already present in the library, otherwise appropriate generators are called automatically ++ M-code generators check if requirede m-functions are already present in the robot object directory, otherwise appropriate generators are called automatically +------------------------------------------------------------------------ +r1053 | malzahnandroid | 2013-01-22 02:11:34 +1000 (Tue, 22 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/@SerialLink/rne_dh.m + +fixed copy paste bug in rne_dh.m l. 212 +------------------------------------------------------------------------ +r1052 | peter.i.corke@gmail.com | 2013-01-21 21:53:35 +1000 (Mon, 21 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/demos/fdyn.m + +Define the robot model +------------------------------------------------------------------------ +r1051 | peter.i.corke@gmail.com | 2013-01-21 21:51:36 +1000 (Mon, 21 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/mdl_puma560.m + +Add joint limits, not quite sure if these number are correct for these DH coordinates +------------------------------------------------------------------------ +r1050 | peter.i.corke@gmail.com | 2013-01-21 21:50:55 +1000 (Mon, 21 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/simulink/roblocks.mdl + +Fix scale error bug in "pose integral" +------------------------------------------------------------------------ +r1049 | peter.i.corke@gmail.com | 2013-01-20 17:00:18 +1000 (Sun, 20 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/RangeBearingSensor.m + +Remove options now parsed by superclass +------------------------------------------------------------------------ +r1048 | peter.i.corke@gmail.com | 2013-01-20 13:13:18 +1000 (Sun, 20 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/Sensor.m + +support an 'animate' option which shows which landmark the sensor is picking up +------------------------------------------------------------------------ +r1047 | peter.i.corke@gmail.com | 2013-01-20 13:12:07 +1000 (Sun, 20 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/unit_test/DifferentialMotionTest.m + +update tests to reflect fix to eul2jac() +------------------------------------------------------------------------ +r1046 | peter.i.corke@gmail.com | 2013-01-20 13:06:45 +1000 (Sun, 20 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/demos/codegen.m + +Added argument to purge() so as to not require manual clicking of confirmation popup +------------------------------------------------------------------------ +r1045 | peter.i.corke@gmail.com | 2013-01-20 13:05:51 +1000 (Sun, 20 Jan 2013) | 2 lines +Changed paths: + M /matlab/robot/trunk/@SerialLink/plot.m + +Added logic for 'fps' option +Added drawnow after pause(), despite documentation saying this is implicit in pause, it doesnt work for 2012b. +------------------------------------------------------------------------ +r1044 | peter.i.corke@gmail.com | 2013-01-20 13:04:43 +1000 (Sun, 20 Jan 2013) | 5 lines +Changed paths: + M /matlab/robot/trunk/EKF.m + +Fix logic around plotting, added 'plot' option to run() +Plot dimensions come from map (if given), else from the 'dim' option to constructor +Fix logic for SLAM case which was not estimating the map! + +plot_map() now has an argument which is the covariance of the ellipses to be overlaid around landmarks +------------------------------------------------------------------------ +r1043 | peter.i.corke@gmail.com | 2013-01-20 11:49:11 +1000 (Sun, 20 Jan 2013) | 2 lines +Changed paths: + M /matlab/robot/trunk/@SerialLink/rne.m + M /matlab/robot/trunk/startup_rtb.m + +We can't use FRNE if the robot is symbolic. +Make RNE a wrapper that invokes mex or m-file according to circumstance. Note that now the FRNE files live in the MEX directory which must be on the path +------------------------------------------------------------------------ +r1042 | peter.i.corke@gmail.com | 2013-01-20 11:47:23 +1000 (Sun, 20 Jan 2013) | 2 lines +Changed paths: + M /matlab/robot/trunk/@SerialLink/rne_dh.m + +Change ' to .', important for symbolic expressions +line 211, MATLAB 2012b complains with it as written for case of symbolic robot and numerical joint angles... +------------------------------------------------------------------------ +r1041 | malzahnandroid | 2013-01-19 01:13:19 +1000 (Sat, 19 Jan 2013) | 1 line +Changed paths: + M /matlab/common/symexpr2slblock.m + M /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m + M /matlab/robot/trunk/@CodeGenerator/gencoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genfkine.m + M /matlab/robot/trunk/@CodeGenerator/genfriction.m + M /matlab/robot/trunk/@CodeGenerator/gengravload.m + M /matlab/robot/trunk/@CodeGenerator/geninertia.m + M /matlab/robot/trunk/@CodeGenerator/geninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genmfuncoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfkine.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@CodeGenerator/genmfungravload.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninertia.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfkine.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@CodeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockjacobian.m + M /matlab/robot/trunk/@CodeGenerator/logmsg.m + M /matlab/robot/trunk/@CodeGenerator/private/constructheaderstring.m + M /matlab/robot/trunk/@CodeGenerator/private/createmconstructor.m + M /matlab/robot/trunk/@CodeGenerator/private/ffindreplace.m + M /matlab/robot/trunk/@CodeGenerator/private/finsertfront.m + M /matlab/robot/trunk/@CodeGenerator/private/getpibugfixstring.m + M /matlab/robot/trunk/@CodeGenerator/private/replaceheader.m + +header formatting +------------------------------------------------------------------------ +r1040 | malzahnandroid | 2013-01-18 21:29:13 +1000 (Fri, 18 Jan 2013) | 1 line +Changed paths: + M /matlab/common/distributeblocks.m + M /matlab/common/doesblockexist.m + M /matlab/common/multidfprintf.m + M /matlab/common/symexpr2slblock.m + M /matlab/robot/trunk/@CodeGenerator/private/constructheaderstring.m + M /matlab/robot/trunk/@CodeGenerator/private/createmconstructor.m + M /matlab/robot/trunk/@CodeGenerator/private/ffindreplace.m + M /matlab/robot/trunk/@CodeGenerator/private/finsertfront.m + M /matlab/robot/trunk/@CodeGenerator/private/getpibugfixstring.m + M /matlab/robot/trunk/@CodeGenerator/private/replaceheader.m + +reformatted headers in common functions and private CodeGenerator methods +------------------------------------------------------------------------ +r1039 | malzahnandroid@gmail.com | 2013-01-18 09:17:30 +1000 (Fri, 18 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m + M /matlab/robot/trunk/@CodeGenerator/gencoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genfkine.m + M /matlab/robot/trunk/@CodeGenerator/genfriction.m + M /matlab/robot/trunk/@CodeGenerator/gengravload.m + M /matlab/robot/trunk/@CodeGenerator/geninertia.m + M /matlab/robot/trunk/@CodeGenerator/geninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genmfuncoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfkine.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@CodeGenerator/genmfungravload.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninertia.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfkine.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@CodeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockjacobian.m + M /matlab/robot/trunk/@CodeGenerator/logmsg.m + ++ comments for public CodeGenerator methods reformatted +------------------------------------------------------------------------ +r1038 | peter.i.corke@gmail.com | 2013-01-12 17:41:37 +1000 (Sat, 12 Jan 2013) | 2 lines +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m + M /matlab/robot/trunk/@CodeGenerator/gencoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genfdyn.m + +Partial updates to doco + +------------------------------------------------------------------------ +r1037 | peter.i.corke@gmail.com | 2013-01-12 16:19:49 +1000 (Sat, 12 Jan 2013) | 2 lines +Changed paths: + M /matlab/robot/trunk/demos/braitnav.m + M /matlab/robot/trunk/demos/bugnav.m + M /matlab/robot/trunk/demos/codegen.m + M /matlab/robot/trunk/demos/drivepose.m + M /matlab/robot/trunk/demos/dstarnav.m + M /matlab/robot/trunk/demos/fdyn.m + M /matlab/robot/trunk/demos/fkine.m + M /matlab/robot/trunk/demos/graphics.m + M /matlab/robot/trunk/demos/idyn.m + M /matlab/robot/trunk/demos/ikine.m + M /matlab/robot/trunk/demos/jacob.m + M /matlab/robot/trunk/demos/particlefilt.m + M /matlab/robot/trunk/demos/prmnav.m + M /matlab/robot/trunk/demos/quadrotor.m + M /matlab/robot/trunk/demos/robot.m + M /matlab/robot/trunk/demos/rotation.m + M /matlab/robot/trunk/demos/slam.m + M /matlab/robot/trunk/demos/symbolic.m + M /matlab/robot/trunk/demos/traj.m + M /matlab/robot/trunk/demos/trans.m + M /matlab/robot/trunk/demos/ztorque.m + +Added copyright notices + +------------------------------------------------------------------------ +r1036 | peter.i.corke@gmail.com | 2013-01-12 16:11:31 +1000 (Sat, 12 Jan 2013) | 4 lines +Changed paths: + A /matlab/robot/trunk/demos/braitnav.m + A /matlab/robot/trunk/demos/bugnav.m + A /matlab/robot/trunk/demos/codegen.m (from /matlab/robot/trunk/demos/rtsymcgendemo.m:1035) + M /matlab/robot/trunk/demos/demos.xml + A /matlab/robot/trunk/demos/drivepose.m + A /matlab/robot/trunk/demos/dstarnav.m + A /matlab/robot/trunk/demos/fdyn.m (from /matlab/robot/trunk/demos/rtfddemo.m:992) + A /matlab/robot/trunk/demos/fkine.m (from /matlab/robot/trunk/demos/rtfkdemo.m:992) + A /matlab/robot/trunk/demos/graphics.m (from /matlab/robot/trunk/demos/rtandemo.m:992) + A /matlab/robot/trunk/demos/idyn.m (from /matlab/robot/trunk/demos/rtidemo.m:992) + A /matlab/robot/trunk/demos/ikine.m (from /matlab/robot/trunk/demos/rtikdemo.m:992) + A /matlab/robot/trunk/demos/jacob.m (from /matlab/robot/trunk/demos/rtjademo.m:992) + A /matlab/robot/trunk/demos/particlefilt.m + A /matlab/robot/trunk/demos/prmnav.m + A /matlab/robot/trunk/demos/quadrotor.m + A /matlab/robot/trunk/demos/robot.m + A /matlab/robot/trunk/demos/rotation.m + D /matlab/robot/trunk/demos/rtandemo.m + D /matlab/robot/trunk/demos/rtfddemo.m + D /matlab/robot/trunk/demos/rtfkdemo.m + D /matlab/robot/trunk/demos/rtidemo.m + D /matlab/robot/trunk/demos/rtikdemo.m + D /matlab/robot/trunk/demos/rtjademo.m + D /matlab/robot/trunk/demos/rtsymcgendemo.m + D /matlab/robot/trunk/demos/rttgdemo.m + D /matlab/robot/trunk/demos/rttrdemo.m + A /matlab/robot/trunk/demos/slam.m + A /matlab/robot/trunk/demos/symbolic.m + A /matlab/robot/trunk/demos/traj.m (from /matlab/robot/trunk/demos/rttgdemo.m:992) + A /matlab/robot/trunk/demos/trans.m (from /matlab/robot/trunk/demos/rttrdemo.m:992) + A /matlab/robot/trunk/demos/ztorque.m + A /matlab/robot/trunk/rtbdemo.m (from /matlab/robot/trunk/rtdemo.m:992) + D /matlab/robot/trunk/rtdemo.m + M /matlab/robot/trunk/unit_test/UtilityTest.m + +Changes to names of demo files. +They are now run using runscript which allows finer grained control of the execution of these scripts. +Demo scripts no longer go into the users path, rtbdemo points at them explicitly. +Added many new demo scripts. +------------------------------------------------------------------------ +r1035 | peter.i.corke@gmail.com | 2013-01-10 21:34:37 +1000 (Thu, 10 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/Bug2.m + +Add error message in path() if goal is not given. +------------------------------------------------------------------------ +r1034 | peter.i.corke@gmail.com | 2013-01-10 21:33:45 +1000 (Thu, 10 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/@SerialLink/plot.m + +Movie frames obtained using plot rather than getframe. Makes resolution better, indep of onscreen figure size, perhaps a bit slower... Also used fullfile instead instead of sprintf +------------------------------------------------------------------------ +r1033 | peter.i.corke@gmail.com | 2013-01-10 21:32:48 +1000 (Thu, 10 Jan 2013) | 1 line +Changed paths: + M /matlab/robot/trunk/@SerialLink/maniplty.m + +Updated doco/comments for new options +------------------------------------------------------------------------ +r1031 | malzahnandroid@gmail.com | 2012-12-24 01:59:56 +1000 (Mon, 24 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/genfdyn.m + ++ fixed wrong function call to genslblockinvdyn in genfdyn.m +------------------------------------------------------------------------ +r1030 | malzahnandroid@gmail.com | 2012-12-24 01:22:24 +1000 (Mon, 24 Dec 2012) | 4 lines +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/genfdyn.m + M /matlab/robot/trunk/@CodeGenerator/geninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@SerialLink/gravload.m + M /matlab/robot/trunk/unit_test/CodeGeneratorTest.m + ++ completed unit_test/CodeGeneratorTest.m ++ fixed bug with wrong sign of friction in generation of inverse and forward dynamics ++ fixed camel case call of distributeblocks in genslblock methods ++ fixed missing inertia matrix check in genfdyn.m +------------------------------------------------------------------------ +r1029 | malzahnandroid@gmail.com | 2012-12-21 09:27:03 +1000 (Fri, 21 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/demos/demos.xml + D /matlab/robot/trunk/demos/html + M /matlab/robot/trunk/demos/rtsymcgendemo.m + M /wiki/FAQ.wiki + ++ adjusted the CodeGenerator demo to the format typically used in the Robotics Toolbox +------------------------------------------------------------------------ +r1028 | malzahnandroid@gmail.com | 2012-12-21 09:08:29 +1000 (Fri, 21 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/demos/html/rtsymcgendemo.html + M /matlab/robot/trunk/demos/html/rtsymcgendemo.png + M /matlab/robot/trunk/demos/rtsymcgendemo.m + ++ cosmetics to rtsymcgendemo.m +------------------------------------------------------------------------ +r1025 | malzahnandroid@gmail.com | 2012-12-21 08:54:58 +1000 (Fri, 21 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/demos/demos.xml + A /matlab/robot/trunk/demos/html + A /matlab/robot/trunk/demos/html/rtsymcgendemo.html + A /matlab/robot/trunk/demos/html/rtsymcgendemo.png + A /matlab/robot/trunk/demos/html/rtsymcgendemo_01.png + A /matlab/robot/trunk/demos/rtsymcgendemo.m + ++ A getting started example for the symbolics and CodeGenerator class functionality +------------------------------------------------------------------------ +r1024 | malzahnandroid@gmail.com | 2012-12-19 10:14:43 +1000 (Wed, 19 Dec 2012) | 4 lines +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/genmfuncoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfkine.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@CodeGenerator/genmfungravload.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninertia.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + D /matlab/robot/trunk/test/symbolicTestBed.m + M /matlab/robot/trunk/unit_test/CodeGeneratorTest.m + ++ fixed bug regarding test on existing robot constructor in genmfunXYZ.m ++ removed obsolete symbolicTestBed ++ probably found a bug in SerialLink/coriolis + +------------------------------------------------------------------------ +r1023 | malzahnandroid | 2012-12-19 09:34:40 +1000 (Wed, 19 Dec 2012) | 3 lines +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m + M /matlab/robot/trunk/@CodeGenerator/gengravload.m + M /matlab/robot/trunk/@CodeGenerator/geninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfuncoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@CodeGenerator/genmfungravload.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninertia.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@SerialLink/gravload.m + M /matlab/robot/trunk/@SerialLink/inertia.m + A /matlab/robot/trunk/unit_test/CodeGeneratorTest.m + ++ added CodeGeneratorTest.m - test for forward and inverse dynamics yet to be completed, inverse dynamics still produces some errors ++ some transposes changed from ' to .' ++ spelling error for distributeblocks commands in genslblockXYZ functions +------------------------------------------------------------------------ +r1022 | malzahnandroid@gmail.com | 2012-12-18 21:10:02 +1000 (Tue, 18 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m + M /matlab/robot/trunk/@CodeGenerator/gencoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genfkine.m + M /matlab/robot/trunk/@CodeGenerator/genfriction.m + M /matlab/robot/trunk/@CodeGenerator/gengravload.m + M /matlab/robot/trunk/@CodeGenerator/geninertia.m + M /matlab/robot/trunk/@CodeGenerator/geninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genmfuncoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfkine.m + M /matlab/robot/trunk/@CodeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@CodeGenerator/genmfungravload.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninertia.m + M /matlab/robot/trunk/@CodeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genmfunjacobian.m + M /matlab/robot/trunk/@CodeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfkine.m + M /matlab/robot/trunk/@CodeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@CodeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@CodeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@CodeGenerator/genslblockjacobian.m + M /matlab/robot/trunk/@CodeGenerator/logmsg.m + M /matlab/robot/trunk/@CodeGenerator/private/getpibugfixstring.m + +codeGenerator renaming complete +------------------------------------------------------------------------ +r1021 | malzahnandroid | 2012-12-18 20:49:30 +1000 (Tue, 18 Dec 2012) | 1 line +Changed paths: + D /matlab/robot/trunk/test/mdl_puma560_3_sym.m + M /matlab/robot/trunk/test/symbolicTestBed.m + +removed obsolete mdl_puma560_3_sym.m +------------------------------------------------------------------------ +r1020 | malzahnandroid | 2012-12-18 20:46:07 +1000 (Tue, 18 Dec 2012) | 1 line +Changed paths: + A /matlab/robot/trunk/@CodeGenerator/CodeGenerator.m (from /matlab/robot/trunk/@CodeGenerator/codeGenerator.m:1019) + D /matlab/robot/trunk/@CodeGenerator/codeGenerator.m + +renamed CodeGenerator constructor in repo browser +------------------------------------------------------------------------ +r1019 | malzahnandroid | 2012-12-18 20:45:26 +1000 (Tue, 18 Dec 2012) | 2 lines +Changed paths: + A /matlab/robot/trunk/@CodeGenerator (from /matlab/robot/trunk/@codeGenerator:1018) + D /matlab/robot/trunk/@codeGenerator + +renamed CodeGenerator from repo browser + +------------------------------------------------------------------------ +r1018 | malzahnandroid | 2012-12-18 20:43:54 +1000 (Tue, 18 Dec 2012) | 2 lines +Changed paths: + M /matlab/robot/trunk/@codeGenerator/codeGenerator.m + M /matlab/robot/trunk/@codeGenerator/genslblockfkine.m + ++ purge function also deletes logfile ++ removed camelcase in genslblockfkine.m +------------------------------------------------------------------------ +r1017 | malzahnandroid@gmail.com | 2012-12-17 05:37:34 +1000 (Mon, 17 Dec 2012) | 4 lines +Changed paths: + M /matlab/common/distributeblocks.m + M /matlab/robot/trunk/@codeGenerator/codeGenerator.m + M /matlab/robot/trunk/@codeGenerator/gencoriolis.m + A /matlab/robot/trunk/@codeGenerator/genfdyn.m + M /matlab/robot/trunk/@codeGenerator/genfkine.m + M /matlab/robot/trunk/@codeGenerator/genfriction.m + M /matlab/robot/trunk/@codeGenerator/gengravload.m + M /matlab/robot/trunk/@codeGenerator/geninertia.m + M /matlab/robot/trunk/@codeGenerator/geninvdyn.m + M /matlab/robot/trunk/@codeGenerator/genjacobian.m + M /matlab/robot/trunk/@codeGenerator/genmfuncoriolis.m + A /matlab/robot/trunk/@codeGenerator/genmfunfdyn.m + M /matlab/robot/trunk/@codeGenerator/genmfunfkine.m + M /matlab/robot/trunk/@codeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@codeGenerator/genmfungravload.m + M /matlab/robot/trunk/@codeGenerator/genmfuninertia.m + M /matlab/robot/trunk/@codeGenerator/genmfuninvdyn.m + M /matlab/robot/trunk/@codeGenerator/genmfunjacobian.m + M /matlab/robot/trunk/@codeGenerator/genslblockcoriolis.m + A /matlab/robot/trunk/@codeGenerator/genslblockfdyn.m + M /matlab/robot/trunk/@codeGenerator/genslblockfkine.m + M /matlab/robot/trunk/@codeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@codeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@codeGenerator/genslblockinertia.m + M /matlab/robot/trunk/@codeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@codeGenerator/genslblockjacobian.m + M /matlab/robot/trunk/@codeGenerator/private/constructheaderstring.m + M /matlab/robot/trunk/test/symbolicTestBed.m + ++ genfdyn, genmfunfdyn, genslblockfdyn added ++ headers revised ++ codeGenerator: purge functionality ++ forward dynamics, inverse dynamics, inertia and coriolis Simulink blocks more tidy: distributeblocks called on created blocks +------------------------------------------------------------------------ +r1016 | malzahnandroid@gmail.com | 2012-12-14 09:22:09 +1000 (Fri, 14 Dec 2012) | 5 lines +Changed paths: + M /matlab/robot/trunk/@SerialLink/coriolis.m + M /matlab/robot/trunk/@SerialLink/fkine.m + M /matlab/robot/trunk/@SerialLink/friction.m + M /matlab/robot/trunk/@SerialLink/gravload.m + M /matlab/robot/trunk/@SerialLink/issym.m + M /matlab/robot/trunk/@SerialLink/rne_dh.m + M /matlab/robot/trunk/@SerialLink/rne_mdh.m + A /matlab/robot/trunk/@codeGenerator/gencoriolis.m + A /matlab/robot/trunk/@codeGenerator/genfriction.m + M /matlab/robot/trunk/@codeGenerator/gengravload.m + A /matlab/robot/trunk/@codeGenerator/geninvdyn.m + A /matlab/robot/trunk/@codeGenerator/genmfuncoriolis.m + A /matlab/robot/trunk/@codeGenerator/genmfunfriction.m + M /matlab/robot/trunk/@codeGenerator/genmfuninertia.m + A /matlab/robot/trunk/@codeGenerator/genmfuninvdyn.m + A /matlab/robot/trunk/@codeGenerator/genslblockcoriolis.m + M /matlab/robot/trunk/@codeGenerator/genslblockfkine.m + A /matlab/robot/trunk/@codeGenerator/genslblockfriction.m + M /matlab/robot/trunk/@codeGenerator/genslblockgravload.m + M /matlab/robot/trunk/@codeGenerator/genslblockinertia.m + A /matlab/robot/trunk/@codeGenerator/genslblockinvdyn.m + M /matlab/robot/trunk/@codeGenerator/genslblockjacobian.m + M /matlab/robot/trunk/Link.m + ++ changed some ' to .' (true transpose) ++ generator functions for: inverse dynamics, coriolis matrix, gravitational load, inertia and joint friction ++ fkine outputs intermediate transforms only if nargout > 1 ++ issym splitted in two functions -> one for SerialLink and one for Link class objects ++ issue with obsolete line in Link/set.Tc fixed +------------------------------------------------------------------------ +r1001 | peter.i.corke@gmail.com | 2012-12-09 12:56:49 +1000 (Sun, 09 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/RangeBearingSensor.m + +Updated notes regarding noise. +------------------------------------------------------------------------ +r1000 | peter.i.corke@gmail.com | 2012-12-09 12:56:15 +1000 (Sun, 09 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/@SerialLink/teach.m + +Don't use findjobj with MATLAB r8.0. Can't make it work, the author says there is no problem... +------------------------------------------------------------------------ +r999 | peter.i.corke@gmail.com | 2012-12-09 12:54:48 +1000 (Sun, 09 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/Link.m + +Fix bug in displaying DH parameters for revolute joint from SerialLink.display +------------------------------------------------------------------------ +r998 | peter.i.corke@gmail.com | 2012-12-09 12:52:15 +1000 (Sun, 09 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/examples/walking.m + +Removed spurious line +------------------------------------------------------------------------ +r997 | peter.i.corke@gmail.com | 2012-12-09 12:51:16 +1000 (Sun, 09 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/angvec2r.m + +typo in comment +------------------------------------------------------------------------ +r996 | peter.i.corke@gmail.com | 2012-12-09 12:50:52 +1000 (Sun, 09 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/@SerialLink/maniplty.m + +Improved logic for handling manipulability of particular Cartesian DOF, rotation, translation or general specified as a logical 6-vector. +------------------------------------------------------------------------ +r995 | peter.i.corke@gmail.com | 2012-12-09 12:49:50 +1000 (Sun, 09 Dec 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/@SerialLink/ikine.m + +Add warning message if initial pose is singular +------------------------------------------------------------------------ +r992 | malzahnandroid@gmail.com | 2012-12-08 06:25:27 +1000 (Sat, 08 Dec 2012) | 4 lines +Changed paths: + M /matlab/common/distributeblocks.m + M /matlab/common/doesblockexist.m + M /matlab/common/multidfprintf.m + M /matlab/common/symexpr2slblock.m + M /matlab/robot/trunk/@codeGenerator/codeGenerator.m + M /matlab/robot/trunk/@codeGenerator/genfkine.m + A /matlab/robot/trunk/@codeGenerator/gengravload.m + A /matlab/robot/trunk/@codeGenerator/geninertia.m + A /matlab/robot/trunk/@codeGenerator/genjacobian.m + M /matlab/robot/trunk/@codeGenerator/genmfunfkine.m + A /matlab/robot/trunk/@codeGenerator/genmfungravload.m + A /matlab/robot/trunk/@codeGenerator/genmfuninertia.m + A /matlab/robot/trunk/@codeGenerator/genmfunjacobian.m + M /matlab/robot/trunk/@codeGenerator/genslblockfkine.m + A /matlab/robot/trunk/@codeGenerator/genslblockgravload.m + A /matlab/robot/trunk/@codeGenerator/genslblockinertia.m + A /matlab/robot/trunk/@codeGenerator/genslblockjacobian.m + A /matlab/robot/trunk/@codeGenerator/logmsg.m + M /matlab/robot/trunk/@codeGenerator/private/constructheaderstring.m + M /matlab/robot/trunk/@codeGenerator/private/createmconstructor.m + M /matlab/robot/trunk/@codeGenerator/private/ffindreplace.m + M /matlab/robot/trunk/@codeGenerator/private/finsertfront.m + M /matlab/robot/trunk/@codeGenerator/private/getpibugfixstring.m + M /matlab/robot/trunk/@codeGenerator/private/replaceheader.m + A /matlab/robot/trunk/test/mdl_puma560_3_sym.m + A /matlab/robot/trunk/test/symbolicTestBed.m + ++ added copyright note in headers ++ added block and function generator methods for robot jacobian, gravload, inertia ++ logmsg function for ease of programming and readability ++ symbolicTestBed.m added +------------------------------------------------------------------------ +r991 | malzahnandroid@gmail.com | 2012-12-04 23:17:54 +1000 (Tue, 04 Dec 2012) | 7 lines +Changed paths: + A /matlab/common/distributeblocks.m + A /matlab/common/doesblockexist.m + A /matlab/common/multidfprintf.m + A /matlab/common/symexpr2slblock.m + M /matlab/common/tb_optparse.m + M /matlab/robot/trunk/@SerialLink/fkine.m + A /matlab/robot/trunk/@SerialLink/issym.m + A /matlab/robot/trunk/@codeGenerator + A /matlab/robot/trunk/@codeGenerator/codeGenerator.m + A /matlab/robot/trunk/@codeGenerator/genfkine.m + A /matlab/robot/trunk/@codeGenerator/genmfunfkine.m + A /matlab/robot/trunk/@codeGenerator/genslblockfkine.m + A /matlab/robot/trunk/@codeGenerator/private + A /matlab/robot/trunk/@codeGenerator/private/constructheaderstring.m + A /matlab/robot/trunk/@codeGenerator/private/createmconstructor.m + A /matlab/robot/trunk/@codeGenerator/private/ffindreplace.m + A /matlab/robot/trunk/@codeGenerator/private/finsertfront.m + A /matlab/robot/trunk/@codeGenerator/private/getpibugfixstring.m + A /matlab/robot/trunk/@codeGenerator/private/replaceheader.m + M /matlab/robot/trunk/Link.m + ++ added code generator class with mat-, m- and simulink code generation for SerialLink fkine ++ tb_optparse.m: modified to work with classes in addition to structs ++ Link.m: handling friction with symbolic Link objects ++ fkine.m: also outputs intermediate transforms ++ issym moved out of SerialLink class definition file into class trunk ++ Simulink block creation utilities added to common ++ multidfprintf.m for formatted textoutput to multiple destinations +------------------------------------------------------------------------ +r967 | peter.i.corke@gmail.com | 2012-11-06 21:32:09 +1000 (Tue, 06 Nov 2012) | 5 lines +Changed paths: + M /matlab/robot/trunk/EKF.m + +Keep map and P0 as properties +Move all initialization from constructor to init() +init() clears history and init's the robot object +run() method calls init(), has option for 'plot' +New plot_error() method plots estimation error against std bounds +------------------------------------------------------------------------ +r966 | peter.i.corke@gmail.com | 2012-11-06 21:28:43 +1000 (Tue, 06 Nov 2012) | 3 lines +Changed paths: + M /matlab/robot/trunk/Link.m + +Fix bugs in concatenating Revolute/Prismatic subclass arrays, these are converted +to Link objects. + +------------------------------------------------------------------------ +r965 | peter.i.corke@gmail.com | 2012-11-06 21:24:28 +1000 (Tue, 06 Nov 2012) | 1 line +Changed paths: + D /matlab/robot/trunk/ishomog.m + D /matlab/robot/trunk/isrot.m + D /matlab/robot/trunk/isvec.m + +These moved to common +------------------------------------------------------------------------ +r964 | peter.i.corke@gmail.com | 2012-11-06 21:24:17 +1000 (Tue, 06 Nov 2012) | 2 lines +Changed paths: + M /matlab/robot/trunk/@SerialLink/plot.m + +Fixed bug in option parsing mode, also added comments for this. +Added options for render, ortho, perspective: not yet used but were meant to be. +------------------------------------------------------------------------ +r963 | peter.i.corke@gmail.com | 2012-11-06 21:23:15 +1000 (Tue, 06 Nov 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/mex/Makefile + +Added CFLAGS=-std=c99 to ensure that //-style comments are handled. +------------------------------------------------------------------------ +r962 | peter.i.corke@gmail.com | 2012-11-06 21:22:27 +1000 (Tue, 06 Nov 2012) | 1 line +Changed paths: + A /matlab/robot/trunk/examples/gait.m + M /matlab/robot/trunk/examples/walking.m + +walking.m used an archaic version of the toolbox, and needs gait.m (now added) +------------------------------------------------------------------------ +r961 | peter.i.corke@gmail.com | 2012-10-30 05:53:26 +1000 (Tue, 30 Oct 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/Link.m + +Bug fix to link concatenation code to support robot concatenation +------------------------------------------------------------------------ +r960 | peter.i.corke@gmail.com | 2012-10-28 14:37:51 +1000 (Sun, 28 Oct 2012) | 6 lines +Changed paths: + M /matlab/robot/trunk/RangeBearingSensor.m + +Rewrite code for range and angle limits: select features that match criteria then randomly sample, rather than other way around. +Changes to h() to support this, can return sensor observations for all features now +Serious bug: noise was not added to sensor output, now fixed. +Added a char method that invokes superclass char method. +Add support for animation using superclass plot method +Change instances of R to W, covariance, to match the book. +------------------------------------------------------------------------ +r959 | peter.i.corke@gmail.com | 2012-10-28 14:34:56 +1000 (Sun, 28 Oct 2012) | 3 lines +Changed paths: + M /matlab/robot/trunk/ParticleFilter.m + +Make w0 a parameter +Add option to disable animation +Add char and display methods to summarize the object state +------------------------------------------------------------------------ +r958 | peter.i.corke@gmail.com | 2012-10-28 14:33:53 +1000 (Sun, 28 Oct 2012) | 2 lines +Changed paths: + M /matlab/robot/trunk/Sensor.m + +Add plot method, shows animation of sensor detecting landmark. +char method now uses map.char() to display info about its map +------------------------------------------------------------------------ +r953 | peter.i.corke@gmail.com | 2012-10-23 09:25:59 +1000 (Tue, 23 Oct 2012) | 4 lines +Changed paths: + M /matlab/robot/trunk/Link.m + M /matlab/robot/trunk/Prismatic.m + M /matlab/robot/trunk/Revolute.m + +Allow a mixture of Revolute and Prismatic objects to be concatenated. +This requires a mod to Link to convert Revolute/Prismatic objects to the +equivalent Link object. + +------------------------------------------------------------------------ +r948 | peter.i.corke@gmail.com | 2012-10-21 12:33:44 +1000 (Sun, 21 Oct 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/mdl_puma560.m + +Modified to use Revolute link subclass +------------------------------------------------------------------------ +r947 | peter.i.corke@gmail.com | 2012-10-21 12:32:07 +1000 (Sun, 21 Oct 2012) | 1 line +Changed paths: + A /matlab/robot/trunk/mdl_puma560_3.m + +Model of first 3 links of Puma, useful for testing symbolic forms. +------------------------------------------------------------------------ +r946 | peter.i.corke@gmail.com | 2012-10-21 12:30:29 +1000 (Sun, 21 Oct 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/simulink/roblocks.mdl + +Fix initial condition assignment for Robot block +------------------------------------------------------------------------ +r945 | peter.i.corke@gmail.com | 2012-10-21 12:26:47 +1000 (Sun, 21 Oct 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/Navigation.m + +Support an SE(2) goal in the char() method +------------------------------------------------------------------------ +r944 | peter.i.corke@gmail.com | 2012-10-21 12:26:15 +1000 (Sun, 21 Oct 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/unit_test/PGraphTest.m + +Extend tests for new PGraph directed edge capability +------------------------------------------------------------------------ +r943 | peter.i.corke@gmail.com | 2012-10-21 12:25:56 +1000 (Sun, 21 Oct 2012) | 1 line +Changed paths: + M /matlab/robot/trunk/RRT.m + +Significant changes in path() method to make it work when start and goal are swapped around. Also required changes in PGraph +------------------------------------------------------------------------ +r941 | peter.i.corke@gmail.com | 2012-10-21 12:23:47 +1000 (Sun, 21 Oct 2012) | 1 line +Changed paths: + D /matlab/robot/trunk/simulink/sl_quadcopter.mdl + D /matlab/robot/trunk/simulink/sl_quadcopter_vs.mdl + A /matlab/robot/trunk/simulink/sl_quadrotor.mdl (from /matlab/robot/trunk/simulink/sl_quadcopter.mdl:909) + A /matlab/robot/trunk/simulink/sl_quadrotor_vs.mdl (from /matlab/robot/trunk/simulink/sl_quadcopter_vs.mdl:819) + +Change quadcopter to quadrotor +------------------------------------------------------------------------ +r935 | peter.i.corke@gmail.com | 2012-10-06 21:14:32 +1000 (Sat, 06 Oct 2012) | 2 lines +Changed paths: + M /matlab/robot/trunk/Link.m + A /matlab/robot/trunk/Prismatic.m + A /matlab/robot/trunk/Revolute.m + +Add 2 subclasses of Link, update comments for Link class. + +------------------------------------------------------------------------ +r934 | peter.i.corke@gmail.com | 2012-10-06 21:13:38 +1000 (Sat, 06 Oct 2012) | 2 lines +Changed paths: + M /matlab/robot/trunk/eul2jac.m + +Bug in the Jacobian, thanks to Erik van Oene for the fix. + +------------------------------------------------------------------------ +r921 | peter.i.corke@gmail.com | 2012-09-29 15:27:31 +1000 (Sat, 29 Sep 2012) | 2 lines +Changed paths: + M /matlab/robot/trunk/mdl_puma560.m + +Rewritten using new Link object creation signature + +------------------------------------------------------------------------ +r920 | peter.i.corke@gmail.com | 2012-09-29 15:26:46 +1000 (Sat, 29 Sep 2012) | 4 lines +Changed paths: + M /matlab/robot/trunk/@SerialLink/SerialLink.m + M /matlab/robot/trunk/@SerialLink/coriolis.m + M /matlab/robot/trunk/@SerialLink/fkine.m + A /matlab/robot/trunk/@SerialLink/gencoords.m + A /matlab/robot/trunk/@SerialLink/genforces.m + M /matlab/robot/trunk/@SerialLink/jacobn.m + M /matlab/robot/trunk/@SerialLink/rne_dh.m + M /matlab/robot/trunk/Link.m + +Modifications to support generation of symbolic expressions. +Significant changes to Link to support symbolics and also a new way +of expressing parameters using key/value parameter notation. + +------------------------------------------------------------------------ +r919 | peter.i.corke@gmail.com | 2012-09-29 15:15:56 +1000 (Sat, 29 Sep 2012) | 2 lines +Changed paths: + A /matlab/robot/trunk/mdl_ball.m + A /matlab/robot/trunk/mdl_coil.m + +New models with large numbers of joints. + +------------------------------------------------------------------------ diff --git a/lwrserv/matlab/rvctools/robot/CITATION b/lwrserv/matlab/rvctools/robot/CITATION new file mode 100644 index 0000000..919f1ce --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/CITATION @@ -0,0 +1,14 @@ +Citing the Toolbox: +------------------ + +If you use the Toolbox and want to cite it in a paper please use: + +@book{Corke11a, + Author = {Peter I. Corke}, + Date-Added = {2011-01-12 08:19:32 +1000}, + Date-Modified = {2011-07-26 12:27:18 +0200}, + Note = {ISBN 978-3-642-20143-1}, + Publisher = {Springer}, + Title = {Robotics, Vision \& Control: Fundamental Algorithms in Matlab}, + Year = {2011}} + diff --git a/lwrserv/matlab/rvctools/robot/CONTRIB b/lwrserv/matlab/rvctools/robot/CONTRIB new file mode 100644 index 0000000..dffb26a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/CONTRIB @@ -0,0 +1,24 @@ +The following files are contributed by others: + +% Robert Biro with Gary Von McMurray + SerialLink.ikine560.m + +%Wynand Swart + mdl_Fanuc10L.m: + mdl_MotomanHP6.m + mdl_S4ABB2p8.m + +%Paul Pounds + quadrotor_dynamics.m + quadrotor_plot.m + +%Paul Newman + EKF.m + +%Joern Malzahn + @CodeGenerator + common/distributeblocks + common/doesblockexist + common/multidfprintf + common/symexpr2slblock + common/simulinkext diff --git a/lwrserv/matlab/rvctools/robot/Contents.m b/lwrserv/matlab/rvctools/robot/Contents.m new file mode 100644 index 0000000..d2057b5 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Contents.m @@ -0,0 +1,216 @@ +% Robotics Toolbox. +% Version 9.8.0 2013-02-12 +% +% +% Homogeneous transformations +% angvec2r - angle/vector to RM +% angvec2tr - angle/vector to HT +% eul2r - Euler angles to RM +% eul2tr - Euler angles to HT +% oa2r - orientation and approach vector to RM +% oa2tr - orientation and approach vector to HT +% r2t - RM to HT +% rt2tr - (R,t) to HT +% rotx - RM for rotation about X-axis +% roty - RM for rotation about Y-axis +% rotz - RM for rotation about Z-axis +% rpy2r - roll/pitch/yaw angles to RM +% rpy2tr - roll/pitch/yaw angles to HT +% se2 - HT in SE(2) +% t2r - HT to RM +% tr2angvec - HT/RM to angle/vector form +% tr2eul - HT/RM to Euler angles +% tr2rpy - HT/RM to roll/pitch/yaw angles +% tr2rt - HT to (R,t) +% tranimate - animate a coordinate frame +% transl - set or extract the translational component of HT +% trnorm - normalize HT +% trplot - plot HT as a coordinate frame +% trplot2 - plot HT, SE(2), as a coordinate frame +% trprint - print an HT +% trotx - HT for rotation about X-axis +% troty - HT for rotation about Y-axis +% trotz - HT for rotation about Z-axis +% +% Homogeneous points and lines +% e2h - Euclidean coordinates to homogeneous +% h2e - homogeneous coordinates to Euclidean +% homline - create line from 2 points +% homtrans - transform points +% +% * HT = homogeneous transformation, a 4x4 matrix, belongs to the group SE(3). +% * RM = RM, an orthonormal 3x3 matrix, belongs to the group SO(3). +% * Functions of the form tr2XX will also accept a RM as the argument. +% +% Differential motion +% delta2tr - differential motion vector to HT +% eul2jac - Euler angles to Jacobian +% rpy2jac - RPY angles to Jacobian +% skew - vector to skew symmetric matrix +% tr2delta - HT to differential motion vector +% tr2jac - HT to Jacobian +% vex - skew symmetric matrix to vector +% wtrans - transform wrench between frames +% +% Trajectory generation +% ctraj - Cartesian trajectory +% jtraj - joint space trajectory +% lspb - 1D trapezoidal trajectory +% mtraj - multi-axis trajectory for arbitrary function +% mstraj - multi-axis multi-segment trajectory +% tpoly - 1D polynomial trajectory +% trinterp - interpolate HT s +% +% Quaternion +% Quaternion - constructor +% / - divide quaternion by quaternion or scalar +% * - multiply quaternion by a quaternion or vector +% inv - invert a quaternion +% norm - norm of a quaternion +% plot - display a quaternion as a 3D rotation +% unit - unitize a quaternion +% interp - interpolate a quaternion +% +% Serial-link manipulator +% SerialLink - construct a serial-link robot object +% Link - construct a robot link object +% Revolute - construct a revolute robot link object +% Prismatic - construct a prismatic robot link object +% * - compound two robots +% friction - return joint friction torques +% nofriction - return a robot object with no friction +% perturb - return a robot object with perturbed parameters +% plot - plot/animate robot +% teach - drive a graphical robot +% CodeGenerator - create efficient run-time kinematic and dynamics code +% +% Models +% mdl_ball - high order ball shaped mechanism +% mdl_coil - high order coil shaped mechanism +% mdl_Fanuc10L - Fanuc 10L (DH, kine) +% mdl_MotomanHP6 - Motoman HP6 (DH, kine) +% mdl_phantomx - Trossen Robotics PhantomX pincher +% mdl_puma560 - Puma 560 data (DH, kine, dyn) +% mdl_puma560_3 - Puma 560, first 3 joints, kine only +% mdl_puma560_3_sym - Puma 560, first 3 joints, symbolic, kine only +% mdl_puma560akb - Puma 560 data (MDH, kine, dyn) +% mdl_p8 - Puma 6-axis robot on a 2-axis XY base +% mdl_S4ABB2p8 - ABB S4 2.8 (DH, kine) +% mdl_stanford - Stanford arm data (DH, kine, dyn) +% mdl_twolink - simple 2-link example (DH, kine) +% DHFactor - convert elementary transformations to DH form +% +% Kinematic +% DHFactor - transform sequence to DH description +% fkine - forward kinematics +% ikine - inverse kinematics (numeric) +% ikine6s - inverse kinematics for 6-axis arm with sph.wrist +% jacob0 - Jacobian in base coordinate frame +% jacobn - Jacobian in end-effector coordinate frame +% maniplty - compute manipulability +% +% Dynamics +% accel - forward dynamics +% cinertia - Cartesian manipulator inertia matrix +% coriolis - centripetal/coriolis torque +% fdyn - forward dynamics +% wtrans - transform a force/moment +% gravload - gravity loading +% inertia - manipulator inertia matrix +% itorque - inertia torque +% rne - inverse dynamics +% +% Mobile robot +% Map - point feature map object +% RandomPath - driver for Vehicle object +% RangeBearingSensor - "laser scanner" object +% Vehicle - construct a mobile robot object +% sl_bicycle - Simulink "bicycle model" of non-holonomic wheeled vehicle +% Navigation - Navigation superclass +% Sensor - robot sensor superclass +% plot_vehicle - plot vehicle +% makemap - make a map, occupancy grid +% mdl_quadcopter - simple quadcopter model +% sl_quadrotor - Simulink model of a flying quadrotor +% +% Localization +% EKF - extended Kalman filter object +% ParticleFilter - Monte Carlo estimator +% +% Path planning +% Bug2 - bug navigation +% DXform - distance transform from map +% Dstar - D* planner +% PRM - probabilistic roadmap planner +% RRT - rapidly exploring random tree +% +% Graphics +% plot2 - plot trajectory +% plotp - plot points +% plot_arrow - draw an arrow +% plot_box - draw a box +% plot_circle - draw a circle +% plot_ellipse - draw an ellipse +% plot_homline - plot homogeneous line +% plot_point - plot points +% plot_poly - plot polygon +% plot_sphere - draw a sphere +% qplot - plot joint angle trajectories +% plot2 - Plot trajectories +% plotp - Plot trajectories +% xaxis - set x-axis scaling +% yaxis - set y-axis scaling +% xyzlabel - label axes x, y and z +% +% Utility +% about - summary of object size and type +% angdiff - subtract 2 angles modulo 2pi +% bresenham - Bresenhan line drawing +% circle - compute/draw points on a circle +% colnorm - columnwise norm of matrix +% diff2 - elementwise diff +% distancexform - compute distance transform +% edgelist - list of edge pixels +% gauss2d - Gaussian distribution in 2D +% ishomog - true if argument is a 4x4 matrix +% ismatrix - true if non scalar +% isrot - true if argument is a 3x3 matrix +% isvec - true if argument is a 3-vector +% numcols - number of columns in matrix +% numrows - number of rows in matrix +% peak - find peak in 1D signal +% peak2 - find peak in 2D signal +% PGraph - general purpose graph class +% polydiff - derivative of polynomial +% Polygon - general purpose polygon class +% randinit - initialize random number generator +% ramp - create linear ramp +% runscript - interactively run a script or demo +% unit - unitize a vector +% tb_optparse - toolbox argument parser +% +% CodeGen support +% distributeblocks - distribute blocks in a Simulink library +% doesblockexist - checks if a Simulink block exists +% multidfprintf - fprintf to multiple files +% symexpr2slblock - embedded Matlab symbolic functions +% simulinkext - determine extension of Simulink files +% +% Demonstrations +% rtbdemo - Serial-link manipulator demonstration +% tripleangle - GUI to demonsrate triple angle rotations +% +% Examples +% sl_quadcopter - Simulink model of a flying quadcopter +% sl_braitenberg - Simulink model a Braitenberg vehicle +% movepoint - non-holonomic vehicle moving to a point +% moveline - non-holonomic vehicle moving to a line +% movepose - non-holonomic vehicle moving to a pose +% walking - example of 4-legged walking robot +% eg_inertia - joint 1 inertia I(q1,q2) +% eg_inertia22 - joint 2 inertia I(q3) +% eg_grav - joint 2 gravity load g(q2,q3) +% +% * located in the examples folder +% +% Copyright (C) 2011 Peter Corke diff --git a/lwrserv/matlab/rvctools/robot/DH.jar b/lwrserv/matlab/rvctools/robot/DH.jar new file mode 100644 index 0000000..4b1b628 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/DH.jar differ diff --git a/lwrserv/matlab/rvctools/robot/DHFactor.java b/lwrserv/matlab/rvctools/robot/DHFactor.java new file mode 100644 index 0000000..71e7319 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/DHFactor.java @@ -0,0 +1,1350 @@ +import java.io.*; +import java.util.*; +import java.lang.*; + +/** + * A class to simplify symbolic transform expressions. + * @author Peter I. Corke peter.i.corke@gmail.com + */ + +/** + * Element is a class that represents one element in a transform expression. + * The transform types represented include: + * TX, TY, TZ pure translation along X, Y and Z axes respectively + * RX, RY, RZ pure rotations about the X, Y and Z axes respectively + * DH Denavit-Hartenberg joint transformation + * + * public boolean istrans() + * public boolean isrot() + * public int axis() + * public boolean isjoint() { + * public boolean factorMatch(int dhWhich, int i, int verbose) { + * public void add(Element e) { + * public Element(int type, int constant) { + * public Element(int type) // new of specified type + * + * public static String toString(Element [] e) { + * public String argString() { + * public String toString() { + * + * Constructors: + * Element(Element e) // clone of argument + * Element(Element e, int type, int sign) // clone of argument with new type + * Element(Element e, int type) // clone of argument with new type + * Element(String s) + */ +class Element { + static final int TX = 0, + TY = 1, + TZ = 2, + RX = 3, + RY = 4, + RZ = 5, + DH_STANDARD = 6, + DH_MODIFIED = 7; + + // one of TX, TY ... RZ, DH_STANDARD/MODIFIED + int type; + + // transform parameters, only one of these is set + String var; // eg. q1, for joint var types + String symconst; // eg. L1, for lengths + int constant; // eg. 90, for angles + + // DH parameters, only set if type is DH_STANDARD/MODIFIED + int theta, + alpha; + String A, + D; + int prismatic; + int offset; + + // an array of counters for the application of each rule + // just for debugging. + static int[] rules = new int[20]; // class variable + + // mapping from type to string + static final String[] typeName = { + "Tx", "Ty", "Tz", "Rx", "Ry", "Rz", "DH", "DHm"}; + + // order of elementary transform for each DH convention + // in each tuple, the first element is the transform type, + // the second is true if it can be a joint variable. + static final int dhStandard[][] = { + {RZ, 1}, {TX, 0}, {TZ, 1}, {RX, 0} }; + static final int dhModified[][] = { + {RX, 0}, {TX, 0}, {RZ, 1}, {TZ, 1} }; + + /* + * Display the number of times each rule was used in the + * conversion. + */ + public static void showRuleUsage() { + for (int i=0; i<20; i++) + if (rules[i] > 0) + System.out.println("Rule " + i + ": " + + rules[i]); + } + + // test if the Element is a translation, eg. TX, TY or TZ + public boolean istrans() { + return (type == TX) || (type == TY) || (type == TZ); + } + + // test if the Element is a rotation, eg. RX, RY or RZ + public boolean isrot() { + return (type == RX) || (type == RY) || (type == RZ); + } + + // true if this transform represents a joint coordinate, ie. not + // a constant + public boolean isjoint() { + return this.var != null; + } + + // return the axis, 0 for X, 1 for Y, 2 for Z + public int axis() { + switch (type) { + case TX: + case RX: + return 0; + case TY: + case RY: + return 1; + case TZ: + case RZ: + return 2; + default: + throw new IllegalArgumentException("bad transform type"); + } + } + + // return the summation of two symbolic parameters as a string + private String symAdd(String s1, String s2) + { + if ( (s1 == null) && (s2 == null) ) + return null; + else if ( (s1 != null) && (s2 == null) ) + return new String(s1); + else if ( (s1 == null) && (s2 != null) ) + return new String(s2); + else { + return s1 + "+" + s2; + } + } + + /** + * Add the argument of another Element to this element. + * assumes that variable has not already been set + * used by factor() to build a DH element + */ + public void add(Element e) { + if ((this.type != DH_STANDARD) && (this.type != DH_MODIFIED)) + throw new IllegalArgumentException("wrong element type " + this); + + System.out.println(" adding: " + this + " += " + e); + switch (e.type) { + case RZ: + if (e.isjoint()) { + this.prismatic = 0; + this.var = e.var; + this.offset = e.constant; + this.theta = 0; + } else + this.theta = e.constant; + break; + case TX: + this.A = e.symconst; break; + case TZ: + if (e.isjoint()) { + this.prismatic = 1; + this.var = e.var; + this.D = null; + } else + this.D = e.symconst; + break; + case RX: + this.alpha = e.constant; break; + default: + throw new IllegalArgumentException("cant factorize " + e); + } + } + /* + public void add(Element e) { + if ((this.type != DH_STANDARD) && (this.type != DH_MODIFIED)) + throw new IllegalArgumentException("wrong element type " + this); + + System.out.println(" adding: " + this + " += " + e); + switch (e.type) { + case RZ: + this.theta = e.argString(); + if (e.isjoint()) + this.prismatic = 0; + break; + case TX: + this.A = e.argString(); break; + case TZ: + this.D = e.argString(); + if (e.isjoint()) + this.prismatic = 1; + break; + case RX: + this.alpha = e.argString(); break; + default: + throw new IllegalArgumentException("cant factorize " + e); + } + } + */ + + + // test if this particular element could be part of a DH term + // eg. Rz(q1) can be, Rx(q1) cannot. + public boolean factorMatch(int dhWhich, int i, int verbose) { + int dhFactors[][]; + boolean match; + + switch (dhWhich) { + case DH_STANDARD: + dhFactors = dhStandard; + break; + case DH_MODIFIED: + dhFactors = dhModified; + break; + default: + throw new IllegalArgumentException("bad DH type"); + } + + match = (this.type == dhFactors[i][0]) && + !((dhFactors[i][1] == 0) && this.isjoint()); + + if (verbose > 0) + System.out.println(" matching " + this + " (i=" + i + ") " + + " to " + typeName[dhFactors[i][0]] + "<" + + dhFactors[i][1] + ">" + " -> " + match); + return match; + } + + /** + * test if two transforms can be merged + * @param e the element to compare with this + * @return - this if no merge to be done + * - null if the result is a null transform + * - new transform resulting from a merge. + */ + Element merge(Element e) { + + /* + * don't merge if dissimilar transform or + * both are joint variables + */ + if ( + (e.type != this.type) || + (e.isjoint() && this.isjoint()) + ) + return this; + + + Element sum = new Element(this); + + sum.var = symAdd(this.var, e.var); + sum.symconst = symAdd(this.symconst, e.symconst); + sum.constant = this.constant + e.constant; + if (Math.abs(sum.constant) > 90) + throw new IllegalArgumentException("rotation angle > 90"); + + /* + * remove a null transform which can result from + * a merge operation + */ + if ( !sum.isjoint() && (sum.symconst == null) && (sum.constant == 0)) { + System.out.println("Eliminate: " + this + " " + e); + return null; + } else { + System.out.println("Merge: " + this + " " + e + " := " + sum); + return sum; + } + + } + + /** + * test if two transforms need to be swapped + * @param e the element to compare with this + * @return - true if swap is required + */ + boolean swap(Element next, int dhWhich) { + /* + * don't swap if both are joint variables + */ + if ( this.isjoint() && next.isjoint() ) + return false; + + switch (dhWhich) { + case Element.DH_STANDARD: + /* + * we want to sort terms into the order: + * RZ + * TX + * TZ + * RX + */ + /* TX TY TZ RX RY RZ */ + int order[] = { 2, 0, 3, 4, 0, 1 }; + if ( + ((this.type == TZ) && (next.type == TX)) || + + /* + * push constant translations through rotational joints + * of the same type + */ + ((this.type == TX) && (next.type == RX) && next.isjoint()) || + ((this.type == TY) && (next.type == RY)) && next.isjoint() || + ((this.type == TZ) && (next.type == RZ)) && next.isjoint() || + + (!this.isjoint() && (this.type == RX) && (next.type == TX)) || + (!this.isjoint() && (this.type == RY) && (next.type == TY)) || + //(!this.isjoint() && (this.type == RZ) && (next.type == TZ)) || + + (!this.isjoint() && !next.isjoint() && (this.type == TZ) && (next.type == RZ)) || + + /* + * move Ty terms to the right + */ + ((this.type == TY) && (next.type == TZ)) || + ((this.type == TY) && (next.type == TX)) + ) { + System.out.println("Swap: " + this + " <-> " + next); + return true; + } + break; + case Element.DH_MODIFIED: + if ( + ((this.type == RX) && (next.type == TX)) || + ((this.type == RY) && (next.type == TY)) || + ((this.type == RZ) && (next.type == TZ)) || + ((this.type == TZ) && (next.type == TX)) + ) { + System.out.println("Swap: " + this + " <-> " + next); + return true; + } + break; + default: + throw new IllegalArgumentException("bad DH type"); + } + return false; + } + + /** + * Substitute this transform for a triple of transforms + * that includes an RZ or TZ. + * + * @return - null if no substituion required + * - array of Elements to substitute + */ + Element[] substituteToZ() { + + Element[] s = new Element[3]; + + switch (this.type) { + case RX: + s[0] = new Element(RY, 90); + s[1] = new Element(this, RZ); + s[2] = new Element(RY, -90); + return s; + case RY: + s[0] = new Element(RX, -90); + s[1] = new Element(this, RZ); + s[2] = new Element(RX, 90); + return s; + case TX: + s[0] = new Element(RY, 90); + s[1] = new Element(this, TZ); + s[2] = new Element(RY, -90); + return s; + case TY: + s[0] = new Element(RX, -90); + s[1] = new Element(this, TZ); + s[2] = new Element(RX, 90); + return s; + default: + return null; + } + } + + Element[] substituteToZ(Element prev) { + + Element[] s = new Element[3]; + + switch (this.type) { + case RY: + s[0] = new Element(RZ, 90); + s[1] = new Element(this, RX); + s[2] = new Element(RZ, -90); + rules[8]++; + return s; + case TY: + if (prev.type == RZ) { + s[0] = new Element(RZ, 90); + s[1] = new Element(this, TX); + s[2] = new Element(RZ, -90); + rules[6]++; + return s; + } else { + s[0] = new Element(RX, -90); + s[1] = new Element(this, TZ); + s[2] = new Element(RX, 90); + rules[7]++; + return s; + } + default: + return null; + } + } + + /** + * Simple rewriting rule for adjacent transform pairs. Attempt to + * eliminate TY and RY. + * @param previous element in list + * @return - null if no substituion required + * - array of Elements to subsitute + */ + Element[] substituteY(Element prev, Element next) { + + Element[] s = new Element[2]; + + if (prev.isjoint() || this.isjoint()) + return null; + + /* note that if rotation is -90 we must make the displacement -ve */ + if ((prev.type == RX) && (this.type == TY)) { + // RX.TY -> TZ.RX + s[0] = new Element(this, TZ, prev.constant); + s[1] = new Element(prev); + rules[0]++; + return s; + } else if ((prev.type == RX) && (this.type == TZ)) { + // RX.TZ -> TY.RX + s[0] = new Element(this, TY, -prev.constant); + s[1] = new Element(prev); + rules[2]++; + return s; + } else if ((prev.type == RY) && (this.type == TX)) { + // RY.TX-> TZ.RY + s[0] = new Element(this, TZ, -prev.constant); + s[1] = new Element(prev); + rules[1]++; + return s; + } else if ((prev.type == RY) && (this.type == TZ)) { + // RY.TZ-> TX.RY + s[0] = new Element(this, TX, prev.constant); + s[1] = new Element(prev); + rules[11]++; + return s; + } else if ((prev.type == TY) && (this.type == RX)) { + // TY.RX -> RX.TZ + s[0] = new Element(this); + s[1] = new Element(prev, TZ, -this.constant); + rules[5]++; + //return s; + return null; + } else if ((prev.type == RY) && (this.type == RX)) { + // RY(Q).RX -> RX.RZ(-Q) + s[0] = new Element(this); + s[1] = new Element(prev, RZ, -1); + rules[3]++; + return s; + } else if ((prev.type == RX) && (this.type == RY)) { + // RX.RY -> RZ.RX + s[0] = new Element(this, RZ); + s[1] = new Element(prev); + rules[4]++; + return s; + } else if ((prev.type == RZ) && (this.type == RX)) { + // RZ.RX -> RX.RY + s[0] = new Element(this); + s[1] = new Element(prev, RY); + //rules[10]++; + //return s; + return null; + } + return null; + } + + /* + * Element contructors. String is of the form: + */ + public Element(int type, int constant) { + this.type = type; + this.var = null; + this.symconst = null; + this.constant = constant; + } + + public Element(int type) { // new of specified type + this.type = type; + } + + public Element(Element e) { // clone of argument + this.type = e.type; + if (e.var != null) + this.var = new String(e.var); + if (e.symconst != null) + this.symconst = new String(e.symconst); + this.constant = e.constant; + } + + /** + * Constructor for Element. + * @param e Template for new Element. + * @param type Replacement type for new Element. + * @param sign Sign of argument, either -1 or +1. + * @return a new Element with specified type and argument. + */ + public Element(Element e, int type, int sign) { // clone of argument with new type + this.type = type; + if (e.var != null) + this.var = new String(e.var); + this.constant = e.constant; + if (e.symconst != null) + this.symconst = new String(e.symconst); + + if (sign < 0) + this.negate(); + } + + public Element(Element e, int type) { // clone of argument with new type + this(e, type, 1); + } + + // negate the arguments of the element + public void negate() { + //System.out.println("negate: " + this.constant + " " + this.symconst); + + // flip the numeric part, easy + this.constant = -this.constant; + + + if (this.symconst != null) { + StringBuffer s = new StringBuffer(this.symconst); + // if no leading sign character insert one (so we can flip it) + if ((s.charAt(0) != '+') && + (s.charAt(0) != '-') + ) + s.insert(0, '+'); + + // go through the string and flip all sign chars + for (int i=0; i= 6) + throw(new IllegalArgumentException("bad transform name" + sType)); + type = i; + + sRest = sRest.substring(1, sRest.length()-1); + switch (sRest.charAt(0)) { + case 'q': + var = sRest; + break; + case 'L': + symconst = sRest; + break; + default: + try { + constant = Integer.parseInt(sRest); + } + catch(NumberFormatException e) { + System.err.println(e.getMessage()); + throw(new IllegalArgumentException("bracket contents")); + } + } + } + + // class method to convert Element vector to string + /* + public static String toString(Element [] e) { + String s = ""; + + for (int i=0; i 0) + s += "+" + offset; + else if (offset < 0) + s += offset; + } else + s += theta; + s += ", "; + + // d + if (prismatic > 0) + s += var; + else + s += (D == null) ? "0" : D; + s += ", "; + + // a + s += (A == null) ? "0" : A; + s += ", "; + + // alpha + s += alpha; + break; + default: + throw new IllegalArgumentException("bad Element type"); + } + return s; + } + + /* + * Return a string representation of the element. + * eg. Rz(q1), Tx(L1), Rx(90), DH(....) + */ + public String toString() { + + String s = typeName[type] + "("; + s += argString(); + s += ")"; + return s; + } + + /* + public String rotation() { + } + + public String translation() { + } + */ +} + +/********************************************************************** +/* A list of Elements. Subclass of Java's arrayList + * + * public int factorize(int dhWhich, int verbose) + * + * public int floatRight() { + * public int swap(int dhWhich) { + * public int substituteToZ() { + * public int substituteToZ2() { + * public int substituteY() { + * public int merge() { + * public void simplify() { + * public ElementList() { // constructor, use superclass + * public String toString() { + */ +class ElementList extends ArrayList { + + /** + * Attempt to group this and subsequent elements into a DH term + * @return: the number of factors matched, zero means no DH term found + * + * Modifies the ElementList and compresses the terms. + */ + public int factorize(int dhWhich, int verbose) { + + int match, jvars; + int i, j, f; + Element e; + int nfactors = 0; + + for (i=0; i= this.size()) + break; + e = (Element) this.get(j); + if ((f == 0) && (verbose > 0)) + System.out.println("Starting at " + e); + if (e.factorMatch(dhWhich, f, verbose) + ) { + j++; // move on to next element + match++; + if (e.isjoint()) + jvars++; + if (jvars > 1) // can only have 1 joint var per DH + break; + } + } + + if ((match == 0) || (jvars == 0)) + continue; // no DH subexpression found, keep looking + + int start, end; + if (verbose > 0) + System.out.println(" found subexpression " + match + " " + jvars); + + start = i; + end = j; + if (jvars > 1) + end--; + + Element dh = new Element(dhWhich); + + for (j=start; j 0) + System.out.println(" result: " + dh); + } + return nfactors; + } + + /** + * Attempt to 'float' translational terms as far to the right as + * possible and across joint boundaries. + */ + public int floatRight() { + Element e, f = null; + int nchanges = 0; + int i, j; + boolean crossed; + + for (i=0; i<(this.size()-1); i++) { + e = (Element) this.get(i); + if (e.isjoint()) + continue; + if (!e.istrans()) + continue; + f = null; + crossed = false; + for (j=i+1; j<(this.size()-1); j++) { + f = (Element) this.get(j); + if (f.istrans()) + continue; + if (f.isrot() && (f.axis() == e.axis())) { + crossed = true; + continue; + } + break; + } + if (crossed && (f != null)) { + System.out.println("Float: " + e + " to " + f); + this.remove(i); + this.add(j-1, e); + nchanges++; + i--; + } + } + + return nchanges; + } + + /** + * Swap adjacent terms according to inbuilt rules so as to achieve + * desired term ordering. + */ + public int swap(int dhWhich) { + Element e; + int total_changes = 0; + int nchanges = 0; + + do { + nchanges = 0; + + for (int i=0; i<(this.size()-1); i++) { + e = (Element) this.get(i); + if (e.swap( (Element) this.get(i+1), dhWhich)) { + this.remove(i); + this.add(i+1, e); + nchanges++; + } + } + total_changes += nchanges; + } while (nchanges > 0); + + return total_changes; + } + + /** + * substitute all non Z joint transforms according to rules. + */ + public int substituteToZ() { + Element e; + Element[] replacement; + int nchanges = 0; + + for (int i=0; i 0) && (nloops++ < 10)); + } + + public ElementList() { // constructur, use superclass + super(); + } + + public String toString() { + String s = ""; + + for (int i=0; i= 0) + return s.substring(i); + i = s.indexOf("-"); + if (i >= 0) + return s.substring(i); + } + return "0"; + } + + static String convertMatlab(String s) + { + if (s == null) + return " 0"; + return " " + s; + } + + public String toMatlab(String robot) { + String dh = "["; + String offs = "["; + String base = ""; + String tool = ""; + String theta, a, d, alpha; + int dhSeenYet = 0; + Element e; + + for (int i=0; i 0) + xform = xform.substring(2); + + // assign this string to base or tool depending on + // whether or not we've seen the DH terms go by + if (dhSeenYet == 0) + base = xform; + else + tool = xform; + } + } + dh += "]"; + offs += "]"; + + // build the matlab string + + s = "robot(" + dh + ", " + robot ; + + if (base.length() > 0) + s += ", 'base', " + base; + if (tool.length() > 0) + s += ", 'tool', " + tool; + s += ");"; + + return s; + } + */ +} + +public class DHFactor { + + ElementList results; + + // Matlab callable constructor + public DHFactor(String src) { + results = parseString(src); + if (!this.isValid()) + System.out.println("DHFactor: error: Incomplete factorization, no DH equivalent found"); + } + + private String angle(Element e) { + return angle(e.constant); + } + + + public String toString() { + return results.toString(); + } + + public String display() { + return results.toString(); + } + + private String angle(int a) + { + if (a == 0) + return "0"; + else if (a == 90) + return "pi/2"; + else if (a == -90) + return "-pi/2"; + else + throw new IllegalArgumentException("bad transform angle"); + } + + private String el2matlab(int from, int to) + { + String xform = ""; + int i; + + for (i=from; i 0) + xform += "*"; + + switch (e.type) { + case Element.RX: xform += "trotx(" + angle(e) + ")"; break; + case Element.RY: xform += "troty(" + angle(e) + ")"; break; + case Element.RZ: xform += "trotz(" + angle(e) + ")"; break; + case Element.TX: xform += "transl(" + e.symconst + ",0,0)"; break; + case Element.TY: xform += "transl(0, " + e.symconst + ",0)"; break; + case Element.TZ: xform += "transl(0,0," + e.symconst + ")"; break; + } + } + if (xform.length() == 0) + xform = "eye(4,4)"; + return xform; + } + + /* + * Create a Toolbox legacy DH matrix. The column order is: + * + * theta d a alpha + */ + public String dh() { + String s = "["; + String theta, d; + Element e; + + for (int i=0; i= 0) { + // we've seen a DH factor before + if ((i-iprev) > 1) { + // but it was too long ago, fail! + return false; + } + } + iprev = i; // note where we saw it + }; + } + return true; + } + + public String offset() { + String s = "["; + Element e; + + for (int i=0; i=0; i--) { + Element e = (Element)results.get(i); + + if ( (e.type == Element.DH_STANDARD) || (e.type == Element.DH_MODIFIED) ) + return el2matlab(i, results.size()); + } + + return "eye(4,4)"; + } + + + // return Matlab Toolbox robot creation command + public String command(String name) { + if (this.isValid()) + return "SerialLink(" + this.dh() + ", 'name', '" + name + + "', 'base', " + this.base() + + ", 'tool', " + this.tool() + + ", 'offset', " + this.offset() + ")"; + else + return "error('incompletely factored transform string')"; + } + + public static ElementList parseFile(String filename) { + BufferedReader src; + String buffer; + + try { + File file = new File(filename); + + if (!file.canRead() || !file.isFile()) + throw new IOException("dh: file access/type error"); + + src = new BufferedReader(new FileReader(file)); + + // read the file and parse it + src = new BufferedReader(new FileReader(file)); + buffer = src.readLine(); + + return parseString(buffer); + } + catch (FileNotFoundException e) { + System.err.println(e.getMessage()); + System.exit(1); + } + catch (IOException e) { + System.err.println(e.getMessage()); + System.exit(1); + } + return null; + } + + public static ElementList parseString(String buffer) { + ElementList l = new ElementList(); + + try { + System.out.println(buffer); + StringTokenizer tokens = new StringTokenizer(buffer, " *."); + + while (tokens.hasMoreTokens()) + l.add( new Element(tokens.nextToken()) ); + + System.out.println(l); + + l.simplify(); + System.out.println(l); + + l.factorize(Element.DH_STANDARD, 0); + System.out.println(l); + + return l; + } + catch (IllegalArgumentException e) { + System.err.println(e.getMessage()); + System.exit(1); + } + return null; + } + + // command line instantiation + // dhfactor file + // dhfactor < stdin + public static void main(String args[]) { + + if (args.length > 0) { + + ElementList l = parseFile(args[0]); + System.err.println( l ); + + } else { + System.err.println("no file name specified\n"); + Element.showRuleUsage(); + System.exit(1); + } + } +} diff --git a/lwrserv/matlab/rvctools/robot/DHFactor.m b/lwrserv/matlab/rvctools/robot/DHFactor.m new file mode 100644 index 0000000..06ebbeb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/DHFactor.m @@ -0,0 +1,65 @@ +%DHFactor Simplify symbolic link transform expressions +% +% F = DHFactor(S) is an object that encodes the kinematic model of a robot +% provided by a string S that represents a chain of elementary transforms from +% the robot's base to its tool tip. The chain of elementary rotations and +% translations is symbolically factored into a sequence of link transforms +% described by DH parameters. +% +% For example: +% s = 'Rz(q1).Rx(q2).Ty(L1).Rx(q3).Tz(L2)'; +% indicates a rotation of q1 about the z-axis, then rotation of q2 about the +% x-axis, translation of L1 about the y-axis, rotation of q3 about the x-axis +% and translation of L2 along the z-axis. +% +% Methods:: +% +% base the base transform as a Java string +% tool the tool transform as a Java string +% command a command string that will create a SerialLink() object +% representing the specified kinematics +% char convert to string representation +% display display in human readable form +% +% Example:: +% +% >> s = 'Rz(q1).Rx(q2).Ty(L1).Rx(q3).Tz(L2)'; +% >> dh = DHFactor(s); +% >> dh +% DH(q1+90, 0, 0, +90).DH(q2, L1, 0, 0).DH(q3-90, L2, 0, 0).Rz(+90).Rx(-90).Rz(-90) +% >> r = eval( dh.command('myrobot') ); +% +% Notes:: +% - Variables starting with q are assumed to be joint coordinates +% - Variables starting with L are length constants. +% - Length constants must be defined in the workspace before executing the +% last line above. +% - Implemented in Java +% - Not all sequences can be converted to DH format, if conversion cannot be +% achieved an error is generated. +% +% Reference:: +% - A simple and systematic approach to assigning Denavit-Hartenberg parameters, +% P.Corke, IEEE Transaction on Robotics, vol. 23, pp. 590-594, June 2007. +% - Robotics, Vision & Control, Sec 7.5.2, 7.7.1, +% Peter Corke, Springer 2011. +% +% See also SerialLink. + +% 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 . +% See also SerialLink. diff --git a/lwrserv/matlab/rvctools/robot/DXform.m b/lwrserv/matlab/rvctools/robot/DXform.m new file mode 100644 index 0000000..f1544e8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/DXform.m @@ -0,0 +1,250 @@ +%DXform Distance transform navigation class +% +% A concrete subclass of the Navigation class that implements the distance +% transform navigation algorithm which computes minimum distance paths. +% +% Methods:: +% +% plan Compute the cost map given a goal and map +% path Compute a path to the goal +% visualize Display the obstacle map (deprecated) +% plot Display the distance function and obstacle map +% plot3d Display the distance function as a surface +% display Print the parameters in human readable form +% char Convert to string +% +% Properties:: +% +% distancemap The distance transform of the occupancy grid. +% metric The distance metric, can be 'euclidean' (default) or 'cityblock' +% +% Example:: +% +% load map1 % load map +% goal = [50,30]; % goal point +% start = [20, 10]; % start point +% dx = DXform(map); % create navigation object +% dx.plan(goal) % create plan for specified goal +% dx.path(start) % animate path from this start location +% +% Notes:: +% - Obstacles are represented by NaN in the distancemap. +% - The value of each element in the distancemap is the shortest distance from the +% corresponding point in the map to the current goal. +% +% References:: +% - Robotics, Vision & Control, Sec 5.2.1, +% Peter Corke, Springer, 2011. +% +% See also Navigation, Dstar, PRM, distancexform. + +% 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 . + +classdef DXform < Navigation + + properties + metric; % distance metric + distancemap; % distance transform results + end + + methods + + function dx = DXform(world, varargin) + %DXform.DXform Distance transform constructor + % + % DX = DXform(MAP, OPTIONS) is a distance transform navigation object, + % and MAP is an occupancy grid, a representation of a planar + % world as a matrix whose elements are 0 (free space) or 1 + % (occupied). + % + % Options:: + % 'goal',G Specify the goal point (2x1) + % 'metric',M Specify the distance metric as 'euclidean' (default) + % or 'cityblock'. + % 'inflate',K Inflate all obstacles by K cells. + % + % Other options are supported by the Navigation superclass. + % + % See also Navigation.Navigation. + + % TODO NEEDS PROPER ARG HANDLER + + + % invoke the superclass constructor + dx = dx@Navigation(world, varargin{:}); + + opt.metric = {'euclidean', 'cityblock'}; + [opt,args] = tb_optparse(opt, varargin); + dx.metric = opt.metric; + + + end + + function s = char(dx) + %DXform.char Convert to string + % + % DX.char() is a string representing the state of the object in + % human-readable form. + % + % See also DXform.display, Navigation.char + + % most of the work is done by the superclass + s = char@Navigation(dx); + + % dxform specific stuff + s = char(s, sprintf(' distance metric: %s', dx.metric)); + if ~isempty(dx.distancemap) + s = char(s, sprintf(' distancemap: computed:')); + else + s = char(s, sprintf(' distancemap: empty:')); + end + end + + % invoked by superclass on a change of goal, mark the distancemap + % as invalid + function goal_change(dx, goal) + + dx.distancemap = []; + if dx.verbose + disp('Goal changed -> distancemap cleared'); + end + end + + + function plan(dx, goal, show) + %DXform.plan Plan path to goal + % + % DX.plan() updates the internal distancemap where the value of each element is + % the minimum distance from the corresponding point to the goal. The goal is + % as specified to the constructor. + % + % DX.plan(GOAL) as above but uses the specified goal. + % + % DX.plan(GOAL, S) as above but displays the evolution of the + % distancemap, with one iteration displayed every S seconds. + % + % Notes:: + % - This may take many seconds. + + if nargin < 3 + show = 0; + end + + if nargin > 1 + dx.goal = goal; + end + + if isempty(dx.goal) + error('No goal specified'); + end + + %dx.occgrid(dx.goal(2), dx.goal(1)) + dx.distancemap = distancexform(dx.occgrid, dx.goal, dx.metric, show); + + end + + function plot(dx, varargin) + %DXform.plot Visualize navigation environment + % + % DX.plot() displays the occupancy grid and the goal distance + % in a new figure. The goal distance is shown by intensity which + % increases with distance from the goal. Obstacles are overlaid + % and shown in red. + % + % DX.plot(P) as above but also overlays a path given by the set + % of points P (Mx2). + % + % See also Navigation.plot. + + plot@Navigation(dx, 'distance', dx.distancemap, varargin{:}); + + end + + function n = next(dx, robot) + if isempty(dx.distancemap) + error('No distancemap computed, you need to plan'); + end + + % list of all possible directions to move from current cell + directions = [ + -1 -1 + 0 -1 + 1 -1 + -1 0 + 0 0 + 1 0 + -1 1 + 0 1 + 1 1]; + + x = robot(1); y = robot(2); + + % find the neighbouring cell that has the smallest distance + mindist = Inf; + mindir = []; + for d=directions' + % use exceptions to catch attempt to move outside the map + try + if dx.distancemap(y+d(1), x+d(2)) < mindist + mindir = d; + mindist = dx.distancemap(y+d(1), x+d(2)); + end + catch + end + end + + x = x + mindir(2); + y = y + mindir(1); + + if all([x;y] == dx.goal) + n = []; % indicate we are at the goal + else + n = [x; y]; % else return the next closest point to the goal + end + end % next + + function plot3d(dx, p, varargin) + %DXform.plot3d 3D costmap view + % + % DX.plot3d() displays the distance function as a 3D surface with + % distance from goal as the vertical axis. Obstacles are "cut out" + % from the surface. + % + % DX.plot3d(P) as above but also overlays a path given by the set + % of points P (Mx2). + % + % DX.plot3d(P, LS) as above but plot the line with the linestyle LS. + % + % See also Navigation.plot. + surf(dx.distancemap); + shading interp + + if nargin > 1 + % plot path if provided + k = sub2ind(size(dx.distancemap), p(:,2), p(:,1)); + height = dx.distancemap(k); + hold on + if isempty(varargin) + varargin{1} = 'k.'; + end + plot3(p(:,1), p(:,2), height, varargin{:}) + hold off + end + end + end % methods +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/Dstar.m b/lwrserv/matlab/rvctools/robot/Dstar.m new file mode 100644 index 0000000..95c7e25 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Dstar.m @@ -0,0 +1,515 @@ +%Dstar D* navigation class +% +% A concrete subclass of the Navigation class that implements the D* +% navigation algorithm. This provides minimum distance paths and +% facilitates incremental replanning. +% +% Methods:: +% plan Compute the cost map given a goal and map +% path Compute a path to the goal +% visualize Display the obstacle map (deprecated) +% plot Display the obstacle map +% costmap_modify Modify the costmap +% modify_cost Modify the costmap (deprecated, use costmap_modify) +% costmap_get Return the current costmap +% costmap_set Set the current costmap +% distancemap_get Set the current distance map +% display Print the parameters in human readable form +% char Convert to string +% +% Properties:: +% costmap Distance from each point to the goal. +% +% Example:: +% load map1 % load map +% goal = [50,30]; +% start=[20,10]; +% ds = Dstar(map); % create navigation object +% ds.plan(goal) % create plan for specified goal +% ds.path(start) % animate path from this start location +% +% Notes:: +% - Obstacles are represented by Inf in the costmap. +% - The value of each element in the costmap is the shortest distance from the +% corresponding point in the map to the current goal. +% +% References:: +% - The D* algorithm for real-time planning of optimal traverses, +% A. Stentz, +% Tech. Rep. CMU-RI-TR-94-37, The Robotics Institute, Carnegie-Mellon University, 1994. +% - Robotics, Vision & Control, Sec 5.2.2, +% Peter Corke, Springer, 2011. +% +% See also Navigation, DXform, PRM. + +% 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 . + + + +% Implementation notes: +% +% All the state is kept in the structure called d +% X is an index into the array of states. +% state pointers are kept as matlab array index rather than row,col format + +%TODO use pgraph class + +% pic 7/09 + +classdef Dstar < Navigation + + properties (SetAccess=private, GetAccess=private) + + costmap % world cost map: obstacle = Inf + G % index of goal point + + % info kept per cell (state) + b % backpointer (0 means not set) + t % tag: NEW OPEN CLOSED + h % distance map, path cost + + % list of open states: 2xN matrix + % each open point is a column, row 1 = index of cell, row 2 = k + openlist + + niter + + changed + + openlist_maxlen % keep track of maximum length + quiet + + % tag state values + NEW = 0; + OPEN = 1; + CLOSED = 2; + end + + methods + + % constructor + function ds = Dstar(world, varargin) + %Dstar.Dstar D* constructor + % + % DS = Dstar(MAP, OPTIONS) is a D* navigation object, and MAP is an + % occupancy grid, a representation of a planar world as a + % matrix whose elements are 0 (free space) or 1 (occupied). + % The occupancy grid is coverted to a costmap with a unit cost + % for traversing a cell. + % + % Options:: + % 'goal',G Specify the goal point (2x1) + % 'metric',M Specify the distance metric as 'euclidean' (default) + % or 'cityblock'. + % 'inflate',K Inflate all obstacles by K cells. + % 'quiet' Don't display the progress spinner + % + % Other options are supported by the Navigation superclass. + % + % See also Navigation.Navigation. + + % invoke the superclass constructor + ds = ds@Navigation(world, varargin{:}); + + opt.quiet = false; + opt = tb_optparse(opt, varargin); + ds.quiet = opt.quiet; + + ds.occgrid2costmap(ds.occgrid); + + + % init the D* state variables + ds.reset(); + if ~isempty(ds.goal) + ds.goal_change(); + end + ds.changed = false; + end + + function reset(ds) + %Dstar.reset Reset the planner + % + % DS.reset() resets the D* planner. The next instantiation + % of DS.plan() will perform a global replan. + + % build the matrices required to hold the state of each cell for D* + ds.b = zeros(size(ds.costmap), 'uint32'); % backpointers + ds.t = zeros(size(ds.costmap), 'uint8'); % tags + ds.h = Inf*ones(size(ds.costmap)); % path cost estimate + ds.openlist = zeros(2,0); % the open list, one column per point + + ds.openlist_maxlen = -Inf; + end + + function goal_change(ds) + + if isempty(ds.b) + return; + end + goal = ds.goal; + + % keep goal in index rather than row,col format + ds.G = sub2ind(size(ds.occgrid), goal(2), goal(1)); + ds.INSERT(ds.G, 0, 'goalset'); + ds.h(ds.G) = 0; + end + + function s = char(ds) + %Dstar.char Convert navigation object to string + % + % DS.char() is a string representing the state of the Dstar + % object in human-readable form. + % + % See also Dstar.display, Navigation.char. + + % most of the work is done by the superclass + s = char@Navigation(ds); + + % Dstar specific stuff + if ~isempty(ds.costmap) + s = char(s, sprintf(' costmap: %dx%d, open list %d', size(ds.costmap), numcols(ds.openlist))); + else + s = char(s, sprintf(' costmap: empty:')); + end + end + + function plot(ds, varargin) + %Dstar.plot Visualize navigation environment + % + % DS.plot() displays the occupancy grid and the goal distance + % in a new figure. The goal distance is shown by intensity which + % increases with distance from the goal. Obstacles are overlaid + % and shown in red. + % + % DS.plot(P) as above but also overlays a path given by the set + % of points P (Mx2). + % + % See also Navigation.plot. + + plot@Navigation(ds, 'distance', ds.h, varargin{:}); + end + + % invoked by Navigation.step + function n = next(ds, current) + + if ds.changed + error('Cost map has changed, replan'); + end + X = sub2ind(size(ds.costmap), current(2), current(1)); + X = ds.b(X); + if X == 0 + n = []; + else + [r,c] = ind2sub(size(ds.costmap), X); + n = [c;r]; + end + end + + function plan(ds, goal) + %Dstar.plan Plan path to goal + % + % DS.plan() updates DS with a costmap of distance to the + % goal from every non-obstacle point in the map. The goal is + % as specified to the constructor. + % + % DS.plan(GOAL) as above but uses the specified goal. + % + % Note:: + % - If a path has already been planned, but the costmap was + % modified, then reinvoking this method will replan, + % incrementally updating the plan at lower cost than a full + % replan. + + + if nargin > 1 + ds.goal = goal; + end + % for replanning no goal is needed, + if isempty(ds.goal) + error('must specify a goal point'); + end + + ds.niter = 0; + while true + if ~ds.quiet && mod(ds.niter, 20) == 0 + ds.spinner(); + end + ds.niter = ds.niter + 1; + + if ds.PROCESS_STATE() < 0 + break; + end + if ds.verbose + disp(' ') + end + end + if ~ds.quiet + fprintf('\r'); + end + ds.changed = false; + end + + function c = distancemap_get(ds) + %Dstar.distancemap_get Get the current distance map + % + % C = DS.distancemap_get() is the current distance map. This map is the same size + % as the occupancy grid and the value of each element is the shortest distance + % from the corresponding point in the map to the current goal. It is computed + % by Dstar.plan. + % + % See also Dstar.plan. + c = ds.h; + end + + % functions should be more consistently named: + % costmap_set + % costmap_get + % costmap_modify + + function c = costmap_get(ds) + %Dstar.costmap_get Get the current costmap + % + % C = DS.costmap_get() is the current costmap. The cost map is the same size + % as the occupancy grid and the value of each element represents the cost + % of traversing the cell. It is autogenerated by the class constructor from + % the occupancy grid such that: + % - free cell (occupancy 0) has a cost of 1 + % - occupied cell (occupancy >0) has a cost of Inf + % + % See also Dstar.costmap_set, Dstar.costmap_modify. + + c = ds.costmap; + end + + function costmap_set(ds, costmap) + %Dstar.costmap_set Set the current costmap + % + % DS.costmap_set(C) sets the current costmap. The cost map is the same size + % as the occupancy grid and the value of each element represents the cost + % of traversing the cell. A high value indicates that the cell is more costly + % (difficult) to traverese. A value of Inf indicates an obstacle. + % + % Notes:: + % - After the cost map is changed the path should be replanned by + % calling DS.plan(). + % + % See also Dstar.costmap_get, Dstar.costmap_modify. + if ~all(size(costmap) == size(ds.occgrid)) + error('costmap must be same size as occupancy grid'); + end + ds.costmap = costmap; + ds.changed = true; + end + + function costmap_modify(ds, point, newcost) + %Dstar.costmap_modify Modify cost map + % + % DS.costmap_modify(P, NEW) modifies the cost map at P=[X,Y] to + % have the value NEW. If P (2xM) and NEW (1xM) then the cost of + % the points defined by the columns of P are set to the corresponding + % elements of NEW. + % + % Notes:: + % - After one or more point costs have been updated the path + % should be replanned by calling DS.plan(). + % - Replaces modify_cost, same syntax. + % + % See also Dstar.costmap_set, Dstar.costmap_get. + + modify_cost(ds, point, newcost); + end + + function modify_cost(ds, point, newcost) + %Dstar.modify_cost Modify cost map + % + % Notes:: + % - Deprecated: use modify_cost instead instead. + % + % See also Dstar.costmap_set, Dstar.costmap_get. + + if numel(point) == 2 + % for case of single point ensure it is a column vector + point = point(:); + end + if numcols(point) ~= numcols(newcost) + error('number of columns in point must match columns in newcost'); + end + for i=1:numcols(point) + X = sub2ind(size(ds.costmap), point(2,i), point(1,i)); + ds.costmap(X) = newcost(i); + end + if ds.t(X) == ds.CLOSED + ds.INSERT(X, ds.h(X), 'modifycost'); + end + ds.changed = true; + end + end % public methods + + methods (Access=protected) + + function occgrid2costmap(ds, og, cost) + if nargin < 3 + cost = 1; + end + ds.costmap = og; + ds.costmap(ds.costmap==1) = Inf; % occupied cells have Inf driving cost + ds.costmap(ds.costmap==0) = cost; % unoccupied cells have driving cost + end + + % The main D* function as per the Stentz paper, comments Ln are the original + % line numbers. + function r = PROCESS_STATE(d) + + %% states with the lowest k value are removed from the + %% open list + X = d.MIN_STATE(); % L1 + + if isempty(X) % L2 + r = -1; + return; + end + + k_old = d.GET_KMIN(); d.DELETE(X); % L3 + + if k_old < d.h(X) % L4 + d.message('k_old < h(X): %f %f\n', k_old, d.h(X)); + for Y=d.neighbours(X) % L5 + if (d.h(Y) <= k_old) && (d.h(X) > d.h(Y)+d.c(Y,X)) % L6 + d.b(X) = Y; + d.h(X) = d.h (Y) + d.c(Y,X); % L7 + end + end + end + + %% can we lower the path cost of any neighbours? + if k_old == d.h(X) % L8 + d.message('k_old == h(X): %f\n', k_old); + for Y=d.neighbours(X) % L9 + if (d.t(Y) == d.NEW) || ... % L10-12 + ( (d.b(Y) == X) && (d.h(Y) ~= (d.h(X) + d.c(X,Y))) ) || ... + ( (d.b(Y) ~= X) && (d.h(Y) > (d.h(X) + d.c(X,Y))) ) + d.b(Y) = X; d.INSERT(Y, d.h(X)+d.c(X,Y), 'L13'); % L13 + end + end + else % L14 + d.message('k_old > h(X)'); + for Y=d.neighbours(X) % L15 + if (d.t(Y) == d.NEW) || ( (d.b(Y) == X) && (d.h(Y) ~= (d.h(X) + d.c(X,Y))) ) + d.b(Y) = X; d.INSERT(Y, d.h(X)+d.c(X,Y), 'L18'); % L18 + else + if ( (d.b(Y) ~= X) && (d.h(Y) > (d.h(X) + d.c(X,Y))) ) + d.INSERT(X, d.h(X), 'L21'); % L21 + else + if (d.b(Y) ~= X) && (d.h(X) > (d.h(Y) + d.c(Y,X))) && ... + (d.t(Y) == d.CLOSED) && d.h(Y) > k_old + d.INSERT(Y, d.h(Y), 'L25'); % L25 + end + end + end + end + end + + r = 0; + return; + end % process_state(0 + + function kk = k(ds, X) + i = ds.openlist(1,:) == X; + kk = ds.openlist(2, i); + end + + function INSERT(ds, X, h_new, where) + + % where is for diagnostic purposes only + ds.message('insert (%s) %d = %f\n', where, X, h_new); + + i = find(ds.openlist(1,:) == X); + if length(i) > 1 + error('D*:INSERT: state in open list %d times', X); + end + + if ds.t(X) == ds.NEW + k_new = h_new; + % add a new column to the open list + ds.openlist = [ds.openlist [X; k_new]]; + elseif ds.t(X) == ds.OPEN + k_new = min( ds.openlist(2,i), h_new ); + elseif ds.t(X) == ds.CLOSED + k_new = min( ds.h(X), h_new ); + % add a new column to the open list + ds.openlist = [ds.openlist [X; k_new]]; + end + + if numcols(ds.openlist) > ds.openlist_maxlen + ds.openlist_maxlen = numcols(ds.openlist); + end + + ds.h(X) = h_new; + ds.t(X) = ds.OPEN; + end + + function DELETE(ds, X) + ds.message('delete %d\n', X); + i = find(ds.openlist(1,:) == X); + if length(i) ~= 1 + error('D*:DELETE: state %d doesnt exist', X); + end + ds.openlist(:,i) = []; % remove the column + ds.t(X) = ds.CLOSED; + end + + % return the index of the open state with the smallest k value + function ms = MIN_STATE(ds) + if isempty(ds.openlist) + ms = []; + else + % find the minimum k value on the openlist + [~,i] = min(ds.openlist(2,:)); + + % return its index + ms = ds.openlist(1,i); + end + end + + function kmin = GET_KMIN(ds) + kmin = min(ds.openlist(2,:)); + end + + % return the cost of moving from state X to state Y + function cost = c(ds, X, Y) + [r,c] = ind2sub(size(ds.costmap), [X; Y]); + dist = sqrt(sum(diff([r c]).^2)); + dcost = (ds.costmap(X) + ds.costmap(Y))/2; + + cost = dist * dcost; + end + + % return index of neighbour states as a row vector + function Y = neighbours(ds, X) + dims = size(ds.costmap); + [r,c] = ind2sub(dims, X); + + % list of 8-way neighbours + Y = [r-1 r-1 r-1 r r r+1 r+1 r+1; c-1 c c+1 c-1 c+1 c-1 c c+1]; + k = (min(Y)>0) & (Y(1,:)<=dims(1)) & (Y(2,:)<=dims(2)); + Y = Y(:,k); + Y = sub2ind(dims, Y(1,:)', Y(2,:)')'; + end + + end % protected methods +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/EKF.m b/lwrserv/matlab/rvctools/robot/EKF.m new file mode 100644 index 0000000..7ee8f7d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/EKF.m @@ -0,0 +1,918 @@ +%EKF Extended Kalman Filter for navigation +% +% This class can be used for: +% - dead reckoning localization +% - map-based localization +% - map making +% - simultaneous localization and mapping (SLAM) +% +% It is used in conjunction with: +% - a kinematic vehicle model that provides odometry output, represented +% by a Vehicle object. +% - The vehicle must be driven within the area of the map and this is +% achieved by connecting the Vehicle object to a Driver object. +% - a map containing the position of a number of landmark points and is +% represented by a Map object. +% - a sensor that returns measurements about landmarks relative to the +% vehicle's location and is represented by a Sensor object subclass. +% +% The EKF object updates its state at each time step, and invokes the +% state update methods of the Vehicle. The complete history of estimated +% state and covariance is stored within the EKF object. +% +% Methods:: +% run run the filter +% plot_xy plot the actual path of the vehicle +% plot_P plot the estimated covariance norm along the path +% plot_map plot estimated feature points and confidence limits +% plot_ellipse plot estimated path with covariance ellipses +% plot_error plot estimation error with standard deviation bounds +% display print the filter state in human readable form +% char convert the filter state to human readable string +% +% Properties:: +% x_est estimated state +% P estimated covariance +% V_est estimated odometry covariance +% W_est estimated sensor covariance +% features maps sensor feature id to filter state element +% robot reference to the Vehicle object +% sensor reference to the Sensor subclass object +% history vector of structs that hold the detailed filter state from +% each time step +% verbose show lots of detail (default false) +% joseph use Joseph form to represent covariance (default true) +% +% Vehicle position estimation (localization):: +% +% Create a vehicle with odometry covariance V, add a driver to it, +% create a Kalman filter with estimated covariance V_est and initial +% state covariance P0 +% veh = Vehicle(V); +% veh.add_driver( RandomPath(20, 2) ); +% ekf = EKF(veh, V_est, P0); +% We run the simulation for 1000 time steps +% ekf.run(1000); +% then plot true vehicle path +% veh.plot_xy('b'); +% and overlay the estimated path +% ekf.plot_xy('r'); +% and overlay uncertainty ellipses at every 20 time steps +% ekf.plot_ellipse(20, 'g'); +% We can plot the covariance against time as +% clf +% ekf.plot_P(); +% +% Map-based vehicle localization:: +% +% Create a vehicle with odometry covariance V, add a driver to it, +% create a map with 20 point features, create a sensor that uses the map +% and vehicle state to estimate feature range and bearing with covariance +% W, the Kalman filter with estimated covariances V_est and W_est and initial +% vehicle state covariance P0 +% veh = Vehicle(V); +% veh.add_driver( RandomPath(20, 2) ); +% map = Map(20); +% sensor = RangeBearingSensor(veh, map, W); +% ekf = EKF(veh, V_est, P0, sensor, W_est, map); +% We run the simulation for 1000 time steps +% ekf.run(1000); +% then plot the map and the true vehicle path +% map.plot(); +% veh.plot_xy('b'); +% and overlay the estimatd path +% ekf.plot_xy('r'); +% and overlay uncertainty ellipses at every 20 time steps +% ekf.plot_ellipse([], 'g'); +% We can plot the covariance against time as +% clf +% ekf.plot_P(); +% +% Vehicle-based map making:: +% +% Create a vehicle with odometry covariance V, add a driver to it, +% create a sensor that uses the map and vehicle state to estimate feature range +% and bearing with covariance W, the Kalman filter with estimated sensor +% covariance W_est and a "perfect" vehicle (no covariance), +% then run the filter for N time steps. +% +% veh = Vehicle(V); +% veh.add_driver( RandomPath(20, 2) ); +% sensor = RangeBearingSensor(veh, map, W); +% ekf = EKF(veh, [], [], sensor, W_est, []); +% We run the simulation for 1000 time steps +% ekf.run(1000); +% Then plot the true map +% map.plot(); +% and overlay the estimated map with 3 sigma ellipses +% ekf.plot_map(3, 'g'); +% +% Simultaneous localization and mapping (SLAM):: +% +% Create a vehicle with odometry covariance V, add a driver to it, +% create a map with 20 point features, create a sensor that uses the map +% and vehicle state to estimate feature range and bearing with covariance +% W, the Kalman filter with estimated covariances V_est and W_est and initial +% state covariance P0, then run the filter to estimate the vehicle state at +% each time step and the map. +% +% veh = Vehicle(V); +% veh.add_driver( RandomPath(20, 2) ); +% map = Map(20); +% sensor = RangeBearingSensor(veh, map, W); +% ekf = EKF(veh, V_est, P0, sensor, W, []); +% We run the simulation for 1000 time steps +% ekf.run(1000); +% then plot the map and the true vehicle path +% map.plot(); +% veh.plot_xy('b'); +% and overlay the estimated path +% ekf.plot_xy('r'); +% and overlay uncertainty ellipses at every 20 time steps +% ekf.plot_ellipse([], 'g'); +% We can plot the covariance against time as +% clf +% ekf.plot_P(); +% Then plot the true map +% map.plot(); +% and overlay the estimated map with 3 sigma ellipses +% ekf.plot_map(3, 'g'); +% +% Reference:: +% +% Robotics, Vision & Control, Chap 6, +% Peter Corke, +% Springer 2011 +% +% Acknowledgement:: +% +% Inspired by code of Paul Newman, Oxford University, +% http://www.robots.ox.ac.uk/~pnewman +% +% See also Vehicle, RandomPath, RangeBearingSensor, Map, ParticleFilter. + +% 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 . +classdef EKF < handle + + %TODO + % add a hook for data association + % show ellipses and laser scan landmark strikes (perhaps this in Map + % class) + % show landmark covar as ellipse or pole + % show vehicle covar as ellipse + % show track + properties + % STATE: + % the state vector is [x_vehicle x_map] where + % x_vehicle is 1x3 and + % x_map is 1x(2N) where N is the number of map features + x_est % estimated state + P_est % estimated covariance + + % Features keeps track of features we've seen before. + % Each column represents a feature. This is a fixed size + % array, indexed by feature id. + % row 1: the start of this feature's state in the feature + % part of the state vector, initially NaN + % row 2: the number of times we've sighted the feature + features % map state + + V_est % estimate of covariance V + W_est % estimate of covariance W + + robot % reference to the robot vehicle + sensor % reference to the sensor + + % FLAGS: + % estVehicle estMap + % 0 0 + % 0 1 make map + % 1 0 dead reckoning + % 1 1 SLAM + estVehicle % flag: estimating vehicle location + estMap % flag: estimating map + + joseph % flag: use Joseph form to compute p + verbose + keepHistory % keep history + P0 % passed initial covariance + map % passed map + + % HISTORY: + % vector of structs to hold EKF history + % .x_est estimated state + % .odo vehicle odometry + % .P estimated covariance matrix + % .innov innovation + % .S + % .K Kalman gain matrix + history + dim % robot workspace dimensions + end + + methods + + % constructor + function ekf = EKF(robot, V_est, P0, varargin) + %EKF.EKF EKF object constructor + % + % E = EKF(VEHICLE, V_EST, P0, OPTIONS) is an EKF that estimates the state + % of the VEHICLE with estimated odometry covariance V_EST (2x2) and + % initial covariance (3x3). + % + % E = EKF(VEHICLE, V_EST, P0, SENSOR, W_EST, MAP, OPTIONS) as above but + % uses information from a VEHICLE mounted sensor, estimated + % sensor covariance W_EST and a MAP. + % + % Options:: + % 'verbose' Be verbose. + % 'nohistory' Don't keep history. + % 'joseph' Use Joseph form for covariance + % 'dim',D Dimension of the robot's workspace. Scalar D is DxD, + % 2-vector D(1)xD(2), 4-vector is D(1) 3 + ekf.sensor = sensor; + ekf.W_est = W_est; + end + ekf.joseph = true; + + ekf.init(); + end + + function init(ekf) + %EKF.init Reset the filter + % + % E.init() resets the filter state and clears the history. + ekf.robot.init(); + + % clear the history + ekf.history = []; + + if isempty(ekf.V_est) + % perfect vehicle case + ekf.estVehicle = false; + ekf.x_est = []; + ekf.P_est = []; + else + % noisy odometry case + ekf.x_est = ekf.robot.x(:); % column vector + ekf.P_est = ekf.P0; + ekf.estVehicle = true; + + end + if isempty(ekf.map) + % no map given, we have to estimate it + ekf.estMap = true; + ekf.features = NaN*zeros(2, ekf.sensor.map.nfeatures); + elseif isnan(ekf.map) + ekf.estMap = false; + else + error('RTB:EKF:badarg', 'shouldnt happen') + end + + + end + + function run(ekf, n, varargin) + %EKF.run Run the filter + % + % E.run(N, OPTIONS) runs the filter for N time steps and shows an animation + % of the vehicle moving. + % + % Options:: + % 'plot' Plot an animation of the vehicle moving + % + % Notes:: + % - All previously estimated states and estimation history are initially + % cleared. + + opt.plot = true; + opt = tb_optparse(opt, varargin); + + ekf.init(); + + if opt.plot + if ~isempty(ekf.sensor) + ekf.sensor.map.plot(); + elseif ~isempty(ekf.dim) + switch length(ekf.dim) + case 1 + d = ekf.dim; + axis([-d d -d d]); + case 2 + w = ekf.dim(1), h = ekf.dim(2); + axis([-w w -h h]); + case 4 + axis(ekf.dim); + end + else + opt.plot = false; + end + end + + % simulation loop + for k=1:n + + if opt.plot + ekf.robot.plot(); + drawnow + end + + ekf.step(opt); + end + end + + function out = plot_xy(ekf, varargin) + %EKF.plot_xy Plot vehicle position + % + % E.plot_xy() overlay the current plot with the estimated vehicle path in + % the xy-plane. + % + % E.plot_xy(LS) as above but the optional line style arguments + % LS are passed to plot. + % + % P = E.plot_xy() returns the estimated vehicle pose trajectory + % as a matrix (Nx3) where each row is x, y, theta. + % + % See also EKF.plot_error, EKF.plot_ellipse, EKF.plot_P. + + + if ekf.estVehicle + xyt = zeros(length(ekf.history), 3); + for i=1:length(ekf.history) + h = ekf.history(i); + xyt(i,:) = h.x_est(1:3)'; + end + if nargout == 0 + plot(xyt(:,1), xyt(:,2), varargin{:}); + end + else + xyt = []; + end + if nargout > 0 + out = xyt; + end + end + + function out = plot_error(ekf, varargin) + %EKF.plot_error Plot vehicle position + % + % E.plot_error(OPTIONS) plot the error between actual and estimated vehicle + % path (x, y, theta). Heading error is wrapped into the range [-pi,pi) + % + % OUT = E.plot_error() is the estimation error versus time as a matrix (Nx3) + % where each row is x, y, theta. + % + % Options:: + % 'bound',S Display the S sigma confidence bounds (default 3). + % If S =0 do not display bounds. + % 'boundcolor',C Display the bounds using color C + % LS Use MATLAB linestyle LS for the plots + % + % Notes:: + % - The bounds show the instantaneous standard deviation associated + % with the state. Observations tend to decrease the uncertainty + % while periods of dead-reckoning increase it. + % - Ideally the error should lie "mostly" within the +/-3sigma + % bounds. + % + % See also EKF.plot_xy, EKF.plot_ellipse, EKF.plot_P. + opt.bounds = 3; + opt.boundcolor = 'r'; + + [opt,args] = tb_optparse(opt, varargin); + + if ekf.estVehicle + err = zeros(length(ekf.history), 3); + for i=1:length(ekf.history) + h = ekf.history(i); + % error is true - estimated + err(i,:) = ekf.robot.x_hist(i,:) - h.x_est(1:3)'; + err(i,3) = angdiff(err(i,3)); + P = diag(h.P); + pxy(i,:) = opt.bounds*sqrt(P(1:3)); + end + if nargout == 0 + clf + t = 1:numrows(pxy); + t = [t t(end:-1:1)]'; + + subplot(311) + if opt.bounds + edge = [pxy(:,1); -pxy(end:-1:1,1)]; + h = patch(t, edge ,opt.boundcolor); + set(h, 'EdgeColor', 'none', 'FaceAlpha', 0.3); + hold on + plot(err(:,1), args{:}); + hold off + end + grid + ylabel('x error') + + subplot(312) + edge = [pxy(:,2); -pxy(end:-1:1,2)]; + h = patch(t, edge, opt.boundcolor); + set(h, 'EdgeColor', 'none', 'FaceAlpha', 0.3); + hold on + plot(err(:,2), args{:}); + hold off + grid + ylabel('y error') + + subplot(313) + edge = [pxy(:,3); -pxy(end:-1:1,3)]; + h = patch(t, edge, opt.boundcolor); + set(h, 'EdgeColor', 'none', 'FaceAlpha', 0.3); + hold on + plot(err(:,3), args{:}); + hold off + grid + xlabel('time (samples)') + ylabel('\theta error') + else + out = pxy; + end + end + end + + function out = plot_map(ekf, covar, varargin) + %EKF.plot_map Plot landmarks + % + % E.plot_map() overlay the current plot with the estimated landmark + % position (a +-marker) and a covariance ellipses. + % + % + % E.plot_map(LS) as above but pass line style arguments + % LS to plot_ellipse. + % + % P = E.plot_map() returns the estimated landmark locations (2xN) + % and column I is the I'th map feature. If the landmark was not + % estimated the corresponding column contains NaNs. + % + % See also plot_ellipse. + + % TODO: some option to plot map evolution, layered ellipses + + if nargin < 2 + covar = 1; + end + + xy = []; + for i=1:numcols(ekf.features) + n = ekf.features(1,i); + if isnan(n) + xy = [xy [NaN; NaN]]; + continue; + end + % n is an index into the *feature* part of the state + % vector, we need to offset it to account for the vehicle + % state if we are estimating vehicle as well + if ekf.estVehicle + n = n + 3; + end + xf = ekf.x_est(n:n+1); + P = ekf.P_est(n:n+1,n:n+1); + % TODO reinstate the interval feature + %plot_ellipse(xf, P, interval, 0, [], varargin{:}); + plot_ellipse(covar^2*P, xf, varargin{:}); + plot(xf(1), xf(2), '+') + + xy = [xy xf]; + end + + if nargout > 0 + out = xy; + end + end + + function out = plot_P(ekf, varargin) + %EKF.plot_P Plot covariance magnitude + % + % E.plot_P() plots the estimated covariance magnitude against + % time step. + % + % E.plot_P(LS) as above but the optional line style arguments + % LS are passed to plot. + % + % M = E.plot_P() returns the estimated covariance magnitude at + % all time steps as a vector. + p = zeros(length(ekf.history),1); + for i=1:length(ekf.history) + p(i) = sqrt(det(ekf.history(i).P)); + end + if nargout == 0 + plot(p, varargin{:}); + xlabel('sample'); + ylabel('(det P)^{0.5}') + else + out = p; + end + end + + function plot_ellipse(ekf, interval, varargin) + %EKF.plot_ellipse Plot vehicle covariance as an ellipse + % + % E.plot_ellipse() overlay the current plot with the estimated + % vehicle position covariance ellipses for 20 points along the + % path. + % + % E.plot_ellipse(I) as above but for I points along the path. + % + % E.plot_ellipse(I, LS) as above but pass line style arguments + % LS to plot_ellipse. If I is [] then assume 20. + % + % See also plot_ellipse. + + if nargin < 2 || isempty(interval) + interval = round(length(ekf.history)/20); + end + holdon = ishold; + hold on + for i=1:interval:length(ekf.history) + h = ekf.history(i); + %plot_ellipse(h.x_est(1:2), h.P(1:2,1:2), 1, 0, [], varargin{:}); + plot_ellipse(h.P(1:2,1:2), h.x_est(1:2), varargin{:}); + end + if ~holdon + hold off + end + end + + function display(ekf) + %EKF.display Display status of EKF object + % + % E.display() displays the state of the EKF object in + % human-readable form. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a EKF object and the command has no trailing + % semicolon. + % + % See also EKF.char. + + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(ekf) ); + end % display() + + function s = char(ekf) + %EKF.char Convert to string + % + % E.char() is a string representing the state of the EKF + % object in human-readable form. + % + % See also EKF.display. + s = sprintf('EKF object: %d states', length(ekf.x_est)); + e = ''; + if ekf.estVehicle + e = [e 'Vehicle ']; + end + if ekf.estMap + e = [e 'Map ']; + end + s = char(s, [' estimating: ' e]); + if ~isempty(ekf.robot) + s = char(s, char(ekf.robot)); + end + if ~isempty(ekf.sensor) + s = char(s, char(ekf.sensor)); + end + s = char(s, ['W_est: ' mat2str(ekf.W_est, 3)] ); + s = char(s, ['V_est: ' mat2str(ekf.V_est, 3)] ); + end + + + end % method + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % P R I V A T E M E T H O D S + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + methods (Access=protected) + function x_est = step(ekf, opt) + + %fprintf('-------step\n'); + % move the robot along its path and get odometry + odo = ekf.robot.step(); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %% do the prediction + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + if ekf.estVehicle + % split the state vector and covariance into chunks for + % vehicle and map + xv_est = ekf.x_est(1:3); + xm_est = ekf.x_est(4:end); + + Pvv_est = ekf.P_est(1:3,1:3); + Pmm_est = ekf.P_est(4:end,4:end); + Pvm_est = ekf.P_est(1:3,4:end); + else + xm_est = ekf.x_est; + %Pvv_est = ekf.P_est; + Pmm_est = ekf.P_est; + end + + if ekf.estVehicle + % evaluate the state update function and the Jacobians + % if vehicle has uncertainty, predict its covariance + xv_pred = ekf.robot.f(xv_est', odo)'; + + Fx = ekf.robot.Fx(xv_est, odo); + Fv = ekf.robot.Fv(xv_est, odo); + Pvv_pred = Fx*Pvv_est*Fx' + Fv*ekf.V_est*Fv'; + else + % otherwise we just take the true robot state + xv_pred = ekf.robot.x; + end + + if ekf.estMap + if ekf.estVehicle + % SLAM case, compute the correlations + Pvm_pred = Fx*Pvm_est; + end + Pmm_pred = Pmm_est; + xm_pred = xm_est; + end + + % put the chunks back together again + if ekf.estVehicle && ~ekf.estMap + % vehicle only + x_pred = xv_pred; + P_pred = Pvv_pred; + elseif ~ekf.estVehicle && ekf.estMap + % map only + x_pred = xm_pred; + P_pred = Pmm_pred; + elseif ekf.estVehicle && ekf.estMap + % vehicle and map + x_pred = [xv_pred; xm_pred]; + P_pred = [ Pvv_pred Pvm_pred; Pvm_pred' Pmm_pred]; + end + + % at this point we have: + % xv_pred the state of the vehicle to use to + % predict observations + % xm_pred the state of the map + % x_pred the full predicted state vector + % P_pred the full predicted covariance matrix + + % initialize the variables that might be computed during + % the update phase + + doUpdatePhase = false; + + %fprintf('x_pred:'); x_pred' + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %% process observations + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + sensorReading = false; + if ~isempty(ekf.sensor) + % read the sensor + [z,js] = ekf.sensor.reading(); + + % if isnan(i) then the sensor has not returned a reading + % at this time interval + sensorReading = ~isnan(js); + end + + if sensorReading + % here for MBL, MM, SLAM + + % compute the innovation + z_pred = ekf.sensor.h(xv_pred', js)'; + innov(1) = z(1) - z_pred(1); + innov(2) = angdiff(z(2), z_pred(2)); + + if ekf.estMap + % the map is estimated MM or SLAM case + if ekf.seenBefore(js) + + % get previous estimate of its state + jx = ekf.features(1,js); + xf = xm_pred(jx:jx+1); + + % compute Jacobian for this particular feature + Hx_k = ekf.sensor.Hxf(xv_pred', xf); + % create the Jacobian for all features + Hx = zeros(2, length(xm_pred)); + Hx(:,jx:jx+1) = Hx_k; + + Hw = ekf.sensor.Hw(xv_pred, xf); + + if ekf.estVehicle + % concatenate Hx for for vehicle and map + Hxv = ekf.sensor.Hx(xv_pred', xf); + Hx = [Hxv Hx]; + end + doUpdatePhase = true; + + % if mod(i, 40) == 0 + % plot_ellipse(x_est(jx:jx+1), P_est(jx:jx+1,jx:jx+1), 5); + % end + else + % get the extended state + [x_pred, P_pred] = ekf.extendMap(P_pred, xv_pred, xm_pred, z, js); + doUpdatePhase = false; + end + else + % the map is given, MBL case + Hx = ekf.sensor.Hx(xv_pred', js); + Hw = ekf.sensor.Hw(xv_pred', js); + doUpdatePhase = true; + end + end + + % doUpdatePhase flag indicates whether or not to do + % the update phase of the filter + % + % DR always false + % map-based localization if sensor reading + % map creation if sensor reading & not first + % sighting + % SLAM if sighting of a previously + % seen feature + + if doUpdatePhase + %fprintf('do update\n'); + %% we have innovation, update state and covariance + % compute x_est and P_est + + % compute innovation covariance + S = Hx*P_pred*Hx' + Hw*ekf.W_est*Hw'; + + % compute the Kalman gain + K = P_pred*Hx' / S; + + % update the state vector + x_est = x_pred + K*innov'; + + if ekf.estVehicle + % wrap heading state for a vehicle + x_est(3) = angdiff(x_est(3)); + end + + % update the covariance + if ekf.joseph + % we use the Joseph form + I = eye(size(P_pred)); + P_est = (I-K*Hx)*P_pred*(I-K*Hx)' + K*ekf.W_est*K'; + else + P_est = P_pred - K*S*K'; + end + + % enforce P to be symmetric + P_est = 0.5*(P_est+P_est'); + else + % no update phase, estimate is same as prediction + x_est = x_pred; + P_est = P_pred; + innov = []; + S = []; + K = []; + end + + %fprintf('X:'); x_est' + + % update the state and covariance for next time + ekf.x_est = x_est; + ekf.P_est = P_est; + + + % record time history + if ekf.keepHistory + hist = []; + hist.x_est = x_est; + hist.odo = odo; + hist.P = P_est; + hist.innov = innov; + hist.S = S; + hist.K = K; + ekf.history = [ekf.history hist]; + end + end + + function s = seenBefore(ekf, jf) + if ~isnan(ekf.features(1,jf)) + %% we have seen this feature before, update number of sightings + if ekf.verbose + fprintf('feature %d seen %d times before, state_idx=%d\n', ... + jf, ekf.features(2,jf), ekf.features(1,jf)); + end + ekf.features(2,jf) = ekf.features(2,jf)+1; + s = true; + else + s = false; + end + end + + + function [x_ext, P_ext] = extendMap(ekf, P, xv, xm, z, jf) + + %% this is a new feature, we haven't seen it before + % estimate position of feature in the world based on + % noisy sensor reading and current vehicle pose + + if ekf.verbose + fprintf('feature %d first sighted\n', jf); + end + + % estimate its position based on observation and vehicle state + xf = ekf.sensor.g(xv, z)'; + + % append this estimate to the state vector + if ekf.estVehicle + x_ext = [xv; xm; xf]; + else + x_ext = [xm; xf]; + end + + % get the Jacobian for the new feature + Gz = ekf.sensor.Gz(xv, z); + + % extend the covariance matrix + if ekf.estVehicle + Gx = ekf.sensor.Gx(xv, z); + n = length(ekf.x_est); + M = [eye(n) zeros(n,2); Gx zeros(2,n-3) Gz]; + P_ext = M*blkdiag(P, ekf.W_est)*M'; + else + P_ext = blkdiag(P, Gz*ekf.W_est*Gz'); + end + + % record the position in the state vector where this + % feature's state starts + ekf.features(1,jf) = length(xm)+1; + %ekf.features(1,jf) = length(ekf.x_est)-1; + ekf.features(2,jf) = 1; % seen it once + + if ekf.verbose + fprintf('extended state vector\n'); + end + + % plot an ellipse at this time +% jx = features(1,jf); +% plot_ellipse(x_est(jx:jx+1), P_est(jx:jx+1,jx:jx+1), 5); + + end + end % private methods +end % classdef + + + + diff --git a/lwrserv/matlab/rvctools/robot/LGPL-LICENCE.txt b/lwrserv/matlab/rvctools/robot/LGPL-LICENCE.txt new file mode 100644 index 0000000..fc8a5de --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/LGPL-LICENCE.txt @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/lwrserv/matlab/rvctools/robot/Link.m b/lwrserv/matlab/rvctools/robot/Link.m new file mode 100644 index 0000000..915559a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Link.m @@ -0,0 +1,810 @@ +%LINK Robot manipulator Link class +% +% A Link object holds all information related to a robot link such as +% kinematics parameters, rigid-body inertial parameters, motor and +% transmission parameters. +% +% Methods:: +% A link transform matrix +% RP joint type: 'R' or 'P' +% friction friction force +% nofriction Link object with friction parameters set to zero +% dyn display link dynamic parameters +% islimit test if joint exceeds soft limit +% isrevolute test if joint is revolute +% isprismatic test if joint is prismatic +% display print the link parameters in human readable form +% char convert to string +% +% Properties (read/write):: +% +% theta kinematic: joint angle +% d kinematic: link offset +% a kinematic: link length +% alpha kinematic: link twist +% sigma kinematic: 0 if revolute, 1 if prismatic +% mdh kinematic: 0 if standard D&H, else 1 +% offset kinematic: joint variable offset +% qlim kinematic: joint variable limits [min max] +% +% m dynamic: link mass +% r dynamic: link COG wrt link coordinate frame 3x1 +% I dynamic: link inertia matrix, symmetric 3x3, about link COG. +% B dynamic: link viscous friction (motor referred) +% Tc dynamic: link Coulomb friction +% +% G actuator: gear ratio +% Jm actuator: motor inertia (motor referred) +% +% Notes:: +% - This is reference class object +% - Link objects can be used in vectors and arrays +% +% References:: +% - Robotics, Vision & Control, Chap 7 +% P. Corke, Springer 2011. +% +% See also Link, Revolute, Prismatic, SerialLink. + +% 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 . + +classdef Link < handle + properties + % kinematic parameters + theta % kinematic: link angle + d % kinematic: link offset + alpha % kinematic: link twist + a % kinematic: link length + sigma % revolute=0, prismatic=1 + mdh % standard DH=0, MDH=1 + offset % joint coordinate offset + name % joint coordinate name + + % dynamic parameters + m % dynamic: link mass + r % dynamic: position of COM with respect to link frame (3x1) + I % dynamic: inertia of link with respect to COM (3x3) + Jm % dynamic: motor inertia + B % dynamic: motor viscous friction (1x1 or 2x1) + + Tc % dynamic: motor Coulomb friction (1x2 or 2x1) + G % dynamic: gear ratio + qlim % joint coordinate limits (2x1) + end + + methods + function l = Link(varargin) + %LINK Create robot link object + % + % This is class constructor function which has several call signatures. + % + % L = Link() is a Link object with default parameters. + % + % L = Link(L1) is a Link object that is a deep copy of the link + % object L1. + % + % L = Link(OPTIONS) is a link object with the kinematic and dynamic + % parameters specified by the key/value pairs. + % + % Key/value pairs:: + % 'theta',TH joint angle, if not specified joint is revolute + % 'd',D joint extension, if not specified joint is prismatic + % 'a',A joint offset (default 0) + % 'alpha',A joint twist (default 0) + % 'standard' defined using standard D&H parameters (default). + % 'modified' defined using modified D&H parameters. + % 'offset',O joint variable offset (default 0) + % 'qlim',L joint limit (default []) + % 'I',I link inertia matrix (3x1, 6x1 or 3x3) + % 'r',R link centre of gravity (3x1) + % 'm',M link mass (1x1) + % 'G',G motor gear ratio (default 0) + % 'B',B joint friction, motor referenced (default 0) + % 'Jm',J motor inertia, motor referenced (default 0) + % 'Tc',T Coulomb friction, motor referenced (1x1 or 2x1), (default [0 0]) + % 'revolute' for a revolute joint (default) + % 'prismatic' for a prismatic joint 'p' + % 'standard' for standard D&H parameters (default). + % 'modified' for modified D&H parameters. + % 'sym' consider all parameter values as symbolic not numeric + % + % - It is an error to specify 'theta' and 'd' + % - The link inertia matrix (3x3) is symmetric and can be specified by giving + % a 3x3 matrix, the diagonal elements [Ixx Iyy Izz], or the moments and products + % of inertia [Ixx Iyy Izz Ixy Iyz Ixz]. + % - All friction quantities are referenced to the motor not the load. + % - Gear ratio is used only to convert motor referenced quantities such as + % friction and interia to the link frame. + % + % Old syntax:: + % L = Link(DH, OPTIONS) is a link object using the specified kinematic + % convention and with parameters: + % - DH = [THETA D A ALPHA SIGMA OFFSET] where OFFSET is a constant displacement + % between the user joint angle vector and the true kinematic solution. + % - DH = [THETA D A ALPHA SIGMA] where SIGMA=0 for a revolute and 1 for a + % prismatic joint, OFFSET is zero. + % - DH = [THETA D A ALPHA], joint is assumed revolute and OFFSET is zero. + % + % Options:: + % + % 'standard' for standard D&H parameters (default). + % 'modified' for modified D&H parameters. + % 'revolute' for a revolute joint, can be abbreviated to 'r' (default) + % 'prismatic' for a prismatic joint, can be abbreviated to 'p' + % + % Examples:: + % A standard Denavit-Hartenberg link + % L3 = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2); + % since 'theta' is not specified the joint is assumed to be revolute, and + % since the kinematic convention is not specified it is assumed 'standard'. + % + % Using the old syntax + % L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'standard'); + % the flag 'standard' is not strictly necessary but adds clarity. + % + % For a modified Denavit-Hartenberg link + % L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'modified'); + % + % Notes:: + % - Link object is a reference object, a subclass of Handle object. + % - Link objects can be used in vectors and arrays. + % - The parameter D is unused in a revolute joint, it is simply a placeholder + % in the vector and the value given is ignored. + % - The parameter THETA is unused in a prismatic joint, it is simply a placeholder + % in the vector and the value given is ignored. + % - The joint offset is a constant added to the joint angle variable before + % forward kinematics and subtracted after inverse kinematics. It is useful + % if you want the robot to adopt a 'sensible' pose for zero joint angle + % configuration. + % - The link dynamic (inertial and motor) parameters are all set to + % zero. These must be set by explicitly assigning the object + % properties: m, r, I, Jm, B, Tc, G. + + %TODO eliminate legacy dyn matrix + + if nargin == 0 + % create an 'empty' Link object + % this call signature is needed to support arrays of Links + + %% kinematic parameters + l.alpha = 0; + l.a = 0; + l.theta = 0; + l.d = 0; + l.sigma = 0; + l.mdh = 0; + l.offset = 0; + l.qlim = []; + + %% dynamic parameters + % these parameters must be set by the user if dynamics is used + l.m = []; + l.r = []; + l.I = []; + + % dynamic params with default (zero friction) + l.Jm = 0; + l.G = 0; + l.B = 0; + l.Tc = [0 0]; + elseif nargin == 1 && isa(varargin{1}, 'Link') + % clone the passed Link object + l = copy(varargin{1}); + + else + % Create a new Link based on parameters + + % parse all possible options + opt.theta = []; + opt.a = 0; + opt.d = []; + opt.alpha = 0; + opt.G = 0; + opt.B = 0; + opt.Tc = [0 0]; + opt.Jm = []; + opt.I = []; + opt.m = []; + opt.r = []; + opt.offset = 0; + opt.qlim = []; + opt.type = {'revolute', 'prismatic', 'fixed'}; + opt.convention = {'standard', 'modified'}; + opt.sym = false; + + [opt,args] = tb_optparse(opt, varargin); + + % return a parameter as a number of symbol depending on + % the 'sym' option + + if isempty(args) + % This is the new call format, where all parameters are + % given by key/value pairs + % + % eg. L3 = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2); + if ~isempty(opt.theta) + % constant value of theta means it must be prismatic + l.theta = value( opt.theta, opt); + l.sigma = 1; + end + + if ~isempty(opt.d) + % constant value of d means it must be revolute + l.d = value( opt.d, opt); + l.sigma = 0; + end + if ~isempty(opt.d) && ~isempty(opt.theta) + error('RTB:Link:badarg', 'specify only one of ''d'' or ''theta'''); + end + + l.a = value( opt.a, opt); + l.alpha = value( opt.alpha, opt); + + l.offset = value( opt.offset, opt); + l.qlim = value( opt.qlim, opt); + + l.m = value( opt.m, opt); + l.r = value( opt.r, opt); + l.I = value( opt.I, opt); + l.Jm = value( opt.Jm, opt); + l.G = value( opt.G, opt); + l.B = value( opt.B, opt); + l.Tc = value( opt.Tc, opt); + else + % This is the old call format, where all parameters are + % given by a vector containing kinematic-only, or + % kinematic plus dynamic parameters + % + % eg. L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'standard'); + dh = args{1}; + if length(dh) < 4 + error('must provide params (theta d a alpha)'); + end + + % set the kinematic parameters + l.theta = dh(1); + l.d = dh(2); + l.a = dh(3); + l.alpha = dh(4); + + l.sigma = 0; + l.offset = 0; + l.mdh = 0; % default to standard D&H + + % optionally set sigma and offset + if length(dh) >= 5 + l.sigma = dh(5); + end + if length(dh) == 6 + l.offset = dh(6); + end + + if length(dh) > 6 + % legacy DYN matrix + + l.sigma = dh(5); + l.mdh = 0; % default to standard D&H + l.offset = 0; + + % it's a legacy DYN matrix + l.m = dh(6); + l.r = dh(7:9)'; % a column vector + v = dh(10:15); + l.I = [ v(1) v(4) v(6) + v(4) v(2) v(5) + v(6) v(5) v(3)]; + if length(dh) > 15 + l.Jm = dh(16); + end + if length(dh) > 16 + l.G = dh(17); + else + l.G = 1; + end + if length(dh) > 17 + l.B = dh(18); + else + l.B = 0.0; + end + if length(dh) > 18 + l.Tc = dh(19:20); + else + l.Tc = [0 0]; + end + l.qlim = []; + else + % we know nothing about the dynamics + l.m = []; + l.r = []; + l.I = []; + l.Jm = []; + l.G = 0; + l.B = 0; + l.Tc = [0 0]; + l.qlim = []; + end + end + + % set the kinematic convention to be used + if strcmp(opt.convention, 'modified') + l.mdh = 1; + else + l.mdh = 0; + end + + end + + function out = value(v, opt) + if opt.sym + out = sym(v); + else + out = v; + end + end + + end % link() + + function tau = friction(l, qd) + %Link.friction Joint friction force + % + % F = L.friction(QD) is the joint friction force/torque for link velocity QD. + % + % Notes:: + % - Friction values are referred to the motor, not the load. + % - Viscous friction is scaled up by G^2. + % - Coulomb friction is scaled up by G. + % - The sign of the gear ratio is used to determine the appropriate + % Coulomb friction value in the non-symmetric case. + + tau = l.B * abs(l.G) * qd; + + if issym(l) + tau = tau + l.Tc; + elseif qd > 0 + tau = tau + l.Tc(1); + elseif qd < 0 + tau = tau + l.Tc(2); + end + tau = -abs(l.G) * tau; % friction opposes motion + end % friction() + + function l2 = nofriction(l, only) + %Link.nofriction Remove friction + % + % LN = L.nofriction() is a link object with the same parameters as L except + % nonlinear (Coulomb) friction parameter is zero. + % + % LN = L.nofriction('all') as above except that viscous and Coulomb friction + % are set to zero. + % + % LN = L.nofriction('coulomb') as above except that Coulomb friction is set to zero. + % + % LN = L.nofriction('viscous') as above except that viscous friction is set to zero. + % + % Notes:: + % - Forward dynamic simulation can be very slow with finite Coulomb friction. + % + % See also SerialLink.nofriction, SerialLink.fdyn. + + l2 = Link(l); + + if nargin == 1 + only = 'coulomb'; + end + + switch only + case 'all' + l2.B = 0; + l2.Tc = [0 0]; + case 'viscous' + l2.B = 0; + case 'coulomb' + l2.Tc = [0 0]; + end + end + + function v = RP(l) + %Link.RP Joint type + % + % c = L.RP() is a character 'R' or 'P' depending on whether joint is revolute or + % prismatic respectively. + % If L is a vector of Link objects return a string of characters in joint order. + v = ''; + for ll=l + if ll.sigma == 0 + v = strcat(v, 'R'); + else + v = strcat(v, 'P'); + end + end + end % RP() + + function set.r(l, v) + %Link.r Set centre of gravity + % + % L.r = R set the link centre of gravity (COG) to R (3-vector). + % + if isempty(v) + return; + end + if length(v) ~= 3 + error('RTB:Link:badarg', 'COG must be a 3-vector'); + end + l.r = v(:)'; + end % set.r() + + function set.Tc(l, v) + %Link.Tc Set Coulomb friction + % + % L.Tc = F set Coulomb friction parameters to [F -F], for a symmetric + % Coulomb friction model. + % + % L.Tc = [FP FM] set Coulomb friction to [FP FM], for an asymmetric + % Coulomb friction model. FP>0 and FM<0. + % + % See also Link.friction. + if isempty(v) + return; + end + + if isa(v,'sym') && ~isempty(findsym(v)) + l.Tc = sym('Tc'); + elseif isa(v,'sym') && isempty(findsym(v)) + v = double(v); + end + + if length(v) == 1 ~isa(v,'sym') + l.Tc = [v -v]; + elseif length(v) == 2 && ~isa(v,'sym') + if v(1) < v(2) + error('RTB:Link:badarg', 'Coulomb friction is [Tc+ Tc-]'); + end + l.Tc = v; + else + error('Coulomb friction vector can have 1 (symmetric) or 2 (asymmetric) elements only') + end + end % set.Tc() + + function set.I(l, v) + %Link.I Set link inertia + % + % L.I = [Ixx Iyy Izz] set link inertia to a diagonal matrix. + % + % L.I = [Ixx Iyy Izz Ixy Iyz Ixz] set link inertia to a symmetric matrix with + % specified inertia and product of intertia elements. + % + % L.I = M set Link inertia matrix to M (3x3) which must be symmetric. + if isempty(v) + return; + end + if all(size(v) == [3 3]) + if isa(v, 'double') && norm(v-v') > eps + error('inertia matrix must be symmetric'); + end + l.I = v; + elseif length(v) == 3 + l.I = diag(v); + elseif length(v) == 6 + l.I = [ v(1) v(4) v(6) + v(4) v(2) v(5) + v(6) v(5) v(3) ]; + else + error('RTB:Link:badarg', 'must set I to 3-vector, 6-vector or symmetric 3x3'); + end + end % set.I() + + function v = islimit(l, q) + %Link.islimit Test joint limits + % + % L.islimit(Q) is true (1) if Q is outside the soft limits set for this joint. + % + % Note:: + % - The limits are not currently used by any Toolbox functions. + if isempty(l.qlim) + error('no limits assigned to link') + end + v = (q > l.qlim(2)) - (q < l.qlim(1)); + end % islimit() + + function v = isrevolute(L) + %Link.isrevolute Test if joint is revolute + % + % L.isrevolute() is true (1) if joint is revolute. + % + % See also Link.isprismatic. + + v = L.sigma == 0; + end + + function v = isprismatic(L) + %Link.isprismatic Test if joint is prismatic + % + % L.isprismatic() is true (1) if joint is prismatic. + % + % See also Link.isrevolute. + v = L.sigma == 1; + end + + + function T = A(L, q) + %Link.A Link transform matrix + % + % T = L.A(Q) is the link homogeneous transformation matrix (4x4) corresponding + % to the link variable Q which is either the Denavit-Hartenberg parameter THETA (revolute) + % or D (prismatic). + % + % Notes:: + % - For a revolute joint the THETA parameter of the link is ignored, and Q used instead. + % - For a prismatic joint the D parameter of the link is ignored, and Q used instead. + % - The link offset parameter is added to Q before computation of the transformation matrix. + sa = sin(L.alpha); ca = cos(L.alpha); + q = q + L.offset; + if L.sigma == 0 + % revolute + st = sin(q); ct = cos(q); + d = L.d; + else + % prismatic + st = sin(L.theta); ct = cos(L.theta); + d = q; + end + + if L.mdh == 0 + % standard DH + + T = [ ct -st*ca st*sa L.a*ct + st ct*ca -ct*sa L.a*st + 0 sa ca d + 0 0 0 1]; + else + % modified DH + + T = [ ct -st 0 L.a + st*ca ct*ca -sa -sa*d + st*sa ct*sa ca ca*d + 0 0 0 1]; + end + + end % A() + + function display(l) + %Link.display Display parameters + % + % L.display() displays the link parameters in compact single line format. If L is a + % vector of Link objects displays one line per element. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a Link object and the command has no trailing + % semicolon. + % + % See also Link.char, Link.dyn, SerialLink.showlink. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(l) ); + end % display() + + function s = char(links, from_robot) + %Link.char Convert to string + % + % s = L.char() is a string showing link parameters in a compact single line format. + % If L is a vector of Link objects return a string with one line per Link. + % + % See also Link.display. + + % display in the order theta d a alpha + if nargin < 2 + from_robot = false; + end + + s = ''; + + for j=1:length(links) + l = links(j); + + conv = l.RP; + if l.mdh == 0 + conv = strcat(conv, ',stdDH'); + else + conv = strcat(conv, ',modDH'); + end + if length(links) == 1 + qname = 'q'; + else + qname = sprintf('q%d', j); + end + + if from_robot + if l.sigma == 1 + % prismatic joint + js = sprintf('|%3d|%11s|%11s|%11s|%11s|', ... + j, ... + render(l.theta), ... + qname, ... + render(l.a), ... + render(l.alpha)); + else + % revolute joint + js = sprintf('|%3d|%11s|%11s|%11s|%11s|', ... + j, ... + qname, ... + render(l.d), ... + render(l.a), ... + render(l.alpha)); + end + else + + if l.sigma == 1 + % prismatic joint + js = sprintf(' theta=%s, d=%s, a=%s, alpha=%s (%s)', ... + render(l.theta), ... + qname, ... + render(l.a), ... + render(l.alpha), ... + conv); + else + % revolute + js = sprintf(' theta=%s, d=%s, a=%s, alpha=%s (%s)', ... + qname, ... + render(l.d), ... + render(l.a), ... + render(l.alpha), ... + conv); + end + end + if isempty(s) + s = js; + else + s = char(s, js); + end + end + + function s = render(v, fmt) + if nargin < 2 + fmt = '%11.4g'; + end + if isa(v, 'double') + s = sprintf(fmt, v); + elseif isa(v, 'sym') + s = char(v); + else + error('RTB:Link:badarg', 'Link parameter must be numeric or symbolic'); + end + end + end % char() + + function dyn(links) + %Link.dyn Show inertial properties of link + % + % L.dyn() displays the inertial properties of the link object in a multi-line + % format. The properties shown are mass, centre of mass, inertia, friction, + % gear ratio and motor properties. + % + % If L is a vector of Link objects show properties for each link. + % + % See also SerialLink.dyn. + + for j=1:numel(links) + l = links(j); + if numel(links) > 1 + fprintf('\nLink %d::', j); + end + fprintf('%s\n', l.char()); + if ~isempty(l.m) + fprintf(' m = %f\n', l.m) + end + if ~isempty(l.r) + fprintf(' r = %f %f %f\n', l.r); + end + if ~isempty(l.I) + fprintf(' I = | %f %f %f |\n', l.I(1,:)); + fprintf(' | %f %f %f |\n', l.I(2,:)); + fprintf(' | %f %f %f |\n', l.I(3,:)); + end + if ~isempty(l.Jm) + fprintf(' Jm = %f\n', l.Jm); + end + if ~isempty(l.B) + fprintf(' Bm = %f\n', l.B); + end + if ~isempty(l.Tc) + fprintf(' Tc = %f(+) %f(-)\n', l.Tc(1), l.Tc(2)); + end + if ~isempty(l.G) + fprintf(' G = %f\n', l.G); + end + if ~isempty(l.qlim) + fprintf(' qlim = %f to %f\n', l.qlim(1), l.qlim(2)); + end + end + end % dyn() + + % Make a copy of a handle object. + % http://www.mathworks.com/matlabcentral/newsreader/view_thread/257925 + function new = copy(this) + + for j=1:length(this) + % Instantiate new object of the same class. + %new(j) = feval(class(this(j))); + new(j) = Link(); + % Copy all non-hidden properties. + p = properties(this(j)); + for i = 1:length(p) + new(j).(p{i}) = this(j).(p{i}); + end + end + end + + function links = horzcat(this, varargin) + links = this.toLink(); + + for i=1:length(varargin) + links = cat(2, links, varargin{i}.toLink()); + end + end + + function links = vertcat(this, varargin) + links = this.horzcat(varargin{:}); + end + + function new = toLink(this) + for j=1:length(this) + new(j) = Link(); + + % Copy all non-hidden properties. + p = properties(this(j)); + for i = 1:length(p) + new(j).(p{i}) = this(j).(p{i}); + end + end + + end + + function res = issym(l) + res = isa(l.alpha,'sym'); + end + + function sl = sym(l) + + sl = Link(l); % clone the link + + if ~isempty(sl.theta) + sl.theta = sym(sl.theta); + end + if ~isempty(sl.d) + sl.d = sym(sl.d); + end + sl.alpha = sym(sl.alpha); + sl.a = sym(sl.a); + sl.offset = sym(sl.offset); + + sl.I = sym(sl.I); + sl.r = sym(sl.r); + sl.m = sym(sl.m); + + sl.Jm = sym(sl.Jm); + sl.G = sym(sl.G); + sl.B = sym(sl.B); + sl.Tc = sym(sl.Tc); + + end + end % methods +end % class diff --git a/lwrserv/matlab/rvctools/robot/Map.m b/lwrserv/matlab/rvctools/robot/Map.m new file mode 100644 index 0000000..eb6658e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Map.m @@ -0,0 +1,173 @@ +%map Map of planar point features +% +% A Map object represents a square 2D environment with a number of landmark +% feature points. +% +% Methods:: +% plot Plot the feature map +% feature Return a specified map feature +% display Display map parameters in human readable form +% char Convert map parameters to human readable string +% +% Properties:: +% map Matrix of map feature coordinates 2xN +% dim The dimensions of the map region x,y in [-dim,dim] +% nfeatures The number of map features N +% +% Examples:: +% +% To create a map for an area where X and Y are in the range -10 to +10 metres +% and with 50 random feature points +% map = Map(50, 10); +% which can be displayed by +% map.plot(); +% +% Reference:: +% +% Robotics, Vision & Control, Chap 6, +% Peter Corke, +% Springer 2011 +% +% See also RangeBearingSensor, EKF. + +% 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 . + +classdef Map < handle +% TODO: +% add a name property, show in char() + + properties + map % map features + dim % map dimension + nfeatures % number of features in map + + verbose + end + + methods + + % constructor + function map = Map(nfeatures, varargin) + %Map.Map Map of point feature landmarks + % + % M = Map(N, DIM, OPTIONS) is a Map object that represents N random point features + % in a planar region bounded by +/-DIM in the x- and y-directions. + % + % Options:: + % 'verbose' Be verbose + + opt = []; + [opt,args] = tb_optparse(opt, varargin); + map.verbose = opt.verbose; + + if ~isempty(args) && isnumeric(args{1}) + dim = args{1}; + else + dim = 10; + end + map.dim = dim; + map.nfeatures = nfeatures; + map.map = dim * (2*rand(2, nfeatures)-1); + map.verbose = false; + end + + function f = feature(map, k) + %Map.feature Return the specified map feature + % + % F = M.feature(K) is the coordinate (2x1) of the K'th feature. + f = map.map(:,k); + end + + function plot(map, varargin) + %Map.plot Plot the map + % + % M.plot() plots the feature map in the current figure, as a square + % region with dimensions given by the M.dim property. Each feature + % is marked by a black diamond. + % + % M.plot(LS) plots the feature map as above, but the arguments LS + % are passed to plot and override the default marker style. + % + % Notes:: + % - The plot is left with HOLD ON. + clf + d = map.dim; + axis([-d d -d d]); + xlabel('x'); + ylabel('y'); + + if nargin == 1 + args = {'kd'}; + else + args = varargin; + end + plot(map.map(1,:)', map.map(2,:)', args{:}); + grid on + hold on + end + + function show(map, varargin) + %map.SHOW Show the feature map + % + % Notes:: + % - Deprecated, use plot method. + warning('show method is deprecated, use plot() instead'); + map.plot(varargin{:}); + end + + function verbosity(map, v) + %map.verbosity Set verbosity + % + % M.verbosity(V) set verbosity to V, where 0 is silent and greater + % values display more information. + map.verbose = v; + end + + function display(map) + %map.display Display map parameters + % + % M.display() display map parameters in a compact + % human readable form. + % + % Notes:: + % - this method is invoked implicitly at the command line when the result + % of an expression is a Map object and the command has no trailing + % semicolon. + % + % See also map.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(map) ); + end % display() + + function s = char(map) + %map.char Convert vehicle parameters and state to a string + % + % s = M.char() is a string showing map parameters in + % a compact human readable format. + s = 'Map object'; + s = char(s, sprintf(' %d features', map.nfeatures)); + s = char(s, sprintf(' dimension %.1f', map.dim)); + end + + end % method + +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/Navigation.m b/lwrserv/matlab/rvctools/robot/Navigation.m new file mode 100644 index 0000000..2465af0 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Navigation.m @@ -0,0 +1,489 @@ +%Navigation Navigation superclass +% +% An abstract superclass for implementing navigation classes. +% +% Methods:: +% plot Display the occupancy grid +% visualize Display the occupancy grid (deprecated) +% plan Plan a path to goal +% path Return/animate a path from start to goal +% display Display the parameters in human readable form +% char Convert to string +% +% rand Uniformly distributed random number +% randn Normally distributed random number +% randi Uniformly distributed random integer +% +% Properties (read only):: +% occgrid Occupancy grid representing the navigation environment +% goal Goal coordinate +% seed0 Random number state +% +% Methods that must be provided in subclass:: +% plan Generate a plan for motion to goal +% next Returns coordinate of next point along path +% +% Methods that may be overriden in a subclass:: +% goal_set The goal has been changed by nav.goal = (a,b) +% navigate_init Start of path planning. +% +% Notes:: +% - Subclasses the MATLAB handle class which means that pass by reference semantics +% apply. +% - A grid world is assumed and vehicle position is quantized to grid cells. +% - Vehicle orientation is not considered. +% - The initial random number state is captured as seed0 to allow rerunning an +% experiment with an interesting outcome. +% +% See also Dstar, Dxform, PRM, RRT. + +% 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 . + +% Peter Corke 8/2009. + +% TODO +% keep dimensions of workspace in this object, have a setaxes() method +% which transfers the dimensions to the current axes. + +classdef Navigation < handle + + properties + occgrid % occupancy grid + goal % goal coordinate + + navhook % function handle, called on each navigation iteration + verbose % verbosity + seed % current random seed + spincount + + randstream + seed0 + end + + + % next() should be protected and abstract, but this doesnt work + % properly + methods (Abstract) + plan(obj) + n = next(obj) + end % method Abstract + + methods + + % TODO fix up set methods for goal + % setup argument callback like features, can we inherit from that. + % occ grid should be an option + + % constructor + + function nav = Navigation(varargin) + %Navigation.Navigation Create a Navigation object + % + % N = Navigation(OCCGRID, OPTIONS) is a Navigation object that holds an + % occupancy grid OCCGRID. A number of options can be be passed. + % + % Options:: + % 'navhook',F Specify a function to be called at every step of path + % 'goal',G Specify the goal point (2x1) + % 'verbose' Display debugging information + % 'inflate',K Inflate all obstacles by K cells. + % 'private' Use private random number stream. + % 'reset' Reset random number stream. + % 'seed',S Set the initial state of the random number stream. S must + % be a proper random number generator state such as saved in + % the seed0 property of an earlier run. + % + % Notes:: + % - In the occupancy grid a value of zero means free space and non-zero means + % occupied (not driveable). + % - Obstacle inflation is performed with a round structuring element (kcircle). + % - The 'private' option creates a private random number stream for the methods + % rand, randn and randi. If not given the global stream is used. + + if nargin >= 1 && isnumeric(varargin{1}) + nav.occgrid = varargin{1}; + varargin = varargin(2:end); + end + + % default values of options + opt.goal = []; + opt.inflate = 0; + opt.navhook = []; + opt.private = false; + opt.reset = false; + opt.seed = []; + + [opt,args] = tb_optparse(opt, varargin); + + % optionally inflate the obstacles + if opt.inflate > 0 + nav.occgrid = idilate(nav.occgrid, kcircle(opt.inflate)); + end + + % copy other options into the object + nav.verbose = opt.verbose; + nav.navhook = opt.navhook; + if ~isempty(opt.goal) + nav.goal = opt.goal(:)'; + end + + % create a private random number stream if required + if opt.private + nav.randstream = RandStream.create('mt19937ar'); + else + nav.randstream = RandStream.getGlobalStream(); + end + + % reset the random number stream if required + if opt.reset + nav.randstream.reset(); + end + + % return the random number stream to known state if required + if ~isempty(opt.seed) + set(nav.randstream.set(opt.seed)); + end + + % save the current state in case it later turns out to give interesting results + nav.seed0 = nav.randstream.State; + + nav.spincount = 0; + end + + function r = rand(nav, varargin) + %Navigation.rand Uniformly distributed random number + % + % R = N.rand() return a uniformly distributed random number from + % a private random number stream. + % + % R = N.rand(M) as above but return a matrix (MxM) of random numbers. + % + % R = N.rand(L,M) as above but return a matrix (LxM) of random numbers. + % + % Notes:: + % - Accepts the same arguments as rand(). + % - Seed is provided to Navigation constructor. + % + % See also rand, RandStream. + r = nav.randstream.rand(varargin{:}); + end + + function r = randn(nav, varargin) + %Navigation.randn Normally distributed random number + % + % R = N.randn() return a normally distributed random number from + % a private random number stream. + % + % R = N.randn(M) as above but return a matrix (MxM) of random numbers. + % + % R = N.randn(L,M) as above but return a matrix (LxM) of random numbers. + % + % + % Notes:: + % - Accepts the same arguments as randn(). + % - Seed is provided to Navigation constructor. + % + % See also randn, RandStream. + r = nav.randstream.randn(varargin{:}); + end + + function r = randi(nav, varargin) + %Navigation.randi Integer random number + % + % I = N.randi(RM) return a uniformly distributed random integer in the + % range 1 to RM from a private random number stream. + % + % I = N.randi(RM, M) as above but return a matrix (MxM) of random integers. + % + % I = N.randn(RM, L,M) as above but return a matrix (LxM) of random integers. + % + % + % Notes:: + % - Accepts the same arguments as randn(). + % - Seed is provided to Navigation constructor. + % + % See also randn, RandStream. + r = nav.randstream.randi(varargin{:}); + end + + % invoked whenever the goal is set + function set.goal(nav, goal) + + if ~isempty(nav.occgrid) && nav.occgrid( goal(2), goal(1)) == 1 + error('Navigation: cant set goal inside obstacle'); + end + + goal = goal(:); + if ~(all(size(goal) == size(nav.goal)) && all(goal == nav.goal)) + % goal has changed + nav.goal = goal(:); + nav.goal_change(); + end + end + + function goal_change(nav) + %Navigation.goal_change Notify change of goal + % + % Invoked when the goal property of the object is changed. Typically this + % is overriden in a subclass to take particular action such as invalidating + % a costmap. + end + + + + function pp = path(nav, start) + %Navigation.path Follow path from start to goal + % + % N.path(START) animates the robot moving from START (2x1) to the goal (which is a + % property of the object). + % + % N.path() as above but first displays the occupancy grid, and prompts the user to + % click a start location. + % the object). + % + % X = N.path(START) returns the path (2xM) from START to the goal (which is a property of + % the object). + % + % The method performs the following steps: + % + % - Get start position interactively if not given + % - Initialized navigation, invoke method N.navigate_init() + % - Visualize the environment, invoke method N.plot() + % - Iterate on the next() method of the subclass + % + % See also Navigation.plot, Navigation.goal. + + % if no start point given, display the map, and prompt the user to select + % a start point + if nargin < 2 + % display the world + nav.plot(); + + % prompt the user to click a goal point + fprintf('** click a starting point '); + [x,y] = ginput(1); + fprintf('\n'); + start = round([x;y]); + end + start = start(:); + + % if no output arguments given, then display the world + if nargout == 0 + % render the world + nav.plot(); + hold on + end + + nav.navigate_init(start); + + p = []; + % robot is a column vector + robot = start; + + % iterate using the next() method until we reach the goal + while true + if nargout == 0 + plot(robot(1), robot(2), 'g.', 'MarkerSize', 12); + drawnow + end + + % move to next point on path + robot = nav.next(robot); + + % are we there yet? + if isempty(robot) + % yes, exit the loop + break + else + % no, append it to the path + p = [p; robot(:)']; + end + + % invoke the navhook function + if isa(nav.navhook, 'function_handle') + nav.navhook(nav, robot(1), robot(2)); + end + end + + % only return the path if required + if nargout > 0 + pp = p; + end + end + + function visualize(nav, varargin) + warning('visualize method deprecated for Navigation classes, use plot instead'); + nav.plot(varargin{:}); + end + + function plot(nav, varargin) + %Navigation.plot Visualize navigation environment + % + % N.plot() displays the occupancy grid in a new figure. + % + % N.plot(P) as above but overlays the points along the path (Mx2) matrix. + % + % Options:: + % 'goal' Superimpose the goal position if set + % 'distance',D Display a distance field D behind the obstacle map. D is + % a matrix of the same size as the occupancy grid. + + opt.goal = false; + opt.distance = []; + + [opt,args] = tb_optparse(opt, varargin); + + clf + if isempty(opt.distance) + % create color map for free space + obstacle: + % free space, color index = 1, white, + % obstacle, color index = 2, red + cmap = [1 1 1; 1 0 0]; % non obstacles are white + image(nav.occgrid+1, 'CDataMapping', 'direct'); + colormap(cmap) + + else + % create color map for distance field + obstacle: + % obstacle, color index = 1, red + % free space, color index > 1, greyscale + + % find maximum distance, ignore infinite values in + % obstacles + d = opt.distance(isfinite(opt.distance)); + maxdist = max(d(:)) + 1; + + % create the color map + cmap = [1 0 0; gray(maxdist)]; + + % ensure obstacles appear as red pixels + opt.distance(nav.occgrid > 0) = 0; + + % display it with colorbar + image(opt.distance+1, 'CDataMapping', 'direct'); + set(gcf, 'Renderer', 'Zbuffer') + colormap(cmap) + colorbar + end + + % label the grid + set(gca, 'Ydir', 'normal'); + xlabel('x'); + ylabel('y'); + grid on + hold on + + if ~isempty(args) + p = args{1}; + if numcols(p) ~= 2 + error('expecting Nx2 matrix of points'); + end + plot(p(:,1), p(:,2), 'g.', 'MarkerSize', 12); + end + + if ~isempty(nav.goal) && opt.goal + plot(nav.goal(1), nav.goal(2), 'bd', 'MarkerFaceColor', 'b'); + end + hold off + end + + function navigate_init(nav, start) + %Navigation.navigate_init Notify start of path + % + % Invoked when the path() method is invoked. Typically overriden in a subclass + % to take particular action such as computing some path parameters. + % start is the initial position for this path, and nav.goal is the final position. + end + + + function display(nav) + %Navigation.display Display status of navigation object + % + % N.display() displays the state of the navigation object in + % human-readable form. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a Navigation object and the command has no trailing + % semicolon. + % + % See also Navigation.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( nav.char() ); + end % display() + + function s = char(nav) + %Navigation.char Convert to string + % + % N.char() is a string representing the state of the navigation + % object in human-readable form. + s = [class(nav) ' navigation class:']; + s = char(s, sprintf(' occupancy grid: %dx%d', size(nav.occgrid))); + if ~isempty(nav.goal) + if length(nav.goal) == 2 + s = char(s, sprintf(' goal: (%d,%d)', nav.goal) ); + else + s = char(s, sprintf(' goal: (%g,%g, %g)', nav.goal) ); + + end + end + end + + + function verbosity(nav, v) + %Navigation.verbosity Set verbosity + % + % N.verbosity(V) set verbosity to V, where 0 is silent and greater + % values display more information. + nav.verbose = v; + end + + % called at each point on the path as + % navhook(nav, robot) + % + % can be used for logging data, animation, etc. + function navhook_set(nav, navhook) + nav.navhook = navhook; + end + + function message(nav, varargin) + %Navigation.message Display debug message + % + % N.message(S) displays the string S if the verbose property is true. + % + % N.message(FMT, ARGS) as above but accepts printf() like semantics. + if nav.verbose + fprintf([class(nav) ' debug:: ' sprintf(varargin{:}) '\n']); + end + end + + function spinner(nav) + %Navigation.spinner Update progress spinner + % + % N.spinner() displays a simple ASCII progress spinner, a rotating bar. + spinchars = '-\|/'; + nav.spincount = nav.spincount + 1; + fprintf('\r%c', spinchars( mod(nav.spincount, length(spinchars))+1 ) ); + end + + end % method + +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/Link.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/Link.m new file mode 100644 index 0000000..680541e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/Link.m @@ -0,0 +1,199 @@ +%LINK create a new LINK object +% +% A LINK object holds all information related to a robot link such as +% kinematics of the joint, rigid-body inertial parameters, motor and +% transmission parameters. +% +% LINK +% LINK(link) +% +% Create a default link, or a clone of the passed link. +% +% A = LINK(q) +% +% Compute the link transform matrix for the link, given the joint +% variable q. +% +% LINK([alpha A theta D sigma]) +% LINK(DH_ROW) create from row of legacy DH matrix +% LINK(DYN_ROW) create from row of legacy DYN matrix +% +% Any of the last 3 forms can have an optional flag argument which is 0 +% for standard D&H parameters and 1 for modified D&H parameters. +% Handling the different kinematic conventions is now hidden within the LINK +% object. +% +% Conceivably all sorts of stuff could live in the LINK object such as +% graphical models of links and so on. + +% MOD HISTORY +% 3/99 modify to use on a LINK object +% 6/99 fix the number of fields inthe object, v5.3 doesn't let me change them +% mod by Francisco Javier Blanco Rodriguez + + +% 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.petercorke.com + +function l = Link(dh, convention) + + % legacy DH matrix + % link([theta d a alpha]) + % link([theta d a alpha sigma]) + % link([theta d a alpha sigma offset]) + + if nargin == 0, + l.theta = 0; + l.d = 0; + l.a = 0; + l.alpha = 0; + l.sigma = 0; + l.offset = 0; + l.mdh = 0; + % it's a legacy DYN matrix + l.m = []; + l.r = []; + v = []; + l.I = []; + l.Jm = []; + l.G = []; + l.B = 0; + l.Tc = [0 0]; + l.qlim = []; + l = class(l, "Link"); + + + elseif isa(dh, 'Link') + l = dh; + elseif length(dh) < 4 + error('must provide params (theta d a alpha)'); + elseif length(dh) <= 6 + % legacy DH matrix + l.theta = dh(1); + l.d = dh(2); + l.a = dh(3); + l.alpha = dh(4); + + + if length(dh) >= 5, + l.sigma = dh(5); + else + l.sigma = 0; + end + if length(dh) >= 6 + l.offset = dh(6); + else + l.offset = 0; + end + l.mdh = 0; + if nargin > 1 + if strncmp(convention, 'mod', 3) == 1 + l.mdh = 1; + elseif strncmp(convention, 'sta', 3) == 1 + l.mdh = 0; + else + error('convention must be modified or standard'); + end + end + + % we know nothing about the dynamics + l.m = []; + l.r = []; + v = []; + l.I = []; + l.Jm = []; + l.G = []; + l.B = 0; + l.Tc = [0 0]; + l.qlim = []; + l = class(l, "Link"); + + + else + % legacy DYN matrix + + l.theta = dh(1); + l.d = dh(2); + l.a = dh(3); + l.alpha = dh(4); + if length(dh) >= 5, + l.sigma = dh(5); + else + l.sigma = 0; + end + l.offset = 0; + l.mdh = 0; + if nargin > 1 + if strncmp(convention, 'mod', 3) == 1 + l.mdh = 1; + elseif strncmp(convention, 'sta', 3) == 1 + l.mdh = 0; + else + error('convention must be modified or standard'); + end + end + % it's a legacy DYN matrix + if length(dh) >= 6, + l.m = [dh(6)]; + else + l.m = []; + end + if length(dh) >= 9, + l.r = dh(7:9); + else + l.r = []; + end + if length(dh) >= 15, + v = dh(10:15); + l.I = [ v(1) v(4) v(6) + v(4) v(2) v(5) + v(6) v(5) v(3)]; + else + v = []; + l.I = []; + end + if length(dh) >= 16, + l.Jm = dh(16); + else + l.Jm = []; + end + if length(dh) >= 17, + l.G = dh(17); + else + l.G = []; + end + if length(dh) >= 18, + l.B = dh(18); + else + l.B = 0; + end + if length(dh) >= 20, + l.Tc = dh(19:20); + else + l.Tc = [0 0]; + end + + l.qlim = []; + l = class(l, "Link"); + + end + +endfunction diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/RP.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/RP.m new file mode 100644 index 0000000..b41d42c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/RP.m @@ -0,0 +1,21 @@ +% 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.petercorke.com +endfunction diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/char.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/char.m new file mode 100644 index 0000000..2a5ecb7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/char.m @@ -0,0 +1,74 @@ +% 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.petercorke.com +function s = char(links, from_robot) + %Link.char String representation of parameters + % + % s = L.char() is a string showing link parameters in compact single line format. + % If L is a vector of Link objects return a string with one line per Link. + % + % See also Link.display. + + % display in the order theta d a alpha + if nargin < 2 + from_robot = false; + end + + s = ''; + for j=1:length(links) + l = links(j); + if from_robot + if l.sigma == 1 + % prismatic joint + js = sprintf('|%3d|%11.4g|%11s|%11.4g|%11.4g|', ... + j, l.theta, sprintf('q%d', j), l.a, l.alpha); + else + js = sprintf('|%3d|%11s|%11.4g|%11.4g|%11.4g|', ... + j, sprintf('q%d', j), l.d, l.a, l.alpha); + end + else + if l.sigma == 0, + conv = 'R'; + else + conv = 'P'; + end + if l.mdh == 0 + conv = [conv ',stdDH']; + else + conv = [conv ',modDH']; + end + if length(links) == 1 + qname = 'q'; + else + qname = sprintf('q%d', j); + end + + if l.sigma == 1 + % prismatic joint + js = sprintf(' theta=%.4g, d=%s, a=%.4g, alpha=%.4g (%s)', ... + l.theta, qname, l.a, l.alpha, conv); + else + js = sprintf(' theta=%s, d=%.4g, a=%.4g, alpha=%.4g (%s)', ... + qname, l.d, l.a, l.alpha, conv); + end + end + s = strvcat(s, js); + end + end % char() diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/display.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/display.m new file mode 100644 index 0000000..f272556 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/display.m @@ -0,0 +1,29 @@ +%DISPLAY display the value of a LINK object + +% 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.petercorke.com + +% Copright (C) Peter Corke 1999 +function display(l) + + disp([inputname(1), ' = ']) + disp( char(l) ); + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/friction.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/friction.m new file mode 100644 index 0000000..295d0bb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/friction.m @@ -0,0 +1,45 @@ +%FRICTION compute friction torque on the LINK object +% +% TAU = FRICTION(LINK, QD) +% +% Return the friction torque on the link moving at speed QD. Depending +% on fields in the LINK object viscous and/or Coulomb friction +% are computed. +% + +% 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.petercorke.com + +function tau = friction(l, qd) +%Link.friction Joint friction force +% +% F = L.friction(QD) is the joint friction force/torque for link velocity QD + tau = 0.0; + + tau = l.B * qd; + + if qd > 0 + tau = tau + l.Tc(1); + elseif qd < 0 + tau = tau + l.Tc(2); + end + tau = -tau; % friction opposes motion +endfunction % friction() diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/nofriction.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/nofriction.m new file mode 100644 index 0000000..5148edb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/nofriction.m @@ -0,0 +1,41 @@ +%NOFRICTION return link object with zero friction +% +% LINK = NOFRICTION(LINK) +% +% + +% 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.petercorke.com +function l2 = nofriction(l, only) +%Link.nofriction Remove friction +% +% LN = L.nofriction() is a link object with the same parameters as L except +% nonlinear (Coulomb) friction parameter is zero. +% +% LN = L.nofriction('all') is a link object with the same parameters as L +% except all friction parameters are zero. + + l2 = Link(l); + if (nargin == 2) && strcmpi(only(1:3), 'all') + l2.B = 0; + end + l2.Tc = [0 0]; +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/show.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/show.m new file mode 100644 index 0000000..887ed3f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/show.m @@ -0,0 +1,41 @@ +%SHOW show all parameters of LINK object +% +% SHOW(link) + +% 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.petercorke.com + +function show(l) + + llab = 6; + for n =fieldnames(l)' + v = getfield(l, char(n)); + name = char(n); + spaces = char(' '*ones(1,llab-length(name))); + val = num2str(v); + label = [name spaces ' = ']; + if numrows(val) > 1, + pad = {label; char(' '*ones(numrows(val)-1,1))}; + else + pad = label; + end + disp([char(pad) val]); + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/sigma.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/sigma.m new file mode 100644 index 0000000..dccdd13 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/sigma.m @@ -0,0 +1,21 @@ +% 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.petercorke.com + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/subsasgn.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/subsasgn.m new file mode 100644 index 0000000..f49535e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/subsasgn.m @@ -0,0 +1,115 @@ +% 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.petercorke.com +function this = subsasgn(this, index, value) + + switch index(1).type + case '.' + switch index(1).subs, + case 'alpha', + this.alpha = value; + case 'a', + this.a = value; + case 'theta', + this.theta = value; + case 'd', + this.d = value; + case 'offset', + this.offset = value; + case 'sigma', + if ischar(value) + this.sigma = lower(value) == 'p'; + else + this.sigma = value; + end + case 'mdh', + this.mdh = value; + case 'G', + this.G = value; + case 'I', + if isempty(value) + return; + end + if all(size(value) == [3 3]) + if norm(value-value') > eps + error('inertia matrix must be symmetric'); + end + this.I = value; + elseif length(value) == 3 + this.I = diag(value); + elseif length(value) == 6 + this.I = [ value(1) value(4) value(6) + value(4) value(2) value(5) + value(6) value(5) value(3)]; + end + case 'r', + if isempty(value) + return; + end + if length(value) ~= 3 + error('COG must be a 3-vector'); + end + this.r = value(:)'; + case 'Jm', + this.Jm = value; + case 'B', + this.B = value; + case 'Tc', + if isempty(value) + return; + end + if length(value) == 1 + this.Tc = [value -value]; + elseif length(value) == 2 + if value(1) < value(2) + error('Coulomb friction is [Tc+ Tc-]'); + end + this.Tc = value; + else + error('Coulomb friction vector can have 1 (symmetric) or 2 (asymmetric) elements only') + end + case 'm', + this.m = value; + case 'qlim', + if length(value) ~= 2, + error('joint limit must have 2 elements'); + end + this.qlim = value; + + + otherwise, error('Unknown method') + end + + case '()' + if numel(index) == 1 + if isempty(this) + this = value; %% this is a crude bug fix + end + + this = builtin('subsasgn', this, index, value); + else + this_subset = this(index(1).subs{:}); % get the subset + + this_subset = subsasgn(this_subset, index(2:end), value); + + this(index(1).subs{:}) = this_subset; % put subset back; + end + end +endfunction diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Link/subsref.m b/lwrserv/matlab/rvctools/robot/Octave/@Link/subsref.m new file mode 100644 index 0000000..5c8d171 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Link/subsref.m @@ -0,0 +1,281 @@ +% 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.petercorke.com +function v = subsref(l, s) + switch s(1).type + case '.' + % NOTE WELL: the following code can't use getfield() since + % getfield() uses this, and Matlab will crash!! + + el = char(s(1).subs); + switch el, + case 'alpha', + v = l.alpha; + case 'a', + v = l.a; + case 'theta', + v = l.theta; + case 'd', + v = l.d; + case 'offset', + v = l.offset; + case 'sigma', + v = l.sigma; + case 'RP', + if l.sigma == 0, + v = 'R'; + else + v = 'P'; + end + case 'mdh', + v = l.mdh; + case 'G', + v = l.G; + case 'I', + v = l.I; + case 'r', + v = l.r; + case 'Jm', + v = l.Jm; + case 'B', + v = l.B; + case 'Tc', + v = l.Tc; + case 'qlim', + v = l.qlim; + case 'islimit', + if s(2).type ~= '()' + error('expecting argument for islimit method'); + end + q = s(2).subs{1}; + v = (q > l.qlim(2)) - (q < l.qlim(1)); + case 'm', + v = l.m; + case 'dh', + v = [l.alpha l.A l.theta l.D l.sigma]; + case 'dyn', + dyn(l); + case 'A', + if s(2).type ~= '()' + error('expecting argument for A method'); + else + + args = s(2).subs; + v = A(l,args{1}); + end + + case 'nofriction', + q = s(2).subs; + v = nofriction(l,q{:}); + otherwise, + disp('Unknown method ref') + end + case '()' + + if numel(s) == 1 + + v = builtin('subsref', l, s); + else + z = s(1).subs; % cell array + + k = z{1}; + l = l(k); + v_subset = subsref(l, s(2:end)); + + v = v_subset; % put subset back; + end + otherwise + error('only .field supported') + end %switch + + +endfunction + +function T = A(L, q) +%Link.A Link transform matrix +% +% T = L.A(Q) is the 4x4 link homogeneous transformation matrix corresponding +% to the link variable Q which is either theta (revolute) or d (prismatic). +% +% Notes:: +% - For a revolute joint the theta parameter of the link is ignored, and Q used instead. +% - For a prismatic joint the d parameter of the link is ignored, and Q used instead. +% - The link offset parameter is added to Q before computation of the transformation matrix. + if L.mdh == 0 + T = linktran([L.alpha L.a L.theta L.d L.sigma], ... + q+L.offset); + else + T = mlinktran([L.alpha L.a L.theta L.d L.sigma], ... + q+L.offset); + end +endfunction % A() + +function t = linktran(a, b, c, d) +%LINKTRAN Compute the link transform from kinematic parameters +% +% LINKTRAN(alpha, an, theta, dn) +% LINKTRAN(DH, q) is a homogeneous +% transformation between link coordinate frames. +% +% alpha is the link twist angle +% an is the link length +% theta is the link rotation angle +% dn is the link offset +% sigma is 0 for a revolute joint, non-zero for prismatic +% +% In the second case, q is substitued for theta or dn according to sigma. +% +% Based on the standard Denavit and Hartenberg notation. + +% Copright (C) Peter Corke 1993 + + if nargin == 4, + alpha = a; + an = b; + theta = c; + dn = d; + else + if numcols(a) < 4, + error('too few columns in DH matrix'); + end + alpha = a(1); + an = a(2); + if numcols(a) > 4, + if a(5) == 0, % revolute + theta = b; + dn = a(4); + else % prismatic + theta = a(3); + dn = b; + end + else + theta = b; % assume revolute if sigma not given + dn = a(4); + end + end + sa = sin(alpha); ca = cos(alpha); + st = sin(theta); ct = cos(theta); + + t = [ ct -st*ca st*sa an*ct + st ct*ca -ct*sa an*st + 0 sa ca dn + 0 0 0 1]; +endfunction +%MLINKTRANS Compute the link transform from kinematic parameters +% +% MLINKTRANS(alpha, an, theta, dn) +% MLINKTRANS(DH, q) is a homogeneous +% transformation between link coordinate frames. +% +% alpha is the link twist angle +% an is the link length +% theta is the link rotation angle +% dn is the link offset +% sigma is 0 for a revolute joint, non-zero for prismatic +% +% In the second case, q is substitued for theta or dn according to sigma. +% +% Based on the modified Denavit and Hartenberg notation. + +% Copright (C) Peter Corke 1993 +function t = mlinktrans(a, b, c, d) + + if nargin == 4, + alpha = a; + an = b; + theta = c; + dn = d; + else + if numcols(a) < 4, + error('too few columns in DH matrix'); + end + alpha = a(1); + an = a(2); + if numcols(a) > 4, + if a(5) == 0, % revolute + theta = b; + dn = a(4); + else % prismatic + theta = a(3); + dn = b; + end + else + theta = b; % assume revolute if no sigma given + dn = a(4); + end + end + sa = sin(alpha); ca = cos(alpha); + st = sin(theta); ct = cos(theta); + + t = [ ct -st 0 an + st*ca ct*ca -sa -sa*dn + st*sa ct*sa ca ca*dn + 0 0 0 1]; +endfunction + +function dyn(l) +%Link.dyn Display the inertial properties of link +% +% L.dyn() displays the inertial properties of the link object in a multi-line format. +% The properties shown are mass, centre of mass, inertia, friction, gear ratio +% and motor properties. +% +% If L is a vector of Link objects show properties for each element. + + if length(l) > 1 + for j=1:length(l) + ll = l(j); + fprintf('%d: %f; %f %f %f; %f %f %f\n', ... + j, ll.m, ll.r, diag(ll.I)); + %dyn(ll); + end + return; + end + + display(l); + if ~isempty(l.m) + fprintf(' m = %f\n', l.m) + end + if ~isempty(l.r) + fprintf(' r = %f %f %f\n', l.r); + end + if ~isempty(l.I) + fprintf(' I = | %f %f %f |\n', l.I(1,:)); + fprintf(' | %f %f %f |\n', l.I(2,:)); + fprintf(' | %f %f %f |\n', l.I(3,:)); + end + if ~isempty(l.Jm) + fprintf(' Jm = %f\n', l.Jm); + end + if ~isempty(l.B) + fprintf(' Bm = %f\n', l.B); + end + if ~isempty(l.Tc) + fprintf(' Tc = %f(+) %f(-)\n', l.Tc(1), l.Tc(2)); + end + if ~isempty(l.G) + fprintf(' G = %f\n', l.G); + end + if ~isempty(l.qlim) + fprintf(' qlim = %f to %f\n', l.qlim(1), l.qlim(2)); + end +endfunction % dyn() + + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/Quaternion.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/Quaternion.m new file mode 100644 index 0000000..51b42d2 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/Quaternion.m @@ -0,0 +1,144 @@ +%QUATERNION constructor for quaternion objects +% +% QUATERNION([s v1 v2 v3]) from 4 elements +% QUATERNION(v, theta) from vector plus angle +% QUATERNION(R) from a 3x3 or 4x4 matrix +% QUATERNION(q) from another quaternion + +% 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.petercorke.com + +function q = Quaternion(a1, a2) + + + if nargin == 0, + q.s = 1; + q.v = [0 0 0]; + q = class (q, 'Quaternion'); + + elseif nargin == 1 + if isa(a1, 'Quaternion') + q = a1; + q = class(q, 'Quaternion'); + elseif isreal (a1) && size(a1) == 1 + q.s = a1(1); + q.v = [0,0,0]; + q = class(q, 'Quaternion'); + elseif isreal (a1) && all (size (a1) == [1 3]) # Quaternion (vector part) + q.s = 0; + q.v = a1(1:3); + q = class(q, 'Quaternion'); + elseif all(size(a1) == [3 3]) + q = Quaternion( tr2q(a1) ); + elseif all(size(a1) == [4 4]) + q = Quaternion( tr2q(a1(1:3,1:3)) ); + elseif all(size(a1) == [1 4]) + q.s = a1(1); + q.v = a1(2:4); + q = class(q, 'Quaternion'); + else + error('unknown dimension of input'); + end + elseif nargin == 2 + if isscalar(a1) && isvector(a2) + q.s = cos(a1/2); + q.v = (sin(a1/2)*unit(a2(:)')); + q = class(q, 'Quaternion'); + end + end +endfunction +%TR2Q Convert homogeneous transform to a unit-quaternion +% +% Q = tr2q(T) +% +% Return a unit quaternion corresponding to the rotational part of the +% homogeneous transform T. +% +% See also Q2TR + + +% Ryan Steidnl based on Robotics Toolbox for MATLAB (v6 and v9) +% + +% 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.petercorke.com +function q = tr2q(t) + qs = sqrt(trace(t)+1)/2.0; + kx = t(3,2) - t(2,3); % Oz - Ay + ky = t(1,3) - t(3,1); % Ax - Nz + kz = t(2,1) - t(1,2); % Ny - Ox + + if (t(1,1) >= t(2,2)) & (t(1,1) >= t(3,3)) + kx1 = t(1,1) - t(2,2) - t(3,3) + 1; % Nx - Oy - Az + 1 + ky1 = t(2,1) + t(1,2); % Ny + Ox + kz1 = t(3,1) + t(1,3); % Nz + Ax + add = (kx >= 0); + elseif (t(2,2) >= t(3,3)) + kx1 = t(2,1) + t(1,2); % Ny + Ox + ky1 = t(2,2) - t(1,1) - t(3,3) + 1; % Oy - Nx - Az + 1 + kz1 = t(3,2) + t(2,3); % Oz + Ay + add = (ky >= 0); + else + kx1 = t(3,1) + t(1,3); % Nz + Ax + ky1 = t(3,2) + t(2,3); % Oz + Ay + kz1 = t(3,3) - t(1,1) - t(2,2) + 1; % Az - Nx - Oy + 1 + add = (kz >= 0); + end + + if add + kx = kx + kx1; + ky = ky + ky1; + kz = kz + kz1; + else + kx = kx - kx1; + ky = ky - ky1; + kz = kz - kz1; + end + nm = norm([kx ky kz]); + if nm == 0, + q = Quaternion([1 0 0 0]); + else + s = sqrt(1 - qs^2) / nm; + qv = s*[kx ky kz]; + + q = Quaternion([qs qv]); + + end +endfunction + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/char.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/char.m new file mode 100644 index 0000000..be02908 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/char.m @@ -0,0 +1,27 @@ +%CHAR create string representation of quaternion object + +% 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.petercorke.com + +% Copright (C) Peter Corke 1999 +function s = char(q) + + s = [num2str(q.s), ' <' num2str(q.v(1)) ', ' num2str(q.v(2)) ', ' num2str(q.v(3)) '>']; diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/display.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/display.m new file mode 100644 index 0000000..800eeb1 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/display.m @@ -0,0 +1,31 @@ +%DISPLAY display the value of a quaternion object + +% 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.petercorke.com + +% Copright (C) Peter Corke 1999 +function display(q) + + disp(' '); + disp([inputname(1), ' = ']) + disp(' '); + disp([' ' char(q)]) + disp(' '); diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/dot.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/dot.m new file mode 100644 index 0000000..55091c7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/dot.m @@ -0,0 +1,25 @@ +% 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.petercorke.com +function qd = dot(q, omega) + E = q.s*eye(3,3) - skew(q.v); + omega = omega(:); + qd = Quaternion(-0.5*q.v*omega, 0.5*E*omega); +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/double.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/double.m new file mode 100644 index 0000000..d0b463e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/double.m @@ -0,0 +1,29 @@ +% 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.petercorke.com +function v = double(q) +%Quaternion.double Convert a quaternion object to a 4-element vector +% +% V = Q.double() is a 4-vector comprising the quaternion +% elements [s vx vy vz]. + + v = [q.s q.v]; +endfunction + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/interp.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/interp.m new file mode 100644 index 0000000..9ad34d0 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/interp.m @@ -0,0 +1,63 @@ +% 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.petercorke.com +function q = interp(Q1, Q2, r) +%Quaternion.interp Interpolate rotations expressed by quaternion objects +% +% QI = Q1.interp(Q2, R) is a unit-quaternion that interpolates between Q1 for R=0 +% to Q2 for R=1. This is a spherical linear interpolation (slerp) that can be +% interpretted as interpolation along a great circle arc on a sphere. +% +% If R is a vector QI is a vector of quaternions, each element +% corresponding to sequential elements of R. +% +% Notes: +% - the value of r is clipped to the interval 0 to 1 +% +% See also ctraj, Quaternion.scale. + + q1 = double(Q1); + q2 = double(Q2); + + theta = acos(q1*q2'); + count = 1; + + % clip values of r + r(r<0) = 0; + r(r>1) = 1; + + if length(r) == 1 + if theta == 0 + q = Q1; + else + q = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ); + end + else + for R=r(:)' + if theta == 0 + qq = Q1; + else + qq = Quaternion( (sin((1-R)*theta) * q1 + sin(R*theta) * q2) / sin(theta) ); + end + q(count) = qq; + count = count + 1; + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/inv.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/inv.m new file mode 100644 index 0000000..5f9343d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/inv.m @@ -0,0 +1,27 @@ +% 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.petercorke.com +function qi = inv(q) +%Quaternion.inv Invert a unit-quaternion +% +% QI = Q.inv() is a quaternion object representing the inverse of Q. + + qi = Quaternion([q.s -q.v]); +endfunction diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/minus.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/minus.m new file mode 100644 index 0000000..8b0182d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/minus.m @@ -0,0 +1,30 @@ +% 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.petercorke.com +function qp = minus(q1, q2) +%Quaternion.minus Subtract two quaternion objects +% +% Q1-Q2 is the element-wise difference of quaternion elements. + + if isa(q1, 'Quaternion') & isa(q2, 'Quaternion') + + qp = Quaternion(double(q1) - double(q2)); + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mpower.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mpower.m new file mode 100644 index 0000000..92be847 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mpower.m @@ -0,0 +1,42 @@ +% 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.petercorke.com +function qp = mpower(q, p) +%Quaternion.mpower Raise quaternion to integer power +% +% Q^N is quaternion Q raised to the integer power N, and computed by repeated multiplication. + + % check that exponent is an integer + if (p - floor(p)) ~= 0 + error('quaternion exponent must be integer'); + end + + qp = q; + + % multiply by itself so many times + for i = 2:abs(p) + qp = qp * q; + end + + % if exponent was negative, invert it + if p<0 + qp = inv(qp); + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mrdivide.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mrdivide.m new file mode 100644 index 0000000..82a544a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mrdivide.m @@ -0,0 +1,35 @@ +% 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.petercorke.com +function qq = mrdivide(q1, q2) +%Quaternion.mrdivide Compute quaternion quotient. +% +% Q1/Q2 is a quaternion formed by Hamilton product of Q1 and inv(Q2) +% Q/S is the element-wise division of quaternion elements by by the scalar S + + if isa(q2, 'Quaternion') + % qq = q1 / q2 + % = q1 * qinv(q2) + + qq = q1 * inv(q2); + elseif isa(q2, 'double') + qq = Quaternion( double(q1) / q2 ); + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mtimes.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mtimes.m new file mode 100644 index 0000000..13f3364 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/mtimes.m @@ -0,0 +1,74 @@ +% 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.petercorke.com +function qp = mtimes(q1, q2) +%Quaternion.mtimes Multiply a quaternion object +% +% Q1*Q2 is a quaternion formed by Hamilton product of two quaternions. +% Q*V is the vector V rotated by the quaternion Q +% Q*S is the element-wise multiplication of quaternion elements by by the scalar S + + if isa(q1, 'Quaternion') & isa(q2, 'Quaternion') + %QQMUL Multiply unit-quaternion by unit-quaternion + % + % QQ = qqmul(Q1, Q2) + % + % Return a product of unit-quaternions. + % + % See also: TR2Q + + + % decompose into scalar and vector components + s1 = q1.s; v1 = q1.v; + s2 = q2.s; v2 = q2.v; + + % form the product + qp = Quaternion([s1*s2-v1*v2' s1*v2+s2*v1+cross(v1,v2)]); + + elseif isa(q1, 'Quaternion') & isa(q2, 'double') + + %QVMUL Multiply vector by unit-quaternion + % + % VT = qvmul(Q, V) + % + % Rotate the vector V by the unit-quaternion Q. + % + % See also: QQMUL, QINV + + if length(q2) == 3 + qp = q1 * Quaternion([0 q2(:)']) * inv(q1); + qp = qp.v(:); + elseif length(q2) == 1 + qp = Quaternion(double(q1)*q2); + else + error('quaternion-vector product: must be a 3-vector or scalar'); + end + + elseif isa(q2, 'Quaternion') & isa(q1, 'double') + if length(q1) == 3 + qp = q2 * Quaternion([0 q1(:)']) * inv(q2); + qp = qp.v; + elseif length(q1) == 1 + qp = Quaternion(double(q2)*q1); + else + error('quaternion-vector product: must be a 3-vector or scalar'); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/norm.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/norm.m new file mode 100644 index 0000000..56c1d40 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/norm.m @@ -0,0 +1,27 @@ +% 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.petercorke.com +function n = norm(q) +%Quaternion.norm Compute the norm of a quaternion +% +% QN = Q.norm(Q) is the scalar norm or magnitude of the quaternion Q. + + n = norm(double(q)); +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/plot.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/plot.m new file mode 100644 index 0000000..af08d9e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/plot.m @@ -0,0 +1,45 @@ +%PLOT plot a quaternion object as a rotated coordinate frame + +% 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.petercorke.com + +% Copright (C) Peter Corke 1999 +function plot(Q) + axis([-1 1 -1 1 -1 1]) + + + o = [0 0 0]'; + x1 = Q*[1 0 0]'; + y1 = Q*[0 1 0]'; + z1 = Q*[0 0 1]'; + + hold on + plot3([0;x1(1)], [0; x1(2)], [0; x1(3)]) + text(x1(1), x1(2), x1(3), 'X') + plot3([0;y1(1)], [0; y1(2)], [0; y1(3)]) + text(y1(1), y1(2), y1(3), 'Y') + plot3([0;z1(1)], [0; z1(2)], [0; z1(3)]) + text(z1(1), z1(2), z1(3), 'Z') + grid on + xlabel('X') + ylabel('Y') + zlabel('Z') + hold off diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/plus.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/plus.m new file mode 100644 index 0000000..1904801 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/plus.m @@ -0,0 +1,30 @@ +% 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.petercorke.com +function qp = plus(q1, q2) +%PLUS Add two quaternion objects +% +% Q1+Q2 is the element-wise sum of quaternion elements. + + if isa(q1, 'Quaternion') & isa(q2, 'Quaternion') + + qp = Quaternion(double(q1) + double(q2)); + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/q2tr.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/q2tr.m new file mode 100644 index 0000000..e0c38a4 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/q2tr.m @@ -0,0 +1,35 @@ +% 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.petercorke.com +function t = q2tr(q) + + q = double(q); + s = q(1); + x = q(2); + y = q(3); + z = q(4); + + r = [ 1-2*(y^2+z^2) 2*(x*y-s*z) 2*(x*z+s*y) + 2*(x*y+s*z) 1-2*(x^2+z^2) 2*(y*z-s*x) + 2*(x*z-s*y) 2*(y*z+s*x) 1-2*(x^2+y^2) ]; + t = eye(4,4); + t(1:3,1:3) = r; + t(4,4) = 1; +endfunction diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/qinterp.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/qinterp.m new file mode 100644 index 0000000..9ecf04b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/qinterp.m @@ -0,0 +1,60 @@ +%QINTERP Interpolate rotations expressed by quaternion objects +% +% QI = qinterp(Q1, Q2, R) +% +% Return a unit-quaternion that interpolates between Q1 and Q2 as R moves +% from 0 to 1. This is a spherical linear interpolation (slerp) that can +% be interpretted as interpolation along a great circle arc on a sphere. +% +% If r is a vector, QI, is a cell array of quaternions. +% +% See also TR2Q + +% 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.petercorke.com + +% MOD HISTORY +% 2/99 convert to use of objects + +% Copright (C) Peter Corke 1999 +function q = qinterp(Q1, Q2, r) + + + q1 = double(Q1); + q2 = double(Q2); + + if (r<0) | (r>1), + error('R out of range'); + end + + theta = acos(q1*q2'); + q = {}; + count = 1; + + if length(r) == 1, + q = quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ); + else + for R=r(:)', + qq = quaternion( (sin((1-R)*theta) * q1 + sin(R*theta) * q2) / sin(theta) ); + q{count} = qq; + count = count + 1; + end + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/scale.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/scale.m new file mode 100644 index 0000000..a1efcc6 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/scale.m @@ -0,0 +1,61 @@ + +% 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.petercorke.com +function q = scale(Q, r) +%Quaternion.scale Interpolate rotations expressed by quaternion objects +% +% QI = Q.scale(R) is a unit-quaternion that interpolates between identity for R=0 +% to Q for R=1. This is a spherical linear interpolation (slerp) that can +% be interpretted as interpolation along a great circle arc on a sphere. +% +% If R is a vector QI is a cell array of quaternions, each element +% corresponding to sequential elements of R. +% +% See also ctraj, Quaternion.interp. + + + q2 = double(Q); + + if any(r<0) || (r>1) + error('r out of range'); + end + q1 = [1 0 0 0]; % identity quaternion + theta = acos(q1*q2'); + + if length(r) == 1 + if theta == 0 + q = Q; + else + q = unit(Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) )); + end + else + count = 1; + for R=r(:)' + if theta == 0 + qq = Q; + else + qq = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ).unit; + end + q(count) = qq; + count = count + 1; + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/subsref.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/subsref.m new file mode 100644 index 0000000..17042b5 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/subsref.m @@ -0,0 +1,81 @@ + +% 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.petercorke.com +function v = subsref(q, s) + if (length (s)<2) + if s(1).type == '.' + + % NOTE WELL: the following code can't use getfield() since + % getfield() uses this, and Matlab will crash!! + + el = char(s(1).subs); + switch el + case 'd' + v = double(q); + case 's' + v = q.s; + case 'v' + v = q.v; + case 'T' + v = q2tr(q); + case 'R' + v = q2tr(q); + v = v(1:3,1:3); + case 'inv' + v = inv(q); + case 'norm' + v = norm(q); + case 'unit' + v = unit(q); + case 'double' + v = double(q); + case 'plot' + v = plot(q); + end + else + error('only .field supported') + end + + elseif (length(s) == 2 ) + if s(1).type == '.' + + % NOTE WELL: the following code can't use getfield() since + % getfield() uses this, and Matlab will crash!! + + el = char(s(1).subs); + args = s(2).subs; + switch el + case 'interp' + v = interp(q,args{:}); + case 'scale' + v = scale(q,args{:}); + case 'dot' + v = dot(q,args{:}); + end + else + error('only .field supported') + end + else + error('only .field supported') + end +endfunction + + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/unit.m b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/unit.m new file mode 100644 index 0000000..0d3dc1f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@Quaternion/unit.m @@ -0,0 +1,28 @@ + +% 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.petercorke.com +function qu = unit(q) +%Quaternion.unit Unitize a quaternion +% +% QU = Q.unit() is a quaternion which is a unitized version of Q + + qu = q / norm(q); +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/SerialLink.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/SerialLink.m new file mode 100644 index 0000000..0d3fa9d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/SerialLink.m @@ -0,0 +1,135 @@ +%ROBOT robot object constructor +% +% ROBOT +% ROBOT(robot) create a copy of an existing ROBOT object +% ROBOT(LINK, ...) create from a cell array of LINK objects +% ROBOT(DH, ...) create from legacy DYN matrix +% ROBOT(DYN, ...) create from legacy DYN matrix +% +% optional trailing arguments are: +% Name robot type or name +% Manufacturer who built it +% Comment general comment +% +% If the legacy matrix forms are used the default name is the workspace +% variable that held the data. + +% 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.petercorke.com +function r = SerialLink(L, varargin) + r.name = 'noname'; + r.manufacturer = ''; + r.comment = ''; + r.links = []; + r.n = 0; + r.mdh = 0; + r.gravity = [0; 0; 9.81]; + r.base = eye(4,4); + r.tool = eye(4,4); + r.handle = []; % graphics handles + r.q = []; % current joint angles + + r.lineopt = {'Color', 'black', 'Linewidth', 4}; + r.shadowopt = {'Color', 0.7*[1 1 1], 'Linewidth', 3}; + r.plotopt = {}; + r = class(r, 'SerialLink'); + + if nargin == 0 + %zero argument constructor + return; + + else + if isa(L, 'SerialLink') + if length(L) == 1 + % clone the passed robot + for j=1:L.n + newlinks(j) = L.links(j); + end + r.links = newlinks; + else + % compound the robots in the vector + r = L(1); + for k=2:length(L) + r = r * L(k); + end + end + elseif isa(L,'Link') + r.links = L; %attach the links + + elseif isa(L, 'double') + % legacy matrix + dh_dyn = L; + clear L + for j=1:numrows(dh_dyn) + L(j) = Link(); + L(j).theta = dh_dyn(j,1); + L(j).d = dh_dyn(j,2); + L(j).a = dh_dyn(j,3); + L(j).alpha = dh_dyn(j,4); + + if numcols(dh_dyn) > 4 + L(j).sigma = dh_dyn(j,5); + end + end + r.links = L; + else + error('unknown type passed to SerialLink'); + end + r.n = length(r.links); + end + %process the rest of the arguments in key, value pairs + opt.name = 'robot'; + opt.comment = []; + opt.manufacturer = []; + opt.base = []; + opt.tool = []; + opt.offset = []; + opt.qlim = []; + opt.plotopt = []; + + [opt,out] = tb_optparse(opt, varargin); + if ~isempty(out) + error( sprintf('unknown option <%s>', out{1})); + end + + % copy the properties to robot object + p = fieldnames(r); + for i=1:length(p) + if isfield(opt, p{i}) && ~isempty(getfield(opt, p{i})) + + r = setfield(r, p{i}, getfield(opt, p{i})); + + end + end + + + % set the robot object mdh status flag + mdh = [r.links.mdh]; + if all(mdh == 0) + r.mdh = mdh(1); + elseif all (mdh == 1) + r.mdh = mdh(1); + else + error('SerialLink has mixed D&H links conventions'); + end + r = class(r, 'SerialLink'); + +endfunction diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/accel.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/accel.m new file mode 100644 index 0000000..4eee703 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/accel.m @@ -0,0 +1,92 @@ +%SerialLink.accel Manipulator forward dynamics +% +% QDD = R.accel(Q, QD, TORQUE) is a vector (Nx1) of joint accelerations that result +% from applying the actuator force/torque to the manipulator robot in state Q and QD. +% If Q, QD, TORQUE are matrices with M rows, then QDD is a matrix with M rows +% of acceleration corresponding to the equivalent rows of Q, QD, TORQUE. +% +% QDD = R.ACCEL(X) as above but X=[Q,QD,TORQUE]. +% +% Note:: +% - Uses the method 1 of Walker and Orin to compute the forward dynamics. +% - This form is useful for simulation of manipulator dynamics, in +% conjunction with a numerical integration function. +% +% See also SerialLink.rne, SerialLink, ode45. + +% 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.petercorke.com + + +function qdd = accel(robot, Q, qd, torque) + + if numcols(Q) ~= robot.n + error('q must have %d columns', robot.n); + end + if numcols(qd) ~= robot.n + error('qd must have %d columns', robot.n); + end + if numcols(torque) ~= robot.n + error('torque must have %d columns', robot.n); + end + + if numrows(Q) > 1 + if numrows(Q) ~= numrows(qd) + error('for trajectory q and qd must have same number of rows'); + end + if numrows(Q) ~= numrows(torque) + error('for trajectory q and torque must have same number of rows'); + end + qdd = []; + for i=1:numrows(Q) + qdd = cat(1, qdd, robot.accel(Q(i,:), qd(i,:), torque(i,:))'); + end + return + end + + n = robot.n; + + if nargin == 2 + % accel(X) + q = Q(1:n); + qd = Q(n+1:2*n); + torque = Q(2*n+1:3*n); + else + % accel(Q, qd, torque) + q = Q; + if length(q) == robot.n, + q = q(:); + qd = qd(:); + end + end + + % compute current manipulator inertia + % torques resulting from unit acceleration of each joint with + % no gravity. + M = rne(robot, ones(n,1)*q', zeros(n,n), eye(n), [0;0;0]); + + % compute gravity and coriolis torque + % torques resulting from zero acceleration at given velocity & + % with gravity acting. + tau = rne(robot, q', qd', zeros(1,n)); + + qdd = inv(M) * (torque(:) - tau'); + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/char.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/char.m new file mode 100644 index 0000000..e9d64b2 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/char.m @@ -0,0 +1,77 @@ + +% 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.petercorke.com +function s = char(robot) +% +% S = R.char() is a string representation of the robot parameters. + + s = ''; + for j=1:length(robot) + r = robot(j); + + % informational line + if r.mdh + convention = 'modDH'; + else + convention = 'stdDH'; + end + s = sprintf('%s (%d axis, %s, %s)', r.name, r.n, config(r), convention); + + % comment and other info + line = ''; + if ~isempty(r.manufacturer) + line = strcat(line, sprintf(' %s;', r.manufacturer)); + end + if ~isempty(r.comment) + line = strcat(line, sprintf(' %s;', r.comment)); + end + s = strvcat(s, line); + + % link parameters + s = strvcat(s, '+---+-----------+-----------+-----------+-----------+'); + s = strvcat(s, '| j | theta | d | a | alpha |'); + s = strvcat(s, '+---+-----------+-----------+-----------+-----------+'); + s = strvcat(s, char(r.links, true)); + s = strvcat(s, '+---+-----------+-----------+-----------+-----------+'); + + % gravity, base, tool + + s_grav = horzcat(strvcat('grav = ', ' ', ' '), num2str(r.gravity)); + s_grav = strvcat(s_grav, " "); + s_base = horzcat(strvcat(" base = ",' ',' ', ' '),num2str(r.base)); + + s_tool = horzcat(strvcat(' tool = ',' ',' ', ' '), num2str(r.tool)); + + line = horzcat(s_grav, s_base, s_tool); + + s = strvcat(s, ' ', line); + if j ~= length(robot) + s = strvcat(s, ' '); + end + end +endfunction + +function v = config(r) + v = ''; + for i=1:r.n + v(i) = r.links(i).RP; + end + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/cinertia.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/cinertia.m new file mode 100644 index 0000000..edbc3a6 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/cinertia.m @@ -0,0 +1,34 @@ +%SerialLink.cinertia Cartesian inertia matrix +% +% M = R.cinertia(Q) is the NxN Cartesian (operational space) inertia matrix which relates +% Cartesian force/torque to Cartesian acceleration at the joint configuration Q, and N +% is the number of robot joints. +% +% See also SerialLink.inertia, SerialLink.rne. + +% 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.petercorke.com + +function Mx = cinertia(robot, q) + J = jacob0(robot, q); + Ji = inv(J); + M = inertia(robot, q); + Mx = Ji' * M * Ji; diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/coriolis.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/coriolis.m new file mode 100644 index 0000000..644c479 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/coriolis.m @@ -0,0 +1,96 @@ +%SerialLink.coriolis Coriolis matrix +% +% C = R.CORIOLIS(Q, QD) is the NxN Coriolis/centripetal matrix for +% the robot in configuration Q and velocity QD, where N is the number of +% joints. The product C*QD is the vector of joint force/torque due to velocity +% coupling. The diagonal elements are due to centripetal effects and the +% off-diagonal elements are due to Coriolis effects. This matrix is also +% known as the velocity coupling matrix, since gives the disturbance forces +% on all joints due to velocity of any joint. +% +% If Q and QD are matrices (DxN), each row is interpretted as a joint state +% vector, and the result (NxNxD) is a 3d-matrix where each plane corresponds +% to a row of Q and QD. +% +% Notes:: +% - joint friction is also a joint force proportional to velocity but it is +% eliminated in the computation of this value. +% - computationally slow, involves N^2/2 invocations of RNE. +% +% See also SerialLink.rne. + +% 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.petercorke.com + +function C = coriolis(robot, q, qd) + + if numcols(q) ~= robot.n + error('q must have %d columns', robot.n); + end + if numcols(qd) ~= robot.n + error('qd must have %d columns', robot.n); + end + + % we need to create a clone robot with no friciton, since friction + % is also proportional to joint velocity + robot2 = nofriction(robot,'all'); + + if numrows(q) > 1 + if numrows(q) ~= numrows(qd) + error('for trajectory q and qd must have same number of rows'); + end + C = []; + for i=1:numrows(q) + C = cat(3, C, robot2.coriolis(q(i,:), qd(i,:))); + end + return + end + + N = robot2.n; + C = zeros(N,N); + Csq = zeros(N,N); + + % find the torques that depend on a single finite joint speed, + % these are due to the squared (centripetal) terms + % + % set QD = [1 0 0 ...] then resulting torque is due to qd_1^2 + for j=1:N + QD = zeros(1,N); + QD(j) = 1; + tau = rne(robot2,q, QD, zeros(size(q)), [0 0 0]'); + Csq(:,j) = Csq(:,j) + tau'; + end + + % find the torques that depend on a pair of finite joint speeds, + % these are due to the product (Coridolis) terms + % set QD = [1 1 0 ...] then resulting torque is due to + % qd_1 qd_2 + qd_1^2 + qd_2^2 + for j=1:N + for k=j+1:N + % find a product term qd_j * qd_k + QD = zeros(1,N); + QD(j) = 1; + QD(k) = 1; + tau = rne(robot2,q, QD, zeros(size(q)), [0 0 0]'); + C(:,k) = C(:,k) + (tau' - Csq(:,k) - Csq(:,j)) * qd(j); + end + end + C = C + Csq * diag(qd); diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/display.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/display.m new file mode 100644 index 0000000..f172d73 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/display.m @@ -0,0 +1,29 @@ +function display(r) +% called to display a robot object + +% 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.petercorke.com + + disp(' '); + disp([inputname(1), ' = ']) + disp(' '); + disp(char(r)) + disp(' '); diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/dyn.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/dyn.m new file mode 100644 index 0000000..8ecae2e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/dyn.m @@ -0,0 +1,35 @@ + +% 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.petercorke.com +function dyn(r) +%SerialLink.dyn Display inertial properties +% +% R.dyn() displays the inertial properties of the SerialLink object in a multi-line +% format. The properties shown are mass, centre of mass, inertia, gear ratio, +% motor inertia and motor friction. +% +% See also Link.dyn. + for j=1:r.n + fprintf('----- link %d\n', j); + l = r.links(j); + l.dyn() + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/fdyn.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/fdyn.m new file mode 100644 index 0000000..0b7b15b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/fdyn.m @@ -0,0 +1,116 @@ +%SerialLink.fdyn Integrate forward dynamics +% +% [T,Q,QD] = R.fdyn(T1, TORQFUN) integrates the dynamics of the robot over +% the time interval 0 to T and returns vectors of time TI, joint position Q +% and joint velocity QD. The initial joint position and velocity are zero. +% The torque applied to the joints is computed by the user function TORQFUN: +% +% [TI,Q,QD] = R.fdyn(T, TORQFUN, Q0, QD0) as above but allows the initial +% joint position and velocity to be specified. +% +% The control torque is computed by a user defined function +% +% TAU = TORQFUN(T, Q, QD, ARG1, ARG2, ...) +% +% where Q and QD are the manipulator joint coordinate and velocity state +% respectively], and T is the current time. +% +% [T,Q,QD] = R.fdyn(T1, TORQFUN, Q0, QD0, ARG1, ARG2, ...) allows optional +% arguments to be passed through to the user function. +% +% Note:: +% - This function performs poorly with non-linear joint friction, such as +% Coulomb friction. The R.nofriction() method can be used to set this +% friction to zero. +% - If TORQFUN is not specified, or is given as 0 or [], then zero torque +% is applied to the manipulator joints. +% - The builtin integration function ode45() is used. +% +% See also SerialLink.accel, SerialLink.nofriction, SerialLink.RNE, ode45. + +% 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.petercorke.com + +function [t, q, qd] = fdyn(robot, t1, torqfun, q0, qd0, varargin) + global __fdyn_robot; + global __fdyn_torqfun; + + n = robot.n; + if nargin == 2 + torqfun = 0; + q0 = zeros(1,n); + qd0 = zeros(1,n); + elseif nargin == 3 + q0 = zeros(1,n); + qd0 = zeros(1,n); + elseif nargin == 4 + qd0 = zeros(1,n); + end + + % concatenate q and qd into the initial state vector + x0= [q0(:); qd0(:)]; + + __fdyn_robot = robot; + __fdyn_torqfun = torqfun; + + dt = t1/100; + t = 0:dt:t1; + + y = lsode(@fdyn2, x0, t); + t = t' + q = y(:,1:n); + qd = y(:,n+1:2*n); + + +%FDYN2 private function called by FDYN +% +% XDD = FDYN2(T, X, FLAG, ROBOT, TORQUEFUN) +% +% Called by FDYN to evaluate the robot velocity and acceleration for +% forward dynamics. T is the current time, X = [Q QD] is the state vector, +% ROBOT is the object being integrated, and TORQUEFUN is the string name of +% the function to compute joint torques and called as +% +% TAU = TORQUEFUN(T, X) +% +% if not given zero joint torques are assumed. +% +% The result is XDD = [QD QDD]. + + +function xd = fdyn2(x, t) + global __fdyn_robot; + global __fdyn_torqfun; + robot = __fdyn_robot; + torqfun = __fdyn_torqfun; + n = robot.n; + q = x(1:n); + qd = x(n+1:2*n); + if isstr(torqfun) + tau = feval(torqfun, t, q, qd); + else + tau = zeros(n,1)'; + end + + qdd = accel(robot, x(1:n,1)', x(n+1:2*n,1)', tau); + xd = [x(n+1:2*n,1); qdd]; + + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/fkine.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/fkine.m new file mode 100644 index 0000000..a90cbc8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/fkine.m @@ -0,0 +1,70 @@ +%SerialLink.fkine Forward kinematics +% +% T = R.fkine(Q) is the pose of the robot end-effector as a homogeneous +% transformation for the joint configuration Q. For an N-axis manipulator +% Q is an N-vector. +% +% If Q is a matrix, the M rows are interpretted as the generalized +% joint coordinates for a sequence of points along a trajectory. Q(i,j) is +% the j'th joint parameter for the i'th trajectory point. In this case +% it returns a 4x4xM matrix where the last subscript is the index +% along the path. +% +% Note:: +% - The robot's base or tool transform, if present, are incorporated into the +% result. +% +% See also SerialLink.ikine, SerialLink.ikine6s. + +% 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.petercorke.com + +function t = fkine(robot, q) + % + % evaluate fkine for each point on a trajectory of + % theta_i or q_i data + % + + n = robot.n; + + L = robot.links; + if numel(q) == n + t = robot.base; + for i=1:n + t = t * L(i).A(q(i)); + end + t = t * robot.tool; + else + if numcols(q) ~= n + error('q must have %d columns', n) + end + t = zeros(4,4,0); + for qv=q', % for each trajectory point + tt = robot.base; + for i=1:n + tt = tt * L(i).A(qv(i)); + end + t = cat(3, t, tt * robot.tool); + end + end + + %robot.T = t; + %robot.notify('Moved'); diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/friction.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/friction.m new file mode 100644 index 0000000..e8ffd02 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/friction.m @@ -0,0 +1,39 @@ +%SerialLink.friction Friction force +% +% TAU = R.friction(QD) is the vector of joint friction forces/torques for the +% robot moving with joint velocities QD. +% +% The friction model includes viscous friction (linear with velocity) +% and Coulomb friction (proportional to sign(QD)). +% +% See also Link.friction. + +% 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.petercorke.com + +function tau = friction(robot, qd) + + L = robot.links; + + for i=1:robot.n, + tau(i) = friction(L(i), qd(i)); + end + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/gravload.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/gravload.m new file mode 100644 index 0000000..38741e8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/gravload.m @@ -0,0 +1,46 @@ +%SerialLink.gravload Gravity loading +% +% TAUG = R.gravload(Q) is the joint gravity loading for the robot in the +% joint configuration Q. Gravitational acceleration is a property of the +% robot object. +% +% If Q is a row vector, the result is a row vector of joint torques. If +% Q is a matrix, each row is interpreted as a joint configuration vector, +% and the result is a matrix each row being the corresponding joint torques. +% +% TAUG = R.gravload(Q, GRAV) is as above but the gravitational +% acceleration vector GRAV is given explicitly. +% +% See also SerialLink.rne, SerialLink.itorque, SerialLink.coriolis. + +% 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.petercorke.com + +function tg = gravload(robot, q, grav) + if numcols(q) ~= robot.n + error('Insufficient columns in q') + end + if nargin == 2 + tg = rne(robot, q, zeros(size(q)), zeros(size(q))); + elseif nargin == 3 + tg = rne(robot, q, zeros(size(q)), zeros(size(q)), grav); + end + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/ikine.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/ikine.m new file mode 100644 index 0000000..cc51baf --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/ikine.m @@ -0,0 +1,131 @@ +%SerialLink.IKINE Inverse manipulator kinematics +% +% Q = R.ikine(T) is the joint coordinates corresponding to the robot +% end-effector pose T which is a homogenenous transform. +% +% Q = R.ikine(T, Q0, OPTIONS) specifies the initial estimate of the joint +% coordinates. +% +% Q = R.ikine(T, Q0, M, OPTIONS) specifies the initial estimate of the joint +% coordinates and a mask matrix. For the case where the manipulator +% has fewer than 6 DOF the solution space has more dimensions than can +% be spanned by the manipulator joint coordinates. In this case +% the mask matrix M specifies the Cartesian DOF (in the wrist coordinate +% frame) that will be ignored in reaching a solution. The mask matrix +% has six elements that correspond to translation in X, Y and Z, and rotation +% about X, Y and Z respectively. The value should be 0 (for ignore) or 1. +% The number of non-zero elements should equal the number of manipulator DOF. +% +% For example when using a 5 DOF manipulator rotation about the wrist z-axis +% might be unimportant in which case M = [1 1 1 1 1 0]. +% +% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence +% and R.ikine() returns the joint coordinates corresponding to each of the +% transforms in the sequence. Q is MxN where N is the number of robot joints. +% The initial estimate of Q for each time step is taken as the solution +% from the previous time step. +% +% Options:: +% +% Notes:: +% - Solution is computed iteratively using the pseudo-inverse of the +% manipulator Jacobian. +% - The inverse kinematic solution is generally not unique, and +% depends on the initial guess Q0 (defaults to 0). +% - Such a solution is completely general, though much less efficient +% than specific inverse kinematic solutions derived symbolically. +% - This approach allows a solution to obtained at a singularity, but +% the joint angles within the null space are arbitrarily assigned. +% +% See also SerialLink.fkine, tr2delta, SerialLink.jacob0, SerialLink.ikine6s. + +% 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.petercorke.com + +function qt = ikine(robot, tr, varargin) + % set default parameters for solution + opt.ilimit = 100; + opt.tol = 1e-6; + opt.alpha = 1; + opt.plot = false; + opt.pinv = false; + + [opt,args] = tb_optparse(opt, varargin); + + n = robot.n; + + % robot.ikine(tr, q) + if length(args) > 0 + q = args{1}; + if isempty(q) + q = zeros(1, n); + else + q = q(:)'; + end + else + q = zeros(1, n); + end + + % robot.ikine(tr, q, m) + if length(args) > 1 + m = args{2}; + m = m(:); + if numel(m) ~= 6 + error('Mask matrix should have 6 elements'); + end + if numel(find(m)) ~= robot.n + error('Mask matrix must have same number of 1s as robot DOF') + end + else + if n < 6 + error('For a manipulator with fewer than 6DOF a mask matrix argument must be specified'); + end + m = ones(6, 1); + end + % make this a logical array so we can index with it + m = logical(m); + + npoints = size(tr,3); % number of points + qt = zeros(npoints, n); % preallocate space for results + tcount = 0; % total iteration count + eprev = Inf; + + save.e = Inf; + save.q = []; + + history = []; + for i=1:npoints + T = tr(:,:,i); + nm = Inf; + count = 0; + + optim = optimset('Display', 'iter', 'TolX', 0, 'TolFun', opt.tol, 'MaxIter', opt.ilimit); + optim + q = fminsearch(@(x) ikinefunc(x, T, robot, m), q, optim); + + end + qt = q; +end + +function E = ikinefunc(q, T, robot, m) + e = tr2delta(fkine(robot, q'), T); + E = norm(e(m)); +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/ikine6s.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/ikine6s.m new file mode 100644 index 0000000..e5da716 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/ikine6s.m @@ -0,0 +1,235 @@ +%SerialLink.ikine6s Inverse kinematics for 6-axis robot with spherical wrist +% +% Q = R.ikine6s(T) is the joint coordinates corresponding to the robot +% end-effector pose T represented by the homogenenous transform. This +% is a analytic solution for a 6-axis robot with a spherical wrist (such as +% the Puma 560). +% +% Q = R.IKINE6S(T, CONFIG) as above but specifies the configuration of the arm in +% the form of a string containing one or more of the configuration codes: +% +% 'l' arm to the left (default) +% 'r' arm to the right +% 'u' elbow up (default) +% 'd' elbow down +% 'n' wrist not flipped (default) +% 'f' wrist flipped (rotated by 180 deg) +% +% Notes:: +% - The inverse kinematic solution is generally not unique, and +% depends on the configuration string. +% +% Reference:: +% +% Inverse kinematics for a PUMA 560 based on the equations by Paul and Zhang +% From The International Journal of Robotics Research +% Vol. 5, No. 2, Summer 1986, p. 32-44 +% +% +% Author:: +% Robert Biro with Gary Von McMurray, +% GTRI/ATRP/IIMB, +% Georgia Institute of Technology +% 2/13/95 +% +% See also SerialLink.FKINE, SerialLink.IKINE. + +function theta = ikine6s(robot, T, varargin) + + if robot.n ~= 6, + error('Solution only applicable for 6DOF manipulator'); + end + + if robot.mdh ~= 0, + error('Solution only applicable for standard DH conventions'); + end + + if ndims(T) == 3, + theta = []; + for k=1:size(T,3), + th = ikine6s(robot, T(:,:,k), varargin{:}); + theta = [theta; th]; + end + + return; + end + L = robot.links; + a1 = L(1).a; + a2 = L(2).a; + a3 = L(3).a; + + if ~isspherical(robot) + error('wrist is not spherical') + end + + d1 = L(1).d; + d2 = L(2).d; + d3 = L(3).d; + d4 = L(4).d; + + if ~ishomog(T), + error('T is not a homog xform'); + end + + % undo base transformation + T = inv(robot.base) * T; + + % The following parameters are extracted from the Homogeneous + % Transformation as defined in equation 1, p. 34 + + Ox = T(1,2); + Oy = T(2,2); + Oz = T(3,2); + + Ax = T(1,3); + Ay = T(2,3); + Az = T(3,3); + + Px = T(1,4); + Py = T(2,4); + Pz = T(3,4); + + % The configuration parameter determines what n1,n2,n4 values are used + % and how many solutions are determined which have values of -1 or +1. + + if nargin < 3, + configuration = ''; + else + configuration = lower(varargin{1}); + end + + % default configuration + + n1 = -1; % L + n2 = -1; % U + n4 = -1; % N + if ~isempty(findstr(configuration, 'l')), + n1 = -1; + end + if ~isempty(findstr(configuration, 'r')), + n1 = 1; + end + if ~isempty(findstr(configuration, 'u')), + if n1 == 1, + n2 = 1; + else + n2 = -1; + end + end + if ~isempty(findstr(configuration, 'd')), + if n1 == 1, + n2 = -1; + else + n2 = 1; + end + end + if ~isempty(findstr(configuration, 'n')), + n4 = 1; + end + if ~isempty(findstr(configuration, 'f')), + n4 = -1; + end + + + % + % Solve for theta(1) + % + % r is defined in equation 38, p. 39. + % theta(1) uses equations 40 and 41, p.39, + % based on the configuration parameter n1 + % + + r=sqrt(Px^2 + Py^2); + if (n1 == 1), + theta(1)= atan2(Py,Px) + asin(d3/r); + else + theta(1)= atan2(Py,Px) + pi - asin(d3/r); + end + + % + % Solve for theta(2) + % + % V114 is defined in equation 43, p.39. + % r is defined in equation 47, p.39. + % Psi is defined in equation 49, p.40. + % theta(2) uses equations 50 and 51, p.40, based on the configuration + % parameter n2 + % + + V114= Px*cos(theta(1)) + Py*sin(theta(1)); + r=sqrt(V114^2 + Pz^2); + Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r)); + if ~isreal(Psi), + warning('point not reachable'); + theta = [NaN NaN NaN NaN NaN NaN]; + return + end + theta(2) = atan2(Pz,V114) + n2*Psi; + + % + % Solve for theta(3) + % + % theta(3) uses equation 57, p. 40. + % + + num = cos(theta(2))*V114+sin(theta(2))*Pz-a2; + den = cos(theta(2))*Pz - sin(theta(2))*V114; + theta(3) = atan2(a3,d4) - atan2(num, den); + + % + % Solve for theta(4) + % + % V113 is defined in equation 62, p. 41. + % V323 is defined in equation 62, p. 41. + % V313 is defined in equation 62, p. 41. + % theta(4) uses equation 61, p.40, based on the configuration + % parameter n4 + % + + V113 = cos(theta(1))*Ax + sin(theta(1))*Ay; + V323 = cos(theta(1))*Ay - sin(theta(1))*Ax; + V313 = cos(theta(2)+theta(3))*V113 + sin(theta(2)+theta(3))*Az; + theta(4) = atan2((n4*V323),(n4*V313)); + %[(n4*V323),(n4*V313)] + + % + % Solve for theta(5) + % + % num is defined in equation 65, p. 41. + % den is defined in equation 65, p. 41. + % theta(5) uses equation 66, p. 41. + % + + num = -cos(theta(4))*V313 - V323*sin(theta(4)); + den = -V113*sin(theta(2)+theta(3)) + Az*cos(theta(2)+theta(3)); + theta(5) = atan2(num,den); + %[num den] + + % + % Solve for theta(6) + % + % V112 is defined in equation 69, p. 41. + % V122 is defined in equation 69, p. 41. + % V312 is defined in equation 69, p. 41. + % V332 is defined in equation 69, p. 41. + % V412 is defined in equation 69, p. 41. + % V432 is defined in equation 69, p. 41. + % num is defined in equation 68, p. 41. + % den is defined in equation 68, p. 41. + % theta(6) uses equation 70, p. 41. + % + + V112 = cos(theta(1))*Ox + sin(theta(1))*Oy; + V132 = sin(theta(1))*Ox - cos(theta(1))*Oy; + V312 = V112*cos(theta(2)+theta(3)) + Oz*sin(theta(2)+theta(3)); + V332 = -V112*sin(theta(2)+theta(3)) + Oz*cos(theta(2)+theta(3)); + V412 = V312*cos(theta(4)) - V132*sin(theta(4)); + V432 = V312*sin(theta(4)) + V132*cos(theta(4)); + num = -V412*cos(theta(5)) - V332*sin(theta(5)); + den = - V432; + theta(6) = atan2(num,den); + + % remove the link offset angles + for i=1:6 + theta(i) = theta(i) - L(i).offset; + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/inertia.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/inertia.m new file mode 100644 index 0000000..cc64a4f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/inertia.m @@ -0,0 +1,59 @@ +%SerialLink.INERTIA Manipulator inertia matrix +% +% I = R.inertia(Q) is the NxN symmetric joint inertia matrix which relates +% joint torque to joint acceleration for the robot at joint configuration Q. +% The diagonal elements I(j,j) are the inertia seen by joint actuator j. +% The off-diagonal elements are coupling inertias that relate acceleration +% on joint i to force/torque on joint j. +% +% If Q is a matrix (DxN), each row is interpretted as a joint state +% vector, and the result (NxNxD) is a 3d-matrix where each plane corresponds +% to the inertia for the corresponding row of Q. +% +% See also SerialLink.RNE, SerialLink.CINERTIA, SerialLink.ITORQUE. + +% 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.petercorke.com + +function M = inertia(robot, q) + if numcols(q) ~= robot.n + error('q must have %d columns', robot.n); + end + + if numrows(q) > 1 + M = []; + for i=1:numrows(q) + M = cat(3, M, robot.inertia(q(i,:))); + end + return + end + + n = robot.n; + + if numel(q) == robot.n, + q = q(:)'; + end + + M = zeros(n,n,0); + for Q = q', + m = rne(robot, ones(n,1)*Q', zeros(n,n), eye(n), [0;0;0]); + M = cat(3, M, m); + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/islimit.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/islimit.m new file mode 100644 index 0000000..ec65dfb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/islimit.m @@ -0,0 +1,35 @@ + +% 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.petercorke.com +function v = islimit(r,q) +%SerialLink.islimit Joint limit test +% +% V = R.ISLIMIT(Q) is a vector of boolean values, one per joint, +% false (0) if Q(i) is within the joint limits, else true (1). + L = r.links; + if length(q) ~= r.n + error('argument for islimit method is wrong length'); + end + v = []; + for i=1:r.n + v = [v; r.links(i).islimit(q(i))]; + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/isspherical.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/isspherical.m new file mode 100644 index 0000000..5438fbc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/isspherical.m @@ -0,0 +1,41 @@ + +% 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.petercorke.com +function v = isspherical(r) +%SerialLink.isspherical Test for spherical wrist +% +% R.isspherical() is true if the robot has a spherical wrist, that is, the +% last 3 axes intersect at a point. +% +% See also SerialLink.ikine6s. + L = r.links(end-2:end); + + v = false; + if ~isempty( find( [L(1).a L(2).a L(3).a L(2).d L(3).d] ~= 0 )) + return + end + + if (abs(L(1).alpha) == pi/2) & (abs(L(1).alpha + L(2).alpha) < eps) + v = true; + return; + end +end + diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/itorque.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/itorque.m new file mode 100644 index 0000000..4e99eef --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/itorque.m @@ -0,0 +1,39 @@ +%SerialLink.itorque Inertia torque +% +% TAUI = R.itorque(Q, QDD) is the inertia force/torque N-vector at the specified +% joint configuration Q and acceleration QDD, that is, TAUI = INERTIA(Q)*QDD. +% +% If Q and QDD are row vectors, the result is a row vector of joint torques. +% If Q and QDD are matrices, each row is interpretted as a joint state +% vector, and the result is a matrix each row being the corresponding joint +% torques. +% +% Note:: +% - If the robot model contains non-zero motor inertia then this will +% included in the result. +% +% See also SerialLink.rne, SerialLink.inertia. + +% 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.petercorke.com + +function it = itorque(robot, q, qdd) + it = rne(robot, q, zeros(size(q)), qdd, [0;0;0]); diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacob0.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacob0.m new file mode 100644 index 0000000..808c5f3 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacob0.m @@ -0,0 +1,85 @@ +%SerialLink.JACOB0 Jacobian in world coordinates +% +% J0 = R.jacob0(Q, OPTIONS) is a 6xN Jacobian matrix for the robot in pose Q. +% The manipulator Jacobian matrix maps joint velocity to end-effector spatial +% velocity V = J0*QD expressed in the world-coordinate frame. +% +% Options:: +% 'rpy' Compute analytical Jacobian with rotation rate in terms of +% roll-pitch-yaw angles +% 'eul' Compute analytical Jacobian with rotation rates in terms of +% Euler angles +% 'trans' Return translational submatrix of Jacobian +% 'rot' Return rotational submatrix of Jacobian +% +% Note:: +% - the Jacobian is computed in the world frame and transformed to the +% end-effector frame. +% - the default Jacobian returned is often referred to as the geometric +% Jacobian, as opposed to the analytical Jacobian. +% +% See also SerialLink.jacobn, deltatr, tr2delta. + +% 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.petercorke.com + +function J0 = jacob0(robot, q, varargin) + + opt.rpy = false; + opt.eul = false; + opt.trans = false; + opt.rot = false; + + opt = tb_optparse(opt, varargin); + + % + % dX_tn = Jn dq + % + Jn = jacobn(robot, q); % Jacobian from joint to wrist space + + % + % convert to Jacobian in base coordinates + % + Tn = fkine(robot, q); % end-effector transformation + R = t2r(Tn); + J0 = [R zeros(3,3); zeros(3,3) R] * Jn; + + if opt.rpy + rpy = tr2rpy( fkine(robot, q) ); + B = rpy2jac(rpy); + if rcond(B) < eps, + error('Representational singularity'); + end + J0 = blkdiag( eye(3,3), inv(B) ) * J0; + elseif opt.eul + eul = tr2eul( fkine(robot, q) ); + B = eul2jac(eul); + if rcond(B) < eps, + error('Representational singularity'); + end + J0 = blkdiag( eye(3,3), inv(B) ) * J0; + end + + if opt.trans + J0 = J0(1:3,:); + elseif opt.rot + J0 = J0(4:6,:); + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacob_dot.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacob_dot.m new file mode 100644 index 0000000..c6292d2 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacob_dot.m @@ -0,0 +1,89 @@ +%SerialLink.jacob_dot Hessian in end-effector frame +% +% JDQ = R.jacob_dot(Q, QD) is the product of the Hessian, derivative of the +% Jacobian, and the joint rates. +% +% Notes:: +% - useful for operational space control +% - not yet tested/debugged. +% +% See also: SerialLink.jacob0, diff2tr, tr2diff. + +% 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.petercorke.com + +function Jdot = jacob_dot(robot, q, qd) + + n = robot.n; + L = robot.links; % get the links + + Tj = robot.base; % this is cumulative transform from base + w = [0;0;0]; + v = [0;0;0]; + for j=1:n, + link = robot.links(j); + Aj = link.A(q(j)); + Tj = Tj * Aj; + R{j} = t2r(Aj); + T{j} = t2r(Tj); + p(:,j) = transl(Tj); % origin of link j + + if j>1, + z(:,j) = T{j-1} * [0 0 1]'; % in world frame + w = R{j}*( w + z(:,j) * qd(j)); + v = v + cross(R{j}*w, R{j-1}*p(:,j)); + %v = R{j-1}'*v + cross(w, p(:,j)); + %w = R{j-1}'* w + z(:,j) * qd(j); + else + z(:,j) = [0 0 1]'; + v = [0 0 0]'; + w = z(:,j) * qd(j); + end + + vel(:,j) = v; % velocity of origin of link j + + omega(:,j) = w; % omega of link j in link j frame + end + omega + z + + J = []; + Jdot = []; + for j=1:n, + if j>1, + t = p(:,n) - p(:,j-1); + td = vel(:,n) - vel(:,j-1); + else + t = p(:,n); + td = vel(:,n); + end + if L(j).RP == 'R', + J_col = [cross(z(:,j), t); z(:,j)]; + Jdot_col = [cross(cross(omega(:,j), z(:,j)), t) + cross(z(:,j), td) ; cross(omega(:,j), z(:,j))]; + else + J_col = [z(:,j); 0;0;0]; + Jdot_col = zeros(6,1); + end + + J = [J J_col]; + Jdot = [Jdot Jdot_col]; + + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacobn.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacobn.m new file mode 100644 index 0000000..c484339 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jacobn.m @@ -0,0 +1,83 @@ +%SerialLink.JACOBN Jacobian in end-effector frame +% +% JN = R.jacobn(Q, options) is a 6xN Jacobian matrix for the robot in +% pose Q. The manipulator Jacobian matrix maps joint velocity to +% end-effector spatial velocity V = J0*QD in the end-effector frame. +% +% Options:: +% 'trans' Return translational submatrix of Jacobian +% 'rot' Return rotational submatrix of Jacobian +% +% Notes:: +% - this Jacobian is often referred to as the geometric Jacobian +% +% Reference:: +% Paul, Shimano, Mayer, +% Differential Kinematic Control Equations for Simple Manipulators, +% IEEE SMC 11(6) 1981, +% pp. 456-460 +% +% See also SerialLink.jacob0, delta2tr, tr2delta. + +% 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.petercorke.com + +function J = jacobn(robot, q, varargin) + + opt.trans = false; + opt.rot = false; + + opt = tb_optparse(opt, varargin); + + n = robot.n; + L = robot.links; % get the links + + J = []; + U = robot.tool; + for j=n:-1:1, + if robot.mdh == 0, + % standard DH convention + U = L(j).A(q(j)) * U; + end + if L(j).RP == 'R', + % revolute axis + d = [ -U(1,1)*U(2,4)+U(2,1)*U(1,4) + -U(1,2)*U(2,4)+U(2,2)*U(1,4) + -U(1,3)*U(2,4)+U(2,3)*U(1,4)]; + delta = U(3,1:3)'; % nz oz az + else + % prismatic axis + d = U(3,1:3)'; % nz oz az + delta = zeros(3,1); % 0 0 0 + end + J = [[d; delta] J]; + + if robot.mdh ~= 0, + % modified DH convention + U = L(j).A(q(j)) * U; + end + end + + if opt.trans + J0 = J0(1:3,:); + elseif opt.rot + J0 = J0(4:6,:); + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jtraj.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jtraj.m new file mode 100644 index 0000000..7d83d04 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/jtraj.m @@ -0,0 +1,46 @@ + +% 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.petercorke.com +function jt = jtraj(r, T1, T2, t, varargin) +%SerialLink.jtraj Create joint space trajectory +% +% Q = R.jtraj(T0, TF, M) is a joint space trajectory where the joint +% coordinates reflect motion from end-effector pose T0 to TF in M steps with +% default zero boundary conditions for velocity and acceleration. +% The trajectory Q is an MxN matrix, with one row per time step, and +% one column per joint, where N is the number of robot joints. +% +% Note:: +% - requires solution of inverse kinematics. R.ikine6s() is used if +% appropriate, else R.ikine(). Additional trailing arguments to R.jtraj() +% are passed as trailing arugments to the these functions. +% +% See also jtraj, SerialLink.ikine, SerialLink.ikine6s. + if isspherical(r) && (r.n == 6) + q1 = ikine6s(r, T1, varargin{:}); + q2 = ikine6s(r, T2, varargin{:}); + else + q1 = ikine(r, T1, varargin{:}); + q2 = ikine(r, T2, varargin{:}); + end + + jt = jtraj(q1, q2, t); +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/maniplty.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/maniplty.m new file mode 100644 index 0000000..0a165c8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/maniplty.m @@ -0,0 +1,122 @@ +%SerialLink.MANIPLTY Manipulability measure +% +% M = R.maniplty(Q, OPTIONS) is the manipulability index measure for the robot +% at the joint configuration Q. It indicates dexterity, how isotropic the +% robot's motion is with respect to the 6 degrees of Cartesian motion. +% The measure is low when the manipulator is close to a singularity. +% If Q is a matrix M is a column vector of manipulability +% indices for each pose specified by a row of Q. +% +% Two measures can be selected: +% - Yoshikawa's manipulability measure is based on the shape of the velocity +% ellipsoid and depends only on kinematic parameters. +% - Asada's manipulability measure is based on the shape of the acceleration +% ellipsoid which in turn is a function of the Cartesian inertia matrix and +% the dynamic parameters. The scalar measure computed here is the ratio of +% the smallest/largest ellipsoid axis. Ideally the ellipsoid would be +% spherical, giving a ratio of 1, but in practice will be less than 1. +% +% Options:: +% 'T' compute manipulability for just transational motion +% 'R' compute manipulability for just rotational motion +% 'yoshikawa' use Asada algorithm (default) +% 'asada' use Asada algorithm +% +% Notes:: +% - by default the measure includes rotational and translational dexterity, but +% this involves adding different units. It can be more useful to look at the +% translational and rotational manipulability separately. +% +% See also SerialLink.inertia, SerialLink.jacob0. + +% 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.petercorke.com + +function [w,mx] = maniplty(robot, q, varargin) + n = robot.n; + + opt.yoshikawa = true; + opt.asada = false; + opt.axes = {'all', 'T', 'R'}; + + opt = tb_optparse(opt, varargin); + + if length(q) == robot.n, + q = q(:)'; + end + + w = []; + MX = []; + + if opt.yoshikawa + for Q = q', + w = [w; yoshi(robot, Q, opt)]; + end + elseif opt.asada + for Q = q', + if nargout > 1 + [ww,mm] = asada(robot, Q); + w = [w; ww]; + MX = cat(3, MX, mm); + else + w = [w; asada(robot, Q, opt)]; + end + end + end + + if nargout > 1 + mx = MX; + end + +function m = yoshi(robot, q, opt) + J = jacob0(robot, q); + switch opt.axes + case 'T' + J = J(1:3,:); + case 'R' + J = J(4:6,:); + end + m = sqrt(det(J * J')); + +function [m, mx] = asada(robot, q, opt) + J = jacob0(robot, q); + + if rank(J) < 6, + warning('robot is in degenerate configuration') + m = 0; + return; + end + + switch opt.axes + case 'T' + J = J(1:3,:) + case 'R' + J = J(4:6,:); + end + Ji = inv(J); + M = inertia(robot, q); + Mx = Ji' * M * Ji; + e = eig(Mx(1:3,1:3)); + m = min(e) / max(e); + + if nargout > 1 + mx = Mx; + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/mtimes.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/mtimes.m new file mode 100644 index 0000000..9791116 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/mtimes.m @@ -0,0 +1,56 @@ +% robot objects can be multiplied r1*r2 which is mechanically equivalent +% to mounting robot r2 on the end of robot r1. +% + +% 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.petercorke.com +function r2 = mtimes(r, l) + %SerialLink.mtimes Join robots + % + % R = R1 * R2 is a robot object that is equivalent to mounting robot R2 + % on the end of robot R1. + if isa(l, 'SerialLink') + r2 = SerialLink(r); + x = r2.links; + for i = 1:length(x) + new_links(i) = Link(x(i)); + end + y = l.links; + for i = 1:length(y) + new_links(i + length(x)) = Link(y(i)); + end + r2.links = new_links; + r2.base = r.base; + r2.n = length(r2.links); + elseif isa(l, 'Link') + r2 = SerialLink(r); + x = r2.links; + for i = 1:length(x) + new_links(i) = Link(x(i)); + end + for i = 1:length(l) + new_links(i + length(x)) = Link(l(i)); + end + + r2.links = new_links; + r2.n = length(r2.links); + end + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/nofriction.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/nofriction.m new file mode 100644 index 0000000..5d12e71 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/nofriction.m @@ -0,0 +1,44 @@ +%SerialLink.nofriction Remove friction +% +% RNF = R.nofriction() is a robot object with the same parameters as R but +% with non-linear (Couolmb) friction coefficients set to zero. +% +% RNF = R.nofriction('all') as above but all friction coefficients set to zero. +% +% Notes: +% - Non-linear (Coulomb) friction can cause numerical problems when integrating +% the equations of motion (R.fdyn). +% - The resulting robot object has its name string modified by prepending 'NF/'. +% +% See also SerialLink.fdyn, Link.nofriction. + +% 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.petercorke.com + +function r2 = nofriction(r, varargin) + + r2 = SerialLink(r); % make a copy + + for j=1:r2.n, + r2.links(j) = r.links(j).nofriction(varargin{:}); + end + + r2.name = ['NF/' r.name]; diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/perturb.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/perturb.m new file mode 100644 index 0000000..80025ac --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/perturb.m @@ -0,0 +1,50 @@ +%SerialLink.perturb Perturb robot parameters +% +% RP = R.perturb(P) is a new robot object in which the dynamic parameters (link +% mass and inertia) have been perturbed. The perturbation is multiplicative so +% that values are multiplied by random numbers in the interval (1-P) to (1+P). +% The name string of the perturbed robot is prefixed by 'P/'. +% +% Useful for investigating the robustness of various model-based control +% schemes. For example to vary parameters in the range +/- 10 percent is: +% r2 = p560.perturb(0.1); + +% 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.petercorke.com + +function r2 = perturb(r, p) + + if nargin == 1, + p = 0.1; % 10 percent disturb by default + end + + + for i=1:r.n, + l2(i) = r.links(i); + s = (2*rand-1)*p + 1; + l2(i).m = l2(i).m * s; + s = (2*rand-1)*p + 1; + l2(i).I = l2(i).I * s; + end + % clone the robot + r2 = SerialLink(r); + r2.links = l2; + r2.name = ['P/' r.name]; diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/plot.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/plot.m new file mode 100644 index 0000000..7872f75 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/plot.m @@ -0,0 +1,478 @@ +%PLOT Graphical robot animation +% +% PLOT(ROBOT, Q) +% PLOT(ROBOT, Q, options) +% +% Displays a graphical animation of a robot based on the +% kinematic model. A stick figure polyline joins the origins of +% the link coordinate frames. +% The robot is displayed at the joint angle Q, or if a matrix it is +% animated as the robot moves along the trajectory. +% +% The graphical robot object holds a copy of the robot object and +% the graphical element is tagged with the robot's name (.name method). +% This state also holds the last joint configuration +% drawn (.q method). +% +% If no robot of this name is currently displayed then a robot will +% be drawn in the current figure. If the robot already exists then +% that graphical model will be found and moved. +% +% MULTIPLE VIEWS OF THE SAME ROBOT +% If one or more plots of this robot already exist then these will all +% be moved according to the argument Q. +% +% MULTIPLE ROBOTS +% Multiple robots can be displayed in the same plot, by using "hold on" +% before calls to plot(robot). +% +% options is a list of any of the following: +% 'workspace' [xmin, xmax ymin ymax zmin zmax] +% 'perspective' 'ortho' controls camera view mode +% 'erase' 'noerase' controls erasure of arm during animation +% 'base' 'nobase' controls display of base 'pedestal' +% 'loop' 'noloop' controls display of base 'pedestal' +% 'wrist' 'nowrist' controls display of wrist +% 'name', 'noname' display the robot's name near the first joint +% 'shadow' 'noshadow' controls display of shadow +% 'xyz' 'noa' wrist axis label +% 'joints' 'nojoints' controls display of joints +% 'mag' scale annotation scale factor +% +% The options come from 3 sources and are processed in the order: +% 1. Cell array of options returned by the function PLOTBOTOPT +% 2. Cell array of options returned by the .plotopt method of the +% robot object. These are set by the .plotopt method. +% 3. List of arguments in the command line. +% +% GRAPHICAL ANNOTATIONS: +% +% The basic stick figure robot can be annotated with +% shadow on the floor +% XYZ wrist axes and labels +% joint cylinders and axes +% +% All of these require some kind of dimension and this is determined +% using a simple heuristic from the workspace dimensions. This dimension +% can be changed by setting the multiplicative scale factor using the +% 'mag' option above. +% +% GETTING GRAPHICAL ROBOT STATE: +% q = PLOT(ROBOT) +% Returns the joint configuration from the state of an existing +% graphical representation of the specified robot. If multiple +% instances exist, that of the first one returned by findobj() is +% given. + +% +% See also PLOTBOTOPT, DRIVEBOT, FKINE, ROBOT. + +% Copright (C) Peter Corke 1993 + +% HANDLES: +% +% A robot comprises a bunch of individual graphical elements and these are +% kept in a structure which can be stored within the .handle element of a +% robot object. +% h.robot the robot stick figure +% h.shadow the robot's shadow +% h.x wrist vectors +% h.y +% h.z +% h.xt wrist vector labels +% h.yt +% h.zt +% +% The plot function returns a new robot object with the handle element set. +% +% For the h.robot object we additionally: +% save this new robot object as its UserData +% tag it with the name field from the robot object +% +% This enables us to find all robots with a given name, in all figures, +% and update them. + +% MOD.HISTORY +% 12/94 make axis scaling adjust to robot kinematic params +% 4/99 use objects +% 2/01 major rewrite, axis names, drivebot, state etc. + + +% 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.petercorke.com +function rnew = plot(robot, tg, varargin) + + % + % q = PLOT(robot) + % return joint coordinates from a graphical robot of given name + % + if nargin == 1, + rh = findobj('Tag', robot.name); + if ~isempty(rh), + r = get(rh(1), 'UserData'); + rnew = r.q; + end + return + end + + np = numrows(tg); + n = robot.n; + + if numcols(tg) ~= n, + error('Insufficient columns in q') + end + + if ~isfield(robot, 'handles'), + % + % if there are no handles in this object we assume it has + % been invoked from the command line not drivebot() so we + % process the options + % + + %%%%%%%%%%%%%% process options + erasemode = 'xor'; + joints = 1; + wrist = 1; + repeat = 1; + shadow = 1; + wrist = 1; + dims = []; + base = 0; + wristlabel = 'xyz'; + projection = 'orthographic'; + magscale = 1; + name = 1; + + % read options string in the order + % 1. robot.plotopt + % 2. the M-file plotbotopt if it exists + % 3. command line arguments + if exist('plotbotopt', 'file') == 2, + options = plotbotopt; + options = [options robot.plotopt varargin]; + else + options = [robot.plotopt varargin]; + end + i = 1; + while i <= length(options), + switch lower(options{i}), + case 'workspace' + dims = options{i+1}; + i = i+1; + case 'mag' + magscale = options{i+1}; + i = i+1; + case 'perspective' + projection = 'perspective'; + case 'ortho' + projection = 'orthographic'; + case 'erase' + erasemode = 'xor'; + case 'noerase' + erasemode = 'none'; + case 'base' + base = 1; + case 'nobase' + base = 0; + case 'loop' + repeat = Inf; + case 'noloop' + repeat = 1; + case 'name', + name = 1; + case 'noname', + name = 0; + case 'wrist' + wrist = 1; + case 'nowrist' + wrist = 0; + case 'shadow' + shadow = 1; + case 'noshadow' + shadow = 0; + case 'xyz' + wristlabel = 'XYZ'; + case 'noa' + wristlabel = 'NOA'; + case 'joints' + joints = 1; + case 'nojoints' + joints = 0; + otherwise + error(['unknown option: ' options{i}]); + end + i = i+1; + end + + if isempty(dims), + % + % simple heuristic to figure the maximum reach of the robot + % + L = robot.links; + reach = 0; + for i=1:n, + reach = reach + abs(L(i).a) + abs(L(i).d); + end + dims = [-reach reach -reach reach -reach reach]; + mag = reach/10; + else + reach = min(abs(dims)); + end + mag = magscale * reach/10; + end + + % + % setup an axis in which to animate the robot + % + if isempty(robot.handle) & isempty(findobj(gcf, 'Tag', robot.name)), + if ~ishold, + clf + axis(dims); + end + figure(gcf); % bring to the top + xlabel('X') + ylabel('Y') + zlabel('Z') + set(gca, 'drawmode', 'fast'); + grid on + + zlim = get(gca, 'ZLim'); + h.zmin = zlim(1); + + if base, + b = transl(robot.base); + line('xdata', [b(1);b(1)], ... + 'ydata', [b(2);b(2)], ... + 'zdata', [h.zmin;b(3)], ... + 'LineWidth', 4, ... + 'color', 'red'); + end + if name, + b = transl(robot.base); + text(b(1), b(2)-mag, [' ' robot.name]) + end + % create a line which we will + % subsequently modify. Set erase mode to xor for fast + % update + % + h.robot = line(robot.lineopt{:}, ... + 'Erasemode', erasemode); + if shadow == 1, + h.shadow = line(robot.shadowopt{:}, ... + 'Erasemode', erasemode); + end + + if wrist == 1, + h.x = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'red', ... + 'erasemode', 'xor'); + h.y = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'green', ... + 'erasemode', 'xor'); + h.z = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'blue', ... + 'erasemode', 'xor'); + h.xt = text(0, 0, wristlabel(1), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + h.yt = text(0, 0, wristlabel(2), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + h.zt = text(0, 0, wristlabel(3), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + end + + % + % display cylinders (revolute) or boxes (pristmatic) at + % each joint, as well as axis centerline. + % + if joints == 1, + L = robot.links; + for i=1:robot.n, + + % cylinder or box to represent the joint + if L(i).sigma == 0, + N = 8; + else + N = 4; + end + + [xc,yc,zc] = cylinder([mag/4, mag/4], N); + zc(zc==0) = -mag/2; + zc(zc==1) = mag/2; + + % add the surface object and color it + h.joint(i) = surface(xc,yc,zc); + %set(h.joint(i), 'erasemode', 'xor'); + set(h.joint(i), 'FaceColor', 'blue'); + + % build a matrix of coordinates so we + % can transform the cylinder in animate() + % and hang it off the cylinder + xyz = [xc(:)'; yc(:)'; zc(:)'; ones(1,2*N+2)]; + set(h.joint(i), 'UserData', xyz); + + % add a dashed line along the axis + h.jointaxis(i) = line('xdata', [0;0], ... + 'ydata', [0;0], ... + 'zdata', [0;0], ... + 'color', 'blue', ... + 'linestyle', '--', ... + 'erasemode', 'xor'); + + end + end + h.mag = mag; + robot.handle = h; + set(h.robot, 'Tag', robot.name); + set(h.robot, 'UserData', robot); + end + + + % save the handles in the passed robot object + + rh = findobj('Tag', robot.name); + for r=1:repeat, + for p=1:np, + for r=rh', + animate( get(r, 'UserData'), tg(p,:)); + end + end + end + + % save the last joint angles away in the graphical robot + for r=rh', + rr = get(r, 'UserData'); + rr.q = tg(end,:); + set(r, 'UserData', rr); + end + + if nargout > 0, + rnew = robot; + end + +function animate(robot, q) + + n = robot.n; + h = robot.handle; + L = robot.links; + + mag = h.mag; + + b = transl(robot.base); + x = b(1); + y = b(2); + z = b(3); + + xs = b(1); + ys = b(2); + zs = h.zmin; + + % compute the link transforms, and record the origin of each frame + % for the animation. + t = robot.base; + Tn = t; + for j=1:n, + Tn(:,:,j) = t; + + + t = t * L(j).A(q(j)); + + x = [x; t(1,4)]; + y = [y; t(2,4)]; + z = [z; t(3,4)]; + xs = [xs; t(1,4)]; + ys = [ys; t(2,4)]; + zs = [zs; h.zmin]; + end + t = t *robot.tool; + + % + % draw the robot stick figure and the shadow + % + set(h.robot,'xdata', x, 'ydata', y, 'zdata', z); + if isfield(h, 'shadow'), + set(h.shadow,'xdata', xs, 'ydata', ys, 'zdata', zs); + end + + + % + % display the joints as cylinders with rotation axes + % + if isfield(h, 'joint'), + xyz_line = [0 0; 0 0; -2*mag 2*mag; 1 1]; + + for j=1:n, + % get coordinate data from the cylinder + xyz = get(h.joint(j), 'UserData'); + xyz = Tn(:,:,j) * xyz; + ncols = numcols(xyz)/2; + xc = reshape(xyz(1,:), 2, ncols); + yc = reshape(xyz(2,:), 2, ncols); + zc = reshape(xyz(3,:), 2, ncols); + + set(h.joint(j), 'Xdata', xc, 'Ydata', yc, ... + 'Zdata', zc); + + xyzl = Tn(:,:,j) * xyz_line; + set(h.jointaxis(j), 'Xdata', xyzl(1,:), 'Ydata', xyzl(2,:), 'Zdata', xyzl(3,:)); + xyzl = Tn(:,:,j) * xyz_line; + h.jointlabel(j) = text(xyzl(1,1), xyzl(2,1) , xyzl(3,1), num2str(j), 'HorizontalAlignment', 'Center'); + end + end + + % + % display the wrist axes and labels + % + if isfield(h, 'x'), + % + % compute the wrist axes, based on final link transformation + % plus the tool transformation. + % + xv = t*[mag;0;0;1]; + yv = t*[0;mag;0;1]; + zv = t*[0;0;mag;1]; + + % + % update the line segments, wrist axis and links + % + set(h.x,'xdata',[t(1,4) xv(1)], 'ydata', [t(2,4) xv(2)], ... + 'zdata', [t(3,4) xv(3)]); + set(h.y,'xdata',[t(1,4) yv(1)], 'ydata', [t(2,4) yv(2)], ... + 'zdata', [t(3,4) yv(3)]); + set(h.z,'xdata',[t(1,4) zv(1)], 'ydata', [t(2,4) zv(2)], ... + 'zdata', [t(3,4) zv(3)]); + + wristlabel = "XYZ"; + % text(xv(1),xv(2),xv(3), ['X']) + % text(yv(1),yv(2),yv(3), ['Y']) + % text(zv(1),zv(2),zv(3), ['Z']) + h.xt = text(xv(1),xv(2),xv(3), wristlabel(1), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + h.yt = text(yv(1),yv(2),yv(3), wristlabel(2), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + h.zt = text(zv(1),zv(2),zv(3), wristlabel(3), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center'); + + % set(h.zt, 'rotation', zv(1:3)); + % set(h.xt, 'Position', xv(1:3)); + % set(h.yt, 'Position', yv(1:3)); + end + + drawnow diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne.m new file mode 100644 index 0000000..6f2afcc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne.m @@ -0,0 +1,68 @@ +%SerialLink.rne Inverse dynamics +% +% TAU = R.rne(Q, QD, QDD) is the joint torque required for the robot R +% to achieve the specified joint position Q, velocity QD and acceleration QDD. +% +% TAU = R.rne(Q, QD, QDD, GRAV) as above but overriding the gravitational +% acceleration vector in the robot object R. +% +% TAU = R.rne(Q, QD, QDD, GRAV, FEXT) as above but specifying a wrench +% acting on the end of the manipulator which is a 6-vector [Fx Fy Fz Mx My Mz]. +% +% TAU = R.rne(X) as above where X=[Q,QD,QDD]. +% +% TAU = R.rne(X, GRAV) as above but overriding the gravitational +% acceleration vector in the robot object R. +% +% TAU = R.rne(X, GRAV, FEXT) as above but specifying a wrench +% acting on the end of the manipulator which is a 6-vector [Fx Fy Fz Mx My Mz]. +% +% If Q,QD and QDD, or X are matrices with M rows representing a trajectory then +% TAU is an MxN matrix with rows corresponding to each trajectory state. +% +% Notes: +% - The robot base transform is ignored +% - The torque computed also contains a contribution due to armature +% inertia. +% - RNE can be either an M-file or a MEX-file. See the manual for details on +% how to configure the MEX-file. The M-file is a wrapper which calls either +% RNE_DH or RNE_MDH depending on the kinematic conventions used by the robot +% object. +% +% See also SerialLink.accel, SerialLink.gravload, SerialLink.inertia. + +% TODO: +% should use tb_optparse +% +% verified against MAPLE code, which is verified by examples +% + +% 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.petercorke.com + + +function [tau,f] = rne(robot, varargin) + if robot.mdh == 0, + [tau,f] = rne_dh(robot, varargin{:}); + else + tau = rne_mdh(robot, varargin{:}); + end +endfunction diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne_dh.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne_dh.m new file mode 100644 index 0000000..1bd5812 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne_dh.m @@ -0,0 +1,223 @@ + +%SERIALLINK.RNE_DH Compute inverse dynamics via recursive Newton-Euler formulation +% +% Recursive Newton-Euler for standard Denavit-Hartenberg notation. Is invoked by +% R.RNE(). +% +% See also SERIALLINK.RNE. + +% +% verified against MAPLE code, which is verified by examples +% + +% 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.petercorke.com +function [tau,wbase] = rne_dh(robot, a1, a2, a3, a4, a5) + + z0 = [0;0;1]; + grav = robot.gravity; % default gravity from the object + fext = zeros(6, 1); + n = robot.n; + + % check that robot object has dynamic parameters for each link + for j=1:n, + link = robot.links(j); + if isempty(link.r) || isempty(link.I) || isempty(link.m) + error('dynamic parameters (m, r, I) not set in link %d', j); + end + end + + % Set debug to: + % 0 no messages + % 1 display results of forward and backward recursions + % 2 display print R and p* + debug = 0; + + if numcols(a1) == 3*n, + Q = a1(:,1:n); + Qd = a1(:,n+1:2*n); + Qdd = a1(:,2*n+1:3*n); + np = numrows(Q); + if nargin >= 3, + grav = a2(:); + end + if nargin == 4, + fext = a3; + end + else + np = numrows(a1); + Q = a1; + Qd = a2; + Qdd = a3; + if numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | ... + numrows(Qd) ~= np | numrows(Qdd) ~= np, + error('bad data'); + end + if nargin >= 5, + grav = a4(:); + end + if nargin == 6, + fext = a5; + end + end + + tau = zeros(np,n); + + for p=1:np, + q = Q(p,:)'; + qd = Qd(p,:)'; + qdd = Qdd(p,:)'; + + Fm = []; + Nm = []; + pstarm = []; + Rm = []; + w = zeros(3,1); + wd = zeros(3,1); + v = zeros(3,1); + vd = grav(:); + + % + % init some variables, compute the link rotation matrices + % + for j=1:n, + link = robot.links(j); + Tj = link.A(q(j)); + if link.RP == 'R', + d = link.d; + else + d = q(j); + end + alpha = link.alpha; + pstar = [link.a; d*sin(alpha); d*cos(alpha)]; + if j == 1, + pstar = t2r(robot.base) * pstar; + Tj = robot.base * Tj; + end + pstarm(:,j) = pstar; + Rm{j} = t2r(Tj); + if debug>1, + Rm{j} + Pstarm(:,j)' + end + end + + % + % the forward recursion + % + for j=1:n, + link = robot.links(j); + + Rt = Rm{j}'; % transpose!! + pstar = pstarm(:,j); + r = link.r; + + % + % statement order is important here + % + if link.RP == 'R', + % revolute axis + wd = Rt*(wd + z0*qdd(j) + ... + cross(w,z0*qd(j))); + w = Rt*(w + z0*qd(j)); + %v = cross(w,pstar) + Rt*v; + vd = cross(wd,pstar) + ... + cross(w, cross(w,pstar)) +Rt*vd; + + else + % prismatic axis + w = Rt*w; + wd = Rt*wd; + vd = Rt*(z0*qdd(j)+vd) + ... + cross(wd,pstar) + ... + 2*cross(w,Rt*z0*qd(j)) +... + cross(w, cross(w,pstar)); + end + + %whos + vhat = cross(wd,r') + ... + cross(w,cross(w,r')) + vd; + F = link.m*vhat; + N = link.I*wd + cross(w,link.I*w); + Fm = [Fm F]; + Nm = [Nm N]; + + if debug, + fprintf('w: '); fprintf('%.3f ', w) + fprintf('\nwd: '); fprintf('%.3f ', wd) + fprintf('\nvd: '); fprintf('%.3f ', vd) + fprintf('\nvdbar: '); fprintf('%.3f ', vhat) + fprintf('\n'); + end + end + + % + % the backward recursion + % + + fext = fext(:); + f = fext(1:3); % force/moments on end of arm + nn = fext(4:6); + + for j=n:-1:1, + link = robot.links(j); + pstar = pstarm(:,j); + + % + % order of these statements is important, since both + % nn and f are functions of previous f. + % + if j == n, + R = eye(3,3); + else + R = Rm{j+1}; + end + r = link.r; + nn = R*(nn + cross(R'*pstar,f)) + ... + cross(pstar+r',Fm(:,j)) + ... + Nm(:,j); + f = R*f + Fm(:,j); + if debug, + fprintf('f: '); fprintf('%.3f ', f) + fprintf('\nn: '); fprintf('%.3f ', nn) + fprintf('\n'); + end + + R = Rm{j}; + if link.RP == 'R', + % revolute + tau(p,j) = nn'*(R'*z0) + ... + link.G^2 * link.Jm*qdd(j) + ... + abs(link.G) * friction(link, qd(j)); + else + % prismatic + tau(p,j) = f'*(R'*z0) + ... + link.G^2 * link.Jm*qdd(j) + ... + abs(link.G) * friction(link, qd(j)); + end + end + % this last bit needs work/testing + R = Rm{1}; + nn = R*(nn); + f = R*f; + wbase = [f; nn]; + end +endfunction diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne_mdh.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne_mdh.m new file mode 100644 index 0000000..efaa668 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/rne_mdh.m @@ -0,0 +1,211 @@ + +%SERIALLINK.RNE_MDH Compute inverse dynamics via recursive Newton-Euler formulation +% +% Recursive Newton-Euler for modified Denavit-Hartenberg notation. Is invoked by +% R.RNE(). +% +% See also SERIALLINK.RNE. + +% 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.petercorke.com + +function tau = rne_mdh(robot, a1, a2, a3, a4, a5) + + z0 = [0;0;1]; + grav = robot.gravity; % default gravity from the object + fext = zeros(6, 1); + + % Set debug to: + % 0 no messages + % 1 display results of forward and backward recursions + % 2 display print R and p* + debug = 0; + + n = robot.n; + if numcols(a1) == 3*n, + Q = a1(:,1:n); + Qd = a1(:,n+1:2*n); + Qdd = a1(:,2*n+1:3*n); + np = numrows(Q); + if nargin >= 3, + grav = a2(:); + end + if nargin == 4, + fext = a3; + end + else + np = numrows(a1); + Q = a1; + Qd = a2; + Qdd = a3; + if numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | ... + numrows(Qd) ~= np | numrows(Qdd) ~= np, + error('bad data'); + end + if nargin >= 5, + grav = a4(:); + end + if nargin == 6, + fext = a5; + end + end + + tau = zeros(np,n); + + for p=1:np, + q = Q(p,:)'; + qd = Qd(p,:)'; + qdd = Qdd(p,:)'; + + Fm = []; + Nm = []; + pstarm = []; + Rm = []; + w = zeros(3,1); + wd = zeros(3,1); + v = zeros(3,1); + vd = grav(:); + + % + % init some variables, compute the link rotation matrices + % + for j=1:n, + link = robot.link{j}; + Tj = link(q(j)); + if link.RP == 'R', + D = link.D; + else + D = q(j); + end + alpha = link.alpha; + pm = [link.A; -D*sin(alpha); D*cos(alpha)]; % (i-1) P i + if j == 1, + pm = t2r(robot.base) * pm; + Tj = robot.base * Tj; + end + Pm(:,j) = pm; + Rm{j} = t2r(Tj); + if debug>1, + Rm{j} + Pm(:,j)' + end + end + + % + % the forward recursion + % + for j=1:n, + link = robot.link{j}; + + R = Rm{j}'; % transpose!! + P = Pm(:,j); + Pc = link.r; + + % + % trailing underscore means new value + % + if link.RP == 'R', + % revolute axis + w_ = R*w + z0*qd(j); + wd_ = R*wd + cross(R*w,z0*qd(j)) + z0*qdd(j); + %v = cross(w,P) + R*v; + vd_ = R * (cross(wd,P) + ... + cross(w, cross(w,P)) + vd); + + else + % prismatic axis + w_ = R*w; + wd_ = R*wd; + %v = R*(z0*qd(j) + v) + cross(w,P); + vd_ = R*(cross(wd,P) + ... + cross(w, cross(w,P)) + vd ... + ) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j); + end + % update variables + w = w_; + wd = wd_; + vd = vd_; + + vdC = cross(wd,Pc) + ... + cross(w,cross(w,Pc)) + vd; + F = link.m*vdC; + N = link.I*wd + cross(w,link.I*w); + Fm = [Fm F]; + Nm = [Nm N]; + if debug, + fprintf('w: '); fprintf('%.3f ', w) + fprintf('\nwd: '); fprintf('%.3f ', wd) + fprintf('\nvd: '); fprintf('%.3f ', vd) + fprintf('\nvdbar: '); fprintf('%.3f ', vdC) + fprintf('\n'); + end + end + + % + % the backward recursion + % + + fext = fext(:); + f = fext(1:3); % force/moments on end of arm + nn = fext(4:6); + + for j=n:-1:1, + + % + % order of these statements is important, since both + % nn and f are functions of previous f. + % + link = robot.link{j}; + + if j == n, + R = eye(3,3); + P = [0;0;0]; + else + R = Rm{j+1}; + P = Pm(:,j+1); % i/P/(i+1) + end + Pc = link.r; + + f_ = R*f + Fm(:,j); + nn_ = Nm(:,j) + R*nn + cross(Pc,Fm(:,j)) + ... + cross(P,R*f); + + f = f_; + nn = nn_; + + if debug, + fprintf('f: '); fprintf('%.3f ', f) + fprintf('\nn: '); fprintf('%.3f ', nn) + fprintf('\n'); + end + if link.RP == 'R', + % revolute + tau(p,j) = nn'*z0 + ... + link.G^2 * link.Jm*qdd(j) + ... + abs(link.G) * friction(link, qd(j)); + else + % prismatic + tau(p,j) = f'*z0 + ... + link.G^2 * link.Jm*qdd(j) + ... + abs(link.G) * friction(link, qd(j)); + end + end + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/showlink.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/showlink.m new file mode 100644 index 0000000..d7d7359 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/showlink.m @@ -0,0 +1,34 @@ +%SerialLink.showlink Show parameters of all links +% +% R.showlink() shows details of all link parameters for the robot object, +% including inertial parameters. +% +% See also Link.showlink, Link. + +% 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.petercorke.com + +function showlink(r) + + for i=1:r.n, + fprintf('Link %d------------------------\n', i); + r.links(i).dyn(); + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/subsasgn.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/subsasgn.m new file mode 100644 index 0000000..49d2bef --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/subsasgn.m @@ -0,0 +1,72 @@ + +% 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.petercorke.com +function r = subsasgn(r, s, v) + + if s(1).type ~= '.' + error('only .field supported') + end + switch s(1).subs, + case 'links' + r.links = v; + case 'lineopt', + r.lineopt = v; + case 'shadowopt', + r.shadowopt = v; + case 'offset', + L = r.links; + for i=1:r.n, + L(i).offset = v(i); + end + case 'qlim', + if numrows(v) ~= r.n, + error('insufficient rows in joint limit matrix'); + end + L = r.links; + for i=1:r.n, + L(i).qlim = v(i,:); + end + case 'q', + r.q = v; + case 'handle', + r.handle = v; + case 'plotopt', + r.plotopt = v; + case 'gravity', + r.gravity = v; + case 'tool', + if ~ishomog(v) + error('base must be a homogeneous transform'); + end + r.tool = v; + case 'base', + if ~ishomog(v) + error('base must be a homogeneous transform'); + end + r.base = v; + case 'name', + r.name = v; + case 'manufacturer', + r.manufacturer = v; + case 'comment', + r.comment = v; + otherwise, error('Unknown method in subsasgn') + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/subsref.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/subsref.m new file mode 100644 index 0000000..e7edeab --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/subsref.m @@ -0,0 +1,211 @@ + +% 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.petercorke.com +function [v, a, b] = subsref(r, s) + + if s(1).type ~= '.' + %error('only .field supported') + end + + % NOTE WELL: the following code can't use getfield() since + % getfield() uses this, and Matlab will crash!! + + el = char(s(1).subs); + switch el, +%------------------------------------------------------------------------------------- +% External Functions + case 'fkine', + q = s(2).subs; + v = fkine(r,q{:}); + + case 'ikine6s', + q = s(2).subs; + v = ikine6s(r,q{:}); + + case 'ikine', + q = s(2).subs; + v = ikine(r,q{:}); + + case 'maniplty', + q = s(2).subs; + [v, a] = maniplty(r,q{:}); + + case 'rne' + q = s(2).subs; + [v, a] = rne(r,q{:}); + + case 'isspherical', + v = isspherical(r); + + case 'gravload' + q = s(2).subs; + v = gravload(r,q{:}); + + case 'inertia' + q = s(2).subs; + v = inertia(r,q{:}); + + case 'cinertia', + q = s(2).subs; + v = cinertia(r,q{:}); + + case 'coriolis' + q = s(2).subs; + v = coriolis(r,q{:}); + + case 'friction' + q = s(2).subs; + v = friction(r,q{:}); + + case 'nofriction' + if numel(s(2).subs) < 1 + v = nofriction(r); + else + q = s(2).subs; + v = nofriction(r,q{:}); + end + + case 'islimit', + q = s(2).subs; + v = islimit(r,q{:}); + + case 'payload', + q = s(2).subs; + l = r.links; + l(r.n).m = q{1}; + l(r.n).r = q{2}; + r.links = l; + v = SerialLink(r); + + case 'jacob0', + q = s(2).subs; + v = jacob0(r,q{:}); + + case 'accel', + q = s(2).subs; + v = accel(r,q{:}); + + case 'plot', + q = s(2).subs; + plot(r,q{:}) + + case 'fdyn', + q = s(2).subs; + [v, a, b] = fdyn(r,q{:}); + + case 'jacobn', + q = s(2).subs; + v = jacobn(r,q{:}); + + case 'jtraj', + q = s(2).subs; + v = jtraj(r,q{:}); + + case 'perturb', + q = s(2).subs; + v = perturb(r,q{:}); + + case 'itorque', + q = s(2).subs; + v = itorque(r,q{:}); + + case 'jacob_dot', + q = s(2).subs; + v = jacob_dot(r,q{:}); + + case 'dyn', + q = s(2).subs; + v = dyn(r,q{:}); + + case 'showlink', + q = s(2).subs; + v = showlink(r,q{:}); + +%------------------------------------------------------------------------------------- + case 'links', + + if length(s) == 1, + v = r.links; + + elseif length(s) == 2, + if s(2).type == '()' + j = s(2).subs; + j = j{:}; + if (j < 1) | (j > r.n) + error('link index out of bounds') + end + end + v = r.links(j); + + elseif length(s) >= 3, + if s(2).type == '()' + j = s(2).subs; + j = j{:}; + if (j < 1) | (j > r.n) + error('link index out of bounds') + end + end + if s(3).subs = 'dyn', + link = r.links(j); + v = link.dyn(); + end + end + case 'offset', + L = r.links; + v = []; + for i=1:r.n, + v = [v; L(i).offset]; + end + case 'qlim', + L = r.links; + v = []; + for i=1:r.n, + v = [v; L(i).qlim]; + end + + case 'n', + v = r.n; + case 'name', + v = r.name; + case 'dh', + v = rdh(r); + case 'dyn' + v = rdyn(r); + case 'gravity' + v = r.gravity; + case 'tool' + v = r.tool; + case 'base' + v = r.base; + case 'mdh', + v = r.mdh; + case 'q', + v = r.q; + case 'plotopt', + v = r.plotopt; + case 'lineopt' + v = r.lineopt; + case 'shadowopt' + v = r.shadowopt; + case {'show', 'handle'} + v = r.handle'; + otherwise, error('Unknown method in subsref') + end diff --git a/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/teach.m b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/teach.m new file mode 100644 index 0000000..f256809 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/@SerialLink/teach.m @@ -0,0 +1,329 @@ +%SerialLink.teach Graphical teach pendant +% +% R.teach() drive a graphical robot by means of a graphical slider panel. +% If no graphical robot exists one is created in a new window. Otherwise +% all current instanes of the graphical robots are driven. +% +% R.teach(Q) specifies the initial joint angle, otherwise it is taken from +% one of the existing graphical robots. +% +% See also SerialLink.plot. + + +% 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.petercorke.com + +%% TODO: +%% make the sliders change the animation while moving +%% http://www.mathworks.com/matlabcentral/newsreader/view_thread/159374 +%% 1. download FINDJOBJ from the file exchange: http://www.mathworks.com/matlabcentral/fileexchange/14317 +%% 2. hSlider = uicontrol('style','slider', ...); %create the slider, get its Matlab handle +%% 3. jSlider = findjobj(hSlider,'nomenu'); %get handle of the underlying java object +%% 4. jSlider.AdjustmentValueChangedCallback = @myMatlabFunction; %set callback +%% +%% Note: you can also use the familiar format: +%% set(jSlider,'AdjustmentValueChangedCallback',@myMatlabFunction) +%% +%% Feel free to explore many other properties (and ~30 callbacks) +%% available in jSlider but not in its Matlab front-end interface hSlider. +%% +%% Warning: FINDJOBJ relies on undocumented and unsupported Matlab/Java +%% functionality. + +function teach(r, varargin) + bgcol = [135 206 250]/255; + + opt.degrees = false; + opt.q0 = []; +% TODO: options for rpy, or eul angle display + + opt = tb_optparse(opt, varargin); + + % drivebot(r, q) + % drivebot(r, 'deg') + scale = ones(r.n,1); + + n = r.n; + width = 300; + height = 40; + minVal = -pi; + maxVal = pi; + + qlim = r.qlim; + if isempty(qlim) + qlim = [minVal*ones(r.n,1) maxVal*ones(r.n,1)]; + end + + if isempty(opt.q0) + q = zeros(1,n); + else + q = opt.q0; + end + + % set up scale factor + scale = []; + for L=r.links + if opt.degrees && L.revolute + scale = [scale 180/pi]; + else + scale = [scale 1]; + end + end + + T6 = r.fkine(q); + fig = figure('Units', 'pixels', ... + 'Position', [0 -height width height*(n+2)+20], ... + 'Color', bgcol); + set(fig,'MenuBar','none') + delete( get(fig, 'Children') ) + + % first we check to see if there are any graphical robots of + % this name, if so we use them, otherwise create a robot plot. + + rh = findobj('Tag', r.name); + + % attempt to get current joint config of graphical robot + if ~isempty(rh) + rr = get(rh(1), 'UserData'); + if ~isempty(rr.q) + q = rr.q; + end + end + + % now make the sliders + for i=1:n + % slider label + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n-i)+20 width*0.1 height*0.4], ... + 'String', sprintf('q%d', i)); + + % slider itself + q(i) = max( qlim(i,1), min( qlim(i,2), q(i) ) ); % clip to range + h(i) = uicontrol(fig, 'Style', 'slider', ... + 'Units', 'pixels', ... + 'Position', [width*0.1 height*(n-i)+20 width*0.7 height*0.4], ... + 'Min', scale(i)*qlim(i,1), ... + 'Max', scale(i)*qlim(i,2), ... + 'Value', scale(i)*q(i), ... + 'Tag', sprintf('Slider%d', i), ... + 'Callback', @(src,event)teach_callback(r.name, i)); + + % if findjobj exists use it, since it lets us get continous callbacks while + % a slider moves + if exist('findjobj') && ~ispc + drawnow + jh = findjobj(h(i),'nomenu'); + jh.AdjustmentValueChangedCallback = {@sliderCallbackFunc, r.name, i}; + end + % text box showing slider value, also editable + h2(i) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [width*0.8 height*(n-i-0.1)+20 width*0.2 height*0.7], ... + 'String', num2str(scale(i)*q(i)), ... + 'Tag', sprintf('Edit%d', i), ... + 'Callback', @(src,event)teach_callback(r.name, i)); + + % hang handles off the slider and edit objects + handles = {h(i) h2(i) scale}; + set(h(i), 'Userdata', handles); + set(h2(i), 'Userdata', handles); + end + + % robot name text box + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'FontSize', 20, ... + 'HorizontalAlignment', 'left', ... + 'Position', [0 height*(n+1.2)+20 0.8*width 0.8*height], ... + 'BackgroundColor', 'white', ... + 'String', r.name); + + % X + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n+0.5)+20 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'x:'); + + h3(1,1) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.06*width height*(n+0.5)+20 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(1,4)), ... + 'Tag', 'T6'); + + % Y + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.26*width height*(n+0.5)+20 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'y:'); + + h3(2,1) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.32*width height*(n+0.5)+20 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(2,4))); + + % Z + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.52*width height*(n+0.5)+20 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'z:'); + + h3(3,1) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.58*width height*(n+0.5)+20 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(3,4))); + + % AX + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n)+20 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'ax:'); + + h3(1,2) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.06*width height*(n)+20 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(1,3))); + + % AY + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.26*width height*(n)+20 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'ay:'); + + h3(2,2) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.32*width height*(n)+20 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(2,3))); + + % AZ + uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', ... + 'BackgroundColor', bgcol, ... + 'Position', [0.52*width height*(n)+20 0.06*width height/2], ... + 'BackgroundColor', 'yellow', ... + 'FontSize', 10, ... + 'HorizontalAlignment', 'right', ... + 'String', 'az:'); + + h3(3,2) = uicontrol(fig, 'Style', 'edit', ... + 'Units', 'pixels', ... + 'Position', [0.58*width height*(n)+20 width*0.2 height*0.6], ... + 'String', sprintf('%.3f', T6(3,3))); + + + set(h3(1,1), 'Userdata', h3); + uicontrol(fig, 'Style', 'pushbutton', ... + 'Units', 'pixels', ... + 'FontSize', 16, ... + 'Position', [0.8*width height*n+20 0.2*width 2*height], ... + 'CallBack', 'delete(gcf)', ... + 'BackgroundColor', 'red', ... + 'String', 'Quit'); + + + if isempty(rh) + figure + r.plot(q); + end +end + +function teach_callback(a, b) +% called on changes to a slider or to the edit box showing joint coordinate +% teach_callback(robot name, joint index) + + name = a; % name of the robot + j = b; % joint index + + % find all graphical objects tagged with the robot name, this is the + % instancs of that robot across all figures + rh = findobj('Tag', name); + + % get the handles {slider, textbox, scale factor} + handles = get(gcbo, 'Userdata'); + scale = handles{3}; + + for r=rh' % for every graphical robot instance + robot = get(r, 'UserData'); % get the robot object + q = robot.q; % get its current joint angles + if isempty(q) + q = zeros(1,robot.n); + end + + if gcbo == handles{1} + % get value from slider + q(j) = get(gcbo, 'Value') / scale(j); + set(handles{2}, 'String', num2str(scale(j)*q(j))); + elseif gcbo == handles{2} + % get value from text box + q(j) = str2num(get(gcbo, 'String')) / scale(j); + set(handles{1}, 'Value', q(j)); + else + warning('shouldnt happen'); + end + robot.q = q; + set(r, 'UserData', robot); + robot.plot(q) + end + + % compute and display the T6 pose + T6 = robot.fkine(q); + h3 = get(findobj('Tag', 'T6'), 'UserData'); + for i=1:3 + set(h3(i,1), 'String', sprintf('%.3f', T6(i,4))); + set(h3(i,2), 'String', sprintf('%.3f', T6(i,3))); + end + + robot.T = T6; + robot.notify('Moved'); + drawnow +end + +function sliderCallbackFunc(src, ev, name, joint) + if get(src,'ValueIsAdjusting') == 1 + try + teach_callback(name, joint); + drawnow + catch + return + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/Octave/README.txt b/lwrserv/matlab/rvctools/robot/Octave/README.txt new file mode 100644 index 0000000..0f73f6e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Octave/README.txt @@ -0,0 +1,60 @@ +--------------------------------------------------------------------------------------------- +Software +--------------------------------------------------------------------------------------------- +This version of the toolbox was developed using Octave 3.4.3 for windows but should still +run on all distributions of octave. + +The latest version of octave can be found here: http://octave.sourceforge.net/ + + +--------------------------------------------------------------------------------------------- +Installation +--------------------------------------------------------------------------------------------- +To install the toolbox in octave the latest version of RVCTools is needed you can get this +form http://www.petercorke.com/Robotics_Toolbox.html + +Unzip the toolbox and place it in the m directory in your octave install (share\octave\3.4.3\m) + +Now copy all three folders from the Octave patch (@Quaternion @Link @SerialLink) to the +directory (m\RVCtools\robot). Copy and replace all files and folders. + +Once this is finished you are ready to use the toolbox in octave. Try >>> mdl_puma560 to +build a robot. + +The code is a mashup of current V9 and the much older V6 code which supports Octave style classes. + + +--------------------------------------------------------------------------------------------- +Functionality of the toolbox +--------------------------------------------------------------------------------------------- +Most of the Toolbox should work as it does in MATLAB although there will be some functions +that don't work correctly due to slight differences between MATLAB and Octave. + +One area of difference is the lack of reference objects in Octave. Link and SerialLink objects +inherit from the handle object in MATLAB which means that methods can change the value of the +object. This is not possible in Octave yet. So in MATLAB you can write + +obj.method(x) + +but in Octave you have to write + +obj = obj.method(x) + +where method(x) changes some property of the object. + + +Quaternion: +----------- +.plot a little glitchy + +Link: +----- +Link seems to be working quite well + + +SerialLink: +----------- +.plot function works but has some glitches +.payload only works when making a new robot e.g. p560 = p560.payload(2.5,[0 0 .1]) +.teach is not implemented in this release +.ikine is not working correctly and will most likely crash. diff --git a/lwrserv/matlab/rvctools/robot/PRM.m b/lwrserv/matlab/rvctools/robot/PRM.m new file mode 100644 index 0000000..3437e6a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/PRM.m @@ -0,0 +1,307 @@ +%PRM Probabilistic RoadMap navigation class +% +% A concrete subclass of the Navigation class that implements the probabilistic +% roadmap navigation algorithm. This performs goal independent planning of +% roadmaps, and at the query stage finds paths between specific start and +% goal points. +% +% Methods:: +% +% plan Compute the roadmap +% path Compute a path to the goal +% visualize Display the obstacle map (deprecated) +% plot Display the obstacle map +% display Display the parameters in human readable form +% char Convert to string +% +% Example:: +% +% load map1 % load map +% goal = [50,30]; % goal point +% start = [20, 10]; % start point +% prm = PRM(map); % create navigation object +% prm.plan() % create roadmaps +% prm.path(start, goal) % animate path from this start location +% +% References:: +% +% - Probabilistic roadmaps for path planning in high dimensional configuration spaces, +% L. Kavraki, P. Svestka, J. Latombe, and M. Overmars, +% IEEE Transactions on Robotics and Automation, vol. 12, pp. 566-580, Aug 1996. +% - Robotics, Vision & Control, Section 5.2.4, +% P. Corke, Springer 2011. +% +% See also Navigation, DXform, Dstar, PGraph. + +% 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 . + + +% Peter Corke 8/2009. + +classdef PRM < Navigation + + properties + npoints % number of sample points + distthresh % distance threshold, links between vertices + % must be less than this. + + graph % graph Object representing random nodes + + vgoal % index of vertex closest to goal + vstart % index of vertex closest to start + localGoal % next vertex on the roadmap + localPath % set of points along path to next vertex + gpath % list of vertices between start and goal + end + + methods + + % constructor + function prm = PRM(varargin) + %PRM.PRM Create a PRM navigation object + % + % P = PRM(MAP, options) is a probabilistic roadmap navigation + % object, and MAP is an occupancy grid, a representation of a + % planar world as a matrix whose elements are 0 (free space) or 1 + % (occupied). + % + % Options:: + % 'npoints',N Number of sample points (default 100) + % 'distthresh',D Distance threshold, edges only connect vertices closer + % than D (default 0.3 max(size(occgrid))) + % + % Other options are supported by the Navigation superclass. + % + % See also Navigation.Navigation. + + + % invoke the superclass constructor, it handles some options + prm = prm@Navigation(varargin{:}); + + % create an empty 2D graph + prm.graph = PGraph(2); + + % parse out PRM specific options and save in the navigation object + opt.npoints = 100; + opt.distthresh = 0.3*max(size(prm.occgrid)); + [opt,args] = tb_optparse(opt, varargin); + prm.npoints = opt.npoints; + prm.distthresh = opt.distthresh; + end + + function plan(prm) + %PRM.plan Create a probabilistic roadmap + % + % P.plan() creates the probabilistic roadmap by randomly + % sampling the free space in the map and building a graph with + % edges connecting close points. The resulting graph is kept + % within the object. + + % build a graph over the free space + prm.message('create the graph'); + + prm.graph.clear(); % empty the graph + create_roadmap(prm); % build the graph + end + + function p = path(prm, start, goal) + %PRM.path Find a path between two points + % + % P.path(START, GOAL) finds and displays a path from START to GOAL + % which is overlaid on the occupancy grid. + % + % X = P.path(START) returns the path (2xM) from START to GOAL. + + if nargin < 3 + error('must specify start and goal'); + end + + % set the goal coordinate + prm.goal = goal; + + % invoke the superclass path function, which iterates on our + % next method + if nargout == 0 + path@Navigation(prm, start); + else + p = path@Navigation(prm, start); + end + end + + % Handler invoked by Navigation.path() to start the navigation process + % + % - find a path through the graph + % - determine vertices closest to start and goal + % - find path to first vertex + function navigate_init(prm, start) + + % find the vertex closest to the goal + prm.vgoal = prm.graph.closest(prm.goal); + + % find the vertex closest to the start + prm.vstart = prm.graph.closest(start); + + % are the vertices connected? + if prm.graph.component(prm.vstart) ~= prm.graph.component(prm.vgoal) + error('PRM:plan:nopath', 'PRM: start and goal not connected: rerun the planner'); + end + + % find a path through the graph + prm.message('planning path through graph'); + prm.graph.goal(prm.vgoal); % set the goal + prm.gpath = prm.graph.path(prm.vstart); + + % the path is a list of nodes from vstart to vgoal + % discard the first vertex, since we plan a local path to it + prm.gpath = prm.gpath(2:end); + + % start the navigation engine with a path to the nearest vertex + prm.graph.highlight_node(prm.vstart); + + prm.localPath = bresenham(start, prm.graph.coord(prm.vstart)); + prm.localPath = prm.localPath(2:end,:); + end + + % Invoked for each step on the path by path() method. + function n = next(prm, p) + + if all(p(:) == prm.goal) + n = []; % signal that we've arrived + return; + end + + % we take the next point from the localPath + if numrows(prm.localPath) == 0 + % local path is consumed, move to next vertex + if isempty(prm.gpath) + % we have arrived at the goal vertex + % make the path from this vertex to the goal coordinate + prm.localPath = bresenham(p, prm.goal); + prm.localPath = prm.localPath(2:end,:); + prm.localGoal = []; + else + % set local goal to next vertex in gpath and remove it from the list + prm.localGoal = prm.gpath(1); + prm.gpath = prm.gpath(2:end); + + % compute local path to the next vertex + prm.localPath = bresenham(p, prm.graph.coord(prm.localGoal)); + prm.localPath = prm.localPath(2:end,:); + prm.graph.highlight_node(prm.localGoal); + end + end + + n = prm.localPath(1,:)'; % take the first point + prm.localPath = prm.localPath(2:end,:); % and remove from the path + end + + function s = char(prm) + %PRM.char Convert to string + % + % P.char() is a string representing the state of the PRM + % object in human-readable form. + % + % See also PRM.display. + + + % invoke the superclass char() method + s = char@Navigation(prm); + + % add PRM specific stuff information + s = char(s, sprintf(' graph size: %d', prm.npoints)); + s = char(s, sprintf(' dist thresh: %f', prm.distthresh)); + s = char(s, char(prm.graph) ); + end + + + function plot(prm, varargin) + %PRM.plot Visualize navigation environment + % + % P.plot() displays the occupancy grid with an optional distance field. + % + % Options:: + % 'goal' Superimpose the goal position if set + % 'nooverlay' Don't overlay the PRM graph + + opt.nooverlay = false; + [opt,args] = tb_optparse(opt, varargin); + + % display the occgrid + plot@Navigation(prm, args{:}); + + if ~opt.nooverlay + hold on + prm.graph.plot()%varargin{:}); + hold off + end + end + + end % method + + methods (Access='protected') + % private methods + % create the roadmap + function create_roadmap(prm) + + for j=1:prm.npoints + % pick a point not in obstacle + while true + x = prm.randi(numcols(prm.occgrid)); + y = prm.randi(numrows(prm.occgrid)); + if prm.occgrid(y,x) == 0 + break; + end + end + new = [x; y]; + + vnew = prm.graph.add_node(new); + + [d,v] = prm.graph.distances(new); + % test neighbours in order of increasing distance + for i=1:length(d) + if v(i) == vnew + continue; + end + if d(i) > prm.distthresh + continue; + end + if ~prm.testpath(new, prm.graph.coord(v(i))) + continue; + end + prm.graph.add_edge(vnew, v(i)); + end + end + end + + % test the path from p1 to p2 is entirely in free space + function c = testpath(prm, p1, p2) + p = bresenham(p1, p2); + + for pp=p' + if prm.occgrid(pp(2), pp(1)) > 0 + c = false; + return; + end + end + c = true; + end + + + end % private methods + +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/ParticleFilter.m b/lwrserv/matlab/rvctools/robot/ParticleFilter.m new file mode 100644 index 0000000..d8237ac --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/ParticleFilter.m @@ -0,0 +1,435 @@ +%ParticleFilter Particle filter class +% +% Monte-carlo based localisation for estimating vehicle pose based on +% odometry and observations of known landmarks. +% +% Methods:: +% run run the particle filter +% plot_xy display estimated vehicle path +% plot_pdf display particle distribution +% +% Properties:: +% robot reference to the robot object +% sensor reference to the sensor object +% history vector of structs that hold the detailed information from +% each time step +% nparticles number of particles used +% x particle states; nparticles x 3 +% weight particle weights; nparticles x 1 +% x_est mean of the particle population +% std standard deviation of the particle population +% Q covariance of noise added to state at each step +% L covariance of likelihood model +% w0 offset in likelihood model +% dim maximum xy dimension +% +% Example:: +% +% Create a landmark map +% map = Map(20); +% and a vehicle with odometry covariance and a driver +% W = diag([0.1, 1*pi/180].^2); +% veh = Vehicle(W); +% veh.add_driver( RandomPath(10) ); +% and create a range bearing sensor +% R = diag([0.005, 0.5*pi/180].^2); +% sensor = RangeBearingSensor(veh, map, R); +% +% For the particle filter we need to define two covariance matrices. The +% first is is the covariance of the random noise added to the particle +% states at each iteration to represent uncertainty in configuration. +% Q = diag([0.1, 0.1, 1*pi/180]).^2; +% and the covariance of the likelihood function applied to innovation +% L = diag([0.1 0.1]); +% Now construct the particle filter +% pf = ParticleFilter(veh, sensor, Q, L, 1000); +% which is configured with 1000 particles. The particles are initially +% uniformly distributed over the 3-dimensional configuration space. +% +% We run the simulation for 1000 time steps +% pf.run(1000); +% then plot the map and the true vehicle path +% map.plot(); +% veh.plot_xy('b'); +% and overlay the mean of the particle cloud +% pf.plot_xy('r'); +% We can plot the standard deviation against time +% plot(pf.std(1:100,:)) +% The particles are a sampled approximation to the PDF and we can display +% this as +% pf.plot_pdf() +% +% Acknowledgement:: +% +% Based on code by Paul Newman, Oxford University, +% http://www.robots.ox.ac.uk/~pnewman +% +% Reference:: +% +% Robotics, Vision & Control, +% Peter Corke, +% Springer 2011 +% +% See also Vehicle, RandomPath, RangeBearingSensor, Map, EKF. + +% 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 . + + +%note this is not coded efficiently but rather to make the ideas clear +%all loops should be vectorized but that gets a little matlab-speak intensive +%and may obliterate the elegance of a particle filter.... + +classdef ParticleFilter < handle +%TODO +% x_est should be a weighted mean +% std should be a weighted std (x-mean)' W (x-mean) ??? + properties + robot + sensor + nparticles + x % particle states; nparticles x 3 + weight % particle weights; nparticles x 1 + x_est % mean of the particle population + std % standard deviation of the particle population + Q % covariance of noise added to state at each step + L % covariance of likelihood model + history + keephistory + dim % maximum xy dimension + + h % graphics handle for particles + randstream + seed0 + w0 + end % properties + + methods + function pf = ParticleFilter(robot, sensor, Q, L, np, varargin) + %ParticleFilter.ParticleFilter Particle filter constructor + % + % PF = ParticleFilter(VEHICLE, SENSOR, Q, L, NP, OPTIONS) is a particle + % filter that estimates the state of the VEHICLE with a sensor + % SENSOR. Q is covariance of the noise added to the particles + % at each step (diffusion), L is the covariance used in the + % sensor likelihood model, and NP is the number of particles. + % + % Options:: + % 'verbose' Be verbose. + % 'private' Use private random number stream. + % 'reset' Reset random number stream. + % 'seed',S Set the initial state of the random number stream. S must + % be a proper random number generator state such as saved in + % the seed0 property of an earlier run. + % 'nohistory' Don't save history. + % + % Notes:: + % - ParticleFilter subclasses Handle, so it is a reference object. + % - The initial particle distribution is uniform over the map, + % essentially the kidnapped robot problem which is quite unrealistic. + % - The 'private' option creates a private random number stream for the methods + % rand, randn and randi. If not given the global stream is used. + % + % + % See also Vehicle, Sensor, RangeBearingSensor, Map. + + pf.robot = robot; + pf.sensor = sensor; + pf.Q = Q; + pf.L = L; + pf.nparticles = np; + + pf.dim = sensor.map.dim; + pf.history = []; + pf.x = []; + pf.weight = []; + pf.w0 = 0.05; + + opt.private = false; + opt.reset = false; + opt.seed = []; + opt.history = true; + + opt = tb_optparse(opt, varargin); + + pf.keephistory = opt.history; + % create a private random number stream if required + if opt.private + pf.randstream = RandStream.create('mt19937ar'); + else + pf.randstream = RandStream.getGlobalStream(); + end + + % reset the random number stream if required + if opt.reset + pf.randstream.reset(); + end + + % return the random number stream to known state if required + if ~isempty(opt.seed) + set(pf.randstream.set(opt.seed)); + end + + % save the current state in case it later turns out to give interesting results + pf.seed0 = pf.randstream.State; + + end + + + function init(pf) + %ParticleFilter.init Initialize the particle filter + % + % PF.init() initializes the particle distribution and clears the + % history. + % + % Notes:: + % - Invoked by the run() method. + pf.robot.init(); + pf.history = []; + + % create initial particle distribution as uniformly randomly distributed + % over the map area and heading angles + pf.x = (2*pf.rand([pf.nparticles,3]) - 1) * diag([pf.dim, pf.dim, pi]); + pf.weight = ones(pf.nparticles, 1); + + pf.x_est = []; + pf.std = []; + end + + function run(pf, niter, varargin) + %ParticleFilter.run Run the particle filter + % + % PF.run(N, OPTIONS) runs the filter for N time steps. + % + % Options:: + % 'noplot' Do not show animation. + % + % Notes:: + % - All previously estimated states and estimation history is + % cleared. + + opt.plot = true; + opt = tb_optparse(opt, varargin); + + pf.init(); + pf.sensor.map.plot(); + a = axis; + a(5:6) = [-pi pi]; + axis(a) + zlabel('heading (rad)'); + + % display the initial particles + pf.h = plot3(pf.x(:,1), pf.x(:,2), pf.x(:,3), 'g.'); + + pf.robot.plot(); + + % iterate over time + for i=1:niter + pf.step(opt); + end + end + + function step(pf, opt) + + %fprintf('---- step\n'); + odo = pf.robot.step(); % move the robot + + % update the particles based on odometry + pf.predict(odo); + + % get a sensor reading + [z,jf] = pf.sensor.reading(); + + if ~isnan(jf) + pf.observe(z, jf); + %fprintf(' observe beacon %d\n', jf); + + pf.select(); + end + % our estimate is simply the mean of the particles + mn = mean(pf.x); + pf.x_est = [pf.x_est; mn]; + s = std(pf.x); + + % std is more complex for angles, need to account for 2pi wrap + s(3) = sqrt(sum(angdiff(pf.x(:,3), mn(3)).^2)) / (pf.nparticles-1); + pf.std = [pf.std; s]; + + % display the updated particles + set(pf.h, 'Xdata', pf.x(:,1), 'Ydata', pf.x(:,2), 'Zdata', pf.x(:,3)); + + if opt.plot + pf.robot.plot(); + drawnow + end + + if pf.keephistory + hist = []; + hist.x_est = pf.x; + hist.w = pf.weight; + pf.history = [pf.history hist]; + end + end + + function plot_pdf(pf) + %ParticleFilter.plot_pdf Plot particles as a PDF + % + % PF.plot_pdf() plots a sparse PDF as a series of vertical line + % segments of height equal to particle weight. + clf + hold on + for p = 1:pf.nparticles + x = pf.x(p,:); + plot3([x(1) x(1)], [x(2) x(2)], [0 pf.weight(p)]); + end + end + + function plot_xy(pf, varargin) + %ParticleFilter.plot_xy Plot vehicle position + % + % PF.plot_xy() plots the estimated vehicle path in the xy-plane. + % + % PF.plot_xy(LS) as above but the optional line style arguments + % LS are passed to plot. + plot(pf.x_est(:,1), pf.x_est(:,2), varargin{:}); + end + + function display(pf) + %ParticleFilter.display Display status of particle filter object + % + % PF.display() displays the state of the ParticleFilter object in + % human-readable form. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a ParticleFilter object and the command has no trailing + % semicolon. + % + % See also ParticleFilter.char. + + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(pf) ); + end % display() + + function s = char(pf) + %ParticleFilter.char Convert to string + % + % PF.char() is a string representing the state of the ParticleFilter + % object in human-readable form. + % + % See also ParticleFilter.display. + s = sprintf('ParticleFilter object: %d particles', pf.nparticles); + if ~isempty(pf.robot) + s = char(s, char(pf.robot) ); + end + if ~isempty(pf.sensor) + s = char(s, char(pf.sensor)); + end + s = char(s, ['Q: ' mat2str(pf.Q, 3)] ); + s = char(s, ['L: ' mat2str(pf.L, 3)] ); + s = char(s, sprintf('w0: %g', pf.w0) ); + end + + + end % methods + + methods(Access=protected) + % step 2 + % update the particle state based on odometry and a random perturbation + function predict(pf, odo) + + % Straightforward code: + % + % for i=1:pf.nparticles + % x = pf.robot.f( pf.x(i,:), odo)' + sqrt(pf.Q)*pf.randn(3,1); + % x(3) = angdiff(x(3)); + % pf.x(i,:) = x; + % + % Vectorized code: + + randvec = pf.randn(pf.nparticles,3); + pf.x = pf.robot.f( pf.x, odo) + randvec*sqrt(pf.Q); + pf.x(:,3) = angdiff(pf.x(:,3)); + end + + % step 3 + % predict observation and score the particles + function observe(pf, z, jf) + + % Straightforward code: + % + % for p = 1:pf.nparticles + % % what do we expect observation to be for this particle? + % % use the sensor model h(.) + % z_pred = pf.sensor.h( pf.x(p,:), jf); + % + % % how different is it + % innov(1) = z(1) - z_pred(1); + % innov(2) = angdiff(z(2), z_pred(2)); + % + % % get likelihood (new importance). Assume Gaussian but any PDF works! + % % If predicted obs is very different from actual obs this score will be low + % % ie. this particle is not very good at predicting the observation. + % % A lower score means it is less likely to be selected for the next generation... + % % The weight is never zero. + % pf.weight(p) = exp(-0.5*innov'*inv(pf.L)*innov) + 0.05; + % end + % + % Vectorized code: + + invL = inv(pf.L); + z_pred = pf.sensor.h( pf.x, jf); + z_pred(:,1) = z(1) - z_pred(:,1); + z_pred(:,2) = angdiff(z(2), z_pred(:,2)); + + LL = -0.5*[invL(1,1); invL(2,2); 2*invL(1,2)]; + e = [z_pred(:,1).^2 z_pred(:,2).^2 z_pred(:,1).*z_pred(:,2)]*LL; + pf.weight = exp(e) + pf.w0; + end + + % step 4 + % select particles based on their weights + function select(pf) + + % particles with large weights will occupy a greater percentage of the + % y axis in a cummulative plot + CDF = cumsum(pf.weight)/sum(pf.weight); + + % so randomly (uniform) choosing y values is more likely to correspond to + % better particles... + iSelect = pf.rand(pf.nparticles,1); + + % find the particle that corresponds to each y value (just a look up) + iNextGeneration = interp1(CDF, 1:pf.nparticles, iSelect, 'nearest', 'extrap'); + + % copy selected particles for next generation.. + pf.x = pf.x(iNextGeneration,:); + end + + function r = rand(pf, varargin) + r = pf.randstream.rand(varargin{:}); + end + + function r = randn(pf, varargin) + r = pf.randstream.randn(varargin{:}); + end + end % private methods +end diff --git a/lwrserv/matlab/rvctools/robot/Prismatic.m b/lwrserv/matlab/rvctools/robot/Prismatic.m new file mode 100644 index 0000000..3f0c9b9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Prismatic.m @@ -0,0 +1,50 @@ +%Prismatic Robot manipulator Prismatic link class +% +% A subclass of the Link class: holds all information related to a robot +% link such as kinematics parameters, rigid-body inertial parameters, motor +% and transmission parameters. +% +% Notes:: +% - This is reference class object +% - Link class objects can be used in vectors and arrays +% +% References:: +% - Robotics, Vision & Control, Chap 7 +% P. Corke, Springer 2011. +% +% See also Link, Revolute, SerialLink. + +% 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 . +classdef Prismatic < Link + methods + function L = Prismatic(varargin) + L = L@Link(varargin{:}); + + if nargin == 0 + L.d = []; + end + if isempty(L.theta) + L.theta = 0; + end + if ~isempty(L.d) + error('d cannot be specified for a prismatic link'); + end + L.sigma = 1; + end + end +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/PrismaticMDH.m b/lwrserv/matlab/rvctools/robot/PrismaticMDH.m new file mode 100644 index 0000000..9375275 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/PrismaticMDH.m @@ -0,0 +1,55 @@ +%PrismaticMDH Robot manipulator prismatic link class for MDH convention +% +% A subclass of the Link class: holds all information related to a +% prismatic (sliding) robot link such as kinematics parameters, rigid-body +% inertial parameters, motor and transmission parameters. +% +% Notes:: +% - This is reference class object +% - Link class objects can be used in vectors and arrays +% - Modified Denavit-Hartenberg parameters are used +% +% References:: +% - Robotics, Vision & Control, Chap 7 +% P. Corke, Springer 2011. +% +% See also Link, Prismatic, RevoluteMDH, SerialLink. + + +% Copyright (C) 1993-2015, 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.petercorke.com +classdef PrismaticMDH < Link + methods + function L = PrismaticMDH(varargin) + L = L@Link(varargin{:}); + + if nargin == 0 + L.d = []; + end + if isempty(L.theta) + L.theta = 0; + end + if ~isempty(L.d) + error('d cannot be specified for a prismatic link'); + end + L.sigma = 1; + L.mdh = 1; + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/Quaternion.m b/lwrserv/matlab/rvctools/robot/Quaternion.m new file mode 100644 index 0000000..a158bf4 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Quaternion.m @@ -0,0 +1,702 @@ +%QUATERNION Quaternion class +% +% A quaternion is a compact method of representing a 3D rotation that has +% computational advantages including speed and numerical robustness. +% A quaternion has 2 parts, a scalar s, and a vector v and is typically +% written: q = s . +% +% A unit-quaternion is one for which s^2+vx^2+vy^2+vz^2 = 1. It can be +% considered as a rotation by an angle theta about a unit-vector V in space where +% +% q = cos (theta/2) < v sin(theta/2)> +% +% Q = Quaternion(X) is a unit-quaternion equivalent to X which can be any +% of: +% - orthonormal rotation matrix. +% - homogeneous transformation matrix (rotation part only). +% - rotation angle and vector +% +% Methods:: +% inv inverse of quaterion +% norm norm of quaternion +% unit unitized quaternion +% plot same options as trplot() +% interp interpolation (slerp) between q and q2, 0<=s<=1 +% scale interpolation (slerp) between identity and q, 0<=s<=1 +% dot derivative of quaternion with angular velocity w +% R equivalent 3x3 rotation matrix +% T equivalent 4x4 homogeneous transform matrix +% +% Arithmetic operators are overloaded:: +% q1==q2 test for quaternion equality +% q1~=q2 test for quaternion inequality +% q+q2 elementwise sum of quaternions +% q-q2 elementwise difference of quaternions +% q*q2 quaternion product +% q*v rotate vector by quaternion, v is 3x1 +% s*q elementwise multiplication of quaternion by scalar +% q/q2 q*q2.inv +% q^n q to power n (integer only) +% +% Properties (read only):: +% s real part +% v vector part +% +% Notes:: +% - Quaternion objects can be used in vectors and arrays +% +% References:: +% - Animating rotation with quaternion curves, +% K. Shoemake, +% in Proceedings of ACM SIGGRAPH, (San Fran cisco), pp. 245-254, 1985. +% - On homogeneous transforms, quaternions, and computational efficiency, +% J. Funda, R. Taylor, and R. Paul, +% IEEE Transactions on Robotics and Automation, vol. 6, pp. 382-388, June 1990. +% - Robotics, Vision & Control, +% P. Corke, Springer 2011. +% +% See also trinterp, trplot. + +% TODO +% properties s, v for the vector case + +% 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 . + +% TODO +% constructor handles R, T trajectory and returns vector +% .r, .t on a quaternion vector?? + +classdef Quaternion + + properties (SetAccess = private) + s % scalar part + v % vector part + end + + methods + + function q = Quaternion(a1, a2) + %Quaternion.Quaternion Constructor for quaternion objects + % + % Construct a quaternion from various other orientation representations. + % + % Q = Quaternion() is the identitity quaternion 1<0,0,0> representing a null rotation. + % + % Q = Quaternion(Q1) is a copy of the quaternion Q1 + % + % Q = Quaternion([S V1 V2 V3]) is a quaternion formed by specifying directly its 4 elements + % + % Q = Quaternion(S) is a quaternion formed from the scalar S and zero vector part: S<0,0,0> + % + % Q = Quaternion(V) is a pure quaternion with the specified vector part: 0 + % + % Q = Quaternion(TH, V) is a unit-quaternion corresponding to rotation of TH about the + % vector V. + % + % Q = Quaternion(R) is a unit-quaternion corresponding to the orthonormal rotation + % matrix R. If R (3x3xN) is a sequence then Q (Nx1) is a vector of Quaternions + % corresponding to the elements of R. + % + % Q = Quaternion(T) is a unit-quaternion equivalent to the rotational + % part of the homogeneous transform T. If T (4x4xN) is a sequence then Q (Nx1) is a + % vector of Quaternions corresponding to the elements of T. + + if nargin == 0 + q.v = [0,0,0]; + q.s = 1; + elseif isa(a1, 'Quaternion') + % Q = Quaternion(q) from another quaternion + q = a1; + elseif nargin == 1 + if isvec(a1, 4) + % Q = Quaternion([s v1 v2 v3]) from 4 elements + a1 = a1(:); + q.s = a1(1); + q.v = a1(2:4)'; + elseif isrot(a1) || ishomog(a1) + % Q = Quaternion(R) from a 3x3 or 4x4 matrix + for i=1:size(a1,3) + q(i) = Quaternion( tr2q(a1(:,:,i)) ); + end + + elseif length(a1) == 3 + % Q = Quaternion(v) from a vector + + q.s = 0; + q.v = a1(:)'; + elseif length(a1) == 1 + % Q = Quaternion(s) from a scalar + q.s = a1(1); + q.v = [0 0 0]; + else + error('unknown dimension of input'); + end + elseif nargin == 2 + if isscalar(a1) && isvector(a2) + % Q = Quaternion(theta, v) from vector plus angle + q.s = cos(a1/2); + q.v = sin(a1/2)*unit(a2(:)'); + else + error ('bad argument to quaternion constructor'); + end + + end + end + + function s = char(q) + %Quaternion.char Convert to string + % + % S = Q.char() is a compact string representation of the quaternion's value + % as a 4-tuple. If Q is a vector then S has one line per element. + + if length(q) > 1 + s = ''; + for qq = q; + s = char(s, char(qq)); + end + return + end + s = [num2str(q.s), ' < ' ... + num2str(q.v(1)) ', ' num2str(q.v(2)) ', ' num2str(q.v(3)) ' >']; + end + + + function display(q) + %Quaternion.display Display the value of a quaternion object + % + % Q.display() displays a compact string representation of the quaternion's value + % as a 4-tuple. If Q is a vector then S has one line per element. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a Quaternion object and the command has no trailing + % semicolon. + % + % See also Quaternion.char. + + + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + if loose + disp(' '); + end + disp(char(q)) + if loose + disp(' '); + end + end + + + function v = double(q) + %Quaternion.double Convert a quaternion to a 4-element vector + % + % V = Q.double() is a 4-vector comprising the quaternion + % elements [s vx vy vz]. + + v = [q.s q.v]; + end + + function qi = inv(q) + %Quaternion.inv Invert a unit-quaternion + % + % QI = Q.inv() is a quaternion object representing the inverse of Q. + + qi = Quaternion([q.s -q.v]); + end + + function qu = unit(q) + %Quaternion.unit Unitize a quaternion + % + % QU = Q.unit() is a unit-quaternion representing the same orientation as Q. + % + % See also Quaternion.norm. + + qu = q / norm(q); + end + + function n = norm(q) + %Quaternion.norm Quaternion magnitude + % + % QN = Q.norm(Q) is the scalar norm or magnitude of the quaternion Q. + % + % Notes:: + % - This is the Euclidean norm of the quaternion written as a 4-vector. + % - A unit-quaternion has a norm of one. + % + % See also Quaternion.unit. + + n = norm(double(q)); + end + + function q = interp(Q1, Q2, r) + %Quaternion.interp Interpolate quaternions + % + % QI = Q1.interp(Q2, S) is a unit-quaternion that interpolates a rotation + % between Q1 for S=0 and Q2 for S=1. + % + % If S is a vector QI is a vector of quaternions, each element + % corresponding to sequential elements of S. + % + % Notes:: + % - This is a spherical linear interpolation (slerp) that can be interpretted + % as interpolation along a great circle arc on a sphere. + % - The value of S is clipped to the interval 0 to 1. + % + % See also ctraj, Quaternion.scale. + + q1 = double(Q1); + q2 = double(Q2); + + theta = acos(q1*q2'); + count = 1; + + % clip values of r + r(r<0) = 0; + r(r>1) = 1; + + if length(r) == 1 + if theta == 0 + q = Q1; + else + q = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ); + end + else + for R=r(:)' + if theta == 0 + qq = Q1; + else + qq = Quaternion( (sin((1-R)*theta) * q1 + sin(R*theta) * q2) / sin(theta) ); + end + q(count) = qq; + count = count + 1; + end + end + end + + + function q = scale(Q, r) + %Quaternion.scale Interpolate rotations expressed by quaternion objects + % + % QI = Q.scale(S) is a unit-quaternion that interpolates between identity for S=0 + % to Q for S=1. This is a spherical linear interpolation (slerp) that can + % be interpretted as interpolation along a great circle arc on a sphere. + % + % If S is a vector QI is a cell array of quaternions, each element + % corresponding to sequential elements of S. + % + % Notes:: + % - This is a spherical linear interpolation (slerp) that can be interpretted + % as interpolation along a great circle arc on a sphere. + % + % See also ctraj, Quaternion.interp. + + + q2 = double(Q); + + if any(r<0) || (r>1) + error('r out of range'); + end + q1 = [1 0 0 0]; % identity quaternion + theta = acos(q1*q2'); + + if length(r) == 1 + if theta == 0 + q = Q; + else + q = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ).unit; + end + else + count = 1; + for R=r(:)' + if theta == 0 + qq = Q; + else + qq = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ).unit; + end + q(count) = qq; + count = count + 1; + end + end + end + + function e = eq(q1, q2) + %EQ Test quaternion equality + % + % Q1==Q2 is true if the quaternions Q1 and Q2 are equal. + % + % Notes:: + % - Overloaded operator '=='. + % - Note that for unit Quaternions Q and -Q are the equivalent + % rotation, so non-equality does not mean rotations are not + % equivalent. + % - If Q1 is a vector of quaternions, each element is compared to + % Q2 and the result is a logical array of the same length as Q1. + % - If Q2 is a vector of quaternions, each element is compared to + % Q1 and the result is a logical array of the same length as Q2. + % - If Q1 and Q2 are vectors of the same length, then the result + % is a logical array + % + % See also Quaternion.ne. + if (numel(q1) == 1) && (numel(q2) == 1) + e = all( eq(q1.double, q2.double) ); + elseif (numel(q1) > 1) && (numel(q2) == 1) + e = zeros(1, numel(q1)); + for i=1:numel(q1) + e(i) = q1(i) == q2; + end + elseif (numel(q1) == 1) && (numel(q2) > 1) + e = zeros(1, numel(q2)); + for i=1:numel(q2) + e(i) = q2(i) == q1; + end + elseif numel(q1) == numel(q2) + e = zeros(1, numel(q1)); + for i=1:numel(q1) + e(i) = q1(i) == q2(i); + end + else + error('RTB:quaternion:badargs'); + end + end + + function e = ne(q1, q2) + %NE Test quaternion inequality + % + % Q1~=Q2 is true if the quaternions Q1 and Q2 are not equal. + % + % Notes:: + % - Overloaded operator '~=' + % - Note that for unit Quaternions Q and -Q are the equivalent + % rotation, so non-equality does not mean rotations are not + % equivalent. + % - If Q1 is a vector of quaternions, each element is compared to + % Q2 and the result is a logical array of the same length as Q1. + % - If Q2 is a vector of quaternions, each element is compared to + % Q1 and the result is a logical array of the same length as Q2. + % - If Q1 and Q2 are vectors of the same length, then the result + % is a logical array. + % + % See also Quaternion.eq. + if (numel(q1) == 1) && (numel(q2) == 1) + e = all( ne(q1.double, q2.double) ); + elseif (numel(q1) > 1) && (numel(q2) == 1) + e = zeros(1, numel(q1)); + for i=1:numel(q1) + e(i) = q1(i) ~= q2; + end + elseif (numel(q1) == 1) && (numel(q2) > 1) + e = zeros(1, numel(q2)); + for i=1:numel(q2) + e(i) = q2(i) ~= q1; + end + elseif numel(q1) == numel(q2) + e = zeros(1, numel(q1)); + for i=1:numel(q1) + e(i) = q1(i) ~= q2(i); + end + else + error('RTB:quaternion:badargs'); + end + end + + function qp = plus(q1, q2) + %PLUS Add quaternions + % + % Q1+Q2 is the element-wise sum of quaternion elements. + % + % Notes:: + % - Overloaded operator '+' + % - The result is not guaranteed to be a unit-quaternion. + % + % See also Quaternion.minus, Quaternion.mtimes. + + if isa(q1, 'Quaternion') && isa(q2, 'Quaternion') + qp = Quaternion(double(q1) + double(q2)); + end + end + + + function qp = minus(q1, q2) + %Quaternion.minus Subtract quaternions + % + % Q1-Q2 is the element-wise difference of quaternion elements. + % + % Notes:: + % - Overloaded operator '-' + % - The result is not guaranteed to be a unit-quaternion. + % + % See also Quaternion.plus, Quaternion.mtimes. + + if isa(q1, 'Quaternion') && isa(q2, 'Quaternion') + + qp = Quaternion(double(q1) - double(q2)); + end + end + + function qp = mtimes(q1, q2) + %Quaternion.mtimes Multiply a quaternion object + % + % Q1*Q2 is a quaternion formed by the Hamilton product of two quaternions. + % Q*V is a vector formed by rotating the vector V by the quaternion Q. + % Q*S is the element-wise multiplication of quaternion elements by the scalar S. + % + % Notes:: + % - Overloaded operator '*' + % + % See also Quaternion.mrdivide, Quaternion.mpower, Quaternion.plus, Quaternion.minus. + + if isa(q1, 'Quaternion') && isa(q2, 'Quaternion') + %QQMUL Multiply unit-quaternion by unit-quaternion + % + % QQ = qqmul(Q1, Q2) + % + % Return a product of unit-quaternions. + % + % See also: TR2Q + + + % decompose into scalar and vector components + s1 = q1.s; v1 = q1.v; + s2 = q2.s; v2 = q2.v; + + % form the product + qp = Quaternion([s1*s2-v1*v2' s1*v2+s2*v1+cross(v1,v2)]); + + elseif isa(q1, 'Quaternion') && isa(q2, 'double') + + %QVMUL Multiply vector by unit-quaternion + % + % VT = qvmul(Q, V) + % + % Rotate the vector V by the unit-quaternion Q. + % + % See also: QQMUL, QINV + + if length(q2) == 3 + qp = q1 * Quaternion([0 q2(:)']) * inv(q1); + qp = qp.v(:); + elseif length(q2) == 1 + qp = Quaternion( double(q1)*q2); + else + error('quaternion-vector product: must be a 3-vector or scalar'); + end + + elseif isa(q2, 'Quaternion') && isa(q1, 'double') + if length(q1) == 3 + qp = q2 * Quaternion([0 q1(:)']) * inv(q2); + qp = qp.v; + elseif length(q1) == 1 + qp = Quaternion( double(q2)*q1); + else + error('quaternion-vector product: must be a 3-vector or scalar'); + end + end + end + + function qp = mpower(q, p) + %Quaternion.mpower Raise quaternion to integer power + % + % Q^N is the quaternion Q raised to the integer power N. + % + % Notes:: + % - Overloaded operator '^' + % - Computed by repeated multiplication. + % + % See also Quaternion.mrdivide, Quaternion.mpower, Quaternion.plus, Quaternion.minus. + + % check that exponent is an integer + if (p - floor(p)) ~= 0 + error('quaternion exponent must be integer'); + end + + qp = q; + + % multiply by itself so many times + for i = 2:abs(p) + qp = qp * q; + end + + % if exponent was negative, invert it + if p<0 + qp = inv(qp); + end + end + + function qq = mrdivide(q1, q2) + %Quaternion.mrdivide Quaternion quotient. + % + % Q1/Q2 is a quaternion formed by Hamilton product of Q1 and inv(Q2). + % Q/S is the element-wise division of quaternion elements by the scalar S. + % + % Notes:: + % - Overloaded operator '/' + % + % See also Quaternion.mtimes, Quaternion.mpower, Quaternion.plus, Quaternion.minus. + + if isa(q2, 'Quaternion') + % qq = q1 / q2 + % = q1 * qinv(q2) + + qq = q1 * inv(q2); + elseif isa(q2, 'double') + qq = Quaternion( double(q1) / q2 ); + end + end + + + function plot(Q, varargin) + %Quaternion.plot Plot a quaternion object + % + % Q.plot(options) plots the quaternion as a rotated coordinate frame. + % + % Options:: + % Options are passed to trplot and include: + % + % 'color',C The color to draw the axes, MATLAB colorspec C + % 'frame',F The frame is named {F} and the subscript on the axis labels is F. + % 'view',V Set plot view parameters V=[az el] angles, or 'auto' + % for view toward origin of coordinate frame + % + % See also trplot. + + %axis([-1 1 -1 1 -1 1]) + + trplot( Q.R, varargin{:}); + drawnow + end + + function r = R(q) + %Quaternion.R Convert toorthonormal rotation matrix + % + % R = Q.R() is the equivalent 3x3 orthonormal rotation matrix. + % + % Notes: + % - For a quaternion sequence returns a rotation matrix sequence. + r = zeros(3,3,numel(q)); + for i=1:numel(q) + r(:,:,i) = t2r( q2tr(q(i)) ); + end + end + + function t = T(q) + %Quaternion.T Convert to homogeneous transformation matrix + % + % T = Q.T() is the equivalent 4x4 homogeneous transformation matrix. + % + % Notes: + % - For a quaternion sequence returns a homogeneous transform matrix sequence + % - Has a zero translational component. + t = zeros(4,4,numel(q)); + for i=1:numel(q) + t(:,:,i) = q2tr(q(i)); + end + end + + function qd = dot(q, omega) + E = q.s*eye(3,3) - skew(q.v); + omega = omega(:); + qd = Quaternion([-0.5*q.v*omega; 0.5*E*omega]); + end + end % methods +end % classdef + +%TR2Q Convert homogeneous transform to a unit-quaternion +% +% Q = tr2q(T) +% +% Return a unit-quaternion corresponding to the rotational part of the +% homogeneous transform T. +% +% See also: Q2TR + +function q = tr2q(t) + + if ishomog(t) + t = t2r(t); + end + qs = sqrt(trace(t)+1)/2.0; + kx = t(3,2) - t(2,3); % Oz - Ay + ky = t(1,3) - t(3,1); % Ax - Nz + kz = t(2,1) - t(1,2); % Ny - Ox + + if (t(1,1) >= t(2,2)) && (t(1,1) >= t(3,3)) + kx1 = t(1,1) - t(2,2) - t(3,3) + 1; % Nx - Oy - Az + 1 + ky1 = t(2,1) + t(1,2); % Ny + Ox + kz1 = t(3,1) + t(1,3); % Nz + Ax + add = (kx >= 0); + elseif (t(2,2) >= t(3,3)) + kx1 = t(2,1) + t(1,2); % Ny + Ox + ky1 = t(2,2) - t(1,1) - t(3,3) + 1; % Oy - Nx - Az + 1 + kz1 = t(3,2) + t(2,3); % Oz + Ay + add = (ky >= 0); + else + kx1 = t(3,1) + t(1,3); % Nz + Ax + ky1 = t(3,2) + t(2,3); % Oz + Ay + kz1 = t(3,3) - t(1,1) - t(2,2) + 1; % Az - Nx - Oy + 1 + add = (kz >= 0); + end + + if add + kx = kx + kx1; + ky = ky + ky1; + kz = kz + kz1; + else + kx = kx - kx1; + ky = ky - ky1; + kz = kz - kz1; + end + nm = norm([kx ky kz]); + if nm == 0 + q = Quaternion([1 0 0 0]); + else + s = sqrt(1 - qs^2) / nm; + qv = s*[kx ky kz]; + + q = Quaternion([qs qv]); + + end +end + + +%Q2TR Convert unit-quaternion to homogeneous transform +% +% T = q2tr(Q) +% +% Return the rotational homogeneous transform corresponding to the unit +% quaternion Q. +% +% See also: TR2Q + +function t = q2tr(q) + + q = double(q); + s = q(1); + x = q(2); + y = q(3); + z = q(4); + + r = [ 1-2*(y^2+z^2) 2*(x*y-s*z) 2*(x*z+s*y) + 2*(x*y+s*z) 1-2*(x^2+z^2) 2*(y*z-s*x) + 2*(x*z-s*y) 2*(y*z+s*x) 1-2*(x^2+y^2) ]; + t = eye(4,4); + t(1:3,1:3) = r; + t(4,4) = 1; +end diff --git a/lwrserv/matlab/rvctools/robot/README b/lwrserv/matlab/rvctools/robot/README new file mode 100644 index 0000000..f67824e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/README @@ -0,0 +1,55 @@ +Installation: +------------ + +simply copy all the .m files into a directory 'robot' + +add the directory robot to your MATLABPATH + +start MATLAB + +>> startup_rvc + +Help: +---- + +1: manual + +robot.pdf is the documentation in standard Mathworks style. It is +formatted for double sided printing which makes for a more compact +manual and saves trees. + +2: desktop help GUI +from the Matlab desktop, choose help, and select the Robotics Toolbox +option. HTML format documentation for all functions is available. + +3: command line +use the help command from the command prompt, eg. help ikine + +4: the web + +http://www.petercorke.com/RTB/r9/html + + +Demos: +----- + +1: invoke the demo GUI +from the command prompt, >> demos, then select the Robotics Toolbox + +2: desktop menu bar +from the menu bar choose demos, and select the Robotics Toolbox + +3: command prompt + >> rtdemos + + +Online resources: +---------------- + +Home page: http://www.petercorke.com +Discussion group: http://groups.google.com/group/robotics-tool-box?hl=en +Manual pages: http://www.petercorke.com/RTB/r9/html + +Please email bug reports, comments or code contribtions to me at rvc@petercorke.com + + Peter Corke diff --git a/lwrserv/matlab/rvctools/robot/RELEASE b/lwrserv/matlab/rvctools/robot/RELEASE new file mode 100644 index 0000000..021debd --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/RELEASE @@ -0,0 +1 @@ +9.8 diff --git a/lwrserv/matlab/rvctools/robot/RRT.m b/lwrserv/matlab/rvctools/robot/RRT.m new file mode 100644 index 0000000..45bb4cb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/RRT.m @@ -0,0 +1,484 @@ +%RRT Class for rapidly-exploring random tree navigation +% +% A concrete subclass of the Navigation class that implements the rapidly +% exploring random tree (RRT) algorithm. This is a kinodynamic planner +% that takes into account the motion constraints of the vehicle. +% +% Methods:: +% +% plan Compute the tree +% path Compute a path +% plot Display the tree +% display Display the parameters in human readable form +% char Convert to string +% +% Example:: +% +% goal = [0,0,0]; +% start = [0,2,0]; +% veh = Vehicle([], 'stlim', 1.2); +% rrt = RRT([], veh, 'goal', goal, 'range', 5); +% rrt.plan() % create navigation tree +% rrt.path(start, goal) % animate path from this start location +% +% Robotics, Vision & Control compatability mode: +% goal = [0,0,0]; +% start = [0,2,0]; +% rrt = RRT(); % create navigation object +% rrt.plan() % create navigation tree +% rrt.path(start, goal) % animate path from this start location +% +% References:: +% - Randomized kinodynamic planning, +% S. LaValle and J. Kuffner, +% International Journal of Robotics Research vol. 20, pp. 378-400, May 2001. +% - Probabilistic roadmaps for path planning in high dimensional configuration spaces, +% L. Kavraki, P. Svestka, J. Latombe, and M. Overmars, +% IEEE Transactions on Robotics and Automation, vol. 12, pp. 566-580, Aug 1996. +% - Robotics, Vision & Control, Section 5.2.5, +% P. Corke, Springer 2011. +% +% See also Navigation, PRM, DXform, Dstar, PGraph. + +% Peter Corke 8/2009. + +%TODO +% more info to the display method +% distance metric choice or weightings +% pass time and model options to the simulation + +classdef RRT < Navigation + + properties + npoints % number of points to find + graph % graph Object representing random nodes + + sim_time % path simulation time + kine_model % simulink model for kinematics + + xrange + yrange + + plant + + speed + steermax + vehicle + end + + methods + + function rrt = RRT(map, vehicle, varargin) + %RRT.RRT Create a RRT navigation object + % + % R = RRT.RRT(MAP, VEH, OPTIONS) is a rapidly exploring tree navigation + % object for a region with obstacles defined by the map object MAP. + % + % R = RRT.RRT() as above but internally creates a Vehicle class object + % and does not support any MAP or OPTIONS. For compatibility with + % RVC book. + % + % Options:: + % 'npoints',N Number of nodes in the tree + % 'time',T Period to simulate dynamic model toward random point + % 'range',R Specify rectangular bounds + % - R scalar; X: -R to +R, Y: -R to +R + % - R (1x2); X: -R(1) to +R(1), Y: -R(2) to +R(2) + % - R (1x4); X: R(1) to R(2), Y: R(3) to R(4) + % 'goal',P Goal position (1x2) or pose (1x3) in workspace + % 'speed',S Speed of vehicle [m/s] (default 1) + % 'steermax',S Maximum steer angle of vehicle [rad] (default 1.2) + % + % Notes:: + % - Does not (yet) support obstacles, ie. MAP is ignored but must be given. + % - 'steermax' selects the range of steering angles that the vehicle will + % be asked to track. If not given the steering angle range of the vehicle + % will be used. + % - There is no check that the steering range or speed is within the limits + % of the vehicle object. + % + % Reference:: + % - Robotics, Vision & Control + % Peter Corke, Springer 2011. p102. + % + % See also Vehicle. + + % invoke the superclass constructor + rrt = rrt@Navigation(varargin{:}); + + rrt.graph = PGraph(3, 'distance', 'SE2'); % graph of points in SE(2) + if nargin == 0 + rrt.vehicle = Vehicle([], 'stlim', 1.2); + else + % RVC book compatability mode + rrt.vehicle = vehicle; + end + + opt.npoints = 500; + opt.time = 0.5; + opt.range = 5; + opt.goal = [0, 0, 0]; + opt.steermax = []; + opt.speed = 1; + + [opt,args] = tb_optparse(opt, varargin); + rrt.npoints = opt.npoints; + rrt.sim_time = opt.time; + + switch length(opt.range) + case 1 + rrt.xrange = [-opt.range opt.range]; + rrt.yrange = [-opt.range opt.range]; + case 2 + rrt.xrange = [-opt.range(1) opt.range(1)]; + rrt.yrange = [-opt.range(2) opt.range(2)]; + case 4 + rrt.xrange = [opt.range(1) opt.range(2)]; + rrt.yrange = [opt.range(3) opt.range(4)]; + otherwise + error('bad range specified'); + end + if ~isempty(opt.steermax) + rrt.steermax = opt.steermax; + else + rrt.steermax = rrt.vehicle.alphalim; + end + rrt.speed = opt.speed; + rrt.goal = opt.goal; + end + + function plan(rrt, varargin) + %RRT.plan Create a rapidly exploring tree + % + % R.plan(OPTIONS) creates the tree roadmap by driving the vehicle + % model toward random goal points. The resulting graph is kept + % within the object. + % + % Options:: + % 'goal',P Goal pose (1x3) + % 'noprogress' Don't show the progress bar + % 'samples' Show samples + % - '.' for each random point x_rand + % - 'o' for the nearest point which is added to the tree + % - red line for the best path + + opt.progress = true; + opt.samples = false; + opt.goal = []; + + opt = tb_optparse(opt, varargin); + + if ~isempty(opt.goal) + rrt.goal = opt.goal; + end + + % build a graph over the free space + rrt.message('create the graph'); + rrt.graph.clear(); + + if rrt.verbose + clf + %idisp(1-rrt.occgrid, 'ynormal', 'nogui'); + hold on + end + + % check goal sanity + if isempty(rrt.goal) + error('no goal specified'); + end + switch length(rrt.goal) + case 2 + rrt.goal = [rrt.goal(:); 0]; + case 3 + otherwise + error('goal must be 3-vector'); + end + + % add the goal point as the first node + rrt.graph.add_node(rrt.goal); + + % graphics setup + if opt.progress + h = waitbar(0, 'RRT planning...'); + end + if opt.samples + clf + hold on + xlabel('x'); ylabel('y'); + end + + for j=1:rrt.npoints % build the tree + + if opt.progress + waitbar(j / rrt.npoints); + end + + % Step 3 + % find random state x,y + + % pick a point not in obstacle + while true + xy = rrt.randxy(); % get random coordinate (x,y) + + % test if lies in the obstacle map (if it exists) + if isempty(rrt.occgrid) + break; + end + try + if rrt.occgrid(ixy(2),ixy(1)) == 0 + break; + end + catch + % index error, point must be off the map + continue; + end + end + theta = rrt.rand*2*pi; + xrand = [xy, theta]'; + if opt.samples + plot(xy(1), xy(2), '.') + end + + % Step 4 + % find the existing node closest in state space + + vnear = rrt.graph.closest(xrand); % nearest vertex + xnear = rrt.graph.coord(vnear); % coord of nearest vertex + + rrt.message('xrand (%g, %g) node %d', xy, vnear); + + % Step 5 + % figure how to drive the robot from xnear to xrand + + ntrials = 50; + + best = rrt.bestpath(xnear, xrand, ntrials); + + xnew = best.path(:,best.k); + if opt.samples + plot(xnew(1), xnew(2), 'o'); + plot2(best.path', 'r'); + drawnow + end + +% % ensure that the path is collision free +% if ~rrt.clearpath(y(:,1:2)) +% disp('path collision'); +% continue; +% end + + % Step 7,8 + % add xnew to the graph, with an edge from xnear + v = rrt.graph.add_node(xnew); + rrt.graph.add_edge(vnear, v); + + rrt.graph.setdata(v, best); + end + + if opt.progress + close(h) + end + rrt.message('graph create done'); + end + + function p_ = path(rrt, xstart, xgoal) + %PRM.path Find a path between two points + % + % X = R.path(START, GOAL) finds a path (Nx3) from state START (1x3) + % to the GOAL (1x3). + % + % P.path(START, GOAL) as above but plots the path in 3D. The nodes + % are shown as circles and the line segments are blue for forward motion + % and red for backward motion. + % + % Notes:: + % - The path starts at the vertex closest to the START state, and ends + % at the vertex closest to the GOAL state. If the tree is sparse this + % might be a poor approximation to the desired start and end. + + g = rrt.graph; + vstart = g.closest(xstart); + vgoal = g.closest(xgoal); + + % find path through the graph using A* search + path = g.Astar(vstart, vgoal); + + % concatenate the vehicle motion segments + cpath = []; + for i = 1:length(path) + p = path(i); + data = g.data(p); + if ~isempty(data) + if i >= length(path) || g.edgedir(p, path(i+1)) > 0 + cpath = [cpath data.path]; + else + cpath = [cpath data.path(:,end:-1:1)]; + + end + end + end + + if nargout == 0 + % plot the path + clf; hold on + + plot2(g.coord(path)', 'o'); % plot the node coordinates + + for i = 1:length(path) + p = path(i) + b = g.data(p); % get path data for segment + + % draw segment with direction dependent color + if ~isempty(b) + % if the vertex has a path leading to it + + if i >= length(path) || g.edgedir(p, path(i+1)) > 0 + % positive edge + % draw from prev vertex to end of path + seg = [g.coord(path(i-1)) b.path]'; + else + % negative edge + % draw reverse path to next next vertex + seg = [ b.path(:,end:-1:1) g.coord(path(i+1))]'; + end + + if b.vel > 0 + plot2(seg, 'b'); + else + plot2(seg, 'r'); + end + end + end + + xlabel('x'); ylabel('y'); zlabel('\theta'); + grid + else + p_ = cpath'; + end + end + + function plot(rrt, varargin) + %RRT.plot Visualize navigation environment + % + % R.plot() displays the navigation tree in 3D. + + clf + rrt.graph.plot('noedges', 'NodeSize', 6, 'NodeFaceColor', 'g', 'NodeEdgeColor', 'g', 'edges'); + + hold on + for i=2:rrt.graph.n + b = rrt.graph.data(i); + plot2(b.path(:,1:b.k)') + end + xlabel('x'); ylabel('y'); zlabel('\theta'); + grid; hold off + end + + % required by abstract superclass + function next(rrt) + end + + function s = char(rrt) + %RRT.char Convert to string + % + % R.char() is a string representing the state of the RRT + % object in human-readable form. + % + % invoke the superclass char() method + s = char@Navigation(rrt); + + % add RRT specific stuff information + s = char(s, sprintf(' region: X %f : %f; Y %f : %f', rrt.xrange, rrt.yrange)); + s = char(s, sprintf(' path time: %f', rrt.sim_time)); + s = char(s, sprintf(' graph size: %d nodes', rrt.npoints)); + s = char(s, char(rrt.graph) ); + if ~isempty(rrt.vehicle) + s = char(s, char(rrt.vehicle) ); + end + end + + function test(rrt) + xy = rrt.randxy() + end + + + end % methods + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% P R I V A T E M E T H O D S +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + methods (Access='protected') + + function best = bestpath(rrt, x0, xg, N) + + % initial and final state as column vectors + x0 = x0(:); xg = xg(:); + + best.d = Inf; + for i=1:N % for multiple trials + + %choose random direction of motion and random steer angle + if rand > 0.5 + vel = rrt.speed; + else + vel = -rrt.speed; + end + steer = (2*rrt.rand - 1) * rrt.steermax; % uniformly distributed + + % simulate motion of vehicle for this speed and steer angle which + % results in a path + x = rrt.vehicle.run2(rrt.sim_time, x0, vel, steer)'; + + %% find point on the path closest to xg + % distance of all path points from goal + d = colnorm( [bsxfun(@minus, x(1:2,:), xg(1:2)); angdiff(x(3,:), xg(3))] ); + % the closest one + [dmin,k] = min(d); + + % is it the best so far? + if dmin < best.d + % yes it is! save it and the inputs that led to it + best.d = dmin; + best.path = x; + best.steer = steer; + best.vel = vel; + best.k = k; + end + end + end + + % generate a random coordinate within the working region + function xy = randxy(rrt) + xy = rrt.rand(1,2) .* [rrt.xrange(2)-rrt.xrange(1) rrt.yrange(2)-rrt.yrange(1)] + ... + [rrt.xrange(1) rrt.yrange(1)]; + end + + % test if a path is obstacle free + function c = clearpath(rrt, xy) + if isempty(rrt.occgrid) + c = true; + return; + end + + xy = round(xy); + try + % test that all points along the path do lie within an obstacle + for pp=xy' + if rrt.occgrid(pp(2), pp(1)) > 0 + c = false; + return; + end + end + c = true; + catch + % come here if we index out of bounds + c = false; + return; + end + end + + + end % private methods +end % class diff --git a/lwrserv/matlab/rvctools/robot/RandomPath.m b/lwrserv/matlab/rvctools/robot/RandomPath.m new file mode 100644 index 0000000..9a39999 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/RandomPath.m @@ -0,0 +1,191 @@ +%RandomPath Vehicle driver class +% +% Create a "driver" object capable of driving a Vehicle object through random +% waypoints within a rectangular region and at constant speed. +% +% The driver object is attached to a Vehicle object by the latter's +% add_driver() method. +% +% Methods:: +% init reset the random number generator +% demand return speed and steer angle to next waypoint +% display display the state and parameters in human readable form +% char convert to string +% +% Properties:: +% goal current goal coordinate +% veh the Vehicle object being controlled +% dim dimensions of the work space (2x1) [m] +% speed speed of travel [m/s] +% closeenough proximity to waypoint at which next is chosen [m] +% +% Example:: +% +% veh = Vehicle(V); +% veh.add_driver( RandomPath(20, 2) ); +% +% Notes:: +% - It is possible in some cases for the vehicle to move outside the desired +% region, for instance if moving to a waypoint near the edge, the limited +% turning circle may cause the vehicle to temporarily move outside. +% - The vehicle chooses a new waypoint when it is closer than property +% closeenough to the current waypoint. +% - Uses its own random number stream so as to not influence the performance +% of other randomized algorithms such as path planning. +% +% Reference:: +% +% Robotics, Vision & Control, Chap 6, +% Peter Corke, +% Springer 2011 +% +% See also Vehicle. + +% 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 . + +classdef RandomPath < handle + properties + goal % current goal + h_goal % graphics handle for goal + veh % the vehicle we are driving + dim + speed % speed of travel + closeenough % proximity to goal before + d_prev + randstream % random stream just for Sensors + end + + methods + + function driver = RandomPath(dim, varargin) + %RandomPath.RandomPath Create a driver object + % + % D = RandomPath(DIM, OPTIONS) returns a "driver" object capable of driving + % a Vehicle object through random waypoints. The waypoints are positioned + % inside a rectangular region bounded by +/- DIM in the x- and y-directions. + % + % Options:: + % 'speed',S Speed along path (default 1m/s). + % 'dthresh',D Distance from goal at which next goal is chosen. + % + % See also Vehicle. + + % TODO options to specify region, maybe accept a Map object? + + driver.dim = dim; + + opt.speed = 1; + opt.dthresh = 0.05 * dim; + opt = tb_optparse(opt, varargin); + + driver.speed = opt.speed; + driver.closeenough = opt.dthresh; + drive.d_prev = Inf; + driver.randstream = RandStream.create('mt19937ar'); + end + + function init(driver) + %RandomPath.init Reset random number generator + % + % R.init() resets the random number generator used to create the waypoints. + % This enables the sequence of random waypoints to be repeated. + % + % See also RANDSTREAM. + driver.goal = []; + driver.randstream.reset(); + end + + % not used + function visualize(driver) + clf + d = driver.dim; + axis([-d d -d d]); + hold on + xlabel('x'); + ylabel('y'); + end + + % private method, invoked from demand() to compute a new waypoint + function setgoal(driver) + r = driver.randstream.rand(2,1); + driver.goal = 0.8 * driver.dim * (r - 0.5)*2; + %fprintf('set goal: (%.1f %.1f)\n', driver.goal); + if isempty(driver.h_goal) + %driver.h_goal = plot(driver.goal(1), driver.goal(2), '*') + else + %set(driver.h_goal, 'Xdata', driver.goal(1), 'Ydata', driver.goal(2)) + end + end + + function [speed, steer] = demand(driver) + %RandomPath.demand Compute speed and heading to waypoint + % + % [SPEED,STEER] = R.demand() returns the speed and steer angle to + % drive the vehicle toward the next waypoint. When the vehicle is + % within R.closeenough a new waypoint is chosen. + % + % See also Vehicle. + if isempty(driver.goal) + driver.setgoal() + end + + speed = driver.speed; + + goal_heading = atan2(driver.goal(2)-driver.veh.x(2), ... + driver.goal(1)-driver.veh.x(1)); + d_heading = angdiff(goal_heading, driver.veh.x(3)); + + steer = d_heading; + + % if nearly at goal point, choose the next one + d = colnorm(driver.veh.x(1:2) - driver.goal); + if d < driver.closeenough + driver.setgoal(); + elseif d > driver.d_prev + driver.setgoal(); + end + driver.d_prev = d; + end + + function display(driver) + %RandomPath.display Display driver parameters and state + % + % R.display() displays driver parameters and state in compact + % human readable form. + % + % See also RandomPath.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(driver) ); + end % display() + + function s = char(driver) + %RandomPath.char Convert to string + % + % s = R.char() is a string showing driver parameters and state in in + % a compact human readable format. + s = 'RandomPath driver object'; + s = char(s, sprintf(' current goal=(%g,%g), dimension %.1f', ... + driver.goal, driver.dim)); + end + + end % methods +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/RangeBearingSensor.m b/lwrserv/matlab/rvctools/robot/RangeBearingSensor.m new file mode 100644 index 0000000..6187aab --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/RangeBearingSensor.m @@ -0,0 +1,360 @@ +%RangeBearingSensor Range and bearing sensor class +% +% A concrete subclass of the Sensor class that implements a range and bearing +% angle sensor that provides robot-centric measurements of point features in +% the world. To enable this it has references to a map of the world (Map object) +% and a robot moving through the world (Vehicle object). +% +% Methods:: +% +% reading range/bearing observation of random feature +% h range/bearing observation of specific feature +% Hx Jacobian matrix dh/dxv +% Hxf Jacobian matrix dh/dxf +% Hw Jacobian matrix dh/dw +% +% g feature positin given vehicle pose and observation +% Gx Jacobian matrix dg/dxv +% Gz Jacobian matrix dg/dz +% +% Properties (read/write):: +% W measurement covariance matrix (2x2) +% interval valid measurements returned every interval'th call to reading() +% +% Reference:: +% +% Robotics, Vision & Control, Chap 6, +% Peter Corke, +% Springer 2011 +% +% See also Sensor, Vehicle, Map, EKF. + +% 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 . + +classdef RangeBearingSensor < Sensor + + properties + W % measurment covariance + r_range % range limits + theta_range % angle limits + + randstream % random stream just for Sensors + + end + + properties (SetAccess = private) + count % number of reading()s + end + + methods + + function s = RangeBearingSensor(robot, map, W, varargin) + %RangeBearingSensor.RangeBearingSensor Range and bearing sensor constructor + % + % S = RangeBearingSensor(VEHICLE, MAP, W, OPTIONS) is an object representing + % a range and bearing angle sensor mounted on the Vehicle object + % VEHICLE and observing an environment of known landmarks represented by the + % map object MAP. The sensor covariance is R (2x2) representing range and bearing + % covariance. + % + % Options:: + % 'range', xmax maximum range of sensor + % 'range', [xmin xmax] minimum and maximum range of sensor + % 'angle', TH detection for angles betwen -TH to +TH + % 'angle', [THMIN THMAX] detection for angles betwen THMIN + % and THMAX + % 'skip', I return a valid reading on every I'th + % call + % 'fail', [TMIN TMAX] sensor simulates failure between + % timesteps TMIN and TMAX + % + % See also Sensor, Vehicle, Map, EKF. + + + % call the superclass constructor + s = s@Sensor(robot, map, varargin{:}); + + s.randstream = RandStream.create('mt19937ar'); + + opt.range = []; + opt.thrange = []; + + [opt,args] = tb_optparse(opt, varargin); + + s.W = W; + if ~isempty(opt.range) + if length(opt.range) == 1 + s.r_range = [0 opt.range]; + elseif length(opt.thrange) == 2 + s.r_range = opt.range; + end + end + if ~isempty(opt.thrange) + if length(opt.thrange) == 1 + s.theta_range = [-opt.thrange opt.thrange]; + elseif length(opt.thrange) == 2 + s.theta_range = opt.thrange; + end + end + + s.count = 0; + end + + function k = selectFeature(s) + k = s.randstream.randi(s.map.nfeatures); + end + + function [z,jf] = reading(s) + %RangeBearingSensor.h Landmark range and bearing + % + % [Z,K] = S.reading() is an observation of a random landmark where + % Z=[R,THETA] is the range and bearing with additive Gaussian noise + % of covariance R (specified to the constructor). K is the index of + % the map feature that was observed. If no valid measurement, ie. no + % features within range, interval subsampling enabled or simulated + % failure the return is Z=[] and K=NaN. + % + % See also RangeBearingSensor.h. + + % model a sensor that emits readings every interval samples + s.count = s.count + 1; + + % check conditions for NOT returning a value + z = []; + jf = NaN; + % sample interval + if mod(s.count, s.interval) ~= 0 + return; + end + % simulated failure + if ~isempty(s.fail) && (s.count >= s.fail(1)) && (s.count <= s.fail(2)) + return; + end + + if ~isempty(s.r_range) || ~isempty(s.theta_range) + % if range and bearing angle limits are in place look for + % any landmarks that match criteria + + % get range/bearing to all landmarks + z = s.h(s.robot.x'); + jf = 1:numcols(s.map.map); + + if ~isempty(s.r_range) + % find all within range + k = find(z(:,1) >= s.r_range(1) & z(:,1) <= s.r_range(2)); + z = z(k,:); + jf = jf(k); + end + if ~isempty(s.theta_range) + % find all within angular range + k = find(z(:,2) >= s.theta_range(1) & z(:,2) <= s.theta_range(2)); + z = z(k,:); + jf = jf(k); + end + + if isempty(k) + % no landmarks found + z = []; + jf = NaN; + elseif length(k) >= 1 + % more than 1 in range, pick a random one + i = s.randstream.randi(length(k)); + z = z(i,:); + jf = jf(i); + end + + else + % randomly choose the feature + jf = s.selectFeature(); + + % compute the range and bearing from robot to feature + z = s.h(s.robot.x', jf); + end + + if s.verbose + fprintf('Sensor:: feature %d: %.1f %.1f\n', k, z); + end + if ~isempty(z) & s.animate + s.plot(jf); + end + end + + + function z = h(s, xv, jf) + %RangeBearingSensor.h Landmark range and bearing + % + % Z = S.h(XV, J) is a sensor observation (1x2), range and bearing, from vehicle at + % pose XV (1x3) to the map feature K. + % + % Z = S.h(XV, XF) as above but compute range and bearing to a feature at coordinate XF. + % + % Z = s.h(XV) as above but computer range and bearing to all + % map features. Z has one row per feature. + % + % Notes:: + % - Noise with covariance W is added to each row of Z. + % - Supports vectorized operation where XV (Nx3) and Z (Nx2). + % + % See also RangeBearingSensor.Hx, RangeBearingSensor.Hw, RangeBearingSensor.Hxf. + + if nargin < 3 + % s.h(XV) + xf = s.map.map; + elseif length(jf) == 1 + % s.h(XV, JF) + xf = s.map.map(:,jf); + else + % s.h(XV, XF) + xf = jf; + end + + % Straightforward code: + % + % dx = xf(1) - xv(1); dy = xf(2) - xv(2); + % + % z = zeros(2,1); + % z(1) = sqrt(dx^2 + dy^2); % range measurement + % z(2) = atan2(dy, dx) - xv(3); % bearing measurement + % + % Vectorized code: + + dx = xf(1,:) - xv(:,1); dy = xf(2,:) - xv(:,2); + z = [sqrt(dx.^2 + dy.^2) atan2(dy, dx)-xv(:,3) ]; % range & bearing measurement + % add noise with covariance W + z = z + s.randstream.randn(size(z)) * sqrt(s.W); + end + + function J = Hx(s, xv, jf) + %RangeBearingSensor.Hx Jacobian dh/dxv + % + % J = S.Hx(XV, K) returns the Jacobian dh/dxv (2x3) at the vehicle + % state XV (3x1) for map feature K. + % + % J = S.Hx(XV, XF) as above but for a feature at coordinate XF. + % + % See also RangeBearingSensor.h. + if length(jf) == 1 + xf = s.map.map(:,jf); + else + xf = jf; + end + if isempty(xv) + xv = s.robot.x; + end + Delta = xf - xv(1:2)'; + r = norm(Delta); + J = [ + -Delta(1)/r, -Delta(2)/r, 0 + Delta(2)/(r^2), -Delta(1)/(r^2), -1 + ]; + end + + function J = Hxf(s, xv, jf) + %RangeBearingSensor.Hxf Jacobian dh/dxf + % + % J = S.Hxf(XV, K) is the Jacobian dh/dxv (2x2) at the vehicle + % state XV (3x1) for map feature K. + % + % J = S.Hxf(XV, XF) as above but for a feature at coordinate XF (1x2). + % + % See also RangeBearingSensor.h. + if length(jf) == 1 + xf = s.map.map(:,jf); + else + xf = jf; + end + Delta = xf - xv(1:2)'; + r = norm(Delta); + J = [ + Delta(1)/r, Delta(2)/r + -Delta(2)/(r^2), Delta(1)/(r^2) + ]; + end + + function J = Hw(s, xv, jf) + %RangeBearingSensor.Hx Jacobian dh/dv + % + % J = S.Hw(XV, K) is the Jacobian dh/dv (2x2) at the vehicle + % state XV (3x1) for map feature K. + % + % See also RangeBearingSensor.h. + J = eye(2,2); + end + + function xf = g(s, xv, z) + %RangeBearingSensor.g Compute landmark location + % + % P = S.g(XV, Z) is the world coordinate (1x2) of a feature given + % the sensor observation Z (1x2) and vehicle state XV (3x1). + % + % See also RangeBearingSensor.Gx, RangeBearingSensor.Gz. + + range = z(1); + bearing = z(2) + xv(3); % bearing angle in vehicle frame + + xf = [xv(1)+range*cos(bearing) xv(2)+range*sin(bearing)]; + end + + function J = Gx(s, xv, z) + %RangeBearingSensor.Gxv Jacobian dg/dx + % + % J = S.Gx(XV, Z) is the Jacobian dg/dxv (2x3) at the vehicle state XV (3x1) for + % sensor observation Z (2x1). + % + % See also RangeBearingSensor.g. + theta = xv(3); + r = z(1); + bearing = z(2); + J = [ + 1, 0, -r*sin(theta + bearing); + 0, 1, r*cos(theta + bearing) + ]; + end + + + function J = Gz(s, xv, z) + %RangeBearingSensor.Gz Jacobian dg/dz + % + % J = S.Gz(XV, Z) is the Jacobian dg/dz (2x2) at the vehicle state XV (3x1) for + % sensor observation Z (2x1). + % + % See also RangeBearingSensor.g. + theta = xv(3); + r = z(1); + bearing = z(2); + J = [ + cos(theta + bearing), -r*sin(theta + bearing); + sin(theta + bearing), r*cos(theta + bearing) + ]; + end + + function str = char(s) + str = char@Sensor(s); + str = char(str, ['W = ', mat2str(s.W, 3)]); + + str = char(str, sprintf('interval %d samples', s.interval) ); + if ~isempty(s.r_range) + str = char(str, sprintf('range: %g to %g', s.r_range) ); + end + if ~isempty(s.theta_range) + str = char(str, sprintf('angle: %g to %g', s.r_range) ); + end + end + + end % method +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/Revolute.m b/lwrserv/matlab/rvctools/robot/Revolute.m new file mode 100644 index 0000000..ac63a6c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Revolute.m @@ -0,0 +1,52 @@ +%Revolute Robot manipulator Revolute link class +% +% A subclass of the Link class: holds all information related to a robot +% link such as kinematics parameters, rigid-body inertial parameters, motor +% and transmission parameters. +% +% Notes:: +% - This is reference class object +% - Link class objects can be used in vectors and arrays +% +% References:: +% - Robotics, Vision & Control, Chap 7 +% P. Corke, Springer 2011. +% +% See also Link, Prismatic, SerialLink. + +% 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 . + +classdef Revolute < Link + methods + function L = Revolute(varargin) + L = L@Link(varargin{:}); + + if nargin == 0 + L.theta = []; + end + L.sigma = 0; + if isempty(L.d) + L.d = 0; + end + if ~isempty(L.theta) + error('theta cannot be specified for a prismatic link'); + end + end + + end +end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/RevoluteMDH.m b/lwrserv/matlab/rvctools/robot/RevoluteMDH.m new file mode 100644 index 0000000..8b7e30d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/RevoluteMDH.m @@ -0,0 +1,58 @@ +%RevoluteMDH Robot manipulator Revolute link class for MDH convention +% +% A subclass of the Link class: holds all information related to a robot +% link such as kinematics parameters, rigid-body inertial parameters, motor +% and transmission parameters. +% +% Notes:: +% - This is reference class object +% - Link class objects can be used in vectors and arrays +% - Modified Denavit-Hartenberg parameters are used +% +% References:: +% - Robotics, Vision & Control, Chap 7 +% P. Corke, Springer 2011. +% +% See also Link, Prismatic, SerialLink. +% See also Link, PrismaticMDH, Revolute, SerialLink. + + +% Copyright (C) 1993-2015, 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.petercorke.com + +classdef RevoluteMDH < Link + methods + function L = RevoluteMDH(varargin) + L = L@Link(varargin{:}); + + if nargin == 0 + L.theta = []; + end + L.sigma = 0; + if isempty(L.d) + L.d = 0; + end + L.mdh = 1; + if ~isempty(L.theta) + error('theta cannot be specified for a revolute link'); + end + end + + end +end diff --git a/lwrserv/matlab/rvctools/robot/RobotArm.m b/lwrserv/matlab/rvctools/robot/RobotArm.m new file mode 100644 index 0000000..e23c9c7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/RobotArm.m @@ -0,0 +1,224 @@ +%RobotArm Serial-link robot arm class +% +% A subclass of SerialLink than includes an interface to a physical robot. +% +% Methods:: +% +% plot display graphical representation of robot +%- +% teach drive the physical and graphical robots +% mirror use the robot as a slave to drive graphics +%- +% jmove joint space motion of the physical robot +% cmove Cartesian space motion of the physical robot +% +% plus all other methods of SerialLink +% +% Properties:: +% +% as per SerialLink class +% +% Note:: +% - the interface to a physical robot, the machine, should be an abstract +% superclass but right now it isn't +% - RobotArm is a subclass of SerialLink. +% - RobotArm is a reference (handle subclass) object. +% - RobotArm objects can be used in vectors and arrays +% +% Reference:: +% - http://www.petercorke.com/doc/robotarm.pdf +% - Robotics, Vision & Control, Chaps 7-9, +% P. Corke, Springer 2011. +% - Robot, Modeling & Control, +% M.Spong, S. Hutchinson & M. Vidyasagar, Wiley 2006. +% +% See also Machine, SerialLink, Link, DHFactor. + +% Copyright (C) 1993-2015, 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.petercorke.com + +classdef RobotArm < SerialLink + + properties + machine + ngripper + end + + methods + function ra = RobotArm(robot, machine, varargin) + %RobotArm.RobotArm Construct a RobotArm object + % + % RA = RobotArm(L, M, OPTIONS) is a robot object defined by a vector of + % Link objects L with a physical robot interface M represented by an object + % of class Machine. + % + % Options:: + % + % 'name', name set robot name property + % 'comment', comment set robot comment property + % 'manufacturer', manuf set robot manufacturer property + % 'base', base set base transformation matrix property + % 'tool', tool set tool transformation matrix property + % 'gravity', g set gravity vector property + % 'plotopt', po set plotting options property + % + % See also SerialLink.SerialLink, Arbotix.Arbotix. + + ra = ra@SerialLink(robot, varargin{:}); + ra.machine = machine; + + ra.ngripper = machine.nservos - ra.n; + end + + function delete(ra) + %RobotArm.delete Destroy the RobotArm object + % + % RA.delete() closes and destroys the machine interface object and the RobotArm + % object. + + % attempt to destroy the machine interfaace + try + ra.machine.disconnect(); + delete(ra.machine); + catch + end + + % cleanup the parent object + delete@SerialLink(ra); + end + + function jmove(ra, qf, t) + %RobotArm.jmove Joint space move + % + % RA.jmove(QD) moves the robot arm to the configuration specified by + % the joint angle vector QD (1xN). + % + % RA.jmove(QD, T) as above but the total move takes T seconds. + % + % Notes:: + % - A joint-space trajectory is computed from the current configuration to QD. + % + % See also RobotArm.cmove, Arbotix.setpath. + + if nargin < 3 + t = 3; + end + + q0 = ra.getq(); + qt = jtraj(q0, qf, 20); + + ra.machine.setpath(qt, t/20); + end + + function cmove(ra, T, varargin) + %RobotArm.cmove Cartesian space move + % + % RA.cmove(T) moves the robot arm to the pose specified by + % the homogeneous transformation (4x4). + % + % Notes:: + % - A joint-space trajectory is computed from the current configuration to + % QD using the jmove() method. + % - If the robot is 6-axis with a spherical wrist inverse kinematics are + % computed using ikine6s() otherwise numerically using ikine(). + % + % See also RobotArm.jmove, Arbotix.setpath. + if ra.isspherical() + q = ra.ikine6s(T, varargin{:}); + else + q = ra.ikine(T, ra.getq(), [1 1 1 1 0 0]); + end + ra.jmove(q); + end + + function q = getq(ra) + %RobotArm.getq Get the robot joint angles + % + % Q = RA.getq() is a vector (1xN) of robot joint angles. + % + % Notes:: + % - If the robot has a gripper, its value is not included in this vector. + + q = ra.machine.getpos(); + q = q(1:ra.n); + end + + function mirror(ra) + %RobotArm.mirror Mirror the robot pose to graphics + % + % RA.mirror() places the robot arm in relaxed mode, and as it is moved by + % hand the graphical animation follows. + % + % See also SerialLink.teach, SerialLink.plot. + + h = msgbox('The robot arm will go to relaxed mode, type q in the figure window to exit', ... + 'Mirror mode', 'warn'); + + ra.machine.relax(); + while true + if get(gcf,'CurrentCharacter') == 'q' + break + end; + + q = ra.machine.getpos(); + ra.plot(q(1:ra.n)); + + end + ra.machine.relax([], false); + + delete(h); + end + + function teach(ra) + %RobotArm.teach Teach the robot + % + % RA.teach() invokes a simple GUI to allow joint space motion, as well + % as showing an animation of the robot on screen. + % + % See also SerialLink.teach, SerialLink.plot. + + q0 = ra.machine.getpos(); + + teach@SerialLink(ra, 'q0', q0(1:ra.n), ... + 'callback', @(q) ra.machine.setpos([q q0(ra.n+1)]) ); + end + + function gripper(ra, open) + %RobotArm.gripper Control the robot gripper + % + % RA.gripper(C) sets the robot gripper according to C which is 0 for closed + % and 1 for open. + % + % Notes:: + % - Not all robots have a gripper. + % - The gripper is assumed to be the last servo motor in the chain. + if open < 0 || open > 1 + error('RTB:RobotArm:badarg', 'gripper control must be in range 0 to 1'); + end + + if ra.ngripper == 0 + error('RTB:RobotArm:nofunc', 'robot has no gripper'); + end + + griplimits = ra.machine.gripper; + a = open*griplimits(1) + (1-open)*griplimits(2) + ra.machine.setpos(ra.n+1, a); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/Sensor.m b/lwrserv/matlab/rvctools/robot/Sensor.m new file mode 100644 index 0000000..97e6a9a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Sensor.m @@ -0,0 +1,134 @@ +%Sensor Sensor superclass +% +% An abstact superclass to represent robot navigation sensors. +% +% Methods:: +% display print the parameters in human readable form +% char convert to string +% +% Properties:: +% robot The Vehicle object on which the sensor is mounted +% map The Map object representing the landmarks around the robot +% +% Reference:: +% +% Robotics, Vision & Control, +% Peter Corke, +% Springer 2011 +% +% See also EKF, Vehicle, Map. + +% 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 . + +classdef Sensor < handle + % TODO, pose option, wrt vehicle + + properties + robot + map + + verbose + + ls + animate % animate sensor measurements + interval % measurement return subsample factor + fail + + + end + + methods + + function s = Sensor(robot, map, varargin) + %Sensor.Sensor Sensor object constructor + % + % S = Sensor(VEHICLE, MAP) is a sensor mounted on the Vehicle object + % VEHICLE and observing the landmark map MAP. + % S = Sensor(VEHICLE, MAP, R) is an instance of the Sensor object mounted + % on a vehicle represented by the object VEHICLE and observing features in the + % world represented by the object MAP. + + opt.skip = 1; + opt.animate = false; + opt.fail = []; + opt.ls = 'r-'; + + opt = tb_optparse(opt, varargin); + + s.interval = opt.skip; + s.animate = opt.animate; + + s.robot = robot; + s.map = map; + s.verbose = false; + s.fail = opt.fail; + s.ls = opt.ls; + + end + + function plot(s, jf) + if isempty(s.ls) + return; + end + + h = findobj(gca, 'tag', 'sensor'); + if isempty(h) + % no sensor line, create one + h = plot(0, 0, s.ls, 'tag', 'sensor'); + end + + % there is a sensor line animate it + + xi = s.map.map(:,jf); + set(h, 'XData', [s.robot.x(1), xi(1)], 'YData', [s.robot.x(2), xi(2)]); + %pause(0.1); + %delete(h); + drawnow + end + + function display(s) + %Sensor.display Display status of sensor object + % + % S.display() displays the state of the sensor object in + % human-readable form. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a Sensor object and the command has no trailing + % semicolon. + % + % See also Sensor.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(s) ); + end % display() + + function str = char(s) + %Sensor.char Convert sensor parameters to a string + % + % s = S.char() is a string showing sensor parameters in + % a compact human readable format. + str = [class(s) ' sensor class:']; + str = char(str, char(s.map)); + end + + end % method +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/Vehicle.m b/lwrserv/matlab/rvctools/robot/Vehicle.m new file mode 100644 index 0000000..214375b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/Vehicle.m @@ -0,0 +1,616 @@ +%Vehicle Car-like vehicle class +% +% This class models the kinematics of a car-like vehicle (bicycle model). For +% given steering and velocity inputs it updates the true vehicle state and returns +% noise-corrupted odometry readings. +% +% Methods:: +% init initialize vehicle state +% f predict next state based on odometry +% step move one time step and return noisy odometry +% control generate the control inputs for the vehicle +% update update the vehicle state +% run run for multiple time steps +% Fx Jacobian of f wrt x +% Fv Jacobian of f wrt odometry noise +% gstep like step() but displays vehicle +% plot plot/animate vehicle on current figure +% plot_xy plot the true path of the vehicle +% add_driver attach a driver object to this vehicle +% display display state/parameters in human readable form +% char convert to string +% +% Static methods:: +% plotv plot/animate a pose on current figure +% +% Properties (read/write):: +% x true vehicle state (3x1) +% V odometry covariance (2x2) +% odometry distance moved in the last interval (2x1) +% rdim dimension of the robot (for drawing) +% L length of the vehicle (wheelbase) +% alphalim steering wheel limit +% maxspeed maximum vehicle speed +% T sample interval +% verbose verbosity +% x_hist history of true vehicle state (Nx3) +% driver reference to the driver object +% x0 initial state, restored on init() +% +% Examples:: +% +% Create a vehicle with odometry covariance +% v = Vehicle( diag([0.1 0.01].^2 ); +% and display its initial state +% v +% now apply a speed (0.2m/s) and steer angle (0.1rad) for 1 time step +% odo = v.update([0.2, 0.1]) +% where odo is the noisy odometry estimate, and the new true vehicle state +% v +% +% We can add a driver object +% v.add_driver( RandomPath(10) ) +% which will move the vehicle within the region -10. + +classdef Vehicle < handle + + properties + % state + x % true state (x,y,theta) + x_hist % x history + + % parameters + L % length of vehicle + alphalim % steering wheel limit + maxspeed % maximum speed + dim % dimension of the world -dim -> +dim in x and y + rdim % dimension of the robot + dt % sample interval + V % odometry covariance + + odometry % distance moved in last interval + + verbose + + driver % driver object + x0 % initial state + end + + methods + + function veh = Vehicle(V, varargin) + %Vehicle Vehicle object constructor + % + % V = Vehicle(V_ACT, OPTIONS) creates a Vehicle object with actual odometry + % covariance V_ACT (2x2) matrix corresponding to the odometry vector [dx dtheta]. + % + % Options:: + % 'stlim',A Steering angle limit (default 0.5 rad) + % 'vmax',S Maximum speed (default 5m/s) + % 'L',L Wheel base (default 1m) + % 'x0',x0 Initial state (default (0,0,0) ) + % 'dt',T Time interval + % 'rdim',R Robot size as fraction of plot window (default 0.2) + % 'verbose' Be verbose + % + % Notes:: + % - Subclasses the MATLAB handle class which means that pass by reference semantics + % apply. + + if ~isnumeric(V) + error('first arg is V'); + end + veh.x = zeros(3,1); + if nargin < 1 + V = zeros(2,2); + end + + opt.stlim = 0.5; + opt.vmax = 5; + opt.L = 1; + opt.rdim = 0.2; + opt.dt = 0.1; + opt.x0 = zeros(3,1); + opt = tb_optparse(opt, varargin); + + veh.V = V; + + veh.dt = opt.dt; + veh.alphalim = opt.stlim; + veh.maxspeed = opt.vmax; + veh.L = opt.L; + veh.x0 = opt.x0(:); + veh.rdim = opt.rdim; + veh.verbose = opt.verbose; + + veh.x_hist = []; + end + + function init(veh, x0) + %Vehicle.init Reset state of vehicle object + % + % V.init() sets the state V.x := V.x0, initializes the driver + % object (if attached) and clears the history. + % + % V.init(X0) as above but the state is initialized to X0. + if nargin > 1 + veh.x = x0(:); + else + veh.x = veh.x0; + end + veh.x_hist = []; + if ~isempty(veh.driver) + veh.driver.init() + end + end + + function add_driver(veh, driver) + %Vehicle.add_driver Add a driver for the vehicle + % + % V.add_driver(D) connects a driver object D to the vehicle. The driver + % object has one public method: + % [speed, steer] = D.demand(); + % that returns a speed and steer angle. + % + % Notes:: + % - The Vehicle.step() method invokes the driver if one is attached. + % + % See also Vehicle.step, RandomPath. + veh.driver = driver; + driver.veh = veh; + end + + function xnext = f(veh, x, odo, w) + %Vehicle.f Predict next state based on odometry + % + % XN = V.f(X, ODO) predict next state XN (1x3) based on current state X (1x3) and + % odometry ODO (1x2) is [distance,change_heading]. + % + % XN = V.f(X, ODO, W) as above but with odometry noise W. + % + % Notes:: + % - Supports vectorized operation where X and XN (Nx3). + if nargin < 4 + w = [0 0]; + end + + dd = odo(1) + w(1); dth = odo(2); + + % straightforward code: + % thp = x(3) + dth; + % xnext = zeros(1,3); + % xnext(1) = x(1) + (dd + w(1))*cos(thp); + % xnext(2) = x(2) + (dd + w(1))*sin(thp); + % xnext(3) = x(3) + dth + w(2); + % + % vectorized code: + + thp = x(:,3) + dth; + xnext = x + [(dd+w(1))*cos(thp) (dd+w(1))*sin(thp) ones(size(x,1),1)*dth+w(2)]; + end + + function odo = update(veh, u) + %Vehicle.update Update the vehicle state + % + % ODO = V.update(U) is the true odometry value for + % motion with U=[speed,steer]. + % + % Notes:: + % - Appends new state to state history property x_hist. + % - Odometry is also saved as property odometry. + + xp = veh.x; % previous state + veh.x(1) = veh.x(1) + u(1)*veh.dt*cos(veh.x(3)); + veh.x(2) = veh.x(2) + u(1)*veh.dt*sin(veh.x(3)); + veh.x(3) = veh.x(3) + u(1)*veh.dt/veh.L * u(2); + odo = [colnorm(veh.x(1:2)-xp(1:2)) veh.x(3)-xp(3)]; + veh.odometry = odo; + + veh.x_hist = [veh.x_hist; veh.x']; % maintain history + end + + + function J = Fx(veh, x, odo) + %Vehicle.Fx Jacobian df/dx + % + % J = V.Fx(X, ODO) is the Jacobian df/dx (3x3) at the state X, for + % odometry input ODO. + % + % See also Vehicle.f, Vehicle.Fv. + dd = odo(1); dth = odo(2); + thp = x(3) + dth; + + J = [ + 1 0 -dd*sin(thp) + 0 1 dd*cos(thp) + 0 0 1 + ]; + end + + function J = Fv(veh, x, odo) + %Vehicle.Fv Jacobian df/dv + % + % J = V.Fv(X, ODO) returns the Jacobian df/dv (3x2) at the state X, for + % odometry input ODO. + % + % See also Vehicle.F, Vehicle.Fx. + dd = odo(1); dth = odo(2); + thp = x(3) + dth; + + J = [ + cos(thp) -dd*sin(thp) + sin(thp) dd*cos(thp) + 0 1 + ]; + end + + function odo = step(veh, varargin) + %Vehicle.step Advance one timestep + % + % ODO = V.step(SPEED, STEER) updates the vehicle state for one timestep + % of motion at specified SPEED and STEER angle, and returns noisy odometry. + % + % ODO = V.step() updates the vehicle state for one timestep of motion and + % returns noisy odometry. If a "driver" is attached then its DEMAND() method + % is invoked to compute speed and steer angle. If no driver is attached + % then speed and steer angle are assumed to be zero. + % + % Notes:: + % - Noise covariance is the property V. + % + % See also Vehicle.control, Vehicle.update, Vehicle.add_driver. + + % get the control input to the vehicle from either passed demand or driver + u = veh.control(varargin{:}); + + % compute the true odometry and update the state + odo = veh.update(u); + + % add noise to the odometry + if veh.V + odo = veh.odometry + randn(1,2)*veh.V; + end + end + + + function u = control(veh, speed, steer) + %Vehicle.control Compute the control input to vehicle + % + % U = V.control(SPEED, STEER) returns a control input (speed,steer) + % based on provided controls SPEED,STEER to which speed and steering + % angle limits have been applied. + % + % U = V.control() returns a control input (speed,steer) from a "driver" + % if one is attached, the driver's DEMAND() method is invoked. If no driver is attached + % then speed and steer angle are assumed to be zero. + % + % See also Vehicle.step, RandomPath. + if nargin < 2 + % if no explicit demand, and a driver is attached, use + % it to provide demand + if ~isempty(veh.driver) + [speed, steer] = veh.driver.demand(); + else + % no demand, do something safe + speed = 0; + steer = 0; + end + end + + % clip the speed + u(1) = min(veh.maxspeed, max(-veh.maxspeed, speed)); + + % clip the steering angle + u(2) = max(-veh.alphalim, min(veh.alphalim, steer)); + end + + function p = run(veh, nsteps) + %Vehicle.run Run the vehicle simulation + % + % V.run(N) runs the vehicle model for N timesteps and plots + % the vehicle pose at each step. + % + % P = V.run(N) runs the vehicle simulation for N timesteps and + % return the state history (Nx3) without plotting. Each row + % is (x,y,theta). + % + % See also Vehicle.step. + + if nargin < 2 + nsteps = 1000; + end + + %veh.clear(); + if ~isempty(veh.driver) + veh.driver.visualize(); + end + + veh.visualize(); + for i=1:nsteps + veh.step(); + if nargout == 0 + % if no output arguments then plot each step + veh.plot(); + drawnow + end + end + p = veh.x_hist; + end + + % TODO run and run2 should become superclass methods... + + function p = run2(veh, T, x0, speed, steer) + %Vehicle.run2 Run the vehicle simulation + % + % P = V.run2(T, X0, SPEED, STEER) runs the vehicle model for a time T with + % speed SPEED and steering angle STEER. P (Nx3) is the path followed and + % each row is (x,y,theta). + % + % Notes:: + % - Faster and more specific version of run() method. + % + % See also Vehicle.run, Vehicle.step. + veh.init(x0); + + for i=1:(T/veh.dt) + veh.update([speed steer]); + end + p = veh.x_hist; + end + + function h = plot(veh, varargin) + %Vehicle.plot Plot vehicle + % + % V.plot(OPTIONS) plots the vehicle on the current axes at a pose given by + % the current state. If the vehicle has been previously plotted its + % pose is updated. The vehicle is depicted as a narrow triangle that + % travels "point first" and has a length V.rdim. + % + % V.plot(X, OPTIONS) plots the vehicle on the current axes at the pose X. + + % H = V.plotv(X, OPTIONS) draws a representation of a ground robot as an + % oriented triangle with pose X (1x3) [x,y,theta]. H is a graphics handle. + % + % V.plotv(H, X) as above but updates the pose of the graphic represented + % by the handle H to pose X. + % + % Options:: + % 'scale',S Draw vehicle with length S x maximum axis dimension + % 'size',S Draw vehicle with length S + % 'color',C Color of vehicle. + % 'fill' Filled + % + % See also Vehicle.plotv. + + h = findobj(gcf, 'Tag', 'Vehicle.plot'); + if isempty(h) + % no instance of vehicle graphical object found + h = Vehicle.plotv(veh.x, varargin{:}); + set(h, 'Tag', 'Vehicle.plot'); % tag it + end + + if ~isempty(varargin) && isnumeric(varargin{1}) + % V.plot(X) + Vehicle.plotv(h, varargin{1}); % use passed value + else + % V.plot() + Vehicle.plotv(h, veh.x); % use current state + end + end + + function out = plot_xy(veh, varargin) + %Vehicle.plot_xy Plots true path followed by vehicle + % + % V.plot_xy() plots the true xy-plane path followed by the vehicle. + % + % V.plot_xy(LS) as above but the line style arguments LS are passed + % to plot. + % + % Notes:: + % - The path is extracted from the x_hist property. + + xyt = veh.x_hist; + if nargout == 0 + plot(xyt(:,1), xyt(:,2), varargin{:}); + else + out = xyt; + end + end + + function visualize(veh) + grid on + end + + function verbosity(veh, v) + %Vehicle.verbosity Set verbosity + % + % V.verbosity(A) set verbosity to A. A=0 means silent. + veh.verbose = v; + end + + function display(nav) + %Vehicle.display Display vehicle parameters and state + % + % V.display() displays vehicle parameters and state in compact + % human readable form. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a Vehicle object and the command has no trailing + % semicolon. + % + % See also Vehicle.char. + + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(nav) ); + end % display() + + function s = char(veh) + %Vehicle.char Convert to a string + % + % s = V.char() is a string showing vehicle parameters and state in in + % a compact human readable format. + % + % See also Vehicle.display. + + s = 'Vehicle object'; + s = char(s, sprintf(... + ' L=%g, maxspeed=%g, alphalim=%g, T=%f, nhist=%d', ... + veh.L, veh.maxspeed, veh.alphalim, veh.dt, ... + numrows(veh.x_hist))); + if ~isempty(veh.V) + s = char(s, sprintf(... + ' V=(%g,%g)', ... + veh.V(1,1), veh.V(2,2))); + end + s = char(s, sprintf(' x=%g, y=%g, theta=%g', veh.x)); + if ~isempty(veh.driver) + s = char(s, ' driven by::'); + s = char(s, [[' '; ' '] char(veh.driver)]); + end + end + + end % method + + methods(Static) + + function h_ = plotv(x, varargin) + %Vehicle.plotv Plot ground vehicle pose + % + % H = Vehicle.plotv(X, OPTIONS) draws a representation of a ground robot as an + % oriented triangle with pose X (1x3) [x,y,theta]. H is a graphics handle. + % If X (Nx3) is a matrix it is considered to represent a trajectory in which case + % the vehicle graphic is animated. + % + % Vehicle.plotv(H, X) as above but updates the pose of the graphic represented + % by the handle H to pose X. + % + % Options:: + % 'scale',S Draw vehicle with length S x maximum axis dimension + % 'size',S Draw vehicle with length S + % 'color',C Color of vehicle. + % 'fill' Filled with solid color as per 'color' option + % 'fps',F Frames per second in animation mode (default 10) + % + % Example:: + % + % Generate some path 3xN + % p = PRM.plan(start, goal); + % Set the axis dimensions to stop them rescaling for every point on the path + % axis([-5 5 -5 5]); + % + % Now invoke the static method + % Vehicle.plotv(p); + % + % Notes:: + % - This is a static method. + + if isscalar(x) && ishandle(x) + % plotv(h, x) + h = x; + x = varargin{1}; + x = x(:)'; + T = transl([x(1:2) 0]) * trotz( x(3) ); + set(h, 'Matrix', T); + return + end + + opt.scale = 1/60; + opt.size = []; + opt.fill = false; + opt.color = 'r'; + opt.fps = 10; + + [opt,args] = tb_optparse(opt, varargin); + + lineprops = { 'Color', opt.color' }; + if opt.fill + lineprops = [lineprops 'fill' opt.color ]; + end + + + % compute the dimensions of the robot + if ~isempty(opt.size) + d = opt.size; + else + % get the current axes dimensions + a = axis; + d = (a(2)+a(4) - a(1)-a(3)) * opt.scale; + end + + % draw it + points = [ + d 0 + -d -0.6*d + -d 0.6*d + ]'; + + h = hgtransform(); + hp = plot_poly(points, lineprops{:}); + for hh=hp + set(hh, 'Parent', h); + end + + if (numel(x) > 3) && (numcols(x) == 3) + % animation mode + for i=1:numrows(x) + T = transl([x(i,1:2) 0]) * trotz( x(i,3) ); + set(h, 'Matrix', T); + pause(1/opt.fps); + end + elseif (numel(x) == 3) + % compute the pose + % convert vector form of pose to SE(3) + + x = x(:)'; + T = transl([x(1:2) 0]) * trotz( x(3) ); + set(h, 'Matrix', T); + else + error('bad pose'); + end + + if nargout > 0 + h_ = h; + end + end + + end % static methods + +end % classdef diff --git a/lwrserv/matlab/rvctools/robot/angvec2r.m b/lwrserv/matlab/rvctools/robot/angvec2r.m new file mode 100644 index 0000000..4e90a01 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/angvec2r.m @@ -0,0 +1,41 @@ +%ANGVEC2R Convert angle and vector orientation to a rotation matrix +% +% R = ANGVEC2R(THETA, V) is an orthonormal rotation matrix, R, +% equivalent to a rotation of THETA about the vector V. +% +% See also eul2r, rpy2r. + + +% 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 . +function R = angvec2r(theta, k) + + if nargin < 2 || ~isscalar(theta) || ~isvec(k) + error('RTB:angvec2r:badarg', 'bad arguments'); + end + + cth = cos(theta); + sth = sin(theta); + vth = (1 - cth); + kx = k(1); ky = k(2); kz = k(3); + + % from Paul's book, p. 28 + R = [ +kx*kx*vth+cth ky*kx*vth-kz*sth kz*kx*vth+ky*sth +kx*ky*vth+kz*sth ky*ky*vth+cth kz*ky*vth-kx*sth +kx*kz*vth-ky*sth ky*kz*vth+kx*sth kz*kz*vth+cth + ]; diff --git a/lwrserv/matlab/rvctools/robot/angvec2tr.m b/lwrserv/matlab/rvctools/robot/angvec2tr.m new file mode 100644 index 0000000..bdaab0d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/angvec2tr.m @@ -0,0 +1,36 @@ +%ANGVEC2TR Convert angle and vector orientation to a homogeneous transform +% +% T = ANGVEC2TR(THETA, V) is a homogeneous transform matrix equivalent to a +% rotation of THETA about the vector V. +% +% Note:: +% - The translational part is zero. +% +% See also EUL2TR, RPY2TR, ANGVEC2R. + + +% 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 . + +function T = angvec2tr(theta, k) + + if nargin < 2 + error('RTB:angvec2tr:badarg', 'bad arguments'); + end + + + T = r2t( angvec2r(theta, k) ); diff --git a/lwrserv/matlab/rvctools/robot/ctraj.m b/lwrserv/matlab/rvctools/robot/ctraj.m new file mode 100644 index 0000000..683b29c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/ctraj.m @@ -0,0 +1,52 @@ +%CTRAJ Cartesian trajectory between two points +% +% TC = CTRAJ(T0, T1, N) is a Cartesian trajectory (4x4xN) from pose T0 to T1 +% with N points that follow a trapezoidal velocity profile along the path. +% The Cartesian trajectory is a homogeneous transform sequence and the last +% subscript being the point index, that is, T(:,:,i) is the i'th point along +% the path. +% +% TC = CTRAJ(T0, T1, S) as above but the elements of S (Nx1) specify the +% fractional distance along the path, and these values are in the range [0 1]. +% The i'th point corresponds to a distance S(i) along the path. +% +% See also LSPB, MSTRAJ, TRINTERP, Quaternion.interp, TRANSL. + + +% 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 . + +function traj = ctraj(T0, T1, t) + + if ~ishomog(T0) || ~ishomog(T1) + error('arguments must be homogeneous transformations'); + end + + % distance along path is a smooth function of time + if isscalar(t) + s = lspb(0, 1, t); + else + s = t(:); + end + + traj = []; + + for S=s' + traj = cat(3, traj, trinterp(T0, T1, S)); + end + + diff --git a/lwrserv/matlab/rvctools/robot/delta2tr.m b/lwrserv/matlab/rvctools/robot/delta2tr.m new file mode 100644 index 0000000..e88dd3b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/delta2tr.m @@ -0,0 +1,30 @@ +%DELTA2TR Convert differential motion to a homogeneous transform +% +% T = DELTA2TR(D) is a homogeneous transform representing differential +% translation and rotation. The vector D=(dx, dy, dz, dRx, dRy, dRz) +% represents an infinitessimal motion, and is an approximation to the spatial +% velocity multiplied by time. +% +% See also TR2DELTA. + + +% 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 . + +function delta = delta2tr(d) + d = d(:); + delta = eye(4,4) + [skew(d(4:6)) d(1:3); 0 0 0 0]; diff --git a/lwrserv/matlab/rvctools/robot/demos/README b/lwrserv/matlab/rvctools/robot/demos/README new file mode 100644 index 0000000..6347ae9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/README @@ -0,0 +1,12 @@ +Robotics Toolbox demo scripts + +Some examples of how to use toolbox functions to perform a variety of +useful/interesting tasks. + +Note that the scripts are not added to your MATLAB path. + +You can run these scripts by: + +- use the rtbdemo function to invoke the scripts via runscript +- cd to the demos folder and run them from the command line +- use addpath to add the demos folder to your MATLAB path diff --git a/lwrserv/matlab/rvctools/robot/demos/TODO b/lwrserv/matlab/rvctools/robot/demos/TODO new file mode 100644 index 0000000..72d2eb8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/TODO @@ -0,0 +1,5 @@ +manipulability, velocity/acceleration ellipsoids +overactuated robot +resolved rate control (is this in Jacobian demo?) + +IMU diff --git a/lwrserv/matlab/rvctools/robot/demos/braitnav.m b/lwrserv/matlab/rvctools/robot/demos/braitnav.m new file mode 100644 index 0000000..85ce3f6 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/braitnav.m @@ -0,0 +1,34 @@ +% 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.petercorke.com + + +%%begin +% A Braitenberg vehicle is a simple reactive machine, that is, it exhibits useful +% behaviours based on how its sensors are connected to its actuators. + +sl_braitenberg + +% In this example the robot moves to the maximum of some scalar field which is +% measured by two separate sensors on the robot. The sensors are simulated by +%the code + +type sensorfield + +% We will run the simulator and see that it moves toward the maxima +sim('sl_braitenberg'); diff --git a/lwrserv/matlab/rvctools/robot/demos/bugnav.m b/lwrserv/matlab/rvctools/robot/demos/bugnav.m new file mode 100644 index 0000000..a491d06 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/bugnav.m @@ -0,0 +1,42 @@ +% 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.petercorke.com + +%%begin + +% Our bug style robot will operate within a grid world that contains free +% space where it can drive and obstacles. We load a map of the world +load map1 +% which loads a variable called map +about map +% and the cells contain zero if it is free space (driveable) and one if it is an +% obstacle. + +% Now we create an instance of a robot with the bug navigation algorithm +bug = Bug2(map) + +% and display its grid world +bug.plot() +% where obstacles are marked in red. + +% Now we define the goal and start coordinates +bug.goal = [50,30]; +start = [20, 10]; + +% then ask the robot to find the path, it will be animated with green dots +bug.path(start); diff --git a/lwrserv/matlab/rvctools/robot/demos/codegen.m b/lwrserv/matlab/rvctools/robot/demos/codegen.m new file mode 100644 index 0000000..ff4dc75 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/codegen.m @@ -0,0 +1,191 @@ +% Copyright (C) 1993-2013, by Jorn 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.petercorke.com + +%%begin + +%% Getting started with symbolics and code generation +% This is a brief example about how we can derive symbolic robot model +% expressions and how we can generate robot specific functions as well as +% real-time capable Simulink blocks using the |CodeGenerator| class. The +% example uses a reduced version of the Puma 560 arm with the first 3 +% links. +% +% A requirement for this demo is that we have the Mathworks Symbolic Toolbox +% installed besides the Robotics Toolbox. +% + +%% Instantiate a |CodeGenerator| class object +% We start off with the instantiation of a |CodeGenerator| class object. +% First, we load the |SerialLink| object for which we intend to generate +% code. +mdl_puma560_3 + +%% +% After that, we find a |SerialLink| object named p560 in the workspace. This +% object is used to instantiate the CodeGenerator. +cGen = CodeGenerator(p560) + +%% Code generation +% By default |CodeGenerator| class objects are configured to generate: +% +% * symbolic expressions +% * m-code +% * Simulink blocks +% +% and they document the CodeGeneration progress on the Matlab console. +% We may modify this behaviour by passing extra arguments to the +% |CodeGenerator| constructor. (Type |help CodeGenerator| for details) +% +% Now let's generate code for the forward kinematics of our reduced Puma +% 560. +symExp = cGen.genfkine + +%% +% The text output to the console may be disabled +cGen.verbose = false; +% +% or logged to disk by specifying a log file name +cGen.logfile = 'roblog.txt' + +%% +% The output variable |symExp| now contains the symbolic expression for the +% forward kinematics. This expression is the same as would be obtained by +% the following code. +symP560 = p560.sym; +q = symP560.gencoords; +symExpDir = symP560.fkine(q) + +% So we have basically two ways for deriving symbolic expressions using the +% Robotics Toolbox. + +%% +% The difference is that in addition the functional output the symbolic +% expression has now been saved to disk along with the generated m-code and +% Simulink blocks. The storage directory is given in |cGen.basepath|, which +% we now add to our search path. +addpath(cGen.basepath) +ls(cGen.basepath) +%% +% The m-code is contained in a specialized robot class. +ls(cGen.robjpath) + +%% Using the generated m-code +% The |mdl_puma560_3| robot definition script defines some special joint +% configurations: +% +% * qz zero joint angle configuration +% * qr vertical 'READY' configuration +% * qstretch arm is stretched out in the X direction +% * qn arm is at a nominal non-singular configuration +% +% The use of the symbolic expressions and generated code will be +% exemplified in the following based on the zero joint angle configuration. +% +% +qz +%% +% With the generic version of the fkine function from the |SerialLink| +% class we would compute the forward kinematics as follows: +tic; Tz1 = p560.fkine(qz); t1 = toc +%% +% In order to use the generated robot specific m-functions we add them to +% the search path and instantiate a new robot object. +addpath(cGen.basepath) +specRob = eval(cGen.getrobfname) +tic; Tz2 = specRob.fkine(qz); t2 = toc + +%% Speedup +% The specialized robot version of fkine runs a little faster +% because it only performs the computations necessary for the specific robot. +% The speedup of the generated robot specific m-code becomes even more appearent if we +% repeat the comparison of the execution times for dynamics +% functions such as: +% +% * gravload -> cGen.gengravload +% * inertia -> cGen.geninertia +% * coriolis -> cGen.gencoriolis +% * invdyn -> cGen.geninvdyn +% +% This way the specialized m-code can be used to decrease simulation times. +% + +%% +% We obtain the exact solution without floating point notation if we use +% the symbolic expression as follows: +tic; Tz1 = subs(symExp,'[q1, q2, q3]',qz); toc +%% +% This is however more time consuming. Most probably we might use the +% symbolic expressions for algorithm development, controller design, +% stability proofs as well as analysis, system identification or teaching. +% +% It is also possible to get the symbolic expressions for the homogenous +% transformations of up to each individual joint. This has been found to be +% useful for example for during derivation of analytical inverse kinematics +% solutions. See the documentation of genfkine for details. +% + +%% Inheritance +% Even though we have not yet generated robot specific code for |SerialLink| +% metods other than |fkine|, we can still use all functionality of +% |SerialLink| objects with our new specialized robot object which inherits +% from |SerialLink|. +J01 = p560.jacob0(qz) +J02 = specRob.jacob0(qz) + +%% A look at the generated Simulink blocks +% The Simulink blocks are stored in a Simulink library file. By opening the +% generated Simulink library we can investigate the already optimized robot +% specific code within the blocks. +% The usage of these blocks is also accompanied with a noticable speedup +% compared to the blocks based on generic |SerialLink| objects. +eval(cGen.slib); +snapnow; +%% +% Beyond the speedup for simulations all blocks in the generated library +% may be directly compiled for real-time operating systems such as xPC-Target or +% dSpace systems for model based control of real hardware setups. +% This way we avoid tedious and error prone reimplementation of the model +% on the target hardware. +% + +bdclose(cGen.slib); + +%% Further information +% For further information on symbolics and code generation see the +% documentation of the |SerialLink| and |CodeGenerator| class. +% +% All generated functions come with their own headers so that information +% about their usage can be found by typing |help funname|. +% +% A list of all available methods provide by the |CodeGenerator| class can be +% displayed. +methods CodeGenerator + +%% +% The same applies to the configurable properties: +properties CodeGenerator + +%% Cleanup +% If we whish to clean our disk from all the generated code, we can simply +% remove it from the search path +rmpath(cGen.basepath) +%% +% and purge everything. +cGen.purge(1) +snapnow diff --git a/lwrserv/matlab/rvctools/robot/demos/demos.xml b/lwrserv/matlab/rvctools/robot/demos/demos.xml new file mode 100644 index 0000000..ed56599 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/demos.xml @@ -0,0 +1,134 @@ + + + +Robotics Toolbox +toolbox +$toolbox/matlab/icons/matlabicon.gif +Robotics Toolbox demonstrations of transform and quaternion primitives, +Jacobians, animations, path planning, forward and inverse kinematics, and forward and inverse dynamics. + + + + + + + + $toolbox/matlab/icons/matlabicon.gif + + + rotation + Ways to represent rotation in 3D + + + + trans + Representing pose in 3D + + + + traj + Representing sequences of poses + + + + robot + Links and serial-link manipulators + + + + fkine + Manipulator forward kinematics: angles to pose + + + + ikine + Manipulator inverse kinematics: pose to angles + + + + graphics + Advanced use of plot and animation + + + + jacob + Infinitissimal motion and manipulator Jacobians + + + + idyn + Inverse manipulator dynamics: joint rates to torque + + + + fdyn + Forward manipulator dynamics: torque to joint rates + + + + symbolic + Using symbolic variables with the Toolbox + + + + codegen + Generate efficient run-time kinematic and dynamic code + + + + + $toolbox/matlab/icons/simulinkicon.gif + + + + ztorque + Simulink + + + + braitnav + Demonstrate a Braitenberg vehicle + + + + drivepose + Control a car-like mobile robot to move toward a desired pose + + + + quadrotor + Control a quadrotor flying vehicle to take off and fly in a circle + + + + + $toolbox/matlab/icons/matlabicon.gif + + + + bugnav + Demonstrate the Bug2 goal finding algorithm + + + + dstarnav + Demonstrate D* path planning + + + + prmnav + Demonstrate probabistic roadmap path planning + + + + slam + Demonstrate the simultaneous estimation of robot pose and landmark positions using an extended Kalman filter (EKF) + + + + particlefilt + Demonstrate the estimation of robot pose using a Monte-Carlo style particle filter + + + + diff --git a/lwrserv/matlab/rvctools/robot/demos/drivepose.m b/lwrserv/matlab/rvctools/robot/demos/drivepose.m new file mode 100644 index 0000000..d8ac43c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/drivepose.m @@ -0,0 +1,46 @@ +% 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.petercorke.com + +%%begin + +% We will show how to make a mobile robot with "car-like" steering drive from +% one pose to another. + +% The goal pose is +xg = [5 5 pi/2]; + +% and the starting pose is +x0 = [5 9 0] + +% We use a Simulink model to represent the dynamics of the vehicle and to +% implement a pose controller +sl_drivepose + +% We run the simulation +r = sim('sl_drivepose'); +% and extract the trajectory of the robot +y = r.find('yout'); + +% which we plot +axis([0 10 0 10]); hold on; grid on +plot(y(:,1), y(:,2)); +% and overlay the initial and final pose of the robot +plot_vehicle(x0, 'r'); plot_vehicle(xg, 'r'); +% Note the complex path the robot had to follow, since it's motion is limited +% by the nature of the steering mechanism. diff --git a/lwrserv/matlab/rvctools/robot/demos/dstarnav.m b/lwrserv/matlab/rvctools/robot/demos/dstarnav.m new file mode 100644 index 0000000..46b6bc5 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/dstarnav.m @@ -0,0 +1,60 @@ +% 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.petercorke.com + +%%begin + +% Our robot will operate within a grid world that contains free +% space where it can drive and obstacles. We load a map of the world +load map1 +% which loads a variable called map +about map + +% The elements of the obstacle map are converted into traversibility values, an +% obstacle has an infinite traversibility while free space has a nominal +% travesibility of 1. + +% Now we create an instance of a robot with the D* navigation algorithm +dstar = Dstar(map, 'quiet'); + +% Now we define the goal and start coordinates +goal = [50,30]; +start = [20, 10]; + +% then ask the robot to plan a path to goal (it will take few seconds) +dstar.plan(goal); + +% Now we can display the obstacles and the cost to reach the goal from every +% point in the world +dstar.plot() +% where the cost scale is shown by the bar to the right. + +% Now we can execute the planned path, it will be animated with green dots +tic; dstar.path(start); toc + +% Now lets change the difficulty of some of the terrain, make it more costly +% to travese +for r=78:85; for c=12:45; dstar.modify_cost([c,r], 2); end; end + +% Now we can replan, but D* does this in an incremental way which is faster than +% recomputing the whole plan from scratch +tic; dstar.plan(); toc +% that took less time than before... + +% and the best path is now +dstar.path(start) diff --git a/lwrserv/matlab/rvctools/robot/demos/fdyn.m b/lwrserv/matlab/rvctools/robot/demos/fdyn.m new file mode 100644 index 0000000..1fcd4aa --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/fdyn.m @@ -0,0 +1,65 @@ +% 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.petercorke.com + +%%begin + +% Forward dynamics is the computation of joint accelerations given position and +% velocity state, and actuator torques. It is useful in simulation of a robot +% control system. +% +% Consider a Puma 560 at rest in the zero angle pose, with zero applied joint +% torques. The joint acceleration would be given by + +mdl_puma560 +p560.accel(qz, zeros(1,6), zeros(1,6)) + +% To be useful for simulation this function must be integrated. fdyn() uses the +% MATLAB function ode45() to integrate the joint acceleration. It also allows +% for a user written function to compute the joint torque as a function of +% manipulator state. +% +% To simulate the motion of the Puma 560 from rest in the zero angle pose +% with zero applied joint torques + +tic +[t q qd] = p560.nofriction().fdyn(10, [], qz); +toc + +% and the resulting motion can be plotted versus time + +subplot(3,1,1) +plot(t,q(:,1)) +xlabel('Time (s)'); +ylabel('Joint 1 (rad)') +subplot(3,1,2) +plot(t,q(:,2)) +xlabel('Time (s)'); +ylabel('Joint 2 (rad)') +subplot(3,1,3) +plot(t,q(:,3)) +xlabel('Time (s)'); +ylabel('Joint 3 (rad)') + +% Clearly the robot is collapsing under gravity, but it is interesting to +% note that rotational velocity of the upper and lower arm are exerting +% centripetal and Coriolis torques on the waist joint, causing it to rotate. + +% This can be shown in animation also +clf +p560.plot(q) diff --git a/lwrserv/matlab/rvctools/robot/demos/fkine.m b/lwrserv/matlab/rvctools/robot/demos/fkine.m new file mode 100644 index 0000000..b9adce8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/fkine.m @@ -0,0 +1,91 @@ +% 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.petercorke.com + +%%begin + +% Forward kinematics is the problem of solving the Cartesian position and +% orientation of a mechanism given knowledge of the kinematic structure and +% the joint coordinates. +% +% We will work with a model of the Puma 560 robot +mdl_puma560 + +% Consider the Puma 560 example again, and the joint coordinates of zero, +% which are defined by the script + +qz + +% The forward kinematics may be computed using fkine() method of the +% p560 robot object + +p560.fkine(qz) + +% returns the homogeneous transform corresponding to the last link of the +% manipulator + +% fkine() can also be used with a time sequence of joint coordinates, or +% trajectory, which is generated by jtraj() + +t = [0:.056:2]; % generate a time vector +q = jtraj(qz, qr, t); % compute the joint coordinate trajectory + +about q + +% then the homogeneous transform for each set of joint coordinates is given by + +T = p560.fkine(q); + +about T + +% where T is a 3-dimensional matrix, the first two dimensions are a 4x4 +% homogeneous transformation and the third dimension is time. +% +% For example, the first point is + +T(:,:,1) + +% and the tenth point is + +T(:,:,10) + +% +% Elements (1:3,4) correspond to the X, Y and Z coordinates respectively, and +% may be plotted against time + +subplot(3,1,1) +plot(t, squeeze(T(1,4,:))) +xlabel('Time (s)'); +ylabel('X (m)') +subplot(3,1,2) +plot(t, squeeze(T(2,4,:))) +xlabel('Time (s)'); +ylabel('Y (m)') +subplot(3,1,3) +plot(t, squeeze(T(3,4,:))) +xlabel('Time (s)'); +ylabel('Z (m)') + +% or we could plot X against Z to get some idea of the Cartesian path followed +% by the manipulator. + +subplot(1,1,1) +plot(squeeze(T(1,4,:)), squeeze(T(3,4,:))); +xlabel('X (m)') +ylabel('Z (m)') +grid diff --git a/lwrserv/matlab/rvctools/robot/demos/graphics.m b/lwrserv/matlab/rvctools/robot/demos/graphics.m new file mode 100644 index 0000000..8aae2c8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/graphics.m @@ -0,0 +1,72 @@ +% 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.petercorke.com + +%%begin +% We will work with a model of the Puma 560 robot +mdl_puma560 + +% The trajectory demonstration has shown how a joint coordinate trajectory +% may be generated + +t = [0:.05:2]'; % generate a time vector +q = jtraj(qz, qr, t); % generate joint coordinate trajectory + +% the overloaded function plot() animates a stick figure robot moving +% along a trajectory. + +p560.plot(q); + +% The drawn line segments do not necessarily correspond to robot links, but +% join the origins of sequential link coordinate frames. +% +% A small right-angle coordinate frame is drawn on the end of the robot to show +% the wrist orientation. +% +% A shadow appears on the ground which helps to give some better idea of the +% 3D object. +% +% This is a 3D plot so using the tools on the figure toolbar you can rotate the +% figure and change your viewpoint. + +%-- +% We can also place additional robots into a figure. +% +% Let's make a clone of the Puma robot, but change its name and base location + +p560_2 = SerialLink(p560, ... + 'name', 'another Puma', ... + 'base', transl(-0.5, 0.5, 0) ) +hold on +p560_2.plot(q); + +% We can also have multiple views of the same robot + +clf +p560.plot(qr); +figure +p560.plot(qr); +view(40,50) +p560.plot(q) + +% Sometimes it's useful to be able to manually drive the robot around to +% get an understanding of how it works. + +p560.teach() +% Use the sliders to control the robot (in fact both views). Hit the red quit +% button when you are done. diff --git a/lwrserv/matlab/rvctools/robot/demos/idyn.m b/lwrserv/matlab/rvctools/robot/demos/idyn.m new file mode 100644 index 0000000..6862ddb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/idyn.m @@ -0,0 +1,78 @@ +% 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.petercorke.com + +%%begin + +% Inverse dynamics computes the joint torques required to achieve the specified +% state of joint position, velocity and acceleration. +% The recursive Newton-Euler formulation is an efficient matrix oriented +% algorithm for computing the inverse dynamics, and is implemented in the +% function rne(). +% +% Inverse dynamics requires inertial and mass parameters of each link, as well +% as the kinematic parameters. This is achieved by augmenting the kinematic +% description matrix with additional columns for the inertial and mass +% parameters for each link. +% +% For example, for a Puma 560 in the zero angle pose, with all joint velocities +% of 5rad/s and accelerations of 1rad/s/s, the joint torques required are + +mdl_puma560 +tau = p560.rne(qz, 5*ones(1,6), ones(1,6)) + +% As with other functions the inverse dynamics can be computed for each point +% on a trajectory. Create a joint coordinate trajectory and compute velocity +% and acceleration as well + +t = [0:.056:2]; % create time vector +[q,qd,qdd] = jtraj(qz, qr, t); % compute joint coordinate trajectory +tau = p560.rne(q, qd, qdd); % compute inverse dynamics + +% Now the joint torques can be plotted as a function of time + +plot(t, tau(:,1:3)); xlabel('Time (s)'); ylabel('Joint torque (Nm)') + +% Much of the torque on joints 2 and 3 of a Puma 560 (mounted conventionally) is +% due to gravity. That component can be computed using gravload() + +taug = p560.gravload(q); +plot(t, taug(:,1:3)); xlabel('Time (s)'); ylabel('Gravity torque (Nm)') + +% Now lets plot that as a fraction of the total torque required over the +% trajectory + +subplot(2,1,1); plot(t,[tau(:,2) taug(:,2)]); xlabel('Time (s)'); ylabel('Torque on joint 2 (Nm)'); +subplot(2,1,2); plot(t,[tau(:,3) taug(:,3)]); xlabel('Time (s)'); ylabel('Torque on joint 3 (Nm)'); + +% The inertia seen by the waist (joint 1) motor changes markedly with robot +% configuration. The function inertia() computes the manipulator inertia matrix +% for any given configuration. +% +% Let's compute the variation in joint 1 inertia, that is M(1,1), as the +% manipulator moves along the trajectory (this may take a few minutes) + +M = p560.inertia(q); +M11 = squeeze(M(1,1,:)); +clf; plot(t, M11); xlabel('Time (s)'); ylabel('Inertia on joint 1 (kgms2)') + +% Clearly the inertia seen by joint 1 varies considerably over this path. +% This is one of many challenges to control design in robotics, achieving +% stability and high-performance in the face of plant variation. In fact +% for this example the inertia varies by a factor of +max(M11)/min(M11) diff --git a/lwrserv/matlab/rvctools/robot/demos/ikine.m b/lwrserv/matlab/rvctools/robot/demos/ikine.m new file mode 100644 index 0000000..9a7f2f4 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/ikine.m @@ -0,0 +1,106 @@ +% 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.petercorke.com + +%%begin + +% Inverse kinematics is the problem of finding the robot joint coordinates, +% given a homogeneous transform representing the last link of the manipulator. +% It is very useful when the path is planned in Cartesian space, for instance +% a straight line path as shown in the trajectory demonstration. +% +% First generate the transform corresponding to a particular joint coordinate, + +mdl_puma560 +q = [0 -pi/4 -pi/4 0 pi/8 0] +T = p560.fkine(q) + +% Now the inverse kinematic procedure for any specific robot can be derived +% symbolically and in general an efficient closed-form solution can be +% obtained. However we are given only a generalized description of the +% manipulator in terms of kinematic parameters so an iterative solution will +% be used. The procedure is slow, and the choice of starting value affects +% search time and the solution found, since in general a manipulator may +% have several poses which result in the same transform for the last +% link. The starting point for the first point may be specified, or else it +% defaults to zero (which is not a particularly good choice in this case) + +qi = p560.ikine(T); + +% and in fact it does not converge + +qi + +% We can help the solution along by using the 'pinv' option + +qi = p560.ikine(T, 'pinv'); +% and the result +qi +% is the same as the original set of joint angles +q + +% However in general this will not be the case, there are multiple +% solutions, and the solution that is found depends on the initial +% choice of angles. +% +% A more efficient approach is to use an analytic solution and the toolbox +% supports the common case of a 6-axis robot arm with a spherical wrist + +qi = p560.ikine6s(T) +% which is different to the original joint angles, but as expected + +p560.fkine(qi) + +% it does give the same end-effector pose. + +% The analytic solution allows the specific solution to be specified +% using a character string and to get the same set of joint angles + +p560.ikine6s(T, 'rdf') +% where we have specified that the robot is in a right-handed configuration +% (r), with its elbow down (d), and the wrist flipped (f). + +% A solution is not always possible, for instance if the specified +% transform describes a point out of reach of the manipulator. As +% mentioned above the solutions are not necessarily unique, and there +% are singularities at which the manipulator loses degrees of freedom +% and joint coordinates become linearly dependent. + +% Inverse kinematics may also be computed for a trajectory. +% If we take a Cartesian straight line path between two poses in 50 steps + +T1 = transl(0.6, -0.5, 0.0) % define the start point +T2 = transl(0.4, 0.5, 0.2) % and destination +T = ctraj(T1, T2, 50); % compute a Cartesian path + +% now solve the inverse kinematics + +q = p560.ikine6s(T); +about q +% which has one row per time step and one column per joint angle + +% Let's examine the joint space trajectory that results in straightline +% Cartesian motion + +subplot(3,1,1); plot(q(:,1)); xlabel('Time (s)'); ylabel('Joint 1 (rad)'); +subplot(3,1,2); plot(q(:,2)); xlabel('Time (s)'); ylabel('Joint 2 (rad)'); +subplot(3,1,3); plot(q(:,3)); xlabel('Time (s)'); ylabel('Joint 3 (rad)'); + +% This joint space trajectory can now be animated +clf +p560.plot(q) diff --git a/lwrserv/matlab/rvctools/robot/demos/jacob.m b/lwrserv/matlab/rvctools/robot/demos/jacob.m new file mode 100644 index 0000000..6652559 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/jacob.m @@ -0,0 +1,152 @@ +% 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.petercorke.com + +%%begin + +% Jacobian and differential motion demonstration +% +% A differential motion can be represented by a 6-element vector with elements +% [dx dy dz drx dry drz] +% +% where the first 3 elements are a differential translation, and the last 3 +% are a differential rotation. When dealing with infinitisimal rotations, +% the order becomes unimportant. The differential motion could be written +% in terms of compounded transforms +% +% transl(dx,dy,dz) * trotx(drx) * troty(dry) * trotz(drz) +% +% but a more direct approach is to use the function diff2tr() + +D = [.1 .2 0 -.2 .1 .1]'; +delta2tr(D) + +% More commonly it is useful to know how a differential motion in one +% coordinate frame appears in another frame. If the second frame is +% represented by the transform + +T = transl(100, 200, 300) * troty(pi/8) * trotz(-pi/4); + +% then the differential motion in the second frame would be given by + +DT = tr2jac(T) * D; +DT' + +% tr2jac() has computed a 6x6 Jacobian matrix which transforms the differential +% changes from the first frame to the next. + +%------ +% The manipulator's Jacobian matrix relates differential joint coordinate +% motion to differential Cartesian motion; +% +% dX = J(q) dQ +% +% For an n-joint manipulator the manipulator Jacobian is a 6 x n matrix and +% is used is many manipulator control schemes. + +% We will work with a model of the Puma 560 robot +mdl_puma560 + +% Two Jacobians are frequently used, which express the Cartesian velocity in +% the world coordinate frame, +% +% We will first choose a particular joint angle configuration for the robot + +q = [0.1 0.75 -2.25 0 .75 0] + +% and then compute the Jacobian in the world coordinate frame +J = p560.jacob0(q) +% which we can see is 6x6 (since the robot has 6 joints) + +% Alternatively the Jacobian can be expressed in the T6 coordinate frame + +J = p560.jacobn(q) +% Note the top right 3x3 block is all zero. This indicates, correctly, that +% motion of joints 4-6 does not cause any translational motion of the robot's +% end-effector. + +% Many control schemes require the inverse of the Jacobian. The Jacobian +% in this example is not singular + +det(J) + +% and may be inverted +Ji = inv(J) + +% A classic control technique is Whitney's resolved rate motion control +% +% dQ/dt = J(q)^-1 dX/dt +% +% where dX/dt is the desired Cartesian velocity, and dQ/dt is the required +% joint velocity to achieve this. + +vel = [1 0 0 0 0 0]'; % translational motion in the X direction +qvel = Ji * vel; +qvel' + +% This is an alternative strategy to computing a Cartesian trajectory +% and solving the inverse kinematics. However like that other scheme, this +% strategy also runs into difficulty at a manipulator singularity where +% the Jacobian is singular. + +% As already stated this Jacobian relates joint velocity to end-effector +% velocity expressed in the end-effector reference frame. We may wish +% instead to specify the velocity in base or world coordinates. +% +% We have already seen how differential motions in one frame can be translated +% to another. Consider the velocity as a differential in the world frame, that +% is, d0X. We can write +% d6X = Jac(T6) d0X + +T6 = p560.fkine(q); % compute the end-effector transform +d6X = tr2jac(T6) * vel; % translate world frame velocity to T6 frame +qvel = Ji * d6X; % compute required joint velocity as before +qvel' + +% Note that this value of joint velocity is quite different to that calculated +% above, which was for motion in the T6 X-axis direction. + +% At a manipulator singularity or degeneracy the Jacobian becomes singular. +% At the Puma's `ready' position for instance, two of the wrist joints are +% aligned resulting in the loss of one degree of freedom. This is revealed by +% the rank of the Jacobian +rank( p560.jacobn(qr) ) + +% and the singular values are +svd( jacobn(p560, qr) ) + +% When not actually at a singularity the Jacobian can provide information +% about how `well-conditioned' the manipulator is for making certain motions, +% and is referred to as `manipulability'. +% +% A number of scalar manipulability measures have been proposed. One by +% Yoshikawa + +p560.maniplty(q, 'yoshikawa') + +% is based purely on kinematic parameters of the manipulator. +% +% Another by Asada takes into account the inertia of the manipulator which +% affects the acceleration achievable in different directions. This measure +% varies from 0 to 1, where 1 indicates uniformity of acceleration in all +% directions + +p560.maniplty(q, 'asada') + +% Both of these measures would indicate that this particular pose is not well +% conditioned. diff --git a/lwrserv/matlab/rvctools/robot/demos/joytest.m b/lwrserv/matlab/rvctools/robot/demos/joytest.m new file mode 100644 index 0000000..7e2014d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/joytest.m @@ -0,0 +1,32 @@ + +% 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.petercorke.com + +%%begin +% if you have a gaming joystick connected to your computer you use the +% sticks to control the position and orientation of a coordinate frame. + +T = eye(4,4); +h = trplot(T, 'axis', [-5 5 -5 5 -5 5]); + +while true + T = joy2tr(T, 'tool'); + trprint(T, 'fmt', '%.1f') + trplot(h, T); +end diff --git a/lwrserv/matlab/rvctools/robot/demos/particlefilt.m b/lwrserv/matlab/rvctools/robot/demos/particlefilt.m new file mode 100644 index 0000000..e1a57f5 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/particlefilt.m @@ -0,0 +1,95 @@ +% 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.petercorke.com + +%%begin + +% Our localization system requires a number of components: +% * a vehicle +% * a map that defines the positions of some known landmarks in the world +% * a sensor, a range-bearing sensor in this case +% * a localization filter, specifically the Monte-Carlo style "particle filter" + +% Creating the vehicle. First we define the covariance of the vehicles's odometry +% which reports distance travelled and change in heading angle + +V = diag([0.1, 1*pi/180].^2); + +% then use this to create an instance of a Vehicle class +veh = Vehicle(V); + +% and then add a "driver" to move it between random waypoints in a square +% region with dimensions from -10 to +10 + +veh.add_driver( RandomPath(10) ); + +% Creating the map. The map covers a square region with dimensions from +% -10 to +10 and contains 20 randomly placed landmarks +map = Map(20, 10); + +% Creating the sensor. We firstly define the covariance of the sensor measurements +% which report distance and bearing angle +W = diag([0.1, 1*pi/180].^2); + +% and then use this to create an instance of the Sensor class. +sensor = RangeBearingSensor(veh, map, W, 'animate'); +% Note that the sensor is mounted on the moving robot and observes the features +% in the world so it is connected to the already created Vehicle and Map objects. + +% Create the filter. The particle filter requires a likelihood function that maps +% the error between expected and actual sensor observation to a weight. The Toolbox +% uses a 2D Gaussian for this and we need to describe it by a covariance matrix +L = diag([0.1 0.1]); + +% The filter also needs a noise model to "drift" the particles at each step, that +% is the hypotheses are randomly moved to model the effect of uncertainty in the +% vehicle's pose +Q = diag([0.1, 0.1, 1*pi/180]).^2; +% the values of this matrix should be consistent with the vehicle uncertainty +% model V given above. + +% Now we create an instance of the particle filter class +pf = ParticleFilter(veh, sensor, Q, L, 1000); +% and connect it to the vehicle and the sensor and give estimates of the vehicle +% and sensor covariance (we never know this is practice). The last argument +% is the number of particles that will be used. Each particle represents a hypothesis +% about the vehicle's pose and a weight (or likeliness). + +% Now we will run the filter for 200 time steps. At each step the vehicle +% moves, reports its odometry and the sensor measurements and the filter updates +% its estimate of the vehicle's pose. +% +% The green dots represent the particles. We see that initially the pose +% hypotheses are very spread out, but soon start to cluster around the actual pose +% of the robot. The pose is 3D (x,y, theta) so if you rotate the graph while the +% simulation is running you can see the theta dimension as well. +pf.run(200); +% all the results of the simulation are stored within the ParticleFilter object + +% First let's plot the map +clf; map.plot() +% and then overlay the path actually taken by the vehicle +veh.plot_xy('b'); +% and then overlay the path estimated by the filter +pf.plot_xy('r'); +% which we see are pretty close once the filter gets going, the initial estimates +% (when the particles are spread widely) are not so good. + +% The uncertainty of the estimate is related to the spread of the particles and +% we can plot that +plot(pf.std); xlabel('time step'); ylabel('standard deviation'); legend('x', 'y', '\theta'); grid diff --git a/lwrserv/matlab/rvctools/robot/demos/prmnav.m b/lwrserv/matlab/rvctools/robot/demos/prmnav.m new file mode 100644 index 0000000..42fd9cd --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/prmnav.m @@ -0,0 +1,53 @@ +% 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.petercorke.com + +%%begin + +% Our robot will operate within a grid world that contains free +% space where it can drive and obstacles. We load a map of the world +load map1 +% which loads a variable called map +about map +% and the cells contain zero if it is free space (driveable) and one if it is an +% obstacle. + +% Now we create an instance of a robot with the PRM navigation algorithm +prm = PRM(map); + +% and because PRM is a probabilistic method we will reset the random number +% generator to a known state +randinit + +% Now we define the goal and start coordinates +goal = [50,30]; +start = [20, 10]; + +% then ask the robot to plan a path to goal (it will take few seconds) +prm.plan(); + +% The roadmap planner does not need to know, yet, the goal or start positions, it +% chooses random points in the world and tries to find obstacle-free paths between +% them (like railway lines or freeways) + +% Now we can display the obstacles and the cost to reach the goal from every +% point in the world +prm.plot(); + +% Now we can execute the planned path, it will be animated with green dots +prm.path(start, goal) diff --git a/lwrserv/matlab/rvctools/robot/demos/quadrotor.m b/lwrserv/matlab/rvctools/robot/demos/quadrotor.m new file mode 100644 index 0000000..77d43cf --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/quadrotor.m @@ -0,0 +1,29 @@ +% 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.petercorke.com + +%%begin + +% A quadrotor is a simple flying vehicle with 4 propellers with their thrust +% axes pointing upwards. This demo shows an animation of a flying quadrotor +% with a nested control system. + +sl_quadrotor + +% Running the simulation shows the vehicle takes off and flying in a cirle. +sim('sl_quadrotor'); diff --git a/lwrserv/matlab/rvctools/robot/demos/robot.m b/lwrserv/matlab/rvctools/robot/demos/robot.m new file mode 100644 index 0000000..a163b8b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/robot.m @@ -0,0 +1,59 @@ +% 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.petercorke.com + +%%begin + +% A serial link manipulator comprises a series of links. Each link is described +% by four Denavit-Hartenberg parameters. +% +% Let's define a simple 2 link manipulator. The first link is + +L1 = Link('d', 0, 'a', 1, 'alpha', pi/2) + +% The Link object we created has a number of properties +L1.a +L1.d + +% and we determine that it is a revolute joint +L1.isrevolute + +% For a given joint angle, say q=0.2 rad, we can determine the link transform +% matrix +L1.A(0.2) + +% The second link is +L2 = Link('d', 0, 'a', 1, 'alpha', 0) + +% Now we need to join these into a serial-link robot manipulator + +bot = SerialLink([L1 L2], 'name', 'my robot') +% The displayed robot object shows a lot of details. It also has a number of +% properties such as the number of joints +bot.n + +% Given the joint angles q1 = 0.1 and q2 = 0.2 we can determine the pose of the +% robot's end-effector + +bot.fkine([0.1 0.2]) +% which is referred to as the forward kinematics of the robot. This, and the +% inverse kinematics are covered in separate demos. + +% Finally we can draw a stick figure of our robot + +bot.plot([0.1 0.2]) diff --git a/lwrserv/matlab/rvctools/robot/demos/rotation.m b/lwrserv/matlab/rvctools/robot/demos/rotation.m new file mode 100644 index 0000000..205ee9f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/rotation.m @@ -0,0 +1,93 @@ +% 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.petercorke.com + +%%begin + +% In the field of robotics there are many possible ways of representing +% orientations of which the most common are: +% - orthonormal rotation matrices (3x3), +% - three angles (1x3), and +% - quaternions. + +% A rotation of pi/2 about the x-axis can be represented as an orthonormal rotation +% matrix + +R = rotx(pi/2) +% which we can see is a 3x3 matrix. + +% Such a matrix has the property that it's columns (and rows) are sets of orthogonal +% unit vectors. The determinant of such a matrix is always 1 + +det(R) + +% Let's create a more complex rotation + +R = rotx(30, 'deg') * roty(50, 'deg') * rotz(10, 'deg') +% where this time we have specified the rotation angle in degrees. + +% Any rotation can be expressed in terms of a single rotation about some axis +% in space + +[theta,vec] = tr2angvec(R) +% where theta is the angle (in radians) and vec is unit vector representing the +% direction of the rotation axis. + +% Commonly rotations are represented by Euler angles + +eul = tr2eul(R) +% which are three angles such that R = rotz(a)*roty(b)*rotz(c), ie. the rotations +% required about the Z, then then the Y, then the Z axis. + +% Rotations are also commonly represented by roll-pitch-yaw angles + +rpy = tr2rpy(R) +% which are three angles such that R = rotx(r)*roty(p)*rotz(y), ie. the rotations +% required about the X, then then the Y, then the Z axis. + +% We can investigate the effects of rotations about different axes +% using this GUI based demonstration. The menu buttons allow the rotation +% axes to be varied +% close the window when you are done. +tripleangle('rpy', 'wait') + +% The final useful form is the quaternion which comprises 4 numbers. We can create +% a quaternion from an orthonormal matrix + +q = Quaternion(R) +% where we can see that it comprises a scalar part and a vector part. To convert back + +q.R +% which is the same of the value of R we determined above. + +% Quaternions are a class and the orientations they represent can be compounded, just +% as we do with rotation matrices by multiplication. + +% First we create two quaternions + +q1 = Quaternion( rotx(pi/2) ) +q2 = Quaternion( roty(pi/2) ) + +% then the rotation of q1 followed by q2 is simply + +q1 * q2 + +% We can also take the inverse of a Quaternion + +q1 * inv(q1) +% which results in a null rotation (zero vector part) diff --git a/lwrserv/matlab/rvctools/robot/demos/scene1.ttt b/lwrserv/matlab/rvctools/robot/demos/scene1.ttt new file mode 100644 index 0000000..424a6be Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/demos/scene1.ttt differ diff --git a/lwrserv/matlab/rvctools/robot/demos/slam.m b/lwrserv/matlab/rvctools/robot/demos/slam.m new file mode 100644 index 0000000..faa0cd1 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/slam.m @@ -0,0 +1,87 @@ +% 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.petercorke.com + +%%begin + +% Our SLAM system requires a number of components: +% * a vehicle +% * a map that defines the positions of some known landmarks in the world +% * a sensor, a range-bearing sensor in this case +% * a SLAM filter + +% Creating the vehicle. First we define the covariance of the vehicles's odometry +% which reports distance travelled and change in heading angle + +V = diag([0.005, 0.5*pi/180].^2); + +% then use this to create an instance of a Vehicle class +veh = Vehicle(V); + +% and then add a "driver" to move it between random waypoints in a square +% region with dimensions from -10 to +10 + +veh.add_driver( RandomPath(10) ); + +% Creating the map. The map covers a square region with dimensions from +% -10 to +10 and contains 20 randomly placed landmarks +map = Map(20, 10); + +% Creating the sensor. We firstly define the covariance of the sensor measurements +% which report distance and bearing angle +W = diag([0.1, 1*pi/180].^2); + +% and then use this to create an instance of the Sensor class. +sensor = RangeBearingSensor(veh, map, W, 'animate'); +% Note that the sensor is mounted on the moving robot and observes the features +% in the world so it is connected to the already created Vehicle and Map objects. + +% Create the filter. First we need to determine the initial covariance of the +% vehicle, this is our uncertainty about its pose (x, y, theta) +P0 = diag([0.005, 0.005, 0.001].^2); + +% Now we create an instance of the EKF filter class +ekf = EKF(veh, V, P0, sensor, W, []); +% and connect it to the vehicle and the sensor and give estimates of the vehicle +% and sensor covariance (we never know this is practice). + +% Now we will run the filter for 1000 time steps. At each step the vehicle +% moves, reports its odometry and the sensor measurements and the filter updates +% its estimate of the vehicle's pose +ekf.run(1000); +% all the results of the simulation are stored within the EKF object + +% First let's plot the map +clf; map.plot() +% and then overlay the path actually taken by the vehicle +veh.plot_xy('b'); +% and then overlay the path estimated by the filter +ekf.plot_xy('r'); +% which we see are pretty close + +% Now let's plot the error in estimating the pose +ekf.plot_error() +% and this is overlaid with the estimated covariance of the error. + +% Remember that the SLAM filter has not only estimated the robot's pose, it has +% simultaneously estimated the positions of the landmarks as well. How well did it +% do at that task? We will show the landmarks in the map again +map.plot(); +% and this time overlay the estimated landmark (with a +) and the 3sigma +% uncertainty bounds as green ellipses +ekf.plot_map(3,'g'); diff --git a/lwrserv/matlab/rvctools/robot/demos/symbolic.m b/lwrserv/matlab/rvctools/robot/demos/symbolic.m new file mode 100644 index 0000000..16ad5a2 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/symbolic.m @@ -0,0 +1,66 @@ +% 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.petercorke.com + +%%begin + +% A large number of Toolbox functions can operate on symbolic rather than +% numeric quantities. +% +% Consider the simple case of a rotation matrix + +syms a +rotx(a) + +% Or a more complex example for Euler angles + +syms a b c +eul2r(a, b, c) + +% Now let's consider a robot link with symbolic parameters + +syms q A D alpha + +L = Link('d', D, 'a', A, 'alpha', a, 'revolute'); + +% the link transform matrix for a joint angle q is then + +L.A(q) + +% Consider now a simple two-link robot, we load the model + +mdl_twolink + +% which has created a robot model in the workspace + +twolink + +% Now this is a numeric robot model, and we need to create a symbolic model + +twolink_sym = twolink.sym() + +% which appears very similar, however all the constants are now symbolic rather +% than numeric. + +% Next define the two joint angles as symbolic variables + +syms q1 q2 + +% and then the forward kinematics is + +twolink_sym.fkine([q1, q2]) diff --git a/lwrserv/matlab/rvctools/robot/demos/traj.m b/lwrserv/matlab/rvctools/robot/demos/traj.m new file mode 100644 index 0000000..eb85949 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/traj.m @@ -0,0 +1,87 @@ +% 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.petercorke.com + +%%begin + +% Frequently we want to define a smooth sequence of positions (or poses) from +% one point to another. First consider the 1-dimensional case. +% +% We define the start and end position + +p0 = -1; +p1 = 2; + +% and a smooth path from p0 to p1 in 50 time steps is given by + +p = tpoly(p0, p1, 50); +about p +% which we see has 50 rows. We can plot this + +plot(p) + +% and see that it does indeed move smoothly from p0 to p1 and that the initial +% and final derivative (and second derivative) is zero. + +% We can also get the velocity and acceleration + +[p,pd,pdd] = tpoly(p0, p1, 50); +subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p'); +subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd'); +subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd'); + +% This path is a 5th order polynomial and it suffers from the disadvantage that +% the velocity is mostly below the maximum possible value. An alternative is + +[p,pd,pdd] = lspb(p0, p1, 50); +subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p'); +subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd'); +subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd'); +% which we see has a trapezoidal velocity profile. + +% Frequently the start and end values are vectors, not scalars, perhaps a 3D +% position or Euler angles. In this case we apply the scalar trajectory function +% to a vector with + +p = mtraj(@tpoly, [0 1 2], [2 1 0], 50); +about p +% and p again has one row per time step, and one column per vector dimension + +clf; plot(p) + +%--- +% Finally, we may wish to interpolate poses. We will define a start and end pose + +T0 = transl(0.4, 0.2, 0) * trotx(pi); +T1 = transl(-0.4, -0.2, 0.3) * troty(pi/2) * trotz(-pi/2); + +% and a smooth sequence between them in 50 steps is + +T = ctraj(T0, T1, 50); +about T +% which is a 4x4x50 matrix. The first pose is + +T(:,:,1) + +% and the 10th pose is + +T(:,:,10) + +% We can plot the motion of this coordinate frame by + +clf; tranimate(T) diff --git a/lwrserv/matlab/rvctools/robot/demos/trans.m b/lwrserv/matlab/rvctools/robot/demos/trans.m new file mode 100644 index 0000000..c5947b9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/trans.m @@ -0,0 +1,63 @@ +% 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.petercorke.com + +%%begin +% In the field of robotics there are many possible ways of representing +% positions and orientations, but the homogeneous transformation is well +% matched to MATLABs powerful tools for matrix manipulation. +% +% Homogeneous transformations describe the relationships between Cartesian +% coordinate frames in terms of translation and orientation. + +% A pure translation of 0.5m in the X direction is represented by + +transl(0.5, 0.0, 0.0) + +% a rotation of 90degrees about the Y axis by + +troty(pi/2) + +% and a rotation of -90degrees about the Z axis by + +trotz(-pi/2) + +% these may be concatenated by multiplication + +t = transl(0.5, 0.0, 0.0) * troty(pi/2) * trotz(-pi/2) + +% +% If this transformation represented the origin of a new coordinate frame with respect +% to the world frame origin (0, 0, 0), that new origin would be given by + +t * [0 0 0 1]' + +% the orientation of the new coordinate frame may be expressed in terms of +% Euler angles + +tr2eul(t) + +% or roll/pitch/yaw angles + +tr2rpy(t) + +% It is important to note that tranform multiplication is in general not +% commutative as shown by the following example + +trotx(pi/2) * trotz(-pi/8) +trotz(-pi/8) * trotx(pi/2) diff --git a/lwrserv/matlab/rvctools/robot/demos/vrepdemo.m b/lwrserv/matlab/rvctools/robot/demos/vrepdemo.m new file mode 100644 index 0000000..f6b24b5 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/vrepdemo.m @@ -0,0 +1,105 @@ +% open a connection to the VREP simulator +% +% Notes:: +% - the VREP constructor needs to know where V-REP is installed. +% - This can come from the environment variable VREP +% - Or you can edit this file to something like +% vrep=VREP('/Applications/V-REP_PRO_EDU_V3_1_3_rev2b_Mac'); + +%%begin + +% create a connection to a running instance of V-REP +vrep = VREP(); + +% let's change our viewpoint, move the camera further away +% +% first we get an object to mirror the default camera +cam = vrep.camera('DefaultCamera'); + +% then get the camera position +p = cam.getpos + +% then set the camera be twice as far away +cam.setpos(2*p) + +% add the human figure Bill +bill = vrep.loadmodel('people/Walking Bill'); + +% get the initial pose of Bill +T = bill.getpose() + +% now we can change his position +bill.setpos([0.1, 0.2, 0]); + +% make him turn to his right +bill.setorient([0 0 -pi/4]) + +% make him lean forward a bit +bill.setorient([0 pi/8 0]) + +% now set him back to his initial position and orientation +bill.setpose(T); + +% now we will setup some nested loops to make him shuffle around a square +orient = [0 0 0]; +for j=1:4 + step = [0.05 0 0]; + bill.setorient(orient); + % move forward + for i=1:30 + % move relative to current pose + bill.setpos(step, bill); + end + % turn at right angles + orient(3) = orient(3) + pi/2; +end + +% remove the model from the scene +bill.remove(); + +% now we will load a scene into VREP that contains Bill, a tree, a mirror +% and a camera +vrep.loadscene(which('scene1.ttt'), 'local'); + +% we will get a handle to the camera +camera = vrep.camera('Vision_sensor'); + +% now we start the simulator, it must be running in order for the +% camera to render + +vrep.simstart(); + +% now we loop as the camera creeps forward (in the z-direction) and we +% display the frames +for i=1:20 + camera.setpos([0 0 0.05], camera); + camera.grab(); + drawnow +end + +% stop the simulation +vrep.simstop(); + +% clear the scene, remove all objects +vrep.clearscene(); + +% add a model of an ABB IRB140 robot +model = vrep.loadmodel('robots/non-mobile/ABB IRB 140'); + +% and create a VREP_arm object +arm = vrep.arm(model); + +% set the pose of the robot's base +arm.setpose( transl(0.3, 0.4, 0)); + +% get the joint angles +q = arm.getq(); + +% now move the robot's joints +arm.setq(q+0.3); + +% clear the scene, remove all objects +vrep.clearscene(); + +% close the connection to the VREP simulator +clear vrep \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/demos/ztorque.m b/lwrserv/matlab/rvctools/robot/demos/ztorque.m new file mode 100644 index 0000000..7ff6b33 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/demos/ztorque.m @@ -0,0 +1,38 @@ +% 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.petercorke.com + +%%begin + +% We will work with a model of the Puma 560 robot +mdl_puma560 + +% to make the numerical simulation work faster we will remove the non-linear +% Coloumb friction that exists in the model + +p560 = p560.nofriction() + +% A simple Simulink model that connects a robot dynamics block +% to a robot plot block + +sl_ztorque + +% The robot has zero applied torque at each joint, so when we run the simulation it +% will collapse under its own weight + +sim('sl_ztorque') diff --git a/lwrserv/matlab/rvctools/robot/distancexform.m b/lwrserv/matlab/rvctools/robot/distancexform.m new file mode 100644 index 0000000..66df58b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/distancexform.m @@ -0,0 +1,106 @@ +%DISTANCEXFORM Distance transform of occupancy grid +% +% D = DISTANCEXFORM(WORLD, GOAL) is the distance transform of the occupancy +% grid WORLD with respect to the specified goal point GOAL = [X,Y]. The +% elements of the grid are 0 from free space and 1 for occupied. +% +% D = DISTANCEXFORM(WORLD, GOAL, METRIC) as above but specifies the distance +% metric as either 'cityblock' or 'Euclidean' +% +% D = DISTANCEXFORM(WORLD, GOAL, METRIC, SHOW) as above but shows an animation +% of the distance transform being formed, with a delay of SHOW seconds between +% frames. +% +% Notes:: +% - The Machine Vision Toolbox function imorph is required. +% - The goal is [X,Y] not MATLAB [row,col] +% +% See also IMORPH, DXform. + +% 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 . + +function d = distancexform(world, goal, metric, show) + + if exist('imorph', 'file') ~= 3 + error('Machine Vision Toolbox is required by this function'); + end + + if nargin < 4 + show = 0; + end + + % set up the distance metrics + if nargin < 3 + metric = 'cityblock'; + end + + if strncmpi(metric, 'cityblock', length(metric)) + m = ones(3,3); + m(2,2) = 0; + elseif strncmpi(metric, 'euclidean', length(metric)) + r2 = sqrt(2); + m = [r2 1 r2; 1 0 1; r2 1 r2]; + else + error('unknown distance metric'); + end + + % for the imorph primitive we need to set the target pixel to 0, + % obstacles to NaN and the rest to Inf. + + if ~isempty(goal) + % specific goal point + if world(goal(2), goal(1)) > 0 + error('goal inside obstacle') + end + % point goal case + world(world>0) = NaN; + world(world==0) = Inf; + world(goal(2), goal(1)) = 0; + else + world = double(world); + world(world==0) = Inf; + world(isfinite(world)) = 0; + idisp(world) + end + + + count = 0; + while 1 + world = imorph(world, m, 'plusmin'); + count = count+1; + if show + cmap = [1 0 0; gray(256)]; + colormap(cmap) + image(world+1, 'CDataMapping', 'direct'); + set(gca, 'Ydir', 'normal'); + xlabel('x'); + ylabel('y'); + pause(show); + end + + if ~any(isinf(world(:))) + % stop if no Inf's left in the map + break; + end + end + + if show + fprintf('%d iterations\n', count); + end + + d = world; diff --git a/lwrserv/matlab/rvctools/robot/eul2jac.m b/lwrserv/matlab/rvctools/robot/eul2jac.m new file mode 100644 index 0000000..b26c736 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/eul2jac.m @@ -0,0 +1,45 @@ +%EUL2JAC Euler angle rate Jacobian +% +% J = EUL2JAC(EUL) is a Jacobian matrix (3x3) that maps Euler angle rates to +% angular velocity at the operating point EUL=[PHI, THETA, PSI]. +% +% J = EUL2JAC(PHI, THETA, PSI) as above but the Euler angles are passed +% as separate arguments. +% +% Notes:: +% - Used in the creation of an analytical Jacobian. +% +% See also RPY2JAC, SERIALlINK.JACOBN. + + +% 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 . + +function J = eul2jac(phi, theta, psi) + + if length(phi) == 3 + % eul2jac([phi theta psi]) + theta = phi(2); + psi = phi(3); + phi = phi(1); + elseif nargin ~= 3 + error('RTB:eul2jac:badarg', 'bad arguments'); + end +J = [ 0, -sin(phi), cos(phi)*sin(theta) + 0, cos(phi), sin(phi)*sin(theta) + 1, 0, cos(theta) ]; + diff --git a/lwrserv/matlab/rvctools/robot/eul2r.m b/lwrserv/matlab/rvctools/robot/eul2r.m new file mode 100644 index 0000000..35d2416 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/eul2r.m @@ -0,0 +1,69 @@ +%EUL2R Convert Euler angles to rotation matrix +% +% R = EUL2R(PHI, THETA, PSI, OPTIONS) is an orthonornal rotation matrix +% equivalent to the specified Euler angles. These correspond to rotations +% about the Z, Y, Z axes respectively. If PHI, THETA, PSI are column vectors +% then they are assumed to represent a trajectory and R is a three dimensional +% matrix, where the last index corresponds to rows of PHI, THETA, PSI. +% +% R = EUL2R(EUL, OPTIONS) as above but the Euler angles are taken from +% consecutive columns of the passed matrix EUL = [PHI THETA PSI]. +% +% Options:: +% 'deg' Compute angles in degrees (radians default) +% +% Note:: +% - The vectors PHI, THETA, PSI must be of the same length. +% +% See also EUL2TR, RPY2TR, TR2EUL. + + +% 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 . + +function R = eul2r(phi, varargin) + opt.deg = false; + [opt,args] = tb_optparse(opt, varargin); + + % unpack the arguments + if numcols(phi) == 3 + theta = phi(:,2); + psi = phi(:,3); + phi = phi(:,1); + elseif nargin >= 3 + theta = args{1}; + psi = args{2}; + else + error('bad arguments') + end + + % optionally convert from degrees + if opt.deg + d2r = pi/180.0; + phi = phi * d2r; + theta = theta * d2r; + psi = psi * d2r; + end + + if numrows(phi) == 1 + R = rotz(phi) * roty(theta) * rotz(psi); + else + R = zeros(3,3,numrows(phi)); + for i=1:numrows(phi) + R(:,:,i) = rotz(phi(i)) * roty(theta(i)) * rotz(psi(i)); + end + end diff --git a/lwrserv/matlab/rvctools/robot/eul2tr.m b/lwrserv/matlab/rvctools/robot/eul2tr.m new file mode 100644 index 0000000..9a46989 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/eul2tr.m @@ -0,0 +1,42 @@ +%EUL2TR Convert Euler angles to homogeneous transform +% +% T = EUL2TR(PHI, THETA, PSI, OPTIONS) is a homogeneous transformation +% equivalent to the specified Euler angles. These correspond to rotations +% about the Z, Y, Z axes respectively. If PHI, THETA, PSI are column vectors +% then they are assumed to represent a trajectory and R is a three dimensional +% matrix, where the last index corresponds to rows of PHI, THETA, PSI. +% +% T = EUL2TR(EUL, OPTIONS) as above but the Euler angles are taken from +% consecutive columns of the passed matrix EUL = [PHI THETA PSI]. +% +% Options:: +% 'deg' Compute angles in degrees (radians default) +% +% Note:: +% - The vectors PHI, THETA, PSI must be of the same length. +% - The translational part is zero. +% +% See also EUL2R, RPY2TR, TR2EUL. + + +% 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 . + +function T = eul2tr(phi, varargin) + + R = eul2r(phi, varargin{:}); + T = r2t(R); diff --git a/lwrserv/matlab/rvctools/robot/examples/braitenberg.m b/lwrserv/matlab/rvctools/robot/examples/braitenberg.m new file mode 100644 index 0000000..0c0c096 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/braitenberg.m @@ -0,0 +1,70 @@ +function p = braitenberg(world, robot) + % these are coordinates (x,y) + + % display the world, obstacle, robot and target + image(world/max(world(:))*100) + colormap(gray); + set(gca, 'Ydir', 'normal'); + xlabel('x'); + ylabel('y'); + hold on + + if nargin < 2 + disp('Click on a start point'); + [x,y] = ginput(1); + robot = [x y]; + end + + if length(robot) == 2 + T = transl( [robot 0] ); + else + T = transl( [robot(1:2) 0] ) * trotz(robot(3)); + end + + + p = []; + + sensorOffset = 2; + forwardStep = 1.0; + + while 1 + + % read the left and right sensor + leftSensor = sensorRead(world, T, transl(0, -sensorOffset, 0)); + rightSensor = sensorRead(world, T, transl(0, sensorOffset, 0)); + + % change our heading based on the ratio of the sensors + dth = rightSensor / leftSensor - 1; + + % bad sensor reading means we fell off the world + if isnan(dth) + break + end + + % update the robot's pose + dT = trotz(dth) * transl(forwardStep, 0, 0); + T = T * dT; + + % display the robot's position + xyz = transl( T ); % get position + rpy = tr2rpy( T ); % get heading angle + plot(xyz(1), xyz(2), 'g.'); + + p = [p; xyz(1:2)' rpy(3)]; + pause(0.1); + + if numrows(p) > 150 + break; + end + end + hold off + +end + +% return the sensor reading for the given robot pose and relative pose +% of the sensor. +function s = sensorRead(field, robot, Tsens) + xyz = transl( robot * Tsens ); + + s = interp2(field, xyz(1), xyz(2)); +end diff --git a/lwrserv/matlab/rvctools/robot/examples/eg_grav.m b/lwrserv/matlab/rvctools/robot/examples/eg_grav.m new file mode 100644 index 0000000..36e29d6 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/eg_grav.m @@ -0,0 +1,9 @@ +[Q2,Q3] = meshgrid(-pi:0.1:pi, -pi:0.1:pi); +for i=1:numcols(Q2), + for j=1:numcols(Q3); + g = p560.gravload([0 Q2(i,j) Q3(i,j) 0 0 0]); + g2(i,j) = g(2); + g3(i,j) = g(3); + end +end +surfl(Q2, Q3, g2); surfl(Q2, Q3, g3); diff --git a/lwrserv/matlab/rvctools/robot/examples/eg_inertia.m b/lwrserv/matlab/rvctools/robot/examples/eg_inertia.m new file mode 100644 index 0000000..f90ad4e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/eg_inertia.m @@ -0,0 +1,9 @@ +[Q2,Q3] = meshgrid(-pi:0.1:pi, -pi:0.1:pi); +for i=1:numcols(Q2), + for j=1:numcols(Q3); + M = p560.inertia([0 Q2(i,j) Q3(i,j) 0 0 0]); + M11(i,j) = M(1,1); + M12(i,j) = M(1,2); + end +end +surfl(Q2, Q3, M11); surfl(Q2, Q3, M12); diff --git a/lwrserv/matlab/rvctools/robot/examples/eg_inertia22.m b/lwrserv/matlab/rvctools/robot/examples/eg_inertia22.m new file mode 100644 index 0000000..08f8939 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/eg_inertia22.m @@ -0,0 +1,8 @@ +Q3 = -pi:0.1:pi; +for j=1:numcols(Q3); + M = p560.inertia([0 0 Q3(j) 0 0 0]); + M22(j) = M(2,2); +end +plot(Q3, M22) +xlabel('q_3 (rad)'); +ylabel('M_{22}'); diff --git a/lwrserv/matlab/rvctools/robot/examples/gait.m b/lwrserv/matlab/rvctools/robot/examples/gait.m new file mode 100644 index 0000000..fd21ee3 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/gait.m @@ -0,0 +1,7 @@ +function q = gait(cycle, k, offset, flip) + k = mod(k+offset-1, numrows(cycle)) + 1; + q = cycle(k,:); + if flip + q(1) = -q(1); % for left-side legs + end +end diff --git a/lwrserv/matlab/rvctools/robot/examples/joytest.m b/lwrserv/matlab/rvctools/robot/examples/joytest.m new file mode 100644 index 0000000..89d8a43 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/joytest.m @@ -0,0 +1,31 @@ + +% 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.petercorke.com + +mdl_puma560 + +T = p560.fkine(qn); +q = qn; + +while true + T=joy2tr(T, 'tool'); + trprint(T, 'fmt', '%.1f') + q = p560.ikine6s(T, q); + p560.plot(q) +end diff --git a/lwrserv/matlab/rvctools/robot/examples/link6.stl b/lwrserv/matlab/rvctools/robot/examples/link6.stl new file mode 100644 index 0000000..3be8071 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/link6.stl @@ -0,0 +1,15290 @@ +solid PN003001p_v3 + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 9.396926e-003 3.420201e-003 4.300000e-002 + vertex 9.848078e-003 1.736482e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.660254e-003 -5.000000e-003 4.300000e-002 + vertex -9.396926e-003 -3.420201e-003 4.300000e-002 + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 9.396926e-003 3.420201e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 8.660254e-003 5.000000e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.660254e-003 -5.000000e-003 4.300000e-002 + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + vertex -1.736482e-003 9.848078e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex -1.736482e-003 9.848078e-003 4.300000e-002 + vertex 0.000000e+000 1.000000e-002 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 1.000000e-002 4.300000e-002 + vertex 1.736482e-003 9.848078e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 1.736482e-003 9.848078e-003 4.300000e-002 + vertex 3.420201e-003 9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 3.420201e-003 9.396926e-003 4.300000e-002 + vertex 5.000000e-003 8.660254e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-003 8.660254e-003 4.300000e-002 + vertex 6.427876e-003 7.660444e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 6.427876e-003 7.660444e-003 4.300000e-002 + vertex 7.660444e-003 6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 7.660444e-003 6.427876e-003 4.300000e-002 + vertex 8.660254e-003 5.000000e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -9.396926e-003 -3.420201e-003 4.300000e-002 + vertex -9.848078e-003 -1.736482e-003 4.300000e-002 + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + vertex -9.848078e-003 -1.736482e-003 4.300000e-002 + vertex -1.000000e-002 1.224647e-018 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + vertex -1.000000e-002 1.224647e-018 4.300000e-002 + vertex -9.848078e-003 1.736482e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.660254e-003 5.000000e-003 4.300000e-002 + vertex -7.660444e-003 6.427876e-003 4.300000e-002 + vertex -6.427876e-003 7.660444e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 9.848078e-003 1.736482e-003 4.300000e-002 + vertex 1.000000e-002 0.000000e+000 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 1.000000e-002 0.000000e+000 4.300000e-002 + vertex 9.848078e-003 -1.736482e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 9.848078e-003 -1.736482e-003 4.300000e-002 + vertex 9.396926e-003 -3.420201e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 9.396926e-003 -3.420201e-003 4.300000e-002 + vertex 8.660254e-003 -5.000000e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 8.660254e-003 -5.000000e-003 4.300000e-002 + vertex 7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 6.427876e-003 -7.660444e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.736482e-003 -9.848078e-003 4.300000e-002 + vertex -3.420201e-003 -9.396926e-003 4.300000e-002 + vertex -5.000000e-003 -8.660254e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -9.848078e-003 1.736482e-003 4.300000e-002 + vertex -9.396926e-003 3.420201e-003 4.300000e-002 + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + vertex -9.396926e-003 3.420201e-003 4.300000e-002 + vertex -8.660254e-003 5.000000e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + vertex -8.660254e-003 5.000000e-003 4.300000e-002 + vertex -5.000000e-003 8.660254e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-003 8.660254e-003 4.300000e-002 + vertex -8.660254e-003 5.000000e-003 4.300000e-002 + vertex -6.427876e-003 7.660444e-003 4.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.427876e-003 -7.660444e-003 4.300000e-002 + vertex 5.000000e-003 -8.660254e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 5.000000e-003 -8.660254e-003 4.300000e-002 + vertex 3.420201e-003 -9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 3.420201e-003 -9.396926e-003 4.300000e-002 + vertex 1.736482e-003 -9.848078e-003 4.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.736482e-003 -9.848078e-003 4.300000e-002 + vertex -1.734723e-018 -1.000000e-002 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex -1.734723e-018 -1.000000e-002 4.300000e-002 + vertex -1.736482e-003 -9.848078e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex -1.736482e-003 -9.848078e-003 4.300000e-002 + vertex -6.427876e-003 -7.660444e-003 4.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.427876e-003 -7.660444e-003 4.300000e-002 + vertex -1.736482e-003 -9.848078e-003 4.300000e-002 + vertex -5.000000e-003 -8.660254e-003 4.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715547e-002 0.000000e+000 + outer loop + vertex 9.848078e-003 1.736482e-003 4.300000e-002 + vertex 1.000000e-002 0.000000e+000 7.200000e-002 + vertex 1.000000e-002 0.000000e+000 4.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715547e-002 0.000000e+000 + outer loop + vertex 1.000000e-002 0.000000e+000 4.300000e-002 + vertex 1.000000e-002 0.000000e+000 7.200000e-002 + vertex 9.848078e-003 -1.736482e-003 7.200000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715547e-002 0.000000e+000 + outer loop + vertex 1.000000e-002 0.000000e+000 4.300000e-002 + vertex 9.848078e-003 -1.736482e-003 7.200000e-002 + vertex 9.848078e-003 -1.736482e-003 4.300000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex 9.848078e-003 -1.736482e-003 4.300000e-002 + vertex 9.848078e-003 -1.736482e-003 7.200000e-002 + vertex 9.396926e-003 -3.420201e-003 7.200000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588190e-001 -1.737854e-018 + outer loop + vertex 9.848078e-003 -1.736482e-003 4.300000e-002 + vertex 9.396926e-003 -3.420201e-003 7.200000e-002 + vertex 9.396926e-003 -3.420201e-003 4.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex 9.396926e-003 -3.420201e-003 4.300000e-002 + vertex 9.396926e-003 -3.420201e-003 7.200000e-002 + vertex 8.660254e-003 -5.000000e-003 7.200000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 -3.475709e-018 + outer loop + vertex 9.396926e-003 -3.420201e-003 4.300000e-002 + vertex 8.660254e-003 -5.000000e-003 7.200000e-002 + vertex 8.660254e-003 -5.000000e-003 4.300000e-002 + endloop + endfacet + facet normal 8.191521e-001 -5.735765e-001 0.000000e+000 + outer loop + vertex 8.660254e-003 -5.000000e-003 4.300000e-002 + vertex 8.660254e-003 -5.000000e-003 7.200000e-002 + vertex 7.660444e-003 -6.427876e-003 7.200000e-002 + endloop + endfacet + facet normal 8.191521e-001 -5.735765e-001 -3.475709e-018 + outer loop + vertex 8.660254e-003 -5.000000e-003 4.300000e-002 + vertex 7.660444e-003 -6.427876e-003 7.200000e-002 + vertex 7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 7.660444e-003 -6.427876e-003 7.200000e-002 + vertex 6.427876e-003 -7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 7.660444e-003 -6.427876e-003 4.300000e-002 + vertex 6.427876e-003 -7.660444e-003 7.200000e-002 + vertex 6.427876e-003 -7.660444e-003 4.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 -8.191521e-001 0.000000e+000 + outer loop + vertex 6.427876e-003 -7.660444e-003 4.300000e-002 + vertex 6.427876e-003 -7.660444e-003 7.200000e-002 + vertex 5.000000e-003 -8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal 5.735765e-001 -8.191521e-001 3.475709e-018 + outer loop + vertex 6.427876e-003 -7.660444e-003 4.300000e-002 + vertex 5.000000e-003 -8.660254e-003 7.200000e-002 + vertex 5.000000e-003 -8.660254e-003 4.300000e-002 + endloop + endfacet + facet normal 4.226184e-001 -9.063077e-001 0.000000e+000 + outer loop + vertex 5.000000e-003 -8.660254e-003 4.300000e-002 + vertex 5.000000e-003 -8.660254e-003 7.200000e-002 + vertex 3.420201e-003 -9.396926e-003 7.200000e-002 + endloop + endfacet + facet normal 4.226184e-001 -9.063077e-001 3.475709e-018 + outer loop + vertex 5.000000e-003 -8.660254e-003 4.300000e-002 + vertex 3.420201e-003 -9.396926e-003 7.200000e-002 + vertex 3.420201e-003 -9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 3.420201e-003 -9.396926e-003 4.300000e-002 + vertex 3.420201e-003 -9.396926e-003 7.200000e-002 + vertex 1.736482e-003 -9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 1.737854e-018 + outer loop + vertex 3.420201e-003 -9.396926e-003 4.300000e-002 + vertex 1.736482e-003 -9.848078e-003 7.200000e-002 + vertex 1.736482e-003 -9.848078e-003 4.300000e-002 + endloop + endfacet + facet normal 8.715547e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.736482e-003 -9.848078e-003 4.300000e-002 + vertex 1.736482e-003 -9.848078e-003 7.200000e-002 + vertex -1.836970e-018 -1.000000e-002 7.200000e-002 + endloop + endfacet + facet normal 8.715547e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.736482e-003 -9.848078e-003 4.300000e-002 + vertex -1.836970e-018 -1.000000e-002 7.200000e-002 + vertex -1.836970e-018 -1.000000e-002 4.300000e-002 + endloop + endfacet + facet normal -8.715547e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.836970e-018 -1.000000e-002 4.300000e-002 + vertex -1.836970e-018 -1.000000e-002 7.200000e-002 + vertex -1.736482e-003 -9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal -8.715547e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.836970e-018 -1.000000e-002 4.300000e-002 + vertex -1.736482e-003 -9.848078e-003 7.200000e-002 + vertex -1.736482e-003 -9.848078e-003 4.300000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -1.736482e-003 -9.848078e-003 4.300000e-002 + vertex -1.736482e-003 -9.848078e-003 7.200000e-002 + vertex -3.420201e-003 -9.396926e-003 7.200000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 -1.737854e-018 + outer loop + vertex -1.736482e-003 -9.848078e-003 4.300000e-002 + vertex -3.420201e-003 -9.396926e-003 7.200000e-002 + vertex -3.420201e-003 -9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal -4.226184e-001 -9.063077e-001 0.000000e+000 + outer loop + vertex -3.420201e-003 -9.396926e-003 4.300000e-002 + vertex -3.420201e-003 -9.396926e-003 7.200000e-002 + vertex -5.000000e-003 -8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal -4.226184e-001 -9.063077e-001 -3.475709e-018 + outer loop + vertex -3.420201e-003 -9.396926e-003 4.300000e-002 + vertex -5.000000e-003 -8.660254e-003 7.200000e-002 + vertex -5.000000e-003 -8.660254e-003 4.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 -8.191521e-001 0.000000e+000 + outer loop + vertex -5.000000e-003 -8.660254e-003 4.300000e-002 + vertex -5.000000e-003 -8.660254e-003 7.200000e-002 + vertex -6.427876e-003 -7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal -5.735765e-001 -8.191521e-001 -3.475709e-018 + outer loop + vertex -5.000000e-003 -8.660254e-003 4.300000e-002 + vertex -6.427876e-003 -7.660444e-003 7.200000e-002 + vertex -6.427876e-003 -7.660444e-003 4.300000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -6.427876e-003 -7.660444e-003 4.300000e-002 + vertex -6.427876e-003 -7.660444e-003 7.200000e-002 + vertex -7.660444e-003 -6.427876e-003 7.200000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -6.427876e-003 -7.660444e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 7.200000e-002 + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal -8.191521e-001 -5.735765e-001 0.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex -7.660444e-003 -6.427876e-003 7.200000e-002 + vertex -8.660254e-003 -5.000000e-003 7.200000e-002 + endloop + endfacet + facet normal -8.191521e-001 -5.735765e-001 3.475709e-018 + outer loop + vertex -7.660444e-003 -6.427876e-003 4.300000e-002 + vertex -8.660254e-003 -5.000000e-003 7.200000e-002 + vertex -8.660254e-003 -5.000000e-003 4.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex -8.660254e-003 -5.000000e-003 4.300000e-002 + vertex -8.660254e-003 -5.000000e-003 7.200000e-002 + vertex -9.396926e-003 -3.420201e-003 7.200000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 3.475709e-018 + outer loop + vertex -8.660254e-003 -5.000000e-003 4.300000e-002 + vertex -9.396926e-003 -3.420201e-003 7.200000e-002 + vertex -9.396926e-003 -3.420201e-003 4.300000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex -9.396926e-003 -3.420201e-003 4.300000e-002 + vertex -9.396926e-003 -3.420201e-003 7.200000e-002 + vertex -9.848078e-003 -1.736482e-003 7.200000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588190e-001 1.737854e-018 + outer loop + vertex -9.396926e-003 -3.420201e-003 4.300000e-002 + vertex -9.848078e-003 -1.736482e-003 7.200000e-002 + vertex -9.848078e-003 -1.736482e-003 4.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715547e-002 0.000000e+000 + outer loop + vertex -9.848078e-003 -1.736482e-003 4.300000e-002 + vertex -9.848078e-003 -1.736482e-003 7.200000e-002 + vertex -1.000000e-002 1.224647e-018 7.200000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715547e-002 0.000000e+000 + outer loop + vertex -9.848078e-003 -1.736482e-003 4.300000e-002 + vertex -1.000000e-002 1.224647e-018 7.200000e-002 + vertex -1.000000e-002 1.224647e-018 4.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715547e-002 0.000000e+000 + outer loop + vertex -1.000000e-002 1.224647e-018 4.300000e-002 + vertex -1.000000e-002 1.224647e-018 7.200000e-002 + vertex -9.848078e-003 1.736482e-003 7.200000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715547e-002 0.000000e+000 + outer loop + vertex -1.000000e-002 1.224647e-018 4.300000e-002 + vertex -9.848078e-003 1.736482e-003 7.200000e-002 + vertex -9.848078e-003 1.736482e-003 4.300000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex -9.848078e-003 1.736482e-003 4.300000e-002 + vertex -9.848078e-003 1.736482e-003 7.200000e-002 + vertex -9.396926e-003 3.420201e-003 7.200000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588190e-001 -1.737854e-018 + outer loop + vertex -9.848078e-003 1.736482e-003 4.300000e-002 + vertex -9.396926e-003 3.420201e-003 7.200000e-002 + vertex -9.396926e-003 3.420201e-003 4.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 0.000000e+000 + outer loop + vertex -9.396926e-003 3.420201e-003 4.300000e-002 + vertex -9.396926e-003 3.420201e-003 7.200000e-002 + vertex -8.660254e-003 5.000000e-003 7.200000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 -3.475709e-018 + outer loop + vertex -9.396926e-003 3.420201e-003 4.300000e-002 + vertex -8.660254e-003 5.000000e-003 7.200000e-002 + vertex -8.660254e-003 5.000000e-003 4.300000e-002 + endloop + endfacet + facet normal -8.191521e-001 5.735765e-001 0.000000e+000 + outer loop + vertex -8.660254e-003 5.000000e-003 4.300000e-002 + vertex -8.660254e-003 5.000000e-003 7.200000e-002 + vertex -7.660444e-003 6.427876e-003 7.200000e-002 + endloop + endfacet + facet normal -8.191521e-001 5.735765e-001 -3.475709e-018 + outer loop + vertex -8.660254e-003 5.000000e-003 4.300000e-002 + vertex -7.660444e-003 6.427876e-003 7.200000e-002 + vertex -7.660444e-003 6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -7.660444e-003 6.427876e-003 4.300000e-002 + vertex -7.660444e-003 6.427876e-003 7.200000e-002 + vertex -6.427876e-003 7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -7.660444e-003 6.427876e-003 4.300000e-002 + vertex -6.427876e-003 7.660444e-003 7.200000e-002 + vertex -6.427876e-003 7.660444e-003 4.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 8.191521e-001 0.000000e+000 + outer loop + vertex -6.427876e-003 7.660444e-003 4.300000e-002 + vertex -6.427876e-003 7.660444e-003 7.200000e-002 + vertex -5.000000e-003 8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal -5.735765e-001 8.191521e-001 3.475709e-018 + outer loop + vertex -6.427876e-003 7.660444e-003 4.300000e-002 + vertex -5.000000e-003 8.660254e-003 7.200000e-002 + vertex -5.000000e-003 8.660254e-003 4.300000e-002 + endloop + endfacet + facet normal -4.226184e-001 9.063077e-001 0.000000e+000 + outer loop + vertex -5.000000e-003 8.660254e-003 4.300000e-002 + vertex -5.000000e-003 8.660254e-003 7.200000e-002 + vertex -3.420201e-003 9.396926e-003 7.200000e-002 + endloop + endfacet + facet normal -4.226184e-001 9.063077e-001 3.475709e-018 + outer loop + vertex -5.000000e-003 8.660254e-003 4.300000e-002 + vertex -3.420201e-003 9.396926e-003 7.200000e-002 + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + vertex -3.420201e-003 9.396926e-003 7.200000e-002 + vertex -1.736482e-003 9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 1.737854e-018 + outer loop + vertex -3.420201e-003 9.396926e-003 4.300000e-002 + vertex -1.736482e-003 9.848078e-003 7.200000e-002 + vertex -1.736482e-003 9.848078e-003 4.300000e-002 + endloop + endfacet + facet normal -8.715547e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -1.736482e-003 9.848078e-003 4.300000e-002 + vertex -1.736482e-003 9.848078e-003 7.200000e-002 + vertex 6.123234e-019 1.000000e-002 7.200000e-002 + endloop + endfacet + facet normal -8.715547e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -1.736482e-003 9.848078e-003 4.300000e-002 + vertex 6.123234e-019 1.000000e-002 7.200000e-002 + vertex 6.123234e-019 1.000000e-002 4.300000e-002 + endloop + endfacet + facet normal 8.715547e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex 6.123234e-019 1.000000e-002 4.300000e-002 + vertex 6.123234e-019 1.000000e-002 7.200000e-002 + vertex 1.736482e-003 9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal 8.715547e-002 9.961947e-001 0.000000e+000 + outer loop + vertex 6.123234e-019 1.000000e-002 4.300000e-002 + vertex 1.736482e-003 9.848078e-003 7.200000e-002 + vertex 1.736482e-003 9.848078e-003 4.300000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 -0.000000e+000 + outer loop + vertex 1.736482e-003 9.848078e-003 4.300000e-002 + vertex 1.736482e-003 9.848078e-003 7.200000e-002 + vertex 3.420201e-003 9.396926e-003 7.200000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 -1.737854e-018 + outer loop + vertex 1.736482e-003 9.848078e-003 4.300000e-002 + vertex 3.420201e-003 9.396926e-003 7.200000e-002 + vertex 3.420201e-003 9.396926e-003 4.300000e-002 + endloop + endfacet + facet normal 4.226184e-001 9.063077e-001 -0.000000e+000 + outer loop + vertex 3.420201e-003 9.396926e-003 4.300000e-002 + vertex 3.420201e-003 9.396926e-003 7.200000e-002 + vertex 5.000000e-003 8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal 4.226184e-001 9.063077e-001 -3.475709e-018 + outer loop + vertex 3.420201e-003 9.396926e-003 4.300000e-002 + vertex 5.000000e-003 8.660254e-003 7.200000e-002 + vertex 5.000000e-003 8.660254e-003 4.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 8.191521e-001 -0.000000e+000 + outer loop + vertex 5.000000e-003 8.660254e-003 4.300000e-002 + vertex 5.000000e-003 8.660254e-003 7.200000e-002 + vertex 6.427876e-003 7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal 5.735765e-001 8.191521e-001 -3.475709e-018 + outer loop + vertex 5.000000e-003 8.660254e-003 4.300000e-002 + vertex 6.427876e-003 7.660444e-003 7.200000e-002 + vertex 6.427876e-003 7.660444e-003 4.300000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 -0.000000e+000 + outer loop + vertex 6.427876e-003 7.660444e-003 4.300000e-002 + vertex 6.427876e-003 7.660444e-003 7.200000e-002 + vertex 7.660444e-003 6.427876e-003 7.200000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex 6.427876e-003 7.660444e-003 4.300000e-002 + vertex 7.660444e-003 6.427876e-003 7.200000e-002 + vertex 7.660444e-003 6.427876e-003 4.300000e-002 + endloop + endfacet + facet normal 8.191521e-001 5.735765e-001 -0.000000e+000 + outer loop + vertex 7.660444e-003 6.427876e-003 4.300000e-002 + vertex 7.660444e-003 6.427876e-003 7.200000e-002 + vertex 8.660254e-003 5.000000e-003 7.200000e-002 + endloop + endfacet + facet normal 8.191521e-001 5.735765e-001 3.475709e-018 + outer loop + vertex 7.660444e-003 6.427876e-003 4.300000e-002 + vertex 8.660254e-003 5.000000e-003 7.200000e-002 + vertex 8.660254e-003 5.000000e-003 4.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 -0.000000e+000 + outer loop + vertex 8.660254e-003 5.000000e-003 4.300000e-002 + vertex 8.660254e-003 5.000000e-003 7.200000e-002 + vertex 9.396926e-003 3.420201e-003 7.200000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 3.475709e-018 + outer loop + vertex 8.660254e-003 5.000000e-003 4.300000e-002 + vertex 9.396926e-003 3.420201e-003 7.200000e-002 + vertex 9.396926e-003 3.420201e-003 4.300000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588190e-001 -0.000000e+000 + outer loop + vertex 9.396926e-003 3.420201e-003 4.300000e-002 + vertex 9.396926e-003 3.420201e-003 7.200000e-002 + vertex 9.848078e-003 1.736482e-003 7.200000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588190e-001 1.737854e-018 + outer loop + vertex 9.396926e-003 3.420201e-003 4.300000e-002 + vertex 9.848078e-003 1.736482e-003 7.200000e-002 + vertex 9.848078e-003 1.736482e-003 4.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715547e-002 -0.000000e+000 + outer loop + vertex 9.848078e-003 1.736482e-003 4.300000e-002 + vertex 9.848078e-003 1.736482e-003 7.200000e-002 + vertex 1.000000e-002 0.000000e+000 7.200000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.215817e-003 -1.694728e-002 7.300000e-002 + vertex -2.250000e-003 -1.733799e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.078461e-002 -1.200000e-002 7.300000e-002 + vertex -2.126980e-002 -1.095950e-002 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.356886e-002 1.039259e-002 7.300000e-002 + vertex 1.329154e-002 1.011526e-002 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.501514e-002 6.418993e-003 7.300000e-002 + vertex 1.540584e-002 6.453176e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.329154e-002 1.011526e-002 7.300000e-002 + vertex 1.306658e-002 9.793993e-003 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.306658e-002 9.793993e-003 7.300000e-002 + vertex 1.290083e-002 9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.290083e-002 9.438538e-003 7.300000e-002 + vertex 1.279932e-002 9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.614014e-002 6.720436e-003 7.300000e-002 + vertex 1.646141e-002 6.945393e-003 7.300000e-002 + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 1.646141e-002 6.945393e-003 7.300000e-002 + vertex 1.673874e-002 7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 1.673874e-002 7.222721e-003 7.300000e-002 + vertex 2.363539e-002 4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.578468e-002 1.078330e-002 7.300000e-002 + vertex 1.540584e-002 1.088481e-002 7.300000e-002 + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.540584e-002 6.453176e-003 7.300000e-002 + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 1.540584e-002 6.453176e-003 7.300000e-002 + vertex 1.578468e-002 6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 1.578468e-002 6.554685e-003 7.300000e-002 + vertex 1.614014e-002 6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.279932e-002 9.059701e-003 7.300000e-002 + vertex 1.276514e-002 8.668993e-003 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.276514e-002 8.668993e-003 7.300000e-002 + vertex 1.279932e-002 8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.279932e-002 8.278284e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.279932e-002 8.278284e-003 7.300000e-002 + vertex 1.290083e-002 7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.290083e-002 7.899447e-003 7.300000e-002 + vertex 1.306658e-002 7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.306658e-002 7.543993e-003 7.300000e-002 + vertex 1.329154e-002 7.222721e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.329154e-002 7.222721e-003 7.300000e-002 + vertex 1.356886e-002 6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.356886e-002 6.945393e-003 7.300000e-002 + vertex 1.389014e-002 6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.673874e-002 7.222721e-003 7.300000e-002 + vertex 1.696369e-002 7.543993e-003 7.300000e-002 + vertex 2.363539e-002 4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.363539e-002 4.167556e-003 7.300000e-002 + vertex 1.696369e-002 7.543993e-003 7.300000e-002 + vertex 1.712945e-002 7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.363539e-002 4.167556e-003 7.300000e-002 + vertex 1.712945e-002 7.899447e-003 7.300000e-002 + vertex 2.255262e-002 8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.501514e-002 1.091899e-002 7.300000e-002 + vertex 1.462443e-002 1.088481e-002 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.389014e-002 6.720436e-003 7.300000e-002 + vertex 1.424559e-002 6.554685e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.424559e-002 6.554685e-003 7.300000e-002 + vertex 1.462443e-002 6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.462443e-002 6.453176e-003 7.300000e-002 + vertex 1.501514e-002 6.418993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.712945e-002 7.899447e-003 7.300000e-002 + vertex 1.723095e-002 8.278284e-003 7.300000e-002 + vertex 2.255262e-002 8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.255262e-002 8.208483e-003 7.300000e-002 + vertex 1.723095e-002 8.278284e-003 7.300000e-002 + vertex 1.726514e-002 8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.255262e-002 8.208483e-003 7.300000e-002 + vertex 1.726514e-002 8.668993e-003 7.300000e-002 + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + vertex 1.726514e-002 8.668993e-003 7.300000e-002 + vertex 1.723095e-002 9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.723095e-002 9.059701e-003 7.300000e-002 + vertex 1.712945e-002 9.438538e-003 7.300000e-002 + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + vertex 1.712945e-002 9.438538e-003 7.300000e-002 + vertex 1.696369e-002 9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + vertex 1.696369e-002 9.793993e-003 7.300000e-002 + vertex 1.673874e-002 1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.462443e-002 1.088481e-002 7.300000e-002 + vertex 1.424559e-002 1.078330e-002 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.424559e-002 1.078330e-002 7.300000e-002 + vertex 1.389014e-002 1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.389014e-002 1.061755e-002 7.300000e-002 + vertex 1.356886e-002 1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.673874e-002 1.011526e-002 7.300000e-002 + vertex 1.646141e-002 1.039259e-002 7.300000e-002 + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + vertex 1.646141e-002 1.039259e-002 7.300000e-002 + vertex 1.614014e-002 1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + vertex 1.614014e-002 1.061755e-002 7.300000e-002 + vertex 1.578468e-002 1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.540584e-002 1.088481e-002 7.300000e-002 + vertex 1.501514e-002 1.091899e-002 7.300000e-002 + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + vertex 1.501514e-002 1.091899e-002 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 2.078461e-002 1.200000e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.078461e-002 1.200000e-002 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 2.005360e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 2.083937e-002 7.300000e-002 + vertex 1.200000e-002 2.078461e-002 7.300000e-002 + vertex 1.188257e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.304399e-002 7.300000e-002 + vertex 1.200000e-002 2.078461e-002 7.300000e-002 + vertex 1.542690e-002 1.838507e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.304399e-002 7.300000e-002 + vertex 1.542690e-002 1.838507e-002 7.300000e-002 + vertex 1.838507e-002 1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.125000e-003 1.928654e-002 7.300000e-002 + vertex -1.446272e-003 1.906159e-002 7.300000e-002 + vertex -4.167556e-003 2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.948557e-003 1.846299e-002 7.300000e-002 + vertex -2.114308e-003 1.810753e-002 7.300000e-002 + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -4.167556e-003 2.363539e-002 7.300000e-002 + vertex -1.446272e-003 1.906159e-002 7.300000e-002 + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + vertex -1.446272e-003 1.906159e-002 7.300000e-002 + vertex -1.723600e-003 1.878426e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + vertex -1.723600e-003 1.878426e-002 7.300000e-002 + vertex -1.948557e-003 1.846299e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.215817e-003 1.772869e-002 7.300000e-002 + vertex -2.250000e-003 1.733799e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -2.250000e-003 1.733799e-002 7.300000e-002 + vertex -2.215817e-003 1.694728e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -2.215817e-003 1.694728e-002 7.300000e-002 + vertex -2.114308e-003 1.656844e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.446272e-003 1.561439e-002 7.300000e-002 + vertex 1.723600e-003 1.589171e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 1.723600e-003 1.589171e-002 7.300000e-002 + vertex 1.948557e-003 1.621299e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -2.114308e-003 1.656844e-002 7.300000e-002 + vertex -1.948557e-003 1.621299e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -1.948557e-003 1.621299e-002 7.300000e-002 + vertex -1.723600e-003 1.589171e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -1.723600e-003 1.589171e-002 7.300000e-002 + vertex -1.446272e-003 1.561439e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 1.508799e-002 7.300000e-002 + vertex 3.907084e-004 1.512217e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.215817e-003 1.772869e-002 7.300000e-002 + vertex 2.114308e-003 1.810753e-002 7.300000e-002 + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 3.907084e-004 1.955380e-002 7.300000e-002 + vertex 0.000000e+000 2.400000e-002 7.300000e-002 + vertex 7.695453e-004 1.945229e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 7.695453e-004 1.945229e-002 7.300000e-002 + vertex 0.000000e+000 2.400000e-002 7.300000e-002 + vertex 4.167556e-003 2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 7.695453e-004 1.945229e-002 7.300000e-002 + vertex 4.167556e-003 2.363539e-002 7.300000e-002 + vertex 1.125000e-003 1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 3.907084e-004 1.955380e-002 7.300000e-002 + vertex 0.000000e+000 1.958799e-002 7.300000e-002 + vertex 0.000000e+000 2.400000e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 2.400000e-002 7.300000e-002 + vertex 0.000000e+000 1.958799e-002 7.300000e-002 + vertex -3.907084e-004 1.955380e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 2.400000e-002 7.300000e-002 + vertex -3.907084e-004 1.955380e-002 7.300000e-002 + vertex -4.167556e-003 2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.167556e-003 2.363539e-002 7.300000e-002 + vertex -3.907084e-004 1.955380e-002 7.300000e-002 + vertex -7.695453e-004 1.945229e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.167556e-003 2.363539e-002 7.300000e-002 + vertex -7.695453e-004 1.945229e-002 7.300000e-002 + vertex -1.125000e-003 1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.446272e-003 1.561439e-002 7.300000e-002 + vertex -1.125000e-003 1.538943e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -1.125000e-003 1.538943e-002 7.300000e-002 + vertex -7.695453e-004 1.522368e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -7.695453e-004 1.522368e-002 7.300000e-002 + vertex -3.907084e-004 1.512217e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 3.907084e-004 1.512217e-002 7.300000e-002 + vertex 7.695453e-004 1.522368e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 7.695453e-004 1.522368e-002 7.300000e-002 + vertex 1.125000e-003 1.538943e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 1.125000e-003 1.538943e-002 7.300000e-002 + vertex 1.446272e-003 1.561439e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.114308e-003 1.810753e-002 7.300000e-002 + vertex 1.948557e-003 1.846299e-002 7.300000e-002 + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + vertex 1.948557e-003 1.846299e-002 7.300000e-002 + vertex 1.723600e-003 1.878426e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + vertex 1.723600e-003 1.878426e-002 7.300000e-002 + vertex 4.167556e-003 2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 4.167556e-003 2.363539e-002 7.300000e-002 + vertex 1.723600e-003 1.878426e-002 7.300000e-002 + vertex 1.446272e-003 1.906159e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 4.167556e-003 2.363539e-002 7.300000e-002 + vertex 1.446272e-003 1.906159e-002 7.300000e-002 + vertex 1.125000e-003 1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.948557e-003 1.621299e-002 7.300000e-002 + vertex 2.114308e-003 1.656844e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 2.114308e-003 1.656844e-002 7.300000e-002 + vertex 2.215817e-003 1.694728e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 2.215817e-003 1.694728e-002 7.300000e-002 + vertex 2.250000e-003 1.733799e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 2.083937e-002 7.300000e-002 + vertex 1.188257e-002 1.304399e-002 7.300000e-002 + vertex 8.208483e-003 2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 8.208483e-003 2.255262e-002 7.300000e-002 + vertex 1.188257e-002 1.304399e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 8.208483e-003 2.255262e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 2.250000e-003 1.733799e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + vertex 2.250000e-003 1.733799e-002 7.300000e-002 + vertex 2.215817e-003 1.772869e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.114308e-003 1.810753e-002 7.300000e-002 + vertex -2.215817e-003 1.772869e-002 7.300000e-002 + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + vertex -2.215817e-003 1.772869e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -8.208483e-003 2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -8.208483e-003 2.255262e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.221714e-002 7.300000e-002 + vertex -2.005360e-002 1.304399e-002 7.300000e-002 + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + vertex -2.005360e-002 1.304399e-002 7.300000e-002 + vertex -1.838507e-002 1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + vertex -1.838507e-002 1.542690e-002 7.300000e-002 + vertex -1.542690e-002 1.838507e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.542690e-002 1.838507e-002 7.300000e-002 + vertex -1.200000e-002 2.078461e-002 7.300000e-002 + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + vertex -1.200000e-002 2.078461e-002 7.300000e-002 + vertex -1.188257e-002 2.083937e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + vertex -1.188257e-002 2.083937e-002 7.300000e-002 + vertex -8.208483e-003 2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 7.300000e-002 + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -2.255262e-002 8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.255262e-002 8.208483e-003 7.300000e-002 + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -2.255262e-002 8.208483e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -2.363539e-002 4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 7.300000e-002 + vertex -2.255262e-002 -8.208483e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.255262e-002 -8.208483e-003 7.300000e-002 + vertex -2.363539e-002 -4.167556e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -2.363539e-002 -4.167556e-003 7.300000e-002 + vertex -2.400000e-002 3.491358e-017 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -2.400000e-002 3.491358e-017 7.300000e-002 + vertex -2.363539e-002 4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.078461e-002 -1.200000e-002 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -2.063257e-002 -1.221714e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.221714e-002 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.221714e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -2.005360e-002 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -2.083937e-002 7.300000e-002 + vertex -1.200000e-002 -2.078461e-002 7.300000e-002 + vertex -1.188257e-002 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 7.300000e-002 + vertex -1.200000e-002 -2.078461e-002 7.300000e-002 + vertex -1.542690e-002 -1.838507e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 7.300000e-002 + vertex -1.542690e-002 -1.838507e-002 7.300000e-002 + vertex -1.838507e-002 -1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -2.083937e-002 7.300000e-002 + vertex -1.188257e-002 -1.304399e-002 7.300000e-002 + vertex -8.208483e-003 -2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.208483e-003 -2.255262e-002 7.300000e-002 + vertex -1.188257e-002 -1.304399e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.208483e-003 -2.255262e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 2.114308e-003 -1.810753e-002 7.300000e-002 + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + vertex 1.948557e-003 -1.846299e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.948557e-003 -1.846299e-002 7.300000e-002 + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + vertex 1.723600e-003 -1.878426e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -2.250000e-003 -1.733799e-002 7.300000e-002 + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + vertex -2.250000e-003 -1.733799e-002 7.300000e-002 + vertex -2.215817e-003 -1.772869e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + vertex -2.215817e-003 -1.772869e-002 7.300000e-002 + vertex -2.114308e-003 -1.810753e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.446272e-003 -1.906159e-002 7.300000e-002 + vertex -4.167556e-003 -2.363539e-002 7.300000e-002 + vertex -1.723600e-003 -1.878426e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.723600e-003 -1.878426e-002 7.300000e-002 + vertex -4.167556e-003 -2.363539e-002 7.300000e-002 + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.723600e-003 -1.878426e-002 7.300000e-002 + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + vertex -1.948557e-003 -1.846299e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.948557e-003 -1.846299e-002 7.300000e-002 + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + vertex -2.114308e-003 -1.810753e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.723600e-003 -1.878426e-002 7.300000e-002 + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + vertex 1.446272e-003 -1.906159e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.446272e-003 -1.906159e-002 7.300000e-002 + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + vertex 4.167556e-003 -2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.446272e-003 -1.906159e-002 7.300000e-002 + vertex 4.167556e-003 -2.363539e-002 7.300000e-002 + vertex 1.125000e-003 -1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.114308e-003 -1.656844e-002 7.300000e-002 + vertex 1.948557e-003 -1.621299e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 1.948557e-003 -1.621299e-002 7.300000e-002 + vertex 1.723600e-003 -1.589171e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 1.723600e-003 -1.589171e-002 7.300000e-002 + vertex 1.446272e-003 -1.561439e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.446272e-003 -1.561439e-002 7.300000e-002 + vertex -1.723600e-003 -1.589171e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.446272e-003 -1.906159e-002 7.300000e-002 + vertex -1.125000e-003 -1.928654e-002 7.300000e-002 + vertex -4.167556e-003 -2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.167556e-003 -2.363539e-002 7.300000e-002 + vertex -1.125000e-003 -1.928654e-002 7.300000e-002 + vertex -7.695453e-004 -1.945229e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.167556e-003 -2.363539e-002 7.300000e-002 + vertex -7.695453e-004 -1.945229e-002 7.300000e-002 + vertex -6.938894e-017 -2.400000e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.938894e-017 -2.400000e-002 7.300000e-002 + vertex -7.695453e-004 -1.945229e-002 7.300000e-002 + vertex -3.907084e-004 -1.955380e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.907084e-004 -1.955380e-002 7.300000e-002 + vertex 0.000000e+000 -1.958799e-002 7.300000e-002 + vertex -6.938894e-017 -2.400000e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.938894e-017 -2.400000e-002 7.300000e-002 + vertex 0.000000e+000 -1.958799e-002 7.300000e-002 + vertex 3.907084e-004 -1.955380e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.938894e-017 -2.400000e-002 7.300000e-002 + vertex 3.907084e-004 -1.955380e-002 7.300000e-002 + vertex 4.167556e-003 -2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 4.167556e-003 -2.363539e-002 7.300000e-002 + vertex 3.907084e-004 -1.955380e-002 7.300000e-002 + vertex 7.695453e-004 -1.945229e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 4.167556e-003 -2.363539e-002 7.300000e-002 + vertex 7.695453e-004 -1.945229e-002 7.300000e-002 + vertex 1.125000e-003 -1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.723600e-003 -1.589171e-002 7.300000e-002 + vertex -1.948557e-003 -1.621299e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -1.948557e-003 -1.621299e-002 7.300000e-002 + vertex -2.114308e-003 -1.656844e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -2.114308e-003 -1.656844e-002 7.300000e-002 + vertex -2.215817e-003 -1.694728e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 -1.508799e-002 7.300000e-002 + vertex -3.907084e-004 -1.512217e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.446272e-003 -1.561439e-002 7.300000e-002 + vertex 1.125000e-003 -1.538943e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 1.125000e-003 -1.538943e-002 7.300000e-002 + vertex 7.695453e-004 -1.522368e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 7.695453e-004 -1.522368e-002 7.300000e-002 + vertex 3.907084e-004 -1.512217e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.215817e-003 -1.772869e-002 7.300000e-002 + vertex 2.250000e-003 -1.733799e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 2.250000e-003 -1.733799e-002 7.300000e-002 + vertex 2.215817e-003 -1.694728e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 2.215817e-003 -1.694728e-002 7.300000e-002 + vertex 2.114308e-003 -1.656844e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.907084e-004 -1.512217e-002 7.300000e-002 + vertex -7.695453e-004 -1.522368e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -7.695453e-004 -1.522368e-002 7.300000e-002 + vertex -1.125000e-003 -1.538943e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -1.125000e-003 -1.538943e-002 7.300000e-002 + vertex -1.446272e-003 -1.561439e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.114308e-003 -1.810753e-002 7.300000e-002 + vertex 2.215817e-003 -1.772869e-002 7.300000e-002 + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + vertex 2.215817e-003 -1.772869e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 8.208483e-003 -2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 8.208483e-003 -2.255262e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 8.208483e-003 -2.255262e-002 7.300000e-002 + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + vertex 1.188257e-002 -2.083937e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.078461e-002 -1.200000e-002 7.300000e-002 + vertex 2.005360e-002 -1.304399e-002 7.300000e-002 + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + vertex 2.005360e-002 -1.304399e-002 7.300000e-002 + vertex 1.838507e-002 -1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.838507e-002 -1.542690e-002 7.300000e-002 + vertex 1.542690e-002 -1.838507e-002 7.300000e-002 + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + vertex 1.542690e-002 -1.838507e-002 7.300000e-002 + vertex 1.200000e-002 -2.078461e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + vertex 1.200000e-002 -2.078461e-002 7.300000e-002 + vertex 1.188257e-002 -2.083937e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.578468e-002 -1.078330e-002 7.300000e-002 + vertex 1.614014e-002 -1.061755e-002 7.300000e-002 + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.279932e-002 -8.278284e-003 7.300000e-002 + vertex 1.276514e-002 -8.668993e-003 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.276514e-002 -8.668993e-003 7.300000e-002 + vertex 1.279932e-002 -9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.290083e-002 -7.899447e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.306658e-002 -7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.306658e-002 -7.543993e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.329154e-002 -7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.614014e-002 -1.061755e-002 7.300000e-002 + vertex 1.646141e-002 -1.039259e-002 7.300000e-002 + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + vertex 1.646141e-002 -1.039259e-002 7.300000e-002 + vertex 1.673874e-002 -1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + vertex 1.673874e-002 -1.011526e-002 7.300000e-002 + vertex 1.696369e-002 -9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.279932e-002 -9.059701e-003 7.300000e-002 + vertex 1.290083e-002 -9.438538e-003 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.290083e-002 -9.438538e-003 7.300000e-002 + vertex 1.306658e-002 -9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.306658e-002 -9.793993e-003 7.300000e-002 + vertex 1.329154e-002 -1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.329154e-002 -1.011526e-002 7.300000e-002 + vertex 1.356886e-002 -1.039259e-002 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.356886e-002 -1.039259e-002 7.300000e-002 + vertex 1.389014e-002 -1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.389014e-002 -1.061755e-002 7.300000e-002 + vertex 1.424559e-002 -1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.424559e-002 -1.078330e-002 7.300000e-002 + vertex 1.462443e-002 -1.088481e-002 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.462443e-002 -1.088481e-002 7.300000e-002 + vertex 1.501514e-002 -1.091899e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.501514e-002 -1.091899e-002 7.300000e-002 + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + vertex 1.501514e-002 -1.091899e-002 7.300000e-002 + vertex 1.540584e-002 -1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + vertex 1.540584e-002 -1.088481e-002 7.300000e-002 + vertex 1.578468e-002 -1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.501514e-002 -6.418993e-003 7.300000e-002 + vertex 1.462443e-002 -6.453176e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.462443e-002 -6.453176e-003 7.300000e-002 + vertex 1.424559e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.696369e-002 -9.793993e-003 7.300000e-002 + vertex 1.712945e-002 -9.438538e-003 7.300000e-002 + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + vertex 1.712945e-002 -9.438538e-003 7.300000e-002 + vertex 1.723095e-002 -9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + vertex 1.723095e-002 -9.059701e-003 7.300000e-002 + vertex 2.255262e-002 -8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.723095e-002 -9.059701e-003 7.300000e-002 + vertex 1.726514e-002 -8.668993e-003 7.300000e-002 + vertex 2.255262e-002 -8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.255262e-002 -8.208483e-003 7.300000e-002 + vertex 1.726514e-002 -8.668993e-003 7.300000e-002 + vertex 1.723095e-002 -8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.255262e-002 -8.208483e-003 7.300000e-002 + vertex 1.723095e-002 -8.278284e-003 7.300000e-002 + vertex 2.363539e-002 -4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.424559e-002 -6.554685e-003 7.300000e-002 + vertex 1.389014e-002 -6.720436e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.389014e-002 -6.720436e-003 7.300000e-002 + vertex 1.356886e-002 -6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.356886e-002 -6.945393e-003 7.300000e-002 + vertex 1.329154e-002 -7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.723095e-002 -8.278284e-003 7.300000e-002 + vertex 1.712945e-002 -7.899447e-003 7.300000e-002 + vertex 2.363539e-002 -4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.363539e-002 -4.167556e-003 7.300000e-002 + vertex 1.712945e-002 -7.899447e-003 7.300000e-002 + vertex 1.696369e-002 -7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.363539e-002 -4.167556e-003 7.300000e-002 + vertex 1.696369e-002 -7.543993e-003 7.300000e-002 + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 1.696369e-002 -7.543993e-003 7.300000e-002 + vertex 1.673874e-002 -7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 1.673874e-002 -7.222721e-003 7.300000e-002 + vertex 1.646141e-002 -6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.646141e-002 -6.945393e-003 7.300000e-002 + vertex 1.614014e-002 -6.720436e-003 7.300000e-002 + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 1.614014e-002 -6.720436e-003 7.300000e-002 + vertex 1.578468e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 1.578468e-002 -6.554685e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.578468e-002 -6.554685e-003 7.300000e-002 + vertex 1.540584e-002 -6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.540584e-002 -6.453176e-003 7.300000e-002 + vertex 1.501514e-002 -6.418993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.095950e-002 7.300000e-002 + vertex 6.625000e-003 -8.738546e-003 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 6.625000e-003 -8.738546e-003 7.300000e-002 + vertex 7.070664e-003 -8.426489e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 7.070664e-003 -8.426489e-003 7.300000e-002 + vertex 8.426489e-003 -7.070664e-003 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 8.426489e-003 -7.070664e-003 7.300000e-002 + vertex 9.526280e-003 -5.500000e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 9.526280e-003 -5.500000e-003 7.300000e-002 + vertex 1.033662e-002 -3.762222e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.290083e-002 -7.899447e-003 7.300000e-002 + vertex 1.279932e-002 -8.278284e-003 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.279932e-002 -8.278284e-003 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.083289e-002 -1.910130e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.083289e-002 -1.910130e-003 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 1.033662e-002 -3.762222e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.095950e-002 7.300000e-002 + vertex 4.629575e-004 -1.095950e-002 7.300000e-002 + vertex 1.910130e-003 -1.083289e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.910130e-003 -1.083289e-002 7.300000e-002 + vertex 3.762222e-003 -1.033662e-002 7.300000e-002 + vertex 6.625000e-003 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.095950e-002 7.300000e-002 + vertex 3.762222e-003 -1.033662e-002 7.300000e-002 + vertex 5.500000e-003 -9.526280e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.095950e-002 7.300000e-002 + vertex 5.500000e-003 -9.526280e-003 7.300000e-002 + vertex 6.625000e-003 -8.738546e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -4.629575e-004 -1.095950e-002 7.300000e-002 + vertex -3.122502e-017 -1.100000e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -3.122502e-017 -1.100000e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 0.000000e+000 -1.508799e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 -1.508799e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 3.907084e-004 -1.512217e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -8.738546e-003 7.300000e-002 + vertex -5.500000e-003 -9.526280e-003 7.300000e-002 + vertex -6.625000e-003 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.095950e-002 7.300000e-002 + vertex -5.500000e-003 -9.526280e-003 7.300000e-002 + vertex -3.762222e-003 -1.033662e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.095950e-002 7.300000e-002 + vertex -3.762222e-003 -1.033662e-002 7.300000e-002 + vertex -1.910130e-003 -1.083289e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -8.738546e-003 7.300000e-002 + vertex -6.625000e-003 -1.095950e-002 7.300000e-002 + vertex -7.070664e-003 -8.426489e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -7.070664e-003 -8.426489e-003 7.300000e-002 + vertex -6.625000e-003 -1.095950e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -7.070664e-003 -8.426489e-003 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -8.426489e-003 -7.070664e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.083289e-002 -1.910130e-003 7.300000e-002 + vertex -1.033662e-002 -3.762222e-003 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.033662e-002 -3.762222e-003 7.300000e-002 + vertex -9.526280e-003 -5.500000e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -9.526280e-003 -5.500000e-003 7.300000e-002 + vertex -8.426489e-003 -7.070664e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.426489e-003 7.070664e-003 7.300000e-002 + vertex -9.526280e-003 5.500000e-003 7.300000e-002 + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -9.526280e-003 5.500000e-003 7.300000e-002 + vertex -1.033662e-002 3.762222e-003 7.300000e-002 + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.033662e-002 3.762222e-003 7.300000e-002 + vertex -1.083289e-002 1.910130e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.083289e-002 1.910130e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.083289e-002 1.910130e-003 7.300000e-002 + vertex -1.100000e-002 1.600206e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.762222e-003 1.033662e-002 7.300000e-002 + vertex -6.625000e-003 1.095950e-002 7.300000e-002 + vertex -1.910130e-003 1.083289e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.910130e-003 1.083289e-002 7.300000e-002 + vertex -6.625000e-003 1.095950e-002 7.300000e-002 + vertex -4.629575e-004 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.762222e-003 1.033662e-002 7.300000e-002 + vertex -5.500000e-003 9.526280e-003 7.300000e-002 + vertex -6.625000e-003 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.095950e-002 7.300000e-002 + vertex -5.500000e-003 9.526280e-003 7.300000e-002 + vertex -6.625000e-003 8.738546e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.095950e-002 7.300000e-002 + vertex -6.625000e-003 8.738546e-003 7.300000e-002 + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -6.625000e-003 8.738546e-003 7.300000e-002 + vertex -7.070664e-003 8.426489e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -7.070664e-003 8.426489e-003 7.300000e-002 + vertex -8.426489e-003 7.070664e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 4.629575e-004 1.095950e-002 7.300000e-002 + vertex 0.000000e+000 1.100000e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 0.000000e+000 1.100000e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex 0.000000e+000 1.508799e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 1.508799e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -3.907084e-004 1.512217e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.910130e-003 1.083289e-002 7.300000e-002 + vertex 6.625000e-003 1.095950e-002 7.300000e-002 + vertex 3.762222e-003 1.033662e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 3.762222e-003 1.033662e-002 7.300000e-002 + vertex 6.625000e-003 1.095950e-002 7.300000e-002 + vertex 5.500000e-003 9.526280e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 9.526280e-003 5.500000e-003 7.300000e-002 + vertex 8.426489e-003 7.070664e-003 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 8.426489e-003 7.070664e-003 7.300000e-002 + vertex 7.070664e-003 8.426489e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 7.070664e-003 8.426489e-003 7.300000e-002 + vertex 6.625000e-003 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.095950e-002 7.300000e-002 + vertex 7.070664e-003 8.426489e-003 7.300000e-002 + vertex 6.625000e-003 8.738546e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.095950e-002 7.300000e-002 + vertex 6.625000e-003 8.738546e-003 7.300000e-002 + vertex 5.500000e-003 9.526280e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.083289e-002 -1.910130e-003 7.300000e-002 + vertex 1.100000e-002 -2.694223e-018 7.300000e-002 + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.100000e-002 -2.694223e-018 7.300000e-002 + vertex 1.083289e-002 1.910130e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 5.204170e-017 7.300000e-002 + vertex 1.083289e-002 1.910130e-003 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.083289e-002 1.910130e-003 7.300000e-002 + vertex 1.033662e-002 3.762222e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.033662e-002 3.762222e-003 7.300000e-002 + vertex 9.526280e-003 5.500000e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.122502e-017 -1.100000e-002 7.300000e-002 + vertex 4.629575e-004 -1.095950e-002 7.300000e-002 + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 4.629575e-004 -1.095950e-002 7.300000e-002 + vertex 6.625000e-003 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 7.300000e-002 + vertex 6.625000e-003 -1.095950e-002 7.300000e-002 + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + vertex 6.625000e-003 -1.095950e-002 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 2.078461e-002 -1.200000e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.078461e-002 -1.200000e-002 7.300000e-002 + vertex 1.188257e-002 -1.095950e-002 7.300000e-002 + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.838507e-002 -1.542690e-002 7.300000e-002 + vertex -2.005360e-002 -1.304399e-002 7.300000e-002 + vertex -1.188257e-002 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 7.300000e-002 + vertex -2.005360e-002 -1.304399e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -6.625000e-003 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 7.300000e-002 + vertex -6.625000e-003 -1.095950e-002 7.300000e-002 + vertex -4.629575e-004 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.629575e-004 -1.095950e-002 7.300000e-002 + vertex -6.625000e-003 -1.095950e-002 7.300000e-002 + vertex -1.910130e-003 -1.083289e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.723095e-002 -8.278284e-003 7.300000e-002 + vertex -1.726514e-002 -8.668993e-003 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.726514e-002 -8.668993e-003 7.300000e-002 + vertex -1.723095e-002 -9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.723095e-002 -9.059701e-003 7.300000e-002 + vertex -1.712945e-002 -9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.578468e-002 -1.078330e-002 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.614014e-002 -1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.614014e-002 -1.061755e-002 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.646141e-002 -1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.712945e-002 -9.438538e-003 7.300000e-002 + vertex -1.696369e-002 -9.793993e-003 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.696369e-002 -9.793993e-003 7.300000e-002 + vertex -1.673874e-002 -1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.673874e-002 -1.011526e-002 7.300000e-002 + vertex -1.646141e-002 -1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.279932e-002 -9.059701e-003 7.300000e-002 + vertex -1.276514e-002 -8.668993e-003 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.279932e-002 -9.059701e-003 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.290083e-002 -9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.424559e-002 -1.078330e-002 7.300000e-002 + vertex -1.389014e-002 -1.061755e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.389014e-002 -1.061755e-002 7.300000e-002 + vertex -1.356886e-002 -1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.578468e-002 -1.078330e-002 7.300000e-002 + vertex -1.540584e-002 -1.088481e-002 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.540584e-002 -1.088481e-002 7.300000e-002 + vertex -1.501514e-002 -1.091899e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.501514e-002 -1.091899e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.501514e-002 -1.091899e-002 7.300000e-002 + vertex -1.462443e-002 -1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.462443e-002 -1.088481e-002 7.300000e-002 + vertex -1.424559e-002 -1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.356886e-002 -1.039259e-002 7.300000e-002 + vertex -1.329154e-002 -1.011526e-002 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.329154e-002 -1.011526e-002 7.300000e-002 + vertex -1.306658e-002 -9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.306658e-002 -9.793993e-003 7.300000e-002 + vertex -1.290083e-002 -9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.462443e-002 -6.453176e-003 7.300000e-002 + vertex -1.501514e-002 -6.418993e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.712945e-002 -7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 -1.095950e-002 7.300000e-002 + vertex -1.712945e-002 -7.899447e-003 7.300000e-002 + vertex -1.723095e-002 -8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.279932e-002 -8.278284e-003 7.300000e-002 + vertex -1.290083e-002 -7.899447e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.646141e-002 -6.945393e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.614014e-002 -6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.614014e-002 -6.720436e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.578468e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.646141e-002 -6.945393e-003 7.300000e-002 + vertex -1.673874e-002 -7.222721e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.673874e-002 -7.222721e-003 7.300000e-002 + vertex -1.696369e-002 -7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.696369e-002 -7.543993e-003 7.300000e-002 + vertex -1.712945e-002 -7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.501514e-002 -6.418993e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.501514e-002 -6.418993e-003 7.300000e-002 + vertex -1.540584e-002 -6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.540584e-002 -6.453176e-003 7.300000e-002 + vertex -1.578468e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.100000e-002 1.600206e-017 7.300000e-002 + vertex -1.083289e-002 -1.910130e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.083289e-002 -1.910130e-003 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.279932e-002 -8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.279932e-002 -8.278284e-003 7.300000e-002 + vertex -1.188257e-002 -1.095950e-002 7.300000e-002 + vertex -1.276514e-002 -8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.290083e-002 -7.899447e-003 7.300000e-002 + vertex -1.306658e-002 -7.543993e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.306658e-002 -7.543993e-003 7.300000e-002 + vertex -1.329154e-002 -7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.329154e-002 -7.222721e-003 7.300000e-002 + vertex -1.356886e-002 -6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.356886e-002 -6.945393e-003 7.300000e-002 + vertex -1.389014e-002 -6.720436e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.389014e-002 -6.720436e-003 7.300000e-002 + vertex -1.424559e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.424559e-002 -6.554685e-003 7.300000e-002 + vertex -1.462443e-002 -6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.723095e-002 9.059701e-003 7.300000e-002 + vertex -1.726514e-002 8.668993e-003 7.300000e-002 + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -1.726514e-002 8.668993e-003 7.300000e-002 + vertex -1.723095e-002 8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -1.723095e-002 8.278284e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.723095e-002 8.278284e-003 7.300000e-002 + vertex -1.712945e-002 7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.712945e-002 7.899447e-003 7.300000e-002 + vertex -1.696369e-002 7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.696369e-002 7.543993e-003 7.300000e-002 + vertex -1.673874e-002 7.222721e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.673874e-002 7.222721e-003 7.300000e-002 + vertex -1.646141e-002 6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.646141e-002 6.945393e-003 7.300000e-002 + vertex -1.614014e-002 6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.424559e-002 6.554685e-003 7.300000e-002 + vertex -1.389014e-002 6.720436e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.424559e-002 6.554685e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.462443e-002 6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.389014e-002 6.720436e-003 7.300000e-002 + vertex -1.356886e-002 6.945393e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.356886e-002 6.945393e-003 7.300000e-002 + vertex -1.329154e-002 7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.329154e-002 7.222721e-003 7.300000e-002 + vertex -1.306658e-002 7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.306658e-002 7.543993e-003 7.300000e-002 + vertex -1.290083e-002 7.899447e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.290083e-002 7.899447e-003 7.300000e-002 + vertex -1.279932e-002 8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.279932e-002 8.278284e-003 7.300000e-002 + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.279932e-002 8.278284e-003 7.300000e-002 + vertex -1.276514e-002 8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.276514e-002 8.668993e-003 7.300000e-002 + vertex -1.279932e-002 9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.614014e-002 6.720436e-003 7.300000e-002 + vertex -1.578468e-002 6.554685e-003 7.300000e-002 + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.578468e-002 6.554685e-003 7.300000e-002 + vertex -1.540584e-002 6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 5.204170e-017 7.300000e-002 + vertex -1.540584e-002 6.453176e-003 7.300000e-002 + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.540584e-002 6.453176e-003 7.300000e-002 + vertex -1.501514e-002 6.418993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 5.204170e-017 7.300000e-002 + vertex -1.501514e-002 6.418993e-003 7.300000e-002 + vertex -1.462443e-002 6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.279932e-002 9.059701e-003 7.300000e-002 + vertex -1.290083e-002 9.438538e-003 7.300000e-002 + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.290083e-002 9.438538e-003 7.300000e-002 + vertex -1.306658e-002 9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.306658e-002 9.793993e-003 7.300000e-002 + vertex -1.329154e-002 1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.329154e-002 1.011526e-002 7.300000e-002 + vertex -1.356886e-002 1.039259e-002 7.300000e-002 + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.356886e-002 1.039259e-002 7.300000e-002 + vertex -1.389014e-002 1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.389014e-002 1.061755e-002 7.300000e-002 + vertex -1.424559e-002 1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.578468e-002 1.078330e-002 7.300000e-002 + vertex -1.614014e-002 1.061755e-002 7.300000e-002 + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -1.614014e-002 1.061755e-002 7.300000e-002 + vertex -1.646141e-002 1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -1.646141e-002 1.039259e-002 7.300000e-002 + vertex -1.673874e-002 1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.673874e-002 1.011526e-002 7.300000e-002 + vertex -1.696369e-002 9.793993e-003 7.300000e-002 + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -1.696369e-002 9.793993e-003 7.300000e-002 + vertex -1.712945e-002 9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -1.712945e-002 9.438538e-003 7.300000e-002 + vertex -1.723095e-002 9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.424559e-002 1.078330e-002 7.300000e-002 + vertex -1.462443e-002 1.088481e-002 7.300000e-002 + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.462443e-002 1.088481e-002 7.300000e-002 + vertex -1.501514e-002 1.091899e-002 7.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.501514e-002 1.091899e-002 7.300000e-002 + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -1.501514e-002 1.091899e-002 7.300000e-002 + vertex -1.540584e-002 1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -1.540584e-002 1.088481e-002 7.300000e-002 + vertex -1.578468e-002 1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.910130e-003 1.083289e-002 7.300000e-002 + vertex 4.629575e-004 1.095950e-002 7.300000e-002 + vertex 6.625000e-003 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.095950e-002 7.300000e-002 + vertex 4.629575e-004 1.095950e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 6.625000e-003 1.095950e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 6.625000e-003 1.304399e-002 7.300000e-002 + vertex 1.188257e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 7.300000e-002 + vertex 1.188257e-002 1.304399e-002 7.300000e-002 + vertex 2.005360e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.005360e-002 1.304399e-002 7.300000e-002 + vertex 1.188257e-002 1.304399e-002 7.300000e-002 + vertex 1.838507e-002 1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 7.300000e-002 + vertex -2.078461e-002 1.200000e-002 7.300000e-002 + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -2.078461e-002 1.200000e-002 7.300000e-002 + vertex -2.063257e-002 1.221714e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.063257e-002 1.095950e-002 7.300000e-002 + vertex -2.063257e-002 1.221714e-002 7.300000e-002 + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -2.063257e-002 1.221714e-002 7.300000e-002 + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 7.300000e-002 + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + vertex -6.625000e-003 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.095950e-002 7.300000e-002 + vertex -1.188257e-002 1.304399e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -6.625000e-003 1.095950e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex -4.629575e-004 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.629575e-004 1.095950e-002 7.300000e-002 + vertex -6.625000e-003 1.304399e-002 7.300000e-002 + vertex 0.000000e+000 1.100000e-002 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715579e-002 -2.216205e-017 + outer loop + vertex 2.462019e-002 4.341205e-003 7.400000e-002 + vertex 2.500000e-002 0.000000e+000 7.750000e-002 + vertex 2.500000e-002 0.000000e+000 7.400000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715579e-002 0.000000e+000 + outer loop + vertex 2.500000e-002 0.000000e+000 7.400000e-002 + vertex 2.500000e-002 0.000000e+000 7.750000e-002 + vertex 2.462019e-002 -4.341205e-003 7.750000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715579e-002 2.216205e-017 + outer loop + vertex 2.500000e-002 0.000000e+000 7.400000e-002 + vertex 2.462019e-002 -4.341205e-003 7.750000e-002 + vertex 2.462019e-002 -4.341205e-003 7.400000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex 2.462019e-002 -4.341205e-003 7.400000e-002 + vertex 2.462019e-002 -4.341205e-003 7.750000e-002 + vertex 2.349232e-002 -8.550503e-003 7.750000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex 2.462019e-002 -4.341205e-003 7.400000e-002 + vertex 2.349232e-002 -8.550503e-003 7.750000e-002 + vertex 2.349232e-002 -8.550503e-003 7.400000e-002 + endloop + endfacet + facet normal 9.063078e-001 -4.226183e-001 0.000000e+000 + outer loop + vertex 2.349232e-002 -8.550503e-003 7.400000e-002 + vertex 2.349232e-002 -8.550503e-003 7.750000e-002 + vertex 2.165063e-002 -1.250000e-002 7.750000e-002 + endloop + endfacet + facet normal 9.063078e-001 -4.226183e-001 -8.864820e-017 + outer loop + vertex 2.349232e-002 -8.550503e-003 7.400000e-002 + vertex 2.165063e-002 -1.250000e-002 7.750000e-002 + vertex 2.165063e-002 -1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal 8.191519e-001 -5.735766e-001 0.000000e+000 + outer loop + vertex 2.165063e-002 -1.250000e-002 7.400000e-002 + vertex 2.165063e-002 -1.250000e-002 7.750000e-002 + vertex 1.915111e-002 -1.606969e-002 7.750000e-002 + endloop + endfacet + facet normal 8.191519e-001 -5.735766e-001 -8.864819e-017 + outer loop + vertex 2.165063e-002 -1.250000e-002 7.400000e-002 + vertex 1.915111e-002 -1.606969e-002 7.750000e-002 + vertex 1.915111e-002 -1.606969e-002 7.400000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 1.915111e-002 -1.606969e-002 7.400000e-002 + vertex 1.915111e-002 -1.606969e-002 7.750000e-002 + vertex 1.606969e-002 -1.915111e-002 7.750000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 1.915111e-002 -1.606969e-002 7.400000e-002 + vertex 1.606969e-002 -1.915111e-002 7.750000e-002 + vertex 1.606969e-002 -1.915111e-002 7.400000e-002 + endloop + endfacet + facet normal 5.735766e-001 -8.191519e-001 0.000000e+000 + outer loop + vertex 1.606969e-002 -1.915111e-002 7.400000e-002 + vertex 1.606969e-002 -1.915111e-002 7.750000e-002 + vertex 1.250000e-002 -2.165063e-002 7.750000e-002 + endloop + endfacet + facet normal 5.735766e-001 -8.191519e-001 8.864819e-017 + outer loop + vertex 1.606969e-002 -1.915111e-002 7.400000e-002 + vertex 1.250000e-002 -2.165063e-002 7.750000e-002 + vertex 1.250000e-002 -2.165063e-002 7.400000e-002 + endloop + endfacet + facet normal 4.226183e-001 -9.063078e-001 0.000000e+000 + outer loop + vertex 1.250000e-002 -2.165063e-002 7.400000e-002 + vertex 1.250000e-002 -2.165063e-002 7.750000e-002 + vertex 8.550503e-003 -2.349232e-002 7.750000e-002 + endloop + endfacet + facet normal 4.226183e-001 -9.063078e-001 8.864820e-017 + outer loop + vertex 1.250000e-002 -2.165063e-002 7.400000e-002 + vertex 8.550503e-003 -2.349232e-002 7.750000e-002 + vertex 8.550503e-003 -2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 8.550503e-003 -2.349232e-002 7.400000e-002 + vertex 8.550503e-003 -2.349232e-002 7.750000e-002 + vertex 4.341205e-003 -2.462019e-002 7.750000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 8.550503e-003 -2.349232e-002 7.400000e-002 + vertex 4.341205e-003 -2.462019e-002 7.750000e-002 + vertex 4.341205e-003 -2.462019e-002 7.400000e-002 + endloop + endfacet + facet normal 8.715579e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 4.341205e-003 -2.462019e-002 7.400000e-002 + vertex 4.341205e-003 -2.462019e-002 7.750000e-002 + vertex -4.592426e-018 -2.500000e-002 7.750000e-002 + endloop + endfacet + facet normal 8.715579e-002 -9.961947e-001 -2.216205e-017 + outer loop + vertex 4.341205e-003 -2.462019e-002 7.400000e-002 + vertex -4.592426e-018 -2.500000e-002 7.750000e-002 + vertex -4.592426e-018 -2.500000e-002 7.400000e-002 + endloop + endfacet + facet normal -8.715579e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -4.592426e-018 -2.500000e-002 7.400000e-002 + vertex -4.592426e-018 -2.500000e-002 7.750000e-002 + vertex -4.341205e-003 -2.462019e-002 7.750000e-002 + endloop + endfacet + facet normal -8.715579e-002 -9.961947e-001 2.216205e-017 + outer loop + vertex -4.592426e-018 -2.500000e-002 7.400000e-002 + vertex -4.341205e-003 -2.462019e-002 7.750000e-002 + vertex -4.341205e-003 -2.462019e-002 7.400000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -4.341205e-003 -2.462019e-002 7.400000e-002 + vertex -4.341205e-003 -2.462019e-002 7.750000e-002 + vertex -8.550503e-003 -2.349232e-002 7.750000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -4.341205e-003 -2.462019e-002 7.400000e-002 + vertex -8.550503e-003 -2.349232e-002 7.750000e-002 + vertex -8.550503e-003 -2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal -4.226183e-001 -9.063078e-001 0.000000e+000 + outer loop + vertex -8.550503e-003 -2.349232e-002 7.400000e-002 + vertex -8.550503e-003 -2.349232e-002 7.750000e-002 + vertex -1.250000e-002 -2.165063e-002 7.750000e-002 + endloop + endfacet + facet normal -4.226183e-001 -9.063078e-001 -8.864820e-017 + outer loop + vertex -8.550503e-003 -2.349232e-002 7.400000e-002 + vertex -1.250000e-002 -2.165063e-002 7.750000e-002 + vertex -1.250000e-002 -2.165063e-002 7.400000e-002 + endloop + endfacet + facet normal -5.735766e-001 -8.191519e-001 0.000000e+000 + outer loop + vertex -1.250000e-002 -2.165063e-002 7.400000e-002 + vertex -1.250000e-002 -2.165063e-002 7.750000e-002 + vertex -1.606969e-002 -1.915111e-002 7.750000e-002 + endloop + endfacet + facet normal -5.735766e-001 -8.191519e-001 -8.864819e-017 + outer loop + vertex -1.250000e-002 -2.165063e-002 7.400000e-002 + vertex -1.606969e-002 -1.915111e-002 7.750000e-002 + vertex -1.606969e-002 -1.915111e-002 7.400000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -1.606969e-002 -1.915111e-002 7.400000e-002 + vertex -1.606969e-002 -1.915111e-002 7.750000e-002 + vertex -1.915111e-002 -1.606969e-002 7.750000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -1.606969e-002 -1.915111e-002 7.400000e-002 + vertex -1.915111e-002 -1.606969e-002 7.750000e-002 + vertex -1.915111e-002 -1.606969e-002 7.400000e-002 + endloop + endfacet + facet normal -8.191519e-001 -5.735766e-001 0.000000e+000 + outer loop + vertex -1.915111e-002 -1.606969e-002 7.400000e-002 + vertex -1.915111e-002 -1.606969e-002 7.750000e-002 + vertex -2.165063e-002 -1.250000e-002 7.750000e-002 + endloop + endfacet + facet normal -8.191519e-001 -5.735766e-001 8.864819e-017 + outer loop + vertex -1.915111e-002 -1.606969e-002 7.400000e-002 + vertex -2.165063e-002 -1.250000e-002 7.750000e-002 + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal -9.063078e-001 -4.226183e-001 0.000000e+000 + outer loop + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + vertex -2.165063e-002 -1.250000e-002 7.750000e-002 + vertex -2.349232e-002 -8.550503e-003 7.750000e-002 + endloop + endfacet + facet normal -9.063078e-001 -4.226183e-001 8.864820e-017 + outer loop + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + vertex -2.349232e-002 -8.550503e-003 7.750000e-002 + vertex -2.349232e-002 -8.550503e-003 7.400000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex -2.349232e-002 -8.550503e-003 7.400000e-002 + vertex -2.349232e-002 -8.550503e-003 7.750000e-002 + vertex -2.462019e-002 -4.341205e-003 7.750000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex -2.349232e-002 -8.550503e-003 7.400000e-002 + vertex -2.462019e-002 -4.341205e-003 7.750000e-002 + vertex -2.462019e-002 -4.341205e-003 7.400000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715579e-002 0.000000e+000 + outer loop + vertex -2.462019e-002 -4.341205e-003 7.400000e-002 + vertex -2.462019e-002 -4.341205e-003 7.750000e-002 + vertex -2.500000e-002 3.061617e-018 7.750000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715579e-002 -2.216205e-017 + outer loop + vertex -2.462019e-002 -4.341205e-003 7.400000e-002 + vertex -2.500000e-002 3.061617e-018 7.750000e-002 + vertex -2.500000e-002 3.061617e-018 7.400000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715579e-002 0.000000e+000 + outer loop + vertex -2.500000e-002 3.061617e-018 7.400000e-002 + vertex -2.500000e-002 3.061617e-018 7.750000e-002 + vertex -2.462019e-002 4.341205e-003 7.750000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715579e-002 2.216205e-017 + outer loop + vertex -2.500000e-002 3.061617e-018 7.400000e-002 + vertex -2.462019e-002 4.341205e-003 7.750000e-002 + vertex -2.462019e-002 4.341205e-003 7.400000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex -2.462019e-002 4.341205e-003 7.400000e-002 + vertex -2.462019e-002 4.341205e-003 7.750000e-002 + vertex -2.349232e-002 8.550503e-003 7.750000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex -2.462019e-002 4.341205e-003 7.400000e-002 + vertex -2.349232e-002 8.550503e-003 7.750000e-002 + vertex -2.349232e-002 8.550503e-003 7.400000e-002 + endloop + endfacet + facet normal -9.063078e-001 4.226183e-001 0.000000e+000 + outer loop + vertex -2.349232e-002 8.550503e-003 7.400000e-002 + vertex -2.349232e-002 8.550503e-003 7.750000e-002 + vertex -2.165063e-002 1.250000e-002 7.750000e-002 + endloop + endfacet + facet normal -9.063078e-001 4.226183e-001 -8.864820e-017 + outer loop + vertex -2.349232e-002 8.550503e-003 7.400000e-002 + vertex -2.165063e-002 1.250000e-002 7.750000e-002 + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal -8.191519e-001 5.735766e-001 0.000000e+000 + outer loop + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + vertex -2.165063e-002 1.250000e-002 7.750000e-002 + vertex -1.915111e-002 1.606969e-002 7.750000e-002 + endloop + endfacet + facet normal -8.191519e-001 5.735766e-001 -8.864819e-017 + outer loop + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + vertex -1.915111e-002 1.606969e-002 7.750000e-002 + vertex -1.915111e-002 1.606969e-002 7.400000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -1.915111e-002 1.606969e-002 7.400000e-002 + vertex -1.915111e-002 1.606969e-002 7.750000e-002 + vertex -1.606969e-002 1.915111e-002 7.750000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -1.915111e-002 1.606969e-002 7.400000e-002 + vertex -1.606969e-002 1.915111e-002 7.750000e-002 + vertex -1.606969e-002 1.915111e-002 7.400000e-002 + endloop + endfacet + facet normal -5.735766e-001 8.191519e-001 0.000000e+000 + outer loop + vertex -1.606969e-002 1.915111e-002 7.400000e-002 + vertex -1.606969e-002 1.915111e-002 7.750000e-002 + vertex -1.250000e-002 2.165063e-002 7.750000e-002 + endloop + endfacet + facet normal -5.735766e-001 8.191519e-001 8.864819e-017 + outer loop + vertex -1.606969e-002 1.915111e-002 7.400000e-002 + vertex -1.250000e-002 2.165063e-002 7.750000e-002 + vertex -1.250000e-002 2.165063e-002 7.400000e-002 + endloop + endfacet + facet normal -4.226183e-001 9.063078e-001 0.000000e+000 + outer loop + vertex -1.250000e-002 2.165063e-002 7.400000e-002 + vertex -1.250000e-002 2.165063e-002 7.750000e-002 + vertex -8.550503e-003 2.349232e-002 7.750000e-002 + endloop + endfacet + facet normal -4.226183e-001 9.063078e-001 8.864820e-017 + outer loop + vertex -1.250000e-002 2.165063e-002 7.400000e-002 + vertex -8.550503e-003 2.349232e-002 7.750000e-002 + vertex -8.550503e-003 2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -8.550503e-003 2.349232e-002 7.400000e-002 + vertex -8.550503e-003 2.349232e-002 7.750000e-002 + vertex -4.341205e-003 2.462019e-002 7.750000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -8.550503e-003 2.349232e-002 7.400000e-002 + vertex -4.341205e-003 2.462019e-002 7.750000e-002 + vertex -4.341205e-003 2.462019e-002 7.400000e-002 + endloop + endfacet + facet normal -8.715579e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -4.341205e-003 2.462019e-002 7.400000e-002 + vertex -4.341205e-003 2.462019e-002 7.750000e-002 + vertex 1.530808e-018 2.500000e-002 7.750000e-002 + endloop + endfacet + facet normal -8.715579e-002 9.961947e-001 -2.216205e-017 + outer loop + vertex -4.341205e-003 2.462019e-002 7.400000e-002 + vertex 1.530808e-018 2.500000e-002 7.750000e-002 + vertex 1.530808e-018 2.500000e-002 7.400000e-002 + endloop + endfacet + facet normal 8.715579e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex 1.530808e-018 2.500000e-002 7.400000e-002 + vertex 1.530808e-018 2.500000e-002 7.750000e-002 + vertex 4.341205e-003 2.462019e-002 7.750000e-002 + endloop + endfacet + facet normal 8.715579e-002 9.961947e-001 2.216205e-017 + outer loop + vertex 1.530808e-018 2.500000e-002 7.400000e-002 + vertex 4.341205e-003 2.462019e-002 7.750000e-002 + vertex 4.341205e-003 2.462019e-002 7.400000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 -0.000000e+000 + outer loop + vertex 4.341205e-003 2.462019e-002 7.400000e-002 + vertex 4.341205e-003 2.462019e-002 7.750000e-002 + vertex 8.550503e-003 2.349232e-002 7.750000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex 4.341205e-003 2.462019e-002 7.400000e-002 + vertex 8.550503e-003 2.349232e-002 7.750000e-002 + vertex 8.550503e-003 2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal 4.226183e-001 9.063078e-001 -0.000000e+000 + outer loop + vertex 8.550503e-003 2.349232e-002 7.400000e-002 + vertex 8.550503e-003 2.349232e-002 7.750000e-002 + vertex 1.250000e-002 2.165063e-002 7.750000e-002 + endloop + endfacet + facet normal 4.226183e-001 9.063078e-001 -8.864820e-017 + outer loop + vertex 8.550503e-003 2.349232e-002 7.400000e-002 + vertex 1.250000e-002 2.165063e-002 7.750000e-002 + vertex 1.250000e-002 2.165063e-002 7.400000e-002 + endloop + endfacet + facet normal 5.735766e-001 8.191519e-001 -0.000000e+000 + outer loop + vertex 1.250000e-002 2.165063e-002 7.400000e-002 + vertex 1.250000e-002 2.165063e-002 7.750000e-002 + vertex 1.606969e-002 1.915111e-002 7.750000e-002 + endloop + endfacet + facet normal 5.735766e-001 8.191519e-001 -8.864819e-017 + outer loop + vertex 1.250000e-002 2.165063e-002 7.400000e-002 + vertex 1.606969e-002 1.915111e-002 7.750000e-002 + vertex 1.606969e-002 1.915111e-002 7.400000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 -0.000000e+000 + outer loop + vertex 1.606969e-002 1.915111e-002 7.400000e-002 + vertex 1.606969e-002 1.915111e-002 7.750000e-002 + vertex 1.915111e-002 1.606969e-002 7.750000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex 1.606969e-002 1.915111e-002 7.400000e-002 + vertex 1.915111e-002 1.606969e-002 7.750000e-002 + vertex 1.915111e-002 1.606969e-002 7.400000e-002 + endloop + endfacet + facet normal 8.191519e-001 5.735766e-001 -0.000000e+000 + outer loop + vertex 1.915111e-002 1.606969e-002 7.400000e-002 + vertex 1.915111e-002 1.606969e-002 7.750000e-002 + vertex 2.165063e-002 1.250000e-002 7.750000e-002 + endloop + endfacet + facet normal 8.191519e-001 5.735766e-001 8.864819e-017 + outer loop + vertex 1.915111e-002 1.606969e-002 7.400000e-002 + vertex 2.165063e-002 1.250000e-002 7.750000e-002 + vertex 2.165063e-002 1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal 9.063078e-001 4.226183e-001 -0.000000e+000 + outer loop + vertex 2.165063e-002 1.250000e-002 7.400000e-002 + vertex 2.165063e-002 1.250000e-002 7.750000e-002 + vertex 2.349232e-002 8.550503e-003 7.750000e-002 + endloop + endfacet + facet normal 9.063078e-001 4.226183e-001 8.864820e-017 + outer loop + vertex 2.165063e-002 1.250000e-002 7.400000e-002 + vertex 2.349232e-002 8.550503e-003 7.750000e-002 + vertex 2.349232e-002 8.550503e-003 7.400000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588190e-001 -0.000000e+000 + outer loop + vertex 2.349232e-002 8.550503e-003 7.400000e-002 + vertex 2.349232e-002 8.550503e-003 7.750000e-002 + vertex 2.462019e-002 4.341205e-003 7.750000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex 2.349232e-002 8.550503e-003 7.400000e-002 + vertex 2.462019e-002 4.341205e-003 7.750000e-002 + vertex 2.462019e-002 4.341205e-003 7.400000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715579e-002 -0.000000e+000 + outer loop + vertex 2.462019e-002 4.341205e-003 7.400000e-002 + vertex 2.462019e-002 4.341205e-003 7.750000e-002 + vertex 2.500000e-002 0.000000e+000 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 8.379494e-003 2.302247e-002 7.750000e-002 + vertex 8.550503e-003 2.349232e-002 7.750000e-002 + vertex 4.254380e-003 2.412779e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.254380e-003 2.412779e-002 7.750000e-002 + vertex 8.550503e-003 2.349232e-002 7.750000e-002 + vertex 4.341205e-003 2.462019e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 4.254380e-003 2.412779e-002 7.750000e-002 + vertex 4.341205e-003 2.462019e-002 7.750000e-002 + vertex 0.000000e+000 2.450000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 0.000000e+000 2.450000e-002 7.750000e-002 + vertex 4.341205e-003 2.462019e-002 7.750000e-002 + vertex 0.000000e+000 2.500000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 0.000000e+000 2.450000e-002 7.750000e-002 + vertex 0.000000e+000 2.500000e-002 7.750000e-002 + vertex -4.254380e-003 2.412779e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -4.254380e-003 2.412779e-002 7.750000e-002 + vertex 0.000000e+000 2.500000e-002 7.750000e-002 + vertex -4.341205e-003 2.462019e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.254380e-003 2.412779e-002 7.750000e-002 + vertex -4.341205e-003 2.462019e-002 7.750000e-002 + vertex -8.379494e-003 2.302247e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -8.379494e-003 2.302247e-002 7.750000e-002 + vertex -4.341205e-003 2.462019e-002 7.750000e-002 + vertex -8.550503e-003 2.349232e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -8.379494e-003 2.302247e-002 7.750000e-002 + vertex -8.550503e-003 2.349232e-002 7.750000e-002 + vertex -1.225000e-002 2.121762e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.225000e-002 2.121762e-002 7.750000e-002 + vertex -8.550503e-003 2.349232e-002 7.750000e-002 + vertex -1.250000e-002 2.165063e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.225000e-002 2.121762e-002 7.750000e-002 + vertex -1.250000e-002 2.165063e-002 7.750000e-002 + vertex -1.574830e-002 1.876809e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.574830e-002 1.876809e-002 7.750000e-002 + vertex -1.250000e-002 2.165063e-002 7.750000e-002 + vertex -1.606969e-002 1.915111e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.574830e-002 1.876809e-002 7.750000e-002 + vertex -1.606969e-002 1.915111e-002 7.750000e-002 + vertex -1.876809e-002 1.574830e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.876809e-002 1.574830e-002 7.750000e-002 + vertex -1.606969e-002 1.915111e-002 7.750000e-002 + vertex -1.915111e-002 1.606969e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.876809e-002 1.574830e-002 7.750000e-002 + vertex -1.915111e-002 1.606969e-002 7.750000e-002 + vertex -2.121762e-002 1.225000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -2.121762e-002 1.225000e-002 7.750000e-002 + vertex -1.915111e-002 1.606969e-002 7.750000e-002 + vertex -2.165063e-002 1.250000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.121762e-002 1.225000e-002 7.750000e-002 + vertex -2.165063e-002 1.250000e-002 7.750000e-002 + vertex -2.302247e-002 8.379494e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -2.302247e-002 8.379494e-003 7.750000e-002 + vertex -2.165063e-002 1.250000e-002 7.750000e-002 + vertex -2.349232e-002 8.550503e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.302247e-002 8.379494e-003 7.750000e-002 + vertex -2.349232e-002 8.550503e-003 7.750000e-002 + vertex -2.412779e-002 4.254380e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -2.412779e-002 4.254380e-003 7.750000e-002 + vertex -2.349232e-002 8.550503e-003 7.750000e-002 + vertex -2.462019e-002 4.341205e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.412779e-002 4.254380e-003 7.750000e-002 + vertex -2.462019e-002 4.341205e-003 7.750000e-002 + vertex -2.450000e-002 3.000385e-018 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.450000e-002 3.000385e-018 7.750000e-002 + vertex -2.462019e-002 4.341205e-003 7.750000e-002 + vertex -2.500000e-002 3.061617e-018 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.450000e-002 3.000385e-018 7.750000e-002 + vertex -2.500000e-002 3.061617e-018 7.750000e-002 + vertex -2.412779e-002 -4.254380e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.412779e-002 -4.254380e-003 7.750000e-002 + vertex -2.500000e-002 3.061617e-018 7.750000e-002 + vertex -2.462019e-002 -4.341205e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.412779e-002 -4.254380e-003 7.750000e-002 + vertex -2.462019e-002 -4.341205e-003 7.750000e-002 + vertex -2.302247e-002 -8.379494e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.302247e-002 -8.379494e-003 7.750000e-002 + vertex -2.462019e-002 -4.341205e-003 7.750000e-002 + vertex -2.349232e-002 -8.550503e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.302247e-002 -8.379494e-003 7.750000e-002 + vertex -2.349232e-002 -8.550503e-003 7.750000e-002 + vertex -2.121762e-002 -1.225000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.121762e-002 -1.225000e-002 7.750000e-002 + vertex -2.349232e-002 -8.550503e-003 7.750000e-002 + vertex -2.165063e-002 -1.250000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.121762e-002 -1.225000e-002 7.750000e-002 + vertex -2.165063e-002 -1.250000e-002 7.750000e-002 + vertex -1.876809e-002 -1.574830e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.876809e-002 -1.574830e-002 7.750000e-002 + vertex -2.165063e-002 -1.250000e-002 7.750000e-002 + vertex -1.915111e-002 -1.606969e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.876809e-002 -1.574830e-002 7.750000e-002 + vertex -1.915111e-002 -1.606969e-002 7.750000e-002 + vertex -1.574830e-002 -1.876809e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.574830e-002 -1.876809e-002 7.750000e-002 + vertex -1.915111e-002 -1.606969e-002 7.750000e-002 + vertex -1.606969e-002 -1.915111e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.574830e-002 -1.876809e-002 7.750000e-002 + vertex -1.606969e-002 -1.915111e-002 7.750000e-002 + vertex -1.225000e-002 -2.121762e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.225000e-002 -2.121762e-002 7.750000e-002 + vertex -1.606969e-002 -1.915111e-002 7.750000e-002 + vertex -1.250000e-002 -2.165063e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.225000e-002 -2.121762e-002 7.750000e-002 + vertex -1.250000e-002 -2.165063e-002 7.750000e-002 + vertex -8.379494e-003 -2.302247e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -8.379494e-003 -2.302247e-002 7.750000e-002 + vertex -1.250000e-002 -2.165063e-002 7.750000e-002 + vertex -8.550503e-003 -2.349232e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -8.379494e-003 -2.302247e-002 7.750000e-002 + vertex -8.550503e-003 -2.349232e-002 7.750000e-002 + vertex -4.254380e-003 -2.412779e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.254380e-003 -2.412779e-002 7.750000e-002 + vertex -8.550503e-003 -2.349232e-002 7.750000e-002 + vertex -4.341205e-003 -2.462019e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.254380e-003 -2.412779e-002 7.750000e-002 + vertex -4.341205e-003 -2.462019e-002 7.750000e-002 + vertex -3.469447e-018 -2.450000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -3.469447e-018 -2.450000e-002 7.750000e-002 + vertex -4.341205e-003 -2.462019e-002 7.750000e-002 + vertex -3.469447e-018 -2.500000e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -3.469447e-018 -2.450000e-002 7.750000e-002 + vertex -3.469447e-018 -2.500000e-002 7.750000e-002 + vertex 4.254380e-003 -2.412779e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.254380e-003 -2.412779e-002 7.750000e-002 + vertex -3.469447e-018 -2.500000e-002 7.750000e-002 + vertex 4.341205e-003 -2.462019e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.254380e-003 -2.412779e-002 7.750000e-002 + vertex 4.341205e-003 -2.462019e-002 7.750000e-002 + vertex 8.379494e-003 -2.302247e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 8.379494e-003 -2.302247e-002 7.750000e-002 + vertex 4.341205e-003 -2.462019e-002 7.750000e-002 + vertex 8.550503e-003 -2.349232e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 8.379494e-003 -2.302247e-002 7.750000e-002 + vertex 8.550503e-003 -2.349232e-002 7.750000e-002 + vertex 1.225000e-002 -2.121762e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.225000e-002 -2.121762e-002 7.750000e-002 + vertex 8.550503e-003 -2.349232e-002 7.750000e-002 + vertex 1.250000e-002 -2.165063e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.225000e-002 -2.121762e-002 7.750000e-002 + vertex 1.250000e-002 -2.165063e-002 7.750000e-002 + vertex 1.574830e-002 -1.876809e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.574830e-002 -1.876809e-002 7.750000e-002 + vertex 1.250000e-002 -2.165063e-002 7.750000e-002 + vertex 1.606969e-002 -1.915111e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.574830e-002 -1.876809e-002 7.750000e-002 + vertex 1.606969e-002 -1.915111e-002 7.750000e-002 + vertex 1.876809e-002 -1.574830e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.876809e-002 -1.574830e-002 7.750000e-002 + vertex 1.606969e-002 -1.915111e-002 7.750000e-002 + vertex 1.915111e-002 -1.606969e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.876809e-002 -1.574830e-002 7.750000e-002 + vertex 1.915111e-002 -1.606969e-002 7.750000e-002 + vertex 2.121762e-002 -1.225000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.121762e-002 -1.225000e-002 7.750000e-002 + vertex 1.915111e-002 -1.606969e-002 7.750000e-002 + vertex 2.165063e-002 -1.250000e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.121762e-002 -1.225000e-002 7.750000e-002 + vertex 2.165063e-002 -1.250000e-002 7.750000e-002 + vertex 2.302247e-002 -8.379494e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.302247e-002 -8.379494e-003 7.750000e-002 + vertex 2.165063e-002 -1.250000e-002 7.750000e-002 + vertex 2.349232e-002 -8.550503e-003 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.302247e-002 -8.379494e-003 7.750000e-002 + vertex 2.349232e-002 -8.550503e-003 7.750000e-002 + vertex 2.412779e-002 -4.254380e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.412779e-002 -4.254380e-003 7.750000e-002 + vertex 2.349232e-002 -8.550503e-003 7.750000e-002 + vertex 2.462019e-002 -4.341205e-003 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.412779e-002 -4.254380e-003 7.750000e-002 + vertex 2.462019e-002 -4.341205e-003 7.750000e-002 + vertex 2.450000e-002 0.000000e+000 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.450000e-002 0.000000e+000 7.750000e-002 + vertex 2.462019e-002 -4.341205e-003 7.750000e-002 + vertex 2.500000e-002 -6.123234e-018 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 2.450000e-002 0.000000e+000 7.750000e-002 + vertex 2.500000e-002 -6.123234e-018 7.750000e-002 + vertex 2.412779e-002 4.254380e-003 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.412779e-002 4.254380e-003 7.750000e-002 + vertex 2.500000e-002 -6.123234e-018 7.750000e-002 + vertex 2.462019e-002 4.341205e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 2.412779e-002 4.254380e-003 7.750000e-002 + vertex 2.462019e-002 4.341205e-003 7.750000e-002 + vertex 2.302247e-002 8.379494e-003 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.302247e-002 8.379494e-003 7.750000e-002 + vertex 2.462019e-002 4.341205e-003 7.750000e-002 + vertex 2.349232e-002 8.550503e-003 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 2.302247e-002 8.379494e-003 7.750000e-002 + vertex 2.349232e-002 8.550503e-003 7.750000e-002 + vertex 2.121762e-002 1.225000e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.121762e-002 1.225000e-002 7.750000e-002 + vertex 2.349232e-002 8.550503e-003 7.750000e-002 + vertex 2.165063e-002 1.250000e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 2.121762e-002 1.225000e-002 7.750000e-002 + vertex 2.165063e-002 1.250000e-002 7.750000e-002 + vertex 1.876809e-002 1.574830e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.876809e-002 1.574830e-002 7.750000e-002 + vertex 2.165063e-002 1.250000e-002 7.750000e-002 + vertex 1.915111e-002 1.606969e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.876809e-002 1.574830e-002 7.750000e-002 + vertex 1.915111e-002 1.606969e-002 7.750000e-002 + vertex 1.574830e-002 1.876809e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.574830e-002 1.876809e-002 7.750000e-002 + vertex 1.915111e-002 1.606969e-002 7.750000e-002 + vertex 1.606969e-002 1.915111e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.574830e-002 1.876809e-002 7.750000e-002 + vertex 1.606969e-002 1.915111e-002 7.750000e-002 + vertex 1.225000e-002 2.121762e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.225000e-002 2.121762e-002 7.750000e-002 + vertex 1.606969e-002 1.915111e-002 7.750000e-002 + vertex 1.250000e-002 2.165063e-002 7.750000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.225000e-002 2.121762e-002 7.750000e-002 + vertex 1.250000e-002 2.165063e-002 7.750000e-002 + vertex 8.379494e-003 2.302247e-002 7.750000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 8.379494e-003 2.302247e-002 7.750000e-002 + vertex 1.250000e-002 2.165063e-002 7.750000e-002 + vertex 8.550503e-003 2.349232e-002 7.750000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715542e-002 0.000000e+000 + outer loop + vertex 2.412779e-002 4.254380e-003 7.750000e-002 + vertex 2.450000e-002 0.000000e+000 7.850000e-002 + vertex 2.450000e-002 0.000000e+000 7.750000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715542e-002 0.000000e+000 + outer loop + vertex 2.450000e-002 0.000000e+000 7.750000e-002 + vertex 2.450000e-002 0.000000e+000 7.850000e-002 + vertex 2.412779e-002 -4.254380e-003 7.850000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715542e-002 0.000000e+000 + outer loop + vertex 2.450000e-002 0.000000e+000 7.750000e-002 + vertex 2.412779e-002 -4.254380e-003 7.850000e-002 + vertex 2.412779e-002 -4.254380e-003 7.750000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588192e-001 0.000000e+000 + outer loop + vertex 2.412779e-002 -4.254380e-003 7.750000e-002 + vertex 2.412779e-002 -4.254380e-003 7.850000e-002 + vertex 2.302247e-002 -8.379494e-003 7.850000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588192e-001 2.434797e-016 + outer loop + vertex 2.412779e-002 -4.254380e-003 7.750000e-002 + vertex 2.302247e-002 -8.379494e-003 7.850000e-002 + vertex 2.302247e-002 -8.379494e-003 7.750000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex 2.302247e-002 -8.379494e-003 7.750000e-002 + vertex 2.302247e-002 -8.379494e-003 7.850000e-002 + vertex 2.121762e-002 -1.225000e-002 7.850000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex 2.302247e-002 -8.379494e-003 7.750000e-002 + vertex 2.121762e-002 -1.225000e-002 7.850000e-002 + vertex 2.121762e-002 -1.225000e-002 7.750000e-002 + endloop + endfacet + facet normal 8.191521e-001 -5.735764e-001 0.000000e+000 + outer loop + vertex 2.121762e-002 -1.225000e-002 7.750000e-002 + vertex 2.121762e-002 -1.225000e-002 7.850000e-002 + vertex 1.876809e-002 -1.574830e-002 7.850000e-002 + endloop + endfacet + facet normal 8.191521e-001 -5.735764e-001 2.434796e-016 + outer loop + vertex 2.121762e-002 -1.225000e-002 7.750000e-002 + vertex 1.876809e-002 -1.574830e-002 7.850000e-002 + vertex 1.876809e-002 -1.574830e-002 7.750000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 1.876809e-002 -1.574830e-002 7.750000e-002 + vertex 1.876809e-002 -1.574830e-002 7.850000e-002 + vertex 1.574830e-002 -1.876809e-002 7.850000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 1.876809e-002 -1.574830e-002 7.750000e-002 + vertex 1.574830e-002 -1.876809e-002 7.850000e-002 + vertex 1.574830e-002 -1.876809e-002 7.750000e-002 + endloop + endfacet + facet normal 5.735764e-001 -8.191521e-001 0.000000e+000 + outer loop + vertex 1.574830e-002 -1.876809e-002 7.750000e-002 + vertex 1.574830e-002 -1.876809e-002 7.850000e-002 + vertex 1.225000e-002 -2.121762e-002 7.850000e-002 + endloop + endfacet + facet normal 5.735764e-001 -8.191521e-001 -2.434796e-016 + outer loop + vertex 1.574830e-002 -1.876809e-002 7.750000e-002 + vertex 1.225000e-002 -2.121762e-002 7.850000e-002 + vertex 1.225000e-002 -2.121762e-002 7.750000e-002 + endloop + endfacet + facet normal 4.226184e-001 -9.063077e-001 0.000000e+000 + outer loop + vertex 1.225000e-002 -2.121762e-002 7.750000e-002 + vertex 1.225000e-002 -2.121762e-002 7.850000e-002 + vertex 8.379494e-003 -2.302247e-002 7.850000e-002 + endloop + endfacet + facet normal 4.226184e-001 -9.063077e-001 0.000000e+000 + outer loop + vertex 1.225000e-002 -2.121762e-002 7.750000e-002 + vertex 8.379494e-003 -2.302247e-002 7.850000e-002 + vertex 8.379494e-003 -2.302247e-002 7.750000e-002 + endloop + endfacet + facet normal 2.588192e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 8.379494e-003 -2.302247e-002 7.750000e-002 + vertex 8.379494e-003 -2.302247e-002 7.850000e-002 + vertex 4.254380e-003 -2.412779e-002 7.850000e-002 + endloop + endfacet + facet normal 2.588192e-001 -9.659258e-001 -2.434797e-016 + outer loop + vertex 8.379494e-003 -2.302247e-002 7.750000e-002 + vertex 4.254380e-003 -2.412779e-002 7.850000e-002 + vertex 4.254380e-003 -2.412779e-002 7.750000e-002 + endloop + endfacet + facet normal 8.715542e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 4.254380e-003 -2.412779e-002 7.750000e-002 + vertex 4.254380e-003 -2.412779e-002 7.850000e-002 + vertex -4.500577e-018 -2.450000e-002 7.850000e-002 + endloop + endfacet + facet normal 8.715542e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 4.254380e-003 -2.412779e-002 7.750000e-002 + vertex -4.500577e-018 -2.450000e-002 7.850000e-002 + vertex -4.500577e-018 -2.450000e-002 7.750000e-002 + endloop + endfacet + facet normal -8.715542e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -4.500577e-018 -2.450000e-002 7.750000e-002 + vertex -4.500577e-018 -2.450000e-002 7.850000e-002 + vertex -4.254380e-003 -2.412779e-002 7.850000e-002 + endloop + endfacet + facet normal -8.715542e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -4.500577e-018 -2.450000e-002 7.750000e-002 + vertex -4.254380e-003 -2.412779e-002 7.850000e-002 + vertex -4.254380e-003 -2.412779e-002 7.750000e-002 + endloop + endfacet + facet normal -2.588192e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -4.254380e-003 -2.412779e-002 7.750000e-002 + vertex -4.254380e-003 -2.412779e-002 7.850000e-002 + vertex -8.379494e-003 -2.302247e-002 7.850000e-002 + endloop + endfacet + facet normal -2.588192e-001 -9.659258e-001 2.434797e-016 + outer loop + vertex -4.254380e-003 -2.412779e-002 7.750000e-002 + vertex -8.379494e-003 -2.302247e-002 7.850000e-002 + vertex -8.379494e-003 -2.302247e-002 7.750000e-002 + endloop + endfacet + facet normal -4.226184e-001 -9.063077e-001 0.000000e+000 + outer loop + vertex -8.379494e-003 -2.302247e-002 7.750000e-002 + vertex -8.379494e-003 -2.302247e-002 7.850000e-002 + vertex -1.225000e-002 -2.121762e-002 7.850000e-002 + endloop + endfacet + facet normal -4.226184e-001 -9.063077e-001 0.000000e+000 + outer loop + vertex -8.379494e-003 -2.302247e-002 7.750000e-002 + vertex -1.225000e-002 -2.121762e-002 7.850000e-002 + vertex -1.225000e-002 -2.121762e-002 7.750000e-002 + endloop + endfacet + facet normal -5.735764e-001 -8.191521e-001 0.000000e+000 + outer loop + vertex -1.225000e-002 -2.121762e-002 7.750000e-002 + vertex -1.225000e-002 -2.121762e-002 7.850000e-002 + vertex -1.574830e-002 -1.876809e-002 7.850000e-002 + endloop + endfacet + facet normal -5.735764e-001 -8.191521e-001 2.434796e-016 + outer loop + vertex -1.225000e-002 -2.121762e-002 7.750000e-002 + vertex -1.574830e-002 -1.876809e-002 7.850000e-002 + vertex -1.574830e-002 -1.876809e-002 7.750000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -1.574830e-002 -1.876809e-002 7.750000e-002 + vertex -1.574830e-002 -1.876809e-002 7.850000e-002 + vertex -1.876809e-002 -1.574830e-002 7.850000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -1.574830e-002 -1.876809e-002 7.750000e-002 + vertex -1.876809e-002 -1.574830e-002 7.850000e-002 + vertex -1.876809e-002 -1.574830e-002 7.750000e-002 + endloop + endfacet + facet normal -8.191521e-001 -5.735764e-001 0.000000e+000 + outer loop + vertex -1.876809e-002 -1.574830e-002 7.750000e-002 + vertex -1.876809e-002 -1.574830e-002 7.850000e-002 + vertex -2.121762e-002 -1.225000e-002 7.850000e-002 + endloop + endfacet + facet normal -8.191521e-001 -5.735764e-001 -2.434796e-016 + outer loop + vertex -1.876809e-002 -1.574830e-002 7.750000e-002 + vertex -2.121762e-002 -1.225000e-002 7.850000e-002 + vertex -2.121762e-002 -1.225000e-002 7.750000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex -2.121762e-002 -1.225000e-002 7.750000e-002 + vertex -2.121762e-002 -1.225000e-002 7.850000e-002 + vertex -2.302247e-002 -8.379494e-003 7.850000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex -2.121762e-002 -1.225000e-002 7.750000e-002 + vertex -2.302247e-002 -8.379494e-003 7.850000e-002 + vertex -2.302247e-002 -8.379494e-003 7.750000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588192e-001 0.000000e+000 + outer loop + vertex -2.302247e-002 -8.379494e-003 7.750000e-002 + vertex -2.302247e-002 -8.379494e-003 7.850000e-002 + vertex -2.412779e-002 -4.254380e-003 7.850000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588192e-001 -2.434797e-016 + outer loop + vertex -2.302247e-002 -8.379494e-003 7.750000e-002 + vertex -2.412779e-002 -4.254380e-003 7.850000e-002 + vertex -2.412779e-002 -4.254380e-003 7.750000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715542e-002 0.000000e+000 + outer loop + vertex -2.412779e-002 -4.254380e-003 7.750000e-002 + vertex -2.412779e-002 -4.254380e-003 7.850000e-002 + vertex -2.450000e-002 3.000385e-018 7.850000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715542e-002 0.000000e+000 + outer loop + vertex -2.412779e-002 -4.254380e-003 7.750000e-002 + vertex -2.450000e-002 3.000385e-018 7.850000e-002 + vertex -2.450000e-002 3.000385e-018 7.750000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715542e-002 0.000000e+000 + outer loop + vertex -2.450000e-002 3.000385e-018 7.750000e-002 + vertex -2.450000e-002 3.000385e-018 7.850000e-002 + vertex -2.412779e-002 4.254380e-003 7.850000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715542e-002 0.000000e+000 + outer loop + vertex -2.450000e-002 3.000385e-018 7.750000e-002 + vertex -2.412779e-002 4.254380e-003 7.850000e-002 + vertex -2.412779e-002 4.254380e-003 7.750000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588192e-001 0.000000e+000 + outer loop + vertex -2.412779e-002 4.254380e-003 7.750000e-002 + vertex -2.412779e-002 4.254380e-003 7.850000e-002 + vertex -2.302247e-002 8.379494e-003 7.850000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588192e-001 2.434797e-016 + outer loop + vertex -2.412779e-002 4.254380e-003 7.750000e-002 + vertex -2.302247e-002 8.379494e-003 7.850000e-002 + vertex -2.302247e-002 8.379494e-003 7.750000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 0.000000e+000 + outer loop + vertex -2.302247e-002 8.379494e-003 7.750000e-002 + vertex -2.302247e-002 8.379494e-003 7.850000e-002 + vertex -2.121762e-002 1.225000e-002 7.850000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 0.000000e+000 + outer loop + vertex -2.302247e-002 8.379494e-003 7.750000e-002 + vertex -2.121762e-002 1.225000e-002 7.850000e-002 + vertex -2.121762e-002 1.225000e-002 7.750000e-002 + endloop + endfacet + facet normal -8.191521e-001 5.735764e-001 0.000000e+000 + outer loop + vertex -2.121762e-002 1.225000e-002 7.750000e-002 + vertex -2.121762e-002 1.225000e-002 7.850000e-002 + vertex -1.876809e-002 1.574830e-002 7.850000e-002 + endloop + endfacet + facet normal -8.191521e-001 5.735764e-001 2.434796e-016 + outer loop + vertex -2.121762e-002 1.225000e-002 7.750000e-002 + vertex -1.876809e-002 1.574830e-002 7.850000e-002 + vertex -1.876809e-002 1.574830e-002 7.750000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -1.876809e-002 1.574830e-002 7.750000e-002 + vertex -1.876809e-002 1.574830e-002 7.850000e-002 + vertex -1.574830e-002 1.876809e-002 7.850000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -1.876809e-002 1.574830e-002 7.750000e-002 + vertex -1.574830e-002 1.876809e-002 7.850000e-002 + vertex -1.574830e-002 1.876809e-002 7.750000e-002 + endloop + endfacet + facet normal -5.735764e-001 8.191521e-001 0.000000e+000 + outer loop + vertex -1.574830e-002 1.876809e-002 7.750000e-002 + vertex -1.574830e-002 1.876809e-002 7.850000e-002 + vertex -1.225000e-002 2.121762e-002 7.850000e-002 + endloop + endfacet + facet normal -5.735764e-001 8.191521e-001 -2.434796e-016 + outer loop + vertex -1.574830e-002 1.876809e-002 7.750000e-002 + vertex -1.225000e-002 2.121762e-002 7.850000e-002 + vertex -1.225000e-002 2.121762e-002 7.750000e-002 + endloop + endfacet + facet normal -4.226184e-001 9.063077e-001 0.000000e+000 + outer loop + vertex -1.225000e-002 2.121762e-002 7.750000e-002 + vertex -1.225000e-002 2.121762e-002 7.850000e-002 + vertex -8.379494e-003 2.302247e-002 7.850000e-002 + endloop + endfacet + facet normal -4.226184e-001 9.063077e-001 0.000000e+000 + outer loop + vertex -1.225000e-002 2.121762e-002 7.750000e-002 + vertex -8.379494e-003 2.302247e-002 7.850000e-002 + vertex -8.379494e-003 2.302247e-002 7.750000e-002 + endloop + endfacet + facet normal -2.588192e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -8.379494e-003 2.302247e-002 7.750000e-002 + vertex -8.379494e-003 2.302247e-002 7.850000e-002 + vertex -4.254380e-003 2.412779e-002 7.850000e-002 + endloop + endfacet + facet normal -2.588192e-001 9.659258e-001 -2.434797e-016 + outer loop + vertex -8.379494e-003 2.302247e-002 7.750000e-002 + vertex -4.254380e-003 2.412779e-002 7.850000e-002 + vertex -4.254380e-003 2.412779e-002 7.750000e-002 + endloop + endfacet + facet normal -8.715542e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -4.254380e-003 2.412779e-002 7.750000e-002 + vertex -4.254380e-003 2.412779e-002 7.850000e-002 + vertex 1.500192e-018 2.450000e-002 7.850000e-002 + endloop + endfacet + facet normal -8.715542e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -4.254380e-003 2.412779e-002 7.750000e-002 + vertex 1.500192e-018 2.450000e-002 7.850000e-002 + vertex 1.500192e-018 2.450000e-002 7.750000e-002 + endloop + endfacet + facet normal 8.715542e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex 1.500192e-018 2.450000e-002 7.750000e-002 + vertex 1.500192e-018 2.450000e-002 7.850000e-002 + vertex 4.254380e-003 2.412779e-002 7.850000e-002 + endloop + endfacet + facet normal 8.715542e-002 9.961947e-001 0.000000e+000 + outer loop + vertex 1.500192e-018 2.450000e-002 7.750000e-002 + vertex 4.254380e-003 2.412779e-002 7.850000e-002 + vertex 4.254380e-003 2.412779e-002 7.750000e-002 + endloop + endfacet + facet normal 2.588192e-001 9.659258e-001 -0.000000e+000 + outer loop + vertex 4.254380e-003 2.412779e-002 7.750000e-002 + vertex 4.254380e-003 2.412779e-002 7.850000e-002 + vertex 8.379494e-003 2.302247e-002 7.850000e-002 + endloop + endfacet + facet normal 2.588192e-001 9.659258e-001 2.434797e-016 + outer loop + vertex 4.254380e-003 2.412779e-002 7.750000e-002 + vertex 8.379494e-003 2.302247e-002 7.850000e-002 + vertex 8.379494e-003 2.302247e-002 7.750000e-002 + endloop + endfacet + facet normal 4.226184e-001 9.063077e-001 -0.000000e+000 + outer loop + vertex 8.379494e-003 2.302247e-002 7.750000e-002 + vertex 8.379494e-003 2.302247e-002 7.850000e-002 + vertex 1.225000e-002 2.121762e-002 7.850000e-002 + endloop + endfacet + facet normal 4.226184e-001 9.063077e-001 0.000000e+000 + outer loop + vertex 8.379494e-003 2.302247e-002 7.750000e-002 + vertex 1.225000e-002 2.121762e-002 7.850000e-002 + vertex 1.225000e-002 2.121762e-002 7.750000e-002 + endloop + endfacet + facet normal 5.735764e-001 8.191521e-001 -0.000000e+000 + outer loop + vertex 1.225000e-002 2.121762e-002 7.750000e-002 + vertex 1.225000e-002 2.121762e-002 7.850000e-002 + vertex 1.574830e-002 1.876809e-002 7.850000e-002 + endloop + endfacet + facet normal 5.735764e-001 8.191521e-001 2.434796e-016 + outer loop + vertex 1.225000e-002 2.121762e-002 7.750000e-002 + vertex 1.574830e-002 1.876809e-002 7.850000e-002 + vertex 1.574830e-002 1.876809e-002 7.750000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 -0.000000e+000 + outer loop + vertex 1.574830e-002 1.876809e-002 7.750000e-002 + vertex 1.574830e-002 1.876809e-002 7.850000e-002 + vertex 1.876809e-002 1.574830e-002 7.850000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex 1.574830e-002 1.876809e-002 7.750000e-002 + vertex 1.876809e-002 1.574830e-002 7.850000e-002 + vertex 1.876809e-002 1.574830e-002 7.750000e-002 + endloop + endfacet + facet normal 8.191521e-001 5.735764e-001 -0.000000e+000 + outer loop + vertex 1.876809e-002 1.574830e-002 7.750000e-002 + vertex 1.876809e-002 1.574830e-002 7.850000e-002 + vertex 2.121762e-002 1.225000e-002 7.850000e-002 + endloop + endfacet + facet normal 8.191521e-001 5.735764e-001 -2.434796e-016 + outer loop + vertex 1.876809e-002 1.574830e-002 7.750000e-002 + vertex 2.121762e-002 1.225000e-002 7.850000e-002 + vertex 2.121762e-002 1.225000e-002 7.750000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 -0.000000e+000 + outer loop + vertex 2.121762e-002 1.225000e-002 7.750000e-002 + vertex 2.121762e-002 1.225000e-002 7.850000e-002 + vertex 2.302247e-002 8.379494e-003 7.850000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 0.000000e+000 + outer loop + vertex 2.121762e-002 1.225000e-002 7.750000e-002 + vertex 2.302247e-002 8.379494e-003 7.850000e-002 + vertex 2.302247e-002 8.379494e-003 7.750000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588192e-001 -0.000000e+000 + outer loop + vertex 2.302247e-002 8.379494e-003 7.750000e-002 + vertex 2.302247e-002 8.379494e-003 7.850000e-002 + vertex 2.412779e-002 4.254380e-003 7.850000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588192e-001 -2.434797e-016 + outer loop + vertex 2.302247e-002 8.379494e-003 7.750000e-002 + vertex 2.412779e-002 4.254380e-003 7.850000e-002 + vertex 2.412779e-002 4.254380e-003 7.750000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715542e-002 -0.000000e+000 + outer loop + vertex 2.412779e-002 4.254380e-003 7.750000e-002 + vertex 2.412779e-002 4.254380e-003 7.850000e-002 + vertex 2.450000e-002 0.000000e+000 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.254380e-003 2.412779e-002 7.850000e-002 + vertex -4.341205e-003 2.462019e-002 7.850000e-002 + vertex 0.000000e+000 2.450000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 2.450000e-002 7.850000e-002 + vertex -4.341205e-003 2.462019e-002 7.850000e-002 + vertex 0.000000e+000 2.500000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 0.000000e+000 2.450000e-002 7.850000e-002 + vertex 0.000000e+000 2.500000e-002 7.850000e-002 + vertex 4.254380e-003 2.412779e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 4.254380e-003 2.412779e-002 7.850000e-002 + vertex 0.000000e+000 2.500000e-002 7.850000e-002 + vertex 4.341205e-003 2.462019e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 4.254380e-003 2.412779e-002 7.850000e-002 + vertex 4.341205e-003 2.462019e-002 7.850000e-002 + vertex 8.379494e-003 2.302247e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 8.379494e-003 2.302247e-002 7.850000e-002 + vertex 4.341205e-003 2.462019e-002 7.850000e-002 + vertex 8.550503e-003 2.349232e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 8.379494e-003 2.302247e-002 7.850000e-002 + vertex 8.550503e-003 2.349232e-002 7.850000e-002 + vertex 1.225000e-002 2.121762e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.225000e-002 2.121762e-002 7.850000e-002 + vertex 8.550503e-003 2.349232e-002 7.850000e-002 + vertex 1.250000e-002 2.165063e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.225000e-002 2.121762e-002 7.850000e-002 + vertex 1.250000e-002 2.165063e-002 7.850000e-002 + vertex 1.574830e-002 1.876809e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.574830e-002 1.876809e-002 7.850000e-002 + vertex 1.250000e-002 2.165063e-002 7.850000e-002 + vertex 1.606969e-002 1.915111e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.574830e-002 1.876809e-002 7.850000e-002 + vertex 1.606969e-002 1.915111e-002 7.850000e-002 + vertex 1.876809e-002 1.574830e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.876809e-002 1.574830e-002 7.850000e-002 + vertex 1.606969e-002 1.915111e-002 7.850000e-002 + vertex 1.915111e-002 1.606969e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.876809e-002 1.574830e-002 7.850000e-002 + vertex 1.915111e-002 1.606969e-002 7.850000e-002 + vertex 2.121762e-002 1.225000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.121762e-002 1.225000e-002 7.850000e-002 + vertex 1.915111e-002 1.606969e-002 7.850000e-002 + vertex 2.165063e-002 1.250000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.121762e-002 1.225000e-002 7.850000e-002 + vertex 2.165063e-002 1.250000e-002 7.850000e-002 + vertex 2.302247e-002 8.379494e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.302247e-002 8.379494e-003 7.850000e-002 + vertex 2.165063e-002 1.250000e-002 7.850000e-002 + vertex 2.349232e-002 8.550503e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.302247e-002 8.379494e-003 7.850000e-002 + vertex 2.349232e-002 8.550503e-003 7.850000e-002 + vertex 2.412779e-002 4.254380e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.412779e-002 4.254380e-003 7.850000e-002 + vertex 2.349232e-002 8.550503e-003 7.850000e-002 + vertex 2.462019e-002 4.341205e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.412779e-002 4.254380e-003 7.850000e-002 + vertex 2.462019e-002 4.341205e-003 7.850000e-002 + vertex 2.450000e-002 -6.000769e-018 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.450000e-002 -6.000769e-018 7.850000e-002 + vertex 2.462019e-002 4.341205e-003 7.850000e-002 + vertex 2.500000e-002 0.000000e+000 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 2.450000e-002 -6.000769e-018 7.850000e-002 + vertex 2.500000e-002 0.000000e+000 7.850000e-002 + vertex 2.412779e-002 -4.254380e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.412779e-002 -4.254380e-003 7.850000e-002 + vertex 2.500000e-002 0.000000e+000 7.850000e-002 + vertex 2.462019e-002 -4.341205e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 2.412779e-002 -4.254380e-003 7.850000e-002 + vertex 2.462019e-002 -4.341205e-003 7.850000e-002 + vertex 2.302247e-002 -8.379494e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.302247e-002 -8.379494e-003 7.850000e-002 + vertex 2.462019e-002 -4.341205e-003 7.850000e-002 + vertex 2.349232e-002 -8.550503e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 2.302247e-002 -8.379494e-003 7.850000e-002 + vertex 2.349232e-002 -8.550503e-003 7.850000e-002 + vertex 2.121762e-002 -1.225000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 2.121762e-002 -1.225000e-002 7.850000e-002 + vertex 2.349232e-002 -8.550503e-003 7.850000e-002 + vertex 2.165063e-002 -1.250000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 2.121762e-002 -1.225000e-002 7.850000e-002 + vertex 2.165063e-002 -1.250000e-002 7.850000e-002 + vertex 1.876809e-002 -1.574830e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.876809e-002 -1.574830e-002 7.850000e-002 + vertex 2.165063e-002 -1.250000e-002 7.850000e-002 + vertex 1.915111e-002 -1.606969e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.876809e-002 -1.574830e-002 7.850000e-002 + vertex 1.915111e-002 -1.606969e-002 7.850000e-002 + vertex 1.574830e-002 -1.876809e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.574830e-002 -1.876809e-002 7.850000e-002 + vertex 1.915111e-002 -1.606969e-002 7.850000e-002 + vertex 1.606969e-002 -1.915111e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.574830e-002 -1.876809e-002 7.850000e-002 + vertex 1.606969e-002 -1.915111e-002 7.850000e-002 + vertex 1.225000e-002 -2.121762e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.225000e-002 -2.121762e-002 7.850000e-002 + vertex 1.606969e-002 -1.915111e-002 7.850000e-002 + vertex 1.250000e-002 -2.165063e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 1.225000e-002 -2.121762e-002 7.850000e-002 + vertex 1.250000e-002 -2.165063e-002 7.850000e-002 + vertex 8.379494e-003 -2.302247e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 8.379494e-003 -2.302247e-002 7.850000e-002 + vertex 1.250000e-002 -2.165063e-002 7.850000e-002 + vertex 8.550503e-003 -2.349232e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 8.379494e-003 -2.302247e-002 7.850000e-002 + vertex 8.550503e-003 -2.349232e-002 7.850000e-002 + vertex 4.254380e-003 -2.412779e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 4.254380e-003 -2.412779e-002 7.850000e-002 + vertex 8.550503e-003 -2.349232e-002 7.850000e-002 + vertex 4.341205e-003 -2.462019e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 4.254380e-003 -2.412779e-002 7.850000e-002 + vertex 4.341205e-003 -2.462019e-002 7.850000e-002 + vertex -3.469447e-018 -2.450000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.469447e-018 -2.450000e-002 7.850000e-002 + vertex 4.341205e-003 -2.462019e-002 7.850000e-002 + vertex -3.469447e-018 -2.500000e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -3.469447e-018 -2.450000e-002 7.850000e-002 + vertex -3.469447e-018 -2.500000e-002 7.850000e-002 + vertex -4.254380e-003 -2.412779e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -4.254380e-003 -2.412779e-002 7.850000e-002 + vertex -3.469447e-018 -2.500000e-002 7.850000e-002 + vertex -4.341205e-003 -2.462019e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.254380e-003 -2.412779e-002 7.850000e-002 + vertex -4.341205e-003 -2.462019e-002 7.850000e-002 + vertex -8.379494e-003 -2.302247e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -8.379494e-003 -2.302247e-002 7.850000e-002 + vertex -4.341205e-003 -2.462019e-002 7.850000e-002 + vertex -8.550503e-003 -2.349232e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.379494e-003 -2.302247e-002 7.850000e-002 + vertex -8.550503e-003 -2.349232e-002 7.850000e-002 + vertex -1.225000e-002 -2.121762e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.225000e-002 -2.121762e-002 7.850000e-002 + vertex -8.550503e-003 -2.349232e-002 7.850000e-002 + vertex -1.250000e-002 -2.165063e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.225000e-002 -2.121762e-002 7.850000e-002 + vertex -1.250000e-002 -2.165063e-002 7.850000e-002 + vertex -1.574830e-002 -1.876809e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.574830e-002 -1.876809e-002 7.850000e-002 + vertex -1.250000e-002 -2.165063e-002 7.850000e-002 + vertex -1.606969e-002 -1.915111e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.574830e-002 -1.876809e-002 7.850000e-002 + vertex -1.606969e-002 -1.915111e-002 7.850000e-002 + vertex -1.876809e-002 -1.574830e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.876809e-002 -1.574830e-002 7.850000e-002 + vertex -1.606969e-002 -1.915111e-002 7.850000e-002 + vertex -1.915111e-002 -1.606969e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.876809e-002 -1.574830e-002 7.850000e-002 + vertex -1.915111e-002 -1.606969e-002 7.850000e-002 + vertex -2.121762e-002 -1.225000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -2.121762e-002 -1.225000e-002 7.850000e-002 + vertex -1.915111e-002 -1.606969e-002 7.850000e-002 + vertex -2.165063e-002 -1.250000e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.121762e-002 -1.225000e-002 7.850000e-002 + vertex -2.165063e-002 -1.250000e-002 7.850000e-002 + vertex -2.302247e-002 -8.379494e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -2.302247e-002 -8.379494e-003 7.850000e-002 + vertex -2.165063e-002 -1.250000e-002 7.850000e-002 + vertex -2.349232e-002 -8.550503e-003 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.302247e-002 -8.379494e-003 7.850000e-002 + vertex -2.349232e-002 -8.550503e-003 7.850000e-002 + vertex -2.412779e-002 -4.254380e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -2.412779e-002 -4.254380e-003 7.850000e-002 + vertex -2.349232e-002 -8.550503e-003 7.850000e-002 + vertex -2.462019e-002 -4.341205e-003 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.412779e-002 -4.254380e-003 7.850000e-002 + vertex -2.462019e-002 -4.341205e-003 7.850000e-002 + vertex -2.450000e-002 3.000385e-018 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.450000e-002 3.000385e-018 7.850000e-002 + vertex -2.462019e-002 -4.341205e-003 7.850000e-002 + vertex -2.500000e-002 3.061617e-018 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.450000e-002 3.000385e-018 7.850000e-002 + vertex -2.500000e-002 3.061617e-018 7.850000e-002 + vertex -2.412779e-002 4.254380e-003 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.412779e-002 4.254380e-003 7.850000e-002 + vertex -2.500000e-002 3.061617e-018 7.850000e-002 + vertex -2.462019e-002 4.341205e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.412779e-002 4.254380e-003 7.850000e-002 + vertex -2.462019e-002 4.341205e-003 7.850000e-002 + vertex -2.302247e-002 8.379494e-003 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.302247e-002 8.379494e-003 7.850000e-002 + vertex -2.462019e-002 4.341205e-003 7.850000e-002 + vertex -2.349232e-002 8.550503e-003 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.302247e-002 8.379494e-003 7.850000e-002 + vertex -2.349232e-002 8.550503e-003 7.850000e-002 + vertex -2.121762e-002 1.225000e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.121762e-002 1.225000e-002 7.850000e-002 + vertex -2.349232e-002 8.550503e-003 7.850000e-002 + vertex -2.165063e-002 1.250000e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -2.121762e-002 1.225000e-002 7.850000e-002 + vertex -2.165063e-002 1.250000e-002 7.850000e-002 + vertex -1.876809e-002 1.574830e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.876809e-002 1.574830e-002 7.850000e-002 + vertex -2.165063e-002 1.250000e-002 7.850000e-002 + vertex -1.915111e-002 1.606969e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.876809e-002 1.574830e-002 7.850000e-002 + vertex -1.915111e-002 1.606969e-002 7.850000e-002 + vertex -1.574830e-002 1.876809e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.574830e-002 1.876809e-002 7.850000e-002 + vertex -1.915111e-002 1.606969e-002 7.850000e-002 + vertex -1.606969e-002 1.915111e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.574830e-002 1.876809e-002 7.850000e-002 + vertex -1.606969e-002 1.915111e-002 7.850000e-002 + vertex -1.225000e-002 2.121762e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.225000e-002 2.121762e-002 7.850000e-002 + vertex -1.606969e-002 1.915111e-002 7.850000e-002 + vertex -1.250000e-002 2.165063e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.225000e-002 2.121762e-002 7.850000e-002 + vertex -1.250000e-002 2.165063e-002 7.850000e-002 + vertex -8.379494e-003 2.302247e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.379494e-003 2.302247e-002 7.850000e-002 + vertex -1.250000e-002 2.165063e-002 7.850000e-002 + vertex -8.550503e-003 2.349232e-002 7.850000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.379494e-003 2.302247e-002 7.850000e-002 + vertex -8.550503e-003 2.349232e-002 7.850000e-002 + vertex -4.254380e-003 2.412779e-002 7.850000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -4.254380e-003 2.412779e-002 7.850000e-002 + vertex -8.550503e-003 2.349232e-002 7.850000e-002 + vertex -4.341205e-003 2.462019e-002 7.850000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715579e-002 -2.216205e-017 + outer loop + vertex 2.462019e-002 4.341205e-003 7.850000e-002 + vertex 2.500000e-002 0.000000e+000 8.200000e-002 + vertex 2.500000e-002 0.000000e+000 7.850000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715579e-002 0.000000e+000 + outer loop + vertex 2.500000e-002 0.000000e+000 7.850000e-002 + vertex 2.500000e-002 0.000000e+000 8.200000e-002 + vertex 2.462019e-002 -4.341205e-003 8.200000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715579e-002 2.216205e-017 + outer loop + vertex 2.500000e-002 0.000000e+000 7.850000e-002 + vertex 2.462019e-002 -4.341205e-003 8.200000e-002 + vertex 2.462019e-002 -4.341205e-003 7.850000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex 2.462019e-002 -4.341205e-003 7.850000e-002 + vertex 2.462019e-002 -4.341205e-003 8.200000e-002 + vertex 2.349232e-002 -8.550503e-003 8.200000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex 2.462019e-002 -4.341205e-003 7.850000e-002 + vertex 2.349232e-002 -8.550503e-003 8.200000e-002 + vertex 2.349232e-002 -8.550503e-003 7.850000e-002 + endloop + endfacet + facet normal 9.063078e-001 -4.226183e-001 0.000000e+000 + outer loop + vertex 2.349232e-002 -8.550503e-003 7.850000e-002 + vertex 2.349232e-002 -8.550503e-003 8.200000e-002 + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal 9.063078e-001 -4.226183e-001 -8.864820e-017 + outer loop + vertex 2.349232e-002 -8.550503e-003 7.850000e-002 + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + vertex 2.165063e-002 -1.250000e-002 7.850000e-002 + endloop + endfacet + facet normal 8.191519e-001 -5.735766e-001 0.000000e+000 + outer loop + vertex 2.165063e-002 -1.250000e-002 7.850000e-002 + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + vertex 1.915111e-002 -1.606969e-002 8.200000e-002 + endloop + endfacet + facet normal 8.191519e-001 -5.735766e-001 -8.864819e-017 + outer loop + vertex 2.165063e-002 -1.250000e-002 7.850000e-002 + vertex 1.915111e-002 -1.606969e-002 8.200000e-002 + vertex 1.915111e-002 -1.606969e-002 7.850000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 1.915111e-002 -1.606969e-002 7.850000e-002 + vertex 1.915111e-002 -1.606969e-002 8.200000e-002 + vertex 1.606969e-002 -1.915111e-002 8.200000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 1.915111e-002 -1.606969e-002 7.850000e-002 + vertex 1.606969e-002 -1.915111e-002 8.200000e-002 + vertex 1.606969e-002 -1.915111e-002 7.850000e-002 + endloop + endfacet + facet normal 5.735766e-001 -8.191519e-001 0.000000e+000 + outer loop + vertex 1.606969e-002 -1.915111e-002 7.850000e-002 + vertex 1.606969e-002 -1.915111e-002 8.200000e-002 + vertex 1.250000e-002 -2.165063e-002 8.200000e-002 + endloop + endfacet + facet normal 5.735766e-001 -8.191519e-001 8.864819e-017 + outer loop + vertex 1.606969e-002 -1.915111e-002 7.850000e-002 + vertex 1.250000e-002 -2.165063e-002 8.200000e-002 + vertex 1.250000e-002 -2.165063e-002 7.850000e-002 + endloop + endfacet + facet normal 4.226183e-001 -9.063078e-001 0.000000e+000 + outer loop + vertex 1.250000e-002 -2.165063e-002 7.850000e-002 + vertex 1.250000e-002 -2.165063e-002 8.200000e-002 + vertex 8.550503e-003 -2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal 4.226183e-001 -9.063078e-001 8.864820e-017 + outer loop + vertex 1.250000e-002 -2.165063e-002 7.850000e-002 + vertex 8.550503e-003 -2.349232e-002 8.200000e-002 + vertex 8.550503e-003 -2.349232e-002 7.850000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 8.550503e-003 -2.349232e-002 7.850000e-002 + vertex 8.550503e-003 -2.349232e-002 8.200000e-002 + vertex 4.341205e-003 -2.462019e-002 8.200000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 8.550503e-003 -2.349232e-002 7.850000e-002 + vertex 4.341205e-003 -2.462019e-002 8.200000e-002 + vertex 4.341205e-003 -2.462019e-002 7.850000e-002 + endloop + endfacet + facet normal 8.715579e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 4.341205e-003 -2.462019e-002 7.850000e-002 + vertex 4.341205e-003 -2.462019e-002 8.200000e-002 + vertex -4.592426e-018 -2.500000e-002 8.200000e-002 + endloop + endfacet + facet normal 8.715579e-002 -9.961947e-001 -2.216205e-017 + outer loop + vertex 4.341205e-003 -2.462019e-002 7.850000e-002 + vertex -4.592426e-018 -2.500000e-002 8.200000e-002 + vertex -4.592426e-018 -2.500000e-002 7.850000e-002 + endloop + endfacet + facet normal -8.715579e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -4.592426e-018 -2.500000e-002 7.850000e-002 + vertex -4.592426e-018 -2.500000e-002 8.200000e-002 + vertex -4.341205e-003 -2.462019e-002 8.200000e-002 + endloop + endfacet + facet normal -8.715579e-002 -9.961947e-001 2.216205e-017 + outer loop + vertex -4.592426e-018 -2.500000e-002 7.850000e-002 + vertex -4.341205e-003 -2.462019e-002 8.200000e-002 + vertex -4.341205e-003 -2.462019e-002 7.850000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -4.341205e-003 -2.462019e-002 7.850000e-002 + vertex -4.341205e-003 -2.462019e-002 8.200000e-002 + vertex -8.550503e-003 -2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -4.341205e-003 -2.462019e-002 7.850000e-002 + vertex -8.550503e-003 -2.349232e-002 8.200000e-002 + vertex -8.550503e-003 -2.349232e-002 7.850000e-002 + endloop + endfacet + facet normal -4.226183e-001 -9.063078e-001 0.000000e+000 + outer loop + vertex -8.550503e-003 -2.349232e-002 7.850000e-002 + vertex -8.550503e-003 -2.349232e-002 8.200000e-002 + vertex -1.250000e-002 -2.165063e-002 8.200000e-002 + endloop + endfacet + facet normal -4.226183e-001 -9.063078e-001 -8.864820e-017 + outer loop + vertex -8.550503e-003 -2.349232e-002 7.850000e-002 + vertex -1.250000e-002 -2.165063e-002 8.200000e-002 + vertex -1.250000e-002 -2.165063e-002 7.850000e-002 + endloop + endfacet + facet normal -5.735766e-001 -8.191519e-001 0.000000e+000 + outer loop + vertex -1.250000e-002 -2.165063e-002 7.850000e-002 + vertex -1.250000e-002 -2.165063e-002 8.200000e-002 + vertex -1.606969e-002 -1.915111e-002 8.200000e-002 + endloop + endfacet + facet normal -5.735766e-001 -8.191519e-001 -8.864819e-017 + outer loop + vertex -1.250000e-002 -2.165063e-002 7.850000e-002 + vertex -1.606969e-002 -1.915111e-002 8.200000e-002 + vertex -1.606969e-002 -1.915111e-002 7.850000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -1.606969e-002 -1.915111e-002 7.850000e-002 + vertex -1.606969e-002 -1.915111e-002 8.200000e-002 + vertex -1.915111e-002 -1.606969e-002 8.200000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -1.606969e-002 -1.915111e-002 7.850000e-002 + vertex -1.915111e-002 -1.606969e-002 8.200000e-002 + vertex -1.915111e-002 -1.606969e-002 7.850000e-002 + endloop + endfacet + facet normal -8.191519e-001 -5.735766e-001 0.000000e+000 + outer loop + vertex -1.915111e-002 -1.606969e-002 7.850000e-002 + vertex -1.915111e-002 -1.606969e-002 8.200000e-002 + vertex -2.165063e-002 -1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal -8.191519e-001 -5.735766e-001 8.864819e-017 + outer loop + vertex -1.915111e-002 -1.606969e-002 7.850000e-002 + vertex -2.165063e-002 -1.250000e-002 8.200000e-002 + vertex -2.165063e-002 -1.250000e-002 7.850000e-002 + endloop + endfacet + facet normal -9.063078e-001 -4.226183e-001 0.000000e+000 + outer loop + vertex -2.165063e-002 -1.250000e-002 7.850000e-002 + vertex -2.165063e-002 -1.250000e-002 8.200000e-002 + vertex -2.349232e-002 -8.550503e-003 8.200000e-002 + endloop + endfacet + facet normal -9.063078e-001 -4.226183e-001 8.864820e-017 + outer loop + vertex -2.165063e-002 -1.250000e-002 7.850000e-002 + vertex -2.349232e-002 -8.550503e-003 8.200000e-002 + vertex -2.349232e-002 -8.550503e-003 7.850000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex -2.349232e-002 -8.550503e-003 7.850000e-002 + vertex -2.349232e-002 -8.550503e-003 8.200000e-002 + vertex -2.462019e-002 -4.341205e-003 8.200000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex -2.349232e-002 -8.550503e-003 7.850000e-002 + vertex -2.462019e-002 -4.341205e-003 8.200000e-002 + vertex -2.462019e-002 -4.341205e-003 7.850000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715579e-002 0.000000e+000 + outer loop + vertex -2.462019e-002 -4.341205e-003 7.850000e-002 + vertex -2.462019e-002 -4.341205e-003 8.200000e-002 + vertex -2.500000e-002 3.061617e-018 8.200000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715579e-002 -2.216205e-017 + outer loop + vertex -2.462019e-002 -4.341205e-003 7.850000e-002 + vertex -2.500000e-002 3.061617e-018 8.200000e-002 + vertex -2.500000e-002 3.061617e-018 7.850000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715579e-002 0.000000e+000 + outer loop + vertex -2.500000e-002 3.061617e-018 7.850000e-002 + vertex -2.500000e-002 3.061617e-018 8.200000e-002 + vertex -2.462019e-002 4.341205e-003 8.200000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715579e-002 2.216205e-017 + outer loop + vertex -2.500000e-002 3.061617e-018 7.850000e-002 + vertex -2.462019e-002 4.341205e-003 8.200000e-002 + vertex -2.462019e-002 4.341205e-003 7.850000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex -2.462019e-002 4.341205e-003 7.850000e-002 + vertex -2.462019e-002 4.341205e-003 8.200000e-002 + vertex -2.349232e-002 8.550503e-003 8.200000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex -2.462019e-002 4.341205e-003 7.850000e-002 + vertex -2.349232e-002 8.550503e-003 8.200000e-002 + vertex -2.349232e-002 8.550503e-003 7.850000e-002 + endloop + endfacet + facet normal -9.063078e-001 4.226183e-001 0.000000e+000 + outer loop + vertex -2.349232e-002 8.550503e-003 7.850000e-002 + vertex -2.349232e-002 8.550503e-003 8.200000e-002 + vertex -2.165063e-002 1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal -9.063078e-001 4.226183e-001 -8.864820e-017 + outer loop + vertex -2.349232e-002 8.550503e-003 7.850000e-002 + vertex -2.165063e-002 1.250000e-002 8.200000e-002 + vertex -2.165063e-002 1.250000e-002 7.850000e-002 + endloop + endfacet + facet normal -8.191519e-001 5.735766e-001 0.000000e+000 + outer loop + vertex -2.165063e-002 1.250000e-002 7.850000e-002 + vertex -2.165063e-002 1.250000e-002 8.200000e-002 + vertex -1.915111e-002 1.606969e-002 8.200000e-002 + endloop + endfacet + facet normal -8.191519e-001 5.735766e-001 -8.864819e-017 + outer loop + vertex -2.165063e-002 1.250000e-002 7.850000e-002 + vertex -1.915111e-002 1.606969e-002 8.200000e-002 + vertex -1.915111e-002 1.606969e-002 7.850000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -1.915111e-002 1.606969e-002 7.850000e-002 + vertex -1.915111e-002 1.606969e-002 8.200000e-002 + vertex -1.606969e-002 1.915111e-002 8.200000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -1.915111e-002 1.606969e-002 7.850000e-002 + vertex -1.606969e-002 1.915111e-002 8.200000e-002 + vertex -1.606969e-002 1.915111e-002 7.850000e-002 + endloop + endfacet + facet normal -5.735766e-001 8.191519e-001 0.000000e+000 + outer loop + vertex -1.606969e-002 1.915111e-002 7.850000e-002 + vertex -1.606969e-002 1.915111e-002 8.200000e-002 + vertex -1.250000e-002 2.165063e-002 8.200000e-002 + endloop + endfacet + facet normal -5.735766e-001 8.191519e-001 8.864819e-017 + outer loop + vertex -1.606969e-002 1.915111e-002 7.850000e-002 + vertex -1.250000e-002 2.165063e-002 8.200000e-002 + vertex -1.250000e-002 2.165063e-002 7.850000e-002 + endloop + endfacet + facet normal -4.226183e-001 9.063078e-001 0.000000e+000 + outer loop + vertex -1.250000e-002 2.165063e-002 7.850000e-002 + vertex -1.250000e-002 2.165063e-002 8.200000e-002 + vertex -8.550503e-003 2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal -4.226183e-001 9.063078e-001 8.864820e-017 + outer loop + vertex -1.250000e-002 2.165063e-002 7.850000e-002 + vertex -8.550503e-003 2.349232e-002 8.200000e-002 + vertex -8.550503e-003 2.349232e-002 7.850000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -8.550503e-003 2.349232e-002 7.850000e-002 + vertex -8.550503e-003 2.349232e-002 8.200000e-002 + vertex -4.341205e-003 2.462019e-002 8.200000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -8.550503e-003 2.349232e-002 7.850000e-002 + vertex -4.341205e-003 2.462019e-002 8.200000e-002 + vertex -4.341205e-003 2.462019e-002 7.850000e-002 + endloop + endfacet + facet normal -8.715579e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -4.341205e-003 2.462019e-002 7.850000e-002 + vertex -4.341205e-003 2.462019e-002 8.200000e-002 + vertex 1.530808e-018 2.500000e-002 8.200000e-002 + endloop + endfacet + facet normal -8.715579e-002 9.961947e-001 -2.216205e-017 + outer loop + vertex -4.341205e-003 2.462019e-002 7.850000e-002 + vertex 1.530808e-018 2.500000e-002 8.200000e-002 + vertex 1.530808e-018 2.500000e-002 7.850000e-002 + endloop + endfacet + facet normal 8.715579e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex 1.530808e-018 2.500000e-002 7.850000e-002 + vertex 1.530808e-018 2.500000e-002 8.200000e-002 + vertex 4.341205e-003 2.462019e-002 8.200000e-002 + endloop + endfacet + facet normal 8.715579e-002 9.961947e-001 2.216205e-017 + outer loop + vertex 1.530808e-018 2.500000e-002 7.850000e-002 + vertex 4.341205e-003 2.462019e-002 8.200000e-002 + vertex 4.341205e-003 2.462019e-002 7.850000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 -0.000000e+000 + outer loop + vertex 4.341205e-003 2.462019e-002 7.850000e-002 + vertex 4.341205e-003 2.462019e-002 8.200000e-002 + vertex 8.550503e-003 2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex 4.341205e-003 2.462019e-002 7.850000e-002 + vertex 8.550503e-003 2.349232e-002 8.200000e-002 + vertex 8.550503e-003 2.349232e-002 7.850000e-002 + endloop + endfacet + facet normal 4.226183e-001 9.063078e-001 -0.000000e+000 + outer loop + vertex 8.550503e-003 2.349232e-002 7.850000e-002 + vertex 8.550503e-003 2.349232e-002 8.200000e-002 + vertex 1.250000e-002 2.165063e-002 8.200000e-002 + endloop + endfacet + facet normal 4.226183e-001 9.063078e-001 -8.864820e-017 + outer loop + vertex 8.550503e-003 2.349232e-002 7.850000e-002 + vertex 1.250000e-002 2.165063e-002 8.200000e-002 + vertex 1.250000e-002 2.165063e-002 7.850000e-002 + endloop + endfacet + facet normal 5.735766e-001 8.191519e-001 -0.000000e+000 + outer loop + vertex 1.250000e-002 2.165063e-002 7.850000e-002 + vertex 1.250000e-002 2.165063e-002 8.200000e-002 + vertex 1.606969e-002 1.915111e-002 8.200000e-002 + endloop + endfacet + facet normal 5.735766e-001 8.191519e-001 -8.864819e-017 + outer loop + vertex 1.250000e-002 2.165063e-002 7.850000e-002 + vertex 1.606969e-002 1.915111e-002 8.200000e-002 + vertex 1.606969e-002 1.915111e-002 7.850000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 -0.000000e+000 + outer loop + vertex 1.606969e-002 1.915111e-002 7.850000e-002 + vertex 1.606969e-002 1.915111e-002 8.200000e-002 + vertex 1.915111e-002 1.606969e-002 8.200000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex 1.606969e-002 1.915111e-002 7.850000e-002 + vertex 1.915111e-002 1.606969e-002 8.200000e-002 + vertex 1.915111e-002 1.606969e-002 7.850000e-002 + endloop + endfacet + facet normal 8.191519e-001 5.735766e-001 -0.000000e+000 + outer loop + vertex 1.915111e-002 1.606969e-002 7.850000e-002 + vertex 1.915111e-002 1.606969e-002 8.200000e-002 + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal 8.191519e-001 5.735766e-001 8.864819e-017 + outer loop + vertex 1.915111e-002 1.606969e-002 7.850000e-002 + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + vertex 2.165063e-002 1.250000e-002 7.850000e-002 + endloop + endfacet + facet normal 9.063078e-001 4.226183e-001 -0.000000e+000 + outer loop + vertex 2.165063e-002 1.250000e-002 7.850000e-002 + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + vertex 2.349232e-002 8.550503e-003 8.200000e-002 + endloop + endfacet + facet normal 9.063078e-001 4.226183e-001 8.864820e-017 + outer loop + vertex 2.165063e-002 1.250000e-002 7.850000e-002 + vertex 2.349232e-002 8.550503e-003 8.200000e-002 + vertex 2.349232e-002 8.550503e-003 7.850000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588190e-001 -0.000000e+000 + outer loop + vertex 2.349232e-002 8.550503e-003 7.850000e-002 + vertex 2.349232e-002 8.550503e-003 8.200000e-002 + vertex 2.462019e-002 4.341205e-003 8.200000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex 2.349232e-002 8.550503e-003 7.850000e-002 + vertex 2.462019e-002 4.341205e-003 8.200000e-002 + vertex 2.462019e-002 4.341205e-003 7.850000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715579e-002 -0.000000e+000 + outer loop + vertex 2.462019e-002 4.341205e-003 7.850000e-002 + vertex 2.462019e-002 4.341205e-003 8.200000e-002 + vertex 2.500000e-002 0.000000e+000 8.200000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 2.078461e-002 -1.200000e-002 8.300000e-002 + vertex 2.126980e-002 -1.095950e-002 8.300000e-002 + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 2.363539e-002 -4.167556e-003 8.300000e-002 + vertex 2.400000e-002 3.491358e-017 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.279932e-002 9.059701e-003 8.300000e-002 + vertex -1.276514e-002 8.668993e-003 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -5.500000e-003 -9.526280e-003 8.300000e-002 + vertex -6.625000e-003 -8.738546e-003 8.300000e-002 + vertex -6.625000e-003 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.083289e-002 -1.910130e-003 8.300000e-002 + vertex -1.100000e-002 -2.694223e-018 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.095950e-002 8.300000e-002 + vertex -6.625000e-003 -8.738546e-003 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -6.625000e-003 -8.738546e-003 8.300000e-002 + vertex -7.070664e-003 -8.426489e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -7.070664e-003 -8.426489e-003 8.300000e-002 + vertex -8.426489e-003 -7.070664e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -8.426489e-003 -7.070664e-003 8.300000e-002 + vertex -9.526280e-003 -5.500000e-003 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -9.526280e-003 -5.500000e-003 8.300000e-002 + vertex -1.033662e-002 -3.762222e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.033662e-002 -3.762222e-003 8.300000e-002 + vertex -1.083289e-002 -1.910130e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.629575e-004 -1.095950e-002 8.300000e-002 + vertex -1.910130e-003 -1.083289e-002 8.300000e-002 + vertex -6.625000e-003 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.095950e-002 8.300000e-002 + vertex -1.910130e-003 -1.083289e-002 8.300000e-002 + vertex -3.762222e-003 -1.033662e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.095950e-002 8.300000e-002 + vertex -3.762222e-003 -1.033662e-002 8.300000e-002 + vertex -5.500000e-003 -9.526280e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.629575e-004 -1.095950e-002 8.300000e-002 + vertex 3.122502e-017 -1.100000e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 3.122502e-017 -1.100000e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.910130e-003 -1.083289e-002 8.300000e-002 + vertex 6.625000e-003 -1.095950e-002 8.300000e-002 + vertex 3.762222e-003 -1.033662e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 3.762222e-003 -1.033662e-002 8.300000e-002 + vertex 6.625000e-003 -1.095950e-002 8.300000e-002 + vertex 5.500000e-003 -9.526280e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 9.526280e-003 -5.500000e-003 8.300000e-002 + vertex 8.426489e-003 -7.070664e-003 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 8.426489e-003 -7.070664e-003 8.300000e-002 + vertex 7.070664e-003 -8.426489e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 7.070664e-003 -8.426489e-003 8.300000e-002 + vertex 6.625000e-003 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.095950e-002 8.300000e-002 + vertex 7.070664e-003 -8.426489e-003 8.300000e-002 + vertex 6.625000e-003 -8.738546e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.095950e-002 8.300000e-002 + vertex 6.625000e-003 -8.738546e-003 8.300000e-002 + vertex 5.500000e-003 -9.526280e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.083289e-002 -1.910130e-003 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.083289e-002 -1.910130e-003 8.300000e-002 + vertex 1.033662e-002 -3.762222e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.033662e-002 -3.762222e-003 8.300000e-002 + vertex 9.526280e-003 -5.500000e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 8.426489e-003 7.070664e-003 8.300000e-002 + vertex 9.526280e-003 5.500000e-003 8.300000e-002 + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 9.526280e-003 5.500000e-003 8.300000e-002 + vertex 1.033662e-002 3.762222e-003 8.300000e-002 + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.033662e-002 3.762222e-003 8.300000e-002 + vertex 1.083289e-002 1.910130e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.083289e-002 1.910130e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.083289e-002 1.910130e-003 8.300000e-002 + vertex 1.100000e-002 1.600206e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.100000e-002 1.600206e-017 8.300000e-002 + vertex 1.083289e-002 -1.910130e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 3.762222e-003 1.033662e-002 8.300000e-002 + vertex 6.625000e-003 1.095950e-002 8.300000e-002 + vertex 1.910130e-003 1.083289e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.910130e-003 1.083289e-002 8.300000e-002 + vertex 6.625000e-003 1.095950e-002 8.300000e-002 + vertex 4.629575e-004 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 3.762222e-003 1.033662e-002 8.300000e-002 + vertex 5.500000e-003 9.526280e-003 8.300000e-002 + vertex 6.625000e-003 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.095950e-002 8.300000e-002 + vertex 5.500000e-003 9.526280e-003 8.300000e-002 + vertex 6.625000e-003 8.738546e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.095950e-002 8.300000e-002 + vertex 6.625000e-003 8.738546e-003 8.300000e-002 + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 6.625000e-003 8.738546e-003 8.300000e-002 + vertex 7.070664e-003 8.426489e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 7.070664e-003 8.426489e-003 8.300000e-002 + vertex 8.426489e-003 7.070664e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -4.629575e-004 1.095950e-002 8.300000e-002 + vertex 0.000000e+000 1.100000e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex 0.000000e+000 1.100000e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.910130e-003 1.083289e-002 8.300000e-002 + vertex -6.625000e-003 1.095950e-002 8.300000e-002 + vertex -3.762222e-003 1.033662e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -3.762222e-003 1.033662e-002 8.300000e-002 + vertex -6.625000e-003 1.095950e-002 8.300000e-002 + vertex -5.500000e-003 9.526280e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.033662e-002 3.762222e-003 8.300000e-002 + vertex -9.526280e-003 5.500000e-003 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.033662e-002 3.762222e-003 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.083289e-002 1.910130e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -9.526280e-003 5.500000e-003 8.300000e-002 + vertex -8.426489e-003 7.070664e-003 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -8.426489e-003 7.070664e-003 8.300000e-002 + vertex -7.070664e-003 8.426489e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -7.070664e-003 8.426489e-003 8.300000e-002 + vertex -6.625000e-003 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.095950e-002 8.300000e-002 + vertex -7.070664e-003 8.426489e-003 8.300000e-002 + vertex -6.625000e-003 8.738546e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.095950e-002 8.300000e-002 + vertex -6.625000e-003 8.738546e-003 8.300000e-002 + vertex -5.500000e-003 9.526280e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.279932e-002 8.278284e-003 8.300000e-002 + vertex -1.290083e-002 7.899447e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.712945e-002 9.438538e-003 8.300000e-002 + vertex -1.696369e-002 9.793993e-003 8.300000e-002 + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -1.696369e-002 9.793993e-003 8.300000e-002 + vertex -1.673874e-002 1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -1.673874e-002 1.011526e-002 8.300000e-002 + vertex -1.646141e-002 1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.501514e-002 1.091899e-002 8.300000e-002 + vertex -1.462443e-002 1.088481e-002 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.462443e-002 1.088481e-002 8.300000e-002 + vertex -1.424559e-002 1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.100000e-002 -2.694223e-018 8.300000e-002 + vertex -1.083289e-002 1.910130e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.083289e-002 1.910130e-003 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.279932e-002 8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.279932e-002 8.278284e-003 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.276514e-002 8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.290083e-002 7.899447e-003 8.300000e-002 + vertex -1.306658e-002 7.543993e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.306658e-002 7.543993e-003 8.300000e-002 + vertex -1.329154e-002 7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.329154e-002 7.222721e-003 8.300000e-002 + vertex -1.356886e-002 6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.255262e-002 8.208483e-003 8.300000e-002 + vertex -2.363539e-002 4.167556e-003 8.300000e-002 + vertex -1.712945e-002 7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.712945e-002 7.899447e-003 8.300000e-002 + vertex -1.723095e-002 8.278284e-003 8.300000e-002 + vertex -2.255262e-002 8.208483e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.255262e-002 8.208483e-003 8.300000e-002 + vertex -1.723095e-002 8.278284e-003 8.300000e-002 + vertex -1.726514e-002 8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.255262e-002 8.208483e-003 8.300000e-002 + vertex -1.726514e-002 8.668993e-003 8.300000e-002 + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -1.726514e-002 8.668993e-003 8.300000e-002 + vertex -1.723095e-002 9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -1.723095e-002 9.059701e-003 8.300000e-002 + vertex -1.712945e-002 9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.646141e-002 1.039259e-002 8.300000e-002 + vertex -1.614014e-002 1.061755e-002 8.300000e-002 + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -1.614014e-002 1.061755e-002 8.300000e-002 + vertex -1.578468e-002 1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -1.578468e-002 1.078330e-002 8.300000e-002 + vertex -1.540584e-002 1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.424559e-002 1.078330e-002 8.300000e-002 + vertex -1.389014e-002 1.061755e-002 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.389014e-002 1.061755e-002 8.300000e-002 + vertex -1.356886e-002 1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.356886e-002 1.039259e-002 8.300000e-002 + vertex -1.329154e-002 1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.329154e-002 1.011526e-002 8.300000e-002 + vertex -1.306658e-002 9.793993e-003 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.306658e-002 9.793993e-003 8.300000e-002 + vertex -1.290083e-002 9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.290083e-002 9.438538e-003 8.300000e-002 + vertex -1.279932e-002 9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.356886e-002 6.945393e-003 8.300000e-002 + vertex -1.389014e-002 6.720436e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.389014e-002 6.720436e-003 8.300000e-002 + vertex -1.424559e-002 6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.424559e-002 6.554685e-003 8.300000e-002 + vertex -1.462443e-002 6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.614014e-002 6.720436e-003 8.300000e-002 + vertex -1.646141e-002 6.945393e-003 8.300000e-002 + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -1.646141e-002 6.945393e-003 8.300000e-002 + vertex -1.673874e-002 7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -1.673874e-002 7.222721e-003 8.300000e-002 + vertex -2.363539e-002 4.167556e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.363539e-002 4.167556e-003 8.300000e-002 + vertex -1.673874e-002 7.222721e-003 8.300000e-002 + vertex -1.696369e-002 7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.363539e-002 4.167556e-003 8.300000e-002 + vertex -1.696369e-002 7.543993e-003 8.300000e-002 + vertex -1.712945e-002 7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.462443e-002 6.453176e-003 8.300000e-002 + vertex -1.501514e-002 6.418993e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.501514e-002 6.418993e-003 8.300000e-002 + vertex -1.540584e-002 6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.540584e-002 6.453176e-003 8.300000e-002 + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -1.540584e-002 6.453176e-003 8.300000e-002 + vertex -1.578468e-002 6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -1.578468e-002 6.554685e-003 8.300000e-002 + vertex -1.614014e-002 6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.540584e-002 1.088481e-002 8.300000e-002 + vertex -1.501514e-002 1.091899e-002 8.300000e-002 + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -1.501514e-002 1.091899e-002 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -2.078461e-002 1.200000e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.078461e-002 1.200000e-002 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -2.005360e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 2.083937e-002 8.300000e-002 + vertex -1.200000e-002 2.078461e-002 8.300000e-002 + vertex -1.188257e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.304399e-002 8.300000e-002 + vertex -1.200000e-002 2.078461e-002 8.300000e-002 + vertex -1.542690e-002 1.838507e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.304399e-002 8.300000e-002 + vertex -1.542690e-002 1.838507e-002 8.300000e-002 + vertex -1.838507e-002 1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -2.215817e-003 1.772869e-002 8.300000e-002 + vertex -2.114308e-003 1.810753e-002 8.300000e-002 + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.215817e-003 1.772869e-002 8.300000e-002 + vertex 2.250000e-003 1.733799e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 2.250000e-003 1.733799e-002 8.300000e-002 + vertex 2.215817e-003 1.694728e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.215817e-003 1.694728e-002 8.300000e-002 + vertex 2.114308e-003 1.656844e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 2.114308e-003 1.656844e-002 8.300000e-002 + vertex 1.948557e-003 1.621299e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 1.948557e-003 1.621299e-002 8.300000e-002 + vertex 1.723600e-003 1.589171e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.446272e-003 1.906159e-002 8.300000e-002 + vertex -4.167556e-003 2.363539e-002 8.300000e-002 + vertex -1.723600e-003 1.878426e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.723600e-003 1.878426e-002 8.300000e-002 + vertex -4.167556e-003 2.363539e-002 8.300000e-002 + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.723600e-003 1.878426e-002 8.300000e-002 + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + vertex -1.948557e-003 1.846299e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.948557e-003 1.846299e-002 8.300000e-002 + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + vertex -2.114308e-003 1.810753e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.446272e-003 1.906159e-002 8.300000e-002 + vertex -1.125000e-003 1.928654e-002 8.300000e-002 + vertex -4.167556e-003 2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.167556e-003 2.363539e-002 8.300000e-002 + vertex -1.125000e-003 1.928654e-002 8.300000e-002 + vertex -7.695453e-004 1.945229e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.167556e-003 2.363539e-002 8.300000e-002 + vertex -7.695453e-004 1.945229e-002 8.300000e-002 + vertex 0.000000e+000 2.400000e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 0.000000e+000 2.400000e-002 8.300000e-002 + vertex -7.695453e-004 1.945229e-002 8.300000e-002 + vertex -3.907084e-004 1.955380e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + vertex 1.948557e-003 1.846299e-002 8.300000e-002 + vertex 2.114308e-003 1.810753e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 7.695453e-004 1.945229e-002 8.300000e-002 + vertex 4.167556e-003 2.363539e-002 8.300000e-002 + vertex 3.907084e-004 1.955380e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 3.907084e-004 1.955380e-002 8.300000e-002 + vertex 4.167556e-003 2.363539e-002 8.300000e-002 + vertex 0.000000e+000 2.400000e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 3.907084e-004 1.955380e-002 8.300000e-002 + vertex 0.000000e+000 2.400000e-002 8.300000e-002 + vertex 0.000000e+000 1.958799e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 0.000000e+000 1.958799e-002 8.300000e-002 + vertex 0.000000e+000 2.400000e-002 8.300000e-002 + vertex -3.907084e-004 1.955380e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 7.695453e-004 1.945229e-002 8.300000e-002 + vertex 1.125000e-003 1.928654e-002 8.300000e-002 + vertex 4.167556e-003 2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.167556e-003 2.363539e-002 8.300000e-002 + vertex 1.125000e-003 1.928654e-002 8.300000e-002 + vertex 1.446272e-003 1.906159e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.167556e-003 2.363539e-002 8.300000e-002 + vertex 1.446272e-003 1.906159e-002 8.300000e-002 + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + vertex 1.446272e-003 1.906159e-002 8.300000e-002 + vertex 1.723600e-003 1.878426e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + vertex 1.723600e-003 1.878426e-002 8.300000e-002 + vertex 1.948557e-003 1.846299e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.723600e-003 1.589171e-002 8.300000e-002 + vertex 1.446272e-003 1.561439e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 1.446272e-003 1.561439e-002 8.300000e-002 + vertex 1.125000e-003 1.538943e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 1.125000e-003 1.538943e-002 8.300000e-002 + vertex 7.695453e-004 1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -7.695453e-004 1.522368e-002 8.300000e-002 + vertex -1.125000e-003 1.538943e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.125000e-003 1.538943e-002 8.300000e-002 + vertex -1.446272e-003 1.561439e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -1.446272e-003 1.561439e-002 8.300000e-002 + vertex -1.723600e-003 1.589171e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -1.723600e-003 1.589171e-002 8.300000e-002 + vertex -1.948557e-003 1.621299e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.948557e-003 1.621299e-002 8.300000e-002 + vertex -2.114308e-003 1.656844e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -2.114308e-003 1.656844e-002 8.300000e-002 + vertex -2.215817e-003 1.694728e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -2.215817e-003 1.694728e-002 8.300000e-002 + vertex -2.250000e-003 1.733799e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 2.083937e-002 8.300000e-002 + vertex -1.188257e-002 1.304399e-002 8.300000e-002 + vertex -8.208483e-003 2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -8.208483e-003 2.255262e-002 8.300000e-002 + vertex -1.188257e-002 1.304399e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -8.208483e-003 2.255262e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -2.250000e-003 1.733799e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + vertex -2.250000e-003 1.733799e-002 8.300000e-002 + vertex -2.215817e-003 1.772869e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 7.695453e-004 1.522368e-002 8.300000e-002 + vertex 3.907084e-004 1.512217e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 3.907084e-004 1.512217e-002 8.300000e-002 + vertex 0.000000e+000 1.508799e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 0.000000e+000 1.508799e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex 0.000000e+000 1.508799e-002 8.300000e-002 + vertex -3.907084e-004 1.512217e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -3.907084e-004 1.512217e-002 8.300000e-002 + vertex -7.695453e-004 1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.114308e-003 1.810753e-002 8.300000e-002 + vertex 2.215817e-003 1.772869e-002 8.300000e-002 + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + vertex 2.215817e-003 1.772869e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 8.208483e-003 2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 8.208483e-003 2.255262e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.221714e-002 8.300000e-002 + vertex 2.005360e-002 1.304399e-002 8.300000e-002 + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + vertex 2.005360e-002 1.304399e-002 8.300000e-002 + vertex 1.838507e-002 1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + vertex 1.838507e-002 1.542690e-002 8.300000e-002 + vertex 1.542690e-002 1.838507e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.542690e-002 1.838507e-002 8.300000e-002 + vertex 1.200000e-002 2.078461e-002 8.300000e-002 + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + vertex 1.200000e-002 2.078461e-002 8.300000e-002 + vertex 1.188257e-002 2.083937e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + vertex 1.188257e-002 2.083937e-002 8.300000e-002 + vertex 8.208483e-003 2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 8.300000e-002 + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 2.255262e-002 8.208483e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.255262e-002 8.208483e-003 8.300000e-002 + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.255262e-002 8.208483e-003 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 2.363539e-002 4.167556e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.363539e-002 4.167556e-003 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 2.400000e-002 3.491358e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 2.126980e-002 -1.095950e-002 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 2.126980e-002 -1.095950e-002 8.300000e-002 + vertex 2.255262e-002 -8.208483e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 2.255262e-002 -8.208483e-003 8.300000e-002 + vertex 2.363539e-002 -4.167556e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.078461e-002 -1.200000e-002 8.300000e-002 + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 2.063257e-002 -1.221714e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.221714e-002 8.300000e-002 + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.221714e-002 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 2.005360e-002 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -2.083937e-002 8.300000e-002 + vertex 1.200000e-002 -2.078461e-002 8.300000e-002 + vertex 1.188257e-002 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 8.300000e-002 + vertex 1.200000e-002 -2.078461e-002 8.300000e-002 + vertex 1.542690e-002 -1.838507e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 8.300000e-002 + vertex 1.542690e-002 -1.838507e-002 8.300000e-002 + vertex 1.838507e-002 -1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -2.083937e-002 8.300000e-002 + vertex 1.188257e-002 -1.304399e-002 8.300000e-002 + vertex 8.208483e-003 -2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 8.208483e-003 -2.255262e-002 8.300000e-002 + vertex 1.188257e-002 -1.304399e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 8.208483e-003 -2.255262e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.125000e-003 -1.928654e-002 8.300000e-002 + vertex -1.446272e-003 -1.906159e-002 8.300000e-002 + vertex -4.167556e-003 -2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -4.167556e-003 -2.363539e-002 8.300000e-002 + vertex -1.446272e-003 -1.906159e-002 8.300000e-002 + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.723600e-003 -1.589171e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -1.948557e-003 -1.621299e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.948557e-003 -1.621299e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -2.114308e-003 -1.656844e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.446272e-003 -1.561439e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 1.125000e-003 -1.538943e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.125000e-003 -1.538943e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 7.695453e-004 -1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.114308e-003 -1.810753e-002 8.300000e-002 + vertex 1.948557e-003 -1.846299e-002 8.300000e-002 + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + vertex 1.948557e-003 -1.846299e-002 8.300000e-002 + vertex 1.723600e-003 -1.878426e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + vertex 1.723600e-003 -1.878426e-002 8.300000e-002 + vertex 4.167556e-003 -2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.167556e-003 -2.363539e-002 8.300000e-002 + vertex 1.723600e-003 -1.878426e-002 8.300000e-002 + vertex 1.446272e-003 -1.906159e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.446272e-003 -1.906159e-002 8.300000e-002 + vertex -1.723600e-003 -1.878426e-002 8.300000e-002 + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + vertex -1.723600e-003 -1.878426e-002 8.300000e-002 + vertex -1.948557e-003 -1.846299e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + vertex -1.948557e-003 -1.846299e-002 8.300000e-002 + vertex -2.114308e-003 -1.810753e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.446272e-003 -1.561439e-002 8.300000e-002 + vertex 1.723600e-003 -1.589171e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 1.723600e-003 -1.589171e-002 8.300000e-002 + vertex 1.948557e-003 -1.621299e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 1.948557e-003 -1.621299e-002 8.300000e-002 + vertex 2.114308e-003 -1.656844e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.114308e-003 -1.656844e-002 8.300000e-002 + vertex 2.215817e-003 -1.694728e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 2.215817e-003 -1.694728e-002 8.300000e-002 + vertex 2.250000e-003 -1.733799e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 2.250000e-003 -1.733799e-002 8.300000e-002 + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + vertex 2.250000e-003 -1.733799e-002 8.300000e-002 + vertex 2.215817e-003 -1.772869e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + vertex 2.215817e-003 -1.772869e-002 8.300000e-002 + vertex 2.114308e-003 -1.810753e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 3.907084e-004 -1.955380e-002 8.300000e-002 + vertex 6.938894e-017 -2.400000e-002 8.300000e-002 + vertex 7.695453e-004 -1.945229e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 7.695453e-004 -1.945229e-002 8.300000e-002 + vertex 6.938894e-017 -2.400000e-002 8.300000e-002 + vertex 4.167556e-003 -2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 7.695453e-004 -1.945229e-002 8.300000e-002 + vertex 4.167556e-003 -2.363539e-002 8.300000e-002 + vertex 1.125000e-003 -1.928654e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.125000e-003 -1.928654e-002 8.300000e-002 + vertex 4.167556e-003 -2.363539e-002 8.300000e-002 + vertex 1.446272e-003 -1.906159e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 3.907084e-004 -1.955380e-002 8.300000e-002 + vertex 0.000000e+000 -1.958799e-002 8.300000e-002 + vertex 6.938894e-017 -2.400000e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.938894e-017 -2.400000e-002 8.300000e-002 + vertex 0.000000e+000 -1.958799e-002 8.300000e-002 + vertex -3.907084e-004 -1.955380e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.938894e-017 -2.400000e-002 8.300000e-002 + vertex -3.907084e-004 -1.955380e-002 8.300000e-002 + vertex -4.167556e-003 -2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.167556e-003 -2.363539e-002 8.300000e-002 + vertex -3.907084e-004 -1.955380e-002 8.300000e-002 + vertex -7.695453e-004 -1.945229e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.167556e-003 -2.363539e-002 8.300000e-002 + vertex -7.695453e-004 -1.945229e-002 8.300000e-002 + vertex -1.125000e-003 -1.928654e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.215817e-003 -1.772869e-002 8.300000e-002 + vertex -2.250000e-003 -1.733799e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -2.250000e-003 -1.733799e-002 8.300000e-002 + vertex -2.215817e-003 -1.694728e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -2.215817e-003 -1.694728e-002 8.300000e-002 + vertex -2.114308e-003 -1.656844e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.723600e-003 -1.589171e-002 8.300000e-002 + vertex -1.446272e-003 -1.561439e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -1.446272e-003 -1.561439e-002 8.300000e-002 + vertex -1.125000e-003 -1.538943e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -1.125000e-003 -1.538943e-002 8.300000e-002 + vertex -7.695453e-004 -1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -7.695453e-004 -1.522368e-002 8.300000e-002 + vertex -3.907084e-004 -1.512217e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -3.907084e-004 -1.512217e-002 8.300000e-002 + vertex 0.000000e+000 -1.508799e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 0.000000e+000 -1.508799e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 0.000000e+000 -1.508799e-002 8.300000e-002 + vertex 3.907084e-004 -1.512217e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 3.907084e-004 -1.512217e-002 8.300000e-002 + vertex 7.695453e-004 -1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.114308e-003 -1.810753e-002 8.300000e-002 + vertex -2.215817e-003 -1.772869e-002 8.300000e-002 + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + vertex -2.215817e-003 -1.772869e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -8.208483e-003 -2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -8.208483e-003 -2.255262e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -8.208483e-003 -2.255262e-002 8.300000e-002 + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + vertex -1.188257e-002 -2.083937e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.078461e-002 -1.200000e-002 8.300000e-002 + vertex -2.005360e-002 -1.304399e-002 8.300000e-002 + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + vertex -2.005360e-002 -1.304399e-002 8.300000e-002 + vertex -1.838507e-002 -1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.838507e-002 -1.542690e-002 8.300000e-002 + vertex -1.542690e-002 -1.838507e-002 8.300000e-002 + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + vertex -1.542690e-002 -1.838507e-002 8.300000e-002 + vertex -1.200000e-002 -2.078461e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + vertex -1.200000e-002 -2.078461e-002 8.300000e-002 + vertex -1.188257e-002 -2.083937e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.363539e-002 -4.167556e-003 8.300000e-002 + vertex -2.255262e-002 -8.208483e-003 8.300000e-002 + vertex -1.723095e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -1.578468e-002 -1.078330e-002 8.300000e-002 + vertex -1.614014e-002 -1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.276514e-002 -8.668993e-003 8.300000e-002 + vertex -1.279932e-002 -9.059701e-003 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.279932e-002 -9.059701e-003 8.300000e-002 + vertex -1.290083e-002 -9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.290083e-002 -9.438538e-003 8.300000e-002 + vertex -1.306658e-002 -9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.389014e-002 -1.061755e-002 8.300000e-002 + vertex -1.424559e-002 -1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.614014e-002 -1.061755e-002 8.300000e-002 + vertex -1.646141e-002 -1.039259e-002 8.300000e-002 + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -1.646141e-002 -1.039259e-002 8.300000e-002 + vertex -1.673874e-002 -1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -1.673874e-002 -1.011526e-002 8.300000e-002 + vertex -1.696369e-002 -9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.723095e-002 -8.278284e-003 8.300000e-002 + vertex -1.712945e-002 -7.899447e-003 8.300000e-002 + vertex -2.363539e-002 -4.167556e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.363539e-002 -4.167556e-003 8.300000e-002 + vertex -1.712945e-002 -7.899447e-003 8.300000e-002 + vertex -1.696369e-002 -7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -2.363539e-002 -4.167556e-003 8.300000e-002 + vertex -1.696369e-002 -7.543993e-003 8.300000e-002 + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -1.696369e-002 -7.543993e-003 8.300000e-002 + vertex -1.673874e-002 -7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -1.673874e-002 -7.222721e-003 8.300000e-002 + vertex -1.646141e-002 -6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.646141e-002 -6.945393e-003 8.300000e-002 + vertex -1.614014e-002 -6.720436e-003 8.300000e-002 + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -1.614014e-002 -6.720436e-003 8.300000e-002 + vertex -1.578468e-002 -6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -1.578468e-002 -6.554685e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.578468e-002 -6.554685e-003 8.300000e-002 + vertex -1.540584e-002 -6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.540584e-002 -6.453176e-003 8.300000e-002 + vertex -1.501514e-002 -6.418993e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.501514e-002 -6.418993e-003 8.300000e-002 + vertex -1.462443e-002 -6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.462443e-002 -6.453176e-003 8.300000e-002 + vertex -1.424559e-002 -6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.306658e-002 -9.793993e-003 8.300000e-002 + vertex -1.329154e-002 -1.011526e-002 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.329154e-002 -1.011526e-002 8.300000e-002 + vertex -1.356886e-002 -1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.356886e-002 -1.039259e-002 8.300000e-002 + vertex -1.389014e-002 -1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.424559e-002 -1.078330e-002 8.300000e-002 + vertex -1.462443e-002 -1.088481e-002 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.462443e-002 -1.088481e-002 8.300000e-002 + vertex -1.501514e-002 -1.091899e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.501514e-002 -1.091899e-002 8.300000e-002 + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -1.501514e-002 -1.091899e-002 8.300000e-002 + vertex -1.540584e-002 -1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -1.540584e-002 -1.088481e-002 8.300000e-002 + vertex -1.578468e-002 -1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.696369e-002 -9.793993e-003 8.300000e-002 + vertex -1.712945e-002 -9.438538e-003 8.300000e-002 + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -1.712945e-002 -9.438538e-003 8.300000e-002 + vertex -1.723095e-002 -9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -1.723095e-002 -9.059701e-003 8.300000e-002 + vertex -2.255262e-002 -8.208483e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.255262e-002 -8.208483e-003 8.300000e-002 + vertex -1.723095e-002 -9.059701e-003 8.300000e-002 + vertex -1.726514e-002 -8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.255262e-002 -8.208483e-003 8.300000e-002 + vertex -1.726514e-002 -8.668993e-003 8.300000e-002 + vertex -1.723095e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.424559e-002 -6.554685e-003 8.300000e-002 + vertex -1.389014e-002 -6.720436e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.389014e-002 -6.720436e-003 8.300000e-002 + vertex -1.356886e-002 -6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.356886e-002 -6.945393e-003 8.300000e-002 + vertex -1.329154e-002 -7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.083289e-002 -1.910130e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.279932e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -1.279932e-002 -8.278284e-003 8.300000e-002 + vertex -1.276514e-002 -8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.329154e-002 -7.222721e-003 8.300000e-002 + vertex -1.306658e-002 -7.543993e-003 8.300000e-002 + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.306658e-002 -7.543993e-003 8.300000e-002 + vertex -1.290083e-002 -7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 5.117434e-017 8.300000e-002 + vertex -1.290083e-002 -7.899447e-003 8.300000e-002 + vertex -1.279932e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 3.122502e-017 -1.100000e-002 8.300000e-002 + vertex -4.629575e-004 -1.095950e-002 8.300000e-002 + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -4.629575e-004 -1.095950e-002 8.300000e-002 + vertex -6.625000e-003 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 -1.304399e-002 8.300000e-002 + vertex -6.625000e-003 -1.095950e-002 8.300000e-002 + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + vertex -6.625000e-003 -1.095950e-002 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 -1.304399e-002 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -2.078461e-002 -1.200000e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -2.078461e-002 -1.200000e-002 8.300000e-002 + vertex -1.188257e-002 -1.095950e-002 8.300000e-002 + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.838507e-002 -1.542690e-002 8.300000e-002 + vertex 2.005360e-002 -1.304399e-002 8.300000e-002 + vertex 1.188257e-002 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 8.300000e-002 + vertex 2.005360e-002 -1.304399e-002 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.304399e-002 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 6.625000e-003 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 -1.304399e-002 8.300000e-002 + vertex 6.625000e-003 -1.095950e-002 8.300000e-002 + vertex 4.629575e-004 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.629575e-004 -1.095950e-002 8.300000e-002 + vertex 6.625000e-003 -1.095950e-002 8.300000e-002 + vertex 1.910130e-003 -1.083289e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.306658e-002 -7.543993e-003 8.300000e-002 + vertex 1.329154e-002 -7.222721e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.696369e-002 -9.793993e-003 8.300000e-002 + vertex 1.673874e-002 -1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.726514e-002 -8.668993e-003 8.300000e-002 + vertex 1.723095e-002 -9.059701e-003 8.300000e-002 + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.723095e-002 -9.059701e-003 8.300000e-002 + vertex 1.712945e-002 -9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.712945e-002 -9.438538e-003 8.300000e-002 + vertex 1.696369e-002 -9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.673874e-002 -1.011526e-002 8.300000e-002 + vertex 1.646141e-002 -1.039259e-002 8.300000e-002 + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.646141e-002 -1.039259e-002 8.300000e-002 + vertex 1.614014e-002 -1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.614014e-002 -1.061755e-002 8.300000e-002 + vertex 1.578468e-002 -1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.673874e-002 -7.222721e-003 8.300000e-002 + vertex 1.696369e-002 -7.543993e-003 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.696369e-002 -7.543993e-003 8.300000e-002 + vertex 1.712945e-002 -7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.712945e-002 -7.899447e-003 8.300000e-002 + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.712945e-002 -7.899447e-003 8.300000e-002 + vertex 1.723095e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.723095e-002 -8.278284e-003 8.300000e-002 + vertex 1.726514e-002 -8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.578468e-002 -1.078330e-002 8.300000e-002 + vertex 1.540584e-002 -1.088481e-002 8.300000e-002 + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.540584e-002 -1.088481e-002 8.300000e-002 + vertex 1.501514e-002 -1.091899e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 -1.095950e-002 8.300000e-002 + vertex 1.501514e-002 -1.091899e-002 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.501514e-002 -1.091899e-002 8.300000e-002 + vertex 1.462443e-002 -1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.462443e-002 -1.088481e-002 8.300000e-002 + vertex 1.424559e-002 -1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.424559e-002 -1.078330e-002 8.300000e-002 + vertex 1.389014e-002 -1.061755e-002 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.389014e-002 -1.061755e-002 8.300000e-002 + vertex 1.356886e-002 -1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.356886e-002 -1.039259e-002 8.300000e-002 + vertex 1.329154e-002 -1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.329154e-002 -1.011526e-002 8.300000e-002 + vertex 1.306658e-002 -9.793993e-003 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.306658e-002 -9.793993e-003 8.300000e-002 + vertex 1.290083e-002 -9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.290083e-002 -9.438538e-003 8.300000e-002 + vertex 1.279932e-002 -9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.279932e-002 -9.059701e-003 8.300000e-002 + vertex 1.276514e-002 -8.668993e-003 8.300000e-002 + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.276514e-002 -8.668993e-003 8.300000e-002 + vertex 1.279932e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 -1.095950e-002 8.300000e-002 + vertex 1.279932e-002 -8.278284e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.279932e-002 -8.278284e-003 8.300000e-002 + vertex 1.290083e-002 -7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.290083e-002 -7.899447e-003 8.300000e-002 + vertex 1.306658e-002 -7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.329154e-002 -7.222721e-003 8.300000e-002 + vertex 1.356886e-002 -6.945393e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.356886e-002 -6.945393e-003 8.300000e-002 + vertex 1.389014e-002 -6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.389014e-002 -6.720436e-003 8.300000e-002 + vertex 1.424559e-002 -6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.424559e-002 -6.554685e-003 8.300000e-002 + vertex 1.462443e-002 -6.453176e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.462443e-002 -6.453176e-003 8.300000e-002 + vertex 1.501514e-002 -6.418993e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.501514e-002 -6.418993e-003 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.501514e-002 -6.418993e-003 8.300000e-002 + vertex 1.540584e-002 -6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.540584e-002 -6.453176e-003 8.300000e-002 + vertex 1.578468e-002 -6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.578468e-002 -6.554685e-003 8.300000e-002 + vertex 1.614014e-002 -6.720436e-003 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.614014e-002 -6.720436e-003 8.300000e-002 + vertex 1.646141e-002 -6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.646141e-002 -6.945393e-003 8.300000e-002 + vertex 1.673874e-002 -7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.696369e-002 9.793993e-003 8.300000e-002 + vertex 1.712945e-002 9.438538e-003 8.300000e-002 + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 1.712945e-002 9.438538e-003 8.300000e-002 + vertex 1.723095e-002 9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.578468e-002 1.078330e-002 8.300000e-002 + vertex 1.614014e-002 1.061755e-002 8.300000e-002 + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.696369e-002 7.543993e-003 8.300000e-002 + vertex 1.673874e-002 7.222721e-003 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.673874e-002 7.222721e-003 8.300000e-002 + vertex 1.646141e-002 6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.646141e-002 6.945393e-003 8.300000e-002 + vertex 1.614014e-002 6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.279932e-002 9.059701e-003 8.300000e-002 + vertex 1.290083e-002 9.438538e-003 8.300000e-002 + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.290083e-002 9.438538e-003 8.300000e-002 + vertex 1.306658e-002 9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.306658e-002 9.793993e-003 8.300000e-002 + vertex 1.329154e-002 1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.723095e-002 9.059701e-003 8.300000e-002 + vertex 1.726514e-002 8.668993e-003 8.300000e-002 + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 1.726514e-002 8.668993e-003 8.300000e-002 + vertex 1.723095e-002 8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 1.723095e-002 8.278284e-003 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.723095e-002 8.278284e-003 8.300000e-002 + vertex 1.712945e-002 7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.712945e-002 7.899447e-003 8.300000e-002 + vertex 1.696369e-002 7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.329154e-002 1.011526e-002 8.300000e-002 + vertex 1.356886e-002 1.039259e-002 8.300000e-002 + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.356886e-002 1.039259e-002 8.300000e-002 + vertex 1.389014e-002 1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.389014e-002 1.061755e-002 8.300000e-002 + vertex 1.424559e-002 1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.329154e-002 7.222721e-003 8.300000e-002 + vertex 1.306658e-002 7.543993e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.424559e-002 1.078330e-002 8.300000e-002 + vertex 1.462443e-002 1.088481e-002 8.300000e-002 + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.462443e-002 1.088481e-002 8.300000e-002 + vertex 1.501514e-002 1.091899e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.501514e-002 1.091899e-002 8.300000e-002 + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 1.501514e-002 1.091899e-002 8.300000e-002 + vertex 1.540584e-002 1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 1.540584e-002 1.088481e-002 8.300000e-002 + vertex 1.578468e-002 1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.614014e-002 1.061755e-002 8.300000e-002 + vertex 1.646141e-002 1.039259e-002 8.300000e-002 + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 1.646141e-002 1.039259e-002 8.300000e-002 + vertex 1.673874e-002 1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 1.673874e-002 1.011526e-002 8.300000e-002 + vertex 1.696369e-002 9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.462443e-002 6.453176e-003 8.300000e-002 + vertex 1.424559e-002 6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.306658e-002 7.543993e-003 8.300000e-002 + vertex 1.290083e-002 7.899447e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.290083e-002 7.899447e-003 8.300000e-002 + vertex 1.279932e-002 8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.279932e-002 8.278284e-003 8.300000e-002 + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.279932e-002 8.278284e-003 8.300000e-002 + vertex 1.276514e-002 8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.276514e-002 8.668993e-003 8.300000e-002 + vertex 1.279932e-002 9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.614014e-002 6.720436e-003 8.300000e-002 + vertex 1.578468e-002 6.554685e-003 8.300000e-002 + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.578468e-002 6.554685e-003 8.300000e-002 + vertex 1.540584e-002 6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 5.117434e-017 8.300000e-002 + vertex 1.540584e-002 6.453176e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.540584e-002 6.453176e-003 8.300000e-002 + vertex 1.501514e-002 6.418993e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.501514e-002 6.418993e-003 8.300000e-002 + vertex 1.462443e-002 6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.424559e-002 6.554685e-003 8.300000e-002 + vertex 1.389014e-002 6.720436e-003 8.300000e-002 + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.389014e-002 6.720436e-003 8.300000e-002 + vertex 1.356886e-002 6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 5.117434e-017 8.300000e-002 + vertex 1.356886e-002 6.945393e-003 8.300000e-002 + vertex 1.329154e-002 7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.910130e-003 1.083289e-002 8.300000e-002 + vertex -4.629575e-004 1.095950e-002 8.300000e-002 + vertex -6.625000e-003 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.095950e-002 8.300000e-002 + vertex -4.629575e-004 1.095950e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -6.625000e-003 1.095950e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -6.625000e-003 1.304399e-002 8.300000e-002 + vertex -1.188257e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex -1.188257e-002 1.095950e-002 8.300000e-002 + vertex -1.188257e-002 1.304399e-002 8.300000e-002 + vertex -2.005360e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -2.005360e-002 1.304399e-002 8.300000e-002 + vertex -1.188257e-002 1.304399e-002 8.300000e-002 + vertex -1.838507e-002 1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.126980e-002 1.095950e-002 8.300000e-002 + vertex 2.078461e-002 1.200000e-002 8.300000e-002 + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 2.078461e-002 1.200000e-002 8.300000e-002 + vertex 2.063257e-002 1.221714e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 2.063257e-002 1.095950e-002 8.300000e-002 + vertex 2.063257e-002 1.221714e-002 8.300000e-002 + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 2.063257e-002 1.221714e-002 8.300000e-002 + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 1.188257e-002 1.095950e-002 8.300000e-002 + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + vertex 6.625000e-003 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.095950e-002 8.300000e-002 + vertex 1.188257e-002 1.304399e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 6.625000e-003 1.095950e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 4.629575e-004 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 1.000000e+000 + outer loop + vertex 4.629575e-004 1.095950e-002 8.300000e-002 + vertex 6.625000e-003 1.304399e-002 8.300000e-002 + vertex 0.000000e+000 1.100000e-002 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715547e-002 0.000000e+000 + outer loop + vertex 9.848078e-003 1.736482e-003 8.400000e-002 + vertex 1.000000e-002 0.000000e+000 1.220000e-001 + vertex 1.000000e-002 0.000000e+000 8.400000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715547e-002 0.000000e+000 + outer loop + vertex 1.000000e-002 0.000000e+000 8.400000e-002 + vertex 1.000000e-002 0.000000e+000 1.220000e-001 + vertex 9.848078e-003 -1.736482e-003 1.220000e-001 + endloop + endfacet + facet normal 9.961947e-001 -8.715547e-002 0.000000e+000 + outer loop + vertex 1.000000e-002 0.000000e+000 8.400000e-002 + vertex 9.848078e-003 -1.736482e-003 1.220000e-001 + vertex 9.848078e-003 -1.736482e-003 8.400000e-002 + endloop + endfacet + facet normal 9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex 9.848078e-003 -1.736482e-003 8.400000e-002 + vertex 9.848078e-003 -1.736482e-003 1.220000e-001 + vertex 9.396926e-003 -3.420201e-003 1.220000e-001 + endloop + endfacet + facet normal 9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex 9.848078e-003 -1.736482e-003 8.400000e-002 + vertex 9.396926e-003 -3.420201e-003 1.220000e-001 + vertex 9.396926e-003 -3.420201e-003 8.400000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex 9.396926e-003 -3.420201e-003 8.400000e-002 + vertex 9.396926e-003 -3.420201e-003 1.220000e-001 + vertex 8.660254e-003 -5.000000e-003 1.220000e-001 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 -3.473095e-018 + outer loop + vertex 9.396926e-003 -3.420201e-003 8.400000e-002 + vertex 8.660254e-003 -5.000000e-003 1.220000e-001 + vertex 8.660254e-003 -5.000000e-003 8.400000e-002 + endloop + endfacet + facet normal 8.191521e-001 -5.735765e-001 0.000000e+000 + outer loop + vertex 8.660254e-003 -5.000000e-003 8.400000e-002 + vertex 8.660254e-003 -5.000000e-003 1.220000e-001 + vertex 7.660444e-003 -6.427876e-003 1.220000e-001 + endloop + endfacet + facet normal 8.191521e-001 -5.735765e-001 0.000000e+000 + outer loop + vertex 8.660254e-003 -5.000000e-003 8.400000e-002 + vertex 7.660444e-003 -6.427876e-003 1.220000e-001 + vertex 7.660444e-003 -6.427876e-003 8.400000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 7.660444e-003 -6.427876e-003 8.400000e-002 + vertex 7.660444e-003 -6.427876e-003 1.220000e-001 + vertex 6.427876e-003 -7.660444e-003 1.220000e-001 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 7.660444e-003 -6.427876e-003 8.400000e-002 + vertex 6.427876e-003 -7.660444e-003 1.220000e-001 + vertex 6.427876e-003 -7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal 5.735765e-001 -8.191521e-001 0.000000e+000 + outer loop + vertex 6.427876e-003 -7.660444e-003 8.400000e-002 + vertex 6.427876e-003 -7.660444e-003 1.220000e-001 + vertex 5.000000e-003 -8.660254e-003 1.220000e-001 + endloop + endfacet + facet normal 5.735765e-001 -8.191521e-001 0.000000e+000 + outer loop + vertex 6.427876e-003 -7.660444e-003 8.400000e-002 + vertex 5.000000e-003 -8.660254e-003 1.220000e-001 + vertex 5.000000e-003 -8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal 4.226184e-001 -9.063077e-001 0.000000e+000 + outer loop + vertex 5.000000e-003 -8.660254e-003 8.400000e-002 + vertex 5.000000e-003 -8.660254e-003 1.220000e-001 + vertex 3.420201e-003 -9.396926e-003 1.220000e-001 + endloop + endfacet + facet normal 4.226184e-001 -9.063077e-001 3.473095e-018 + outer loop + vertex 5.000000e-003 -8.660254e-003 8.400000e-002 + vertex 3.420201e-003 -9.396926e-003 1.220000e-001 + vertex 3.420201e-003 -9.396926e-003 8.400000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 3.420201e-003 -9.396926e-003 8.400000e-002 + vertex 3.420201e-003 -9.396926e-003 1.220000e-001 + vertex 1.736482e-003 -9.848078e-003 1.220000e-001 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 3.420201e-003 -9.396926e-003 8.400000e-002 + vertex 1.736482e-003 -9.848078e-003 1.220000e-001 + vertex 1.736482e-003 -9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal 8.715547e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.736482e-003 -9.848078e-003 8.400000e-002 + vertex 1.736482e-003 -9.848078e-003 1.220000e-001 + vertex -1.836970e-018 -1.000000e-002 1.220000e-001 + endloop + endfacet + facet normal 8.715547e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.736482e-003 -9.848078e-003 8.400000e-002 + vertex -1.836970e-018 -1.000000e-002 1.220000e-001 + vertex -1.836970e-018 -1.000000e-002 8.400000e-002 + endloop + endfacet + facet normal -8.715547e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.836970e-018 -1.000000e-002 8.400000e-002 + vertex -1.836970e-018 -1.000000e-002 1.220000e-001 + vertex -1.736482e-003 -9.848078e-003 1.220000e-001 + endloop + endfacet + facet normal -8.715547e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.836970e-018 -1.000000e-002 8.400000e-002 + vertex -1.736482e-003 -9.848078e-003 1.220000e-001 + vertex -1.736482e-003 -9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -1.736482e-003 -9.848078e-003 8.400000e-002 + vertex -1.736482e-003 -9.848078e-003 1.220000e-001 + vertex -3.420201e-003 -9.396926e-003 1.220000e-001 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -1.736482e-003 -9.848078e-003 8.400000e-002 + vertex -3.420201e-003 -9.396926e-003 1.220000e-001 + vertex -3.420201e-003 -9.396926e-003 8.400000e-002 + endloop + endfacet + facet normal -4.226184e-001 -9.063077e-001 0.000000e+000 + outer loop + vertex -3.420201e-003 -9.396926e-003 8.400000e-002 + vertex -3.420201e-003 -9.396926e-003 1.220000e-001 + vertex -5.000000e-003 -8.660254e-003 1.220000e-001 + endloop + endfacet + facet normal -4.226184e-001 -9.063077e-001 -3.473095e-018 + outer loop + vertex -3.420201e-003 -9.396926e-003 8.400000e-002 + vertex -5.000000e-003 -8.660254e-003 1.220000e-001 + vertex -5.000000e-003 -8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal -5.735765e-001 -8.191521e-001 0.000000e+000 + outer loop + vertex -5.000000e-003 -8.660254e-003 8.400000e-002 + vertex -5.000000e-003 -8.660254e-003 1.220000e-001 + vertex -6.427876e-003 -7.660444e-003 1.220000e-001 + endloop + endfacet + facet normal -5.735765e-001 -8.191521e-001 0.000000e+000 + outer loop + vertex -5.000000e-003 -8.660254e-003 8.400000e-002 + vertex -6.427876e-003 -7.660444e-003 1.220000e-001 + vertex -6.427876e-003 -7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -6.427876e-003 -7.660444e-003 8.400000e-002 + vertex -6.427876e-003 -7.660444e-003 1.220000e-001 + vertex -7.660444e-003 -6.427876e-003 1.220000e-001 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -6.427876e-003 -7.660444e-003 8.400000e-002 + vertex -7.660444e-003 -6.427876e-003 1.220000e-001 + vertex -7.660444e-003 -6.427876e-003 8.400000e-002 + endloop + endfacet + facet normal -8.191521e-001 -5.735765e-001 0.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 8.400000e-002 + vertex -7.660444e-003 -6.427876e-003 1.220000e-001 + vertex -8.660254e-003 -5.000000e-003 1.220000e-001 + endloop + endfacet + facet normal -8.191521e-001 -5.735765e-001 0.000000e+000 + outer loop + vertex -7.660444e-003 -6.427876e-003 8.400000e-002 + vertex -8.660254e-003 -5.000000e-003 1.220000e-001 + vertex -8.660254e-003 -5.000000e-003 8.400000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex -8.660254e-003 -5.000000e-003 8.400000e-002 + vertex -8.660254e-003 -5.000000e-003 1.220000e-001 + vertex -9.396926e-003 -3.420201e-003 1.220000e-001 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 3.473095e-018 + outer loop + vertex -8.660254e-003 -5.000000e-003 8.400000e-002 + vertex -9.396926e-003 -3.420201e-003 1.220000e-001 + vertex -9.396926e-003 -3.420201e-003 8.400000e-002 + endloop + endfacet + facet normal -9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex -9.396926e-003 -3.420201e-003 8.400000e-002 + vertex -9.396926e-003 -3.420201e-003 1.220000e-001 + vertex -9.848078e-003 -1.736482e-003 1.220000e-001 + endloop + endfacet + facet normal -9.659258e-001 -2.588190e-001 0.000000e+000 + outer loop + vertex -9.396926e-003 -3.420201e-003 8.400000e-002 + vertex -9.848078e-003 -1.736482e-003 1.220000e-001 + vertex -9.848078e-003 -1.736482e-003 8.400000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715547e-002 0.000000e+000 + outer loop + vertex -9.848078e-003 -1.736482e-003 8.400000e-002 + vertex -9.848078e-003 -1.736482e-003 1.220000e-001 + vertex -1.000000e-002 1.224647e-018 1.220000e-001 + endloop + endfacet + facet normal -9.961947e-001 -8.715547e-002 0.000000e+000 + outer loop + vertex -9.848078e-003 -1.736482e-003 8.400000e-002 + vertex -1.000000e-002 1.224647e-018 1.220000e-001 + vertex -1.000000e-002 1.224647e-018 8.400000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715547e-002 0.000000e+000 + outer loop + vertex -1.000000e-002 1.224647e-018 8.400000e-002 + vertex -1.000000e-002 1.224647e-018 1.220000e-001 + vertex -9.848078e-003 1.736482e-003 1.220000e-001 + endloop + endfacet + facet normal -9.961947e-001 8.715547e-002 0.000000e+000 + outer loop + vertex -1.000000e-002 1.224647e-018 8.400000e-002 + vertex -9.848078e-003 1.736482e-003 1.220000e-001 + vertex -9.848078e-003 1.736482e-003 8.400000e-002 + endloop + endfacet + facet normal -9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex -9.848078e-003 1.736482e-003 8.400000e-002 + vertex -9.848078e-003 1.736482e-003 1.220000e-001 + vertex -9.396926e-003 3.420201e-003 1.220000e-001 + endloop + endfacet + facet normal -9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex -9.848078e-003 1.736482e-003 8.400000e-002 + vertex -9.396926e-003 3.420201e-003 1.220000e-001 + vertex -9.396926e-003 3.420201e-003 8.400000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 0.000000e+000 + outer loop + vertex -9.396926e-003 3.420201e-003 8.400000e-002 + vertex -9.396926e-003 3.420201e-003 1.220000e-001 + vertex -8.660254e-003 5.000000e-003 1.220000e-001 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 -3.473095e-018 + outer loop + vertex -9.396926e-003 3.420201e-003 8.400000e-002 + vertex -8.660254e-003 5.000000e-003 1.220000e-001 + vertex -8.660254e-003 5.000000e-003 8.400000e-002 + endloop + endfacet + facet normal -8.191521e-001 5.735765e-001 0.000000e+000 + outer loop + vertex -8.660254e-003 5.000000e-003 8.400000e-002 + vertex -8.660254e-003 5.000000e-003 1.220000e-001 + vertex -7.660444e-003 6.427876e-003 1.220000e-001 + endloop + endfacet + facet normal -8.191521e-001 5.735765e-001 0.000000e+000 + outer loop + vertex -8.660254e-003 5.000000e-003 8.400000e-002 + vertex -7.660444e-003 6.427876e-003 1.220000e-001 + vertex -7.660444e-003 6.427876e-003 8.400000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -7.660444e-003 6.427876e-003 8.400000e-002 + vertex -7.660444e-003 6.427876e-003 1.220000e-001 + vertex -6.427876e-003 7.660444e-003 1.220000e-001 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -7.660444e-003 6.427876e-003 8.400000e-002 + vertex -6.427876e-003 7.660444e-003 1.220000e-001 + vertex -6.427876e-003 7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal -5.735765e-001 8.191521e-001 0.000000e+000 + outer loop + vertex -6.427876e-003 7.660444e-003 8.400000e-002 + vertex -6.427876e-003 7.660444e-003 1.220000e-001 + vertex -5.000000e-003 8.660254e-003 1.220000e-001 + endloop + endfacet + facet normal -5.735765e-001 8.191521e-001 0.000000e+000 + outer loop + vertex -6.427876e-003 7.660444e-003 8.400000e-002 + vertex -5.000000e-003 8.660254e-003 1.220000e-001 + vertex -5.000000e-003 8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal -4.226184e-001 9.063077e-001 0.000000e+000 + outer loop + vertex -5.000000e-003 8.660254e-003 8.400000e-002 + vertex -5.000000e-003 8.660254e-003 1.220000e-001 + vertex -3.420201e-003 9.396926e-003 1.220000e-001 + endloop + endfacet + facet normal -4.226184e-001 9.063077e-001 3.473095e-018 + outer loop + vertex -5.000000e-003 8.660254e-003 8.400000e-002 + vertex -3.420201e-003 9.396926e-003 1.220000e-001 + vertex -3.420201e-003 9.396926e-003 8.400000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -3.420201e-003 9.396926e-003 8.400000e-002 + vertex -3.420201e-003 9.396926e-003 1.220000e-001 + vertex -1.736482e-003 9.848078e-003 1.220000e-001 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -3.420201e-003 9.396926e-003 8.400000e-002 + vertex -1.736482e-003 9.848078e-003 1.220000e-001 + vertex -1.736482e-003 9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal -8.715547e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -1.736482e-003 9.848078e-003 8.400000e-002 + vertex -1.736482e-003 9.848078e-003 1.220000e-001 + vertex 6.123234e-019 1.000000e-002 1.220000e-001 + endloop + endfacet + facet normal -8.715547e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -1.736482e-003 9.848078e-003 8.400000e-002 + vertex 6.123234e-019 1.000000e-002 1.220000e-001 + vertex 6.123234e-019 1.000000e-002 8.400000e-002 + endloop + endfacet + facet normal 8.715547e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex 6.123234e-019 1.000000e-002 8.400000e-002 + vertex 6.123234e-019 1.000000e-002 1.220000e-001 + vertex 1.736482e-003 9.848078e-003 1.220000e-001 + endloop + endfacet + facet normal 8.715547e-002 9.961947e-001 0.000000e+000 + outer loop + vertex 6.123234e-019 1.000000e-002 8.400000e-002 + vertex 1.736482e-003 9.848078e-003 1.220000e-001 + vertex 1.736482e-003 9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 -0.000000e+000 + outer loop + vertex 1.736482e-003 9.848078e-003 8.400000e-002 + vertex 1.736482e-003 9.848078e-003 1.220000e-001 + vertex 3.420201e-003 9.396926e-003 1.220000e-001 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex 1.736482e-003 9.848078e-003 8.400000e-002 + vertex 3.420201e-003 9.396926e-003 1.220000e-001 + vertex 3.420201e-003 9.396926e-003 8.400000e-002 + endloop + endfacet + facet normal 4.226184e-001 9.063077e-001 -0.000000e+000 + outer loop + vertex 3.420201e-003 9.396926e-003 8.400000e-002 + vertex 3.420201e-003 9.396926e-003 1.220000e-001 + vertex 5.000000e-003 8.660254e-003 1.220000e-001 + endloop + endfacet + facet normal 4.226184e-001 9.063077e-001 -3.473095e-018 + outer loop + vertex 3.420201e-003 9.396926e-003 8.400000e-002 + vertex 5.000000e-003 8.660254e-003 1.220000e-001 + vertex 5.000000e-003 8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal 5.735765e-001 8.191521e-001 -0.000000e+000 + outer loop + vertex 5.000000e-003 8.660254e-003 8.400000e-002 + vertex 5.000000e-003 8.660254e-003 1.220000e-001 + vertex 6.427876e-003 7.660444e-003 1.220000e-001 + endloop + endfacet + facet normal 5.735765e-001 8.191521e-001 0.000000e+000 + outer loop + vertex 5.000000e-003 8.660254e-003 8.400000e-002 + vertex 6.427876e-003 7.660444e-003 1.220000e-001 + vertex 6.427876e-003 7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 -0.000000e+000 + outer loop + vertex 6.427876e-003 7.660444e-003 8.400000e-002 + vertex 6.427876e-003 7.660444e-003 1.220000e-001 + vertex 7.660444e-003 6.427876e-003 1.220000e-001 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex 6.427876e-003 7.660444e-003 8.400000e-002 + vertex 7.660444e-003 6.427876e-003 1.220000e-001 + vertex 7.660444e-003 6.427876e-003 8.400000e-002 + endloop + endfacet + facet normal 8.191521e-001 5.735765e-001 -0.000000e+000 + outer loop + vertex 7.660444e-003 6.427876e-003 8.400000e-002 + vertex 7.660444e-003 6.427876e-003 1.220000e-001 + vertex 8.660254e-003 5.000000e-003 1.220000e-001 + endloop + endfacet + facet normal 8.191521e-001 5.735765e-001 0.000000e+000 + outer loop + vertex 7.660444e-003 6.427876e-003 8.400000e-002 + vertex 8.660254e-003 5.000000e-003 1.220000e-001 + vertex 8.660254e-003 5.000000e-003 8.400000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 -0.000000e+000 + outer loop + vertex 8.660254e-003 5.000000e-003 8.400000e-002 + vertex 8.660254e-003 5.000000e-003 1.220000e-001 + vertex 9.396926e-003 3.420201e-003 1.220000e-001 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 3.473095e-018 + outer loop + vertex 8.660254e-003 5.000000e-003 8.400000e-002 + vertex 9.396926e-003 3.420201e-003 1.220000e-001 + vertex 9.396926e-003 3.420201e-003 8.400000e-002 + endloop + endfacet + facet normal 9.659258e-001 2.588190e-001 -0.000000e+000 + outer loop + vertex 9.396926e-003 3.420201e-003 8.400000e-002 + vertex 9.396926e-003 3.420201e-003 1.220000e-001 + vertex 9.848078e-003 1.736482e-003 1.220000e-001 + endloop + endfacet + facet normal 9.659258e-001 2.588190e-001 0.000000e+000 + outer loop + vertex 9.396926e-003 3.420201e-003 8.400000e-002 + vertex 9.848078e-003 1.736482e-003 1.220000e-001 + vertex 9.848078e-003 1.736482e-003 8.400000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715547e-002 -0.000000e+000 + outer loop + vertex 9.848078e-003 1.736482e-003 8.400000e-002 + vertex 9.848078e-003 1.736482e-003 1.220000e-001 + vertex 1.000000e-002 0.000000e+000 1.220000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.500000e-002 1.250000e-002 1.280000e-001 + vertex -4.500000e-002 1.250000e-002 1.280000e-001 + vertex 4.500000e-002 -1.250000e-002 1.280000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.500000e-002 -1.250000e-002 1.280000e-001 + vertex -4.500000e-002 1.250000e-002 1.280000e-001 + vertex -4.500000e-002 -1.250000e-002 1.280000e-001 + endloop + endfacet + facet normal 1.000000e+000 0.000000e+000 0.000000e+000 + outer loop + vertex -4.500000e-002 1.250000e-002 2.430000e-001 + vertex -4.500000e-002 -1.250000e-002 2.430000e-001 + vertex -4.500000e-002 1.250000e-002 1.280000e-001 + endloop + endfacet + facet normal 1.000000e+000 0.000000e+000 0.000000e+000 + outer loop + vertex -4.500000e-002 1.250000e-002 1.280000e-001 + vertex -4.500000e-002 -1.250000e-002 2.430000e-001 + vertex -4.500000e-002 -1.250000e-002 1.280000e-001 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 2.430000e-001 + vertex -5.000000e-002 -1.250000e-002 2.430000e-001 + vertex -4.500000e-002 1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex -4.500000e-002 1.250000e-002 2.430000e-001 + vertex -5.000000e-002 -1.250000e-002 2.430000e-001 + vertex -4.500000e-002 -1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal -1.000000e+000 0.000000e+000 0.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -5.000000e-002 1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal -1.000000e+000 -0.000000e+000 0.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 2.430000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -5.000000e-002 -1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 1.100000e-002 -3.469447e-018 1.230000e-001 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.083289e-002 -1.910130e-003 1.230000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -1.100000e-002 3.469447e-018 1.230000e-001 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.100000e-002 3.469447e-018 1.230000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -1.100000e-002 3.469447e-018 1.230000e-001 + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -1.083289e-002 1.910130e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -1.083289e-002 -1.910130e-003 1.230000e-001 + vertex -1.033662e-002 -3.762222e-003 1.230000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -1.033662e-002 -3.762222e-003 1.230000e-001 + vertex -9.526280e-003 -5.500000e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -9.526280e-003 -5.500000e-003 1.230000e-001 + vertex -8.426489e-003 -7.070664e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 3.762222e-003 -1.033662e-002 1.230000e-001 + vertex 5.500000e-003 -9.526280e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.500000e-003 -9.526280e-003 1.230000e-001 + vertex 7.070664e-003 -8.426489e-003 1.230000e-001 + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 7.070664e-003 -8.426489e-003 1.230000e-001 + vertex 8.426489e-003 -7.070664e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 8.426489e-003 -7.070664e-003 1.230000e-001 + vertex 9.526280e-003 -5.500000e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 9.526280e-003 -5.500000e-003 1.230000e-001 + vertex 1.033662e-002 -3.762222e-003 1.230000e-001 + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 1.033662e-002 -3.762222e-003 1.230000e-001 + vertex 1.083289e-002 -1.910130e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 1.083289e-002 -1.910130e-003 1.230000e-001 + vertex 1.100000e-002 -3.469447e-018 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 1.100000e-002 -3.469447e-018 1.230000e-001 + vertex 1.083289e-002 1.910130e-003 1.230000e-001 + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 1.083289e-002 1.910130e-003 1.230000e-001 + vertex 1.033662e-002 3.762222e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 1.033662e-002 3.762222e-003 1.230000e-001 + vertex 9.526280e-003 5.500000e-003 1.230000e-001 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 0.000000e+000 1.100000e-002 1.230000e-001 + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex 0.000000e+000 1.100000e-002 1.230000e-001 + vertex -1.910130e-003 1.083289e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -1.910130e-003 1.083289e-002 1.230000e-001 + vertex -3.762222e-003 1.033662e-002 1.230000e-001 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -3.762222e-003 1.033662e-002 1.230000e-001 + vertex -5.500000e-003 9.526280e-003 1.230000e-001 + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -5.500000e-003 9.526280e-003 1.230000e-001 + vertex -7.070664e-003 8.426489e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -7.070664e-003 8.426489e-003 1.230000e-001 + vertex -8.426489e-003 7.070664e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -8.426489e-003 -7.070664e-003 1.230000e-001 + vertex -7.070664e-003 -8.426489e-003 1.230000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -7.070664e-003 -8.426489e-003 1.230000e-001 + vertex -5.500000e-003 -9.526280e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -5.500000e-003 -9.526280e-003 1.230000e-001 + vertex -3.762222e-003 -1.033662e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 + outer loop + vertex -3.762222e-003 -1.033662e-002 1.230000e-001 + vertex -1.910130e-003 -1.083289e-002 1.230000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -1.910130e-003 -1.083289e-002 1.230000e-001 + vertex -3.469447e-017 -1.100000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -3.469447e-017 -1.100000e-002 1.230000e-001 + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex -3.469447e-017 -1.100000e-002 1.230000e-001 + vertex 1.910130e-003 -1.083289e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 1.910130e-003 -1.083289e-002 1.230000e-001 + vertex 3.762222e-003 -1.033662e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 9.526280e-003 5.500000e-003 1.230000e-001 + vertex 8.426489e-003 7.070664e-003 1.230000e-001 + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 8.426489e-003 7.070664e-003 1.230000e-001 + vertex 7.070664e-003 8.426489e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 7.070664e-003 8.426489e-003 1.230000e-001 + vertex 5.500000e-003 9.526280e-003 1.230000e-001 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -8.426489e-003 7.070664e-003 1.230000e-001 + vertex -9.526280e-003 5.500000e-003 1.230000e-001 + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -9.526280e-003 5.500000e-003 1.230000e-001 + vertex -1.033662e-002 3.762222e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -1.033662e-002 3.762222e-003 1.230000e-001 + vertex -1.083289e-002 1.910130e-003 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.500000e-003 9.526280e-003 1.230000e-001 + vertex 3.762222e-003 1.033662e-002 1.230000e-001 + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 3.762222e-003 1.033662e-002 1.230000e-001 + vertex 1.910130e-003 1.083289e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 1.910130e-003 1.083289e-002 1.230000e-001 + vertex 0.000000e+000 1.100000e-002 1.230000e-001 + endloop + endfacet + facet normal -0.000000e+000 -1.000000e+000 -0.000000e+000 + outer loop + vertex -4.500000e-002 -1.250000e-002 2.430000e-001 + vertex -5.000000e-002 -1.250000e-002 2.430000e-001 + vertex -4.500000e-002 -1.250000e-002 1.280000e-001 + endloop + endfacet + facet normal -0.000000e+000 -1.000000e+000 0.000000e+000 + outer loop + vertex -4.500000e-002 -1.250000e-002 1.280000e-001 + vertex -5.000000e-002 -1.250000e-002 2.430000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 -1.000000e+000 -0.000000e+000 + outer loop + vertex -4.500000e-002 -1.250000e-002 1.280000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 4.500000e-002 -1.250000e-002 1.280000e-001 + endloop + endfacet + facet normal 0.000000e+000 -1.000000e+000 -0.000000e+000 + outer loop + vertex 4.500000e-002 -1.250000e-002 1.280000e-001 + vertex -5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 -1.000000e+000 0.000000e+000 + outer loop + vertex 4.500000e-002 -1.250000e-002 1.280000e-001 + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 4.500000e-002 -1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal 0.000000e+000 -1.000000e+000 0.000000e+000 + outer loop + vertex 4.500000e-002 -1.250000e-002 2.430000e-001 + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + vertex 5.000000e-002 -1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal -0.000000e+000 1.000000e+000 0.000000e+000 + outer loop + vertex 4.500000e-002 1.250000e-002 2.430000e-001 + vertex 5.000000e-002 1.250000e-002 2.430000e-001 + vertex 4.500000e-002 1.250000e-002 1.280000e-001 + endloop + endfacet + facet normal -0.000000e+000 1.000000e+000 0.000000e+000 + outer loop + vertex 4.500000e-002 1.250000e-002 1.280000e-001 + vertex 5.000000e-002 1.250000e-002 2.430000e-001 + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 1.000000e+000 0.000000e+000 + outer loop + vertex 4.500000e-002 1.250000e-002 1.280000e-001 + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex -4.500000e-002 1.250000e-002 1.280000e-001 + endloop + endfacet + facet normal 0.000000e+000 1.000000e+000 0.000000e+000 + outer loop + vertex -4.500000e-002 1.250000e-002 1.280000e-001 + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 0.000000e+000 1.000000e+000 -0.000000e+000 + outer loop + vertex -4.500000e-002 1.250000e-002 1.280000e-001 + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -4.500000e-002 1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal 0.000000e+000 1.000000e+000 0.000000e+000 + outer loop + vertex -4.500000e-002 1.250000e-002 2.430000e-001 + vertex -5.000000e-002 1.250000e-002 1.230000e-001 + vertex -5.000000e-002 1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal 1.000000e+000 0.000000e+000 0.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 2.430000e-001 + vertex 5.000000e-002 -1.250000e-002 2.430000e-001 + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal 1.000000e+000 0.000000e+000 0.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 1.230000e-001 + vertex 5.000000e-002 -1.250000e-002 2.430000e-001 + vertex 5.000000e-002 -1.250000e-002 1.230000e-001 + endloop + endfacet + facet normal -0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 4.500000e-002 1.250000e-002 2.430000e-001 + vertex 4.500000e-002 -1.250000e-002 2.430000e-001 + vertex 5.000000e-002 1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal 0.000000e+000 0.000000e+000 1.000000e+000 + outer loop + vertex 5.000000e-002 1.250000e-002 2.430000e-001 + vertex 4.500000e-002 -1.250000e-002 2.430000e-001 + vertex 5.000000e-002 -1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal -1.000000e+000 0.000000e+000 0.000000e+000 + outer loop + vertex 4.500000e-002 1.250000e-002 1.280000e-001 + vertex 4.500000e-002 -1.250000e-002 1.280000e-001 + vertex 4.500000e-002 1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal -1.000000e+000 -0.000000e+000 0.000000e+000 + outer loop + vertex 4.500000e-002 1.250000e-002 2.430000e-001 + vertex 4.500000e-002 -1.250000e-002 1.280000e-001 + vertex 4.500000e-002 -1.250000e-002 2.430000e-001 + endloop + endfacet + facet normal -9.961947e-001 8.715576e-002 4.340143e-019 + outer loop + vertex 2.215817e-003 1.694728e-002 7.300000e-002 + vertex 2.250000e-003 1.733799e-002 8.300000e-002 + vertex 2.250000e-003 1.733799e-002 7.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715576e-002 0.000000e+000 + outer loop + vertex 2.250000e-003 1.733799e-002 7.300000e-002 + vertex 2.250000e-003 1.733799e-002 8.300000e-002 + vertex 2.215817e-003 1.772869e-002 8.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715576e-002 -4.340143e-019 + outer loop + vertex 2.250000e-003 1.733799e-002 7.300000e-002 + vertex 2.215817e-003 1.772869e-002 8.300000e-002 + vertex 2.215817e-003 1.772869e-002 7.300000e-002 + endloop + endfacet + facet normal -9.659260e-001 -2.588184e-001 0.000000e+000 + outer loop + vertex 2.215817e-003 1.772869e-002 7.300000e-002 + vertex 2.215817e-003 1.772869e-002 8.300000e-002 + vertex 2.114308e-003 1.810753e-002 8.300000e-002 + endloop + endfacet + facet normal -9.659260e-001 -2.588184e-001 1.736057e-018 + outer loop + vertex 2.215817e-003 1.772869e-002 7.300000e-002 + vertex 2.114308e-003 1.810753e-002 8.300000e-002 + vertex 2.114308e-003 1.810753e-002 7.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex 2.114308e-003 1.810753e-002 7.300000e-002 + vertex 2.114308e-003 1.810753e-002 8.300000e-002 + vertex 1.948557e-003 1.846299e-002 8.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 1.736057e-018 + outer loop + vertex 2.114308e-003 1.810753e-002 7.300000e-002 + vertex 1.948557e-003 1.846299e-002 8.300000e-002 + vertex 1.948557e-003 1.846299e-002 7.300000e-002 + endloop + endfacet + facet normal -8.191525e-001 -5.735758e-001 0.000000e+000 + outer loop + vertex 1.948557e-003 1.846299e-002 7.300000e-002 + vertex 1.948557e-003 1.846299e-002 8.300000e-002 + vertex 1.723600e-003 1.878426e-002 8.300000e-002 + endloop + endfacet + facet normal -8.191525e-001 -5.735758e-001 3.472114e-018 + outer loop + vertex 1.948557e-003 1.846299e-002 7.300000e-002 + vertex 1.723600e-003 1.878426e-002 8.300000e-002 + vertex 1.723600e-003 1.878426e-002 7.300000e-002 + endloop + endfacet + facet normal -7.071062e-001 -7.071074e-001 0.000000e+000 + outer loop + vertex 1.723600e-003 1.878426e-002 7.300000e-002 + vertex 1.723600e-003 1.878426e-002 8.300000e-002 + vertex 1.446272e-003 1.906159e-002 8.300000e-002 + endloop + endfacet + facet normal -7.071062e-001 -7.071074e-001 -3.472114e-018 + outer loop + vertex 1.723600e-003 1.878426e-002 7.300000e-002 + vertex 1.446272e-003 1.906159e-002 8.300000e-002 + vertex 1.446272e-003 1.906159e-002 7.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 -8.191520e-001 0.000000e+000 + outer loop + vertex 1.446272e-003 1.906159e-002 7.300000e-002 + vertex 1.446272e-003 1.906159e-002 8.300000e-002 + vertex 1.125000e-003 1.928654e-002 8.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 -8.191520e-001 0.000000e+000 + outer loop + vertex 1.446272e-003 1.906159e-002 7.300000e-002 + vertex 1.125000e-003 1.928654e-002 8.300000e-002 + vertex 1.125000e-003 1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal -4.226182e-001 -9.063078e-001 0.000000e+000 + outer loop + vertex 1.125000e-003 1.928654e-002 7.300000e-002 + vertex 1.125000e-003 1.928654e-002 8.300000e-002 + vertex 7.695453e-004 1.945229e-002 8.300000e-002 + endloop + endfacet + facet normal -4.226182e-001 -9.063078e-001 -1.736057e-018 + outer loop + vertex 1.125000e-003 1.928654e-002 7.300000e-002 + vertex 7.695453e-004 1.945229e-002 8.300000e-002 + vertex 7.695453e-004 1.945229e-002 7.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 -9.659261e-001 0.000000e+000 + outer loop + vertex 7.695453e-004 1.945229e-002 7.300000e-002 + vertex 7.695453e-004 1.945229e-002 8.300000e-002 + vertex 3.907084e-004 1.955380e-002 8.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 -9.659261e-001 -1.736057e-018 + outer loop + vertex 7.695453e-004 1.945229e-002 7.300000e-002 + vertex 3.907084e-004 1.955380e-002 8.300000e-002 + vertex 3.907084e-004 1.955380e-002 7.300000e-002 + endloop + endfacet + facet normal -8.715754e-002 -9.961945e-001 0.000000e+000 + outer loop + vertex 3.907084e-004 1.955380e-002 7.300000e-002 + vertex 3.907084e-004 1.955380e-002 8.300000e-002 + vertex 1.377728e-019 1.958799e-002 8.300000e-002 + endloop + endfacet + facet normal -8.715754e-002 -9.961945e-001 0.000000e+000 + outer loop + vertex 3.907084e-004 1.955380e-002 7.300000e-002 + vertex 1.377728e-019 1.958799e-002 8.300000e-002 + vertex 1.377728e-019 1.958799e-002 7.300000e-002 + endloop + endfacet + facet normal 8.715754e-002 -9.961945e-001 0.000000e+000 + outer loop + vertex 1.377728e-019 1.958799e-002 7.300000e-002 + vertex 1.377728e-019 1.958799e-002 8.300000e-002 + vertex -3.907084e-004 1.955380e-002 8.300000e-002 + endloop + endfacet + facet normal 8.715754e-002 -9.961945e-001 0.000000e+000 + outer loop + vertex 1.377728e-019 1.958799e-002 7.300000e-002 + vertex -3.907084e-004 1.955380e-002 8.300000e-002 + vertex -3.907084e-004 1.955380e-002 7.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 -9.659261e-001 0.000000e+000 + outer loop + vertex -3.907084e-004 1.955380e-002 7.300000e-002 + vertex -3.907084e-004 1.955380e-002 8.300000e-002 + vertex -7.695453e-004 1.945229e-002 8.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 -9.659261e-001 1.736057e-018 + outer loop + vertex -3.907084e-004 1.955380e-002 7.300000e-002 + vertex -7.695453e-004 1.945229e-002 8.300000e-002 + vertex -7.695453e-004 1.945229e-002 7.300000e-002 + endloop + endfacet + facet normal 4.226182e-001 -9.063078e-001 0.000000e+000 + outer loop + vertex -7.695453e-004 1.945229e-002 7.300000e-002 + vertex -7.695453e-004 1.945229e-002 8.300000e-002 + vertex -1.125000e-003 1.928654e-002 8.300000e-002 + endloop + endfacet + facet normal 4.226182e-001 -9.063078e-001 1.736057e-018 + outer loop + vertex -7.695453e-004 1.945229e-002 7.300000e-002 + vertex -1.125000e-003 1.928654e-002 8.300000e-002 + vertex -1.125000e-003 1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 -8.191520e-001 0.000000e+000 + outer loop + vertex -1.125000e-003 1.928654e-002 7.300000e-002 + vertex -1.125000e-003 1.928654e-002 8.300000e-002 + vertex -1.446272e-003 1.906159e-002 8.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 -8.191520e-001 0.000000e+000 + outer loop + vertex -1.125000e-003 1.928654e-002 7.300000e-002 + vertex -1.446272e-003 1.906159e-002 8.300000e-002 + vertex -1.446272e-003 1.906159e-002 7.300000e-002 + endloop + endfacet + facet normal 7.071062e-001 -7.071074e-001 0.000000e+000 + outer loop + vertex -1.446272e-003 1.906159e-002 7.300000e-002 + vertex -1.446272e-003 1.906159e-002 8.300000e-002 + vertex -1.723600e-003 1.878426e-002 8.300000e-002 + endloop + endfacet + facet normal 7.071062e-001 -7.071074e-001 3.472114e-018 + outer loop + vertex -1.446272e-003 1.906159e-002 7.300000e-002 + vertex -1.723600e-003 1.878426e-002 8.300000e-002 + vertex -1.723600e-003 1.878426e-002 7.300000e-002 + endloop + endfacet + facet normal 8.191525e-001 -5.735758e-001 0.000000e+000 + outer loop + vertex -1.723600e-003 1.878426e-002 7.300000e-002 + vertex -1.723600e-003 1.878426e-002 8.300000e-002 + vertex -1.948557e-003 1.846299e-002 8.300000e-002 + endloop + endfacet + facet normal 8.191525e-001 -5.735758e-001 -3.472114e-018 + outer loop + vertex -1.723600e-003 1.878426e-002 7.300000e-002 + vertex -1.948557e-003 1.846299e-002 8.300000e-002 + vertex -1.948557e-003 1.846299e-002 7.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex -1.948557e-003 1.846299e-002 7.300000e-002 + vertex -1.948557e-003 1.846299e-002 8.300000e-002 + vertex -2.114308e-003 1.810753e-002 8.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 -1.736057e-018 + outer loop + vertex -1.948557e-003 1.846299e-002 7.300000e-002 + vertex -2.114308e-003 1.810753e-002 8.300000e-002 + vertex -2.114308e-003 1.810753e-002 7.300000e-002 + endloop + endfacet + facet normal 9.659260e-001 -2.588184e-001 0.000000e+000 + outer loop + vertex -2.114308e-003 1.810753e-002 7.300000e-002 + vertex -2.114308e-003 1.810753e-002 8.300000e-002 + vertex -2.215817e-003 1.772869e-002 8.300000e-002 + endloop + endfacet + facet normal 9.659260e-001 -2.588184e-001 -1.736057e-018 + outer loop + vertex -2.114308e-003 1.810753e-002 7.300000e-002 + vertex -2.215817e-003 1.772869e-002 8.300000e-002 + vertex -2.215817e-003 1.772869e-002 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715576e-002 0.000000e+000 + outer loop + vertex -2.215817e-003 1.772869e-002 7.300000e-002 + vertex -2.215817e-003 1.772869e-002 8.300000e-002 + vertex -2.250000e-003 1.733799e-002 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715576e-002 4.340143e-019 + outer loop + vertex -2.215817e-003 1.772869e-002 7.300000e-002 + vertex -2.250000e-003 1.733799e-002 8.300000e-002 + vertex -2.250000e-003 1.733799e-002 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715576e-002 -0.000000e+000 + outer loop + vertex -2.250000e-003 1.733799e-002 7.300000e-002 + vertex -2.250000e-003 1.733799e-002 8.300000e-002 + vertex -2.215817e-003 1.694728e-002 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715576e-002 -4.340143e-019 + outer loop + vertex -2.250000e-003 1.733799e-002 7.300000e-002 + vertex -2.215817e-003 1.694728e-002 8.300000e-002 + vertex -2.215817e-003 1.694728e-002 7.300000e-002 + endloop + endfacet + facet normal 9.659257e-001 2.588196e-001 -0.000000e+000 + outer loop + vertex -2.215817e-003 1.694728e-002 7.300000e-002 + vertex -2.215817e-003 1.694728e-002 8.300000e-002 + vertex -2.114308e-003 1.656844e-002 8.300000e-002 + endloop + endfacet + facet normal 9.659257e-001 2.588196e-001 -1.736057e-018 + outer loop + vertex -2.215817e-003 1.694728e-002 7.300000e-002 + vertex -2.114308e-003 1.656844e-002 8.300000e-002 + vertex -2.114308e-003 1.656844e-002 7.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 -0.000000e+000 + outer loop + vertex -2.114308e-003 1.656844e-002 7.300000e-002 + vertex -2.114308e-003 1.656844e-002 8.300000e-002 + vertex -1.948557e-003 1.621299e-002 8.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 1.736057e-018 + outer loop + vertex -2.114308e-003 1.656844e-002 7.300000e-002 + vertex -1.948557e-003 1.621299e-002 8.300000e-002 + vertex -1.948557e-003 1.621299e-002 7.300000e-002 + endloop + endfacet + facet normal 8.191525e-001 5.735758e-001 -0.000000e+000 + outer loop + vertex -1.948557e-003 1.621299e-002 7.300000e-002 + vertex -1.948557e-003 1.621299e-002 8.300000e-002 + vertex -1.723600e-003 1.589171e-002 8.300000e-002 + endloop + endfacet + facet normal 8.191525e-001 5.735758e-001 3.472114e-018 + outer loop + vertex -1.948557e-003 1.621299e-002 7.300000e-002 + vertex -1.723600e-003 1.589171e-002 8.300000e-002 + vertex -1.723600e-003 1.589171e-002 7.300000e-002 + endloop + endfacet + facet normal 7.071074e-001 7.071062e-001 -0.000000e+000 + outer loop + vertex -1.723600e-003 1.589171e-002 7.300000e-002 + vertex -1.723600e-003 1.589171e-002 8.300000e-002 + vertex -1.446272e-003 1.561439e-002 8.300000e-002 + endloop + endfacet + facet normal 7.071074e-001 7.071062e-001 -3.472114e-018 + outer loop + vertex -1.723600e-003 1.589171e-002 7.300000e-002 + vertex -1.446272e-003 1.561439e-002 8.300000e-002 + vertex -1.446272e-003 1.561439e-002 7.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 8.191520e-001 -0.000000e+000 + outer loop + vertex -1.446272e-003 1.561439e-002 7.300000e-002 + vertex -1.446272e-003 1.561439e-002 8.300000e-002 + vertex -1.125000e-003 1.538943e-002 8.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 8.191520e-001 0.000000e+000 + outer loop + vertex -1.446272e-003 1.561439e-002 7.300000e-002 + vertex -1.125000e-003 1.538943e-002 8.300000e-002 + vertex -1.125000e-003 1.538943e-002 7.300000e-002 + endloop + endfacet + facet normal 4.226182e-001 9.063078e-001 -0.000000e+000 + outer loop + vertex -1.125000e-003 1.538943e-002 7.300000e-002 + vertex -1.125000e-003 1.538943e-002 8.300000e-002 + vertex -7.695453e-004 1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal 4.226182e-001 9.063078e-001 -1.736057e-018 + outer loop + vertex -1.125000e-003 1.538943e-002 7.300000e-002 + vertex -7.695453e-004 1.522368e-002 8.300000e-002 + vertex -7.695453e-004 1.522368e-002 7.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 9.659261e-001 -0.000000e+000 + outer loop + vertex -7.695453e-004 1.522368e-002 7.300000e-002 + vertex -7.695453e-004 1.522368e-002 8.300000e-002 + vertex -3.907084e-004 1.512217e-002 8.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 9.659261e-001 -1.736057e-018 + outer loop + vertex -7.695453e-004 1.522368e-002 7.300000e-002 + vertex -3.907084e-004 1.512217e-002 8.300000e-002 + vertex -3.907084e-004 1.512217e-002 7.300000e-002 + endloop + endfacet + facet normal 8.715754e-002 9.961945e-001 -0.000000e+000 + outer loop + vertex -3.907084e-004 1.512217e-002 7.300000e-002 + vertex -3.907084e-004 1.512217e-002 8.300000e-002 + vertex -4.133183e-019 1.508799e-002 8.300000e-002 + endloop + endfacet + facet normal 8.715754e-002 9.961945e-001 0.000000e+000 + outer loop + vertex -3.907084e-004 1.512217e-002 7.300000e-002 + vertex -4.133183e-019 1.508799e-002 8.300000e-002 + vertex -4.133183e-019 1.508799e-002 7.300000e-002 + endloop + endfacet + facet normal -8.715754e-002 9.961945e-001 0.000000e+000 + outer loop + vertex -4.133183e-019 1.508799e-002 7.300000e-002 + vertex -4.133183e-019 1.508799e-002 8.300000e-002 + vertex 3.907084e-004 1.512217e-002 8.300000e-002 + endloop + endfacet + facet normal -8.715754e-002 9.961945e-001 0.000000e+000 + outer loop + vertex -4.133183e-019 1.508799e-002 7.300000e-002 + vertex 3.907084e-004 1.512217e-002 8.300000e-002 + vertex 3.907084e-004 1.512217e-002 7.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 9.659261e-001 0.000000e+000 + outer loop + vertex 3.907084e-004 1.512217e-002 7.300000e-002 + vertex 3.907084e-004 1.512217e-002 8.300000e-002 + vertex 7.695453e-004 1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 9.659261e-001 1.736057e-018 + outer loop + vertex 3.907084e-004 1.512217e-002 7.300000e-002 + vertex 7.695453e-004 1.522368e-002 8.300000e-002 + vertex 7.695453e-004 1.522368e-002 7.300000e-002 + endloop + endfacet + facet normal -4.226182e-001 9.063078e-001 0.000000e+000 + outer loop + vertex 7.695453e-004 1.522368e-002 7.300000e-002 + vertex 7.695453e-004 1.522368e-002 8.300000e-002 + vertex 1.125000e-003 1.538943e-002 8.300000e-002 + endloop + endfacet + facet normal -4.226182e-001 9.063078e-001 1.736057e-018 + outer loop + vertex 7.695453e-004 1.522368e-002 7.300000e-002 + vertex 1.125000e-003 1.538943e-002 8.300000e-002 + vertex 1.125000e-003 1.538943e-002 7.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 8.191520e-001 0.000000e+000 + outer loop + vertex 1.125000e-003 1.538943e-002 7.300000e-002 + vertex 1.125000e-003 1.538943e-002 8.300000e-002 + vertex 1.446272e-003 1.561439e-002 8.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 8.191520e-001 0.000000e+000 + outer loop + vertex 1.125000e-003 1.538943e-002 7.300000e-002 + vertex 1.446272e-003 1.561439e-002 8.300000e-002 + vertex 1.446272e-003 1.561439e-002 7.300000e-002 + endloop + endfacet + facet normal -7.071074e-001 7.071062e-001 0.000000e+000 + outer loop + vertex 1.446272e-003 1.561439e-002 7.300000e-002 + vertex 1.446272e-003 1.561439e-002 8.300000e-002 + vertex 1.723600e-003 1.589171e-002 8.300000e-002 + endloop + endfacet + facet normal -7.071074e-001 7.071062e-001 3.472114e-018 + outer loop + vertex 1.446272e-003 1.561439e-002 7.300000e-002 + vertex 1.723600e-003 1.589171e-002 8.300000e-002 + vertex 1.723600e-003 1.589171e-002 7.300000e-002 + endloop + endfacet + facet normal -8.191525e-001 5.735758e-001 0.000000e+000 + outer loop + vertex 1.723600e-003 1.589171e-002 7.300000e-002 + vertex 1.723600e-003 1.589171e-002 8.300000e-002 + vertex 1.948557e-003 1.621299e-002 8.300000e-002 + endloop + endfacet + facet normal -8.191525e-001 5.735758e-001 -3.472114e-018 + outer loop + vertex 1.723600e-003 1.589171e-002 7.300000e-002 + vertex 1.948557e-003 1.621299e-002 8.300000e-002 + vertex 1.948557e-003 1.621299e-002 7.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 0.000000e+000 + outer loop + vertex 1.948557e-003 1.621299e-002 7.300000e-002 + vertex 1.948557e-003 1.621299e-002 8.300000e-002 + vertex 2.114308e-003 1.656844e-002 8.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 -1.736057e-018 + outer loop + vertex 1.948557e-003 1.621299e-002 7.300000e-002 + vertex 2.114308e-003 1.656844e-002 8.300000e-002 + vertex 2.114308e-003 1.656844e-002 7.300000e-002 + endloop + endfacet + facet normal -9.659257e-001 2.588196e-001 0.000000e+000 + outer loop + vertex 2.114308e-003 1.656844e-002 7.300000e-002 + vertex 2.114308e-003 1.656844e-002 8.300000e-002 + vertex 2.215817e-003 1.694728e-002 8.300000e-002 + endloop + endfacet + facet normal -9.659257e-001 2.588196e-001 1.736057e-018 + outer loop + vertex 2.114308e-003 1.656844e-002 7.300000e-002 + vertex 2.215817e-003 1.694728e-002 8.300000e-002 + vertex 2.215817e-003 1.694728e-002 7.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715576e-002 0.000000e+000 + outer loop + vertex 2.215817e-003 1.694728e-002 7.300000e-002 + vertex 2.215817e-003 1.694728e-002 8.300000e-002 + vertex 2.250000e-003 1.733799e-002 8.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715517e-002 4.340143e-019 + outer loop + vertex -1.279932e-002 8.278284e-003 7.300000e-002 + vertex -1.276514e-002 8.668993e-003 8.300000e-002 + vertex -1.276514e-002 8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715517e-002 0.000000e+000 + outer loop + vertex -1.276514e-002 8.668993e-003 7.300000e-002 + vertex -1.276514e-002 8.668993e-003 8.300000e-002 + vertex -1.279932e-002 9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715517e-002 -4.340143e-019 + outer loop + vertex -1.276514e-002 8.668993e-003 7.300000e-002 + vertex -1.279932e-002 9.059701e-003 8.300000e-002 + vertex -1.279932e-002 9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex -1.279932e-002 9.059701e-003 7.300000e-002 + vertex -1.279932e-002 9.059701e-003 8.300000e-002 + vertex -1.290083e-002 9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex -1.279932e-002 9.059701e-003 7.300000e-002 + vertex -1.290083e-002 9.438538e-003 8.300000e-002 + vertex -1.290083e-002 9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal -9.063080e-001 -4.226178e-001 0.000000e+000 + outer loop + vertex -1.290083e-002 9.438538e-003 7.300000e-002 + vertex -1.290083e-002 9.438538e-003 8.300000e-002 + vertex -1.306658e-002 9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal -9.063080e-001 -4.226178e-001 0.000000e+000 + outer loop + vertex -1.290083e-002 9.438538e-003 7.300000e-002 + vertex -1.306658e-002 9.793993e-003 8.300000e-002 + vertex -1.306658e-002 9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex -1.306658e-002 9.793993e-003 7.300000e-002 + vertex -1.306658e-002 9.793993e-003 8.300000e-002 + vertex -1.329154e-002 1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex -1.306658e-002 9.793993e-003 7.300000e-002 + vertex -1.329154e-002 1.011526e-002 8.300000e-002 + vertex -1.329154e-002 1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -1.329154e-002 1.011526e-002 7.300000e-002 + vertex -1.329154e-002 1.011526e-002 8.300000e-002 + vertex -1.356886e-002 1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal -7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex -1.329154e-002 1.011526e-002 7.300000e-002 + vertex -1.356886e-002 1.039259e-002 8.300000e-002 + vertex -1.356886e-002 1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal -5.735769e-001 -8.191517e-001 0.000000e+000 + outer loop + vertex -1.356886e-002 1.039259e-002 7.300000e-002 + vertex -1.356886e-002 1.039259e-002 8.300000e-002 + vertex -1.389014e-002 1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal -5.735769e-001 -8.191517e-001 0.000000e+000 + outer loop + vertex -1.356886e-002 1.039259e-002 7.300000e-002 + vertex -1.389014e-002 1.061755e-002 8.300000e-002 + vertex -1.389014e-002 1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal -4.226178e-001 -9.063080e-001 0.000000e+000 + outer loop + vertex -1.389014e-002 1.061755e-002 7.300000e-002 + vertex -1.389014e-002 1.061755e-002 8.300000e-002 + vertex -1.424559e-002 1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal -4.226178e-001 -9.063080e-001 0.000000e+000 + outer loop + vertex -1.389014e-002 1.061755e-002 7.300000e-002 + vertex -1.424559e-002 1.078330e-002 8.300000e-002 + vertex -1.424559e-002 1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 -9.659261e-001 0.000000e+000 + outer loop + vertex -1.424559e-002 1.078330e-002 7.300000e-002 + vertex -1.424559e-002 1.078330e-002 8.300000e-002 + vertex -1.462443e-002 1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 -9.659261e-001 0.000000e+000 + outer loop + vertex -1.424559e-002 1.078330e-002 7.300000e-002 + vertex -1.462443e-002 1.088481e-002 8.300000e-002 + vertex -1.462443e-002 1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal -8.715517e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.462443e-002 1.088481e-002 7.300000e-002 + vertex -1.462443e-002 1.088481e-002 8.300000e-002 + vertex -1.501514e-002 1.091899e-002 8.300000e-002 + endloop + endfacet + facet normal -8.715517e-002 -9.961947e-001 4.340143e-019 + outer loop + vertex -1.462443e-002 1.088481e-002 7.300000e-002 + vertex -1.501514e-002 1.091899e-002 8.300000e-002 + vertex -1.501514e-002 1.091899e-002 7.300000e-002 + endloop + endfacet + facet normal 8.715517e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.501514e-002 1.091899e-002 7.300000e-002 + vertex -1.501514e-002 1.091899e-002 8.300000e-002 + vertex -1.540584e-002 1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal 8.715517e-002 -9.961947e-001 -4.340143e-019 + outer loop + vertex -1.501514e-002 1.091899e-002 7.300000e-002 + vertex -1.540584e-002 1.088481e-002 8.300000e-002 + vertex -1.540584e-002 1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal 2.588185e-001 -9.659260e-001 0.000000e+000 + outer loop + vertex -1.540584e-002 1.088481e-002 7.300000e-002 + vertex -1.540584e-002 1.088481e-002 8.300000e-002 + vertex -1.578468e-002 1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 2.588185e-001 -9.659260e-001 1.736057e-018 + outer loop + vertex -1.540584e-002 1.088481e-002 7.300000e-002 + vertex -1.578468e-002 1.078330e-002 8.300000e-002 + vertex -1.578468e-002 1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal 4.226169e-001 -9.063084e-001 0.000000e+000 + outer loop + vertex -1.578468e-002 1.078330e-002 7.300000e-002 + vertex -1.578468e-002 1.078330e-002 8.300000e-002 + vertex -1.614014e-002 1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal 4.226169e-001 -9.063084e-001 0.000000e+000 + outer loop + vertex -1.578468e-002 1.078330e-002 7.300000e-002 + vertex -1.614014e-002 1.061755e-002 8.300000e-002 + vertex -1.614014e-002 1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal 5.735781e-001 -8.191509e-001 0.000000e+000 + outer loop + vertex -1.614014e-002 1.061755e-002 7.300000e-002 + vertex -1.614014e-002 1.061755e-002 8.300000e-002 + vertex -1.646141e-002 1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal 5.735781e-001 -8.191509e-001 -3.472114e-018 + outer loop + vertex -1.614014e-002 1.061755e-002 7.300000e-002 + vertex -1.646141e-002 1.039259e-002 8.300000e-002 + vertex -1.646141e-002 1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 7.071056e-001 -7.071080e-001 0.000000e+000 + outer loop + vertex -1.646141e-002 1.039259e-002 7.300000e-002 + vertex -1.646141e-002 1.039259e-002 8.300000e-002 + vertex -1.673874e-002 1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 7.071056e-001 -7.071080e-001 0.000000e+000 + outer loop + vertex -1.646141e-002 1.039259e-002 7.300000e-002 + vertex -1.673874e-002 1.011526e-002 8.300000e-002 + vertex -1.673874e-002 1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex -1.673874e-002 1.011526e-002 7.300000e-002 + vertex -1.673874e-002 1.011526e-002 8.300000e-002 + vertex -1.696369e-002 9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex -1.673874e-002 1.011526e-002 7.300000e-002 + vertex -1.696369e-002 9.793993e-003 8.300000e-002 + vertex -1.696369e-002 9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 9.063080e-001 -4.226178e-001 0.000000e+000 + outer loop + vertex -1.696369e-002 9.793993e-003 7.300000e-002 + vertex -1.696369e-002 9.793993e-003 8.300000e-002 + vertex -1.712945e-002 9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 9.063080e-001 -4.226178e-001 0.000000e+000 + outer loop + vertex -1.696369e-002 9.793993e-003 7.300000e-002 + vertex -1.712945e-002 9.438538e-003 8.300000e-002 + vertex -1.712945e-002 9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex -1.712945e-002 9.438538e-003 7.300000e-002 + vertex -1.712945e-002 9.438538e-003 8.300000e-002 + vertex -1.723095e-002 9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex -1.712945e-002 9.438538e-003 7.300000e-002 + vertex -1.723095e-002 9.059701e-003 8.300000e-002 + vertex -1.723095e-002 9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 9.961950e-001 -8.715281e-002 0.000000e+000 + outer loop + vertex -1.723095e-002 9.059701e-003 7.300000e-002 + vertex -1.723095e-002 9.059701e-003 8.300000e-002 + vertex -1.726514e-002 8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 9.961950e-001 -8.715281e-002 -4.340143e-019 + outer loop + vertex -1.723095e-002 9.059701e-003 7.300000e-002 + vertex -1.726514e-002 8.668993e-003 8.300000e-002 + vertex -1.726514e-002 8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal 9.961950e-001 8.715281e-002 -0.000000e+000 + outer loop + vertex -1.726514e-002 8.668993e-003 7.300000e-002 + vertex -1.726514e-002 8.668993e-003 8.300000e-002 + vertex -1.723095e-002 8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 9.961950e-001 8.715281e-002 4.340143e-019 + outer loop + vertex -1.726514e-002 8.668993e-003 7.300000e-002 + vertex -1.723095e-002 8.278284e-003 8.300000e-002 + vertex -1.723095e-002 8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 2.588179e-001 -0.000000e+000 + outer loop + vertex -1.723095e-002 8.278284e-003 7.300000e-002 + vertex -1.723095e-002 8.278284e-003 8.300000e-002 + vertex -1.712945e-002 7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex -1.723095e-002 8.278284e-003 7.300000e-002 + vertex -1.712945e-002 7.899447e-003 8.300000e-002 + vertex -1.712945e-002 7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal 9.063078e-001 4.226182e-001 -0.000000e+000 + outer loop + vertex -1.712945e-002 7.899447e-003 7.300000e-002 + vertex -1.712945e-002 7.899447e-003 8.300000e-002 + vertex -1.696369e-002 7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal 9.063078e-001 4.226182e-001 1.736057e-018 + outer loop + vertex -1.712945e-002 7.899447e-003 7.300000e-002 + vertex -1.696369e-002 7.543993e-003 8.300000e-002 + vertex -1.696369e-002 7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 5.735769e-001 -0.000000e+000 + outer loop + vertex -1.696369e-002 7.543993e-003 7.300000e-002 + vertex -1.696369e-002 7.543993e-003 8.300000e-002 + vertex -1.673874e-002 7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex -1.696369e-002 7.543993e-003 7.300000e-002 + vertex -1.673874e-002 7.222721e-003 8.300000e-002 + vertex -1.673874e-002 7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 7.071050e-001 7.071086e-001 -0.000000e+000 + outer loop + vertex -1.673874e-002 7.222721e-003 7.300000e-002 + vertex -1.673874e-002 7.222721e-003 8.300000e-002 + vertex -1.646141e-002 6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 7.071050e-001 7.071086e-001 0.000000e+000 + outer loop + vertex -1.673874e-002 7.222721e-003 7.300000e-002 + vertex -1.646141e-002 6.945393e-003 8.300000e-002 + vertex -1.646141e-002 6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 5.735781e-001 8.191509e-001 -0.000000e+000 + outer loop + vertex -1.646141e-002 6.945393e-003 7.300000e-002 + vertex -1.646141e-002 6.945393e-003 8.300000e-002 + vertex -1.614014e-002 6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal 5.735781e-001 8.191509e-001 3.472114e-018 + outer loop + vertex -1.646141e-002 6.945393e-003 7.300000e-002 + vertex -1.614014e-002 6.720436e-003 8.300000e-002 + vertex -1.614014e-002 6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal 4.226169e-001 9.063084e-001 -0.000000e+000 + outer loop + vertex -1.614014e-002 6.720436e-003 7.300000e-002 + vertex -1.614014e-002 6.720436e-003 8.300000e-002 + vertex -1.578468e-002 6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 4.226169e-001 9.063084e-001 0.000000e+000 + outer loop + vertex -1.614014e-002 6.720436e-003 7.300000e-002 + vertex -1.578468e-002 6.554685e-003 8.300000e-002 + vertex -1.578468e-002 6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal 2.588196e-001 9.659257e-001 -0.000000e+000 + outer loop + vertex -1.578468e-002 6.554685e-003 7.300000e-002 + vertex -1.578468e-002 6.554685e-003 8.300000e-002 + vertex -1.540584e-002 6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 2.588196e-001 9.659257e-001 1.736057e-018 + outer loop + vertex -1.578468e-002 6.554685e-003 7.300000e-002 + vertex -1.540584e-002 6.453176e-003 8.300000e-002 + vertex -1.540584e-002 6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 8.715635e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex -1.540584e-002 6.453176e-003 7.300000e-002 + vertex -1.540584e-002 6.453176e-003 8.300000e-002 + vertex -1.501514e-002 6.418993e-003 8.300000e-002 + endloop + endfacet + facet normal 8.715635e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -1.540584e-002 6.453176e-003 7.300000e-002 + vertex -1.501514e-002 6.418993e-003 8.300000e-002 + vertex -1.501514e-002 6.418993e-003 7.300000e-002 + endloop + endfacet + facet normal -8.715635e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -1.501514e-002 6.418993e-003 7.300000e-002 + vertex -1.501514e-002 6.418993e-003 8.300000e-002 + vertex -1.462443e-002 6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal -8.715635e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -1.501514e-002 6.418993e-003 7.300000e-002 + vertex -1.462443e-002 6.453176e-003 8.300000e-002 + vertex -1.462443e-002 6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 0.000000e+000 + outer loop + vertex -1.462443e-002 6.453176e-003 7.300000e-002 + vertex -1.462443e-002 6.453176e-003 8.300000e-002 + vertex -1.424559e-002 6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal -2.588190e-001 9.659258e-001 1.736057e-018 + outer loop + vertex -1.462443e-002 6.453176e-003 7.300000e-002 + vertex -1.424559e-002 6.554685e-003 8.300000e-002 + vertex -1.424559e-002 6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal -4.226178e-001 9.063080e-001 0.000000e+000 + outer loop + vertex -1.424559e-002 6.554685e-003 7.300000e-002 + vertex -1.424559e-002 6.554685e-003 8.300000e-002 + vertex -1.389014e-002 6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal -4.226178e-001 9.063080e-001 0.000000e+000 + outer loop + vertex -1.424559e-002 6.554685e-003 7.300000e-002 + vertex -1.389014e-002 6.720436e-003 8.300000e-002 + vertex -1.389014e-002 6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal -5.735769e-001 8.191517e-001 0.000000e+000 + outer loop + vertex -1.389014e-002 6.720436e-003 7.300000e-002 + vertex -1.389014e-002 6.720436e-003 8.300000e-002 + vertex -1.356886e-002 6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal -5.735769e-001 8.191517e-001 0.000000e+000 + outer loop + vertex -1.389014e-002 6.720436e-003 7.300000e-002 + vertex -1.356886e-002 6.945393e-003 8.300000e-002 + vertex -1.356886e-002 6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal -7.071062e-001 7.071074e-001 0.000000e+000 + outer loop + vertex -1.356886e-002 6.945393e-003 7.300000e-002 + vertex -1.356886e-002 6.945393e-003 8.300000e-002 + vertex -1.329154e-002 7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal -7.071062e-001 7.071074e-001 -3.472114e-018 + outer loop + vertex -1.356886e-002 6.945393e-003 7.300000e-002 + vertex -1.329154e-002 7.222721e-003 8.300000e-002 + vertex -1.329154e-002 7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex -1.329154e-002 7.222721e-003 7.300000e-002 + vertex -1.329154e-002 7.222721e-003 8.300000e-002 + vertex -1.306658e-002 7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex -1.329154e-002 7.222721e-003 7.300000e-002 + vertex -1.306658e-002 7.543993e-003 8.300000e-002 + vertex -1.306658e-002 7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal -9.063078e-001 4.226182e-001 0.000000e+000 + outer loop + vertex -1.306658e-002 7.543993e-003 7.300000e-002 + vertex -1.306658e-002 7.543993e-003 8.300000e-002 + vertex -1.290083e-002 7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal -9.063078e-001 4.226182e-001 -1.736057e-018 + outer loop + vertex -1.306658e-002 7.543993e-003 7.300000e-002 + vertex -1.290083e-002 7.899447e-003 8.300000e-002 + vertex -1.290083e-002 7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex -1.290083e-002 7.899447e-003 7.300000e-002 + vertex -1.290083e-002 7.899447e-003 8.300000e-002 + vertex -1.279932e-002 8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex -1.290083e-002 7.899447e-003 7.300000e-002 + vertex -1.279932e-002 8.278284e-003 8.300000e-002 + vertex -1.279932e-002 8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715517e-002 0.000000e+000 + outer loop + vertex -1.279932e-002 8.278284e-003 7.300000e-002 + vertex -1.279932e-002 8.278284e-003 8.300000e-002 + vertex -1.276514e-002 8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715517e-002 4.340143e-019 + outer loop + vertex -1.279932e-002 -9.059701e-003 7.300000e-002 + vertex -1.276514e-002 -8.668993e-003 8.300000e-002 + vertex -1.276514e-002 -8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715517e-002 0.000000e+000 + outer loop + vertex -1.276514e-002 -8.668993e-003 7.300000e-002 + vertex -1.276514e-002 -8.668993e-003 8.300000e-002 + vertex -1.279932e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715517e-002 -4.340143e-019 + outer loop + vertex -1.276514e-002 -8.668993e-003 7.300000e-002 + vertex -1.279932e-002 -8.278284e-003 8.300000e-002 + vertex -1.279932e-002 -8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex -1.279932e-002 -8.278284e-003 7.300000e-002 + vertex -1.279932e-002 -8.278284e-003 8.300000e-002 + vertex -1.290083e-002 -7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex -1.279932e-002 -8.278284e-003 7.300000e-002 + vertex -1.290083e-002 -7.899447e-003 8.300000e-002 + vertex -1.290083e-002 -7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal -9.063078e-001 -4.226182e-001 0.000000e+000 + outer loop + vertex -1.290083e-002 -7.899447e-003 7.300000e-002 + vertex -1.290083e-002 -7.899447e-003 8.300000e-002 + vertex -1.306658e-002 -7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal -9.063078e-001 -4.226182e-001 1.736057e-018 + outer loop + vertex -1.290083e-002 -7.899447e-003 7.300000e-002 + vertex -1.306658e-002 -7.543993e-003 8.300000e-002 + vertex -1.306658e-002 -7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex -1.306658e-002 -7.543993e-003 7.300000e-002 + vertex -1.306658e-002 -7.543993e-003 8.300000e-002 + vertex -1.329154e-002 -7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex -1.306658e-002 -7.543993e-003 7.300000e-002 + vertex -1.329154e-002 -7.222721e-003 8.300000e-002 + vertex -1.329154e-002 -7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal -7.071062e-001 -7.071074e-001 0.000000e+000 + outer loop + vertex -1.329154e-002 -7.222721e-003 7.300000e-002 + vertex -1.329154e-002 -7.222721e-003 8.300000e-002 + vertex -1.356886e-002 -6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal -7.071062e-001 -7.071074e-001 3.472114e-018 + outer loop + vertex -1.329154e-002 -7.222721e-003 7.300000e-002 + vertex -1.356886e-002 -6.945393e-003 8.300000e-002 + vertex -1.356886e-002 -6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal -5.735769e-001 -8.191517e-001 0.000000e+000 + outer loop + vertex -1.356886e-002 -6.945393e-003 7.300000e-002 + vertex -1.356886e-002 -6.945393e-003 8.300000e-002 + vertex -1.389014e-002 -6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal -5.735769e-001 -8.191517e-001 0.000000e+000 + outer loop + vertex -1.356886e-002 -6.945393e-003 7.300000e-002 + vertex -1.389014e-002 -6.720436e-003 8.300000e-002 + vertex -1.389014e-002 -6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal -4.226178e-001 -9.063080e-001 0.000000e+000 + outer loop + vertex -1.389014e-002 -6.720436e-003 7.300000e-002 + vertex -1.389014e-002 -6.720436e-003 8.300000e-002 + vertex -1.424559e-002 -6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal -4.226178e-001 -9.063080e-001 0.000000e+000 + outer loop + vertex -1.389014e-002 -6.720436e-003 7.300000e-002 + vertex -1.424559e-002 -6.554685e-003 8.300000e-002 + vertex -1.424559e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex -1.424559e-002 -6.554685e-003 7.300000e-002 + vertex -1.424559e-002 -6.554685e-003 8.300000e-002 + vertex -1.462443e-002 -6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal -2.588190e-001 -9.659258e-001 -1.736057e-018 + outer loop + vertex -1.424559e-002 -6.554685e-003 7.300000e-002 + vertex -1.462443e-002 -6.453176e-003 8.300000e-002 + vertex -1.462443e-002 -6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal -8.715635e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.462443e-002 -6.453176e-003 7.300000e-002 + vertex -1.462443e-002 -6.453176e-003 8.300000e-002 + vertex -1.501514e-002 -6.418993e-003 8.300000e-002 + endloop + endfacet + facet normal -8.715635e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.462443e-002 -6.453176e-003 7.300000e-002 + vertex -1.501514e-002 -6.418993e-003 8.300000e-002 + vertex -1.501514e-002 -6.418993e-003 7.300000e-002 + endloop + endfacet + facet normal 8.715635e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.501514e-002 -6.418993e-003 7.300000e-002 + vertex -1.501514e-002 -6.418993e-003 8.300000e-002 + vertex -1.540584e-002 -6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 8.715635e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex -1.501514e-002 -6.418993e-003 7.300000e-002 + vertex -1.540584e-002 -6.453176e-003 8.300000e-002 + vertex -1.540584e-002 -6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 2.588196e-001 -9.659257e-001 0.000000e+000 + outer loop + vertex -1.540584e-002 -6.453176e-003 7.300000e-002 + vertex -1.540584e-002 -6.453176e-003 8.300000e-002 + vertex -1.578468e-002 -6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 2.588196e-001 -9.659257e-001 -1.736057e-018 + outer loop + vertex -1.540584e-002 -6.453176e-003 7.300000e-002 + vertex -1.578468e-002 -6.554685e-003 8.300000e-002 + vertex -1.578468e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal 4.226169e-001 -9.063084e-001 0.000000e+000 + outer loop + vertex -1.578468e-002 -6.554685e-003 7.300000e-002 + vertex -1.578468e-002 -6.554685e-003 8.300000e-002 + vertex -1.614014e-002 -6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal 4.226169e-001 -9.063084e-001 0.000000e+000 + outer loop + vertex -1.578468e-002 -6.554685e-003 7.300000e-002 + vertex -1.614014e-002 -6.720436e-003 8.300000e-002 + vertex -1.614014e-002 -6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal 5.735781e-001 -8.191509e-001 0.000000e+000 + outer loop + vertex -1.614014e-002 -6.720436e-003 7.300000e-002 + vertex -1.614014e-002 -6.720436e-003 8.300000e-002 + vertex -1.646141e-002 -6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 5.735781e-001 -8.191509e-001 -3.472114e-018 + outer loop + vertex -1.614014e-002 -6.720436e-003 7.300000e-002 + vertex -1.646141e-002 -6.945393e-003 8.300000e-002 + vertex -1.646141e-002 -6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 7.071050e-001 -7.071086e-001 0.000000e+000 + outer loop + vertex -1.646141e-002 -6.945393e-003 7.300000e-002 + vertex -1.646141e-002 -6.945393e-003 8.300000e-002 + vertex -1.673874e-002 -7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 7.071050e-001 -7.071086e-001 0.000000e+000 + outer loop + vertex -1.646141e-002 -6.945393e-003 7.300000e-002 + vertex -1.673874e-002 -7.222721e-003 8.300000e-002 + vertex -1.673874e-002 -7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex -1.673874e-002 -7.222721e-003 7.300000e-002 + vertex -1.673874e-002 -7.222721e-003 8.300000e-002 + vertex -1.696369e-002 -7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex -1.673874e-002 -7.222721e-003 7.300000e-002 + vertex -1.696369e-002 -7.543993e-003 8.300000e-002 + vertex -1.696369e-002 -7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 9.063078e-001 -4.226182e-001 0.000000e+000 + outer loop + vertex -1.696369e-002 -7.543993e-003 7.300000e-002 + vertex -1.696369e-002 -7.543993e-003 8.300000e-002 + vertex -1.712945e-002 -7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 9.063078e-001 -4.226182e-001 -1.736057e-018 + outer loop + vertex -1.696369e-002 -7.543993e-003 7.300000e-002 + vertex -1.712945e-002 -7.899447e-003 8.300000e-002 + vertex -1.712945e-002 -7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex -1.712945e-002 -7.899447e-003 7.300000e-002 + vertex -1.712945e-002 -7.899447e-003 8.300000e-002 + vertex -1.723095e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex -1.712945e-002 -7.899447e-003 7.300000e-002 + vertex -1.723095e-002 -8.278284e-003 8.300000e-002 + vertex -1.723095e-002 -8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 9.961950e-001 -8.715281e-002 0.000000e+000 + outer loop + vertex -1.723095e-002 -8.278284e-003 7.300000e-002 + vertex -1.723095e-002 -8.278284e-003 8.300000e-002 + vertex -1.726514e-002 -8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 9.961950e-001 -8.715281e-002 -4.340143e-019 + outer loop + vertex -1.723095e-002 -8.278284e-003 7.300000e-002 + vertex -1.726514e-002 -8.668993e-003 8.300000e-002 + vertex -1.726514e-002 -8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal 9.961950e-001 8.715281e-002 -0.000000e+000 + outer loop + vertex -1.726514e-002 -8.668993e-003 7.300000e-002 + vertex -1.726514e-002 -8.668993e-003 8.300000e-002 + vertex -1.723095e-002 -9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 9.961950e-001 8.715281e-002 4.340143e-019 + outer loop + vertex -1.726514e-002 -8.668993e-003 7.300000e-002 + vertex -1.723095e-002 -9.059701e-003 8.300000e-002 + vertex -1.723095e-002 -9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 2.588179e-001 -0.000000e+000 + outer loop + vertex -1.723095e-002 -9.059701e-003 7.300000e-002 + vertex -1.723095e-002 -9.059701e-003 8.300000e-002 + vertex -1.712945e-002 -9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex -1.723095e-002 -9.059701e-003 7.300000e-002 + vertex -1.712945e-002 -9.438538e-003 8.300000e-002 + vertex -1.712945e-002 -9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal 9.063080e-001 4.226178e-001 -0.000000e+000 + outer loop + vertex -1.712945e-002 -9.438538e-003 7.300000e-002 + vertex -1.712945e-002 -9.438538e-003 8.300000e-002 + vertex -1.696369e-002 -9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 9.063080e-001 4.226178e-001 0.000000e+000 + outer loop + vertex -1.712945e-002 -9.438538e-003 7.300000e-002 + vertex -1.696369e-002 -9.793993e-003 8.300000e-002 + vertex -1.696369e-002 -9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 5.735769e-001 -0.000000e+000 + outer loop + vertex -1.696369e-002 -9.793993e-003 7.300000e-002 + vertex -1.696369e-002 -9.793993e-003 8.300000e-002 + vertex -1.673874e-002 -1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex -1.696369e-002 -9.793993e-003 7.300000e-002 + vertex -1.673874e-002 -1.011526e-002 8.300000e-002 + vertex -1.673874e-002 -1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal 7.071056e-001 7.071080e-001 -0.000000e+000 + outer loop + vertex -1.673874e-002 -1.011526e-002 7.300000e-002 + vertex -1.673874e-002 -1.011526e-002 8.300000e-002 + vertex -1.646141e-002 -1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal 7.071056e-001 7.071080e-001 0.000000e+000 + outer loop + vertex -1.673874e-002 -1.011526e-002 7.300000e-002 + vertex -1.646141e-002 -1.039259e-002 8.300000e-002 + vertex -1.646141e-002 -1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 5.735781e-001 8.191509e-001 -0.000000e+000 + outer loop + vertex -1.646141e-002 -1.039259e-002 7.300000e-002 + vertex -1.646141e-002 -1.039259e-002 8.300000e-002 + vertex -1.614014e-002 -1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal 5.735781e-001 8.191509e-001 3.472114e-018 + outer loop + vertex -1.646141e-002 -1.039259e-002 7.300000e-002 + vertex -1.614014e-002 -1.061755e-002 8.300000e-002 + vertex -1.614014e-002 -1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal 4.226169e-001 9.063084e-001 -0.000000e+000 + outer loop + vertex -1.614014e-002 -1.061755e-002 7.300000e-002 + vertex -1.614014e-002 -1.061755e-002 8.300000e-002 + vertex -1.578468e-002 -1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 4.226169e-001 9.063084e-001 0.000000e+000 + outer loop + vertex -1.614014e-002 -1.061755e-002 7.300000e-002 + vertex -1.578468e-002 -1.078330e-002 8.300000e-002 + vertex -1.578468e-002 -1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal 2.588185e-001 9.659260e-001 -0.000000e+000 + outer loop + vertex -1.578468e-002 -1.078330e-002 7.300000e-002 + vertex -1.578468e-002 -1.078330e-002 8.300000e-002 + vertex -1.540584e-002 -1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal 2.588185e-001 9.659260e-001 -1.736057e-018 + outer loop + vertex -1.578468e-002 -1.078330e-002 7.300000e-002 + vertex -1.540584e-002 -1.088481e-002 8.300000e-002 + vertex -1.540584e-002 -1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal 8.715517e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex -1.540584e-002 -1.088481e-002 7.300000e-002 + vertex -1.540584e-002 -1.088481e-002 8.300000e-002 + vertex -1.501514e-002 -1.091899e-002 8.300000e-002 + endloop + endfacet + facet normal 8.715517e-002 9.961947e-001 4.340143e-019 + outer loop + vertex -1.540584e-002 -1.088481e-002 7.300000e-002 + vertex -1.501514e-002 -1.091899e-002 8.300000e-002 + vertex -1.501514e-002 -1.091899e-002 7.300000e-002 + endloop + endfacet + facet normal -8.715517e-002 9.961947e-001 0.000000e+000 + outer loop + vertex -1.501514e-002 -1.091899e-002 7.300000e-002 + vertex -1.501514e-002 -1.091899e-002 8.300000e-002 + vertex -1.462443e-002 -1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal -8.715517e-002 9.961947e-001 -4.340143e-019 + outer loop + vertex -1.501514e-002 -1.091899e-002 7.300000e-002 + vertex -1.462443e-002 -1.088481e-002 8.300000e-002 + vertex -1.462443e-002 -1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 9.659261e-001 0.000000e+000 + outer loop + vertex -1.462443e-002 -1.088481e-002 7.300000e-002 + vertex -1.462443e-002 -1.088481e-002 8.300000e-002 + vertex -1.424559e-002 -1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 9.659261e-001 0.000000e+000 + outer loop + vertex -1.462443e-002 -1.088481e-002 7.300000e-002 + vertex -1.424559e-002 -1.078330e-002 8.300000e-002 + vertex -1.424559e-002 -1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal -4.226178e-001 9.063080e-001 0.000000e+000 + outer loop + vertex -1.424559e-002 -1.078330e-002 7.300000e-002 + vertex -1.424559e-002 -1.078330e-002 8.300000e-002 + vertex -1.389014e-002 -1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal -4.226178e-001 9.063080e-001 0.000000e+000 + outer loop + vertex -1.424559e-002 -1.078330e-002 7.300000e-002 + vertex -1.389014e-002 -1.061755e-002 8.300000e-002 + vertex -1.389014e-002 -1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal -5.735769e-001 8.191517e-001 0.000000e+000 + outer loop + vertex -1.389014e-002 -1.061755e-002 7.300000e-002 + vertex -1.389014e-002 -1.061755e-002 8.300000e-002 + vertex -1.356886e-002 -1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal -5.735769e-001 8.191517e-001 0.000000e+000 + outer loop + vertex -1.389014e-002 -1.061755e-002 7.300000e-002 + vertex -1.356886e-002 -1.039259e-002 8.300000e-002 + vertex -1.356886e-002 -1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -1.356886e-002 -1.039259e-002 7.300000e-002 + vertex -1.356886e-002 -1.039259e-002 8.300000e-002 + vertex -1.329154e-002 -1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal -7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex -1.356886e-002 -1.039259e-002 7.300000e-002 + vertex -1.329154e-002 -1.011526e-002 8.300000e-002 + vertex -1.329154e-002 -1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex -1.329154e-002 -1.011526e-002 7.300000e-002 + vertex -1.329154e-002 -1.011526e-002 8.300000e-002 + vertex -1.306658e-002 -9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex -1.329154e-002 -1.011526e-002 7.300000e-002 + vertex -1.306658e-002 -9.793993e-003 8.300000e-002 + vertex -1.306658e-002 -9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal -9.063080e-001 4.226178e-001 0.000000e+000 + outer loop + vertex -1.306658e-002 -9.793993e-003 7.300000e-002 + vertex -1.306658e-002 -9.793993e-003 8.300000e-002 + vertex -1.290083e-002 -9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal -9.063080e-001 4.226178e-001 0.000000e+000 + outer loop + vertex -1.306658e-002 -9.793993e-003 7.300000e-002 + vertex -1.290083e-002 -9.438538e-003 8.300000e-002 + vertex -1.290083e-002 -9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex -1.290083e-002 -9.438538e-003 7.300000e-002 + vertex -1.290083e-002 -9.438538e-003 8.300000e-002 + vertex -1.279932e-002 -9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex -1.290083e-002 -9.438538e-003 7.300000e-002 + vertex -1.279932e-002 -9.059701e-003 8.300000e-002 + vertex -1.279932e-002 -9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715517e-002 0.000000e+000 + outer loop + vertex -1.279932e-002 -9.059701e-003 7.300000e-002 + vertex -1.279932e-002 -9.059701e-003 8.300000e-002 + vertex -1.276514e-002 -8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715576e-002 4.340143e-019 + outer loop + vertex 2.215817e-003 -1.772869e-002 7.300000e-002 + vertex 2.250000e-003 -1.733799e-002 8.300000e-002 + vertex 2.250000e-003 -1.733799e-002 7.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715576e-002 0.000000e+000 + outer loop + vertex 2.250000e-003 -1.733799e-002 7.300000e-002 + vertex 2.250000e-003 -1.733799e-002 8.300000e-002 + vertex 2.215817e-003 -1.694728e-002 8.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 -8.715576e-002 -4.340143e-019 + outer loop + vertex 2.250000e-003 -1.733799e-002 7.300000e-002 + vertex 2.215817e-003 -1.694728e-002 8.300000e-002 + vertex 2.215817e-003 -1.694728e-002 7.300000e-002 + endloop + endfacet + facet normal -9.659257e-001 -2.588196e-001 0.000000e+000 + outer loop + vertex 2.215817e-003 -1.694728e-002 7.300000e-002 + vertex 2.215817e-003 -1.694728e-002 8.300000e-002 + vertex 2.114308e-003 -1.656844e-002 8.300000e-002 + endloop + endfacet + facet normal -9.659257e-001 -2.588196e-001 -1.736057e-018 + outer loop + vertex 2.215817e-003 -1.694728e-002 7.300000e-002 + vertex 2.114308e-003 -1.656844e-002 8.300000e-002 + vertex 2.114308e-003 -1.656844e-002 7.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex 2.114308e-003 -1.656844e-002 7.300000e-002 + vertex 2.114308e-003 -1.656844e-002 8.300000e-002 + vertex 1.948557e-003 -1.621299e-002 8.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 -4.226184e-001 1.736057e-018 + outer loop + vertex 2.114308e-003 -1.656844e-002 7.300000e-002 + vertex 1.948557e-003 -1.621299e-002 8.300000e-002 + vertex 1.948557e-003 -1.621299e-002 7.300000e-002 + endloop + endfacet + facet normal -8.191525e-001 -5.735758e-001 0.000000e+000 + outer loop + vertex 1.948557e-003 -1.621299e-002 7.300000e-002 + vertex 1.948557e-003 -1.621299e-002 8.300000e-002 + vertex 1.723600e-003 -1.589171e-002 8.300000e-002 + endloop + endfacet + facet normal -8.191525e-001 -5.735758e-001 3.472114e-018 + outer loop + vertex 1.948557e-003 -1.621299e-002 7.300000e-002 + vertex 1.723600e-003 -1.589171e-002 8.300000e-002 + vertex 1.723600e-003 -1.589171e-002 7.300000e-002 + endloop + endfacet + facet normal -7.071074e-001 -7.071062e-001 0.000000e+000 + outer loop + vertex 1.723600e-003 -1.589171e-002 7.300000e-002 + vertex 1.723600e-003 -1.589171e-002 8.300000e-002 + vertex 1.446272e-003 -1.561439e-002 8.300000e-002 + endloop + endfacet + facet normal -7.071074e-001 -7.071062e-001 -3.472114e-018 + outer loop + vertex 1.723600e-003 -1.589171e-002 7.300000e-002 + vertex 1.446272e-003 -1.561439e-002 8.300000e-002 + vertex 1.446272e-003 -1.561439e-002 7.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 -8.191520e-001 0.000000e+000 + outer loop + vertex 1.446272e-003 -1.561439e-002 7.300000e-002 + vertex 1.446272e-003 -1.561439e-002 8.300000e-002 + vertex 1.125000e-003 -1.538943e-002 8.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 -8.191520e-001 0.000000e+000 + outer loop + vertex 1.446272e-003 -1.561439e-002 7.300000e-002 + vertex 1.125000e-003 -1.538943e-002 8.300000e-002 + vertex 1.125000e-003 -1.538943e-002 7.300000e-002 + endloop + endfacet + facet normal -4.226182e-001 -9.063078e-001 0.000000e+000 + outer loop + vertex 1.125000e-003 -1.538943e-002 7.300000e-002 + vertex 1.125000e-003 -1.538943e-002 8.300000e-002 + vertex 7.695453e-004 -1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal -4.226182e-001 -9.063078e-001 -1.736057e-018 + outer loop + vertex 1.125000e-003 -1.538943e-002 7.300000e-002 + vertex 7.695453e-004 -1.522368e-002 8.300000e-002 + vertex 7.695453e-004 -1.522368e-002 7.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 -9.659261e-001 0.000000e+000 + outer loop + vertex 7.695453e-004 -1.522368e-002 7.300000e-002 + vertex 7.695453e-004 -1.522368e-002 8.300000e-002 + vertex 3.907084e-004 -1.512217e-002 8.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 -9.659261e-001 -1.736057e-018 + outer loop + vertex 7.695453e-004 -1.522368e-002 7.300000e-002 + vertex 3.907084e-004 -1.512217e-002 8.300000e-002 + vertex 3.907084e-004 -1.512217e-002 7.300000e-002 + endloop + endfacet + facet normal -8.715754e-002 -9.961945e-001 0.000000e+000 + outer loop + vertex 3.907084e-004 -1.512217e-002 7.300000e-002 + vertex 3.907084e-004 -1.512217e-002 8.300000e-002 + vertex 1.377728e-019 -1.508799e-002 8.300000e-002 + endloop + endfacet + facet normal -8.715754e-002 -9.961945e-001 0.000000e+000 + outer loop + vertex 3.907084e-004 -1.512217e-002 7.300000e-002 + vertex 1.377728e-019 -1.508799e-002 8.300000e-002 + vertex 1.377728e-019 -1.508799e-002 7.300000e-002 + endloop + endfacet + facet normal 8.715754e-002 -9.961945e-001 0.000000e+000 + outer loop + vertex 1.377728e-019 -1.508799e-002 7.300000e-002 + vertex 1.377728e-019 -1.508799e-002 8.300000e-002 + vertex -3.907084e-004 -1.512217e-002 8.300000e-002 + endloop + endfacet + facet normal 8.715754e-002 -9.961945e-001 0.000000e+000 + outer loop + vertex 1.377728e-019 -1.508799e-002 7.300000e-002 + vertex -3.907084e-004 -1.512217e-002 8.300000e-002 + vertex -3.907084e-004 -1.512217e-002 7.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 -9.659261e-001 0.000000e+000 + outer loop + vertex -3.907084e-004 -1.512217e-002 7.300000e-002 + vertex -3.907084e-004 -1.512217e-002 8.300000e-002 + vertex -7.695453e-004 -1.522368e-002 8.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 -9.659261e-001 1.736057e-018 + outer loop + vertex -3.907084e-004 -1.512217e-002 7.300000e-002 + vertex -7.695453e-004 -1.522368e-002 8.300000e-002 + vertex -7.695453e-004 -1.522368e-002 7.300000e-002 + endloop + endfacet + facet normal 4.226182e-001 -9.063078e-001 0.000000e+000 + outer loop + vertex -7.695453e-004 -1.522368e-002 7.300000e-002 + vertex -7.695453e-004 -1.522368e-002 8.300000e-002 + vertex -1.125000e-003 -1.538943e-002 8.300000e-002 + endloop + endfacet + facet normal 4.226182e-001 -9.063078e-001 1.736057e-018 + outer loop + vertex -7.695453e-004 -1.522368e-002 7.300000e-002 + vertex -1.125000e-003 -1.538943e-002 8.300000e-002 + vertex -1.125000e-003 -1.538943e-002 7.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 -8.191520e-001 0.000000e+000 + outer loop + vertex -1.125000e-003 -1.538943e-002 7.300000e-002 + vertex -1.125000e-003 -1.538943e-002 8.300000e-002 + vertex -1.446272e-003 -1.561439e-002 8.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 -8.191520e-001 0.000000e+000 + outer loop + vertex -1.125000e-003 -1.538943e-002 7.300000e-002 + vertex -1.446272e-003 -1.561439e-002 8.300000e-002 + vertex -1.446272e-003 -1.561439e-002 7.300000e-002 + endloop + endfacet + facet normal 7.071074e-001 -7.071062e-001 0.000000e+000 + outer loop + vertex -1.446272e-003 -1.561439e-002 7.300000e-002 + vertex -1.446272e-003 -1.561439e-002 8.300000e-002 + vertex -1.723600e-003 -1.589171e-002 8.300000e-002 + endloop + endfacet + facet normal 7.071074e-001 -7.071062e-001 3.472114e-018 + outer loop + vertex -1.446272e-003 -1.561439e-002 7.300000e-002 + vertex -1.723600e-003 -1.589171e-002 8.300000e-002 + vertex -1.723600e-003 -1.589171e-002 7.300000e-002 + endloop + endfacet + facet normal 8.191525e-001 -5.735758e-001 0.000000e+000 + outer loop + vertex -1.723600e-003 -1.589171e-002 7.300000e-002 + vertex -1.723600e-003 -1.589171e-002 8.300000e-002 + vertex -1.948557e-003 -1.621299e-002 8.300000e-002 + endloop + endfacet + facet normal 8.191525e-001 -5.735758e-001 -3.472114e-018 + outer loop + vertex -1.723600e-003 -1.589171e-002 7.300000e-002 + vertex -1.948557e-003 -1.621299e-002 8.300000e-002 + vertex -1.948557e-003 -1.621299e-002 7.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 0.000000e+000 + outer loop + vertex -1.948557e-003 -1.621299e-002 7.300000e-002 + vertex -1.948557e-003 -1.621299e-002 8.300000e-002 + vertex -2.114308e-003 -1.656844e-002 8.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 -4.226184e-001 -1.736057e-018 + outer loop + vertex -1.948557e-003 -1.621299e-002 7.300000e-002 + vertex -2.114308e-003 -1.656844e-002 8.300000e-002 + vertex -2.114308e-003 -1.656844e-002 7.300000e-002 + endloop + endfacet + facet normal 9.659257e-001 -2.588196e-001 0.000000e+000 + outer loop + vertex -2.114308e-003 -1.656844e-002 7.300000e-002 + vertex -2.114308e-003 -1.656844e-002 8.300000e-002 + vertex -2.215817e-003 -1.694728e-002 8.300000e-002 + endloop + endfacet + facet normal 9.659257e-001 -2.588196e-001 1.736057e-018 + outer loop + vertex -2.114308e-003 -1.656844e-002 7.300000e-002 + vertex -2.215817e-003 -1.694728e-002 8.300000e-002 + vertex -2.215817e-003 -1.694728e-002 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715576e-002 0.000000e+000 + outer loop + vertex -2.215817e-003 -1.694728e-002 7.300000e-002 + vertex -2.215817e-003 -1.694728e-002 8.300000e-002 + vertex -2.250000e-003 -1.733799e-002 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715576e-002 4.340143e-019 + outer loop + vertex -2.215817e-003 -1.694728e-002 7.300000e-002 + vertex -2.250000e-003 -1.733799e-002 8.300000e-002 + vertex -2.250000e-003 -1.733799e-002 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715576e-002 -0.000000e+000 + outer loop + vertex -2.250000e-003 -1.733799e-002 7.300000e-002 + vertex -2.250000e-003 -1.733799e-002 8.300000e-002 + vertex -2.215817e-003 -1.772869e-002 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715576e-002 -4.340143e-019 + outer loop + vertex -2.250000e-003 -1.733799e-002 7.300000e-002 + vertex -2.215817e-003 -1.772869e-002 8.300000e-002 + vertex -2.215817e-003 -1.772869e-002 7.300000e-002 + endloop + endfacet + facet normal 9.659260e-001 2.588184e-001 -0.000000e+000 + outer loop + vertex -2.215817e-003 -1.772869e-002 7.300000e-002 + vertex -2.215817e-003 -1.772869e-002 8.300000e-002 + vertex -2.114308e-003 -1.810753e-002 8.300000e-002 + endloop + endfacet + facet normal 9.659260e-001 2.588184e-001 1.736057e-018 + outer loop + vertex -2.215817e-003 -1.772869e-002 7.300000e-002 + vertex -2.114308e-003 -1.810753e-002 8.300000e-002 + vertex -2.114308e-003 -1.810753e-002 7.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 -0.000000e+000 + outer loop + vertex -2.114308e-003 -1.810753e-002 7.300000e-002 + vertex -2.114308e-003 -1.810753e-002 8.300000e-002 + vertex -1.948557e-003 -1.846299e-002 8.300000e-002 + endloop + endfacet + facet normal 9.063077e-001 4.226184e-001 1.736057e-018 + outer loop + vertex -2.114308e-003 -1.810753e-002 7.300000e-002 + vertex -1.948557e-003 -1.846299e-002 8.300000e-002 + vertex -1.948557e-003 -1.846299e-002 7.300000e-002 + endloop + endfacet + facet normal 8.191525e-001 5.735758e-001 -0.000000e+000 + outer loop + vertex -1.948557e-003 -1.846299e-002 7.300000e-002 + vertex -1.948557e-003 -1.846299e-002 8.300000e-002 + vertex -1.723600e-003 -1.878426e-002 8.300000e-002 + endloop + endfacet + facet normal 8.191525e-001 5.735758e-001 3.472114e-018 + outer loop + vertex -1.948557e-003 -1.846299e-002 7.300000e-002 + vertex -1.723600e-003 -1.878426e-002 8.300000e-002 + vertex -1.723600e-003 -1.878426e-002 7.300000e-002 + endloop + endfacet + facet normal 7.071062e-001 7.071074e-001 -0.000000e+000 + outer loop + vertex -1.723600e-003 -1.878426e-002 7.300000e-002 + vertex -1.723600e-003 -1.878426e-002 8.300000e-002 + vertex -1.446272e-003 -1.906159e-002 8.300000e-002 + endloop + endfacet + facet normal 7.071062e-001 7.071074e-001 -3.472114e-018 + outer loop + vertex -1.723600e-003 -1.878426e-002 7.300000e-002 + vertex -1.446272e-003 -1.906159e-002 8.300000e-002 + vertex -1.446272e-003 -1.906159e-002 7.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 8.191520e-001 -0.000000e+000 + outer loop + vertex -1.446272e-003 -1.906159e-002 7.300000e-002 + vertex -1.446272e-003 -1.906159e-002 8.300000e-002 + vertex -1.125000e-003 -1.928654e-002 8.300000e-002 + endloop + endfacet + facet normal 5.735765e-001 8.191520e-001 0.000000e+000 + outer loop + vertex -1.446272e-003 -1.906159e-002 7.300000e-002 + vertex -1.125000e-003 -1.928654e-002 8.300000e-002 + vertex -1.125000e-003 -1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal 4.226182e-001 9.063078e-001 -0.000000e+000 + outer loop + vertex -1.125000e-003 -1.928654e-002 7.300000e-002 + vertex -1.125000e-003 -1.928654e-002 8.300000e-002 + vertex -7.695453e-004 -1.945229e-002 8.300000e-002 + endloop + endfacet + facet normal 4.226182e-001 9.063078e-001 -1.736057e-018 + outer loop + vertex -1.125000e-003 -1.928654e-002 7.300000e-002 + vertex -7.695453e-004 -1.945229e-002 8.300000e-002 + vertex -7.695453e-004 -1.945229e-002 7.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 9.659261e-001 -0.000000e+000 + outer loop + vertex -7.695453e-004 -1.945229e-002 7.300000e-002 + vertex -7.695453e-004 -1.945229e-002 8.300000e-002 + vertex -3.907084e-004 -1.955380e-002 8.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 9.659261e-001 -1.736057e-018 + outer loop + vertex -7.695453e-004 -1.945229e-002 7.300000e-002 + vertex -3.907084e-004 -1.955380e-002 8.300000e-002 + vertex -3.907084e-004 -1.955380e-002 7.300000e-002 + endloop + endfacet + facet normal 8.715754e-002 9.961945e-001 -0.000000e+000 + outer loop + vertex -3.907084e-004 -1.955380e-002 7.300000e-002 + vertex -3.907084e-004 -1.955380e-002 8.300000e-002 + vertex -4.133183e-019 -1.958799e-002 8.300000e-002 + endloop + endfacet + facet normal 8.715754e-002 9.961945e-001 0.000000e+000 + outer loop + vertex -3.907084e-004 -1.955380e-002 7.300000e-002 + vertex -4.133183e-019 -1.958799e-002 8.300000e-002 + vertex -4.133183e-019 -1.958799e-002 7.300000e-002 + endloop + endfacet + facet normal -8.715754e-002 9.961945e-001 0.000000e+000 + outer loop + vertex -4.133183e-019 -1.958799e-002 7.300000e-002 + vertex -4.133183e-019 -1.958799e-002 8.300000e-002 + vertex 3.907084e-004 -1.955380e-002 8.300000e-002 + endloop + endfacet + facet normal -8.715754e-002 9.961945e-001 0.000000e+000 + outer loop + vertex -4.133183e-019 -1.958799e-002 7.300000e-002 + vertex 3.907084e-004 -1.955380e-002 8.300000e-002 + vertex 3.907084e-004 -1.955380e-002 7.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 9.659261e-001 0.000000e+000 + outer loop + vertex 3.907084e-004 -1.955380e-002 7.300000e-002 + vertex 3.907084e-004 -1.955380e-002 8.300000e-002 + vertex 7.695453e-004 -1.945229e-002 8.300000e-002 + endloop + endfacet + facet normal -2.588179e-001 9.659261e-001 1.736057e-018 + outer loop + vertex 3.907084e-004 -1.955380e-002 7.300000e-002 + vertex 7.695453e-004 -1.945229e-002 8.300000e-002 + vertex 7.695453e-004 -1.945229e-002 7.300000e-002 + endloop + endfacet + facet normal -4.226182e-001 9.063078e-001 0.000000e+000 + outer loop + vertex 7.695453e-004 -1.945229e-002 7.300000e-002 + vertex 7.695453e-004 -1.945229e-002 8.300000e-002 + vertex 1.125000e-003 -1.928654e-002 8.300000e-002 + endloop + endfacet + facet normal -4.226182e-001 9.063078e-001 1.736057e-018 + outer loop + vertex 7.695453e-004 -1.945229e-002 7.300000e-002 + vertex 1.125000e-003 -1.928654e-002 8.300000e-002 + vertex 1.125000e-003 -1.928654e-002 7.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 8.191520e-001 0.000000e+000 + outer loop + vertex 1.125000e-003 -1.928654e-002 7.300000e-002 + vertex 1.125000e-003 -1.928654e-002 8.300000e-002 + vertex 1.446272e-003 -1.906159e-002 8.300000e-002 + endloop + endfacet + facet normal -5.735765e-001 8.191520e-001 0.000000e+000 + outer loop + vertex 1.125000e-003 -1.928654e-002 7.300000e-002 + vertex 1.446272e-003 -1.906159e-002 8.300000e-002 + vertex 1.446272e-003 -1.906159e-002 7.300000e-002 + endloop + endfacet + facet normal -7.071062e-001 7.071074e-001 0.000000e+000 + outer loop + vertex 1.446272e-003 -1.906159e-002 7.300000e-002 + vertex 1.446272e-003 -1.906159e-002 8.300000e-002 + vertex 1.723600e-003 -1.878426e-002 8.300000e-002 + endloop + endfacet + facet normal -7.071062e-001 7.071074e-001 3.472114e-018 + outer loop + vertex 1.446272e-003 -1.906159e-002 7.300000e-002 + vertex 1.723600e-003 -1.878426e-002 8.300000e-002 + vertex 1.723600e-003 -1.878426e-002 7.300000e-002 + endloop + endfacet + facet normal -8.191525e-001 5.735758e-001 0.000000e+000 + outer loop + vertex 1.723600e-003 -1.878426e-002 7.300000e-002 + vertex 1.723600e-003 -1.878426e-002 8.300000e-002 + vertex 1.948557e-003 -1.846299e-002 8.300000e-002 + endloop + endfacet + facet normal -8.191525e-001 5.735758e-001 -3.472114e-018 + outer loop + vertex 1.723600e-003 -1.878426e-002 7.300000e-002 + vertex 1.948557e-003 -1.846299e-002 8.300000e-002 + vertex 1.948557e-003 -1.846299e-002 7.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 0.000000e+000 + outer loop + vertex 1.948557e-003 -1.846299e-002 7.300000e-002 + vertex 1.948557e-003 -1.846299e-002 8.300000e-002 + vertex 2.114308e-003 -1.810753e-002 8.300000e-002 + endloop + endfacet + facet normal -9.063077e-001 4.226184e-001 -1.736057e-018 + outer loop + vertex 1.948557e-003 -1.846299e-002 7.300000e-002 + vertex 2.114308e-003 -1.810753e-002 8.300000e-002 + vertex 2.114308e-003 -1.810753e-002 7.300000e-002 + endloop + endfacet + facet normal -9.659260e-001 2.588184e-001 0.000000e+000 + outer loop + vertex 2.114308e-003 -1.810753e-002 7.300000e-002 + vertex 2.114308e-003 -1.810753e-002 8.300000e-002 + vertex 2.215817e-003 -1.772869e-002 8.300000e-002 + endloop + endfacet + facet normal -9.659260e-001 2.588184e-001 -1.736057e-018 + outer loop + vertex 2.114308e-003 -1.810753e-002 7.300000e-002 + vertex 2.215817e-003 -1.772869e-002 8.300000e-002 + vertex 2.215817e-003 -1.772869e-002 7.300000e-002 + endloop + endfacet + facet normal -9.961947e-001 8.715576e-002 0.000000e+000 + outer loop + vertex 2.215817e-003 -1.772869e-002 7.300000e-002 + vertex 2.215817e-003 -1.772869e-002 8.300000e-002 + vertex 2.250000e-003 -1.733799e-002 8.300000e-002 + endloop + endfacet + facet normal -9.961950e-001 8.715281e-002 -4.340143e-019 + outer loop + vertex 1.723095e-002 -9.059701e-003 7.300000e-002 + vertex 1.726514e-002 -8.668993e-003 8.300000e-002 + vertex 1.726514e-002 -8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal -9.961950e-001 -8.715281e-002 0.000000e+000 + outer loop + vertex 1.726514e-002 -8.668993e-003 7.300000e-002 + vertex 1.726514e-002 -8.668993e-003 8.300000e-002 + vertex 1.723095e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal -9.961950e-001 -8.715281e-002 4.340143e-019 + outer loop + vertex 1.726514e-002 -8.668993e-003 7.300000e-002 + vertex 1.723095e-002 -8.278284e-003 8.300000e-002 + vertex 1.723095e-002 -8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex 1.723095e-002 -8.278284e-003 7.300000e-002 + vertex 1.723095e-002 -8.278284e-003 8.300000e-002 + vertex 1.712945e-002 -7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex 1.723095e-002 -8.278284e-003 7.300000e-002 + vertex 1.712945e-002 -7.899447e-003 8.300000e-002 + vertex 1.712945e-002 -7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal -9.063078e-001 -4.226182e-001 0.000000e+000 + outer loop + vertex 1.712945e-002 -7.899447e-003 7.300000e-002 + vertex 1.712945e-002 -7.899447e-003 8.300000e-002 + vertex 1.696369e-002 -7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal -9.063078e-001 -4.226182e-001 1.736057e-018 + outer loop + vertex 1.712945e-002 -7.899447e-003 7.300000e-002 + vertex 1.696369e-002 -7.543993e-003 8.300000e-002 + vertex 1.696369e-002 -7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex 1.696369e-002 -7.543993e-003 7.300000e-002 + vertex 1.696369e-002 -7.543993e-003 8.300000e-002 + vertex 1.673874e-002 -7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex 1.696369e-002 -7.543993e-003 7.300000e-002 + vertex 1.673874e-002 -7.222721e-003 8.300000e-002 + vertex 1.673874e-002 -7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal -7.071050e-001 -7.071086e-001 0.000000e+000 + outer loop + vertex 1.673874e-002 -7.222721e-003 7.300000e-002 + vertex 1.673874e-002 -7.222721e-003 8.300000e-002 + vertex 1.646141e-002 -6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal -7.071050e-001 -7.071086e-001 0.000000e+000 + outer loop + vertex 1.673874e-002 -7.222721e-003 7.300000e-002 + vertex 1.646141e-002 -6.945393e-003 8.300000e-002 + vertex 1.646141e-002 -6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal -5.735781e-001 -8.191509e-001 0.000000e+000 + outer loop + vertex 1.646141e-002 -6.945393e-003 7.300000e-002 + vertex 1.646141e-002 -6.945393e-003 8.300000e-002 + vertex 1.614014e-002 -6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal -5.735781e-001 -8.191509e-001 3.472114e-018 + outer loop + vertex 1.646141e-002 -6.945393e-003 7.300000e-002 + vertex 1.614014e-002 -6.720436e-003 8.300000e-002 + vertex 1.614014e-002 -6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal -4.226169e-001 -9.063084e-001 0.000000e+000 + outer loop + vertex 1.614014e-002 -6.720436e-003 7.300000e-002 + vertex 1.614014e-002 -6.720436e-003 8.300000e-002 + vertex 1.578468e-002 -6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal -4.226169e-001 -9.063084e-001 0.000000e+000 + outer loop + vertex 1.614014e-002 -6.720436e-003 7.300000e-002 + vertex 1.578468e-002 -6.554685e-003 8.300000e-002 + vertex 1.578468e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal -2.588196e-001 -9.659257e-001 0.000000e+000 + outer loop + vertex 1.578468e-002 -6.554685e-003 7.300000e-002 + vertex 1.578468e-002 -6.554685e-003 8.300000e-002 + vertex 1.540584e-002 -6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal -2.588196e-001 -9.659257e-001 1.736057e-018 + outer loop + vertex 1.578468e-002 -6.554685e-003 7.300000e-002 + vertex 1.540584e-002 -6.453176e-003 8.300000e-002 + vertex 1.540584e-002 -6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal -8.715635e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.540584e-002 -6.453176e-003 7.300000e-002 + vertex 1.540584e-002 -6.453176e-003 8.300000e-002 + vertex 1.501514e-002 -6.418993e-003 8.300000e-002 + endloop + endfacet + facet normal -8.715635e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.540584e-002 -6.453176e-003 7.300000e-002 + vertex 1.501514e-002 -6.418993e-003 8.300000e-002 + vertex 1.501514e-002 -6.418993e-003 7.300000e-002 + endloop + endfacet + facet normal 8.715635e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.501514e-002 -6.418993e-003 7.300000e-002 + vertex 1.501514e-002 -6.418993e-003 8.300000e-002 + vertex 1.462443e-002 -6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 8.715635e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.501514e-002 -6.418993e-003 7.300000e-002 + vertex 1.462443e-002 -6.453176e-003 8.300000e-002 + vertex 1.462443e-002 -6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 0.000000e+000 + outer loop + vertex 1.462443e-002 -6.453176e-003 7.300000e-002 + vertex 1.462443e-002 -6.453176e-003 8.300000e-002 + vertex 1.424559e-002 -6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 2.588190e-001 -9.659258e-001 1.736057e-018 + outer loop + vertex 1.462443e-002 -6.453176e-003 7.300000e-002 + vertex 1.424559e-002 -6.554685e-003 8.300000e-002 + vertex 1.424559e-002 -6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal 4.226178e-001 -9.063080e-001 0.000000e+000 + outer loop + vertex 1.424559e-002 -6.554685e-003 7.300000e-002 + vertex 1.424559e-002 -6.554685e-003 8.300000e-002 + vertex 1.389014e-002 -6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal 4.226178e-001 -9.063080e-001 0.000000e+000 + outer loop + vertex 1.424559e-002 -6.554685e-003 7.300000e-002 + vertex 1.389014e-002 -6.720436e-003 8.300000e-002 + vertex 1.389014e-002 -6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal 5.735769e-001 -8.191517e-001 0.000000e+000 + outer loop + vertex 1.389014e-002 -6.720436e-003 7.300000e-002 + vertex 1.389014e-002 -6.720436e-003 8.300000e-002 + vertex 1.356886e-002 -6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 5.735769e-001 -8.191517e-001 0.000000e+000 + outer loop + vertex 1.389014e-002 -6.720436e-003 7.300000e-002 + vertex 1.356886e-002 -6.945393e-003 8.300000e-002 + vertex 1.356886e-002 -6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 7.071062e-001 -7.071074e-001 0.000000e+000 + outer loop + vertex 1.356886e-002 -6.945393e-003 7.300000e-002 + vertex 1.356886e-002 -6.945393e-003 8.300000e-002 + vertex 1.329154e-002 -7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 7.071062e-001 -7.071074e-001 -3.472114e-018 + outer loop + vertex 1.356886e-002 -6.945393e-003 7.300000e-002 + vertex 1.329154e-002 -7.222721e-003 8.300000e-002 + vertex 1.329154e-002 -7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex 1.329154e-002 -7.222721e-003 7.300000e-002 + vertex 1.329154e-002 -7.222721e-003 8.300000e-002 + vertex 1.306658e-002 -7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex 1.329154e-002 -7.222721e-003 7.300000e-002 + vertex 1.306658e-002 -7.543993e-003 8.300000e-002 + vertex 1.306658e-002 -7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 9.063078e-001 -4.226182e-001 0.000000e+000 + outer loop + vertex 1.306658e-002 -7.543993e-003 7.300000e-002 + vertex 1.306658e-002 -7.543993e-003 8.300000e-002 + vertex 1.290083e-002 -7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 9.063078e-001 -4.226182e-001 -1.736057e-018 + outer loop + vertex 1.306658e-002 -7.543993e-003 7.300000e-002 + vertex 1.290083e-002 -7.899447e-003 8.300000e-002 + vertex 1.290083e-002 -7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex 1.290083e-002 -7.899447e-003 7.300000e-002 + vertex 1.290083e-002 -7.899447e-003 8.300000e-002 + vertex 1.279932e-002 -8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex 1.290083e-002 -7.899447e-003 7.300000e-002 + vertex 1.279932e-002 -8.278284e-003 8.300000e-002 + vertex 1.279932e-002 -8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715517e-002 0.000000e+000 + outer loop + vertex 1.279932e-002 -8.278284e-003 7.300000e-002 + vertex 1.279932e-002 -8.278284e-003 8.300000e-002 + vertex 1.276514e-002 -8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715517e-002 4.340143e-019 + outer loop + vertex 1.279932e-002 -8.278284e-003 7.300000e-002 + vertex 1.276514e-002 -8.668993e-003 8.300000e-002 + vertex 1.276514e-002 -8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715517e-002 -0.000000e+000 + outer loop + vertex 1.276514e-002 -8.668993e-003 7.300000e-002 + vertex 1.276514e-002 -8.668993e-003 8.300000e-002 + vertex 1.279932e-002 -9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715517e-002 -4.340143e-019 + outer loop + vertex 1.276514e-002 -8.668993e-003 7.300000e-002 + vertex 1.279932e-002 -9.059701e-003 8.300000e-002 + vertex 1.279932e-002 -9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 2.588179e-001 -0.000000e+000 + outer loop + vertex 1.279932e-002 -9.059701e-003 7.300000e-002 + vertex 1.279932e-002 -9.059701e-003 8.300000e-002 + vertex 1.290083e-002 -9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex 1.279932e-002 -9.059701e-003 7.300000e-002 + vertex 1.290083e-002 -9.438538e-003 8.300000e-002 + vertex 1.290083e-002 -9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal 9.063080e-001 4.226178e-001 -0.000000e+000 + outer loop + vertex 1.290083e-002 -9.438538e-003 7.300000e-002 + vertex 1.290083e-002 -9.438538e-003 8.300000e-002 + vertex 1.306658e-002 -9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 9.063080e-001 4.226178e-001 0.000000e+000 + outer loop + vertex 1.290083e-002 -9.438538e-003 7.300000e-002 + vertex 1.306658e-002 -9.793993e-003 8.300000e-002 + vertex 1.306658e-002 -9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 5.735769e-001 -0.000000e+000 + outer loop + vertex 1.306658e-002 -9.793993e-003 7.300000e-002 + vertex 1.306658e-002 -9.793993e-003 8.300000e-002 + vertex 1.329154e-002 -1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex 1.306658e-002 -9.793993e-003 7.300000e-002 + vertex 1.329154e-002 -1.011526e-002 8.300000e-002 + vertex 1.329154e-002 -1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 -0.000000e+000 + outer loop + vertex 1.329154e-002 -1.011526e-002 7.300000e-002 + vertex 1.329154e-002 -1.011526e-002 8.300000e-002 + vertex 1.356886e-002 -1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal 7.071068e-001 7.071068e-001 0.000000e+000 + outer loop + vertex 1.329154e-002 -1.011526e-002 7.300000e-002 + vertex 1.356886e-002 -1.039259e-002 8.300000e-002 + vertex 1.356886e-002 -1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 5.735769e-001 8.191517e-001 -0.000000e+000 + outer loop + vertex 1.356886e-002 -1.039259e-002 7.300000e-002 + vertex 1.356886e-002 -1.039259e-002 8.300000e-002 + vertex 1.389014e-002 -1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal 5.735769e-001 8.191517e-001 0.000000e+000 + outer loop + vertex 1.356886e-002 -1.039259e-002 7.300000e-002 + vertex 1.389014e-002 -1.061755e-002 8.300000e-002 + vertex 1.389014e-002 -1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal 4.226178e-001 9.063080e-001 -0.000000e+000 + outer loop + vertex 1.389014e-002 -1.061755e-002 7.300000e-002 + vertex 1.389014e-002 -1.061755e-002 8.300000e-002 + vertex 1.424559e-002 -1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 4.226178e-001 9.063080e-001 0.000000e+000 + outer loop + vertex 1.389014e-002 -1.061755e-002 7.300000e-002 + vertex 1.424559e-002 -1.078330e-002 8.300000e-002 + vertex 1.424559e-002 -1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 9.659261e-001 -0.000000e+000 + outer loop + vertex 1.424559e-002 -1.078330e-002 7.300000e-002 + vertex 1.424559e-002 -1.078330e-002 8.300000e-002 + vertex 1.462443e-002 -1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 9.659261e-001 0.000000e+000 + outer loop + vertex 1.424559e-002 -1.078330e-002 7.300000e-002 + vertex 1.462443e-002 -1.088481e-002 8.300000e-002 + vertex 1.462443e-002 -1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal 8.715517e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex 1.462443e-002 -1.088481e-002 7.300000e-002 + vertex 1.462443e-002 -1.088481e-002 8.300000e-002 + vertex 1.501514e-002 -1.091899e-002 8.300000e-002 + endloop + endfacet + facet normal 8.715517e-002 9.961947e-001 4.340143e-019 + outer loop + vertex 1.462443e-002 -1.088481e-002 7.300000e-002 + vertex 1.501514e-002 -1.091899e-002 8.300000e-002 + vertex 1.501514e-002 -1.091899e-002 7.300000e-002 + endloop + endfacet + facet normal -8.715517e-002 9.961947e-001 0.000000e+000 + outer loop + vertex 1.501514e-002 -1.091899e-002 7.300000e-002 + vertex 1.501514e-002 -1.091899e-002 8.300000e-002 + vertex 1.540584e-002 -1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal -8.715517e-002 9.961947e-001 -4.340143e-019 + outer loop + vertex 1.501514e-002 -1.091899e-002 7.300000e-002 + vertex 1.540584e-002 -1.088481e-002 8.300000e-002 + vertex 1.540584e-002 -1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal -2.588185e-001 9.659260e-001 0.000000e+000 + outer loop + vertex 1.540584e-002 -1.088481e-002 7.300000e-002 + vertex 1.540584e-002 -1.088481e-002 8.300000e-002 + vertex 1.578468e-002 -1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal -2.588185e-001 9.659260e-001 1.736057e-018 + outer loop + vertex 1.540584e-002 -1.088481e-002 7.300000e-002 + vertex 1.578468e-002 -1.078330e-002 8.300000e-002 + vertex 1.578468e-002 -1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal -4.226169e-001 9.063084e-001 0.000000e+000 + outer loop + vertex 1.578468e-002 -1.078330e-002 7.300000e-002 + vertex 1.578468e-002 -1.078330e-002 8.300000e-002 + vertex 1.614014e-002 -1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal -4.226169e-001 9.063084e-001 0.000000e+000 + outer loop + vertex 1.578468e-002 -1.078330e-002 7.300000e-002 + vertex 1.614014e-002 -1.061755e-002 8.300000e-002 + vertex 1.614014e-002 -1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal -5.735781e-001 8.191509e-001 0.000000e+000 + outer loop + vertex 1.614014e-002 -1.061755e-002 7.300000e-002 + vertex 1.614014e-002 -1.061755e-002 8.300000e-002 + vertex 1.646141e-002 -1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal -5.735781e-001 8.191509e-001 -3.472114e-018 + outer loop + vertex 1.614014e-002 -1.061755e-002 7.300000e-002 + vertex 1.646141e-002 -1.039259e-002 8.300000e-002 + vertex 1.646141e-002 -1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal -7.071056e-001 7.071080e-001 0.000000e+000 + outer loop + vertex 1.646141e-002 -1.039259e-002 7.300000e-002 + vertex 1.646141e-002 -1.039259e-002 8.300000e-002 + vertex 1.673874e-002 -1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal -7.071056e-001 7.071080e-001 0.000000e+000 + outer loop + vertex 1.646141e-002 -1.039259e-002 7.300000e-002 + vertex 1.673874e-002 -1.011526e-002 8.300000e-002 + vertex 1.673874e-002 -1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex 1.673874e-002 -1.011526e-002 7.300000e-002 + vertex 1.673874e-002 -1.011526e-002 8.300000e-002 + vertex 1.696369e-002 -9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex 1.673874e-002 -1.011526e-002 7.300000e-002 + vertex 1.696369e-002 -9.793993e-003 8.300000e-002 + vertex 1.696369e-002 -9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal -9.063080e-001 4.226178e-001 0.000000e+000 + outer loop + vertex 1.696369e-002 -9.793993e-003 7.300000e-002 + vertex 1.696369e-002 -9.793993e-003 8.300000e-002 + vertex 1.712945e-002 -9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal -9.063080e-001 4.226178e-001 0.000000e+000 + outer loop + vertex 1.696369e-002 -9.793993e-003 7.300000e-002 + vertex 1.712945e-002 -9.438538e-003 8.300000e-002 + vertex 1.712945e-002 -9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex 1.712945e-002 -9.438538e-003 7.300000e-002 + vertex 1.712945e-002 -9.438538e-003 8.300000e-002 + vertex 1.723095e-002 -9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex 1.712945e-002 -9.438538e-003 7.300000e-002 + vertex 1.723095e-002 -9.059701e-003 8.300000e-002 + vertex 1.723095e-002 -9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal -9.961950e-001 8.715281e-002 0.000000e+000 + outer loop + vertex 1.723095e-002 -9.059701e-003 7.300000e-002 + vertex 1.723095e-002 -9.059701e-003 8.300000e-002 + vertex 1.726514e-002 -8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal -9.961950e-001 8.715281e-002 -4.340143e-019 + outer loop + vertex 1.723095e-002 8.278284e-003 7.300000e-002 + vertex 1.726514e-002 8.668993e-003 8.300000e-002 + vertex 1.726514e-002 8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal -9.961950e-001 -8.715281e-002 0.000000e+000 + outer loop + vertex 1.726514e-002 8.668993e-003 7.300000e-002 + vertex 1.726514e-002 8.668993e-003 8.300000e-002 + vertex 1.723095e-002 9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal -9.961950e-001 -8.715281e-002 4.340143e-019 + outer loop + vertex 1.726514e-002 8.668993e-003 7.300000e-002 + vertex 1.723095e-002 9.059701e-003 8.300000e-002 + vertex 1.723095e-002 9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex 1.723095e-002 9.059701e-003 7.300000e-002 + vertex 1.723095e-002 9.059701e-003 8.300000e-002 + vertex 1.712945e-002 9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex 1.723095e-002 9.059701e-003 7.300000e-002 + vertex 1.712945e-002 9.438538e-003 8.300000e-002 + vertex 1.712945e-002 9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal -9.063080e-001 -4.226178e-001 0.000000e+000 + outer loop + vertex 1.712945e-002 9.438538e-003 7.300000e-002 + vertex 1.712945e-002 9.438538e-003 8.300000e-002 + vertex 1.696369e-002 9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal -9.063080e-001 -4.226178e-001 0.000000e+000 + outer loop + vertex 1.712945e-002 9.438538e-003 7.300000e-002 + vertex 1.696369e-002 9.793993e-003 8.300000e-002 + vertex 1.696369e-002 9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex 1.696369e-002 9.793993e-003 7.300000e-002 + vertex 1.696369e-002 9.793993e-003 8.300000e-002 + vertex 1.673874e-002 1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex 1.696369e-002 9.793993e-003 7.300000e-002 + vertex 1.673874e-002 1.011526e-002 8.300000e-002 + vertex 1.673874e-002 1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal -7.071056e-001 -7.071080e-001 0.000000e+000 + outer loop + vertex 1.673874e-002 1.011526e-002 7.300000e-002 + vertex 1.673874e-002 1.011526e-002 8.300000e-002 + vertex 1.646141e-002 1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal -7.071056e-001 -7.071080e-001 0.000000e+000 + outer loop + vertex 1.673874e-002 1.011526e-002 7.300000e-002 + vertex 1.646141e-002 1.039259e-002 8.300000e-002 + vertex 1.646141e-002 1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal -5.735781e-001 -8.191509e-001 0.000000e+000 + outer loop + vertex 1.646141e-002 1.039259e-002 7.300000e-002 + vertex 1.646141e-002 1.039259e-002 8.300000e-002 + vertex 1.614014e-002 1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal -5.735781e-001 -8.191509e-001 3.472114e-018 + outer loop + vertex 1.646141e-002 1.039259e-002 7.300000e-002 + vertex 1.614014e-002 1.061755e-002 8.300000e-002 + vertex 1.614014e-002 1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal -4.226169e-001 -9.063084e-001 0.000000e+000 + outer loop + vertex 1.614014e-002 1.061755e-002 7.300000e-002 + vertex 1.614014e-002 1.061755e-002 8.300000e-002 + vertex 1.578468e-002 1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal -4.226169e-001 -9.063084e-001 0.000000e+000 + outer loop + vertex 1.614014e-002 1.061755e-002 7.300000e-002 + vertex 1.578468e-002 1.078330e-002 8.300000e-002 + vertex 1.578468e-002 1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal -2.588185e-001 -9.659260e-001 0.000000e+000 + outer loop + vertex 1.578468e-002 1.078330e-002 7.300000e-002 + vertex 1.578468e-002 1.078330e-002 8.300000e-002 + vertex 1.540584e-002 1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal -2.588185e-001 -9.659260e-001 -1.736057e-018 + outer loop + vertex 1.578468e-002 1.078330e-002 7.300000e-002 + vertex 1.540584e-002 1.088481e-002 8.300000e-002 + vertex 1.540584e-002 1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal -8.715517e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.540584e-002 1.088481e-002 7.300000e-002 + vertex 1.540584e-002 1.088481e-002 8.300000e-002 + vertex 1.501514e-002 1.091899e-002 8.300000e-002 + endloop + endfacet + facet normal -8.715517e-002 -9.961947e-001 4.340143e-019 + outer loop + vertex 1.540584e-002 1.088481e-002 7.300000e-002 + vertex 1.501514e-002 1.091899e-002 8.300000e-002 + vertex 1.501514e-002 1.091899e-002 7.300000e-002 + endloop + endfacet + facet normal 8.715517e-002 -9.961947e-001 0.000000e+000 + outer loop + vertex 1.501514e-002 1.091899e-002 7.300000e-002 + vertex 1.501514e-002 1.091899e-002 8.300000e-002 + vertex 1.462443e-002 1.088481e-002 8.300000e-002 + endloop + endfacet + facet normal 8.715517e-002 -9.961947e-001 -4.340143e-019 + outer loop + vertex 1.501514e-002 1.091899e-002 7.300000e-002 + vertex 1.462443e-002 1.088481e-002 8.300000e-002 + vertex 1.462443e-002 1.088481e-002 7.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 -9.659261e-001 0.000000e+000 + outer loop + vertex 1.462443e-002 1.088481e-002 7.300000e-002 + vertex 1.462443e-002 1.088481e-002 8.300000e-002 + vertex 1.424559e-002 1.078330e-002 8.300000e-002 + endloop + endfacet + facet normal 2.588179e-001 -9.659261e-001 0.000000e+000 + outer loop + vertex 1.462443e-002 1.088481e-002 7.300000e-002 + vertex 1.424559e-002 1.078330e-002 8.300000e-002 + vertex 1.424559e-002 1.078330e-002 7.300000e-002 + endloop + endfacet + facet normal 4.226178e-001 -9.063080e-001 0.000000e+000 + outer loop + vertex 1.424559e-002 1.078330e-002 7.300000e-002 + vertex 1.424559e-002 1.078330e-002 8.300000e-002 + vertex 1.389014e-002 1.061755e-002 8.300000e-002 + endloop + endfacet + facet normal 4.226178e-001 -9.063080e-001 0.000000e+000 + outer loop + vertex 1.424559e-002 1.078330e-002 7.300000e-002 + vertex 1.389014e-002 1.061755e-002 8.300000e-002 + vertex 1.389014e-002 1.061755e-002 7.300000e-002 + endloop + endfacet + facet normal 5.735769e-001 -8.191517e-001 0.000000e+000 + outer loop + vertex 1.389014e-002 1.061755e-002 7.300000e-002 + vertex 1.389014e-002 1.061755e-002 8.300000e-002 + vertex 1.356886e-002 1.039259e-002 8.300000e-002 + endloop + endfacet + facet normal 5.735769e-001 -8.191517e-001 0.000000e+000 + outer loop + vertex 1.389014e-002 1.061755e-002 7.300000e-002 + vertex 1.356886e-002 1.039259e-002 8.300000e-002 + vertex 1.356886e-002 1.039259e-002 7.300000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 1.356886e-002 1.039259e-002 7.300000e-002 + vertex 1.356886e-002 1.039259e-002 8.300000e-002 + vertex 1.329154e-002 1.011526e-002 8.300000e-002 + endloop + endfacet + facet normal 7.071068e-001 -7.071068e-001 0.000000e+000 + outer loop + vertex 1.356886e-002 1.039259e-002 7.300000e-002 + vertex 1.329154e-002 1.011526e-002 8.300000e-002 + vertex 1.329154e-002 1.011526e-002 7.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex 1.329154e-002 1.011526e-002 7.300000e-002 + vertex 1.329154e-002 1.011526e-002 8.300000e-002 + vertex 1.306658e-002 9.793993e-003 8.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 -5.735769e-001 0.000000e+000 + outer loop + vertex 1.329154e-002 1.011526e-002 7.300000e-002 + vertex 1.306658e-002 9.793993e-003 8.300000e-002 + vertex 1.306658e-002 9.793993e-003 7.300000e-002 + endloop + endfacet + facet normal 9.063080e-001 -4.226178e-001 0.000000e+000 + outer loop + vertex 1.306658e-002 9.793993e-003 7.300000e-002 + vertex 1.306658e-002 9.793993e-003 8.300000e-002 + vertex 1.290083e-002 9.438538e-003 8.300000e-002 + endloop + endfacet + facet normal 9.063080e-001 -4.226178e-001 0.000000e+000 + outer loop + vertex 1.306658e-002 9.793993e-003 7.300000e-002 + vertex 1.290083e-002 9.438538e-003 8.300000e-002 + vertex 1.290083e-002 9.438538e-003 7.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex 1.290083e-002 9.438538e-003 7.300000e-002 + vertex 1.290083e-002 9.438538e-003 8.300000e-002 + vertex 1.279932e-002 9.059701e-003 8.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 -2.588179e-001 0.000000e+000 + outer loop + vertex 1.290083e-002 9.438538e-003 7.300000e-002 + vertex 1.279932e-002 9.059701e-003 8.300000e-002 + vertex 1.279932e-002 9.059701e-003 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715517e-002 0.000000e+000 + outer loop + vertex 1.279932e-002 9.059701e-003 7.300000e-002 + vertex 1.279932e-002 9.059701e-003 8.300000e-002 + vertex 1.276514e-002 8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 -8.715517e-002 4.340143e-019 + outer loop + vertex 1.279932e-002 9.059701e-003 7.300000e-002 + vertex 1.276514e-002 8.668993e-003 8.300000e-002 + vertex 1.276514e-002 8.668993e-003 7.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715517e-002 -0.000000e+000 + outer loop + vertex 1.276514e-002 8.668993e-003 7.300000e-002 + vertex 1.276514e-002 8.668993e-003 8.300000e-002 + vertex 1.279932e-002 8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal 9.961947e-001 8.715517e-002 -4.340143e-019 + outer loop + vertex 1.276514e-002 8.668993e-003 7.300000e-002 + vertex 1.279932e-002 8.278284e-003 8.300000e-002 + vertex 1.279932e-002 8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 2.588179e-001 -0.000000e+000 + outer loop + vertex 1.279932e-002 8.278284e-003 7.300000e-002 + vertex 1.279932e-002 8.278284e-003 8.300000e-002 + vertex 1.290083e-002 7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal 9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex 1.279932e-002 8.278284e-003 7.300000e-002 + vertex 1.290083e-002 7.899447e-003 8.300000e-002 + vertex 1.290083e-002 7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal 9.063078e-001 4.226182e-001 -0.000000e+000 + outer loop + vertex 1.290083e-002 7.899447e-003 7.300000e-002 + vertex 1.290083e-002 7.899447e-003 8.300000e-002 + vertex 1.306658e-002 7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal 9.063078e-001 4.226182e-001 1.736057e-018 + outer loop + vertex 1.290083e-002 7.899447e-003 7.300000e-002 + vertex 1.306658e-002 7.543993e-003 8.300000e-002 + vertex 1.306658e-002 7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 5.735769e-001 -0.000000e+000 + outer loop + vertex 1.306658e-002 7.543993e-003 7.300000e-002 + vertex 1.306658e-002 7.543993e-003 8.300000e-002 + vertex 1.329154e-002 7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal 8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex 1.306658e-002 7.543993e-003 7.300000e-002 + vertex 1.329154e-002 7.222721e-003 8.300000e-002 + vertex 1.329154e-002 7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal 7.071062e-001 7.071074e-001 -0.000000e+000 + outer loop + vertex 1.329154e-002 7.222721e-003 7.300000e-002 + vertex 1.329154e-002 7.222721e-003 8.300000e-002 + vertex 1.356886e-002 6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal 7.071062e-001 7.071074e-001 3.472114e-018 + outer loop + vertex 1.329154e-002 7.222721e-003 7.300000e-002 + vertex 1.356886e-002 6.945393e-003 8.300000e-002 + vertex 1.356886e-002 6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal 5.735769e-001 8.191517e-001 -0.000000e+000 + outer loop + vertex 1.356886e-002 6.945393e-003 7.300000e-002 + vertex 1.356886e-002 6.945393e-003 8.300000e-002 + vertex 1.389014e-002 6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal 5.735769e-001 8.191517e-001 0.000000e+000 + outer loop + vertex 1.356886e-002 6.945393e-003 7.300000e-002 + vertex 1.389014e-002 6.720436e-003 8.300000e-002 + vertex 1.389014e-002 6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal 4.226178e-001 9.063080e-001 -0.000000e+000 + outer loop + vertex 1.389014e-002 6.720436e-003 7.300000e-002 + vertex 1.389014e-002 6.720436e-003 8.300000e-002 + vertex 1.424559e-002 6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal 4.226178e-001 9.063080e-001 0.000000e+000 + outer loop + vertex 1.389014e-002 6.720436e-003 7.300000e-002 + vertex 1.424559e-002 6.554685e-003 8.300000e-002 + vertex 1.424559e-002 6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 -0.000000e+000 + outer loop + vertex 1.424559e-002 6.554685e-003 7.300000e-002 + vertex 1.424559e-002 6.554685e-003 8.300000e-002 + vertex 1.462443e-002 6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal 2.588190e-001 9.659258e-001 -1.736057e-018 + outer loop + vertex 1.424559e-002 6.554685e-003 7.300000e-002 + vertex 1.462443e-002 6.453176e-003 8.300000e-002 + vertex 1.462443e-002 6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal 8.715635e-002 9.961947e-001 -0.000000e+000 + outer loop + vertex 1.462443e-002 6.453176e-003 7.300000e-002 + vertex 1.462443e-002 6.453176e-003 8.300000e-002 + vertex 1.501514e-002 6.418993e-003 8.300000e-002 + endloop + endfacet + facet normal 8.715635e-002 9.961947e-001 0.000000e+000 + outer loop + vertex 1.462443e-002 6.453176e-003 7.300000e-002 + vertex 1.501514e-002 6.418993e-003 8.300000e-002 + vertex 1.501514e-002 6.418993e-003 7.300000e-002 + endloop + endfacet + facet normal -8.715635e-002 9.961947e-001 0.000000e+000 + outer loop + vertex 1.501514e-002 6.418993e-003 7.300000e-002 + vertex 1.501514e-002 6.418993e-003 8.300000e-002 + vertex 1.540584e-002 6.453176e-003 8.300000e-002 + endloop + endfacet + facet normal -8.715635e-002 9.961947e-001 0.000000e+000 + outer loop + vertex 1.501514e-002 6.418993e-003 7.300000e-002 + vertex 1.540584e-002 6.453176e-003 8.300000e-002 + vertex 1.540584e-002 6.453176e-003 7.300000e-002 + endloop + endfacet + facet normal -2.588196e-001 9.659257e-001 0.000000e+000 + outer loop + vertex 1.540584e-002 6.453176e-003 7.300000e-002 + vertex 1.540584e-002 6.453176e-003 8.300000e-002 + vertex 1.578468e-002 6.554685e-003 8.300000e-002 + endloop + endfacet + facet normal -2.588196e-001 9.659257e-001 -1.736057e-018 + outer loop + vertex 1.540584e-002 6.453176e-003 7.300000e-002 + vertex 1.578468e-002 6.554685e-003 8.300000e-002 + vertex 1.578468e-002 6.554685e-003 7.300000e-002 + endloop + endfacet + facet normal -4.226169e-001 9.063084e-001 0.000000e+000 + outer loop + vertex 1.578468e-002 6.554685e-003 7.300000e-002 + vertex 1.578468e-002 6.554685e-003 8.300000e-002 + vertex 1.614014e-002 6.720436e-003 8.300000e-002 + endloop + endfacet + facet normal -4.226169e-001 9.063084e-001 0.000000e+000 + outer loop + vertex 1.578468e-002 6.554685e-003 7.300000e-002 + vertex 1.614014e-002 6.720436e-003 8.300000e-002 + vertex 1.614014e-002 6.720436e-003 7.300000e-002 + endloop + endfacet + facet normal -5.735781e-001 8.191509e-001 0.000000e+000 + outer loop + vertex 1.614014e-002 6.720436e-003 7.300000e-002 + vertex 1.614014e-002 6.720436e-003 8.300000e-002 + vertex 1.646141e-002 6.945393e-003 8.300000e-002 + endloop + endfacet + facet normal -5.735781e-001 8.191509e-001 -3.472114e-018 + outer loop + vertex 1.614014e-002 6.720436e-003 7.300000e-002 + vertex 1.646141e-002 6.945393e-003 8.300000e-002 + vertex 1.646141e-002 6.945393e-003 7.300000e-002 + endloop + endfacet + facet normal -7.071050e-001 7.071086e-001 0.000000e+000 + outer loop + vertex 1.646141e-002 6.945393e-003 7.300000e-002 + vertex 1.646141e-002 6.945393e-003 8.300000e-002 + vertex 1.673874e-002 7.222721e-003 8.300000e-002 + endloop + endfacet + facet normal -7.071050e-001 7.071086e-001 0.000000e+000 + outer loop + vertex 1.646141e-002 6.945393e-003 7.300000e-002 + vertex 1.673874e-002 7.222721e-003 8.300000e-002 + vertex 1.673874e-002 7.222721e-003 7.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex 1.673874e-002 7.222721e-003 7.300000e-002 + vertex 1.673874e-002 7.222721e-003 8.300000e-002 + vertex 1.696369e-002 7.543993e-003 8.300000e-002 + endloop + endfacet + facet normal -8.191517e-001 5.735769e-001 0.000000e+000 + outer loop + vertex 1.673874e-002 7.222721e-003 7.300000e-002 + vertex 1.696369e-002 7.543993e-003 8.300000e-002 + vertex 1.696369e-002 7.543993e-003 7.300000e-002 + endloop + endfacet + facet normal -9.063078e-001 4.226182e-001 0.000000e+000 + outer loop + vertex 1.696369e-002 7.543993e-003 7.300000e-002 + vertex 1.696369e-002 7.543993e-003 8.300000e-002 + vertex 1.712945e-002 7.899447e-003 8.300000e-002 + endloop + endfacet + facet normal -9.063078e-001 4.226182e-001 -1.736057e-018 + outer loop + vertex 1.696369e-002 7.543993e-003 7.300000e-002 + vertex 1.712945e-002 7.899447e-003 8.300000e-002 + vertex 1.712945e-002 7.899447e-003 7.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex 1.712945e-002 7.899447e-003 7.300000e-002 + vertex 1.712945e-002 7.899447e-003 8.300000e-002 + vertex 1.723095e-002 8.278284e-003 8.300000e-002 + endloop + endfacet + facet normal -9.659261e-001 2.588179e-001 0.000000e+000 + outer loop + vertex 1.712945e-002 7.899447e-003 7.300000e-002 + vertex 1.723095e-002 8.278284e-003 8.300000e-002 + vertex 1.723095e-002 8.278284e-003 7.300000e-002 + endloop + endfacet + facet normal -9.961950e-001 8.715281e-002 0.000000e+000 + outer loop + vertex 1.723095e-002 8.278284e-003 7.300000e-002 + vertex 1.723095e-002 8.278284e-003 8.300000e-002 + vertex 1.726514e-002 8.668993e-003 8.300000e-002 + endloop + endfacet + facet normal 4.063526e-001 -5.803317e-001 -7.057567e-001 + outer loop + vertex 6.427876e-003 -7.660444e-003 7.200000e-002 + vertex 6.625000e-003 -8.738546e-003 7.300000e-002 + vertex 5.000000e-003 -8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal 4.063528e-001 -5.803314e-001 -7.057569e-001 + outer loop + vertex 5.000000e-003 -8.660254e-003 7.200000e-002 + vertex 6.625000e-003 -8.738546e-003 7.300000e-002 + vertex 5.500000e-003 -9.526280e-003 7.300000e-002 + endloop + endfacet + facet normal 2.994057e-001 -6.420774e-001 -7.057570e-001 + outer loop + vertex 5.000000e-003 -8.660254e-003 7.200000e-002 + vertex 5.500000e-003 -9.526280e-003 7.300000e-002 + vertex 3.420201e-003 -9.396926e-003 7.200000e-002 + endloop + endfacet + facet normal 2.994056e-001 -6.420776e-001 -7.057568e-001 + outer loop + vertex 3.420201e-003 -9.396926e-003 7.200000e-002 + vertex 5.500000e-003 -9.526280e-003 7.300000e-002 + vertex 3.762222e-003 -1.033662e-002 7.300000e-002 + endloop + endfacet + facet normal 1.833614e-001 -6.843143e-001 -7.057568e-001 + outer loop + vertex 3.420201e-003 -9.396926e-003 7.200000e-002 + vertex 3.762222e-003 -1.033662e-002 7.300000e-002 + vertex 1.736482e-003 -9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal 1.833614e-001 -6.843143e-001 -7.057567e-001 + outer loop + vertex 3.762222e-003 -1.033662e-002 7.300000e-002 + vertex 1.910130e-003 -1.083289e-002 7.300000e-002 + vertex 1.736482e-003 -9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal 6.174598e-002 -7.057583e-001 -7.057567e-001 + outer loop + vertex 1.736482e-003 -9.848078e-003 7.200000e-002 + vertex 1.910130e-003 -1.083289e-002 7.300000e-002 + vertex 4.629575e-004 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 6.174564e-002 -7.057582e-001 -7.057570e-001 + outer loop + vertex 1.736482e-003 -9.848078e-003 7.200000e-002 + vertex 4.629575e-004 -1.095950e-002 7.300000e-002 + vertex -1.387779e-018 -1.000000e-002 7.200000e-002 + endloop + endfacet + facet normal 6.174536e-002 -7.057582e-001 -7.057569e-001 + outer loop + vertex 4.629575e-004 -1.095950e-002 7.300000e-002 + vertex -1.526557e-018 -1.100000e-002 7.300000e-002 + vertex -1.387779e-018 -1.000000e-002 7.200000e-002 + endloop + endfacet + facet normal -6.174536e-002 -7.057582e-001 -7.057569e-001 + outer loop + vertex -1.387779e-018 -1.000000e-002 7.200000e-002 + vertex -1.526557e-018 -1.100000e-002 7.300000e-002 + vertex -4.629575e-004 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -6.174564e-002 -7.057582e-001 -7.057570e-001 + outer loop + vertex -1.387779e-018 -1.000000e-002 7.200000e-002 + vertex -4.629575e-004 -1.095950e-002 7.300000e-002 + vertex -1.736482e-003 -9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal -6.174598e-002 -7.057583e-001 -7.057567e-001 + outer loop + vertex -1.736482e-003 -9.848078e-003 7.200000e-002 + vertex -4.629575e-004 -1.095950e-002 7.300000e-002 + vertex -1.910130e-003 -1.083289e-002 7.300000e-002 + endloop + endfacet + facet normal -1.833614e-001 -6.843143e-001 -7.057567e-001 + outer loop + vertex -1.736482e-003 -9.848078e-003 7.200000e-002 + vertex -1.910130e-003 -1.083289e-002 7.300000e-002 + vertex -3.420201e-003 -9.396926e-003 7.200000e-002 + endloop + endfacet + facet normal -1.833614e-001 -6.843143e-001 -7.057568e-001 + outer loop + vertex -3.420201e-003 -9.396926e-003 7.200000e-002 + vertex -1.910130e-003 -1.083289e-002 7.300000e-002 + vertex -3.762222e-003 -1.033662e-002 7.300000e-002 + endloop + endfacet + facet normal -2.994058e-001 -6.420775e-001 -7.057568e-001 + outer loop + vertex -3.420201e-003 -9.396926e-003 7.200000e-002 + vertex -3.762222e-003 -1.033662e-002 7.300000e-002 + vertex -5.000000e-003 -8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal -2.994055e-001 -6.420774e-001 -7.057570e-001 + outer loop + vertex -3.762222e-003 -1.033662e-002 7.300000e-002 + vertex -5.500000e-003 -9.526280e-003 7.300000e-002 + vertex -5.000000e-003 -8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal -4.063528e-001 -5.803314e-001 -7.057569e-001 + outer loop + vertex -5.000000e-003 -8.660254e-003 7.200000e-002 + vertex -5.500000e-003 -9.526280e-003 7.300000e-002 + vertex -6.625000e-003 -8.738546e-003 7.300000e-002 + endloop + endfacet + facet normal -4.063526e-001 -5.803317e-001 -7.057567e-001 + outer loop + vertex -5.000000e-003 -8.660254e-003 7.200000e-002 + vertex -6.625000e-003 -8.738546e-003 7.300000e-002 + vertex -6.427876e-003 -7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal -4.063518e-001 -5.803321e-001 -7.057570e-001 + outer loop + vertex -6.427876e-003 -7.660444e-003 7.200000e-002 + vertex -6.625000e-003 -8.738546e-003 7.300000e-002 + vertex -7.070664e-003 -8.426489e-003 7.300000e-002 + endloop + endfacet + facet normal -5.009527e-001 -5.009527e-001 -7.057570e-001 + outer loop + vertex -6.427876e-003 -7.660444e-003 7.200000e-002 + vertex -7.070664e-003 -8.426489e-003 7.300000e-002 + vertex -7.660444e-003 -6.427876e-003 7.200000e-002 + endloop + endfacet + facet normal -5.009527e-001 -5.009527e-001 -7.057570e-001 + outer loop + vertex -7.660444e-003 -6.427876e-003 7.200000e-002 + vertex -7.070664e-003 -8.426489e-003 7.300000e-002 + vertex -8.426489e-003 -7.070664e-003 7.300000e-002 + endloop + endfacet + facet normal -5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex -7.660444e-003 -6.427876e-003 7.200000e-002 + vertex -8.426489e-003 -7.070664e-003 7.300000e-002 + vertex -8.660254e-003 -5.000000e-003 7.200000e-002 + endloop + endfacet + facet normal -5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex -8.660254e-003 -5.000000e-003 7.200000e-002 + vertex -8.426489e-003 -7.070664e-003 7.300000e-002 + vertex -9.526280e-003 -5.500000e-003 7.300000e-002 + endloop + endfacet + facet normal -6.420774e-001 -2.994057e-001 -7.057570e-001 + outer loop + vertex -8.660254e-003 -5.000000e-003 7.200000e-002 + vertex -9.526280e-003 -5.500000e-003 7.300000e-002 + vertex -9.396926e-003 -3.420201e-003 7.200000e-002 + endloop + endfacet + facet normal -6.420776e-001 -2.994056e-001 -7.057568e-001 + outer loop + vertex -9.396926e-003 -3.420201e-003 7.200000e-002 + vertex -9.526280e-003 -5.500000e-003 7.300000e-002 + vertex -1.033662e-002 -3.762222e-003 7.300000e-002 + endloop + endfacet + facet normal -6.843143e-001 -1.833614e-001 -7.057568e-001 + outer loop + vertex -9.396926e-003 -3.420201e-003 7.200000e-002 + vertex -1.033662e-002 -3.762222e-003 7.300000e-002 + vertex -9.848078e-003 -1.736482e-003 7.200000e-002 + endloop + endfacet + facet normal -6.843143e-001 -1.833614e-001 -7.057567e-001 + outer loop + vertex -9.848078e-003 -1.736482e-003 7.200000e-002 + vertex -1.033662e-002 -3.762222e-003 7.300000e-002 + vertex -1.083289e-002 -1.910130e-003 7.300000e-002 + endloop + endfacet + facet normal -7.057584e-001 -6.174567e-002 -7.057567e-001 + outer loop + vertex -9.848078e-003 -1.736482e-003 7.200000e-002 + vertex -1.083289e-002 -1.910130e-003 7.300000e-002 + vertex -1.000000e-002 1.942890e-018 7.200000e-002 + endloop + endfacet + facet normal -7.057582e-001 -6.174582e-002 -7.057569e-001 + outer loop + vertex -1.000000e-002 1.942890e-018 7.200000e-002 + vertex -1.083289e-002 -1.910130e-003 7.300000e-002 + vertex -1.100000e-002 2.137179e-018 7.300000e-002 + endloop + endfacet + facet normal -7.057582e-001 6.174565e-002 -7.057569e-001 + outer loop + vertex -1.000000e-002 1.942890e-018 7.200000e-002 + vertex -1.100000e-002 2.137179e-018 7.300000e-002 + vertex -9.848078e-003 1.736482e-003 7.200000e-002 + endloop + endfacet + facet normal -7.057583e-001 6.174584e-002 -7.057567e-001 + outer loop + vertex -9.848078e-003 1.736482e-003 7.200000e-002 + vertex -1.100000e-002 2.137179e-018 7.300000e-002 + vertex -1.083289e-002 1.910130e-003 7.300000e-002 + endloop + endfacet + facet normal -6.843143e-001 1.833614e-001 -7.057567e-001 + outer loop + vertex -9.848078e-003 1.736482e-003 7.200000e-002 + vertex -1.083289e-002 1.910130e-003 7.300000e-002 + vertex -9.396926e-003 3.420201e-003 7.200000e-002 + endloop + endfacet + facet normal -6.843143e-001 1.833614e-001 -7.057568e-001 + outer loop + vertex -9.396926e-003 3.420201e-003 7.200000e-002 + vertex -1.083289e-002 1.910130e-003 7.300000e-002 + vertex -1.033662e-002 3.762222e-003 7.300000e-002 + endloop + endfacet + facet normal -6.420775e-001 2.994058e-001 -7.057568e-001 + outer loop + vertex -9.396926e-003 3.420201e-003 7.200000e-002 + vertex -1.033662e-002 3.762222e-003 7.300000e-002 + vertex -8.660254e-003 5.000000e-003 7.200000e-002 + endloop + endfacet + facet normal -6.420774e-001 2.994055e-001 -7.057570e-001 + outer loop + vertex -8.660254e-003 5.000000e-003 7.200000e-002 + vertex -1.033662e-002 3.762222e-003 7.300000e-002 + vertex -9.526280e-003 5.500000e-003 7.300000e-002 + endloop + endfacet + facet normal -5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex -8.660254e-003 5.000000e-003 7.200000e-002 + vertex -9.526280e-003 5.500000e-003 7.300000e-002 + vertex -7.660444e-003 6.427876e-003 7.200000e-002 + endloop + endfacet + facet normal -5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex -7.660444e-003 6.427876e-003 7.200000e-002 + vertex -9.526280e-003 5.500000e-003 7.300000e-002 + vertex -8.426489e-003 7.070664e-003 7.300000e-002 + endloop + endfacet + facet normal -5.009527e-001 5.009527e-001 -7.057570e-001 + outer loop + vertex -7.660444e-003 6.427876e-003 7.200000e-002 + vertex -8.426489e-003 7.070664e-003 7.300000e-002 + vertex -6.427876e-003 7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal -5.009527e-001 5.009527e-001 -7.057570e-001 + outer loop + vertex -8.426489e-003 7.070664e-003 7.300000e-002 + vertex -7.070664e-003 8.426489e-003 7.300000e-002 + vertex -6.427876e-003 7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal -4.063518e-001 5.803321e-001 -7.057570e-001 + outer loop + vertex -6.427876e-003 7.660444e-003 7.200000e-002 + vertex -7.070664e-003 8.426489e-003 7.300000e-002 + vertex -6.625000e-003 8.738546e-003 7.300000e-002 + endloop + endfacet + facet normal -4.063526e-001 5.803317e-001 -7.057567e-001 + outer loop + vertex -6.427876e-003 7.660444e-003 7.200000e-002 + vertex -6.625000e-003 8.738546e-003 7.300000e-002 + vertex -5.000000e-003 8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal -4.063528e-001 5.803314e-001 -7.057569e-001 + outer loop + vertex -5.000000e-003 8.660254e-003 7.200000e-002 + vertex -6.625000e-003 8.738546e-003 7.300000e-002 + vertex -5.500000e-003 9.526280e-003 7.300000e-002 + endloop + endfacet + facet normal -2.994057e-001 6.420774e-001 -7.057570e-001 + outer loop + vertex -5.000000e-003 8.660254e-003 7.200000e-002 + vertex -5.500000e-003 9.526280e-003 7.300000e-002 + vertex -3.420201e-003 9.396926e-003 7.200000e-002 + endloop + endfacet + facet normal -2.994056e-001 6.420776e-001 -7.057568e-001 + outer loop + vertex -3.420201e-003 9.396926e-003 7.200000e-002 + vertex -5.500000e-003 9.526280e-003 7.300000e-002 + vertex -3.762222e-003 1.033662e-002 7.300000e-002 + endloop + endfacet + facet normal -1.833614e-001 6.843143e-001 -7.057568e-001 + outer loop + vertex -3.420201e-003 9.396926e-003 7.200000e-002 + vertex -3.762222e-003 1.033662e-002 7.300000e-002 + vertex -1.736482e-003 9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal -1.833614e-001 6.843143e-001 -7.057567e-001 + outer loop + vertex -3.762222e-003 1.033662e-002 7.300000e-002 + vertex -1.910130e-003 1.083289e-002 7.300000e-002 + vertex -1.736482e-003 9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal -6.174598e-002 7.057583e-001 -7.057567e-001 + outer loop + vertex -1.736482e-003 9.848078e-003 7.200000e-002 + vertex -1.910130e-003 1.083289e-002 7.300000e-002 + vertex -4.629575e-004 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -6.174564e-002 7.057582e-001 -7.057570e-001 + outer loop + vertex -1.736482e-003 9.848078e-003 7.200000e-002 + vertex -4.629575e-004 1.095950e-002 7.300000e-002 + vertex 1.387779e-018 1.000000e-002 7.200000e-002 + endloop + endfacet + facet normal -6.174536e-002 7.057582e-001 -7.057569e-001 + outer loop + vertex -4.629575e-004 1.095950e-002 7.300000e-002 + vertex 1.526557e-018 1.100000e-002 7.300000e-002 + vertex 1.387779e-018 1.000000e-002 7.200000e-002 + endloop + endfacet + facet normal 6.174536e-002 7.057582e-001 -7.057569e-001 + outer loop + vertex 1.387779e-018 1.000000e-002 7.200000e-002 + vertex 1.526557e-018 1.100000e-002 7.300000e-002 + vertex 4.629575e-004 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 6.174564e-002 7.057582e-001 -7.057570e-001 + outer loop + vertex 1.387779e-018 1.000000e-002 7.200000e-002 + vertex 4.629575e-004 1.095950e-002 7.300000e-002 + vertex 1.736482e-003 9.848078e-003 7.200000e-002 + endloop + endfacet + facet normal 6.174598e-002 7.057583e-001 -7.057567e-001 + outer loop + vertex 1.736482e-003 9.848078e-003 7.200000e-002 + vertex 4.629575e-004 1.095950e-002 7.300000e-002 + vertex 1.910130e-003 1.083289e-002 7.300000e-002 + endloop + endfacet + facet normal 1.833614e-001 6.843143e-001 -7.057567e-001 + outer loop + vertex 1.736482e-003 9.848078e-003 7.200000e-002 + vertex 1.910130e-003 1.083289e-002 7.300000e-002 + vertex 3.420201e-003 9.396926e-003 7.200000e-002 + endloop + endfacet + facet normal 1.833614e-001 6.843143e-001 -7.057568e-001 + outer loop + vertex 3.420201e-003 9.396926e-003 7.200000e-002 + vertex 1.910130e-003 1.083289e-002 7.300000e-002 + vertex 3.762222e-003 1.033662e-002 7.300000e-002 + endloop + endfacet + facet normal 2.994058e-001 6.420775e-001 -7.057568e-001 + outer loop + vertex 3.420201e-003 9.396926e-003 7.200000e-002 + vertex 3.762222e-003 1.033662e-002 7.300000e-002 + vertex 5.000000e-003 8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal 2.994055e-001 6.420774e-001 -7.057570e-001 + outer loop + vertex 3.762222e-003 1.033662e-002 7.300000e-002 + vertex 5.500000e-003 9.526280e-003 7.300000e-002 + vertex 5.000000e-003 8.660254e-003 7.200000e-002 + endloop + endfacet + facet normal 4.063528e-001 5.803314e-001 -7.057569e-001 + outer loop + vertex 5.000000e-003 8.660254e-003 7.200000e-002 + vertex 5.500000e-003 9.526280e-003 7.300000e-002 + vertex 6.625000e-003 8.738546e-003 7.300000e-002 + endloop + endfacet + facet normal 4.063526e-001 5.803317e-001 -7.057567e-001 + outer loop + vertex 5.000000e-003 8.660254e-003 7.200000e-002 + vertex 6.625000e-003 8.738546e-003 7.300000e-002 + vertex 6.427876e-003 7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal 4.063518e-001 5.803321e-001 -7.057570e-001 + outer loop + vertex 6.427876e-003 7.660444e-003 7.200000e-002 + vertex 6.625000e-003 8.738546e-003 7.300000e-002 + vertex 7.070664e-003 8.426489e-003 7.300000e-002 + endloop + endfacet + facet normal 5.009527e-001 5.009527e-001 -7.057570e-001 + outer loop + vertex 6.427876e-003 7.660444e-003 7.200000e-002 + vertex 7.070664e-003 8.426489e-003 7.300000e-002 + vertex 7.660444e-003 6.427876e-003 7.200000e-002 + endloop + endfacet + facet normal 5.009527e-001 5.009527e-001 -7.057570e-001 + outer loop + vertex 7.660444e-003 6.427876e-003 7.200000e-002 + vertex 7.070664e-003 8.426489e-003 7.300000e-002 + vertex 8.426489e-003 7.070664e-003 7.300000e-002 + endloop + endfacet + facet normal 5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex 7.660444e-003 6.427876e-003 7.200000e-002 + vertex 8.426489e-003 7.070664e-003 7.300000e-002 + vertex 8.660254e-003 5.000000e-003 7.200000e-002 + endloop + endfacet + facet normal 5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex 8.660254e-003 5.000000e-003 7.200000e-002 + vertex 8.426489e-003 7.070664e-003 7.300000e-002 + vertex 9.526280e-003 5.500000e-003 7.300000e-002 + endloop + endfacet + facet normal 6.420774e-001 2.994057e-001 -7.057570e-001 + outer loop + vertex 8.660254e-003 5.000000e-003 7.200000e-002 + vertex 9.526280e-003 5.500000e-003 7.300000e-002 + vertex 9.396926e-003 3.420201e-003 7.200000e-002 + endloop + endfacet + facet normal 6.420776e-001 2.994056e-001 -7.057568e-001 + outer loop + vertex 9.396926e-003 3.420201e-003 7.200000e-002 + vertex 9.526280e-003 5.500000e-003 7.300000e-002 + vertex 1.033662e-002 3.762222e-003 7.300000e-002 + endloop + endfacet + facet normal 6.843143e-001 1.833614e-001 -7.057568e-001 + outer loop + vertex 9.396926e-003 3.420201e-003 7.200000e-002 + vertex 1.033662e-002 3.762222e-003 7.300000e-002 + vertex 9.848078e-003 1.736482e-003 7.200000e-002 + endloop + endfacet + facet normal 6.843143e-001 1.833614e-001 -7.057567e-001 + outer loop + vertex 9.848078e-003 1.736482e-003 7.200000e-002 + vertex 1.033662e-002 3.762222e-003 7.300000e-002 + vertex 1.083289e-002 1.910130e-003 7.300000e-002 + endloop + endfacet + facet normal 7.057584e-001 6.174567e-002 -7.057567e-001 + outer loop + vertex 9.848078e-003 1.736482e-003 7.200000e-002 + vertex 1.083289e-002 1.910130e-003 7.300000e-002 + vertex 1.000000e-002 0.000000e+000 7.200000e-002 + endloop + endfacet + facet normal 7.057582e-001 6.174582e-002 -7.057569e-001 + outer loop + vertex 1.000000e-002 0.000000e+000 7.200000e-002 + vertex 1.083289e-002 1.910130e-003 7.300000e-002 + vertex 1.100000e-002 0.000000e+000 7.300000e-002 + endloop + endfacet + facet normal 7.057582e-001 -6.174565e-002 -7.057569e-001 + outer loop + vertex 1.000000e-002 0.000000e+000 7.200000e-002 + vertex 1.100000e-002 0.000000e+000 7.300000e-002 + vertex 9.848078e-003 -1.736482e-003 7.200000e-002 + endloop + endfacet + facet normal 7.057583e-001 -6.174584e-002 -7.057567e-001 + outer loop + vertex 9.848078e-003 -1.736482e-003 7.200000e-002 + vertex 1.100000e-002 0.000000e+000 7.300000e-002 + vertex 1.083289e-002 -1.910130e-003 7.300000e-002 + endloop + endfacet + facet normal 6.843143e-001 -1.833614e-001 -7.057567e-001 + outer loop + vertex 9.848078e-003 -1.736482e-003 7.200000e-002 + vertex 1.083289e-002 -1.910130e-003 7.300000e-002 + vertex 9.396926e-003 -3.420201e-003 7.200000e-002 + endloop + endfacet + facet normal 6.843143e-001 -1.833614e-001 -7.057568e-001 + outer loop + vertex 9.396926e-003 -3.420201e-003 7.200000e-002 + vertex 1.083289e-002 -1.910130e-003 7.300000e-002 + vertex 1.033662e-002 -3.762222e-003 7.300000e-002 + endloop + endfacet + facet normal 6.420775e-001 -2.994058e-001 -7.057568e-001 + outer loop + vertex 9.396926e-003 -3.420201e-003 7.200000e-002 + vertex 1.033662e-002 -3.762222e-003 7.300000e-002 + vertex 8.660254e-003 -5.000000e-003 7.200000e-002 + endloop + endfacet + facet normal 6.420774e-001 -2.994055e-001 -7.057570e-001 + outer loop + vertex 8.660254e-003 -5.000000e-003 7.200000e-002 + vertex 1.033662e-002 -3.762222e-003 7.300000e-002 + vertex 9.526280e-003 -5.500000e-003 7.300000e-002 + endloop + endfacet + facet normal 5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex 8.660254e-003 -5.000000e-003 7.200000e-002 + vertex 9.526280e-003 -5.500000e-003 7.300000e-002 + vertex 7.660444e-003 -6.427876e-003 7.200000e-002 + endloop + endfacet + facet normal 5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex 7.660444e-003 -6.427876e-003 7.200000e-002 + vertex 9.526280e-003 -5.500000e-003 7.300000e-002 + vertex 8.426489e-003 -7.070664e-003 7.300000e-002 + endloop + endfacet + facet normal 5.009527e-001 -5.009527e-001 -7.057570e-001 + outer loop + vertex 7.660444e-003 -6.427876e-003 7.200000e-002 + vertex 8.426489e-003 -7.070664e-003 7.300000e-002 + vertex 6.427876e-003 -7.660444e-003 7.200000e-002 + endloop + endfacet + facet normal 5.009527e-001 -5.009527e-001 -7.057570e-001 + outer loop + vertex 6.427876e-003 -7.660444e-003 7.200000e-002 + vertex 8.426489e-003 -7.070664e-003 7.300000e-002 + vertex 7.070664e-003 -8.426489e-003 7.300000e-002 + endloop + endfacet + facet normal 4.063518e-001 -5.803321e-001 -7.057570e-001 + outer loop + vertex 6.427876e-003 -7.660444e-003 7.200000e-002 + vertex 7.070664e-003 -8.426489e-003 7.300000e-002 + vertex 6.625000e-003 -8.738546e-003 7.300000e-002 + endloop + endfacet + facet normal 5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex 2.005360e-002 -1.304399e-002 7.300000e-002 + vertex 2.165063e-002 -1.250000e-002 7.400000e-002 + vertex 1.838507e-002 -1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal 5.803317e-001 -4.063528e-001 -7.057567e-001 + outer loop + vertex 1.838507e-002 -1.542690e-002 7.300000e-002 + vertex 2.165063e-002 -1.250000e-002 7.400000e-002 + vertex 1.915111e-002 -1.606969e-002 7.400000e-002 + endloop + endfacet + facet normal 5.009529e-001 -5.009529e-001 -7.057566e-001 + outer loop + vertex 1.838507e-002 -1.542690e-002 7.300000e-002 + vertex 1.915111e-002 -1.606969e-002 7.400000e-002 + vertex 1.542690e-002 -1.838507e-002 7.300000e-002 + endloop + endfacet + facet normal 5.009529e-001 -5.009529e-001 -7.057566e-001 + outer loop + vertex 1.542690e-002 -1.838507e-002 7.300000e-002 + vertex 1.915111e-002 -1.606969e-002 7.400000e-002 + vertex 1.606969e-002 -1.915111e-002 7.400000e-002 + endloop + endfacet + facet normal 4.063527e-001 -5.803318e-001 -7.057567e-001 + outer loop + vertex 1.542690e-002 -1.838507e-002 7.300000e-002 + vertex 1.606969e-002 -1.915111e-002 7.400000e-002 + vertex 1.200000e-002 -2.078461e-002 7.300000e-002 + endloop + endfacet + facet normal 4.063527e-001 -5.803315e-001 -7.057569e-001 + outer loop + vertex 1.200000e-002 -2.078461e-002 7.300000e-002 + vertex 1.606969e-002 -1.915111e-002 7.400000e-002 + vertex 1.250000e-002 -2.165063e-002 7.400000e-002 + endloop + endfacet + facet normal 2.994070e-001 -6.420767e-001 -7.057570e-001 + outer loop + vertex 1.200000e-002 -2.078461e-002 7.300000e-002 + vertex 1.250000e-002 -2.165063e-002 7.400000e-002 + vertex 1.188257e-002 -2.083937e-002 7.300000e-002 + endloop + endfacet + facet normal 2.994057e-001 -6.420775e-001 -7.057568e-001 + outer loop + vertex 1.188257e-002 -2.083937e-002 7.300000e-002 + vertex 1.250000e-002 -2.165063e-002 7.400000e-002 + vertex 8.550503e-003 -2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal 2.994056e-001 -6.420774e-001 -7.057570e-001 + outer loop + vertex 1.188257e-002 -2.083937e-002 7.300000e-002 + vertex 8.550503e-003 -2.349232e-002 7.400000e-002 + vertex 8.208483e-003 -2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal -1.833614e-001 -6.843142e-001 -7.057568e-001 + outer loop + vertex -8.550503e-003 -2.349232e-002 7.400000e-002 + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + vertex -4.341205e-003 -2.462019e-002 7.400000e-002 + endloop + endfacet + facet normal -1.833612e-001 -6.843140e-001 -7.057571e-001 + outer loop + vertex -4.341205e-003 -2.462019e-002 7.400000e-002 + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + vertex -4.167556e-003 -2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal -6.174586e-002 -7.057580e-001 -7.057571e-001 + outer loop + vertex -4.341205e-003 -2.462019e-002 7.400000e-002 + vertex -4.167556e-003 -2.363539e-002 7.300000e-002 + vertex -3.469447e-018 -2.500000e-002 7.400000e-002 + endloop + endfacet + facet normal -6.174596e-002 -7.057582e-001 -7.057569e-001 + outer loop + vertex -3.469447e-018 -2.500000e-002 7.400000e-002 + vertex -4.167556e-003 -2.363539e-002 7.300000e-002 + vertex -3.330669e-018 -2.400000e-002 7.300000e-002 + endloop + endfacet + facet normal 6.174587e-002 -7.057582e-001 -7.057569e-001 + outer loop + vertex -3.469447e-018 -2.500000e-002 7.400000e-002 + vertex -3.330669e-018 -2.400000e-002 7.300000e-002 + vertex 4.341205e-003 -2.462019e-002 7.400000e-002 + endloop + endfacet + facet normal 6.174595e-002 -7.057580e-001 -7.057571e-001 + outer loop + vertex 4.341205e-003 -2.462019e-002 7.400000e-002 + vertex -3.330669e-018 -2.400000e-002 7.300000e-002 + vertex 4.167556e-003 -2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal 1.833614e-001 -6.843139e-001 -7.057571e-001 + outer loop + vertex 4.341205e-003 -2.462019e-002 7.400000e-002 + vertex 4.167556e-003 -2.363539e-002 7.300000e-002 + vertex 8.550503e-003 -2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal 1.833613e-001 -6.843144e-001 -7.057567e-001 + outer loop + vertex 8.550503e-003 -2.349232e-002 7.400000e-002 + vertex 4.167556e-003 -2.363539e-002 7.300000e-002 + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 1.833616e-001 -6.843139e-001 -7.057571e-001 + outer loop + vertex 8.550503e-003 -2.349232e-002 7.400000e-002 + vertex 6.625000e-003 -2.297692e-002 7.300000e-002 + vertex 8.208483e-003 -2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal -2.994057e-001 -6.420775e-001 -7.057568e-001 + outer loop + vertex -1.250000e-002 -2.165063e-002 7.400000e-002 + vertex -1.188257e-002 -2.083937e-002 7.300000e-002 + vertex -8.550503e-003 -2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal -2.994056e-001 -6.420774e-001 -7.057570e-001 + outer loop + vertex -8.550503e-003 -2.349232e-002 7.400000e-002 + vertex -1.188257e-002 -2.083937e-002 7.300000e-002 + vertex -8.208483e-003 -2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal -1.833616e-001 -6.843139e-001 -7.057571e-001 + outer loop + vertex -8.550503e-003 -2.349232e-002 7.400000e-002 + vertex -8.208483e-003 -2.255262e-002 7.300000e-002 + vertex -6.625000e-003 -2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal -5.803331e-001 -4.063502e-001 -7.057571e-001 + outer loop + vertex -2.078461e-002 -1.200000e-002 7.300000e-002 + vertex -2.063257e-002 -1.221714e-002 7.300000e-002 + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal -5.803316e-001 -4.063535e-001 -7.057564e-001 + outer loop + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + vertex -2.063257e-002 -1.221714e-002 7.300000e-002 + vertex -2.005360e-002 -1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal -5.803316e-001 -4.063527e-001 -7.057568e-001 + outer loop + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + vertex -2.005360e-002 -1.304399e-002 7.300000e-002 + vertex -1.915111e-002 -1.606969e-002 7.400000e-002 + endloop + endfacet + facet normal -5.803318e-001 -4.063527e-001 -7.057567e-001 + outer loop + vertex -1.915111e-002 -1.606969e-002 7.400000e-002 + vertex -2.005360e-002 -1.304399e-002 7.300000e-002 + vertex -1.838507e-002 -1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal -5.009529e-001 -5.009529e-001 -7.057566e-001 + outer loop + vertex -1.915111e-002 -1.606969e-002 7.400000e-002 + vertex -1.838507e-002 -1.542690e-002 7.300000e-002 + vertex -1.606969e-002 -1.915111e-002 7.400000e-002 + endloop + endfacet + facet normal -5.009529e-001 -5.009529e-001 -7.057566e-001 + outer loop + vertex -1.606969e-002 -1.915111e-002 7.400000e-002 + vertex -1.838507e-002 -1.542690e-002 7.300000e-002 + vertex -1.542690e-002 -1.838507e-002 7.300000e-002 + endloop + endfacet + facet normal -4.063528e-001 -5.803317e-001 -7.057567e-001 + outer loop + vertex -1.606969e-002 -1.915111e-002 7.400000e-002 + vertex -1.542690e-002 -1.838507e-002 7.300000e-002 + vertex -1.250000e-002 -2.165063e-002 7.400000e-002 + endloop + endfacet + facet normal -4.063525e-001 -5.803316e-001 -7.057570e-001 + outer loop + vertex -1.250000e-002 -2.165063e-002 7.400000e-002 + vertex -1.542690e-002 -1.838507e-002 7.300000e-002 + vertex -1.200000e-002 -2.078461e-002 7.300000e-002 + endloop + endfacet + facet normal -2.994070e-001 -6.420767e-001 -7.057570e-001 + outer loop + vertex -1.250000e-002 -2.165063e-002 7.400000e-002 + vertex -1.200000e-002 -2.078461e-002 7.300000e-002 + vertex -1.188257e-002 -2.083937e-002 7.300000e-002 + endloop + endfacet + facet normal -5.803331e-001 4.063502e-001 -7.057571e-001 + outer loop + vertex -2.063257e-002 1.221714e-002 7.300000e-002 + vertex -2.078461e-002 1.200000e-002 7.300000e-002 + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal -6.420774e-001 2.994057e-001 -7.057570e-001 + outer loop + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + vertex -2.078461e-002 1.200000e-002 7.300000e-002 + vertex -2.126980e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -6.420774e-001 2.994057e-001 -7.057570e-001 + outer loop + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + vertex -2.126980e-002 1.095950e-002 7.300000e-002 + vertex -2.349232e-002 8.550503e-003 7.400000e-002 + endloop + endfacet + facet normal -6.420773e-001 2.994056e-001 -7.057570e-001 + outer loop + vertex -2.349232e-002 8.550503e-003 7.400000e-002 + vertex -2.126980e-002 1.095950e-002 7.300000e-002 + vertex -2.255262e-002 8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal -6.843140e-001 1.833614e-001 -7.057570e-001 + outer loop + vertex -2.349232e-002 8.550503e-003 7.400000e-002 + vertex -2.255262e-002 8.208483e-003 7.300000e-002 + vertex -2.462019e-002 4.341205e-003 7.400000e-002 + endloop + endfacet + facet normal -6.843140e-001 1.833614e-001 -7.057571e-001 + outer loop + vertex -2.462019e-002 4.341205e-003 7.400000e-002 + vertex -2.255262e-002 8.208483e-003 7.300000e-002 + vertex -2.363539e-002 4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal -7.057580e-001 6.174586e-002 -7.057571e-001 + outer loop + vertex -2.462019e-002 4.341205e-003 7.400000e-002 + vertex -2.363539e-002 4.167556e-003 7.300000e-002 + vertex -2.500000e-002 4.857226e-018 7.400000e-002 + endloop + endfacet + facet normal -7.057582e-001 6.174596e-002 -7.057569e-001 + outer loop + vertex -2.500000e-002 4.857226e-018 7.400000e-002 + vertex -2.363539e-002 4.167556e-003 7.300000e-002 + vertex -2.400000e-002 4.662937e-018 7.300000e-002 + endloop + endfacet + facet normal -7.057582e-001 -6.174587e-002 -7.057569e-001 + outer loop + vertex -2.500000e-002 4.857226e-018 7.400000e-002 + vertex -2.400000e-002 4.662937e-018 7.300000e-002 + vertex -2.462019e-002 -4.341205e-003 7.400000e-002 + endloop + endfacet + facet normal -7.057580e-001 -6.174595e-002 -7.057571e-001 + outer loop + vertex -2.462019e-002 -4.341205e-003 7.400000e-002 + vertex -2.400000e-002 4.662937e-018 7.300000e-002 + vertex -2.363539e-002 -4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal -6.843139e-001 -1.833614e-001 -7.057571e-001 + outer loop + vertex -2.462019e-002 -4.341205e-003 7.400000e-002 + vertex -2.363539e-002 -4.167556e-003 7.300000e-002 + vertex -2.349232e-002 -8.550503e-003 7.400000e-002 + endloop + endfacet + facet normal -6.843140e-001 -1.833614e-001 -7.057570e-001 + outer loop + vertex -2.349232e-002 -8.550503e-003 7.400000e-002 + vertex -2.363539e-002 -4.167556e-003 7.300000e-002 + vertex -2.255262e-002 -8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal -6.420773e-001 -2.994056e-001 -7.057570e-001 + outer loop + vertex -2.349232e-002 -8.550503e-003 7.400000e-002 + vertex -2.255262e-002 -8.208483e-003 7.300000e-002 + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal -6.420774e-001 -2.994056e-001 -7.057569e-001 + outer loop + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + vertex -2.255262e-002 -8.208483e-003 7.300000e-002 + vertex -2.126980e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal -6.420774e-001 -2.994057e-001 -7.057570e-001 + outer loop + vertex -2.165063e-002 -1.250000e-002 7.400000e-002 + vertex -2.126980e-002 -1.095950e-002 7.300000e-002 + vertex -2.078461e-002 -1.200000e-002 7.300000e-002 + endloop + endfacet + facet normal -1.833616e-001 6.843139e-001 -7.057571e-001 + outer loop + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + vertex -8.208483e-003 2.255262e-002 7.300000e-002 + vertex -8.550503e-003 2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal -2.994056e-001 6.420774e-001 -7.057570e-001 + outer loop + vertex -8.550503e-003 2.349232e-002 7.400000e-002 + vertex -8.208483e-003 2.255262e-002 7.300000e-002 + vertex -1.188257e-002 2.083937e-002 7.300000e-002 + endloop + endfacet + facet normal -2.994057e-001 6.420775e-001 -7.057568e-001 + outer loop + vertex -8.550503e-003 2.349232e-002 7.400000e-002 + vertex -1.188257e-002 2.083937e-002 7.300000e-002 + vertex -1.250000e-002 2.165063e-002 7.400000e-002 + endloop + endfacet + facet normal -2.994070e-001 6.420767e-001 -7.057570e-001 + outer loop + vertex -1.250000e-002 2.165063e-002 7.400000e-002 + vertex -1.188257e-002 2.083937e-002 7.300000e-002 + vertex -1.200000e-002 2.078461e-002 7.300000e-002 + endloop + endfacet + facet normal -4.063527e-001 5.803315e-001 -7.057569e-001 + outer loop + vertex -1.250000e-002 2.165063e-002 7.400000e-002 + vertex -1.200000e-002 2.078461e-002 7.300000e-002 + vertex -1.606969e-002 1.915111e-002 7.400000e-002 + endloop + endfacet + facet normal -4.063527e-001 5.803318e-001 -7.057567e-001 + outer loop + vertex -1.606969e-002 1.915111e-002 7.400000e-002 + vertex -1.200000e-002 2.078461e-002 7.300000e-002 + vertex -1.542690e-002 1.838507e-002 7.300000e-002 + endloop + endfacet + facet normal -5.009529e-001 5.009529e-001 -7.057566e-001 + outer loop + vertex -1.606969e-002 1.915111e-002 7.400000e-002 + vertex -1.542690e-002 1.838507e-002 7.300000e-002 + vertex -1.915111e-002 1.606969e-002 7.400000e-002 + endloop + endfacet + facet normal -5.009529e-001 5.009529e-001 -7.057566e-001 + outer loop + vertex -1.915111e-002 1.606969e-002 7.400000e-002 + vertex -1.542690e-002 1.838507e-002 7.300000e-002 + vertex -1.838507e-002 1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal -5.803317e-001 4.063528e-001 -7.057567e-001 + outer loop + vertex -1.915111e-002 1.606969e-002 7.400000e-002 + vertex -1.838507e-002 1.542690e-002 7.300000e-002 + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal -5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + vertex -1.838507e-002 1.542690e-002 7.300000e-002 + vertex -2.005360e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal -5.803316e-001 4.063535e-001 -7.057564e-001 + outer loop + vertex -2.165063e-002 1.250000e-002 7.400000e-002 + vertex -2.005360e-002 1.304399e-002 7.300000e-002 + vertex -2.063257e-002 1.221714e-002 7.300000e-002 + endloop + endfacet + facet normal -1.833613e-001 6.843144e-001 -7.057567e-001 + outer loop + vertex -6.625000e-003 2.297692e-002 7.300000e-002 + vertex -8.550503e-003 2.349232e-002 7.400000e-002 + vertex -4.167556e-003 2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal -1.833614e-001 6.843139e-001 -7.057571e-001 + outer loop + vertex -4.167556e-003 2.363539e-002 7.300000e-002 + vertex -8.550503e-003 2.349232e-002 7.400000e-002 + vertex -4.341205e-003 2.462019e-002 7.400000e-002 + endloop + endfacet + facet normal -6.174595e-002 7.057580e-001 -7.057571e-001 + outer loop + vertex -4.167556e-003 2.363539e-002 7.300000e-002 + vertex -4.341205e-003 2.462019e-002 7.400000e-002 + vertex 3.330669e-018 2.400000e-002 7.300000e-002 + endloop + endfacet + facet normal -6.174587e-002 7.057582e-001 -7.057569e-001 + outer loop + vertex 3.330669e-018 2.400000e-002 7.300000e-002 + vertex -4.341205e-003 2.462019e-002 7.400000e-002 + vertex 3.469447e-018 2.500000e-002 7.400000e-002 + endloop + endfacet + facet normal 6.174596e-002 7.057582e-001 -7.057569e-001 + outer loop + vertex 3.330669e-018 2.400000e-002 7.300000e-002 + vertex 3.469447e-018 2.500000e-002 7.400000e-002 + vertex 4.167556e-003 2.363539e-002 7.300000e-002 + endloop + endfacet + facet normal 6.174586e-002 7.057580e-001 -7.057571e-001 + outer loop + vertex 4.167556e-003 2.363539e-002 7.300000e-002 + vertex 3.469447e-018 2.500000e-002 7.400000e-002 + vertex 4.341205e-003 2.462019e-002 7.400000e-002 + endloop + endfacet + facet normal 1.833612e-001 6.843140e-001 -7.057571e-001 + outer loop + vertex 4.167556e-003 2.363539e-002 7.300000e-002 + vertex 4.341205e-003 2.462019e-002 7.400000e-002 + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + endloop + endfacet + facet normal 1.833614e-001 6.843142e-001 -7.057568e-001 + outer loop + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + vertex 4.341205e-003 2.462019e-002 7.400000e-002 + vertex 8.550503e-003 2.349232e-002 7.400000e-002 + endloop + endfacet + facet normal 1.833616e-001 6.843139e-001 -7.057571e-001 + outer loop + vertex 6.625000e-003 2.297692e-002 7.300000e-002 + vertex 8.550503e-003 2.349232e-002 7.400000e-002 + vertex 8.208483e-003 2.255262e-002 7.300000e-002 + endloop + endfacet + facet normal 2.994056e-001 6.420774e-001 -7.057570e-001 + outer loop + vertex 8.208483e-003 2.255262e-002 7.300000e-002 + vertex 8.550503e-003 2.349232e-002 7.400000e-002 + vertex 1.188257e-002 2.083937e-002 7.300000e-002 + endloop + endfacet + facet normal 2.994057e-001 6.420775e-001 -7.057568e-001 + outer loop + vertex 1.188257e-002 2.083937e-002 7.300000e-002 + vertex 8.550503e-003 2.349232e-002 7.400000e-002 + vertex 1.250000e-002 2.165063e-002 7.400000e-002 + endloop + endfacet + facet normal 2.994070e-001 6.420767e-001 -7.057570e-001 + outer loop + vertex 1.188257e-002 2.083937e-002 7.300000e-002 + vertex 1.250000e-002 2.165063e-002 7.400000e-002 + vertex 1.200000e-002 2.078461e-002 7.300000e-002 + endloop + endfacet + facet normal 4.063525e-001 5.803316e-001 -7.057570e-001 + outer loop + vertex 1.200000e-002 2.078461e-002 7.300000e-002 + vertex 1.250000e-002 2.165063e-002 7.400000e-002 + vertex 1.542690e-002 1.838507e-002 7.300000e-002 + endloop + endfacet + facet normal 4.063528e-001 5.803317e-001 -7.057567e-001 + outer loop + vertex 1.542690e-002 1.838507e-002 7.300000e-002 + vertex 1.250000e-002 2.165063e-002 7.400000e-002 + vertex 1.606969e-002 1.915111e-002 7.400000e-002 + endloop + endfacet + facet normal 5.009529e-001 5.009529e-001 -7.057566e-001 + outer loop + vertex 1.542690e-002 1.838507e-002 7.300000e-002 + vertex 1.606969e-002 1.915111e-002 7.400000e-002 + vertex 1.838507e-002 1.542690e-002 7.300000e-002 + endloop + endfacet + facet normal 5.009529e-001 5.009529e-001 -7.057566e-001 + outer loop + vertex 1.838507e-002 1.542690e-002 7.300000e-002 + vertex 1.606969e-002 1.915111e-002 7.400000e-002 + vertex 1.915111e-002 1.606969e-002 7.400000e-002 + endloop + endfacet + facet normal 5.803318e-001 4.063527e-001 -7.057567e-001 + outer loop + vertex 1.838507e-002 1.542690e-002 7.300000e-002 + vertex 1.915111e-002 1.606969e-002 7.400000e-002 + vertex 2.005360e-002 1.304399e-002 7.300000e-002 + endloop + endfacet + facet normal 5.803316e-001 4.063527e-001 -7.057568e-001 + outer loop + vertex 2.005360e-002 1.304399e-002 7.300000e-002 + vertex 1.915111e-002 1.606969e-002 7.400000e-002 + vertex 2.165063e-002 1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal 5.803316e-001 4.063526e-001 -7.057570e-001 + outer loop + vertex 2.005360e-002 1.304399e-002 7.300000e-002 + vertex 2.165063e-002 1.250000e-002 7.400000e-002 + vertex 2.078461e-002 1.200000e-002 7.300000e-002 + endloop + endfacet + facet normal 5.803316e-001 -4.063526e-001 -7.057570e-001 + outer loop + vertex 2.005360e-002 -1.304399e-002 7.300000e-002 + vertex 2.078461e-002 -1.200000e-002 7.300000e-002 + vertex 2.165063e-002 -1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal 6.420774e-001 -2.994057e-001 -7.057570e-001 + outer loop + vertex 2.165063e-002 -1.250000e-002 7.400000e-002 + vertex 2.078461e-002 -1.200000e-002 7.300000e-002 + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 6.420774e-001 -2.994057e-001 -7.057570e-001 + outer loop + vertex 2.165063e-002 -1.250000e-002 7.400000e-002 + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + vertex 2.349232e-002 -8.550503e-003 7.400000e-002 + endloop + endfacet + facet normal 6.420773e-001 -2.994056e-001 -7.057570e-001 + outer loop + vertex 2.349232e-002 -8.550503e-003 7.400000e-002 + vertex 2.126980e-002 -1.095950e-002 7.300000e-002 + vertex 2.255262e-002 -8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal 6.843140e-001 -1.833614e-001 -7.057570e-001 + outer loop + vertex 2.349232e-002 -8.550503e-003 7.400000e-002 + vertex 2.255262e-002 -8.208483e-003 7.300000e-002 + vertex 2.462019e-002 -4.341205e-003 7.400000e-002 + endloop + endfacet + facet normal 6.843140e-001 -1.833614e-001 -7.057571e-001 + outer loop + vertex 2.462019e-002 -4.341205e-003 7.400000e-002 + vertex 2.255262e-002 -8.208483e-003 7.300000e-002 + vertex 2.363539e-002 -4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal 7.057580e-001 -6.174586e-002 -7.057571e-001 + outer loop + vertex 2.462019e-002 -4.341205e-003 7.400000e-002 + vertex 2.363539e-002 -4.167556e-003 7.300000e-002 + vertex 2.500000e-002 0.000000e+000 7.400000e-002 + endloop + endfacet + facet normal 7.057582e-001 -6.174596e-002 -7.057569e-001 + outer loop + vertex 2.500000e-002 0.000000e+000 7.400000e-002 + vertex 2.363539e-002 -4.167556e-003 7.300000e-002 + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + endloop + endfacet + facet normal 7.057582e-001 6.174587e-002 -7.057569e-001 + outer loop + vertex 2.500000e-002 0.000000e+000 7.400000e-002 + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 2.462019e-002 4.341205e-003 7.400000e-002 + endloop + endfacet + facet normal 7.057580e-001 6.174595e-002 -7.057571e-001 + outer loop + vertex 2.462019e-002 4.341205e-003 7.400000e-002 + vertex 2.400000e-002 0.000000e+000 7.300000e-002 + vertex 2.363539e-002 4.167556e-003 7.300000e-002 + endloop + endfacet + facet normal 6.843139e-001 1.833614e-001 -7.057571e-001 + outer loop + vertex 2.462019e-002 4.341205e-003 7.400000e-002 + vertex 2.363539e-002 4.167556e-003 7.300000e-002 + vertex 2.349232e-002 8.550503e-003 7.400000e-002 + endloop + endfacet + facet normal 6.843140e-001 1.833614e-001 -7.057570e-001 + outer loop + vertex 2.349232e-002 8.550503e-003 7.400000e-002 + vertex 2.363539e-002 4.167556e-003 7.300000e-002 + vertex 2.255262e-002 8.208483e-003 7.300000e-002 + endloop + endfacet + facet normal 6.420773e-001 2.994056e-001 -7.057570e-001 + outer loop + vertex 2.349232e-002 8.550503e-003 7.400000e-002 + vertex 2.255262e-002 8.208483e-003 7.300000e-002 + vertex 2.165063e-002 1.250000e-002 7.400000e-002 + endloop + endfacet + facet normal 6.420774e-001 2.994056e-001 -7.057569e-001 + outer loop + vertex 2.165063e-002 1.250000e-002 7.400000e-002 + vertex 2.255262e-002 8.208483e-003 7.300000e-002 + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + endloop + endfacet + facet normal 6.420774e-001 2.994057e-001 -7.057570e-001 + outer loop + vertex 2.165063e-002 1.250000e-002 7.400000e-002 + vertex 2.126980e-002 1.095950e-002 7.300000e-002 + vertex 2.078461e-002 1.200000e-002 7.300000e-002 + endloop + endfacet + facet normal -5.803294e-001 -4.063510e-001 7.057596e-001 + outer loop + vertex -2.005360e-002 -1.304399e-002 8.300000e-002 + vertex -2.165063e-002 -1.250000e-002 8.200000e-002 + vertex -1.838507e-002 -1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal -5.803296e-001 -4.063513e-001 7.057593e-001 + outer loop + vertex -1.838507e-002 -1.542690e-002 8.300000e-002 + vertex -2.165063e-002 -1.250000e-002 8.200000e-002 + vertex -1.915111e-002 -1.606969e-002 8.200000e-002 + endloop + endfacet + facet normal -5.009511e-001 -5.009511e-001 7.057593e-001 + outer loop + vertex -1.838507e-002 -1.542690e-002 8.300000e-002 + vertex -1.915111e-002 -1.606969e-002 8.200000e-002 + vertex -1.542690e-002 -1.838507e-002 8.300000e-002 + endloop + endfacet + facet normal -5.009511e-001 -5.009511e-001 7.057593e-001 + outer loop + vertex -1.542690e-002 -1.838507e-002 8.300000e-002 + vertex -1.915111e-002 -1.606969e-002 8.200000e-002 + vertex -1.606969e-002 -1.915111e-002 8.200000e-002 + endloop + endfacet + facet normal -4.063512e-001 -5.803297e-001 7.057593e-001 + outer loop + vertex -1.542690e-002 -1.838507e-002 8.300000e-002 + vertex -1.606969e-002 -1.915111e-002 8.200000e-002 + vertex -1.200000e-002 -2.078461e-002 8.300000e-002 + endloop + endfacet + facet normal -4.063511e-001 -5.803294e-001 7.057596e-001 + outer loop + vertex -1.200000e-002 -2.078461e-002 8.300000e-002 + vertex -1.606969e-002 -1.915111e-002 8.200000e-002 + vertex -1.250000e-002 -2.165063e-002 8.200000e-002 + endloop + endfacet + facet normal -2.994059e-001 -6.420743e-001 7.057596e-001 + outer loop + vertex -1.200000e-002 -2.078461e-002 8.300000e-002 + vertex -1.250000e-002 -2.165063e-002 8.200000e-002 + vertex -1.188257e-002 -2.083937e-002 8.300000e-002 + endloop + endfacet + facet normal -2.994046e-001 -6.420751e-001 7.057595e-001 + outer loop + vertex -1.188257e-002 -2.083937e-002 8.300000e-002 + vertex -1.250000e-002 -2.165063e-002 8.200000e-002 + vertex -8.550503e-003 -2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal -2.994045e-001 -6.420750e-001 7.057596e-001 + outer loop + vertex -1.188257e-002 -2.083937e-002 8.300000e-002 + vertex -8.550503e-003 -2.349232e-002 8.200000e-002 + vertex -8.208483e-003 -2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal 1.833608e-001 -6.843116e-001 7.057595e-001 + outer loop + vertex 8.550503e-003 -2.349232e-002 8.200000e-002 + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + vertex 4.341205e-003 -2.462019e-002 8.200000e-002 + endloop + endfacet + facet normal 1.833605e-001 -6.843114e-001 7.057598e-001 + outer loop + vertex 4.341205e-003 -2.462019e-002 8.200000e-002 + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + vertex 4.167556e-003 -2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal 6.174563e-002 -7.057554e-001 7.057597e-001 + outer loop + vertex 4.341205e-003 -2.462019e-002 8.200000e-002 + vertex 4.167556e-003 -2.363539e-002 8.300000e-002 + vertex 3.469447e-018 -2.500000e-002 8.200000e-002 + endloop + endfacet + facet normal 6.174574e-002 -7.057556e-001 7.057595e-001 + outer loop + vertex 3.469447e-018 -2.500000e-002 8.200000e-002 + vertex 4.167556e-003 -2.363539e-002 8.300000e-002 + vertex 3.330669e-018 -2.400000e-002 8.300000e-002 + endloop + endfacet + facet normal -6.174564e-002 -7.057556e-001 7.057595e-001 + outer loop + vertex 3.469447e-018 -2.500000e-002 8.200000e-002 + vertex 3.330669e-018 -2.400000e-002 8.300000e-002 + vertex -4.341205e-003 -2.462019e-002 8.200000e-002 + endloop + endfacet + facet normal -6.174572e-002 -7.057554e-001 7.057597e-001 + outer loop + vertex -4.341205e-003 -2.462019e-002 8.200000e-002 + vertex 3.330669e-018 -2.400000e-002 8.300000e-002 + vertex -4.167556e-003 -2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal -1.833607e-001 -6.843114e-001 7.057597e-001 + outer loop + vertex -4.341205e-003 -2.462019e-002 8.200000e-002 + vertex -4.167556e-003 -2.363539e-002 8.300000e-002 + vertex -8.550503e-003 -2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal -1.833606e-001 -6.843119e-001 7.057593e-001 + outer loop + vertex -8.550503e-003 -2.349232e-002 8.200000e-002 + vertex -4.167556e-003 -2.363539e-002 8.300000e-002 + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal -1.833610e-001 -6.843113e-001 7.057597e-001 + outer loop + vertex -8.550503e-003 -2.349232e-002 8.200000e-002 + vertex -6.625000e-003 -2.297692e-002 8.300000e-002 + vertex -8.208483e-003 -2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal 2.994046e-001 -6.420751e-001 7.057595e-001 + outer loop + vertex 1.250000e-002 -2.165063e-002 8.200000e-002 + vertex 1.188257e-002 -2.083937e-002 8.300000e-002 + vertex 8.550503e-003 -2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal 2.994045e-001 -6.420750e-001 7.057596e-001 + outer loop + vertex 8.550503e-003 -2.349232e-002 8.200000e-002 + vertex 1.188257e-002 -2.083937e-002 8.300000e-002 + vertex 8.208483e-003 -2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal 1.833610e-001 -6.843113e-001 7.057597e-001 + outer loop + vertex 8.550503e-003 -2.349232e-002 8.200000e-002 + vertex 8.208483e-003 -2.255262e-002 8.300000e-002 + vertex 6.625000e-003 -2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal 5.803310e-001 -4.063486e-001 7.057597e-001 + outer loop + vertex 2.078461e-002 -1.200000e-002 8.300000e-002 + vertex 2.063257e-002 -1.221714e-002 8.300000e-002 + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal 5.803294e-001 -4.063520e-001 7.057590e-001 + outer loop + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + vertex 2.063257e-002 -1.221714e-002 8.300000e-002 + vertex 2.005360e-002 -1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 5.803294e-001 -4.063512e-001 7.057595e-001 + outer loop + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + vertex 2.005360e-002 -1.304399e-002 8.300000e-002 + vertex 1.915111e-002 -1.606969e-002 8.200000e-002 + endloop + endfacet + facet normal 5.803297e-001 -4.063512e-001 7.057593e-001 + outer loop + vertex 1.915111e-002 -1.606969e-002 8.200000e-002 + vertex 2.005360e-002 -1.304399e-002 8.300000e-002 + vertex 1.838507e-002 -1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal 5.009511e-001 -5.009511e-001 7.057593e-001 + outer loop + vertex 1.915111e-002 -1.606969e-002 8.200000e-002 + vertex 1.838507e-002 -1.542690e-002 8.300000e-002 + vertex 1.606969e-002 -1.915111e-002 8.200000e-002 + endloop + endfacet + facet normal 5.009511e-001 -5.009511e-001 7.057593e-001 + outer loop + vertex 1.606969e-002 -1.915111e-002 8.200000e-002 + vertex 1.838507e-002 -1.542690e-002 8.300000e-002 + vertex 1.542690e-002 -1.838507e-002 8.300000e-002 + endloop + endfacet + facet normal 4.063513e-001 -5.803296e-001 7.057593e-001 + outer loop + vertex 1.606969e-002 -1.915111e-002 8.200000e-002 + vertex 1.542690e-002 -1.838507e-002 8.300000e-002 + vertex 1.250000e-002 -2.165063e-002 8.200000e-002 + endloop + endfacet + facet normal 4.063511e-001 -5.803294e-001 7.057596e-001 + outer loop + vertex 1.250000e-002 -2.165063e-002 8.200000e-002 + vertex 1.542690e-002 -1.838507e-002 8.300000e-002 + vertex 1.200000e-002 -2.078461e-002 8.300000e-002 + endloop + endfacet + facet normal 2.994059e-001 -6.420743e-001 7.057596e-001 + outer loop + vertex 1.250000e-002 -2.165063e-002 8.200000e-002 + vertex 1.200000e-002 -2.078461e-002 8.300000e-002 + vertex 1.188257e-002 -2.083937e-002 8.300000e-002 + endloop + endfacet + facet normal 5.803310e-001 4.063486e-001 7.057597e-001 + outer loop + vertex 2.063257e-002 1.221714e-002 8.300000e-002 + vertex 2.078461e-002 1.200000e-002 8.300000e-002 + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal 6.420750e-001 2.994046e-001 7.057596e-001 + outer loop + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + vertex 2.078461e-002 1.200000e-002 8.300000e-002 + vertex 2.126980e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 6.420750e-001 2.994046e-001 7.057596e-001 + outer loop + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + vertex 2.126980e-002 1.095950e-002 8.300000e-002 + vertex 2.349232e-002 8.550503e-003 8.200000e-002 + endloop + endfacet + facet normal 6.420749e-001 2.994045e-001 7.057596e-001 + outer loop + vertex 2.349232e-002 8.550503e-003 8.200000e-002 + vertex 2.126980e-002 1.095950e-002 8.300000e-002 + vertex 2.255262e-002 8.208483e-003 8.300000e-002 + endloop + endfacet + facet normal 6.843114e-001 1.833607e-001 7.057597e-001 + outer loop + vertex 2.349232e-002 8.550503e-003 8.200000e-002 + vertex 2.255262e-002 8.208483e-003 8.300000e-002 + vertex 2.462019e-002 4.341205e-003 8.200000e-002 + endloop + endfacet + facet normal 6.843114e-001 1.833607e-001 7.057597e-001 + outer loop + vertex 2.462019e-002 4.341205e-003 8.200000e-002 + vertex 2.255262e-002 8.208483e-003 8.300000e-002 + vertex 2.363539e-002 4.167556e-003 8.300000e-002 + endloop + endfacet + facet normal 7.057554e-001 6.174563e-002 7.057597e-001 + outer loop + vertex 2.462019e-002 4.341205e-003 8.200000e-002 + vertex 2.363539e-002 4.167556e-003 8.300000e-002 + vertex 2.500000e-002 4.857226e-018 8.200000e-002 + endloop + endfacet + facet normal 7.057556e-001 6.174574e-002 7.057595e-001 + outer loop + vertex 2.500000e-002 4.857226e-018 8.200000e-002 + vertex 2.363539e-002 4.167556e-003 8.300000e-002 + vertex 2.400000e-002 4.662937e-018 8.300000e-002 + endloop + endfacet + facet normal 7.057556e-001 -6.174564e-002 7.057595e-001 + outer loop + vertex 2.500000e-002 4.857226e-018 8.200000e-002 + vertex 2.400000e-002 4.662937e-018 8.300000e-002 + vertex 2.462019e-002 -4.341205e-003 8.200000e-002 + endloop + endfacet + facet normal 7.057554e-001 -6.174572e-002 7.057597e-001 + outer loop + vertex 2.462019e-002 -4.341205e-003 8.200000e-002 + vertex 2.400000e-002 4.662937e-018 8.300000e-002 + vertex 2.363539e-002 -4.167556e-003 8.300000e-002 + endloop + endfacet + facet normal 6.843114e-001 -1.833607e-001 7.057597e-001 + outer loop + vertex 2.462019e-002 -4.341205e-003 8.200000e-002 + vertex 2.363539e-002 -4.167556e-003 8.300000e-002 + vertex 2.349232e-002 -8.550503e-003 8.200000e-002 + endloop + endfacet + facet normal 6.843114e-001 -1.833607e-001 7.057597e-001 + outer loop + vertex 2.349232e-002 -8.550503e-003 8.200000e-002 + vertex 2.363539e-002 -4.167556e-003 8.300000e-002 + vertex 2.255262e-002 -8.208483e-003 8.300000e-002 + endloop + endfacet + facet normal 6.420749e-001 -2.994045e-001 7.057596e-001 + outer loop + vertex 2.349232e-002 -8.550503e-003 8.200000e-002 + vertex 2.255262e-002 -8.208483e-003 8.300000e-002 + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal 6.420751e-001 -2.994045e-001 7.057595e-001 + outer loop + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + vertex 2.255262e-002 -8.208483e-003 8.300000e-002 + vertex 2.126980e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 6.420750e-001 -2.994046e-001 7.057596e-001 + outer loop + vertex 2.165063e-002 -1.250000e-002 8.200000e-002 + vertex 2.126980e-002 -1.095950e-002 8.300000e-002 + vertex 2.078461e-002 -1.200000e-002 8.300000e-002 + endloop + endfacet + facet normal 1.833610e-001 6.843113e-001 7.057597e-001 + outer loop + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + vertex 8.208483e-003 2.255262e-002 8.300000e-002 + vertex 8.550503e-003 2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal 2.994045e-001 6.420750e-001 7.057596e-001 + outer loop + vertex 8.550503e-003 2.349232e-002 8.200000e-002 + vertex 8.208483e-003 2.255262e-002 8.300000e-002 + vertex 1.188257e-002 2.083937e-002 8.300000e-002 + endloop + endfacet + facet normal 2.994046e-001 6.420751e-001 7.057595e-001 + outer loop + vertex 8.550503e-003 2.349232e-002 8.200000e-002 + vertex 1.188257e-002 2.083937e-002 8.300000e-002 + vertex 1.250000e-002 2.165063e-002 8.200000e-002 + endloop + endfacet + facet normal 2.994059e-001 6.420743e-001 7.057596e-001 + outer loop + vertex 1.250000e-002 2.165063e-002 8.200000e-002 + vertex 1.188257e-002 2.083937e-002 8.300000e-002 + vertex 1.200000e-002 2.078461e-002 8.300000e-002 + endloop + endfacet + facet normal 4.063511e-001 5.803294e-001 7.057596e-001 + outer loop + vertex 1.250000e-002 2.165063e-002 8.200000e-002 + vertex 1.200000e-002 2.078461e-002 8.300000e-002 + vertex 1.606969e-002 1.915111e-002 8.200000e-002 + endloop + endfacet + facet normal 4.063512e-001 5.803297e-001 7.057593e-001 + outer loop + vertex 1.606969e-002 1.915111e-002 8.200000e-002 + vertex 1.200000e-002 2.078461e-002 8.300000e-002 + vertex 1.542690e-002 1.838507e-002 8.300000e-002 + endloop + endfacet + facet normal 5.009511e-001 5.009511e-001 7.057593e-001 + outer loop + vertex 1.606969e-002 1.915111e-002 8.200000e-002 + vertex 1.542690e-002 1.838507e-002 8.300000e-002 + vertex 1.915111e-002 1.606969e-002 8.200000e-002 + endloop + endfacet + facet normal 5.009511e-001 5.009511e-001 7.057593e-001 + outer loop + vertex 1.915111e-002 1.606969e-002 8.200000e-002 + vertex 1.542690e-002 1.838507e-002 8.300000e-002 + vertex 1.838507e-002 1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal 5.803296e-001 4.063513e-001 7.057593e-001 + outer loop + vertex 1.915111e-002 1.606969e-002 8.200000e-002 + vertex 1.838507e-002 1.542690e-002 8.300000e-002 + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal 5.803294e-001 4.063510e-001 7.057596e-001 + outer loop + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + vertex 1.838507e-002 1.542690e-002 8.300000e-002 + vertex 2.005360e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal 5.803294e-001 4.063520e-001 7.057590e-001 + outer loop + vertex 2.165063e-002 1.250000e-002 8.200000e-002 + vertex 2.005360e-002 1.304399e-002 8.300000e-002 + vertex 2.063257e-002 1.221714e-002 8.300000e-002 + endloop + endfacet + facet normal 1.833606e-001 6.843119e-001 7.057593e-001 + outer loop + vertex 6.625000e-003 2.297692e-002 8.300000e-002 + vertex 8.550503e-003 2.349232e-002 8.200000e-002 + vertex 4.167556e-003 2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal 1.833607e-001 6.843114e-001 7.057597e-001 + outer loop + vertex 4.167556e-003 2.363539e-002 8.300000e-002 + vertex 8.550503e-003 2.349232e-002 8.200000e-002 + vertex 4.341205e-003 2.462019e-002 8.200000e-002 + endloop + endfacet + facet normal 6.174572e-002 7.057554e-001 7.057597e-001 + outer loop + vertex 4.167556e-003 2.363539e-002 8.300000e-002 + vertex 4.341205e-003 2.462019e-002 8.200000e-002 + vertex -3.330669e-018 2.400000e-002 8.300000e-002 + endloop + endfacet + facet normal 6.174564e-002 7.057556e-001 7.057595e-001 + outer loop + vertex -3.330669e-018 2.400000e-002 8.300000e-002 + vertex 4.341205e-003 2.462019e-002 8.200000e-002 + vertex -3.469447e-018 2.500000e-002 8.200000e-002 + endloop + endfacet + facet normal -6.174574e-002 7.057556e-001 7.057595e-001 + outer loop + vertex -3.330669e-018 2.400000e-002 8.300000e-002 + vertex -3.469447e-018 2.500000e-002 8.200000e-002 + vertex -4.167556e-003 2.363539e-002 8.300000e-002 + endloop + endfacet + facet normal -6.174563e-002 7.057554e-001 7.057597e-001 + outer loop + vertex -4.167556e-003 2.363539e-002 8.300000e-002 + vertex -3.469447e-018 2.500000e-002 8.200000e-002 + vertex -4.341205e-003 2.462019e-002 8.200000e-002 + endloop + endfacet + facet normal -1.833605e-001 6.843114e-001 7.057598e-001 + outer loop + vertex -4.167556e-003 2.363539e-002 8.300000e-002 + vertex -4.341205e-003 2.462019e-002 8.200000e-002 + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + endloop + endfacet + facet normal -1.833608e-001 6.843116e-001 7.057595e-001 + outer loop + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + vertex -4.341205e-003 2.462019e-002 8.200000e-002 + vertex -8.550503e-003 2.349232e-002 8.200000e-002 + endloop + endfacet + facet normal -1.833610e-001 6.843113e-001 7.057597e-001 + outer loop + vertex -6.625000e-003 2.297692e-002 8.300000e-002 + vertex -8.550503e-003 2.349232e-002 8.200000e-002 + vertex -8.208483e-003 2.255262e-002 8.300000e-002 + endloop + endfacet + facet normal -2.994045e-001 6.420750e-001 7.057596e-001 + outer loop + vertex -8.208483e-003 2.255262e-002 8.300000e-002 + vertex -8.550503e-003 2.349232e-002 8.200000e-002 + vertex -1.188257e-002 2.083937e-002 8.300000e-002 + endloop + endfacet + facet normal -2.994046e-001 6.420751e-001 7.057595e-001 + outer loop + vertex -1.188257e-002 2.083937e-002 8.300000e-002 + vertex -8.550503e-003 2.349232e-002 8.200000e-002 + vertex -1.250000e-002 2.165063e-002 8.200000e-002 + endloop + endfacet + facet normal -2.994059e-001 6.420743e-001 7.057596e-001 + outer loop + vertex -1.188257e-002 2.083937e-002 8.300000e-002 + vertex -1.250000e-002 2.165063e-002 8.200000e-002 + vertex -1.200000e-002 2.078461e-002 8.300000e-002 + endloop + endfacet + facet normal -4.063511e-001 5.803294e-001 7.057596e-001 + outer loop + vertex -1.200000e-002 2.078461e-002 8.300000e-002 + vertex -1.250000e-002 2.165063e-002 8.200000e-002 + vertex -1.542690e-002 1.838507e-002 8.300000e-002 + endloop + endfacet + facet normal -4.063513e-001 5.803296e-001 7.057593e-001 + outer loop + vertex -1.542690e-002 1.838507e-002 8.300000e-002 + vertex -1.250000e-002 2.165063e-002 8.200000e-002 + vertex -1.606969e-002 1.915111e-002 8.200000e-002 + endloop + endfacet + facet normal -5.009511e-001 5.009511e-001 7.057593e-001 + outer loop + vertex -1.542690e-002 1.838507e-002 8.300000e-002 + vertex -1.606969e-002 1.915111e-002 8.200000e-002 + vertex -1.838507e-002 1.542690e-002 8.300000e-002 + endloop + endfacet + facet normal -5.009511e-001 5.009511e-001 7.057593e-001 + outer loop + vertex -1.838507e-002 1.542690e-002 8.300000e-002 + vertex -1.606969e-002 1.915111e-002 8.200000e-002 + vertex -1.915111e-002 1.606969e-002 8.200000e-002 + endloop + endfacet + facet normal -5.803297e-001 4.063512e-001 7.057593e-001 + outer loop + vertex -1.838507e-002 1.542690e-002 8.300000e-002 + vertex -1.915111e-002 1.606969e-002 8.200000e-002 + vertex -2.005360e-002 1.304399e-002 8.300000e-002 + endloop + endfacet + facet normal -5.803294e-001 4.063512e-001 7.057595e-001 + outer loop + vertex -2.005360e-002 1.304399e-002 8.300000e-002 + vertex -1.915111e-002 1.606969e-002 8.200000e-002 + vertex -2.165063e-002 1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal -5.803294e-001 4.063511e-001 7.057596e-001 + outer loop + vertex -2.005360e-002 1.304399e-002 8.300000e-002 + vertex -2.165063e-002 1.250000e-002 8.200000e-002 + vertex -2.078461e-002 1.200000e-002 8.300000e-002 + endloop + endfacet + facet normal -5.803294e-001 -4.063511e-001 7.057596e-001 + outer loop + vertex -2.005360e-002 -1.304399e-002 8.300000e-002 + vertex -2.078461e-002 -1.200000e-002 8.300000e-002 + vertex -2.165063e-002 -1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal -6.420750e-001 -2.994046e-001 7.057596e-001 + outer loop + vertex -2.165063e-002 -1.250000e-002 8.200000e-002 + vertex -2.078461e-002 -1.200000e-002 8.300000e-002 + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal -6.420750e-001 -2.994046e-001 7.057596e-001 + outer loop + vertex -2.165063e-002 -1.250000e-002 8.200000e-002 + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -2.349232e-002 -8.550503e-003 8.200000e-002 + endloop + endfacet + facet normal -6.420749e-001 -2.994045e-001 7.057596e-001 + outer loop + vertex -2.349232e-002 -8.550503e-003 8.200000e-002 + vertex -2.126980e-002 -1.095950e-002 8.300000e-002 + vertex -2.255262e-002 -8.208483e-003 8.300000e-002 + endloop + endfacet + facet normal -6.843114e-001 -1.833607e-001 7.057597e-001 + outer loop + vertex -2.349232e-002 -8.550503e-003 8.200000e-002 + vertex -2.255262e-002 -8.208483e-003 8.300000e-002 + vertex -2.462019e-002 -4.341205e-003 8.200000e-002 + endloop + endfacet + facet normal -6.843114e-001 -1.833607e-001 7.057597e-001 + outer loop + vertex -2.462019e-002 -4.341205e-003 8.200000e-002 + vertex -2.255262e-002 -8.208483e-003 8.300000e-002 + vertex -2.363539e-002 -4.167556e-003 8.300000e-002 + endloop + endfacet + facet normal -7.057554e-001 -6.174563e-002 7.057597e-001 + outer loop + vertex -2.462019e-002 -4.341205e-003 8.200000e-002 + vertex -2.363539e-002 -4.167556e-003 8.300000e-002 + vertex -2.500000e-002 0.000000e+000 8.200000e-002 + endloop + endfacet + facet normal -7.057556e-001 -6.174574e-002 7.057595e-001 + outer loop + vertex -2.500000e-002 0.000000e+000 8.200000e-002 + vertex -2.363539e-002 -4.167556e-003 8.300000e-002 + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + endloop + endfacet + facet normal -7.057556e-001 6.174564e-002 7.057595e-001 + outer loop + vertex -2.500000e-002 0.000000e+000 8.200000e-002 + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -2.462019e-002 4.341205e-003 8.200000e-002 + endloop + endfacet + facet normal -7.057554e-001 6.174572e-002 7.057597e-001 + outer loop + vertex -2.462019e-002 4.341205e-003 8.200000e-002 + vertex -2.400000e-002 0.000000e+000 8.300000e-002 + vertex -2.363539e-002 4.167556e-003 8.300000e-002 + endloop + endfacet + facet normal -6.843114e-001 1.833607e-001 7.057597e-001 + outer loop + vertex -2.462019e-002 4.341205e-003 8.200000e-002 + vertex -2.363539e-002 4.167556e-003 8.300000e-002 + vertex -2.349232e-002 8.550503e-003 8.200000e-002 + endloop + endfacet + facet normal -6.843114e-001 1.833607e-001 7.057597e-001 + outer loop + vertex -2.349232e-002 8.550503e-003 8.200000e-002 + vertex -2.363539e-002 4.167556e-003 8.300000e-002 + vertex -2.255262e-002 8.208483e-003 8.300000e-002 + endloop + endfacet + facet normal -6.420749e-001 2.994045e-001 7.057596e-001 + outer loop + vertex -2.349232e-002 8.550503e-003 8.200000e-002 + vertex -2.255262e-002 8.208483e-003 8.300000e-002 + vertex -2.165063e-002 1.250000e-002 8.200000e-002 + endloop + endfacet + facet normal -6.420751e-001 2.994045e-001 7.057595e-001 + outer loop + vertex -2.165063e-002 1.250000e-002 8.200000e-002 + vertex -2.255262e-002 8.208483e-003 8.300000e-002 + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal -6.420750e-001 2.994046e-001 7.057596e-001 + outer loop + vertex -2.165063e-002 1.250000e-002 8.200000e-002 + vertex -2.126980e-002 1.095950e-002 8.300000e-002 + vertex -2.078461e-002 1.200000e-002 8.300000e-002 + endloop + endfacet + facet normal -4.063526e-001 -5.803317e-001 7.057567e-001 + outer loop + vertex -6.427876e-003 -7.660444e-003 8.400000e-002 + vertex -6.625000e-003 -8.738546e-003 8.300000e-002 + vertex -5.000000e-003 -8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal -4.063528e-001 -5.803314e-001 7.057569e-001 + outer loop + vertex -5.000000e-003 -8.660254e-003 8.400000e-002 + vertex -6.625000e-003 -8.738546e-003 8.300000e-002 + vertex -5.500000e-003 -9.526280e-003 8.300000e-002 + endloop + endfacet + facet normal -2.994057e-001 -6.420774e-001 7.057570e-001 + outer loop + vertex -5.000000e-003 -8.660254e-003 8.400000e-002 + vertex -5.500000e-003 -9.526280e-003 8.300000e-002 + vertex -3.420201e-003 -9.396926e-003 8.400000e-002 + endloop + endfacet + facet normal -2.994056e-001 -6.420776e-001 7.057568e-001 + outer loop + vertex -3.420201e-003 -9.396926e-003 8.400000e-002 + vertex -5.500000e-003 -9.526280e-003 8.300000e-002 + vertex -3.762222e-003 -1.033662e-002 8.300000e-002 + endloop + endfacet + facet normal -1.833614e-001 -6.843143e-001 7.057568e-001 + outer loop + vertex -3.420201e-003 -9.396926e-003 8.400000e-002 + vertex -3.762222e-003 -1.033662e-002 8.300000e-002 + vertex -1.736482e-003 -9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal -1.833614e-001 -6.843143e-001 7.057567e-001 + outer loop + vertex -3.762222e-003 -1.033662e-002 8.300000e-002 + vertex -1.910130e-003 -1.083289e-002 8.300000e-002 + vertex -1.736482e-003 -9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal -6.174598e-002 -7.057583e-001 7.057567e-001 + outer loop + vertex -1.736482e-003 -9.848078e-003 8.400000e-002 + vertex -1.910130e-003 -1.083289e-002 8.300000e-002 + vertex -4.629575e-004 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal -6.174564e-002 -7.057582e-001 7.057570e-001 + outer loop + vertex -1.736482e-003 -9.848078e-003 8.400000e-002 + vertex -4.629575e-004 -1.095950e-002 8.300000e-002 + vertex 1.387779e-018 -1.000000e-002 8.400000e-002 + endloop + endfacet + facet normal -6.174536e-002 -7.057582e-001 7.057569e-001 + outer loop + vertex -4.629575e-004 -1.095950e-002 8.300000e-002 + vertex 1.526557e-018 -1.100000e-002 8.300000e-002 + vertex 1.387779e-018 -1.000000e-002 8.400000e-002 + endloop + endfacet + facet normal 6.174536e-002 -7.057582e-001 7.057569e-001 + outer loop + vertex 1.387779e-018 -1.000000e-002 8.400000e-002 + vertex 1.526557e-018 -1.100000e-002 8.300000e-002 + vertex 4.629575e-004 -1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 6.174564e-002 -7.057582e-001 7.057570e-001 + outer loop + vertex 1.387779e-018 -1.000000e-002 8.400000e-002 + vertex 4.629575e-004 -1.095950e-002 8.300000e-002 + vertex 1.736482e-003 -9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal 6.174598e-002 -7.057583e-001 7.057567e-001 + outer loop + vertex 1.736482e-003 -9.848078e-003 8.400000e-002 + vertex 4.629575e-004 -1.095950e-002 8.300000e-002 + vertex 1.910130e-003 -1.083289e-002 8.300000e-002 + endloop + endfacet + facet normal 1.833614e-001 -6.843143e-001 7.057567e-001 + outer loop + vertex 1.736482e-003 -9.848078e-003 8.400000e-002 + vertex 1.910130e-003 -1.083289e-002 8.300000e-002 + vertex 3.420201e-003 -9.396926e-003 8.400000e-002 + endloop + endfacet + facet normal 1.833614e-001 -6.843143e-001 7.057568e-001 + outer loop + vertex 3.420201e-003 -9.396926e-003 8.400000e-002 + vertex 1.910130e-003 -1.083289e-002 8.300000e-002 + vertex 3.762222e-003 -1.033662e-002 8.300000e-002 + endloop + endfacet + facet normal 2.994058e-001 -6.420775e-001 7.057568e-001 + outer loop + vertex 3.420201e-003 -9.396926e-003 8.400000e-002 + vertex 3.762222e-003 -1.033662e-002 8.300000e-002 + vertex 5.000000e-003 -8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal 2.994055e-001 -6.420774e-001 7.057570e-001 + outer loop + vertex 3.762222e-003 -1.033662e-002 8.300000e-002 + vertex 5.500000e-003 -9.526280e-003 8.300000e-002 + vertex 5.000000e-003 -8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal 4.063528e-001 -5.803314e-001 7.057569e-001 + outer loop + vertex 5.000000e-003 -8.660254e-003 8.400000e-002 + vertex 5.500000e-003 -9.526280e-003 8.300000e-002 + vertex 6.625000e-003 -8.738546e-003 8.300000e-002 + endloop + endfacet + facet normal 4.063526e-001 -5.803317e-001 7.057567e-001 + outer loop + vertex 5.000000e-003 -8.660254e-003 8.400000e-002 + vertex 6.625000e-003 -8.738546e-003 8.300000e-002 + vertex 6.427876e-003 -7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal 4.063518e-001 -5.803321e-001 7.057570e-001 + outer loop + vertex 6.427876e-003 -7.660444e-003 8.400000e-002 + vertex 6.625000e-003 -8.738546e-003 8.300000e-002 + vertex 7.070664e-003 -8.426489e-003 8.300000e-002 + endloop + endfacet + facet normal 5.009527e-001 -5.009527e-001 7.057570e-001 + outer loop + vertex 6.427876e-003 -7.660444e-003 8.400000e-002 + vertex 7.070664e-003 -8.426489e-003 8.300000e-002 + vertex 7.660444e-003 -6.427876e-003 8.400000e-002 + endloop + endfacet + facet normal 5.009527e-001 -5.009527e-001 7.057570e-001 + outer loop + vertex 7.660444e-003 -6.427876e-003 8.400000e-002 + vertex 7.070664e-003 -8.426489e-003 8.300000e-002 + vertex 8.426489e-003 -7.070664e-003 8.300000e-002 + endloop + endfacet + facet normal 5.803316e-001 -4.063525e-001 7.057570e-001 + outer loop + vertex 7.660444e-003 -6.427876e-003 8.400000e-002 + vertex 8.426489e-003 -7.070664e-003 8.300000e-002 + vertex 8.660254e-003 -5.000000e-003 8.400000e-002 + endloop + endfacet + facet normal 5.803316e-001 -4.063525e-001 7.057570e-001 + outer loop + vertex 8.660254e-003 -5.000000e-003 8.400000e-002 + vertex 8.426489e-003 -7.070664e-003 8.300000e-002 + vertex 9.526280e-003 -5.500000e-003 8.300000e-002 + endloop + endfacet + facet normal 6.420774e-001 -2.994057e-001 7.057570e-001 + outer loop + vertex 8.660254e-003 -5.000000e-003 8.400000e-002 + vertex 9.526280e-003 -5.500000e-003 8.300000e-002 + vertex 9.396926e-003 -3.420201e-003 8.400000e-002 + endloop + endfacet + facet normal 6.420776e-001 -2.994056e-001 7.057568e-001 + outer loop + vertex 9.396926e-003 -3.420201e-003 8.400000e-002 + vertex 9.526280e-003 -5.500000e-003 8.300000e-002 + vertex 1.033662e-002 -3.762222e-003 8.300000e-002 + endloop + endfacet + facet normal 6.843143e-001 -1.833614e-001 7.057568e-001 + outer loop + vertex 9.396926e-003 -3.420201e-003 8.400000e-002 + vertex 1.033662e-002 -3.762222e-003 8.300000e-002 + vertex 9.848078e-003 -1.736482e-003 8.400000e-002 + endloop + endfacet + facet normal 6.843143e-001 -1.833614e-001 7.057567e-001 + outer loop + vertex 9.848078e-003 -1.736482e-003 8.400000e-002 + vertex 1.033662e-002 -3.762222e-003 8.300000e-002 + vertex 1.083289e-002 -1.910130e-003 8.300000e-002 + endloop + endfacet + facet normal 7.057584e-001 -6.174567e-002 7.057567e-001 + outer loop + vertex 9.848078e-003 -1.736482e-003 8.400000e-002 + vertex 1.083289e-002 -1.910130e-003 8.300000e-002 + vertex 1.000000e-002 1.942890e-018 8.400000e-002 + endloop + endfacet + facet normal 7.057582e-001 -6.174582e-002 7.057569e-001 + outer loop + vertex 1.000000e-002 1.942890e-018 8.400000e-002 + vertex 1.083289e-002 -1.910130e-003 8.300000e-002 + vertex 1.100000e-002 2.137179e-018 8.300000e-002 + endloop + endfacet + facet normal 7.057582e-001 6.174565e-002 7.057569e-001 + outer loop + vertex 1.000000e-002 1.942890e-018 8.400000e-002 + vertex 1.100000e-002 2.137179e-018 8.300000e-002 + vertex 9.848078e-003 1.736482e-003 8.400000e-002 + endloop + endfacet + facet normal 7.057583e-001 6.174584e-002 7.057567e-001 + outer loop + vertex 9.848078e-003 1.736482e-003 8.400000e-002 + vertex 1.100000e-002 2.137179e-018 8.300000e-002 + vertex 1.083289e-002 1.910130e-003 8.300000e-002 + endloop + endfacet + facet normal 6.843143e-001 1.833614e-001 7.057567e-001 + outer loop + vertex 9.848078e-003 1.736482e-003 8.400000e-002 + vertex 1.083289e-002 1.910130e-003 8.300000e-002 + vertex 9.396926e-003 3.420201e-003 8.400000e-002 + endloop + endfacet + facet normal 6.843143e-001 1.833614e-001 7.057568e-001 + outer loop + vertex 9.396926e-003 3.420201e-003 8.400000e-002 + vertex 1.083289e-002 1.910130e-003 8.300000e-002 + vertex 1.033662e-002 3.762222e-003 8.300000e-002 + endloop + endfacet + facet normal 6.420775e-001 2.994058e-001 7.057568e-001 + outer loop + vertex 9.396926e-003 3.420201e-003 8.400000e-002 + vertex 1.033662e-002 3.762222e-003 8.300000e-002 + vertex 8.660254e-003 5.000000e-003 8.400000e-002 + endloop + endfacet + facet normal 6.420774e-001 2.994055e-001 7.057570e-001 + outer loop + vertex 8.660254e-003 5.000000e-003 8.400000e-002 + vertex 1.033662e-002 3.762222e-003 8.300000e-002 + vertex 9.526280e-003 5.500000e-003 8.300000e-002 + endloop + endfacet + facet normal 5.803316e-001 4.063525e-001 7.057570e-001 + outer loop + vertex 8.660254e-003 5.000000e-003 8.400000e-002 + vertex 9.526280e-003 5.500000e-003 8.300000e-002 + vertex 7.660444e-003 6.427876e-003 8.400000e-002 + endloop + endfacet + facet normal 5.803316e-001 4.063525e-001 7.057570e-001 + outer loop + vertex 7.660444e-003 6.427876e-003 8.400000e-002 + vertex 9.526280e-003 5.500000e-003 8.300000e-002 + vertex 8.426489e-003 7.070664e-003 8.300000e-002 + endloop + endfacet + facet normal 5.009527e-001 5.009527e-001 7.057570e-001 + outer loop + vertex 7.660444e-003 6.427876e-003 8.400000e-002 + vertex 8.426489e-003 7.070664e-003 8.300000e-002 + vertex 6.427876e-003 7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal 5.009527e-001 5.009527e-001 7.057570e-001 + outer loop + vertex 8.426489e-003 7.070664e-003 8.300000e-002 + vertex 7.070664e-003 8.426489e-003 8.300000e-002 + vertex 6.427876e-003 7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal 4.063518e-001 5.803321e-001 7.057570e-001 + outer loop + vertex 6.427876e-003 7.660444e-003 8.400000e-002 + vertex 7.070664e-003 8.426489e-003 8.300000e-002 + vertex 6.625000e-003 8.738546e-003 8.300000e-002 + endloop + endfacet + facet normal 4.063526e-001 5.803317e-001 7.057567e-001 + outer loop + vertex 6.427876e-003 7.660444e-003 8.400000e-002 + vertex 6.625000e-003 8.738546e-003 8.300000e-002 + vertex 5.000000e-003 8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal 4.063528e-001 5.803314e-001 7.057569e-001 + outer loop + vertex 5.000000e-003 8.660254e-003 8.400000e-002 + vertex 6.625000e-003 8.738546e-003 8.300000e-002 + vertex 5.500000e-003 9.526280e-003 8.300000e-002 + endloop + endfacet + facet normal 2.994057e-001 6.420774e-001 7.057570e-001 + outer loop + vertex 5.000000e-003 8.660254e-003 8.400000e-002 + vertex 5.500000e-003 9.526280e-003 8.300000e-002 + vertex 3.420201e-003 9.396926e-003 8.400000e-002 + endloop + endfacet + facet normal 2.994056e-001 6.420776e-001 7.057568e-001 + outer loop + vertex 3.420201e-003 9.396926e-003 8.400000e-002 + vertex 5.500000e-003 9.526280e-003 8.300000e-002 + vertex 3.762222e-003 1.033662e-002 8.300000e-002 + endloop + endfacet + facet normal 1.833614e-001 6.843143e-001 7.057568e-001 + outer loop + vertex 3.420201e-003 9.396926e-003 8.400000e-002 + vertex 3.762222e-003 1.033662e-002 8.300000e-002 + vertex 1.736482e-003 9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal 1.833614e-001 6.843143e-001 7.057567e-001 + outer loop + vertex 3.762222e-003 1.033662e-002 8.300000e-002 + vertex 1.910130e-003 1.083289e-002 8.300000e-002 + vertex 1.736482e-003 9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal 6.174598e-002 7.057583e-001 7.057567e-001 + outer loop + vertex 1.736482e-003 9.848078e-003 8.400000e-002 + vertex 1.910130e-003 1.083289e-002 8.300000e-002 + vertex 4.629575e-004 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal 6.174564e-002 7.057582e-001 7.057570e-001 + outer loop + vertex 1.736482e-003 9.848078e-003 8.400000e-002 + vertex 4.629575e-004 1.095950e-002 8.300000e-002 + vertex -1.387779e-018 1.000000e-002 8.400000e-002 + endloop + endfacet + facet normal 6.174536e-002 7.057582e-001 7.057569e-001 + outer loop + vertex 4.629575e-004 1.095950e-002 8.300000e-002 + vertex -1.526557e-018 1.100000e-002 8.300000e-002 + vertex -1.387779e-018 1.000000e-002 8.400000e-002 + endloop + endfacet + facet normal -6.174536e-002 7.057582e-001 7.057569e-001 + outer loop + vertex -1.387779e-018 1.000000e-002 8.400000e-002 + vertex -1.526557e-018 1.100000e-002 8.300000e-002 + vertex -4.629575e-004 1.095950e-002 8.300000e-002 + endloop + endfacet + facet normal -6.174564e-002 7.057582e-001 7.057570e-001 + outer loop + vertex -1.387779e-018 1.000000e-002 8.400000e-002 + vertex -4.629575e-004 1.095950e-002 8.300000e-002 + vertex -1.736482e-003 9.848078e-003 8.400000e-002 + endloop + endfacet + facet normal -6.174598e-002 7.057583e-001 7.057567e-001 + outer loop + vertex -1.736482e-003 9.848078e-003 8.400000e-002 + vertex -4.629575e-004 1.095950e-002 8.300000e-002 + vertex -1.910130e-003 1.083289e-002 8.300000e-002 + endloop + endfacet + facet normal -1.833614e-001 6.843143e-001 7.057567e-001 + outer loop + vertex -1.736482e-003 9.848078e-003 8.400000e-002 + vertex -1.910130e-003 1.083289e-002 8.300000e-002 + vertex -3.420201e-003 9.396926e-003 8.400000e-002 + endloop + endfacet + facet normal -1.833614e-001 6.843143e-001 7.057568e-001 + outer loop + vertex -3.420201e-003 9.396926e-003 8.400000e-002 + vertex -1.910130e-003 1.083289e-002 8.300000e-002 + vertex -3.762222e-003 1.033662e-002 8.300000e-002 + endloop + endfacet + facet normal -2.994058e-001 6.420775e-001 7.057568e-001 + outer loop + vertex -3.420201e-003 9.396926e-003 8.400000e-002 + vertex -3.762222e-003 1.033662e-002 8.300000e-002 + vertex -5.000000e-003 8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal -2.994055e-001 6.420774e-001 7.057570e-001 + outer loop + vertex -3.762222e-003 1.033662e-002 8.300000e-002 + vertex -5.500000e-003 9.526280e-003 8.300000e-002 + vertex -5.000000e-003 8.660254e-003 8.400000e-002 + endloop + endfacet + facet normal -4.063528e-001 5.803314e-001 7.057569e-001 + outer loop + vertex -5.000000e-003 8.660254e-003 8.400000e-002 + vertex -5.500000e-003 9.526280e-003 8.300000e-002 + vertex -6.625000e-003 8.738546e-003 8.300000e-002 + endloop + endfacet + facet normal -4.063526e-001 5.803317e-001 7.057567e-001 + outer loop + vertex -5.000000e-003 8.660254e-003 8.400000e-002 + vertex -6.625000e-003 8.738546e-003 8.300000e-002 + vertex -6.427876e-003 7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal -4.063518e-001 5.803321e-001 7.057570e-001 + outer loop + vertex -6.427876e-003 7.660444e-003 8.400000e-002 + vertex -6.625000e-003 8.738546e-003 8.300000e-002 + vertex -7.070664e-003 8.426489e-003 8.300000e-002 + endloop + endfacet + facet normal -5.009527e-001 5.009527e-001 7.057570e-001 + outer loop + vertex -6.427876e-003 7.660444e-003 8.400000e-002 + vertex -7.070664e-003 8.426489e-003 8.300000e-002 + vertex -7.660444e-003 6.427876e-003 8.400000e-002 + endloop + endfacet + facet normal -5.009527e-001 5.009527e-001 7.057570e-001 + outer loop + vertex -7.660444e-003 6.427876e-003 8.400000e-002 + vertex -7.070664e-003 8.426489e-003 8.300000e-002 + vertex -8.426489e-003 7.070664e-003 8.300000e-002 + endloop + endfacet + facet normal -5.803316e-001 4.063525e-001 7.057570e-001 + outer loop + vertex -7.660444e-003 6.427876e-003 8.400000e-002 + vertex -8.426489e-003 7.070664e-003 8.300000e-002 + vertex -8.660254e-003 5.000000e-003 8.400000e-002 + endloop + endfacet + facet normal -5.803316e-001 4.063525e-001 7.057570e-001 + outer loop + vertex -8.660254e-003 5.000000e-003 8.400000e-002 + vertex -8.426489e-003 7.070664e-003 8.300000e-002 + vertex -9.526280e-003 5.500000e-003 8.300000e-002 + endloop + endfacet + facet normal -6.420774e-001 2.994057e-001 7.057570e-001 + outer loop + vertex -8.660254e-003 5.000000e-003 8.400000e-002 + vertex -9.526280e-003 5.500000e-003 8.300000e-002 + vertex -9.396926e-003 3.420201e-003 8.400000e-002 + endloop + endfacet + facet normal -6.420776e-001 2.994056e-001 7.057568e-001 + outer loop + vertex -9.396926e-003 3.420201e-003 8.400000e-002 + vertex -9.526280e-003 5.500000e-003 8.300000e-002 + vertex -1.033662e-002 3.762222e-003 8.300000e-002 + endloop + endfacet + facet normal -6.843143e-001 1.833614e-001 7.057568e-001 + outer loop + vertex -9.396926e-003 3.420201e-003 8.400000e-002 + vertex -1.033662e-002 3.762222e-003 8.300000e-002 + vertex -9.848078e-003 1.736482e-003 8.400000e-002 + endloop + endfacet + facet normal -6.843143e-001 1.833614e-001 7.057567e-001 + outer loop + vertex -9.848078e-003 1.736482e-003 8.400000e-002 + vertex -1.033662e-002 3.762222e-003 8.300000e-002 + vertex -1.083289e-002 1.910130e-003 8.300000e-002 + endloop + endfacet + facet normal -7.057584e-001 6.174567e-002 7.057567e-001 + outer loop + vertex -9.848078e-003 1.736482e-003 8.400000e-002 + vertex -1.083289e-002 1.910130e-003 8.300000e-002 + vertex -1.000000e-002 0.000000e+000 8.400000e-002 + endloop + endfacet + facet normal -7.057582e-001 6.174582e-002 7.057569e-001 + outer loop + vertex -1.000000e-002 0.000000e+000 8.400000e-002 + vertex -1.083289e-002 1.910130e-003 8.300000e-002 + vertex -1.100000e-002 0.000000e+000 8.300000e-002 + endloop + endfacet + facet normal -7.057582e-001 -6.174565e-002 7.057569e-001 + outer loop + vertex -1.000000e-002 0.000000e+000 8.400000e-002 + vertex -1.100000e-002 0.000000e+000 8.300000e-002 + vertex -9.848078e-003 -1.736482e-003 8.400000e-002 + endloop + endfacet + facet normal -7.057583e-001 -6.174584e-002 7.057567e-001 + outer loop + vertex -9.848078e-003 -1.736482e-003 8.400000e-002 + vertex -1.100000e-002 0.000000e+000 8.300000e-002 + vertex -1.083289e-002 -1.910130e-003 8.300000e-002 + endloop + endfacet + facet normal -6.843143e-001 -1.833614e-001 7.057567e-001 + outer loop + vertex -9.848078e-003 -1.736482e-003 8.400000e-002 + vertex -1.083289e-002 -1.910130e-003 8.300000e-002 + vertex -9.396926e-003 -3.420201e-003 8.400000e-002 + endloop + endfacet + facet normal -6.843143e-001 -1.833614e-001 7.057568e-001 + outer loop + vertex -9.396926e-003 -3.420201e-003 8.400000e-002 + vertex -1.083289e-002 -1.910130e-003 8.300000e-002 + vertex -1.033662e-002 -3.762222e-003 8.300000e-002 + endloop + endfacet + facet normal -6.420775e-001 -2.994058e-001 7.057568e-001 + outer loop + vertex -9.396926e-003 -3.420201e-003 8.400000e-002 + vertex -1.033662e-002 -3.762222e-003 8.300000e-002 + vertex -8.660254e-003 -5.000000e-003 8.400000e-002 + endloop + endfacet + facet normal -6.420774e-001 -2.994055e-001 7.057570e-001 + outer loop + vertex -8.660254e-003 -5.000000e-003 8.400000e-002 + vertex -1.033662e-002 -3.762222e-003 8.300000e-002 + vertex -9.526280e-003 -5.500000e-003 8.300000e-002 + endloop + endfacet + facet normal -5.803316e-001 -4.063525e-001 7.057570e-001 + outer loop + vertex -8.660254e-003 -5.000000e-003 8.400000e-002 + vertex -9.526280e-003 -5.500000e-003 8.300000e-002 + vertex -7.660444e-003 -6.427876e-003 8.400000e-002 + endloop + endfacet + facet normal -5.803316e-001 -4.063525e-001 7.057570e-001 + outer loop + vertex -7.660444e-003 -6.427876e-003 8.400000e-002 + vertex -9.526280e-003 -5.500000e-003 8.300000e-002 + vertex -8.426489e-003 -7.070664e-003 8.300000e-002 + endloop + endfacet + facet normal -5.009527e-001 -5.009527e-001 7.057570e-001 + outer loop + vertex -7.660444e-003 -6.427876e-003 8.400000e-002 + vertex -8.426489e-003 -7.070664e-003 8.300000e-002 + vertex -6.427876e-003 -7.660444e-003 8.400000e-002 + endloop + endfacet + facet normal -5.009527e-001 -5.009527e-001 7.057570e-001 + outer loop + vertex -6.427876e-003 -7.660444e-003 8.400000e-002 + vertex -8.426489e-003 -7.070664e-003 8.300000e-002 + vertex -7.070664e-003 -8.426489e-003 8.300000e-002 + endloop + endfacet + facet normal -4.063518e-001 -5.803321e-001 7.057570e-001 + outer loop + vertex -6.427876e-003 -7.660444e-003 8.400000e-002 + vertex -7.070664e-003 -8.426489e-003 8.300000e-002 + vertex -6.625000e-003 -8.738546e-003 8.300000e-002 + endloop + endfacet + facet normal 6.843143e-001 1.833614e-001 -7.057567e-001 + outer loop + vertex 1.083289e-002 1.910130e-003 1.230000e-001 + vertex 9.848078e-003 1.736482e-003 1.220000e-001 + vertex 1.033662e-002 3.762222e-003 1.230000e-001 + endloop + endfacet + facet normal 6.843143e-001 1.833615e-001 -7.057568e-001 + outer loop + vertex 1.033662e-002 3.762222e-003 1.230000e-001 + vertex 9.848078e-003 1.736482e-003 1.220000e-001 + vertex 9.396926e-003 3.420201e-003 1.220000e-001 + endloop + endfacet + facet normal 6.420776e-001 2.994056e-001 -7.057568e-001 + outer loop + vertex 1.033662e-002 3.762222e-003 1.230000e-001 + vertex 9.396926e-003 3.420201e-003 1.220000e-001 + vertex 9.526280e-003 5.500000e-003 1.230000e-001 + endloop + endfacet + facet normal 6.420774e-001 2.994057e-001 -7.057570e-001 + outer loop + vertex 9.526280e-003 5.500000e-003 1.230000e-001 + vertex 9.396926e-003 3.420201e-003 1.220000e-001 + vertex 8.660254e-003 5.000000e-003 1.220000e-001 + endloop + endfacet + facet normal 5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex 9.526280e-003 5.500000e-003 1.230000e-001 + vertex 8.660254e-003 5.000000e-003 1.220000e-001 + vertex 8.426489e-003 7.070664e-003 1.230000e-001 + endloop + endfacet + facet normal 5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex 8.426489e-003 7.070664e-003 1.230000e-001 + vertex 8.660254e-003 5.000000e-003 1.220000e-001 + vertex 7.660444e-003 6.427876e-003 1.220000e-001 + endloop + endfacet + facet normal 5.009527e-001 5.009527e-001 -7.057570e-001 + outer loop + vertex 8.426489e-003 7.070664e-003 1.230000e-001 + vertex 7.660444e-003 6.427876e-003 1.220000e-001 + vertex 7.070664e-003 8.426489e-003 1.230000e-001 + endloop + endfacet + facet normal 5.009527e-001 5.009527e-001 -7.057570e-001 + outer loop + vertex 7.070664e-003 8.426489e-003 1.230000e-001 + vertex 7.660444e-003 6.427876e-003 1.220000e-001 + vertex 6.427876e-003 7.660444e-003 1.220000e-001 + endloop + endfacet + facet normal 4.063525e-001 5.803316e-001 -7.057570e-001 + outer loop + vertex 7.070664e-003 8.426489e-003 1.230000e-001 + vertex 6.427876e-003 7.660444e-003 1.220000e-001 + vertex 5.500000e-003 9.526280e-003 1.230000e-001 + endloop + endfacet + facet normal 4.063525e-001 5.803316e-001 -7.057570e-001 + outer loop + vertex 5.500000e-003 9.526280e-003 1.230000e-001 + vertex 6.427876e-003 7.660444e-003 1.220000e-001 + vertex 5.000000e-003 8.660254e-003 1.220000e-001 + endloop + endfacet + facet normal 2.994055e-001 6.420774e-001 -7.057570e-001 + outer loop + vertex 5.500000e-003 9.526280e-003 1.230000e-001 + vertex 5.000000e-003 8.660254e-003 1.220000e-001 + vertex 3.762222e-003 1.033662e-002 1.230000e-001 + endloop + endfacet + facet normal 2.994058e-001 6.420775e-001 -7.057568e-001 + outer loop + vertex 3.762222e-003 1.033662e-002 1.230000e-001 + vertex 5.000000e-003 8.660254e-003 1.220000e-001 + vertex 3.420201e-003 9.396926e-003 1.220000e-001 + endloop + endfacet + facet normal 1.833614e-001 6.843143e-001 -7.057568e-001 + outer loop + vertex 3.762222e-003 1.033662e-002 1.230000e-001 + vertex 3.420201e-003 9.396926e-003 1.220000e-001 + vertex 1.910130e-003 1.083289e-002 1.230000e-001 + endloop + endfacet + facet normal 1.833614e-001 6.843143e-001 -7.057567e-001 + outer loop + vertex 1.910130e-003 1.083289e-002 1.230000e-001 + vertex 3.420201e-003 9.396926e-003 1.220000e-001 + vertex 1.736482e-003 9.848078e-003 1.220000e-001 + endloop + endfacet + facet normal 6.174584e-002 7.057583e-001 -7.057567e-001 + outer loop + vertex 1.910130e-003 1.083289e-002 1.230000e-001 + vertex 1.736482e-003 9.848078e-003 1.220000e-001 + vertex 1.526557e-018 1.100000e-002 1.230000e-001 + endloop + endfacet + facet normal 6.174565e-002 7.057582e-001 -7.057569e-001 + outer loop + vertex 1.526557e-018 1.100000e-002 1.230000e-001 + vertex 1.736482e-003 9.848078e-003 1.220000e-001 + vertex 1.387779e-018 1.000000e-002 1.220000e-001 + endloop + endfacet + facet normal -6.174582e-002 7.057582e-001 -7.057569e-001 + outer loop + vertex 1.526557e-018 1.100000e-002 1.230000e-001 + vertex 1.387779e-018 1.000000e-002 1.220000e-001 + vertex -1.910130e-003 1.083289e-002 1.230000e-001 + endloop + endfacet + facet normal -6.174567e-002 7.057584e-001 -7.057567e-001 + outer loop + vertex -1.910130e-003 1.083289e-002 1.230000e-001 + vertex 1.387779e-018 1.000000e-002 1.220000e-001 + vertex -1.736482e-003 9.848078e-003 1.220000e-001 + endloop + endfacet + facet normal -1.833614e-001 6.843143e-001 -7.057567e-001 + outer loop + vertex -1.910130e-003 1.083289e-002 1.230000e-001 + vertex -1.736482e-003 9.848078e-003 1.220000e-001 + vertex -3.762222e-003 1.033662e-002 1.230000e-001 + endloop + endfacet + facet normal -1.833615e-001 6.843143e-001 -7.057568e-001 + outer loop + vertex -3.762222e-003 1.033662e-002 1.230000e-001 + vertex -1.736482e-003 9.848078e-003 1.220000e-001 + vertex -3.420201e-003 9.396926e-003 1.220000e-001 + endloop + endfacet + facet normal -2.994056e-001 6.420776e-001 -7.057568e-001 + outer loop + vertex -3.762222e-003 1.033662e-002 1.230000e-001 + vertex -3.420201e-003 9.396926e-003 1.220000e-001 + vertex -5.500000e-003 9.526280e-003 1.230000e-001 + endloop + endfacet + facet normal -2.994057e-001 6.420774e-001 -7.057570e-001 + outer loop + vertex -5.500000e-003 9.526280e-003 1.230000e-001 + vertex -3.420201e-003 9.396926e-003 1.220000e-001 + vertex -5.000000e-003 8.660254e-003 1.220000e-001 + endloop + endfacet + facet normal -4.063525e-001 5.803316e-001 -7.057570e-001 + outer loop + vertex -5.500000e-003 9.526280e-003 1.230000e-001 + vertex -5.000000e-003 8.660254e-003 1.220000e-001 + vertex -7.070664e-003 8.426489e-003 1.230000e-001 + endloop + endfacet + facet normal -4.063525e-001 5.803316e-001 -7.057570e-001 + outer loop + vertex -7.070664e-003 8.426489e-003 1.230000e-001 + vertex -5.000000e-003 8.660254e-003 1.220000e-001 + vertex -6.427876e-003 7.660444e-003 1.220000e-001 + endloop + endfacet + facet normal -5.009527e-001 5.009527e-001 -7.057570e-001 + outer loop + vertex -7.070664e-003 8.426489e-003 1.230000e-001 + vertex -6.427876e-003 7.660444e-003 1.220000e-001 + vertex -8.426489e-003 7.070664e-003 1.230000e-001 + endloop + endfacet + facet normal -5.009527e-001 5.009527e-001 -7.057570e-001 + outer loop + vertex -8.426489e-003 7.070664e-003 1.230000e-001 + vertex -6.427876e-003 7.660444e-003 1.220000e-001 + vertex -7.660444e-003 6.427876e-003 1.220000e-001 + endloop + endfacet + facet normal -5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex -8.426489e-003 7.070664e-003 1.230000e-001 + vertex -7.660444e-003 6.427876e-003 1.220000e-001 + vertex -9.526280e-003 5.500000e-003 1.230000e-001 + endloop + endfacet + facet normal -5.803316e-001 4.063525e-001 -7.057570e-001 + outer loop + vertex -9.526280e-003 5.500000e-003 1.230000e-001 + vertex -7.660444e-003 6.427876e-003 1.220000e-001 + vertex -8.660254e-003 5.000000e-003 1.220000e-001 + endloop + endfacet + facet normal -6.420774e-001 2.994055e-001 -7.057570e-001 + outer loop + vertex -9.526280e-003 5.500000e-003 1.230000e-001 + vertex -8.660254e-003 5.000000e-003 1.220000e-001 + vertex -1.033662e-002 3.762222e-003 1.230000e-001 + endloop + endfacet + facet normal -6.420775e-001 2.994058e-001 -7.057568e-001 + outer loop + vertex -1.033662e-002 3.762222e-003 1.230000e-001 + vertex -8.660254e-003 5.000000e-003 1.220000e-001 + vertex -9.396926e-003 3.420201e-003 1.220000e-001 + endloop + endfacet + facet normal -6.843143e-001 1.833614e-001 -7.057568e-001 + outer loop + vertex -1.033662e-002 3.762222e-003 1.230000e-001 + vertex -9.396926e-003 3.420201e-003 1.220000e-001 + vertex -1.083289e-002 1.910130e-003 1.230000e-001 + endloop + endfacet + facet normal -6.843143e-001 1.833614e-001 -7.057567e-001 + outer loop + vertex -1.083289e-002 1.910130e-003 1.230000e-001 + vertex -9.396926e-003 3.420201e-003 1.220000e-001 + vertex -9.848078e-003 1.736482e-003 1.220000e-001 + endloop + endfacet + facet normal -7.057583e-001 6.174584e-002 -7.057567e-001 + outer loop + vertex -1.083289e-002 1.910130e-003 1.230000e-001 + vertex -9.848078e-003 1.736482e-003 1.220000e-001 + vertex -1.100000e-002 2.137179e-018 1.230000e-001 + endloop + endfacet + facet normal -7.057582e-001 6.174565e-002 -7.057569e-001 + outer loop + vertex -1.100000e-002 2.137179e-018 1.230000e-001 + vertex -9.848078e-003 1.736482e-003 1.220000e-001 + vertex -1.000000e-002 1.942890e-018 1.220000e-001 + endloop + endfacet + facet normal -7.057582e-001 -6.174582e-002 -7.057569e-001 + outer loop + vertex -1.100000e-002 2.137179e-018 1.230000e-001 + vertex -1.000000e-002 1.942890e-018 1.220000e-001 + vertex -1.083289e-002 -1.910130e-003 1.230000e-001 + endloop + endfacet + facet normal -7.057584e-001 -6.174567e-002 -7.057567e-001 + outer loop + vertex -1.083289e-002 -1.910130e-003 1.230000e-001 + vertex -1.000000e-002 1.942890e-018 1.220000e-001 + vertex -9.848078e-003 -1.736482e-003 1.220000e-001 + endloop + endfacet + facet normal -6.843143e-001 -1.833614e-001 -7.057567e-001 + outer loop + vertex -1.083289e-002 -1.910130e-003 1.230000e-001 + vertex -9.848078e-003 -1.736482e-003 1.220000e-001 + vertex -1.033662e-002 -3.762222e-003 1.230000e-001 + endloop + endfacet + facet normal -6.843143e-001 -1.833615e-001 -7.057568e-001 + outer loop + vertex -1.033662e-002 -3.762222e-003 1.230000e-001 + vertex -9.848078e-003 -1.736482e-003 1.220000e-001 + vertex -9.396926e-003 -3.420201e-003 1.220000e-001 + endloop + endfacet + facet normal -6.420776e-001 -2.994056e-001 -7.057568e-001 + outer loop + vertex -1.033662e-002 -3.762222e-003 1.230000e-001 + vertex -9.396926e-003 -3.420201e-003 1.220000e-001 + vertex -9.526280e-003 -5.500000e-003 1.230000e-001 + endloop + endfacet + facet normal -6.420774e-001 -2.994057e-001 -7.057570e-001 + outer loop + vertex -9.526280e-003 -5.500000e-003 1.230000e-001 + vertex -9.396926e-003 -3.420201e-003 1.220000e-001 + vertex -8.660254e-003 -5.000000e-003 1.220000e-001 + endloop + endfacet + facet normal -5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex -9.526280e-003 -5.500000e-003 1.230000e-001 + vertex -8.660254e-003 -5.000000e-003 1.220000e-001 + vertex -8.426489e-003 -7.070664e-003 1.230000e-001 + endloop + endfacet + facet normal -5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex -8.426489e-003 -7.070664e-003 1.230000e-001 + vertex -8.660254e-003 -5.000000e-003 1.220000e-001 + vertex -7.660444e-003 -6.427876e-003 1.220000e-001 + endloop + endfacet + facet normal -5.009527e-001 -5.009527e-001 -7.057570e-001 + outer loop + vertex -8.426489e-003 -7.070664e-003 1.230000e-001 + vertex -7.660444e-003 -6.427876e-003 1.220000e-001 + vertex -7.070664e-003 -8.426489e-003 1.230000e-001 + endloop + endfacet + facet normal -5.009527e-001 -5.009527e-001 -7.057570e-001 + outer loop + vertex -7.070664e-003 -8.426489e-003 1.230000e-001 + vertex -7.660444e-003 -6.427876e-003 1.220000e-001 + vertex -6.427876e-003 -7.660444e-003 1.220000e-001 + endloop + endfacet + facet normal -4.063525e-001 -5.803316e-001 -7.057570e-001 + outer loop + vertex -7.070664e-003 -8.426489e-003 1.230000e-001 + vertex -6.427876e-003 -7.660444e-003 1.220000e-001 + vertex -5.500000e-003 -9.526280e-003 1.230000e-001 + endloop + endfacet + facet normal -4.063525e-001 -5.803316e-001 -7.057570e-001 + outer loop + vertex -5.500000e-003 -9.526280e-003 1.230000e-001 + vertex -6.427876e-003 -7.660444e-003 1.220000e-001 + vertex -5.000000e-003 -8.660254e-003 1.220000e-001 + endloop + endfacet + facet normal -2.994055e-001 -6.420774e-001 -7.057570e-001 + outer loop + vertex -5.500000e-003 -9.526280e-003 1.230000e-001 + vertex -5.000000e-003 -8.660254e-003 1.220000e-001 + vertex -3.762222e-003 -1.033662e-002 1.230000e-001 + endloop + endfacet + facet normal -2.994058e-001 -6.420775e-001 -7.057568e-001 + outer loop + vertex -3.762222e-003 -1.033662e-002 1.230000e-001 + vertex -5.000000e-003 -8.660254e-003 1.220000e-001 + vertex -3.420201e-003 -9.396926e-003 1.220000e-001 + endloop + endfacet + facet normal -1.833614e-001 -6.843143e-001 -7.057568e-001 + outer loop + vertex -3.762222e-003 -1.033662e-002 1.230000e-001 + vertex -3.420201e-003 -9.396926e-003 1.220000e-001 + vertex -1.910130e-003 -1.083289e-002 1.230000e-001 + endloop + endfacet + facet normal -1.833614e-001 -6.843143e-001 -7.057567e-001 + outer loop + vertex -1.910130e-003 -1.083289e-002 1.230000e-001 + vertex -3.420201e-003 -9.396926e-003 1.220000e-001 + vertex -1.736482e-003 -9.848078e-003 1.220000e-001 + endloop + endfacet + facet normal -6.174584e-002 -7.057583e-001 -7.057567e-001 + outer loop + vertex -1.910130e-003 -1.083289e-002 1.230000e-001 + vertex -1.736482e-003 -9.848078e-003 1.220000e-001 + vertex -1.526557e-018 -1.100000e-002 1.230000e-001 + endloop + endfacet + facet normal -6.174565e-002 -7.057582e-001 -7.057569e-001 + outer loop + vertex -1.526557e-018 -1.100000e-002 1.230000e-001 + vertex -1.736482e-003 -9.848078e-003 1.220000e-001 + vertex -1.387779e-018 -1.000000e-002 1.220000e-001 + endloop + endfacet + facet normal 6.174582e-002 -7.057582e-001 -7.057569e-001 + outer loop + vertex -1.526557e-018 -1.100000e-002 1.230000e-001 + vertex -1.387779e-018 -1.000000e-002 1.220000e-001 + vertex 1.910130e-003 -1.083289e-002 1.230000e-001 + endloop + endfacet + facet normal 6.174567e-002 -7.057584e-001 -7.057567e-001 + outer loop + vertex 1.910130e-003 -1.083289e-002 1.230000e-001 + vertex -1.387779e-018 -1.000000e-002 1.220000e-001 + vertex 1.736482e-003 -9.848078e-003 1.220000e-001 + endloop + endfacet + facet normal 1.833614e-001 -6.843143e-001 -7.057567e-001 + outer loop + vertex 1.910130e-003 -1.083289e-002 1.230000e-001 + vertex 1.736482e-003 -9.848078e-003 1.220000e-001 + vertex 3.762222e-003 -1.033662e-002 1.230000e-001 + endloop + endfacet + facet normal 1.833615e-001 -6.843143e-001 -7.057568e-001 + outer loop + vertex 3.762222e-003 -1.033662e-002 1.230000e-001 + vertex 1.736482e-003 -9.848078e-003 1.220000e-001 + vertex 3.420201e-003 -9.396926e-003 1.220000e-001 + endloop + endfacet + facet normal 2.994056e-001 -6.420776e-001 -7.057568e-001 + outer loop + vertex 3.762222e-003 -1.033662e-002 1.230000e-001 + vertex 3.420201e-003 -9.396926e-003 1.220000e-001 + vertex 5.500000e-003 -9.526280e-003 1.230000e-001 + endloop + endfacet + facet normal 2.994057e-001 -6.420774e-001 -7.057570e-001 + outer loop + vertex 5.500000e-003 -9.526280e-003 1.230000e-001 + vertex 3.420201e-003 -9.396926e-003 1.220000e-001 + vertex 5.000000e-003 -8.660254e-003 1.220000e-001 + endloop + endfacet + facet normal 4.063525e-001 -5.803316e-001 -7.057570e-001 + outer loop + vertex 5.500000e-003 -9.526280e-003 1.230000e-001 + vertex 5.000000e-003 -8.660254e-003 1.220000e-001 + vertex 7.070664e-003 -8.426489e-003 1.230000e-001 + endloop + endfacet + facet normal 4.063525e-001 -5.803316e-001 -7.057570e-001 + outer loop + vertex 7.070664e-003 -8.426489e-003 1.230000e-001 + vertex 5.000000e-003 -8.660254e-003 1.220000e-001 + vertex 6.427876e-003 -7.660444e-003 1.220000e-001 + endloop + endfacet + facet normal 5.009527e-001 -5.009527e-001 -7.057570e-001 + outer loop + vertex 7.070664e-003 -8.426489e-003 1.230000e-001 + vertex 6.427876e-003 -7.660444e-003 1.220000e-001 + vertex 8.426489e-003 -7.070664e-003 1.230000e-001 + endloop + endfacet + facet normal 5.009527e-001 -5.009527e-001 -7.057570e-001 + outer loop + vertex 8.426489e-003 -7.070664e-003 1.230000e-001 + vertex 6.427876e-003 -7.660444e-003 1.220000e-001 + vertex 7.660444e-003 -6.427876e-003 1.220000e-001 + endloop + endfacet + facet normal 5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex 8.426489e-003 -7.070664e-003 1.230000e-001 + vertex 7.660444e-003 -6.427876e-003 1.220000e-001 + vertex 9.526280e-003 -5.500000e-003 1.230000e-001 + endloop + endfacet + facet normal 5.803316e-001 -4.063525e-001 -7.057570e-001 + outer loop + vertex 9.526280e-003 -5.500000e-003 1.230000e-001 + vertex 7.660444e-003 -6.427876e-003 1.220000e-001 + vertex 8.660254e-003 -5.000000e-003 1.220000e-001 + endloop + endfacet + facet normal 6.420774e-001 -2.994055e-001 -7.057570e-001 + outer loop + vertex 9.526280e-003 -5.500000e-003 1.230000e-001 + vertex 8.660254e-003 -5.000000e-003 1.220000e-001 + vertex 1.033662e-002 -3.762222e-003 1.230000e-001 + endloop + endfacet + facet normal 6.420775e-001 -2.994058e-001 -7.057568e-001 + outer loop + vertex 1.033662e-002 -3.762222e-003 1.230000e-001 + vertex 8.660254e-003 -5.000000e-003 1.220000e-001 + vertex 9.396926e-003 -3.420201e-003 1.220000e-001 + endloop + endfacet + facet normal 6.843143e-001 -1.833614e-001 -7.057568e-001 + outer loop + vertex 1.033662e-002 -3.762222e-003 1.230000e-001 + vertex 9.396926e-003 -3.420201e-003 1.220000e-001 + vertex 1.083289e-002 -1.910130e-003 1.230000e-001 + endloop + endfacet + facet normal 6.843143e-001 -1.833614e-001 -7.057567e-001 + outer loop + vertex 1.083289e-002 -1.910130e-003 1.230000e-001 + vertex 9.396926e-003 -3.420201e-003 1.220000e-001 + vertex 9.848078e-003 -1.736482e-003 1.220000e-001 + endloop + endfacet + facet normal 7.057583e-001 -6.174584e-002 -7.057567e-001 + outer loop + vertex 1.083289e-002 -1.910130e-003 1.230000e-001 + vertex 9.848078e-003 -1.736482e-003 1.220000e-001 + vertex 1.100000e-002 0.000000e+000 1.230000e-001 + endloop + endfacet + facet normal 7.057582e-001 -6.174565e-002 -7.057569e-001 + outer loop + vertex 1.100000e-002 0.000000e+000 1.230000e-001 + vertex 9.848078e-003 -1.736482e-003 1.220000e-001 + vertex 1.000000e-002 0.000000e+000 1.220000e-001 + endloop + endfacet + facet normal 7.057582e-001 6.174582e-002 -7.057569e-001 + outer loop + vertex 1.100000e-002 0.000000e+000 1.230000e-001 + vertex 1.000000e-002 0.000000e+000 1.220000e-001 + vertex 1.083289e-002 1.910130e-003 1.230000e-001 + endloop + endfacet + facet normal 7.057584e-001 6.174567e-002 -7.057567e-001 + outer loop + vertex 1.083289e-002 1.910130e-003 1.230000e-001 + vertex 1.000000e-002 0.000000e+000 1.220000e-001 + vertex 9.848078e-003 1.736482e-003 1.220000e-001 + endloop + endfacet +endsolid \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/examples/map1.mat b/lwrserv/matlab/rvctools/robot/examples/map1.mat new file mode 100644 index 0000000..8030ef1 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/examples/map1.mat differ diff --git a/lwrserv/matlab/rvctools/robot/examples/moveline.m b/lwrserv/matlab/rvctools/robot/examples/moveline.m new file mode 100644 index 0000000..c24feeb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/moveline.m @@ -0,0 +1,24 @@ +% target loine +L = [1 -2 4]; +clf +axis([0 10 0 10]); +hold on +xyzlabel +x = [0 10]; +y = -(L(1)*x+L(3))/L(2); +plot(x, y, 'k--'); +grid on + +xc = 5; yc = 5; +N = 4; +radius = 3; + +for i=1:N + th = (i-1)*2*pi/N; + x0 = [xc+radius*cos(th) yc+radius*sin(th) th+pi/2]; + + plot_vehicle(x0, 'r'); + r = sim('sl_driveline'); + y = r.find('yout'); + plot(y(:,1), y(:,2)); +end diff --git a/lwrserv/matlab/rvctools/robot/examples/movepoint.m b/lwrserv/matlab/rvctools/robot/examples/movepoint.m new file mode 100644 index 0000000..6c88cf2 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/movepoint.m @@ -0,0 +1,20 @@ +xg = [5 5]; % goal position +clf +axis([0 10 0 10]); +hold on +xyzlabel +grid on +xc = 5; yc = 5; +N = 8; +radius = 3; + +for i=1:N + th = (i-1)*2*pi/N; + x0 = [xc+radius*cos(th) yc+radius*sin(th) th+pi/2]; + + plot_vehicle(x0, 'r'); + r = sim('sl_drivepoint'); + y = r.find('yout'); + plot(y(:,1), y(:,2)); +end +plot(xg(1), xg(2), '*') diff --git a/lwrserv/matlab/rvctools/robot/examples/movepose.m b/lwrserv/matlab/rvctools/robot/examples/movepose.m new file mode 100644 index 0000000..ebd0415 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/movepose.m @@ -0,0 +1,20 @@ +xg = [5 5 pi/2]; +clf +axis([0 10 0 10]); +hold on +xyzlabel +grid on +xc = 5; yc = 5; +N = 8; +radius = 4; + +for i=1:N + th = (i-1)*2*pi/N; + x0 = [xc+radius*cos(th) yc+radius*sin(th) 0]; + + plot_vehicle(x0, 'r'); + r = sim('sl_drivepose'); + y = r.find('yout'); + plot(y(:,1), y(:,2)); +end +plot_vehicle(xg, 'r'); diff --git a/lwrserv/matlab/rvctools/robot/examples/sensorfield.m b/lwrserv/matlab/rvctools/robot/examples/sensorfield.m new file mode 100644 index 0000000..9eec0fe --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/sensorfield.m @@ -0,0 +1,5 @@ +function sensor = sensorfield(x, y) + + xc = 60; yc = 90; + + sensor = 200 ./ ((x-xc).^2 + (y-yc).^2 + 200); diff --git a/lwrserv/matlab/rvctools/robot/examples/walking.m b/lwrserv/matlab/rvctools/robot/examples/walking.m new file mode 100644 index 0000000..effe490 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/examples/walking.m @@ -0,0 +1,96 @@ +% set the dimensions of the two leg links +L1 = 0.1; L2 = 0.1; + +% create the leg links based on DH parameters +% theta d a alpha +links(1) = Link([ 0 0 0 pi/2 ], 'standard'); +links(2) = Link([ 0 0 L1 0 ], 'standard'); +links(3) = Link([ 0 0 -L2 0 ], 'standard'); + +% now create a robot to represent a single leg +leg = SerialLink(links, 'name', 'leg', 'offset', [pi/2 0 -pi/2]); + +% define the key parameters of the gait trajectory, walking in the +% x-direction +xf = 5; xb = -xf; % forward and backward limits for foot on ground +y = 5; % distance of foot from body along y-axis +zu = 2; zd = 5; % height of foot when up and down +% define the rectangular path taken by the foot +segments = [xf y zd; xb y zd; xb y zu; xf y zu] * 0.01; + +% build the gait. the points are: +% 1 start of walking stroke +% 2 end of walking stroke +% 3 end of foot raise +% 4 foot raised and forward +% +% The segments times are : +% 1->2 3s +% 2->3 0.5s +% 3->4 1s +% 4->1 0.5ss +% +% A total of 4s, of which 3s is walking and 1s is reset. At 0.01s sample +% time this is exactly 400 steps long. +% +% We use a finite acceleration time to get a nice smooth path, which means +% that the foot never actually goes through any of these points. This +% makes setting the initial robot pose and velocity difficult. +% +% Intead we create a longer cyclic path: 1, 2, 3, 4, 1, 2, 3, 4. The +% first 1->2 segment includes the initial ramp up, and the final 3->4 +% has the slow down. However the middle 2->3->4->1 is smooth cyclic +% motion so we "cut it out" and use it. +segments = [segments; segments]; +tseg = [3 0.25 0.5 0.25]'; +tseg = [1; tseg; tseg]; +x = mstraj(segments, [], tseg, segments(1,:), 0.01, 0.1); + +% pull out the cycle +xcycle = x(100:500,:); +qcycle = leg.ikine( transl(xcycle), [], [1 1 1 0 0 0], 'pinv' ); + +% dimensions of the robot's rectangular body, width and height, the legs +% are at each corner. +W = 0.1; L = 0.2; + +% a bit of optimization. We use a lot of plotting options to +% make the animation fast: turn off annotations like wrist axes, ground +% shadow, joint axes, no smooth shading. Rather than parse the switches +% each cycle we pre-digest them here into a plotopt struct. +plotopt = leg.plot({'noraise', 'nobase', 'noshadow', ... + 'nowrist', 'nojaxes'}); +% plotopt = leg.plot({'noraise', 'norender', 'nobase', 'noshadow', ... +% 'nowrist', 'nojaxes', 'ortho'}); + + +% create 4 leg robots. Each is a clone of the leg robot we built above, +% has a unique name, and a base transform to represent it's position +% on the body of the walking robot. +legs(1) = SerialLink(leg, 'name', 'leg1'); +legs(2) = SerialLink(leg, 'name', 'leg2', 'base', transl(-L, 0, 0)); +legs(3) = SerialLink(leg, 'name', 'leg3', 'base', transl(-L, -W, 0)*trotz(pi)); +legs(4) = SerialLink(leg, 'name', 'leg4', 'base', transl(0, -W, 0)*trotz(pi)); + +% create a fixed size axis for the robot, and set z positive downward +clf; axis([-0.3 0.1 -0.2 0.2 -0.15 0.05]); set(gca,'Zdir', 'reverse') +hold on +% draw the robot's body +patch([0 -L -L 0], [0 0 -W -W], [0 0 0 0], ... + 'FaceColor', 'r', 'FaceAlpha', 0.5) +% instantiate each robot in the axes +for i=1:4 + legs(i).plot(qcycle(1,:), plotopt); +end +hold off + +% walk! +k = 1; +while 1 + legs(1).plot( gait(qcycle, k, 0, 0), plotopt); + legs(2).plot( gait(qcycle, k, 100, 0), plotopt); + legs(3).plot( gait(qcycle, k, 200, 1), plotopt); + legs(4).plot( gait(qcycle, k, 300, 1), plotopt); + drawnow + k = k+1; +end diff --git a/lwrserv/matlab/rvctools/robot/info.xml b/lwrserv/matlab/rvctools/robot/info.xml new file mode 100644 index 0000000..401ecfc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info.xml @@ -0,0 +1,34 @@ + + + +R14 +Robotics +toolbox +ApplicationIcon.MATLAB +info + + + + + +doc robot/ +$toolbox/matlab/general/bookicon.gif + + + + +rtbstartup +$toolbox/matlab/general/demoicon.gif + + + + +web http://www.petercorke.com -browser; +$toolbox/matlab/general/webicon.gif + + + + + diff --git a/lwrserv/matlab/rvctools/robot/info/acknowledgements.html b/lwrserv/matlab/rvctools/robot/info/acknowledgements.html new file mode 100644 index 0000000..5d96678 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/acknowledgements.html @@ -0,0 +1,22 @@ + + +Acknowledgements + + + + + +

Robotics Toolbox for Matlab

+

Acknowledgements

+
+The following have contributed to the Robotics Toolbox + + + + + + + +
Joern MalzahnCodeGenerator module
Paul Pounds Quadcopter code
Wynand SmartVarious arm robot models
Paul NewmanInspiration for the mobile robot estimation classes
Robert Biro and Gary Von McMurraySolution for Puma560 robot
+ + diff --git a/lwrserv/matlab/rvctools/robot/info/contents.html b/lwrserv/matlab/rvctools/robot/info/contents.html new file mode 100644 index 0000000..c870f09 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/contents.html @@ -0,0 +1,237 @@ + + +
+

Homogeneous transformations

+

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
angvec2rangle/vector to RM
angvec2trangle/vector to HT
eul2rEuler angles to RM
eul2trEuler angles to HT
oa2rorientation and approach vector to RM
oa2trorientation and approach vector to HT
r2tRM to HT
rt2tr(R,t) to HT
rotxRM for rotation about X-axis
rotyRM for rotation about Y-axis
rotzRM for rotation about Z-axis
rpy2rroll/pitch/yaw angles to RM
rpy2trroll/pitch/yaw angles to HT
se2HT in SE(2)
t2rHT to RM
tr2angvecHT/RM to angle/vector form
tr2eulHT/RM to Euler angles
tr2rpyHT/RM to roll/pitch/yaw angles
tr2rtHT to (R,t)
tranimateanimate a coordinate frame
translset or extract the translational component of HT
trnormnormalize HT
trplotplot HT as a coordinate frame
trplot2plot HT, SE(2), as a coordinate frame
trprintprint an HT
trotxHT for rotation about X-axis
trotyHT for rotation about Y-axis
trotzHT for rotation about Z-axis

+

Homogeneous points and lines

+

+ + + + +
e2hEuclidean coordinates to homogeneous
h2ehomogeneous coordinates to Euclidean
homlinecreate line from 2 points
homtranstransform points

+

    +
  • HT = homogeneous transformation, a 4x4 matrix, belongs to the group SE(3).
    • +
  • RM = RM, an orthonormal 3x3 matrix, belongs to the group SO(3).
    • +
  • Functions of the form tr2XX will also accept a RM as the argument.
    • +

+

Differential motion

+

+ + + + + + + + +
delta2trdifferential motion vector to HT
eul2jacEuler angles to Jacobian
rpy2jacRPY angles to Jacobian
skewvector to skew symmetric matrix
tr2deltaHT to differential motion vector
tr2jacHT to Jacobian
vexskew symmetric matrix to vector
wtranstransform wrench between frames

+

Trajectory generation

+

+ + + + + + + +
ctrajCartesian trajectory
jtrajjoint space trajectory
lspb1D trapezoidal trajectory
mtrajmulti-axis trajectory for arbitrary function
mstrajmulti-axis multi-segment trajectory
tpoly1D polynomial trajectory
trinterpinterpolate HT s

+

Quaternion

+

+ + + + + + + + +
Quaternionconstructor
/divide quaternion by quaternion or scalar
*multiply quaternion by a quaternion or vector
Quaternion.invinvert a quaternion
Quaternion.normnorm of a quaternion
Quaternion.plotdisplay a quaternion as a 3D rotation
Quaternion.unitunitize a quaternion
Quaternion.interpinterpolate a quaternion

+

Serial-link manipulator

+

+ + + + + + + + + + + +
SerialLinkconstruct a serial-link robot object
Linkconstruct a robot link object
Revoluteconstruct a revolute robot link object
Prismaticconstruct a prismatic robot link object
*compound two robots
SerialLink.frictionreturn joint friction torques
SerialLink.nofrictionreturn a robot object with no friction
SerialLink.perturbreturn a robot object with perturbed parameters
SerialLink.plotplot/animate robot
SerialLink.teachdrive a graphical robot
CodeGeneratorcreate efficient run-time kinematic and dynamics code

+

Models

+

+ + + + + + + + + + + + + + +
mdl_ballhigh order ball shaped mechanism
mdl_coilhigh order coil shaped mechanism
mdl_Fanuc10LFanuc 10L (DH, kine)
mdl_MotomanHP6Motoman HP6 (DH, kine)
mdl_phantomxTrossen Robotics PhantomX pincher
mdl_puma560Puma 560 data (DH, kine, dyn)
mdl_puma560_3Puma 560, first 3 joints, kine only
mdl_puma560_3_symPuma 560, first 3 joints, symbolic, kine only
mdl_puma560akbPuma 560 data (MDH, kine, dyn)
mdl_p8Puma 6-axis robot on a 2-axis XY base
mdl_S4ABB2p8ABB S4 2.8 (DH, kine)
mdl_stanfordStanford arm data (DH, kine, dyn)
mdl_twolinksimple 2-link example (DH, kine)
DHFactorconvert elementary transformations to DH form

+

Kinematic

+

+ + + + + + + +
DHFactortransform sequence to DH description
SerialLink.fkineforward kinematics
SerialLink.ikineinverse kinematics (numeric)
SerialLink.ikine6sinverse kinematics for 6-axis arm with sph.wrist
SerialLink.jacob0Jacobian in base coordinate frame
SerialLink.jacobnJacobian in end-effector coordinate frame
SerialLink.manipltycompute manipulability

+

Dynamics

+

+ + + + + + + + + +
SerialLink.accelforward dynamics
SerialLink.cinertiaCartesian manipulator inertia matrix
SerialLink.corioliscentripetal/coriolis torque
SerialLink.fdynforward dynamics
wtranstransform a force/moment
SerialLink.gravloadgravity loading
SerialLink.inertiamanipulator inertia matrix
SerialLink.itorqueinertia torque
SerialLink.rneinverse dynamics

+

Mobile robot

+

+ + + + + + + + + + + +
Mappoint feature map object
RandomPathdriver for Vehicle object
RangeBearingSensor"laser scanner" object
Vehicleconstruct a mobile robot object
sl_bicycleSimulink "bicycle model" of non-holonomic wheeled vehicle
NavigationNavigation superclass
Sensorrobot sensor superclass
plot_vehicleplot vehicle
makemapmake a map, occupancy grid
mdl_quadcoptersimple quadcopter model
sl_quadrotorSimulink model of a flying quadrotor

+

Localization

+

+ + +
EKFextended Kalman filter object
ParticleFilterMonte Carlo estimator

+

Path planning

+

+ + + + + +
Bug2bug navigation
DXformdistance transform from map
DstarD* planner
PRMprobabilistic roadmap planner
RRTrapidly exploring random tree

+

Graphics

+

+ + + + + + + + + + + + + + + + +
plot2plot trajectory
plotpplot points
plot_arrowdraw an arrow
plot_boxdraw a box
plot_circledraw a circle
plot_ellipsedraw an ellipse
plot_homlineplot homogeneous line
plot_pointplot points
plot_polyplot polygon
plot_spheredraw a sphere
qplotplot joint angle trajectories
plot2Plot trajectories
plotpPlot trajectories
xaxisset x-axis scaling
yaxisset y-axis scaling
xyzlabellabel axes x, y and z

+

Utility

+

+ + + + + + + + + + + + + + + + + + + + + + + + + +
aboutsummary of object size and type
angdiffsubtract 2 angles modulo 2pi
bresenhamBresenhan line drawing
circlecompute/draw points on a circle
colnormcolumnwise norm of matrix
diff2elementwise diff
distancexformcompute distance transform
edgelistlist of edge pixels
gauss2dGaussian distribution in 2D
ishomogtrue if argument is a 4x4 matrix
ismatrixtrue if non scalar
isrottrue if argument is a 3x3 matrix
isvectrue if argument is a 3-vector
numcolsnumber of columns in matrix
numrowsnumber of rows in matrix
peakfind peak in 1D signal
peak2find peak in 2D signal
PGraphgeneral purpose graph class
polydiffderivative of polynomial
Polygongeneral purpose polygon class
randinitinitialize random number generator
rampcreate linear ramp
runscriptinteractively run a script or demo
unitunitize a vector
tb_optparsetoolbox argument parser

+

CodeGen support

+

+ + + + + +
distributeblocksdistribute blocks in a Simulink library
doesblockexistchecks if a Simulink block exists
multidfprintffprintf to multiple files
symexpr2slblockembedded Matlab symbolic functions
simulinkextdetermine extension of Simulink files

+

Demonstrations

+

+ + +
rtbdemoSerial-link manipulator demonstration
tripleangleGUI to demonsrate triple angle rotations

+

Examples

+

+ + + + + + + + + +
sl_quadcopterSimulink model of a flying quadcopter
sl_braitenbergSimulink model a Braitenberg vehicle
movepointnon-holonomic vehicle moving to a point
movelinenon-holonomic vehicle moving to a line
moveposenon-holonomic vehicle moving to a pose
walkingexample of 4-legged walking robot
eg_inertiajoint 1 inertia I(q1,q2)
eg_inertia22joint 2 inertia I(q3)
eg_gravjoint 2 gravity load g(q2,q3)

+

    +
  • located in the examples folder
    • +

+

+

Copyright © 1990-2011 Peter Corke

+ + diff --git a/lwrserv/matlab/rvctools/robot/info/contents_alpha.html b/lwrserv/matlab/rvctools/robot/info/contents_alpha.html new file mode 100644 index 0000000..0728849 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/contents_alpha.html @@ -0,0 +1,184 @@ +

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
*multiply quaternion by a quaternion or vector
*compound two robots
/divide quaternion by quaternion or scalar
aboutsummary of object size and type
angdiffsubtract 2 angles modulo 2pi
angvec2rangle/vector to RM
angvec2trangle/vector to HT
bresenhamBresenhan line drawing
Bug2bug navigation
circlecompute/draw points on a circle
CodeGeneratorcreate efficient run-time kinematic and dynamics code
colnormcolumnwise norm of matrix
ctrajCartesian trajectory
delta2trdifferential motion vector to HT
DHFactorconvert elementary transformations to DH form
DHFactortransform sequence to DH description
diff2elementwise diff
distancexformcompute distance transform
distributeblocksdistribute blocks in a Simulink library
doesblockexistchecks if a Simulink block exists
DstarD* planner
DXformdistance transform from map
e2hEuclidean coordinates to homogeneous
edgelistlist of edge pixels
eg_gravjoint 2 gravity load g(q2,q3)
eg_inertiajoint 1 inertia I(q1,q2)
eg_inertia22joint 2 inertia I(q3)
EKFextended Kalman filter object
eul2jacEuler angles to Jacobian
eul2rEuler angles to RM
eul2trEuler angles to HT
gauss2dGaussian distribution in 2D
h2ehomogeneous coordinates to Euclidean
homlinecreate line from 2 points
homtranstransform points
ishomogtrue if argument is a 4x4 matrix
ismatrixtrue if non scalar
isrottrue if argument is a 3x3 matrix
isvectrue if argument is a 3-vector
jtrajjoint space trajectory
Linkconstruct a robot link object
lspb1D trapezoidal trajectory
makemapmake a map, occupancy grid
Mappoint feature map object
mdl_ballhigh order ball shaped mechanism
mdl_coilhigh order coil shaped mechanism
mdl_Fanuc10LFanuc 10L (DH, kine)
mdl_MotomanHP6Motoman HP6 (DH, kine)
mdl_p8Puma 6-axis robot on a 2-axis XY base
mdl_phantomxTrossen Robotics PhantomX pincher
mdl_puma560Puma 560 data (DH, kine, dyn)
mdl_puma560_3Puma 560, first 3 joints, kine only
mdl_puma560_3_symPuma 560, first 3 joints, symbolic, kine only
mdl_puma560akbPuma 560 data (MDH, kine, dyn)
mdl_quadcoptersimple quadcopter model
mdl_S4ABB2p8ABB S4 2.8 (DH, kine)
mdl_stanfordStanford arm data (DH, kine, dyn)
mdl_twolinksimple 2-link example (DH, kine)
movelinenon-holonomic vehicle moving to a line
movepointnon-holonomic vehicle moving to a point
moveposenon-holonomic vehicle moving to a pose
mstrajmulti-axis multi-segment trajectory
mtrajmulti-axis trajectory for arbitrary function
multidfprintffprintf to multiple files
NavigationNavigation superclass
numcolsnumber of columns in matrix
numrowsnumber of rows in matrix
oa2rorientation and approach vector to RM
oa2trorientation and approach vector to HT
ParticleFilterMonte Carlo estimator
peakfind peak in 1D signal
peak2find peak in 2D signal
PGraphgeneral purpose graph class
plot2plot trajectory
plot2Plot trajectories
plot_arrowdraw an arrow
plot_boxdraw a box
plot_circledraw a circle
plot_ellipsedraw an ellipse
plot_homlineplot homogeneous line
plot_pointplot points
plot_polyplot polygon
plot_spheredraw a sphere
plot_vehicleplot vehicle
plotbotopt
plotpplot points
plotpPlot trajectories
polydiffderivative of polynomial
Polygongeneral purpose polygon class
Prismaticconstruct a prismatic robot link object
PRMprobabilistic roadmap planner
qplotplot joint angle trajectories
Quaternionconstructor
Quaternion.interpinterpolate a quaternion
Quaternion.invinvert a quaternion
Quaternion.normnorm of a quaternion
Quaternion.plotdisplay a quaternion as a 3D rotation
Quaternion.unitunitize a quaternion
r2tRM to HT
rampcreate linear ramp
randinitinitialize random number generator
RandomPathdriver for Vehicle object
RangeBearingSensor"laser scanner" object
Revoluteconstruct a revolute robot link object
rotxRM for rotation about X-axis
rotyRM for rotation about Y-axis
rotzRM for rotation about Z-axis
rpy2jacRPY angles to Jacobian
rpy2rroll/pitch/yaw angles to RM
rpy2trroll/pitch/yaw angles to HT
RRTrapidly exploring random tree
rt2tr(R,t) to HT
rtbdemoSerial-link manipulator demonstration
runscriptinteractively run a script or demo
se2HT in SE(2)
Sensorrobot sensor superclass
SerialLinkconstruct a serial-link robot object
SerialLink.accelforward dynamics
SerialLink.cinertiaCartesian manipulator inertia matrix
SerialLink.corioliscentripetal/coriolis torque
SerialLink.fdynforward dynamics
SerialLink.fkineforward kinematics
SerialLink.frictionreturn joint friction torques
SerialLink.gravloadgravity loading
SerialLink.ikineinverse kinematics (numeric)
SerialLink.ikine6sinverse kinematics for 6-axis arm with sph.wrist
SerialLink.inertiamanipulator inertia matrix
SerialLink.itorqueinertia torque
SerialLink.jacob0Jacobian in base coordinate frame
SerialLink.jacobnJacobian in end-effector coordinate frame
SerialLink.manipltycompute manipulability
SerialLink.nofrictionreturn a robot object with no friction
SerialLink.perturbreturn a robot object with perturbed parameters
SerialLink.plotplot/animate robot
SerialLink.rneinverse dynamics
SerialLink.teachdrive a graphical robot
simulinkextdetermine extension of Simulink files
skewvector to skew symmetric matrix
sl_bicycleSimulink "bicycle model" of non-holonomic wheeled vehicle
sl_braitenbergSimulink model a Braitenberg vehicle
sl_quadcopterSimulink model of a flying quadcopter
sl_quadrotorSimulink model of a flying quadrotor
symexpr2slblockembedded Matlab symbolic functions
t2rHT to RM
tb_optparsetoolbox argument parser
tpoly1D polynomial trajectory
tr2angvecHT/RM to angle/vector form
tr2deltaHT to differential motion vector
tr2eulHT/RM to Euler angles
tr2jacHT to Jacobian
tr2rpyHT/RM to roll/pitch/yaw angles
tr2rtHT to (R,t)
tranimateanimate a coordinate frame
translset or extract the translational component of HT
trinterpinterpolate HT s
tripleangleGUI to demonsrate triple angle rotations
trnormnormalize HT
trotxHT for rotation about X-axis
trotyHT for rotation about Y-axis
trotzHT for rotation about Z-axis
trplotplot HT as a coordinate frame
trplot2plot HT, SE(2), as a coordinate frame
trprintprint an HT
unitunitize a vector
Vehicleconstruct a mobile robot object
vexskew symmetric matrix to vector
walkingexample of 4-legged walking robot
wtranstransform wrench between frames
wtranstransform a force/moment
xaxisset x-axis scaling
xyzlabellabel axes x, y and z
yaxisset y-axis scaling

+

    +
  • HT = homogeneous transformation, a 4x4 matrix, belongs to the group SE(3).
    • +
  • RM = RM, an orthonormal 3x3 matrix, belongs to the group SO(3).
    • +
  • Functions of the form tr2XX will also accept a RM as the argument.
    • +
  • located in the examples folder
    • +

+
+

Copyright © 1990-2011 Peter Corke

+ + diff --git a/lwrserv/matlab/rvctools/robot/info/contents_toc.html b/lwrserv/matlab/rvctools/robot/info/contents_toc.html new file mode 100644 index 0000000..e388ca7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/contents_toc.html @@ -0,0 +1,310 @@ + + + + Function Reference (Optimization Toolbox™) + + + +

 

+ + + + + +

Function Reference

Click for Alphabetical List Alphabetical List
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Homogeneous transformationsManipulate homogeneous transformations and orthonormal rotation matrices
    Homogeneous points and linesManipulate homogeneous points and lines
    Differential motionConvert differential motion between coordinate frames
    Trajectory generationCreate motion trajectory and interpolate poses
    QuaternionManipulate unit quaternions
    Serial-link manipulatorClass representing the kinematics and dynamics of a serial-link manipulator
        ModelsModels of serial-link manipulators
        KinematicSerial-link manipulator kinematic functions
        DynamicsSerial-link manipulator dynamic functions
    Mobile robotClasses for mobile robotics problems
        LocalizationMobile robot localization algorithms
        Path planningMobile robot path planning algorithms
    GraphicsUseful graphic primitives
    UtilityMiscellaneous functions
        CodeGen support
    Demonstrations
    Examples
+

Back to Top

+

Homogeneous transformations

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
angvec2rangle/vector to RM
angvec2trangle/vector to HT
eul2rEuler angles to RM
eul2trEuler angles to HT
oa2rorientation and approach vector to RM
oa2trorientation and approach vector to HT
r2tRM to HT
rt2tr(R,t) to HT
rotxRM for rotation about X-axis
rotyRM for rotation about Y-axis
rotzRM for rotation about Z-axis
rpy2rroll/pitch/yaw angles to RM
rpy2trroll/pitch/yaw angles to HT
se2HT in SE(2)
t2rHT to RM
tr2angvecHT/RM to angle/vector form
tr2eulHT/RM to Euler angles
tr2rpyHT/RM to roll/pitch/yaw angles
tr2rtHT to (R,t)
tranimateanimate a coordinate frame
translset or extract the translational component of HT
trnormnormalize HT
trplotplot HT as a coordinate frame
trplot2plot HT, SE(2), as a coordinate frame
trprintprint an HT
trotxHT for rotation about X-axis
trotyHT for rotation about Y-axis
trotzHT for rotation about Z-axis
+

Back to Top

+

Homogeneous points and lines

+ + + + + + +
e2hEuclidean coordinates to homogeneous
h2ehomogeneous coordinates to Euclidean
homlinecreate line from 2 points
homtranstransform points
+

Back to Top

+

Differential motion

+ + + + + + + + + + +
delta2trdifferential motion vector to HT
eul2jacEuler angles to Jacobian
rpy2jacRPY angles to Jacobian
skewvector to skew symmetric matrix
tr2deltaHT to differential motion vector
tr2jacHT to Jacobian
vexskew symmetric matrix to vector
wtranstransform wrench between frames
+

Back to Top

+

Trajectory generation

+ + + + + + + + + +
ctrajCartesian trajectory
jtrajjoint space trajectory
lspb1D trapezoidal trajectory
mtrajmulti-axis trajectory for arbitrary function
mstrajmulti-axis multi-segment trajectory
tpoly1D polynomial trajectory
trinterpinterpolate HT s
+

Back to Top

+

Quaternion

+ + + + + + + + + + +
Quaternionconstructor
/divide quaternion by quaternion or scalar
*multiply quaternion by a quaternion or vector
Quaternion.invinvert a quaternion
Quaternion.normnorm of a quaternion
Quaternion.plotdisplay a quaternion as a 3D rotation
Quaternion.unitunitize a quaternion
Quaternion.interpinterpolate a quaternion
+

Back to Top

+

Serial-link manipulator

+ + + + + + + + + + + + + +
SerialLinkconstruct a serial-link robot object
Linkconstruct a robot link object
Revoluteconstruct a revolute robot link object
Prismaticconstruct a prismatic robot link object
*compound two robots
SerialLink.frictionreturn joint friction torques
SerialLink.nofrictionreturn a robot object with no friction
SerialLink.perturbreturn a robot object with perturbed parameters
SerialLink.plotplot/animate robot
SerialLink.teachdrive a graphical robot
CodeGeneratorcreate efficient run-time kinematic and dynamics code
+

Back to Top

+

Models

+ + + + + + + + + + + + + + + + +
mdl_ballhigh order ball shaped mechanism
mdl_coilhigh order coil shaped mechanism
mdl_Fanuc10LFanuc 10L (DH, kine)
mdl_MotomanHP6Motoman HP6 (DH, kine)
mdl_phantomxTrossen Robotics PhantomX pincher
mdl_puma560Puma 560 data (DH, kine, dyn)
mdl_puma560_3Puma 560, first 3 joints, kine only
mdl_puma560_3_symPuma 560, first 3 joints, symbolic, kine only
mdl_puma560akbPuma 560 data (MDH, kine, dyn)
mdl_p8Puma 6-axis robot on a 2-axis XY base
mdl_S4ABB2p8ABB S4 2.8 (DH, kine)
mdl_stanfordStanford arm data (DH, kine, dyn)
mdl_twolinksimple 2-link example (DH, kine)
DHFactorconvert elementary transformations to DH form
+

Back to Top

+

Kinematic

+ + + + + + + + + +
DHFactortransform sequence to DH description
SerialLink.fkineforward kinematics
SerialLink.ikineinverse kinematics (numeric)
SerialLink.ikine6sinverse kinematics for 6-axis arm with sph.wrist
SerialLink.jacob0Jacobian in base coordinate frame
SerialLink.jacobnJacobian in end-effector coordinate frame
SerialLink.manipltycompute manipulability
+

Back to Top

+

Dynamics

+ + + + + + + + + + + +
SerialLink.accelforward dynamics
SerialLink.cinertiaCartesian manipulator inertia matrix
SerialLink.corioliscentripetal/coriolis torque
SerialLink.fdynforward dynamics
wtranstransform a force/moment
SerialLink.gravloadgravity loading
SerialLink.inertiamanipulator inertia matrix
SerialLink.itorqueinertia torque
SerialLink.rneinverse dynamics
+

Back to Top

+

Mobile robot

+ + + + + + + + + + + + + +
Mappoint feature map object
RandomPathdriver for Vehicle object
RangeBearingSensor"laser scanner" object
Vehicleconstruct a mobile robot object
sl_bicycleSimulink "bicycle model" of non-holonomic wheeled vehicle
NavigationNavigation superclass
Sensorrobot sensor superclass
plot_vehicleplot vehicle
makemapmake a map, occupancy grid
mdl_quadcoptersimple quadcopter model
sl_quadrotorSimulink model of a flying quadrotor
+

Back to Top

+

Localization

+ + + + +
EKFextended Kalman filter object
ParticleFilterMonte Carlo estimator
+

Back to Top

+

Path planning

+ + + + + + + +
Bug2bug navigation
DXformdistance transform from map
DstarD* planner
PRMprobabilistic roadmap planner
RRTrapidly exploring random tree
+

Back to Top

+

Graphics

+ + + + + + + + + + + + + + + + + + +
plot2plot trajectory
plotpplot points
plot_arrowdraw an arrow
plot_boxdraw a box
plot_circledraw a circle
plot_ellipsedraw an ellipse
plot_homlineplot homogeneous line
plot_pointplot points
plot_polyplot polygon
plot_spheredraw a sphere
qplotplot joint angle trajectories
plot2Plot trajectories
plotpPlot trajectories
xaxisset x-axis scaling
yaxisset y-axis scaling
xyzlabellabel axes x, y and z
+

Back to Top

+

Utility

+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
aboutsummary of object size and type
angdiffsubtract 2 angles modulo 2pi
bresenhamBresenhan line drawing
circlecompute/draw points on a circle
colnormcolumnwise norm of matrix
diff2elementwise diff
distancexformcompute distance transform
edgelistlist of edge pixels
gauss2dGaussian distribution in 2D
ishomogtrue if argument is a 4x4 matrix
ismatrixtrue if non scalar
isrottrue if argument is a 3x3 matrix
isvectrue if argument is a 3-vector
numcolsnumber of columns in matrix
numrowsnumber of rows in matrix
peakfind peak in 1D signal
peak2find peak in 2D signal
PGraphgeneral purpose graph class
polydiffderivative of polynomial
Polygongeneral purpose polygon class
randinitinitialize random number generator
rampcreate linear ramp
runscriptinteractively run a script or demo
unitunitize a vector
tb_optparsetoolbox argument parser
+

Back to Top

+

CodeGen support

+ + + + + + + +
distributeblocksdistribute blocks in a Simulink library
doesblockexistchecks if a Simulink block exists
multidfprintffprintf to multiple files
symexpr2slblockembedded Matlab symbolic functions
simulinkextdetermine extension of Simulink files
+

Back to Top

+

Demonstrations

+ + + + +
rtbdemoSerial-link manipulator demonstration
tripleangleGUI to demonsrate triple angle rotations
+

Back to Top

+

Examples

+ + + + + + + + + + + +
sl_quadcopterSimulink model of a flying quadcopter
sl_braitenbergSimulink model a Braitenberg vehicle
movepointnon-holonomic vehicle moving to a point
movelinenon-holonomic vehicle moving to a line
moveposenon-holonomic vehicle moving to a pose
walkingexample of 4-legged walking robot
eg_inertiajoint 1 inertia I(q1,q2)
eg_inertia22joint 2 inertia I(q3)
eg_gravjoint 2 gravity load g(q2,q3)
+

Back to Top

diff --git a/lwrserv/matlab/rvctools/robot/info/helpfuncbycat.xml b/lwrserv/matlab/rvctools/robot/info/helpfuncbycat.xml new file mode 100644 index 0000000..bce24f9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/helpfuncbycat.xml @@ -0,0 +1,209 @@ + +Functions + Homogeneous transformations + angvec2rangle/vector to RM + angvec2trangle/vector to HT + eul2rEuler angles to RM + eul2trEuler angles to HT + oa2rorientation and approach vector to RM + oa2trorientation and approach vector to HT + r2tRM to HT + rt2tr(R,t) to HT + rotxRM for rotation about X-axis + rotyRM for rotation about Y-axis + rotzRM for rotation about Z-axis + rpy2rroll/pitch/yaw angles to RM + rpy2trroll/pitch/yaw angles to HT + se2HT in SE(2) + t2rHT to RM + tr2angvecHT/RM to angle/vector form + tr2eulHT/RM to Euler angles + tr2rpyHT/RM to roll/pitch/yaw angles + tr2rtHT to (R,t) + tranimateanimate a coordinate frame + translset or extract the translational component of HT + trnormnormalize HT + trplotplot HT as a coordinate frame + trplot2plot HT, SE(2), as a coordinate frame + trprintprint an HT + trotxHT for rotation about X-axis + trotyHT for rotation about Y-axis + trotzHT for rotation about Z-axis + + Homogeneous points and lines + e2hEuclidean coordinates to homogeneous + h2ehomogeneous coordinates to Euclidean + homlinecreate line from 2 points + homtranstransform points + + Differential motion + delta2trdifferential motion vector to HT + eul2jacEuler angles to Jacobian + rpy2jacRPY angles to Jacobian + skewvector to skew symmetric matrix + tr2deltaHT to differential motion vector + tr2jacHT to Jacobian + vexskew symmetric matrix to vector + wtranstransform wrench between frames + + Trajectory generation + ctrajCartesian trajectory + jtrajjoint space trajectory + lspb1D trapezoidal trajectory + mtrajmulti-axis trajectory for arbitrary function + mstrajmulti-axis multi-segment trajectory + tpoly1D polynomial trajectory + trinterpinterpolate HT s + + Quaternion + Quaternionconstructor + /divide quaternion by quaternion or scalar + *multiply quaternion by a quaternion or vector + invinvert a quaternion + normnorm of a quaternion + plotdisplay a quaternion as a 3D rotation + unitunitize a quaternion + interpinterpolate a quaternion + + Serial-link manipulator + SerialLinkconstruct a serial-link robot object + Linkconstruct a robot link object + Revoluteconstruct a revolute robot link object + Prismaticconstruct a prismatic robot link object + *compound two robots + frictionreturn joint friction torques + nofrictionreturn a robot object with no friction + perturbreturn a robot object with perturbed parameters + plotplot/animate robot + teachdrive a graphical robot + CodeGeneratorcreate efficient run-time kinematic and dynamics code + + Models + mdl_ballhigh order ball shaped mechanism + mdl_coilhigh order coil shaped mechanism + mdl_Fanuc10LFanuc 10L (DH, kine) + mdl_MotomanHP6Motoman HP6 (DH, kine) + mdl_phantomxTrossen Robotics PhantomX pincher + mdl_puma560Puma 560 data (DH, kine, dyn) + mdl_puma560_3Puma 560, first 3 joints, kine only + mdl_puma560_3_symPuma 560, first 3 joints, symbolic, kine only + mdl_puma560akbPuma 560 data (MDH, kine, dyn) + mdl_p8Puma 6-axis robot on a 2-axis XY base + mdl_S4ABB2p8ABB S4 2.8 (DH, kine) + mdl_stanfordStanford arm data (DH, kine, dyn) + mdl_twolinksimple 2-link example (DH, kine) + DHFactorconvert elementary transformations to DH form + + Kinematic + DHFactortransform sequence to DH description + fkineforward kinematics + ikineinverse kinematics (numeric) + ikine6sinverse kinematics for 6-axis arm with sph.wrist + jacob0Jacobian in base coordinate frame + jacobnJacobian in end-effector coordinate frame + manipltycompute manipulability + + Dynamics + accelforward dynamics + cinertiaCartesian manipulator inertia matrix + corioliscentripetal/coriolis torque + fdynforward dynamics + wtranstransform a force/moment + gravloadgravity loading + inertiamanipulator inertia matrix + itorqueinertia torque + rneinverse dynamics + + Mobile robot + Mappoint feature map object + RandomPathdriver for Vehicle object + RangeBearingSensor"laser scanner" object + Vehicleconstruct a mobile robot object + sl_bicycleSimulink "bicycle model" of non-holonomic wheeled vehicle + NavigationNavigation superclass + Sensorrobot sensor superclass + plot_vehicleplot vehicle + makemapmake a map, occupancy grid + mdl_quadcoptersimple quadcopter model + sl_quadrotorSimulink model of a flying quadrotor + + Localization + EKFextended Kalman filter object + ParticleFilterMonte Carlo estimator + + Path planning + Bug2bug navigation + DXformdistance transform from map + DstarD* planner + PRMprobabilistic roadmap planner + RRTrapidly exploring random tree + + Graphics + plot2plot trajectory + plotpplot points + plot_arrowdraw an arrow + plot_boxdraw a box + plot_circledraw a circle + plot_ellipsedraw an ellipse + plot_homlineplot homogeneous line + plot_pointplot points + plot_polyplot polygon + plot_spheredraw a sphere + qplotplot joint angle trajectories + plot2Plot trajectories + plotpPlot trajectories + xaxisset x-axis scaling + yaxisset y-axis scaling + xyzlabellabel axes x, y and z + + Utility + aboutsummary of object size and type + angdiffsubtract 2 angles modulo 2pi + bresenhamBresenhan line drawing + circlecompute/draw points on a circle + colnormcolumnwise norm of matrix + diff2elementwise diff + distancexformcompute distance transform + edgelistlist of edge pixels + gauss2dGaussian distribution in 2D + ishomogtrue if argument is a 4x4 matrix + ismatrixtrue if non scalar + isrottrue if argument is a 3x3 matrix + isvectrue if argument is a 3-vector + numcolsnumber of columns in matrix + numrowsnumber of rows in matrix + peakfind peak in 1D signal + peak2find peak in 2D signal + PGraphgeneral purpose graph class + polydiffderivative of polynomial + Polygongeneral purpose polygon class + randinitinitialize random number generator + rampcreate linear ramp + runscriptinteractively run a script or demo + unitunitize a vector + tb_optparsetoolbox argument parser + + CodeGen support + distributeblocksdistribute blocks in a Simulink library + doesblockexistchecks if a Simulink block exists + multidfprintffprintf to multiple files + symexpr2slblockembedded Matlab symbolic functions + simulinkextdetermine extension of Simulink files + + Demonstrations + rtbdemoSerial-link manipulator demonstration + tripleangleGUI to demonsrate triple angle rotations + + Examples + sl_quadcopterSimulink model of a flying quadcopter + sl_braitenbergSimulink model a Braitenberg vehicle + movepointnon-holonomic vehicle moving to a point + movelinenon-holonomic vehicle moving to a line + moveposenon-holonomic vehicle moving to a pose + walkingexample of 4-legged walking robot + eg_inertiajoint 1 inertia I(q1,q2) + eg_inertia22joint 2 inertia I(q3) + eg_gravjoint 2 gravity load g(q2,q3) + + + diff --git a/lwrserv/matlab/rvctools/robot/info/helptoc.xml b/lwrserv/matlab/rvctools/robot/info/helptoc.xml new file mode 100644 index 0000000..7ff7eab --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/helptoc.xml @@ -0,0 +1,44 @@ + + + + + + +Robotics Toolbox for MATLAB + + + + + + + + + Simulink + + + Manual + + + Examples + + + About + + + Acknowledgements + + + Release Notes & Version History + + + Copyrights & License + + + Web Pages + Robotics Toolbox web page + Robotics Toolbox discussion group + Robotics, Vision & Control (the book) + + + + diff --git a/lwrserv/matlab/rvctools/robot/info/icons/bullet_orange.gif b/lwrserv/matlab/rvctools/robot/info/icons/bullet_orange.gif new file mode 100644 index 0000000..6a26b18 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/info/icons/bullet_orange.gif differ diff --git a/lwrserv/matlab/rvctools/robot/info/icons/more_arrows.gif b/lwrserv/matlab/rvctools/robot/info/icons/more_arrows.gif new file mode 100644 index 0000000..d85a7d7 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/info/icons/more_arrows.gif differ diff --git a/lwrserv/matlab/rvctools/robot/info/introduction.html b/lwrserv/matlab/rvctools/robot/info/introduction.html new file mode 100644 index 0000000..de16d6d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/introduction.html @@ -0,0 +1,48 @@ + + +Introduction + + + + + +

Robotics Toolbox for Matlab

+ +

Introduction

+
+Welcome to the Robotics Toolbox for MATLAB. + +The toolbox is based on Matlab code developed by Peter Corke for his PhD research over the period 1990 to 1994. +An early article about the toolbox was published in the +IEEE Robotics & Automation magazine (Volume 3, Issue 1, March 1996). + +Since that time it has been periodically updated, most often, to accomodate changes to the Matlab language. The biggest such change was the introduction of Matlab objects which made the representation of quaternions, robots and links much more convenient. More recently the toolbox was updated to handle modified DH (Craig) as well as standard DH notation. +A more recent article +IEEE Robotics & Automation magazine article (Volume 14, Issue 4, Dec. 2007 Page(s):16 - 17) discusses the toolbox and its +companion toolbox for machine vision. + +Several versions now exist and in different languages such as Python, SciLab and LabView. + + +

Advantages of using the Robotics Toolbox

+ +The Toolboxes have some important virtues. +Firstly, they have been around for a long time and used by many people for many different problems so the code is entitled to some +level of trust. +The Toolbox provides a ``gold standard" with which to compare new algorithms or even the same algorithms coded +in new languages or executing in new environments. + +Secondly, they allow the user to work with real problems, not trivial examples. +For real robots, those with more than two links, or real images with +millions of pixels the computation is beyond unaided human ability. +Thirdly, they allow us to gain insight which is otherwise lost in the complexity. +We can rapidly and easily experiment, play "what if" games, and depict the +results graphically using Matlab's powerful display tools such as 2D and 3D graphs and images. +Matlab that will be familiar to most engineering students and + +Fourthly, the Toolbox code makes many common algorithms tangible and accessible. +You can read the code, you can apply it to your own problems, and you can extend it or rewrite it. +At the very least it gives you a headstart. + + + diff --git a/lwrserv/matlab/rvctools/robot/info/license.html b/lwrserv/matlab/rvctools/robot/info/license.html new file mode 100644 index 0000000..5be000c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/license.html @@ -0,0 +1,704 @@ + + +GNU GENERAL PUBLIC LICENSE + + + + + +

Robotics Toolbox for Matlab

+

Copyrights & License

+
+

Copyrights

+The Robotics Toolbox for Matlab is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later version. +
+
+
+

GNU GENERAL PUBLIC LICENSE

+

Version 3, 29 June 2007

+ +

Copyright © 2007 Free Software Foundation, Inc. <http://fsf.org/>

+ Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed.

+ +

Preamble

+ +

The GNU General Public License is a free, copyleft license for +software and other kinds of works.

+ +

The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too.

+ +

When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things.

+ +

To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others.

+ +

For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights.

+ +

Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it.

+ +

For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions.

+ +

Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users.

+ +

Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free.

+ +

The precise terms and conditions for copying, distribution and +modification follow.

+ +

TERMS AND CONDITIONS

+ +

0. Definitions.

+ +

“This License” refers to version 3 of the GNU General Public License.

+ +

“Copyright” also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks.

+ +

“The Program” refers to any copyrightable work licensed under this +License. Each licensee is addressed as “you”. “Licensees” and +“recipients” may be individuals or organizations.

+ +

To “modify” a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a “modified version” of the +earlier work or a work “based on” the earlier work.

+ +

A “covered work” means either the unmodified Program or a work based +on the Program.

+ +

To “propagate” a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well.

+ +

To “convey” a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying.

+ +

An interactive user interface displays “Appropriate Legal Notices” +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion.

+ +

1. Source Code.

+ +

The “source code” for a work means the preferred form of the work +for making modifications to it. “Object code” means any non-source +form of a work.

+ +

A “Standard Interface” means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language.

+ +

The “System Libraries” of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +“Major Component”, in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it.

+ +

The “Corresponding Source” for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work.

+ +

The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source.

+ +

The Corresponding Source for a work in source code form is that +same work.

+ +

2. Basic Permissions.

+ +

All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law.

+ +

You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you.

+ +

Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary.

+ +

3. Protecting Users' Legal Rights From Anti-Circumvention Law.

+ +

No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures.

+ +

When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures.

+ +

4. Conveying Verbatim Copies.

+ +

You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program.

+ +

You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee.

+ +

5. Conveying Modified Source Versions.

+ +

You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions:

+ +
    +
  • a) The work must carry prominent notices stating that you modified + it, and giving a relevant date.
    • + +
  • b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + “keep intact all notices”.
    • + +
  • c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it.
    • + +
  • d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so.
    • +
+ +

A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +“aggregate” if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate.

+ +

6. Conveying Non-Source Forms.

+ +

You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways:

+ +
    +
  • a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange.
    • + +
  • b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge.
    • + +
  • c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b.
    • + +
  • d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements.
    • + +
  • e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d.
    • +
+ +

A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work.

+ +

A “User Product” is either (1) a “consumer product”, which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, “normally used” refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product.

+ +

“Installation Information” for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made.

+ +

If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM).

+ +

The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network.

+ +

Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying.

+ +

7. Additional Terms.

+ +

“Additional permissions” are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions.

+ +

When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission.

+ +

Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms:

+ +
    +
  • a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or
    • + +
  • b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or
    • + +
  • c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or
    • + +
  • d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or
    • + +
  • e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or
    • + +
  • f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors.
    • +
+ +

All other non-permissive additional terms are considered “further +restrictions” within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying.

+ +

If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms.

+ +

Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way.

+ +

8. Termination.

+ +

You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11).

+ +

However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation.

+ +

Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice.

+ +

Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10.

+ +

9. Acceptance Not Required for Having Copies.

+ +

You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so.

+ +

10. Automatic Licensing of Downstream Recipients.

+ +

Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License.

+ +

An “entity transaction” is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts.

+ +

You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it.

+ +

11. Patents.

+ +

A “contributor” is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's “contributor version”.

+ +

A contributor's “essential patent claims” are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, “control” includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License.

+ +

Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version.

+ +

In the following three paragraphs, a “patent license” is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To “grant” such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party.

+ +

If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. “Knowingly relying” means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid.

+ +

If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it.

+ +

A patent license is “discriminatory” if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007.

+ +

Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law.

+ +

12. No Surrender of Others' Freedom.

+ +

If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program.

+ +

13. Use with the GNU Affero General Public License.

+ +

Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such.

+ +

14. Revised Versions of this License.

+ +

The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns.

+ +

Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License “or any later version” applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation.

+ +

If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program.

+ +

Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version.

+ +

15. Disclaimer of Warranty.

+ +

THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM “AS IS” WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION.

+ +

16. Limitation of Liability.

+ +

IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES.

+ +

17. Interpretation of Sections 15 and 16.

+ +

If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee.

+ +

END OF TERMS AND CONDITIONS

+ +

How to Apply These Terms to Your New Programs

+ +

If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms.

+ +

To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the “copyright” line and a pointer to where the full notice is found.

+ +
    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program 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 General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+ +

Also add information on how to contact you by electronic and paper mail.

+ +

If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode:

+ +
    <program>  Copyright (C) <year>  <name of author>
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+ +

The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an “about box”.

+ +

You should also get your employer (if you work as a programmer) or school, +if any, to sign a “copyright disclaimer” for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +<http://www.gnu.org/licenses/>.

+ +

The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +<http://www.gnu.org/philosophy/why-not-lgpl.html>.

+ + + + diff --git a/lwrserv/matlab/rvctools/robot/info/release.html b/lwrserv/matlab/rvctools/robot/info/release.html new file mode 100644 index 0000000..e2a4ade --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/release.html @@ -0,0 +1,139 @@ + + +Release Notes & Copyrights + + + + + +

Robotics Toolbox for Matlab

+

Release Notes & Version History

+
+ +

Robotics Toolbox version 9

+
    +
  • Changes:
    • +
      +
    • The manual (robot.pdf) no longer contains a per function description. All documentation is now in the m-file, making maintenance and consistency easier.
      • +
    • The Functions link from the Toolbox help browser lists all functions with hyperlinks to the indiviual help entries.
      • +
    • The Robot class is now named SerialLink to be more specific.
      • +
    • Almost all functions that operate on a SerialLink object are now methods rather +than functions, for example plot() or fkine(). In practice this makes little +difference to the user but operations can now be expressed as robot.plot(q) or +plot(robot, q). Toolbox documentation now prefers the former convention which is more +aligned with object-oriented practice. +
    • The parametrers to the Link object constructor are now in the order: theta, d, a, alpha. Why this order? It's the order in which the link transform is created: RZ(theta) TZ(d) TX(a) RX(alpha).
      • +
    • All robot models now begin with the prefix mdl_, so puma560 is now +mdl_puma560. +
    • The function drivebot is now the SerialLink method teach.
      • +
    • The function ikine560 is now the SerialLink method ikine6s to indicate +that it works for any 6-axis robot with a spherical wrist.
      • +
    • The link class is now named Link to adhere to the convention that all classes begin with a capital letter.
      • +
    • The quaternion class is now named Quaternion +to adhere to the convention that all classes begin with a capital letter.
      • +
    • A number of utility functions have been moved into the a directory common since +they are not robot specific.
      • +
    • skew no longer accepts a skew symmetric matrix as an argument and returns a 3-vector, this functionality is provided by the new function vex.
      • +
    • tr2diff and diff2tr are now called tr2delta and delta2tr
      • +
    • ctraj with a scalar argument now spaces the points according to a trapezoidal +velocity profile (see lspb). To obtain even spacing provide a uniformly spaced vector +as the third argument, eg. linspace(0, 1, N).
      • +
    + +
  • New features:
    • +
      +
    • Model of a mobile robot, Vehicle, that has the "bicycle" kinematic model (car-like). For given inputs it updates the robot state and returns noise corrupted odometry measurements. This can be used in conjunction with a "driver" class such as RandomPath which drives the vehicle between random waypoints within a specified rectangular region.
      • +
    • Model of a laser scanner RangeBearingSensor, subclass of Sensor, that works in conjunction with +a Map object to return range and bearing to invariant point features in the environment.
      • +
    • Extended Kalman filter EKF can be used to perform localization by dead reckoning or map featuers, map buildings and simultaneous localization and mapping.
      • +
    • Path planning classes: distance transform DXform, D* lattice planner Dstar, +probabilistic roadmap planner PRM, and rapidly exploring random tree RRT.
      • +
    • The RPY functions tr2rpy and rpy2tr assume that the roll, pitch, yaw +rotations are about the X, Y, Z axes which is consistent with +common conventions for vehicles (planes, ships, ground vehicles). For some applications (eg. cameras) it useful to consider the rotations about the Z, Y, Z axes, and this behaviour can be +obtained by using the option 'zyx' with these functions (note this is the pre release 8 behaviour).
      • +
    • jsingu
      • +
    • jsingu
      • +
    • lspb
      • +
    • tpoly
      • +
    • qplot
      • +
    • mtraj
      • +
    • mstraj
      • +
    • wtrans
      • +
    • se2
      • +
    • se3
      • +
    • trprint compact display of a transform in various formats.
      • +
    • trplot
      • +
    • trplot2 as above but for SE(2)
      • +
    • tranimate
      • +
    • DHFactor a simple means to generate the Denavit-Hartenberg kinematic model of a robot from a sequence of elementary transforms.
      • +
    • Monte Carlo estimator ParticleFilter.
      • +
    • vex performs the inverse function to skew, it converts a skew-symmetric matrix to a 3-vector.
      • +
    • Pgraph represents a non-directed embedded graph, supports plotting and minimum cost path finding.
      • +
    • Polygon a generic 2D polygon class that supports plotting, intersectio/union/difference of polygons, line/polygon intersection, point/polygon containment.
      • +
    • plot_box plot a box given TL/BR corners or center+WH, with options for edge color, fill color and transparency.
      • +
    • plot_circle plot one or more circles, with options for edge color, fill color and transparency.
      • +
    • plot_sphere plot a sphere, with options for edge color, fill color and transparency.
      • +
    • plot_ellipse plot an ellipse, with options for edge color, fill color and transparency.
      • +
    • plot_ellipsoid plot an ellipsoid, with options for edge color, fill color and transparency.
      • +
    • plot_poly plot a polygon, with options for edge color, fill color and transparency.
      • +
    • about one line summary of a matrix or class, compact version of whos
      • +
    • tb_optparse general argument handler and options parser, used internally in many +functions.
      • +
    + +
  • Bugfixes:
    • +
      +
    • Improved error messages in many functions
      • +
    • Removed trailing commas from if and for statements
      • +
    + +
  • Known bugs and limitations:
    • +
      +
    +
  • April, 2011
    • +
+ +
+To see which version of the Robotics Toolbox is currently installed, you can type +
+    info = ver('robot')
+
+The result should look like this: +
+info = 
+
+       Name: 'Robotics Toolbox'
+    Version: '9.0'
+    Release: ''
+       Date: '01-Apr-2011'
+
+ +

Version History

+ +

Robotics Toolbox version 8

+
    +
  • New Features:
    • +
      +
    • First toolbox version
      • +
    • Functions features based on modules release-0.3.3 (October 7th, 2007)
      • +
        +
      • Files added:
        • +
          +
        +
      • Files removed:
        • +
          +
        +
      • Features added:
        • +
          +
        +
      • Bugs fixed:
        • +
          +
        +
      +
    +
  • October 7th, 2007
    • +
+ + + diff --git a/lwrserv/matlab/rvctools/robot/info/robot_product_page.html b/lwrserv/matlab/rvctools/robot/info/robot_product_page.html new file mode 100644 index 0000000..0a21c0e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/info/robot_product_page.html @@ -0,0 +1,88 @@ + + + Robotics Toolbox + +
Robotics Toolbox
+
+ + + + + +
+ + + + +
Functions:
bulletBy Category
bulletAlphabetical List
+
+
+
What's New
+ + + +
bulletRelease Notes
Summarizes new features, bug fixes, etc.
+
+
Printable (PDF) Documentation on the Web
+ + + +
bulletPDF Documents
Provides printable versions of documentation for the Robotics Toolbox
+

+ + + +

© 1990-2011 Peter Corke.

+ diff --git a/lwrserv/matlab/rvctools/robot/info/rtb-montage-notext.png b/lwrserv/matlab/rvctools/robot/info/rtb-montage-notext.png new file mode 100644 index 0000000..8fa2067 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/info/rtb-montage-notext.png differ diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/AllSensorsReadRoomba.m b/lwrserv/matlab/rvctools/robot/interfaces/@Create/AllSensorsReadRoomba.m new file mode 100755 index 0000000..e410665 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/@Create/AllSensorsReadRoomba.m @@ -0,0 +1,117 @@ +function [BumpRight, BumpLeft, BumpFront, Wall, virtWall, CliffLft, ... + CliffRgt, CliffFrntLft, CliffFrntRgt, LeftCurrOver, RightCurrOver, ... + DirtL, DirtR, ButtonPlay, ButtonAdv, Dist, Angle, ... + Volts, Current, Temp, Charge, Capacity, pCharge] = AllSensorsReadRoomba(serPort); +%[BumpRight, BumpLeft, BumpFront, Wall, virtWall, CliffLft, ... +% CliffRgt, CliffFrntLft, CliffFrntRgt, LeftCurrOver, RightCurrOver, ... +% DirtL, DirtR, ButtonPlay, ButtonAdv, Dist, Angle, ... +% Volts, Current, Temp, Charge, Capacity, pCharge] = AllSensorsReadRoomba(serPort) +% Reads Roomba Sensors +% [BumpRight (0/1), BumpLeft(0/1), BumpFront(0/1), Wall(0/1), virtWall(0/1), CliffLft(0/1), ... +% CliffRgt(0/1), CliffFrntLft(0/1), CliffFrntRgt(0/1), LeftCurrOver (0/1), RightCurrOver(0/1), ... +% DirtL(0/1), DirtR(0/1), ButtonPlay(0/1), ButtonAdv(0/1), Dist (meters since last call), Angle (rad since last call), ... +% Volts (V), Current (Amps), Temp (celcius), Charge (milliamphours), Capacity (milliamphours), pCharge (percent)] +% Can add others if you like, see code +% Esposito 3/2008 +% initialize preliminary return values +% By; Joel Esposito, US Naval Academy, 2011 +BumpRight = nan; +BumpLeft = nan; +BumpFront = nan; +Wall = nan; +virtWall = nan; +CliffLft = nan; +CliffRgt = nan; +CliffFrntLft = nan; +CliffFrntRgt = nan; +LeftCurrOver = nan; +RightCurrOver = nan; +DirtL = nan; +DirtR = nan; +ButtonPlay = nan; +ButtonAdv = nan; +Dist = nan; +Angle = nan; +Volts = nan; +Current = nan; +Temp = nan; +Charge = nan; +Capacity = nan; +pCharge = nan; + + +try + +%Flush buffer +N = serPort.BytesAvailable(); +while(N~=0) +fread(serPort,N); +N = serPort.BytesAvailable(); +end + +warning off +global td +sensorPacket = []; +% flushing buffer +confirmation = (fread(serPort,1)); +while ~isempty(confirmation) + confirmation = (fread(serPort,26)); +end + + +%% Get (142) ALL(0) data fields +fwrite(serPort, [142 0]); + +%% Read data fields +BmpWheDrps = dec2bin(fread(serPort, 1),8); % + +BumpRight = bin2dec(BmpWheDrps(end)) % 0 no bump, 1 bump +BumpLeft = bin2dec(BmpWheDrps(end-1)) +if BumpRight*BumpLeft==1 + BumpRight =0; + BumpLeft = 0; + BumpFront =1; +else + BumpFront = 0; +end +Wall = fread(serPort, 1) %0 no wall, 1 wall + +CliffLft = fread(serPort, 1) % no cliff, 1 cliff +CliffFrntLft = fread(serPort, 1) +CliffFrntRgt = fread(serPort, 1) +CliffRgt = fread(serPort, 1) + +virtWall = fread(serPort, 1)%0 no wall, 1 wall + +motorCurr = dec2bin( fread(serPort, 1),8 ); +Low1 = motorCurr(end); % 0 no over curr, 1 over Curr +Low0 = motorCurr(end-1); % 0 no over curr, 1 over Curr +Low2 = motorCurr(end-2); % 0 no over curr, 1 over Curr +LeftCurrOver = motorCurr(end-3) % 0 no over curr, 1 over Curr +RightCurrOver = motorCurr(end-4) % 0 no over curr, 1 over Curr + + +DirtL = fread(serPort, 1) +DirtR = fread(serPort, 1) + +RemoteCode = fread(serPort, 1); % coudl be used by remote or to communicate with sendIR command +Buttons = dec2bin(fread(serPort, 1),8); +ButtonPlay = Buttons(end) +ButtonAdv = Buttons(end-2) + +Dist = fread(serPort, 1, 'int16')/1000 % convert to Meters, signed, average dist wheels traveled since last time called...caps at +/-32 +Angle = fread(serPort, 1, 'int16')*pi/180 % convert to radians, signed, since last time called, CCW positive + +ChargeState = fread(serPort, 1); +Volts = fread(serPort, 1, 'uint16')/1000 +Current = fread(serPort, 1, 'int16')/1000 % neg sourcing, pos charging +Temp = fread(serPort, 1, 'int8') +Charge = fread(serPort, 1, 'uint16') % in mAhours +Capacity = fread(serPort, 1, 'uint16') +pCharge = Charge/Capacity *100 % May be inaccurate +%checksum = fread(serPort, 1) + +pause(td) +catch + disp('WARNING: function did not terminate correctly. Output may be unreliable.') +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffFrontLeftSignalStrengthRoomba.m b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffFrontLeftSignalStrengthRoomba.m new file mode 100644 index 0000000..b192d83 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffFrontLeftSignalStrengthRoomba.m @@ -0,0 +1,32 @@ +function [Signal] = CliffFrontLeftSignalStrengthRoomba(serPort); +%[Signal] = CliffFrontLeftSignalStrengthRoomba(serPort) +%Displays the strength of the front left cliff sensor's signal. +%Ranges between 0-100 percent signal + + + +% By; Joel Esposito, US Naval Academy, 2011 + +%Initialize preliminary return values +Signal = nan; + +try + +%Flush Buffer +N = serPort.BytesAvailable(); +while(N~=0) +fread(serPort,N); +N = serPort.BytesAvailable(); +end + +warning off +global td +fwrite(serPort, [142]); fwrite(serPort,29); + +Strength = fread(serPort, 1, 'uint16'); +Signal=(Strength/4095)*100; + +pause(td) +catch + disp('WARNING: function did not terminate correctly. Output may be unreliable.') +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffFrontRightSensorRoomba.m b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffFrontRightSensorRoomba.m new file mode 100644 index 0000000..37ce55f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffFrontRightSensorRoomba.m @@ -0,0 +1,30 @@ +function [state] = CliffFrontRightSensorRoomba(serPort); +%[state] = CliffFrontRightSensorRoomba(serPort) +% Specifies the state of the Cliff Front Right sensor +% Either triggered or not triggered + +% By; Joel Esposito, US Naval Academy, 2011 + +%Initialize preliminary return values +state = nan; + +try + +%Flush Buffer +N = serPort.BytesAvailable(); +while(N~=0) +fread(serPort,N); +N = serPort.BytesAvailable(); +end + +warning off +global td + +fwrite(serPort, [142]); fwrite(serPort,11); +CliffFrntRght = dec2bin(fread(serPort, 1)); +state = bin2dec(CliffFrntRght(end)); + +pause(td) +catch + disp('WARNING: function did not terminate correctly. Output may be unreliable.') +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffRightSensorRoomba.m b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffRightSensorRoomba.m new file mode 100755 index 0000000..735eb6b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffRightSensorRoomba.m @@ -0,0 +1,28 @@ +function [state] = CliffRightSensorRoomba(serPort); +%[state] = CliffRightSensorRoomba(serPort) +% Specifies the state of the cliff right sensor +% Either triggered or not triggered + +% By; Joel Esposito, US Naval Academy, 2011 +%Initialize Preliminary return values +state = nan; + +try + +%Flush Buffer +N = serPort.BytesAvailable(); +while(N~=0) +fread(serPort,N); +N = serPort.BytesAvailable(); +end +warning off +global td + +fwrite(serPort, [142]); fwrite(serPort,12); + +CliffRight = dec2bin(fread(serPort, 1)); +state = bin2dec(CliffRight(end)); +pause(td) +catch + disp('WARNING: function did not terminate correctly. Output may be unreliable.') +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffRightSignalStrengthRoomba.m b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffRightSignalStrengthRoomba.m new file mode 100755 index 0000000..f3a4fcd --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CliffRightSignalStrengthRoomba.m @@ -0,0 +1,31 @@ +function [Signal] = CliffRightSignalStrengthRoomba(serPort); +%[Signal] = CliffRightSignalStrengthRoomba(serPort) +%Displays the strength of the right cliff sensor's signal. +%Ranges between 0-100 percent signal + + +% By; Joel Esposito, US Naval Academy, 2011 + +%Initialize preliminary return values +Signal = nan; + +try + +%Flush Buffer +N = serPort.BytesAvailable(); +while(N~=0) +fread(serPort,N); +N = serPort.BytesAvailable(); +end + +warning off +global td +fwrite(serPort, [142]); fwrite(serPort,31); + +Strength = fread(serPort, 1, 'uint16'); +Signal=(Strength/4095)*100; + +pause(td) +catch + disp('WARNING: function did not terminate correctly. Output may be unreliable.') +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/CurrentTesterRoomba.m b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CurrentTesterRoomba.m new file mode 100644 index 0000000..3cd2901 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/@Create/CurrentTesterRoomba.m @@ -0,0 +1,31 @@ +function [Current] = CurrentTesterRoomba(serPort); +%[Current] = CurrentTesterRoomba(serPort) +% Displays the current (in amps) flowing into or out of Create's battery. +% Negative currents indicate that current is flowing out of the battery. +% Positive currents indicate that current is flowing into the battery. + + +% By; Joel Esposito, US Naval Academy, 2011 + +%Initialize preliminary return values +Current = nan; + +try + +%Flush Buffer +N = serPort.BytesAvailable(); +while(N~=0) +fread(serPort,N); +N = serPort.BytesAvailable(); +end + +warning off +global td +fwrite(serPort, [142]); fwrite(serPort,23); + +Current = fread(serPort, 1, 'int16')/1000; + +pause(td) +catch + disp('WARNING: function did not terminate correctly. Output may be unreliable.') +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/KeyBoardControl.gif b/lwrserv/matlab/rvctools/robot/interfaces/@Create/KeyBoardControl.gif new file mode 100755 index 0000000..e2f5753 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/interfaces/@Create/KeyBoardControl.gif differ diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/KeyBoardControl.png b/lwrserv/matlab/rvctools/robot/interfaces/@Create/KeyBoardControl.png new file mode 100755 index 0000000..44904e1 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/interfaces/@Create/KeyBoardControl.png differ diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/SetFwdVelRadiusRoomba.m b/lwrserv/matlab/rvctools/robot/interfaces/@Create/SetFwdVelRadiusRoomba.m new file mode 100644 index 0000000..54970fa --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/@Create/SetFwdVelRadiusRoomba.m @@ -0,0 +1,44 @@ +function [] = SetFwdVelRadiusRoomba(serPort, FwdVel, Radius); +%[] = SetFwdVelRadiusRoomba(serPort, FwdVel, Radius) + +% Moves Roomba by setting forward vel and turn radius +% serPort is a serial port object created by Roombainit +% FwdVel is forward vel in m/sec [-0.5, 0.5], +% Radius in meters, postive turns left, negative turns right [-2,2]. +% Special cases: Straight = inf +% Turn in place clockwise = -eps +% Turn in place counter-clockwise = eps + +% By; Joel Esposito, US Naval Academy, 2011 +try + +%Flush Buffer +N = serPort.BytesAvailable(); +while(N~=0) +fread(serPort,N); +N = serPort.BytesAvailable(); +end + +warning off +global td +%% Convert to millimeters +FwdVelMM = min( max(FwdVel,-.5), .5)*1000; +if isinf(Radius) + RadiusMM = 32768; +elseif Radius == eps + RadiusMM = 1; +elseif Radius == -eps + RadiusMM = -1; +else + RadiusMM = min( max(Radius*1000,-2000), 2000); +end + +%fwrite(serPort, [137, 255, 56, 1, 244]) + +fwrite(serPort, [137]); fwrite(serPort,FwdVelMM, 'int16'); fwrite(serPort,RadiusMM, 'int16'); +disp('moving!') +pause(td) +catch + disp('WARNING: function did not terminate correctly. Output may be unreliable.') +end + diff --git a/lwrserv/matlab/rvctools/robot/interfaces/@Create/leds.m b/lwrserv/matlab/rvctools/robot/interfaces/@Create/leds.m new file mode 100644 index 0000000..61178c5 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/@Create/leds.m @@ -0,0 +1,28 @@ +function [] = leds(robot, LED, Color, Intensity) +%[] = SetLEDsRoomba(serPort, LED,Color, Intensity) +% Manipulates LEDS +% LED = 0 for Play or 1 for Advance +% Color determines which color(Red/Green) that the Power LED will +% illuminate as, from 0-100% +% 0 is pure green, 100 is pure red. +% Intensity determines how bright the Power LED appears from 1-100% + + +% By; Joel Esposito, US Naval Academy, 2011 + + switch LED + case 0 + LED=bin2dec('00000010'); + case 1 + LED=bin2dec('00001000'); + otherwise + error('LED is 0 or 1'); + end + + aColor= (Color/100)*255; + aIntensity = (Intensity/100)*255; + + robot.write([139 LED aColor aIntensity]); + + pause(0.05); + diff --git a/lwrserv/matlab/rvctools/robot/interfaces/Arbotix.m b/lwrserv/matlab/rvctools/robot/interfaces/Arbotix.m new file mode 100644 index 0000000..0415a60 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/Arbotix.m @@ -0,0 +1,792 @@ +%Arbotix Interface to Arbotix robot-arm controller +% +% A concrete subclass of the abstract Machine class that implements a +% connection over a serial port to an Arbotix robot-arm controller. +% +% Methods:: +% Arbotix Constructor, establishes serial communications +% delete Destructor, closes serial connection +% getpos Get joint angles +% setpos Set joint angles and optionally speed +% setpath Load a trajectory into Arbotix RAM +% relax Control relax (zero torque) state +% setled Control LEDs on servos +% gettemp Temperature of motors +%- +% writedata1 Write byte data to servo control table +% writedata2 Write word data to servo control table +% readdata Read servo control table +%- +% command Execute command on servo +% flush Flushes serial data buffer +% receive Receive data +% +% Example:: +% arb=Arbotix('port', '/dev/tty.usbserial-A800JDPN', 'nservos', 5); +% q = arb.getpos(); +% arb.setpos(q + 0.1); +% +% Notes:: +% - This is experimental code. +% - Considers the robot as a string of motors, and the last joint is +% assumed to be the gripper. This should be abstracted, at the moment this +% is done in RobotArm. +% - Connects via serial port to an Arbotix controller running the pypose +% sketch. +% +% See also Machine, RobotArm. + +% Copyright (C) 1993-2015, 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.petercorke.com + +% Copyright (c) 2013 Peter Corke + +% only a subset of Arbotix commands supported +% READDATA, WRITEDATA (1 or 2 bytes only) +% WRITESYNC and ACTION not supported but should be + +% Should subclass an abstract superclass Machine + +classdef Arbotix < Machine + + properties + serPort; + nservos; + + gripper + end + + properties (Constant) + %%% define some important memory locations + ADDR_VERSION = 2; + ADDR_DELAYTIME = 5; + ADDR_CWLIMIT = 6; + ADDR_CCWLIMIT = 8; + ADDR_ALARM_LED = 17; + ADDR_ALARM_SHUTDOWN = 18; + ADDR_LED = 25; + ADDR_TORQUE = 24 + ADDR_GOAL = 30; + ADDR_SPEED = 32; + ADDR_POS = 36; + ADDR_TEMP = 43; + + %%% define the instructions + % native Arbotix instructions + INS_READ_DATA = 2; + INS_WRITE_DATA = 3; + + % pseudo Arbotix instructions, implemented by Arbotix pypose + INS_ARB_SIZE_POSE = 7; + INS_ARB_LOAD_POSE = 8; + INS_ARB_LOAD_SEQ = 9; + INS_ARB_PLAY_SEQ = 10; + INS_ARB_LOOP_SEQ = 11; + INS_ARB_TEST = 25; + end + + % Communications format: + % + % To Arbotix: + % + % 0xff 0xff ID LEN INSTRUC PAYLOAD CHKSUM + % + % ID single byte, identifies the servo on the bus 0-(N-1) + % LEN single byte, indicates the number of bytes to follow the LEN byte incl checksum + % INSTRUC a single byte saying what to do: + % 2 read data from control table: PAYLOAD=START_ADR, NUM_BYTES + % 3 write data to control table: PAYLOAD=START_ADR, P1, P2, ... + % + % 7 set #DOF: PAYLOAD=NP + % 8 load pose: PAYLOAD=POSE# $Q_1 $Q_2 ... $Q_NP + % 9 load sequence: PAYLOAD=POSE# $T POSE# $T ... 255 + % 10 run sequence + % 11 loop sequence + % + % CHKSUM is a single byte, modulo 256 sum over ID .. PAYLOAD. + % + % Notes:: + % - $X means that X is a 16-bit (word) quantity. + % - Arbotix only passes instructions 2 and 3 to the servos, other Dynamixel + % instructions are inaccessible. + % - Instructions 7-11 are implemented by the Arbotix pypose sketch. + % + % Methods:: + % writedata1 Writes byte data to control table + % writedata2 Writes word data to control table + % readdata Reads data from control table + % command Invokes instruction on servo + + + methods + function arb = Arbotix(varargin) + %Arbotix.Arbotix Create Arbotix interface object + % + % ARB = Arbotix(OPTIONS) is an object that represents a connection to a chain + % of Arbotix servos connected via an Arbotix controller and serial link to the + % host computer. + % + % Options:: + % 'port',P Name of the serial port device, eg. /dev/tty.USB0 + % 'baud',B Set baud rate (default 38400) + % 'debug',D Debug level, show communications packets (default 0) + % 'nservos',N Number of servos in the chain + + opt.port = ''; + opt.debug = false; + opt.nservos = []; + opt.baud = 38400; + + opt = tb_optparse(opt, varargin); + + arb.serPort = opt.port; + arb.debug = opt.debug; + arb.nservos = opt.nservos; + + arb.connect(opt); + + % open and closed amount + arb.gripper = [0 2.6]; + + end + + function connect(arb, opt) + %Arbotix.connect Connect to the physical robot controller + % + % ARB.connect() establish a serial connection to the physical robot + % controller. + % + % See also Arbotix.disconnect. + + % clean up any previous instances of the port, can happen... + for tty = instrfind('port', opt.port) + if ~isempty(tty) + disp(['serPort ' tty.port 'is in use. Closing it.']) + fclose(tty); + delete(tty); + end + end + + if opt.verbose + disp('Establishing connection to Arbotix chain...'); + end + + arb.serPort = serial(arb.serPort,'BaudRate', opt.baud); + set(arb.serPort,'InputBufferSize',1000) + set(arb.serPort, 'Timeout', 1) + set(arb.serPort, 'Tag', 'Arbotix') + + if opt.verbose + disp('Opening connection to Arbotix chain...'); + end + + pause(0.5); + try + fopen(arb.serPort); + catch me + disp('open failed'); + me.message + return + end + + arb.flush(); + end + + function disconnect(arb) + %Arbotix.disconnect Disconnect from the physical robot controller + % + % ARB.disconnect() closes the serial connection. + % + % See also Arbotix.connect. + + tty = instrfind('port', arb.serPort.port); + fclose(tty); + delete(tty); + end + + function s = char(arb) + %Arbotix.char Convert Arbotix status to string + % + % C = ARB.char() is a string that succinctly describes the status + % of the Arbotix controller link. + + % show serport Status, number of servos + s = sprintf('Arbotix chain on serPort %s (%s)', ... + arb.serPort.port, get(arb.serPort, 'Status')); + if arb.nservos + s = strvcat(s, sprintf(' %d servos in chain', arb.nservos)); + end + end + + function display(arb) + %Arbotix.display Display parameters + % + % ARB.display() displays the servo parameters in compact single line format. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a Arbotix object and the command has no trailing + % semicolon. + % + % See also Arbotix.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(arb) ); + end % display() + + + function p = getpos(arb, id) + %Arbotix.getpos Get position + % + % P = ARB.GETPOS(ID) is the position (0-1023) of servo ID. + % + % P = ARB.GETPOS([]) is a vector (1xN) of positions of servos 1 to N. + % + % Notes:: + % - N is defined at construction time by the 'nservos' option. + % + % See also Arbotix.e2a. + + arb.flush(); + + if nargin < 2 + id = []; + end + + if ~isempty(id) + retval = arb.readdata(id, Arbotix.ADDR_POS, 2); + p = Arbotix.e2a( retval.params*[1; 256] ); + else + if isempty(arb.nservos) + error('RTB:Arbotix:notspec', 'Number of servos not specified'); + end + p = []; + for j=1:arb.nservos + retval = arb.readdata(j, Arbotix.ADDR_POS, 2); + p(j) = Arbotix.e2a( retval.params*[1; 256] ); + end + end + end + + function setpos(arb, varargin) + %Arbotix.setpos Set position + % + % ARB.SETPOS(ID, POS) sets the position (0-1023) of servo ID. + % ARB.SETPOS(ID, POS, SPEED) as above but also sets the speed. + % + % ARB.SETPOS(POS) sets the position of servos 1-N to corresponding elements + % of the vector POS (1xN). + % ARB.SETPOS(POS, SPEED) as above but also sets the velocity SPEED (1xN). + % + % Notes:: + % - ID is in the range 1 to N + % - N is defined at construction time by the 'nservos' option. + % - SPEED varies from 0 to 1023, 0 means largest possible speed. + % + % See also Arbotix.a2e. + + + if length(varargin{1}) > 1 + % vector mode + pos = varargin{1}; + + if isempty(arb.nservos) + error('RTB:Arbotix:notspec', 'Number of servos not specified'); + end + if length(pos) ~= arb.nservos + error('RTB:Arbotix:badarg', 'Length of POS vector must match number of servos'); + end + for j=1:arb.nservos + arb.writedata2(j, Arbotix.ADDR_GOAL, Arbotix.a2e( pos(j) )); + end + + % need a separate write for this, since pypose writes max of 2 bytes + if nargin == 3 + speed = varargin{2}; + if length(speed) ~= arb.nservos + error('RTB:Arbotix:badarg', 'Length of SPEED vector must match number of servos'); + end + for j=1:arb.nservos + arb.writedata2(j, Arbotix.ADDR_SPEED, speed(j)); + end + end + else + % single joint mode + id = varargin{1}; pos = varargin{2}; + + arb.writedata2(id, Arbotix.ADDR_GOAL, Arbotix.a2e( pos )); + + if nargin == 4 + speed = varargin{3}; + arb.writedata2(id, Arbotix.ADDR_SPEED, speed); + end + end + end + + + function setpath(arb, jt, t) + %Arbotix.setpath Load a path into Arbotix controller + % + % ARB.setpath(JT) stores the path JT (PxN) in the Arbotix controller + % where P is the number of points on the path and N is the number of + % robot joints. Allows for smooth multi-axis motion. + % + % See also Arbotix.a2e. + + % will the path fit in Arbotix memory? + if numrows(jt) > 30 + error('RTB:Arbotix:badarg', 'Too many points on trajectory (30 max)'); + end + + jt = Arbotix.a2e(jt); % convert to encoder values + + % set the number of servos + arb.command(253, Arbotix.INS_ARB_SIZE_POSE, uint8(numcols(jt))); + pause(0.2); % this delay is important + + % set the poses + % payload: q1 q2 .. qN + for i=1:numrows(jt) + pose = jt(i,:); + arb.command(253, Arbotix.INS_ARB_LOAD_POSE, [i-1 typecast( uint16(pose), 'uint8')]); + end + + % set the sequence in which to execute the poses + if nargin < 3 + dt = 200; % milliseconds between poses + else + dt = t*1000; + end + dt8 = typecast( uint16(dt), 'uint8'); + + + cmd = []; + for i=1:numrows(jt) + cmd = [cmd uint8(i-1) dt8]; + end + + cmd = [cmd 255 0 0]; % mark end of sequence + arb.command(253, Arbotix.INS_ARB_LOAD_SEQ, cmd); + + + % now do it + arb.command(253, Arbotix.INS_ARB_PLAY_SEQ); + + end + + + + + function relax(arb, id, status) + %Arbotix.relax Control relax state + % + % ARB.RELAX(ID) causes the servo ID to enter zero-torque (relax state) + % ARB.RELAX(ID, FALSE) causes the servo ID to enter position-control mode + % ARB.RELAX([]) causes servos 1 to N to relax + % ARB.RELAX() as above + % ARB.RELAX([], FALSE) causes servos 1 to N to enter position-control mode. + % + % Notes:: + % - N is defined at construction time by the 'nservos' option. + + if nargin == 1 || isempty(id) + % vector mode + if isempty(arb.nservos) + error('RTB:Arbotix:notspec', 'Number of servos not specified'); + end + if nargin < 3 + % relax mode + for j=1:arb.nservos + arb.writedata1(j, Arbotix.ADDR_TORQUE, 0); + end + else + for j=1:arb.nservos + arb.writedata1(j, Arbotix.ADDR_TORQUE, status==false); + end + end + else + % single joint mode + if nargin < 3 + % relax mode + arb.writedata1(id, Arbotix.ADDR_TORQUE, 0); + else + arb.writedata1(id, Arbotix.ADDR_TORQUE, status==false); + end + end + end + + function setled(arb, id, status) + %Arbotix.led Set LEDs on servos + % + % ARB.led(ID, STATUS) sets the LED on servo ID to on or off according + % to the STATUS (logical). + % + % ARB.led([], STATUS) as above but the LEDs on servos 1 to N are set. + % + % Notes:: + % - N is defined at construction time by the 'nservos' option. + + + if isempty(id) + % vector mode + if isempty(arb.nservos) + error('RTB:Arbotix:notspec', 'Number of servos not specified'); + end + + for j=1:arb.nservos + + arb.writedata1(j, Arbotix.ADDR_LED, status); + end + else + % single joint mode + + arb.writedata1(id, Arbotix.ADDR_LED, status); + end + end + + + function t = gettemp(arb, id) + %Arbotix.gettemp Get temperature + % + % T = ARB.GETTEMP(ID) is the tempeature (deg C) of servo ID. + % + % T = ARB.GETTEMP() is a vector (1xN) of the temperature of servos 1 to N. + % + % Notes:: + % - N is defined at construction time by the 'nservos' option. + + arb.flush(); + + if nargin == 2 + retval = arb.readdata(id, Arbotix.ADDR_TEMP, 1); + t = retval.params; + elseif nargin < 2 + if isempty(arb.nservos) + error('RTB:Arbotix:notspec', 'Number of servos not specified'); + end + t = []; + for j=1:arb.nservos + retval = arb.readdata(j, Arbotix.ADDR_TEMP, 1); + t(j) = retval.params; + end + end + end + + function retval = readdata(arb, id, addr, len) + %Arbotix.readdata Read byte data from servo control table + % + % R = ARB.READDATA(ID, ADDR) reads the successive elements of the servo + % control table for servo ID, starting at address ADDR. The complete + % return status in the structure R, and the byte data is a vector in the + % field 'params'. + % + % Notes:: + % - ID is in the range 0 to N-1, where N is the number of servos in the system. + % - If 'debug' was enabled in the constructor then the hex values are echoed + % to the screen as well as being sent to the Arbotix. + % + % See also Arbotix.receive, Arbotix.command. + + arb.command(id, Arbotix.INS_READ_DATA, [addr len]); + + retval = arb.receive(); + end + + function writedata1(arb, id, addr, data) + %Arbotix.writedata1 Write byte data to servo control table + % + % ARB.WRITEDATA1(ID, ADDR, DATA) writes the successive elements of DATA to the + % servo control table for servo ID, starting at address ADDR. The values of + % DATA must be in the range 0 to 255. + % + % Notes:: + % - ID is in the range 0 to N-1, where N is the number of servos in the system. + % - If 'debug' was enabled in the constructor then the hex values are echoed + % to the screen as well as being sent to the Arbotix. + % + % See also Arbotix.writedata2, Arbotix.command. + + % each element of data is converted to a single byte + + arb.command(id, Arbotix.INS_WRITE_DATA, [addr uint8(data)]); + end + + function writedata2(arb, id, addr, data) + %Arbotix.writedata2 Write word data to servo control table + % + % ARB.WRITEDATA2(ID, ADDR, DATA) writes the successive elements of DATA to the + % servo control table for servo ID, starting at address ADDR. The values of + % DATA must be in the range 0 to 65535. + % + % Notes:: + % - ID is in the range 0 to N-1, where N is the number of servos in the system. + % - If 'debug' was enabled in the constructor then the hex values are echoed + % to the screen as well as being sent to the Arbotix. + % + % See also Arbotix.writedata1, Arbotix.command. + + % each element of data is converted to a two byte value + arb.command(id, Arbotix.INS_WRITE_DATA, [addr typecast( uint16(data), 'uint8')]); + end + + + function out = command(arb, id, instruc, data) + %Arbotix.command Execute command on servo + % + % R = ARB.COMMAND(ID, INSTRUC) executes the instruction INSTRUC on servo ID. + % + % R = ARB.COMMAND(ID, INSTRUC, DATA) as above but the vector DATA forms the + % payload of the command message, and all numeric values in DATA must be + % in the range 0 to 255. + % + % The optional output argument R is a structure holding the return status. + % + % Notes:: + % - ID is in the range 0 to N-1, where N is the number of servos in the system. + % - Values for INSTRUC are defined as class properties INS_*. + % - If 'debug' was enabled in the constructor then the hex values are echoed + % to the screen as well as being sent to the Arbotix. + % - If an output argument is requested the serial channel is flushed first. + % + % See also Arbotix.receive, Arbotix.flush. + + if nargout > 0 + arb.flush(); + end + + if isempty(id) + id = 254; % 0xFE is broadcast + end + + if nargin < 4 + data = []; + end + out = [id length(data)+2 instruc data]; + cs = bitcmp(uint8( mod(sum(out), 256))); + out = [255 255 uint8(out) cs]; + if arb.debug > 0 + fprintf('send: '); + fprintf('0x%02x ', out); + fprintf('\n'); + end + fwrite(arb.serPort, out); + + if nargout > 0 + out = arb.receive(); + end + + end + + + function out = flush(robot) + %Arbotix.flush Flush the receive buffer + % + % ARB.FLUSH() flushes the serial input buffer, data is discarded. + % + % S = ARB.FLUSH() as above but returns a vector of all bytes flushed from + % the channel. + % + % Notes:: + % - Every command sent to the Arbotix elicits a reply. + % - The method receive() should be called after every command. + % - Some Arbotix commands also return diagnostic text information. + % + % See also Arbotix.receive, Arbotix.parse. + + %Flush Buffer + N = robot.serPort.BytesAvailable(); + data = []; + % this returns a maximum of input buffer size + while (N ~= 0) + data = [data; fread(robot.serPort, N)]; + pause(0.1); % seem to need this + N = robot.serPort.BytesAvailable(); + end + + if nargout > 0 + out = data; + end + + end + + + function s = receive(arb) + %Arbotix.receive Decode Arbotix return packet + % + % R = ARB.RECEIVE() reads and parses the return packet from the Arbotix + % and returns a structure with the following fields: + % id The id of the servo that sent the message + % error Error code, 0 means OK + % params The returned parameters, can be a vector of byte values + % + % Notes:: + % - Every command sent to the Arbotix elicits a reply. + % - The method receive() should be called after every command. + % - Some Arbotix commands also return diagnostic text information. + % - If 'debug' was enabled in the constructor then the hex values are echoed + + % + % See also Arbotix.command, Arbotix.flush. + state = 0; + if arb.debug > 0 + fprintf('receive: '); + end + while true + c = fread(arb.serPort, 1, 'uint8'); + if arb.debug > 0 + fprintf('0x%02x ', c); + end + switch state + case 0 % expecting first 0xff + if c == 255 + state = 1; + end + case 1 % expecting second 0xff + if c == 255 + state = 2; + end + case 2 % expecting id + s.id = c; + state = 3; + case 3 % expecting length + len = c; + count = len-2; + params = []; + state = 4; + case 4 % expecting error code + s.error = c; + state = 5; + case 5 % expecting parameters + params = [params c]; + count = count - 1; + if count == 0 + state = 6; + s.params = params; + end + case 6 % expecting checksum + cs = bitcmp(uint8( mod(s.id + len + sum(params), 256))); + + if arb.debug > 0 + fprintf('[0x%02x]\n', cs); + end + if cs ~= c + fprintf('checksum fail: is %d, should be %d\n', ... + c, cs); + end + state = 0; + + + break; + end + end + end + +% % Low-level Dynamixel bus functions not supported by pypose sketch +% % Need to create better code for the Arbotix board +% +% function setpos_sync(arb, pos, speed) +% % pos, speed are vectors +% end +% function discover(arb) +% % find how many servos in the chain +% end +% function ping(arb, id) +% arb.command(id, 1); +% +% retval = arb.receive(); +% retval +% end +% +% function regwrite(arb, id, addr, data) +% arb.command(id, 4, [addr data]); +% end +% +% function action(arb) +% arb.command(id, 5); +% end +% +% function reset(arb, id) +% arb.command(id, 6); +% end +% +% function syncwrite(arb, addr, matrix) +% % one column per actuator +% arb.command(id, hex2dec('83')); +% end + end + + methods(Static) + function a = e2a(e) + %Arbotix.e2a Convert encoder to angle + % + % A = ARB.E2A(E) is a vector of joint angles A corresponding to the + % vector of encoder values E. + % + % TODO: + % - Scale factor is constant, should be a parameter to constructor. + a = (e - 512) / 512 * 150 / 180 * pi; + + end + + function e = a2e(a) + %Arbotix.a2e Convert angle to encoder + % + % E = ARB.A2E(A) is a vector of encoder values E corresponding to the + % vector of joint angles A. + % TODO: + % - Scale factor is constant, should be a parameter to constructor. + e = a / pi * 180 / 150 * 512 + 512; + end + + function parse(s) + %Arbotix.parse Parse Arbotix return strings + % + % ARB.PARSE(S) parses the string returned from the Arbotix controller and + % prints diagnostic text. The string S contains a mixture of Dynamixel + % style return packets and diagnostic text. + % + % Notes:: + % - Every command sent to the Arbotix elicits a reply. + % - The method receive() should be called after every command. + % - Some Arbotix commands also return diagnostic text information. + % + % See also Arbotix.flush, Arbotix.command. + + str = []; + + while length(s) > 0 + if s(1) == 255 && s(2) == 255 + % we have a packet + len = s(4); + pkt = s(1:4+len); + s = s(4+len+1:end); + else + % we have a regular string character + str = [str s(1)]; + s = s(2:end); + end + end + fprintf('str: %s\n', char(str)); + end + + end +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/DataReader.class b/lwrserv/matlab/rvctools/robot/interfaces/DataReader.class new file mode 100644 index 0000000..41b57c4 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/interfaces/DataReader.class differ diff --git a/lwrserv/matlab/rvctools/robot/interfaces/DataReader.java b/lwrserv/matlab/rvctools/robot/interfaces/DataReader.java new file mode 100644 index 0000000..fbb717f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/DataReader.java @@ -0,0 +1,39 @@ +import java.io.*; + +class DataReader +{ +public DataReader(DataInput data_input) +{ + m_data_input = data_input; +} + +public byte[] readBuffer(int length) +{ + byte[] buffer = new byte[length]; + + try + { + m_data_input.readFully(buffer, 0, length); + } + + catch (StreamCorruptedException e) + { + System.out.println("Stream Corrupted Exception Occured"); + buffer = new byte[0]; + } + catch (EOFException e) + { + System.out.println("EOF Reached"); + buffer = new byte[0]; + } + catch (IOException e) + { + System.out.println("IO Exception Occured"); + buffer = new byte[0]; + } + + return buffer; +} + +private DataInput m_data_input; +} diff --git a/lwrserv/matlab/rvctools/robot/interfaces/DataReader.m b/lwrserv/matlab/rvctools/robot/interfaces/DataReader.m new file mode 100644 index 0000000..e69de29 diff --git a/lwrserv/matlab/rvctools/robot/interfaces/Machine.m b/lwrserv/matlab/rvctools/robot/interfaces/Machine.m new file mode 100644 index 0000000..1dbe2d9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/Machine.m @@ -0,0 +1,65 @@ +%Machine Superclass for an interface to a physical robot controller +% +% An abstract superclass for implementing connections to physical robot controllers. +% +% +% Abstract methods:: +% These methods must be implemented in a subclass +% +% open Establishes connection to physical robot +% close Close connection to physical robot +% getpos Get joint angles +% setpos Set joint angles and optionally speed +% +% Notes:: +% - Subclasses the MATLAB handle class which means that pass by reference semantics +% apply. +% - experimental code. +% +% See also Arbotix, RobotArm. + +% Copyright (C) 1993-2015, 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.petercorke.com + +% Copyright (c) 2013 Peter Corke + +% only a subset of Arbotix commands supported +% READDATA, WRITEDATA (1 or 2 bytes only) +% WRITESYNC and ACTION not supported but should be + +% Should subclass an abstract superclass Machine + +classdef Machine < handle + + properties + debug; + end + + methods (Abstract) + connect(m, varargin) + disconnect(m) + p = getpos(m) + setpos(m, varargin) + end + + methods + function m = Machine(varargin) + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/SensorLog.m b/lwrserv/matlab/rvctools/robot/interfaces/SensorLog.m new file mode 100644 index 0000000..b8352fc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/SensorLog.m @@ -0,0 +1,166 @@ +%SensorLog Receive data from SensorLog app +% +% Class to receive data from the SensorLog iPhone app +% +% S = SensorLogconnect to a server and read a message +% +% Usage - message = client(host, port, number_of_retries) +% +% Usage: +% start the matlab code +% start the server +% + +% Copyright (C) 1993-2015, 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.petercorke.com + +classdef SensorLog < handle + properties + socket + stream + d_stream + struct + names + timer + end + + properties (Hidden) + cleanup + end + + + methods + + function s = SensorLog(host, port, timeout) + + + + import java.net.Socket + import java.io.* + + + retry = 0; + number_of_retries = 5; + message = []; + + if nargin < 3 + timeout = 30; + end + + while true + + retry = retry + 1; + if ((number_of_retries > 0) && (retry > number_of_retries)) + fprintf(1, 'Too many retries\n'); + break; + end + + try + fprintf(1, 'Retry %d connecting to %s:%d\n', ... + retry, host, port); + + % throws if unable to connect + s.socket = Socket(host, port); + + % get a buffered data input stream from the socket + s.stream = s.socket.getInputStream; + s.d_stream = DataInputStream(s.stream); + + fprintf(1, 'Connected to server\n'); + + break; + + + catch me + me + me.message + % pause before retrying + pause(1); + end + end + +% s.timer = timer('Period', 0.5, ... +% 'TimerFcn', @timercb, ... +% 'UserData', s, ... +% 'executionMode', 'fixedSpacing'); +% +% start(s.timer) + end + + + function delete(s) + if ~isempty(s.socket) + s.socket.close; + end +% stop(s.timer) +% set(s.timer, 'UserData', []); +% delete(s.timer) + + end + + function flush(s) + end + + function plot(s) + end + + function msg = get(s) + % read data from the socket - wait a short time first + bytes_available = s.stream.available; + %fprintf(1, 'Reading %d bytes\n', bytes_available); + data_reader = DataReader(s.d_stream); + msg = data_reader.readBuffer(bytes_available); + + msg = char(msg'); % Data comes out as a column vector + + for line = strsplit(msg, '\n') + line = line{1}; + + fields = strsplit(line, ','); + if strcmp(fields{1}, 'time') + % have a header line + s.struct = []; + for i=1:length(fields) + s.struct = setfield(s.struct, fields{i}, 0); + end + fprintf('received header line\n'); + s.names = fieldnames(s.struct) + continue; + else + if ~isempty(s.names) + for i=1:length(fields) + s.struct = setfield(s.struct, s.names{i}, str2num(fields{i})); + end + end + end + + end + msg = s.struct; + end + + end +end +function timercb(t, event) + 1 + %s = get(t, 'UserData'); + %s.get() + +end + + diff --git a/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP.m b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP.m new file mode 100644 index 0000000..7cda27a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP.m @@ -0,0 +1,1035 @@ +%VREP V-REP simulator communications object +% +% A VREP object holds all information related to the state of a connection +% to an instance of the V-REP simulator running on this or a networked +% computer. Allows the creation of references to other objects/models in +% V-REP which can be manipulated in MATLAB. +% +% This class handles the interface to the simulator and low-level object +% handle operations. +% +% Methods throw exception if an error occurs. +% +% Methods:: +% +% gethandle get handle to named object +% getchildren get children belonging to handle +% getobjname get names of objects +%- +% object return a VREP_obj object for named object +% arm return a VREP_arm object for named robot +% camera return a VREP_camera object for named vosion sensor +% hokuyo return a VREP_hokuyo object for named Hokuyo scanner +%- +% getpos return position of object given handle +% setpos set position of object given handle +% getorient return orientation of object given handle +% setorient set orientation of object given handle +% getpose return pose of object given handle +% setpose set pose of object given handle +%- +% setobjparam_bool set object boolean parameter +% setobjparam_int set object integer parameter +% setobjparam_float set object float parameter +% getobjparam_bool get object boolean parameter +% getobjparam_int get object integer parameter +% getobjparam_float get object float parameter +%- +% signal_int send named integer signal +% signal_float send named float signal +% signal_str send named string signal +%- +% setparam_bool set simulator boolean parameter +% setparam_int set simulator integer parameter +% setparam_str set simulator string parameter +% setparam_float set simulator float parameter +% getparam_bool get simulator boolean parameter +% getparam_int get simulator integer parameter +% getparam_str get simulator string parameter +% getparam_float get simulator float parameter +%- +% delete shutdown the connection and cleanup +%- +% simstart start the simulator running +% simstop stop the simulator running +% simpause pause the simulator +% getversion get V-REP version number +% checkcomms return status of connection +% pausecomms pause the comms +%- +% loadscene load a scene file +% clearscene clear the current scene +% loadmodel load a model into current scene +%- +% display print the link parameters in human readable form +% char convert to string +% +% See also VREP_obj, VREP_arm, VREP_camera, VREP_hokuyo. + +% Copyright (C) 1993-2015, 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.petercorke.com + +classdef VREP < handle + + properties(GetAccess=public, SetAccess=protected) + vrep % the remApi object + client % the comms handle + path % path to V-REP root + mode % the communications mode + libpath % list of libraries added to path + version % the version as given by user 304, 311 etc. + end + + + methods + + function obj = VREP(varargin) + %VREP.VREP VREP object constructor + % + % v = VREP(OPTIONS) create a connection to an instance of the V-REP + % simulator. + % + % Options:: + % 'timeout',T Timeout T in ms (default 2000) + % 'cycle',C Cycle time C in ms (default 5) + % 'port',P Override communications port + % 'reconnect' Reconnect on error (default noreconnect) + % 'path',P The path to VREP install directory + % + % Notes:: + % - The default path is taken from the environment variable VREP + + opt.timeout = 2000; + opt.cycle = 5; + opt.port = 19997; + opt.reconnect = false; + opt.path = getenv('VREP'); + + [opt,args] = tb_optparse(opt, varargin); + + + % setup paths + if isempty(opt.path) + error('RTB:VREP:badargs', 'no VREP path specified'); + end + + if ~exist(opt.path, 'dir') + error('RTB:VREP:badargs', 'Root VREP folder %s does not exist', opt.path); + end + obj.path = opt.path; + + + % % for 3.0.4 + % libpath = { fullfile(path, 'programming', 'remoteApi') + % fullfile(path, 'programming', 'Matlab') + % fullfile(path, 'programming', 'remoteApiSharedLib') + % }; + % port = 19998; + + % for 3.1.x + libpath = { fullfile(opt.path, 'programming', 'remoteApi') + fullfile(opt.path, 'programming', 'remoteApiBindings', 'matlab', 'matlab') + fullfile(opt.path, 'programming', 'remoteApiBindings', 'lib', 'lib') + }; + + addpath( libpath{:} ); + + obj.libpath = libpath; + + obj.vrep = remApi('remoteApi','extApi.h'); + + % IP address and port + % wait until connected + % do not attempt to reconnect + % timeout is 2000ms + % comms cycle time is 5ms + + + % establish the connection + obj.client = obj.vrep.simxStart('127.0.0.1', opt.port, true, ... + ~opt.reconnect, opt.timeout, opt.cycle); + + if obj.client < 0 + error('RTB:VREP:noconnect', 'Can''t connect to V-REP simulator'); + end + + obj.mode = obj.vrep.simx_opmode_oneshot_wait; + + % get actual version number as a string + vint = obj.getversion(); + v = ''; + for i=1:3 + v = [num2str(rem(vint,100)) v]; + vint = floor(vint/100); + if i < 3 + v = ['.' v]; + end + end + obj.version = v; + end + + function display(v) + %VREP.display Display parameters + % + % V.display() displays the VREP parameters in compact format. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a VREP object and the command has no trailing + % semicolon. + % + % See also VREP.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(v) ); + end % display() + + function s = char(obj) + %VREP.char Convert to string + % + % V.char() is a string representation the VREP parameters in human + % readable foramt. + % + % See also VREP.display. + + s = sprintf('V-REP robotic simulator interface (active=%d)', obj.checkcomms() ); + + s = strvcat(s, sprintf('path: %s (ver %s)', obj.path, obj.version)); + switch obj.mode + case obj.vrep.simx_opmode_oneshot + s = strvcat(s, 'mode: simx_opmode_oneshot (non-blocking)'); + case obj.vrep.simx_opmode_oneshot_wait + s = strvcat(s, 'mode: simx_opmode_oneshot_wait (blocking)'); + case obj.vrep.simx_opmode_streaming + s = strvcat(s, 'mode: simx_opmode_streaming (non-blocking)'); + end + end + + function delete(obj) + %VREP.delete VREP object destructor + % + % delete(v) closes the connection to the V-REP simulator + + disp('--VREP destructor called'); + obj.vrep.simxFinish(obj.client); + obj.vrep.delete(); % explicitly call the destructor! + obj.vrep = []; + + rmpath( obj.libpath{:} ); + end + + %---- wrapper functions for getting object handles + function [h,s] = gethandle(obj, fmt, varargin) + %VREP.gethandle Return handle to VREP object + % + % H = V.gethandle(NAME) is an integer handle for named V-REP object. + % + % H = V.gethandle(FMT, ARGLIST) as above but the name is formed + % from sprintf(FMT, ARGLIST). + % + % See also sprintf. + [s,h] = obj.vrep.simxGetObjectHandle(obj.client, sprintf(fmt, varargin{:}), obj.mode); + end + + function children = getchildren(obj, h) + %VREP.getchildren Find children of object + % + % C = V.getchildren(H) is a vector of integer handles for the children of + % the V-REP object denoted by the integer handle H. + i = 0; + while true + [s,ch] = obj.vrep.simxGetObjectChild(obj.client, h, i, obj.mode); + if s < 0 + break + end + i = i+1; + children(i) = ch; + end + end + + function out = getobjname(obj, h) + %VREP.getname Find names of objects + % + % V.getobjname() will display the names and object handle (integers) for + % all objects in the current scene. + % + % NAME = V.getobjname(H) will return the name of the object with handle H. + + [s,handles,~,~,strd] = obj.vrep.simxGetObjectGroupData(obj.client, obj.vrep.sim_appobj_object_type, 0, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + + if nargin < 2 + name = strd(:,1); + else + name = strd{handles == h,1}; + end + + if (nargout == 0) && (nargin < 2) + for i=1:length(name) + fprintf('%3d: %s\n', handles(i), strd{i}); + return + end + end + out = name; + end + + % HOW DO I GET OBJECT TYPE FROM V-REP?? +% % Scene object types +% sim_object_shape_type =0; +% sim_object_joint_type =1; +% sim_object_graph_type =2; +% sim_object_camera_type =3; +% sim_object_dummy_type =4; +% sim_object_proximitysensor_type =5; +% sim_object_reserved1 =6; +% sim_object_reserved2 =7; +% sim_object_path_type =8; +% sim_object_visionsensor_type =9; +% sim_object_volume_type =10; +% sim_object_mill_type =11; +% sim_object_forcesensor_type =12; +% sim_object_light_type =13; +% sim_object_mirror_type =14; + + %---- simulation control + function simstart(obj) + %VREP.simstart Start V-REP simulation + % + % V.simstart() starts the V-REP simulation engine. + % + % See also VREP.simstop, VREP.simpause. + s = obj.vrep.simxStartSimulation(obj.client, obj.mode); + end + + function simstop(obj) + %VREP.simstop Stop V-REP simulation + % + % V.simstop() stops the V-REP simulation engine. + % + % See also VREP.simstart. + s = obj.vrep.simxStopSimulation(obj.client, obj.mode); + end + + function simpause(obj) + %VREP.pause Pause V-REP simulation + % + % V.simpause() pauses the V-REP simulation engine. Use + % V.simstart() to resume the simulation. + % + % See also VREP.simstart. + s = obj.vrep.simxPauseSimulation(obj.client, obj.mode); + end + + function v = getversion(obj) + %VREP.getversion Get version of the V-REP simulator + % + % V.getversion() is the version of the V-REP simulator + % server as an integer MNNNN where M is the major version + % number and NNNN is the minor version number. + [s,v] = obj.vrep.simxGetIntegerParameter(obj.client, obj.vrep.sim_intparam_program_version, obj.mode); + end + + function s = checkcomms(obj) + %VREP.checkcomms Check communications to V-REP simulator + % + % V.checkcomms() is true if a valid connection to the V-REP simulator exists. + s = obj.vrep.simxGetConnectionId(obj.client); + end + + function pausecomms(obj, onoff) + %VREP.pausecomms Pause communcations to the V-REP simulator + % + % V.pausecomms(P) pauses communications to the V-REP simulation engine if P is true + % else resumes it. Useful to ensure an atomic update of + % simulator state. + + s = obj.vrep.simxPauseCommunication(obj.client, onoff, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function loadscene(obj, name, varargin) + %VREP.loadscene Load a scene into the V-REP simulator + % + % V.loadscene(FILE, OPTIONS) loads the scene file FILE with extension .ttt + % into the simulator. + % + % Options:: + % 'local' The file is loaded relative to the MATLAB client's current + % folder, otherwise from the V-REP root folder. + % + % Example:: + % vrep.loadscene('2IndustrialRobots'); + % + % Notes:: + % - If a relative filename is given in non-local (server) mode it is + % relative to the V-REP scenes folder. + % + % See also VREP.clearscene. + + opt.local = false; + + opt = tb_optparse(opt, varargin); + + if ~opt.local + if name(1) ~= '/' + % its a relative path, add in full path + name = fullfile(obj.path, 'scenes', [name '.ttt']); + end + end + s = obj.vrep.simxLoadScene(obj.client, name, opt.local, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function clearscene(obj, name, varargin) + %VREP.clearscene Clear current scene in the V-REP simulator + % + % V.clearscene() clears the current scene and switches to another open + % scene, if none, a new (default) scene is created. + % + % See also VREP.loadscene. + + s = obj.vrep.simxCloseScene(obj.client, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function m = loadmodel(obj, model, varargin) + %VREP.loadmodel Load a model into the V-REP simulator + % + % M = V.loadmodel(FILE, OPTIONS) loads the model file FILE with extension .ttm + % into the simulator and returns a VREP_obj object that mirrors it in + % MATLAB. + % + % Options:: + % 'local' The file is loaded relative to the MATLAB client's current + % folder, otherwise from the V-REP root folder. + % + % Example:: + % vrep.loadmodel('people/Walking Bill'); + % + % Notes:: + % - If a relative filename is given in non-local (server) mode it is + % relative to the V-REP models folder. + % + % See also VREP.arm, VREP.camera, VREP.object. + + opt.local = false; + opt.name = []; + opt = tb_optparse(opt, varargin); + + if ~opt.local + if model(1) ~= '/' + % its a relative path, add in full path + model = fullfile(obj.path, 'models', [model '.ttm']); + end + end + [s,bh] = obj.vrep.simxLoadModel(obj.client, model, opt.local, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + m = obj.object(bh, varargin{:}); + pause(0.5); + end + + + %---- signals + + function signal_int(obj, name, val) + %VREP.signal_int Send an integer signal to the V-REP simulator + % + % V.signal_int(NAME, VAL) send an integer signal with name NAME + % and value VAL to the V-REP simulation engine. + s = obj.vrep.simxSetIntegerSignal(obj.client, name, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function signal_float(obj, name, val) + %VREP.signal_float Send a float signal to the V-REP simulator + % + % V.signal_float(NAME, VAL) send a float signal with name NAME + % and value VAL to the V-REP simulation engine. + s = obj.vrep.simxSetFloatSignal(obj.client, name, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function signal_str(obj, name, val) + %VREP.signal_str Send a string signal to the V-REP simulator + % + % V.signal_str(NAME, VAL) send a string signal with name NAME + % and value VAL to the V-REP simulation engine. + s = obj.vrep.simxSetStringSignal(obj.client, name, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + %---- set/get simulator parameters + function setparam_bool(obj, paramid, val) + %VREP.setparam_bool Set boolean parameter of the V-REP simulator + % + % V.setparam_bool(NAME, VAL) sets the boolean parameter with name NAME + % to value VAL within the V-REP simulation engine. + % + % See also VREP.getparam_bool. + if isstr(paramid) + paramid = eval(['obj.vrep.' paramid]); + end + s = obj.vrep.simxSetBooleanParameter(obj.client, paramid, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function setparam_int(obj, paramid, val) + %VREP.setparam_int Set integer parameter of the V-REP simulator + % + % V.setparam_int(NAME, VAL) sets the integer parameter with name NAME + % to value VAL within the V-REP simulation engine. + % + % See also VREP.getparam_int. + if isstr(paramid) + paramid = eval(['obj.vrep.' paramid]); + end + s = obj.vrep.simxSetIntegerParameter(obj.client, paramid, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function setparam_str(obj, paramid, val) + %VREP.setparam_str Set string parameter of the V-REP simulator + % + % V.setparam_str(NAME, VAL) sets the integer parameter with name NAME + % to value VAL within the V-REP simulation engine. + % + % See also VREP.getparam_str. + if isstr(paramid) + paramid = eval(['obj.vrep.' paramid]); + end + s = obj.vrep.simxSetStringParameter(obj.client, paramid, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function setparam_float(obj, paramid, val) + %VREP.setparam_float Set float parameter of the V-REP simulator + % + % V.setparam_float(NAME, VAL) sets the float parameter with name NAME + % to value VAL within the V-REP simulation engine. + % + % See also VREP.getparam_float. + if isstr(paramid) + paramid = eval(['obj.vrep.' paramid]); + end + s = obj.vrep.simxSetFloatingParameter(obj.client, paramid, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function v = getparam_bool(obj, paramid) + %VREP.getparam_bool Get boolean parameter of the V-REP simulator + % + % V.getparam_bool(NAME) is the boolean parameter with name NAME + % from the V-REP simulation engine. + % + % Example:: + % v = VREP(); + % v.getparam_bool('sim_boolparam_mirrors_enabled') + % + % See also VREP.setparam_bool. + if isstr(paramid) + paramid = eval(['obj.vrep.' paramid]); + end + [s,v] = obj.vrep.simxGetBooleanParameter(obj.client, paramid, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function v = getparam_int(obj, paramid) + %VREP.getparam_int Get integer parameter of the V-REP simulator + % + % V.getparam_int(NAME) is the integer parameter with name NAME + % from the V-REP simulation engine. + % + % Example:: + % v = VREP(); + % v.getparam_int('sim_intparam_settings') + % + % See also VREP.setparam_int. + + if isstr(paramid) + paramid = eval(['obj.vrep.' paramid]); + end + [s,v] = obj.vrep.simxGetIntegerParameter(obj.client, paramid, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function v = getparam_str(obj, paramid) + %VREP.getparam_str Get string parameter of the V-REP simulator + % + % V.getparam_str(NAME) is the string parameter with name NAME + % from the V-REP simulation engine. + % + % Example:: + % v = VREP(); + % v.getparam_str('sim_stringparam_application_path') + % + % See also VREP.setparam_str. + + if isstr(paramid) + paramid = eval(['obj.vrep.' paramid]); + end + [s,v] = obj.vrep.simxGetStringParameter(obj.client, paramid, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function v = getparam_float(obj, paramid) + %VREP.getparam_float Get float parameter of the V-REP simulator + % + % V.getparam_float(NAME) gets the float parameter with name NAME + % from the V-REP simulation engine. + % + % Example:: + % v = VREP(); + % v.getparam_float('sim_floatparam_simulation_time_step') + % + % See also VREP.setparam_float. + if isstr(paramid) + paramid = eval(['obj.vrep.' paramid]); + end + [s,v] = obj.vrep.simxGetFloatingParameter(obj.client, paramid, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + %---- set/get object parameters + function setobjparam_bool(obj, h, paramid, val) + %VREP.setobjparam_bool Set boolean parameter of a V-REP object + % + % V.setobjparam_bool(H, PARAM, VAL) sets the boolean parameter + % with identifier PARAM of object H to value VAL. + s = obj.vrep.simxSetObjectIntParameter(obj.client, h, paramid, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function setobjparam_int(obj, h, paramid, val) + %VREP.setobjparam_int Set Integer parameter of a V-REP object + % + % V.setobjparam_int(H, PARAM, VAL) sets the integer parameter + % with identifier PARAM of object H to value VAL. + s = obj.vrep.simxSetObjectIntParameter(obj.client, h, paramid, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function setobjparam_float(obj, h, paramid, val) + %VREP.setobjparam_float Set float parameter of a V-REP object + % + % V.setobjparam_float(H, PARAM, VAL) sets the float parameter + % with identifier PARAM of object H to value VAL. + s = obj.vrep.simxSetObjectFloatParameter(obj.client, h, paramid, val, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function val = getobjparam_bool(obj, h, paramid) + %VREP.getobjparam_bool Get boolean parameter of a V-REP object + % + % V.getobjparam_bool(H, PARAM) gets the boolean parameter + % with identifier PARAM of object with integer handle H. + [s,val] = obj.vrep.simxGetObjectIntParameter(obj.client, h, paramid, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function val = getobjparam_int(obj, h, paramid) + %VREP.getobjparam_int Get integer parameter of a V-REP object + % + % V.getobjparam_int(H, PARAM) gets the integer parameter + % with identifier PARAM of object with integer handle H. + [s,val] = obj.vrep.simxGetObjectIntParameter(obj.client, h, paramid, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function val = getobjparam_float(obj, h, paramid) + %VREP.getobjparam_float Get float parameter of a V-REP object + % + % V.getobjparam_float(H, PARAM) gets the float parameter + % with identifier PARAM of object with integer handle H. + [s,val] = obj.vrep.simxGetObjectFloatParameter(obj.client, h, paramid, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + %---- wrapper functions for using joint objects + function setjoint(obj, h, q) + %VREP.setjoint Set value of V-REP joint object + % + % V.setjoint(H, Q) sets the position of joint object with integer handle H + % to the value Q. + s = obj.vrep.simxSetJointPosition(obj.client, h, q, obj.mode); + end + + function q = getjoint(obj, h) + %VREP.getjoint Get value of V-REP joint object + % + % V.getjoint(H, Q) is the position of joint object with integer handle H. + [s,q] = obj.vrep.simxGetJointPosition(obj.client, h, obj.mode); + + if s ~= 0 + throw( obj.except(s) ); + end + end + + function setjointtarget(obj, h, q) + %VREP.setjointtarget Set target value of V-REP joint object + % + % V.setjointtarget(H, Q) sets the target position of joint object with integer handle H + % to the value Q. + s = obj.vrep.simxSetJointTargetPosition(obj.client, h, q, obj.mode); + + if s ~= 0 + throw( obj.except(s) ); + end + end + + function setjointvel(obj, h, qd) + %VREP.setjointvel Set velocity of V-REP joint object + % + % V.setjointvel(H, QD) sets the target velocity of joint object with integer handle H + % to the value QD. + s = obj.vrep.simxSetJointTargetVelocity(obj.client, h, qd, obj.mode); + + if s ~= 0 + throw( obj.except(s) ); + end + end + + + %---- wrapper functions for position of objects + function setpos(obj, h, t, relto) + %VREP.setpos Set position of V-REP object + % + % V.setpos(H, T) sets the position of V-REP object with integer + % handle H to T (1x3). + % + % V.setpos(H, T, HR) as above but position is set relative to the + % position of object with integer handle HR. + % + % See also VREP.getpos, VREP.setpose, VREP.setorient. + + if nargin < 4 + relto = -1; + end + + s = obj.vrep.simxSetObjectPosition(obj.client, h, relto, t, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function t = getpos(obj, h, relto) + %VREP.getpos Get position of V-REP object + % + % V.getpos(H) is the position (1x3) of the V-REP object with integer + % handle H. + % + % V.getpos(H, HR) as above but position is relative to the + % position of object with integer handle HR. + % + % See also VREP.setpose, VREP.getpose, VREP.getorient. + if nargin < 4 + relto = -1; + end + + [s,t] = obj.vrep.simxGetObjectPosition(obj.client, h, relto, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + %---- wrapper functions for pose of objects + + function setpose(obj, h, T, relto) + %VREP.setpose Set pose of V-REP object + % + % V.setpos(H, T) sets the pose of V-REP object with integer + % handle H according to homogeneous transform T (4x4). + % + % V.setpos(H, T, HR) as above but pose is set relative to the + % pose of object with integer handle HR. + % + % See also VREP.getpose, VREP.setpos, VREP.setorient. + + if nargin < 4 + relto = -1; + elseif isa(relto, 'VREP_base') + relto = relto.h; + end + + pos = transl(T); + eul = tr2eul(T, 'deg'); + s = obj.vrep.simxSetObjectPosition(obj.client, h, relto, pos, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + s = obj.vrep.simxSetObjectOrientation(obj.client, h, relto, eul, obj.mode); + if s < 0 + throw( obj.except(s) ); + end + end + + function T = getpose(obj, h, relto) + %VREP.getpose Get pose of V-REP object + % + % T = V.getpose(H) is the pose of the V-REP object with integer + % handle H as a homogeneous transformation matrix (4x4). + % + % T = V.getpose(H, HR) as above but pose is relative to the + % pose of object with integer handle R. + % + % See also VREP.setpose, VREP.getpos, VREP.getorient. + if nargin < 4 + relto = -1; + elseif isa(relto, 'VREP_base') + relto = relto.h; + end + + [s,pos] = obj.vrep.simxGetObjectPosition(obj.client, h, relto, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + [s,eul] = obj.vrep.simxGetObjectOrientation(obj.client, h, relto, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + + T = transl(pos) * eul2tr(eul, 'deg'); + end + + %---- wrapper functions for orientation of objects + + function setorient(obj, h, R, relto) + %VREP.setorient Set orientation of V-REP object + % + % V.setorient(H, R) sets the orientation of V-REP object with integer + % handle H to that given by rotation matrix R (3x3). + % + % V.setorient(H, T) sets the orientation of V-REP object with integer + % handle H to rotational component of homogeneous transformation matrix + % T (4x4). + % + % V.setorient(H, E) sets the orientation of V-REP object with integer + % handle H to ZYZ Euler angles (1x3). + % + % V.setorient(H, X, HR) as above but orientation is set relative to the + % orientation of object with integer handle HR. + % + % See also VREP.getorient, VREP.setpos, VREP.setpose. + + if nargin < 4 + relto = -1; + elseif isa(relto, 'VREP_base') + relto = relto.h; + end + + if isrot(R) || ishomog(R) + eul = tr2eul(R); + elseif isvec(R, 3) + eul = R; + end + + s = obj.vrep.simxSetObjectOrientation(obj.client, h, relto, eul, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + end + + function R = getorient(obj, h, relto, varargin) + %VREP.getorient Get orientation of V-REP object + % + % R = V.getorient(H) is the orientation of the V-REP object with integer + % handle H as a rotation matrix (3x3). + % + % EUL = V.getorient(H, 'euler', OPTIONS) as above but returns ZYZ Euler + % angles. + % + % V.getorient(H, HRR) as above but orientation is relative to the + % position of object with integer handle HR. + % + % V.getorient(H, HRR, 'euler', OPTIONS) as above but returns ZYZ Euler + % angles. + % + % Options:: + % See tr2eul. + % + % See also VREP.setorient, VREP.getpos, VREP.getpose. + + if nargin < 4 + relto = -1; + elseif isa(relto, 'VREP_base') + relto = relto.h; + end + + opt.euler = false; + [opt,args] = tb_optparse(opt, varargin); + + [s,eul] = obj.vrep.simxGetObjectOrientation(obj.client, h, relto, obj.mode); + if s ~= 0 + throw( obj.except(s) ); + end + + if opt.euler + R = eul; + else + R = eul2tr(eul, args{:}); + end + end + + %---- factory methods to create instances of specific objects that mirror VREP objects + function a = arm(obj, name) + %VREP.arm Return VREP_arm object + % + % V.arm(name) is a factory method that returns a VREP_arm object for the V-REP robot + % object named NAME. + % + % Example:: + % vrep.arm('IRB 140'); + % + % See also VREP_arm. + + if isa(name, 'VREP_obj') + h = name.h; + name = name.getname(); + end + a = VREP_arm(obj, name); + end + + function o = object(obj, varargin) + %VREP.arm Return VREP_obj object + % + % V.objet(name) is a factory method that returns a VREP_obj object for the V-REP + % object or model named NAME. + % Example:: + % vrep.obj('Walking Bill'); + % + % See also VREP_obj. + o = VREP_obj(obj, varargin{:}); + end + + function m = mobile(obj, name) + %VREP.mobile Return VREP_mobile object + % + % V.mobile(name) is a factory method that returns a + % VREP_mobile object for the V-REP mobile base object named NAME. + % + % See also VREP_mobile. + m = VREP_mobile(obj, name); + end + + function m = camera(obj, name) + %VREP.camera Return VREP_camera object + % + % V.camera(name) is a factory method that returns a VREP_camera object for the V-REP vision + % sensor object named NAME. + % + % See also VREP_camera. + m = VREP_camera(obj, name); + end + + function m = hokuyo(obj, name) + %VREP.hokuyo Return VREP_hokuyo object + % + % V.hokuyo(name) is a factory method that returns a VREP_hokuyo + % object for the V-REP Hokuyo laser scanner object named NAME. + % + % See also VREP_hokuyo. + m = VREP_mobile(obj, name); + end + + function m = youbot(obj, name) + %VREP.youbot Return VREP_youbot object + % + % V.youbot(name) is a factory method that returns a VREP_youbot + % object for the V-REP YouBot object named NAME. + % + % See also VREP_youbot. + m = VREP_youbot(obj, name); + end + end + methods(Access=private) + function e = except(obj, s) + switch (s) + case obj.vrep.simx_return_novalue_flag %1 + msgid = 'VREP:noreply'; + err = 'No command reply in the input buffer'; + case obj.vrep.simx_return_timeout_flag %2 + msgid = 'VREP:timedout'; + err = 'function timed out (probably the network is down or too slow)'; + case obj.vrep.simx_return_illegal_opmode_flag %4 + msgid = 'VREP:notsupported'; + err = 'The specified operation mode is not supported for the given function'; + case obj.vrep.simx_return_remote_error_flag %8 + msgid = 'VREP:serversideerrror'; + err = 'function caused an error on the server side (e.g. an invalid handle was specified)'; + case obj.vrep.simx_return_split_progress_flag %16 + msgid = 'VREP:busy'; + err = 'The communication thread is still processing previous split command of the same type'; + case obj.vrep.simx_return_local_error_flag %32 + msgid = 'VREP:clientsideerror'; + err = 'The function caused an error on the client side'; + case obj.vrep.simx_return_initialize_error_flag %64 + msgid = 'VREP:nostarted'; + err = 'simxStart was not yet called' + end + + e = MException(msgid, err); + end +end +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_arm.m b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_arm.m new file mode 100644 index 0000000..2313e52 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_arm.m @@ -0,0 +1,368 @@ +%VREP_arm Mirror of V-REP robot arm object +% +% Mirror objects are MATLAB objects that reflect the state of objects in +% the V-REP environment. Methods allow the V-REP state to be examined or +% changed. +% +% This is a concrete class, derived from VREP_mirror, for all V-REP robot +% arm objects and allows access to joint variables. +% +% Methods throw exception if an error occurs. +% +% Example:: +% vrep = VREP(); +% arm = vrep.arm('IRB140'); +% q = arm.getq(); +% arm.setq(zeros(1,6)); +% arm.setpose(T); % set pose of base +% +% Methods:: +% +% getq get joint coordinates +% setq set joint coordinates +% setjointmode set joint control parameters +% animate animate a joint coordinate trajectory +% teach graphical teach pendant +% +% Superclass methods (VREP_obj):: +% getpos get position of object +% setpos set position of object +% getorient get orientation of object +% setorient set orientation of object +% getpose get pose of object given +% setpose set pose of object +% +% can be used to set/get the pose of the robot base. +% +% Superclass methods (VREP_mirror):: +% getname get object name +%- +% setparam_bool set object boolean parameter +% setparam_int set object integer parameter +% setparam_float set object float parameter +% +% getparam_bool get object boolean parameter +% getparam_int get object integer parameter +% getparam_float get object float parameter +% +% Properties:: +% n Number of joints +% +% See also VREP_mirror, VREP_obj, VREP_arm, VREP_camera, VREP_hokuyo. + +% Copyright (C) 1993-2015, 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.petercorke.com + +classdef VREP_arm < VREP_obj + + properties(GetAccess=public, SetAccess=protected) + q + joint % VREP joint object handles + n % number of joints + + end + + + methods + + function arm = VREP_arm(vrep, name, varargin) + %VREP_arm.VREP_arm Create a robot arm mirror object + % + % ARM = VREP_arm(NAME, OPTIONS) is a mirror object that corresponds to the + % robot arm named NAME in the V-REP environment. + % + % Options:: + % 'fmt',F Specify format for joint object names (default '%s_joint%d') + % + % Notes:: + % - The number of joints is found by searching for objects + % with names systematically derived from the root object name, by + % default named NAME_N where N is the joint number starting at 0. + % + % See also VREP.arm. + + + h = vrep.gethandle(name); + if h == 0 + error('no such object as %s in the scene', name); + end + arm = arm@VREP_obj(vrep, name); + + opt.fmt = '%s_joint%d'; + opt = tb_optparse(opt, varargin); + + arm.name = name; + + % find all the _joint objects, we don't know how many joints so we + % keep going till we get an error + j = 1; + while true + [h,s] = vrep.gethandle(opt.fmt, name, j); + if s ~= 0 + break + end + arm.joint(j) = h; + + j = j+1; + end + + arm.n = j - 1; + + % set all joints to passive mode + % for j=1:arm.n + % arm.vrep.simxSetJointMode(arm.client, arm.joint(j), arm.vrep.sim_jointmode_passive, arm.vrep.simx_opmode_oneshot_wait); + % end + end + + function q = getq(arm) + %VREP_arm.getq Get joint angles of V-REP robot + % + % ARM.getq() is the vector of joint angles (1xN) from the corresponding + % robot arm in the V-REP simulation. + % + % See also VREP_arm.setq. + for j=1:arm.n + q(j) = arm.vrep.getjoint(arm.joint(j)); + end + end + + function setq(arm, q) + %VREP_arm.setq Set joint angles of V-REP robot + % + % ARM.setq(Q) sets the joint angles of the corresponding + % robot arm in the V-REP simulation to Q (1xN). + % + % See also VREP_arm.getq. + for j=1:arm.n + arm.vrep.setjoint(arm.joint(j), q(j)); + end + end + + function setqt(arm, q) + %VREP_arm.setq Set joint angles of V-REP robot + % + % ARM.setq(Q) sets the joint angles of the corresponding + % robot arm in the V-REP simulation to Q (1xN). + for j=1:arm.n + arm.vrep.setjointtarget(arm.joint(j), q(j)); + end + end + + function setjointmode(arm, motor, control) + %VREP_arm.setjointmode Set joint mode + % + % ARM.setjointmode(M, C) sets the motor enable M (0 or 1) and motor control + % C (0 or 1) parameters for all joints of this robot arm. + + for j=1:arm.n + arm.vrep.setobjparam_int(arm.joint(j), 2000, 1); %motor enable + arm.vrep.setobjparam_int(arm.joint(j), 2001, 1); %motor control enable + + end + + end + function animate(arm, qt, varargin) + %VREP_arm.setq Animate V-REP robot + % + % R.animate(QT, OPTIONS) animates the corresponding V-REP robot with + % configurations taken from consecutive rows of QT (MxN) which represents + % an M-point trajectory and N is the number of robot joints. + % + % Options:: + % 'delay',D Delay (s) betwen frames for animation (default 0.1) + % 'fps',fps Number of frames per second for display, inverse of 'delay' option + % '[no]loop' Loop over the trajectory forever + % + % See also SerialLink.plot. + + opt.delay = 0.1; + opt.fps = []; + opt.loop = false; + opt = tb_optparse(opt, varargin); + + if ~isempty(opt.fps) + opt.delay = 1/opt.fps; + end + + while true + for i=1:numrows(qt) + arm.setq(qt(i,:)); + pause(opt.delay); + end + if ~opt.loop + break; + end + end + end + + + function teach(obj, varargin) + %VREP_arm.teach Graphical teach pendant + % + % R.teach(OPTIONS) drive a V-REP robot by means of a graphical slider panel. + % + % Options:: + % 'degrees' Display angles in degrees (default radians) + % 'q0',q Set initial joint coordinates + % + % + % Notes:: + % - The slider limits are all assumed to be [-pi, +pi] + % + % See also SerialLink.plot. + + n = obj.n; + + %------------------------------- + % parameters for teach panel + bgcol = [135 206 250]/255; % background color + height = 1/(n+2); % height of slider rows + %------------------------------- + opt.degrees = false; + opt.q0 = []; + % TODO: options for rpy, or eul angle display + + opt = tb_optparse(opt, varargin); + + % drivebot(r, q) + % drivebot(r, 'deg') + + + if isempty(opt.q0) + q = obj.getq(); + else + q = opt.q0; + end + + % set up scale factor, from actual limits in radians/metres to display units + qscale = ones(n,1); + for j=1:n + if opt.degrees && L.isrevolute + qscale(j) = 180/pi; + end + qlim(j,:) = [-pi pi]; + end + + handles.qscale = qscale; + + panel = figure(... + 'BusyAction', 'cancel', ... + 'HandleVisibility', 'off', ... + 'Color', bgcol); + pos = get(panel, 'Position'); + pos(3:4) = [300 400]; + set(panel, 'Position', pos); + set(panel,'MenuBar','none') + set(panel, 'name', 'Teach'); + delete( get(panel, 'Children') ) + + + %---- now make the sliders + for j=1:n + % slider label + uicontrol(panel, 'Style', 'text', ... + 'Units', 'normalized', ... + 'BackgroundColor', bgcol, ... + 'Position', [0 height*(n-j) 0.15 height], ... + 'FontUnits', 'normalized', ... + 'FontSize', 0.5, ... + 'String', sprintf('q%d', j)); + + % slider itself + q(j) = max( qlim(j,1), min( qlim(j,2), q(j) ) ); % clip to range + handles.slider(j) = uicontrol(panel, 'Style', 'slider', ... + 'Units', 'normalized', ... + 'Position', [0.15 height*(n-j) 0.65 height], ... + 'Min', qlim(j,1), ... + 'Max', qlim(j,2), ... + 'Value', q(j), ... + 'Tag', sprintf('Slider%d', j)); + + % text box showing slider value, also editable + handles.edit(j) = uicontrol(panel, 'Style', 'edit', ... + 'Units', 'normalized', ... + 'Position', [0.80 height*(n-j)+.01 0.20 height*0.9], ... + 'BackgroundColor', bgcol, ... + 'String', num2str(qscale(j)*q(j), 3), ... + 'HorizontalAlignment', 'left', ... + 'FontUnits', 'normalized', ... + 'FontSize', 0.4, ... + 'Tag', sprintf('Edit%d', j)); + end + + %---- robot name text box + uicontrol(panel, 'Style', 'text', ... + 'Units', 'normalized', ... + 'FontUnits', 'normalized', ... + 'FontSize', 1, ... + 'HorizontalAlignment', 'center', ... + 'Position', [0.05 1-height*1.5 0.9 height], ... + 'BackgroundColor', 'white', ... + 'String', obj.name); + + handles.arm = obj; + + % now assign the callbacks + for j=1:n + % text edit box + set(handles.edit(j), ... + 'Interruptible', 'off', ... + 'Callback', @(src,event)teach_callback(j, handles, src)); + + % slider + set(handles.slider(j), ... + 'Interruptible', 'off', ... + 'BusyAction', 'queue', ... + 'Callback', @(src,event)teach_callback(j, handles, src)); + end + end + end +end + +function teach_callback(j, handles, src) + + % called on changes to a slider or to the edit box showing joint coordinate + % + % src the object that caused the event + % name name of the robot + % j the joint index concerned (1..N) + % slider true if the + + qscale = handles.qscale; + + switch get(src, 'Style') + case 'slider' + % slider changed, get value and reflect it to edit box + newval = get(src, 'Value'); + set(handles.edit(j), 'String', num2str(qscale(j)*newval)); + case 'edit' + % edit box changed, get value and reflect it to slider + newval = str2double(get(src, 'String')) / qscale(j); + set(handles.slider(j), 'Value', newval); + end + %fprintf('newval %d %f\n', j, newval); + + handles.arm.vrep.setjoint(handles.arm.joint(j), newval); + +end + + + + + diff --git a/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_camera.m b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_camera.m new file mode 100644 index 0000000..4167e54 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_camera.m @@ -0,0 +1,226 @@ +%VREP_camera Mirror of V-REP vision sensor object +% +% Mirror objects are MATLAB objects that reflect the state of objects in +% the V-REP environment. Methods allow the V-REP state to be examined or +% changed. +% +% This is a concrete class, derived from VREP_mirror, for all V-REP vision +% sensor objects and allows access to images and image parameters. +% +% Methods throw exception if an error occurs. +% +% Example:: +% vrep = VREP(); +% camera = vrep.camera('Vision_sensor'); +% im = camera.grab(); +% camera.setpose(T); +% R = camera.getorient(); +% +% Methods:: +% +% grab return an image from simulated camera +% setangle set field of view +% setresolution set image resolution +% setclipping set clipping boundaries +% +% Superclass methods (VREP_obj):: +% getpos get position of object +% setpos set position of object +% getorient get orientation of object +% setorient set orientation of object +% getpose get pose of object +% setpose set pose of object +% +% can be used to set/get the pose of the robot base. +% +% Superclass methods (VREP_mirror):: +% getname get object name +%- +% setparam_bool set object boolean parameter +% setparam_int set object integer parameter +% setparam_float set object float parameter +% +% getparam_bool get object boolean parameter +% getparam_int get object integer parameter +% getparam_float get object float parameter +% +% See also VREP_mirror, VREP_obj, VREP_arm, VREP_camera, VREP_hokuyo. + +% Copyright (C) 1993-2015, 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.petercorke.com + +classdef VREP_camera < VREP_obj + + properties + end + + methods + function obj = VREP_camera(vrep, name) + %VREP_camera.VREP_camera Create a camera mirror object + % + % C = VREP_camera(NAME, OPTIONS) is a mirror object that corresponds to the + % vision senor named NAME in the V-REP environment. + % + % Options:: + % 'fov',A Specify field of view in degreees (default 60) + % 'resolution',N Specify resolution. If scalar NxN else N(1)xN(2) + % 'clipping',Z Specify near Z(1) and far Z(2) clipping boundaries + % + % Notes:: + % - Default parameters are set in the V-REP environmen + % - Can be applied to "DefaultCamera" which controls the view in the + % simulator GUI. + % + % See also VREP_obj. + obj = obj@VREP_obj(vrep, name); + end + + function im = setclipping(obj, near, far) + %VREP_camera.setclipping Set clipping boundaries for V-REP vision sensor + % + % C.setclipping(NEAR, FAR) set clipping boundaries to the + % range of Z from NEAR to FAR. Objects outside this range + % will not be rendered. + % + % See also VREP_camera.getclipping. + + obj.setparam_float(1000, near); + obj.setparam_float(1001, far); + end + + function clip = getclipping(obj) + %VREP_camera.getclipping Get clipping boundaries for V-REP vision sensor + % + % C.getclipping() is the near and far clipping boundaries (1x2) in the + % Z-direction as a 2-vector [NEAR,FAR]. + % + % See also VREP_camera.setclipping. + + clip(1) = obj.getparam_float(1000); + clip(2) = obj.getparam_float(1001); + end + + function im = setangle(obj, angle) + %VREP_camera.setangle Set field of view for V-REP vision sensor + % + % C.setangle(FOV) set the field-of-view angle to FOV in + % radians. + % + % See also VREP_camera.getangle. + + obj.setparam_float(1004, angle); + end + + function fov = getangle(obj) + %VREP_camera.fetangle Fet field of view for V-REP vision sensor + % + % FOV = C.getangle(FOV) is the field-of-view angle to FOV in + % radians. + % + % See also VREP_camera.setangle. + + fov = obj.getparam_float(1004); + end + + function setresolution(obj, res) + %VREP_camera.setresolution Set resolution for V-REP vision sensor + % + % C.setresolution(R) set image resolution to RxR if R is a scalar or + % R(1)xR(2) if it is a 2-vector. + % + % Notes:: + % - By default V-REP cameras seem to have very low (32x32) resolution. + % - Frame rate will decrease as frame size increases. + % + % See also VREP_camera.getresolution. + if isscalar(res) + rx = res; ry = res; + else + rx = res(1); ry = res(2); + end + obj.setparam_int(1002, rx); + obj.setparam_int(1003, ry); + end + + function res = getresolution(obj) + %VREP_camera.getresolution Get resolution for V-REP vision sensor + % + % R = C.getresolution() is the image resolution (1x2) of the + % vision sensor R(1)xR(2). + % + % See also VREP_camera.setresolution. + + res(1) = obj.getparam_int(1002); + res(2) = obj.getparam_int(1003); + end + + function im = grab(obj, varargin) + %VREP_camera.grab Get image from V-REP vision sensor + % + % IM = C.grab(OPTIONS) is an image (WxH) returned from the V-REP + % vision sensor. + % + % C.grab(OPTIONS) as above but the image is displayed using + % idisp. + % + % Options:: + % 'grey' Return a greyscale image (default color). + % + % Notes:: + % - V-REP simulator must be running. + % - Color images can be quite dark, ensure good light sources. + % - Uses the signal 'handle_rgb_sensor' to trigger a single + % image generation. + % + % See also idisp, VREP.simstart. + + opt.grey = false; + opt = tb_optparse(opt, varargin); + + % Ask the sensor to turn itself on, take A SINGLE 3D IMAGE, and turn itself off again + obj.vrep.signal_int('handle_rgb_sensor', 1); + %fprintf('Capturing image...\n'); + [s,res,image] = obj.vrep.vrep.simxGetVisionSensorImage2(obj.vrep.client, obj.h, opt.grey, obj.vrep.vrep.simx_opmode_oneshot_wait); + if s ~= 0 + throw( obj.except(s) ); + end + %fprintf('Captured %dx%d image\n', res(1), res(2)); + + if nargout == 0 + idisp(image); + elseif nargout > 0 + im = image; + end + end + + function s = char(obj) + %VREP_camera.char Convert to string + % + % V.char() is a string representation the VREP parameters in human + % readable foramt. + % + % See also VREP.display. + + s = sprintf('VREP camera object: %s', obj.name); + s = strvcat(s, sprintf('resolution: %d x %d', obj.getresolution())); + s = strvcat(s, sprintf('clipping: %g to %g', obj.getclipping())); + + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_hokuyo.m b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_hokuyo.m new file mode 100644 index 0000000..58cf50e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_hokuyo.m @@ -0,0 +1,33 @@ + +% Copyright (C) 1993-2015, 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.petercorke.com +classdef VREP_rangefinder < VREP_obj + + properties + end + + methods + function obj = VREP_rangefinder(vrep, name) + obj = obj@VREP_base(vrep, name); + end + + function im = getimpage(obj) + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_mirror.m b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_mirror.m new file mode 100644 index 0000000..a2adbb6 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_mirror.m @@ -0,0 +1,196 @@ +%VREP_mirror V-REP mirror object class +% +% Mirror objects are MATLAB objects that reflect the state of objects in +% the V-REP environment. Methods allow the V-REP state to be examined or +% changed. +% +% This abstract class is the root class for all V-REP mirror objects. +% +% Methods throw exception if an error occurs. +% +% Methods:: +% getname get object name +%- +% setparam_bool set object boolean parameter +% setparam_int set object integer parameter +% setparam_float set object float parameter +% getparam_bool get object boolean parameter +% getparam_int get object integer parameter +% getparam_float get object float parameter +%- +% remove remove object from scene +% display display object info +% char convert to string +% +% Properties (read only):: +% h V-REP integer handle for the object +% name Name of the object in V-REP +% vrep Reference to the V-REP connection object +% +% Notes:: +% - This has nothing to do with mirror objects in V-REP itself which are +% shiny reflective surfaces. +% +% See also VREP_obj, VREP_arm, VREP_camera, VREP_hokuyo. + +% Copyright (C) 1993-2015, 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.petercorke.com + +classdef (Abstract=true) VREP_mirror < handle + + properties(GetAccess=public, SetAccess=protected) + vrep + h + name + end + + + methods + + function obj = VREP_mirror(vrep, varargin) + %VREP_mirror.VREP_mirror Construct VREP_mirror object + % + % OBJ = VREP_mirror(NAME) is a V-REP mirror object that represents the + % object named NAME in the V-REP simulator. + obj.vrep = vrep; + + if nargin > 1 + if isstr(varargin{1}) + % passed a string name, convert to handle + obj.h = vrep.gethandle(varargin{1}); + obj.name = varargin{1}; + else + obj.h = varargin{1}; + if nargin > 2 + obj.name = varargin{2}; + else + obj.name = []; + end + end + end + end + + function name = getname(obj) + %VREP_mirror.getname Get object name + % + % OBJ.getname() is the name of the object in the VREP simulator. + name = obj.vrep.getobjname(obj.h); + end + + function setparam_bool(obj, paramid, val) + %VREP_mirror.setparam_bool Set boolean parameter of V-REP object + % + % OBJ.setparam_bool(ID, VAL) sets the boolean parameter with ID + % to value VAL within the V-REP simulation engine. + % + % See also VREP_mirror.getparam_bool, VREP_mirror.setparam_int, + % VREP_mirror.setparam_float. + obj.vrep.setobjparam_bool(obj.h, paramid, val); + end + function setparam_int(obj, paramid, val) + %VREP_mirror.setparam_bool Set integer parameter of V-REP object + % + % OBJ.setparam_int(ID, VAL) sets the integer parameter with ID + % to value VAL within the V-REP simulation engine. + % + % See also VREP_mirror.getparam_int, VREP_mirror.setparam_bool, + % VREP_mirror.setparam_float. + obj.vrep.setobjparam_int(obj.h, paramid, val); + end + function setparam_float(obj, paramid, val) + %VREP_mirror.setparam_float Set float parameter of V-REP object + % + % OBJ.setparam_float(ID, VAL) sets the float parameter with ID + % to value VAL within the V-REP simulation engine. + % + % See also VREP_mirror.getparam_float, VREP_mirror.setparam_bool, + % VREP_mirror.setparam_int. + obj.vrep.setobjparam_float(obj.h, paramid, val); + end + + function val = getparam_bool(obj, paramid) + %VREP_mirror.getparam_bool Get boolean parameter of V-REP object + % + % OBJ.getparam_bool(ID) is the boolean parameter with ID + % of the corresponding V-REP object. + % + % See also VREP_mirror.setparam_bool, VREP_mirror.getparam_int, + % VREP_mirror.getparam_float. + val = obj.vrep.getobjparam_bool(obj, obj.h, paramid, val); + end + function val = getparam_int(obj, paramid) + %VREP_mirror.getparam_int Get integer parameter of V-REP object + % + % OBJ.getparam_int(ID) is the integer parameter with ID + % of the corresponding V-REP object. + % + % See also VREP_mirror.setparam_int, VREP_mirror.getparam_bool, + % VREP_mirror.getparam_float. + val = obj.vrep.getobjparam_int(obj.h, paramid); + end + function val = getparam_float(obj, paramid) + %VREP_mirror.getparam_float Get float parameter of V-REP object + % + % OBJ.getparam_float(ID) is the float parameter with ID + % of the corresponding V-REP object. + % + % See also VREP_mirror.setparam_bool, VREP_mirror.getparam_bool, + % VREP_mirror.getparam_int. + val = obj.vrep.getobjparam_float(obj.h, paramid); + end + + function remove(obj) + s = obj.vrep.vrep.simxRemoveModel(obj.vrep.client, obj.h, obj.vrep.mode); + if s ~= 0 + throw( obj.vrep.except(s) ); + end + end + + + function display(l) + %VREP_mirror.display Display parameters + % + % OBJ.display() displays the VREP parameters in compact format. + % + % Notes:: + % - This method is invoked implicitly at the command line when the result + % of an expression is a VREP object and the command has no trailing + % semicolon. + % + % See also VREP.char. + loose = strcmp( get(0, 'FormatSpacing'), 'loose'); + if loose + disp(' '); + end + disp([inputname(1), ' = ']) + disp( char(l) ); + end % display() + + function s = char(obj) + %VREP_mirror.char Convert to string + % + % OBJ.char() is a string representation the VREP parameters in human + % readable foramt. + % + % See also VREP.display. + + s = sprintf('VREP mirror object: %s', obj.name); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_mobile.m b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_mobile.m new file mode 100644 index 0000000..ab89b68 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_mobile.m @@ -0,0 +1,65 @@ + +% Copyright (C) 1993-2015, 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.petercorke.com +classdef VREP_mobile < handle + + properties + vrep + left + right % VREP joint object handles + end + + methods + + function mob = VREP_mobile(vrep, name) + + mob.vrep = vrep; + + + % find all the _joint objects, we don't know how many joints so we + % keep going till we get an error + + mob.left = vrep.gethandle('%s_leftJoint', name); + mob.right = vrep.gethandle('%s_rightJoint', name); + + % set all joints to passive mode + % for j=1:arm.njoints + % arm.vrep.simxSetJointMode(arm.client, arm.joint(j), arm.vrep.sim_jointmode_passive, arm.vrep.simx_opmode_oneshot_wait); + % end + end + + function q = getq(mob) + q = [ + mob.vrep.getjoint(mob.left) + mob.vrep.getjoint(mob.right) + ]; + end + + function setq(mob, q) + mob.vrep.setjoint(mob.left, q(1)); + mob.vrep.setjoint(mob.right, q(2)); + end + + function setqd(mob, qd) + mob.vrep.setjointvel(mob.left, qd(1)); + mob.vrep.setjointvel(mob.right, qd(2)); + end + end +end + diff --git a/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_obj.m b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_obj.m new file mode 100644 index 0000000..2059d7e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_obj.m @@ -0,0 +1,195 @@ +%VREP_obj V-REP mirror of simple object +% +% Mirror objects are MATLAB objects that reflect objects in the V-REP +% environment. Methods allow the V-REP state to be examined or changed. +% +% This is a concrete class, derived from VREP_mirror, for all V-REP objects +% and allows access to pose and object parameters. +% +% Example:: +% vrep = VREP(); +% bill = vrep.object('Bill'); % get the human figure Bill +% bill.setpos([1,2,0]); +% bill.setorient([0 pi/2 0]); +% +% Methods throw exception if an error occurs. +% +% Methods:: +% +% getpos get position of object +% setpos set position of object +% getorient get orientation of object +% setorient set orientation of object +% getpose get pose of object +% setpose set pose of object +% +% Superclass methods (VREP_mirror):: +% getname get object name +%- +% setparam_bool set object boolean parameter +% setparam_int set object integer parameter +% setparam_float set object float parameter +%- +% getparam_bool get object boolean parameter +% getparam_int get object integer parameter +% getparam_float get object float parameter +%- +% display print the link parameters in human readable form +% char convert to string +% +% See also VREP_mirror, VREP_obj, VREP_arm, VREP_camera, VREP_hokuyo. + +% Copyright (C) 1993-2015, 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.petercorke.com + +classdef VREP_obj < VREP_mirror + + properties + end + + methods + + function obj = VREP_obj(vrep, varargin) + %VREP_obj.VREP_obj VREP_obj mirror object constructor + % + % v = VREP_base(NAME) creates a V-REP mirror object for a + % simple V-REP object type. + obj = obj@VREP_mirror(vrep, varargin{:}); + end + + function p = getpos(obj, relto) + %VREP_obj.getpos Get position of V-REP object + % + % V.getpos() is the position (1x3) of the corresponding V-REP object. + % + % V.getpos(BASE) as above but position is relative to the VREP_obj + % object BASE. + % + % See also VREP_obj.setpos, VREP_obj.getorient, VREP_obj.getpose. + if nargin == 1 + p = obj.vrep.getpos(obj.h); + elseif nargin == 2 && isa(relto, 'VREP_base') + p = obj.vrep.getpos(obj.h, relto.h); + end + end + + function setpos(obj, p, relto) + %VREP_obj.setpos Set position of V-REP object + % + % V.setpos(T) sets the position of the corresponding V-REP object + % to T (1x3). + % + % V.setpos(T, BASE) as above but position is set relative to the + % position of the VREP_obj object BASE. + % + % See also VREP_obj.getpos, VREP_obj.setorient, VREP_obj.setpose. + if nargin == 2 + obj.vrep.setpos(obj.h, p); + elseif nargin == 3 && isa(relto, 'VREP_mirror') + obj.vrep.setpos(obj.h, p, relto.h); + end + end + + function p = getorient(obj, varargin) + %VREP_obj.getorient Get orientation of V-REP object + % + % V.getorient() is the orientation of the corresponding V-REP + % object as a rotation matrix (3x3). + % + % V.getorient('euler', OPTIONS) as above but returns ZYZ Euler + % angles. + % + % V.getorient(BASE) is the orientation of the corresponding V-REP + % object relative to the VREP_obj object BASE. + % + % V.getorient(BASE, 'euler', OPTIONS) as above but returns ZYZ Euler + % angles. + % + % Options:: + % See tr2eul. + % + % See also VREP_obj.setorient, VREP_obj.getopos, VREP_obj.getpose. + if nargin == 1 + p = obj.vrep.getorient(obj.h); + else + if isa(varargin{1}, 'VREP_base') + p = obj.vrep.getorient(obj.h, relto.h, varargin{2:end}); + else + p = obj.vrep.getorient(obj.h, relto.h, varargin{:}); + end + end + end + + function setorient(obj, p, relto) + %VREP_obj.setorient Set orientation of V-REP object + % + % V.setorient(R) sets the orientation of the corresponding V-REP to rotation matrix R (3x3). + % + % V.setorient(T) sets the orientation of the corresponding V-REP object to rotational component of homogeneous transformation matrix + % T (4x4). + % + % V.setorient(E) sets the orientation of the corresponding V-REP object to ZYZ Euler angles (1x3). + % + % V.setorient(X, BASE) as above but orientation is set relative to the + % orientation of VREP_obj object BASE. + % + % See also VREP_obj.getorient, VREP_obj.setpos, VREP_obj.setpose. + if nargin == 2 + obj.vrep.setorient(obj.h, p); + + elseif nargin == 3 && isa(relto, 'VREP_base') + obj.vrep.setorient(obj.h, p, relto.h); + end + end + + function p = getpose(obj, relto) + %VREP_obj.getpose Get pose of V-REP object + % + % V.getpose() is the pose (4x4) of the the corresponding V-REP object. + % + % V.getpose(BASE) as above but pose is relative to the + % pose the VREP_obj object BASE. + % + % See also VREP_obj.setpose, VREP_obj.getorient, VREP_obj.getpos. + if nargin == 1 + p = obj.vrep.getpose(obj.h); + elseif nargin == 2 && isa(relto, 'VREP_base') + p = obj.vrep.getpose(obj.h, relto.h); + end + end + + function setpose(obj, p, relto) + %VREP_obj.setpose Set pose of V-REP object + % + % V.setpose(T) sets the pose of the corresponding V-REP object + % to T (4x4). + % + % V.setpose(T, BASE) as above but pose is set relative to the + % pose of the VREP_obj object BASE. + % + % See also VREP_obj.getpose, VREP_obj.setorient, VREP_obj.setpos. + if nargin == 2 + obj.vrep.setpose(obj.h, p); + elseif nargin == 3 && isa(relto, 'VREP_base') + obj.vrep.setpose(obj.h, p, relto.h); + end + end + end +end + diff --git a/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_rangefinder.m b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_rangefinder.m new file mode 100644 index 0000000..bd25a42 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/VREP/VREP_rangefinder.m @@ -0,0 +1,20 @@ + +% Copyright (C) 1993-2015, 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.petercorke.com + diff --git a/lwrserv/matlab/rvctools/robot/interfaces/client.m b/lwrserv/matlab/rvctools/robot/interfaces/client.m new file mode 100644 index 0000000..d9ab08a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/client.m @@ -0,0 +1,66 @@ +% CLIENT connect to a server and read a message +% +% Usage - message = client(host, port, number_of_retries) +function message = client(host, port, number_of_retries) + + import java.net.Socket + import java.io.* + + + retry = 0; + input_socket = []; + message = []; + + while true + + retry = retry + 1; + if ((number_of_retries > 0) && (retry > number_of_retries)) + fprintf(1, 'Too many retries\n'); + break; + end + + try + fprintf(1, 'Retry %d connecting to %s:%d\n', ... + retry, host, port); + + % throws if unable to connect + input_socket = Socket(host, port); + + % get a buffered data input stream from the socket + input_stream = input_socket.getInputStream; + d_input_stream = DataInputStream(input_stream); + + fprintf(1, 'Connected to server\n'); + + % read data from the socket - wait a short time first + for i=1:20 + pause(0.5); + bytes_available = input_stream.available; + %fprintf(1, 'Reading %d bytes\n', bytes_available); + data_reader = DataReader(d_input_stream); + message = data_reader.readBuffer(bytes_available); + + message = char(message'); % Data comes out as a column vector + message + end + +% data_reader = DataReader(d_input_stream); +% message = data_reader.readBuffer(bytes_available); +% +% message = char(message'); % Data comes out as a column vector + + % cleanup + input_socket.close; + break; + + catch me + me + if ~isempty(input_socket) + input_socket.close; + end + + % pause before retrying + pause(1); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/ArmControl.cpp b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/ArmControl.cpp new file mode 100644 index 0000000..07e885c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/ArmControl.cpp @@ -0,0 +1,288 @@ +/* + * VisualServoing is a tutorial program for introducing students to + * robotics. + * + * Copyright 2009, 2010 Kevin Quigley and + * Marsette Vona + * + * VisualServoing is free software: you can redistribute it andor modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * VisualServoing 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * as the file COPYING along with VisualServoing. If not, see + * . + */ +#include "ArmControl.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +/*! + * Initialize control using the SSC32Controller. Setup the saved park + * command and the arm to park. + * + * \param s The SSC32Controller. + * \param slave If true, the WRIST joint depends on the values of + * SHOULDER and ELBOW to remain horizontal. + */ +ArmControl::ArmControl(SSC32Controller& s, bool slave) + : control(&s), + slave_theta3(slave), + position(NUM_JOINTS), offset(NUM_JOINTS), + max_angle(NUM_JOINTS), min_angle(NUM_JOINTS), + set_busy(false), max_rate_set(false), + park_position(NUM_JOINTS) +{ + + for (unsigned int i = 0; i < position.size(); i++) position(i) = 0.0; + for (unsigned int i = 0; i < offset.size(); i++) offset(i) = 0; + + park_position(0) = pwmToTheta(1500); + park_position(1) = pwmToTheta(2100); + park_position(2) = pwmToTheta(2000); + park_position(3) = pwmToTheta(2400); + park_position(4) = pwmToTheta(1800); + std::cout << "park position: " << park_position << std::endl; + + park_cmd.addMove(Arm::BASE, 1500); // values on p31 + park_cmd.addMove(Arm::SHOULDER, 2100); + park_cmd.addMove(Arm::ELBOW, 2000); + park_cmd.addMove(Arm::WRIST, 2400); + park_cmd.addMove(Arm::GRIP, 1800); + park_cmd.minTime(4000); // 4 sec + + control->move(park_cmd); + //park(); // TBD - same, but sets position? +} + +/*! + * Destroy this objects, releasing all servos from control on exit. + */ +ArmControl::~ArmControl(void) { + // printf("parking\n"); //TBD this causes ugly delay on app shutdown + // control->move(park_cmd); + // sleep(5); + printf("release servos\n"); + control->releaseAll(); + // sleep(1); +} + +/*! + * Move by the specified delta angle with relation to the current + * angular position of the joints. + * + * \param[in] dtheta The requested change to the current joint angles + * (radians). + */ +void +ArmControl::moveDelta(const ublas::vector& dtheta) { + + for (unsigned int i = 0; i < dtheta.size(); i++) + if (dtheta(i) != dtheta(i)) + throw std::runtime_error(std::string("commanded dtheta NaN at index ")+ + boost::lexical_cast(i)); + + for (unsigned int i = 0; i < dtheta.size(); i++) + if(fabs(dtheta(i)) == std::numeric_limits::infinity()) + throw std::runtime_error(std::string("commanded dtheta inf at index ")+ + boost::lexical_cast(i)); + + for (unsigned int i = 0; i < (dtheta.size() < position.size() + ? dtheta.size() : position.size()); i++) + position(i) += dtheta(i); + + moveToPosition(position, 50); +} + +/*! + * Move to the specified joint angle position taking at least time to + * move. The move may take longer than time to complete. + * + * \param[in] pos The requested joint position (radians). + * \param[in] time The minimum time of movement (milliseconds). + */ +void +ArmControl::moveToPosition(const ublas::vector& pos, + unsigned int time) { + + for (unsigned int i = 0; i < pos.size(); i++) + if (pos(i) != pos(i)) + throw std::runtime_error(std::string("commanded position NaN at index ")+ + boost::lexical_cast(i)); + + for (unsigned int i = 0; i < pos.size(); i++) + if(fabs(pos(i)) == std::numeric_limits::infinity()) + throw std::runtime_error(std::string("commanded position inf at index ")+ + boost::lexical_cast(i)); + + position = pos; + + printf("commanded position ["); + for (unsigned int i = 0; i < position.size(); i++) + printf("%10f,", position(i)); + printf("]\n"); + + if (slave_theta3) + position(3) = M_PI_2 - position(1) - position(2); + + for (unsigned int i = 0; i < position.size(); i++) { + if (position(i) < min_angle(i)) + position(i) = min_angle(i); + else if (position(i) > max_angle(i)) + position(i) = max_angle(i); + } + + printf("actual position ["); + for (unsigned int i = 0; i < position.size(); i++) + printf("%10f,", position(i)); + printf("]\n"); + + std::cout << "offsets: " << offset << std::endl; + Command cmd; + cmd.addMove(Arm::BASE, + thetaToPwm(position(0), offset(Arm::BASE)), + max_rate_set ? max_rate : Command::ANY_SPEED); + cmd.addMove(Arm::SHOULDER, + thetaToPwm(position(1), offset(Arm::SHOULDER)), + max_rate_set ? max_rate : Command::ANY_SPEED); + cmd.addMove(Arm::ELBOW, + thetaToPwm(position(2), offset(Arm::ELBOW)), + max_rate_set ? max_rate : Command::ANY_SPEED); + if (slave_theta3) + cmd.addMove(Arm::WRIST, + thetaToPwm(M_PI_2 - position(1) - position(2), + offset(Arm::WRIST)), + max_rate_set ? max_rate : Command::ANY_SPEED); + else cmd.addMove(Arm::WRIST, thetaToPwm(position(3), offset(Arm::WRIST))); + //cmd.addMove(Arm::GRIP, thetaToPwm(position(4), offset(Arm::GRIP))); + + if (time != 0) cmd.minTime(time); + + control->move(cmd); +} + +/*! + * Convert an angle to a PWM length, accounting for a calibration offset. + * + * \param[in] theta The angle to convert (radians). + * \param[in] offset Calibration PWM offset (microseconds). + * + * \return The equivalent PWM length (microseconds). + */ +unsigned int +ArmControl::thetaToPwm(double theta, int offset) { + // scale theta to -PI:PI + //while (theta < -M_PI) theta -= ((int)theta%(int)M_PI)*M_PI; + //while (theta > M_PI) theta -= ((int)theta%(int)M_PI)*M_PI; + + // PI/2:-PI/2 == 600:2400 + // negate theta because +theta is low PWM + unsigned int pwm = trunc((-theta + M_PI_2) * 1800 / M_PI) + 600 + offset; + if (pwm < 500) return 500; + if (pwm > 2500) return 2500; + return pwm; +} + +/*! + * Convert the PWM length to an angle, accounting for a calibration + * offset. + * + * \param[in] pwm The PWM length (microseconds). + * \param[in] offset Calibration PWM offset (microseconds). + * + * \return The equivalent angle (radians). + */ +double +ArmControl::pwmToTheta(unsigned int pwm, int offset) { + // PI/2:-PI/2 == 600:2400 + // theta value is negative because low PWM is +theta + return -((((double)pwm - 600.0 - offset) * M_PI / 1800.0) - M_PI_2); +} + +/*! + * Command the arm to a saved \a home position. + */ +void +ArmControl::home(void) { + bool temp = slave_theta3; + slave_theta3 = true; + ublas::vectorpos(ublas::zero_vector(NUM_JOINTS)); + pos(0) = 0.0; + pos(1) = -M_PI_4; + pos(2) = M_PI_2; + moveToPosition(pos,1000); + slave_theta3 = temp; +} + +/*! + * Command the grip to close to a position which would grab the ball. + */ +void +ArmControl::grabBall(void) { + Command cmd; + cmd.addMove(Arm::GRIP, 1350 + offset(Arm::GRIP), 500); + control->move(cmd); +} + +/*! + * Command the grip to close to a position which would grab a + * specially designed marker-holder for the class exercise. + */ +void +ArmControl::grabMarker(void) { + Command cmd; + cmd.addMove(Arm::GRIP, 1350 + offset(Arm::GRIP) - 200, 500); + control->move(cmd); +} + +/*! + * Open the gripper. + */ +void +ArmControl::openGrip(void) { + Command cmd; + cmd.addMove(Arm::GRIP, 1350 + offset(Arm::GRIP) + 500, 500); + control->move(cmd); +} + +/*! + * Stop movement of the arm. + * \b NOTE: Although the SSC-32 documentation lists: " = + * Cancel the current command, ASCII 27." (e.g.0x1b) this does + * not stop single or group moves in progress. The only way to + * effectively do this is to read each servo position and then command + * that position again to override any existing command. Since + * querying is not currently implemented, calling stop() releases all + * servo power. + */ +void +ArmControl::stop(void) { + control->releaseAll(); +} + +/*! + * \param[in,out] v The provided vector of degree values, converted in + * place to radian values. + */ +void +ArmControl::degreesToRadians(ublas::vector& v) { v *= M_PI / 180.0; } + +/*! + * \param[in,out] v The provided vector of radian values, converted in + * place to degree values. + */ +void +ArmControl::radiansToDegrees(ublas::vector& v) { v *= 180.0 / M_PI; } diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/ArmControl.hpp b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/ArmControl.hpp new file mode 100644 index 0000000..ce9e370 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/ArmControl.hpp @@ -0,0 +1,119 @@ +/*! + * \class ArmControl + * \brief Convert requested joint angles into servo control commands. + * + * Manage the rate and joint limits allowed for movement of the arm. + * Convert commands from absolute or delta joint angles in radians to + * PWM command strings that the SSC32Controller can handle. Apply + * maximum rate (in microseconds per second of PWM) or minimum time + * (in seconds to complete move) constraints as applicable. + */ + +/* + * VisualServoing is a tutorial program for introducing students to + * robotics. + * + * Copyright 2009, 2010 Kevin Quigley and + * Marsette Vona + * + * VisualServoing is free software: you can redistribute it andor modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * VisualServoing 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * as the file COPYING along with VisualServoing. If not, see + * . + */ +#ifndef ARM_CONTROL_HPP +#define ARM_CONTROL_HPP + +#include "Command.hpp" +#include "SSC32Controller.hpp" +#include +#include + +using namespace boost::numeric; + +#define NUM_JOINTS 5 //!< Number of joint in arm + +namespace Arm { + //! Servo port numbers. Change here if configuration differs. + enum { + BASE = 4, + SHOULDER = 3, + ELBOW = 2, + WRIST = 1, + GRIP = 0, + }; +} + +/*! + * Class for handling control commands and translating into commands + * used by the SSC32Controller. + */ +class ArmControl { + +public: + ArmControl(SSC32Controller& s, bool slave_theta3 = true); + ~ArmControl(void); + + void slaveWrist(bool s) { slave_theta3 = s; }; + void setRateLimit(unsigned int r) { max_rate_set = true; max_rate = r; }; + void clearRateLimit(void) {max_rate_set = false;}; + + void moveDelta(const ublas::vector& dtheta); + const ublas::vector& getCurrentAngles(void) {return position;}; + + void moveToPosition(const ublas::vector& pos, + unsigned int time = 0); + + void setOffset(const ublas::vector off) {offset = off;}; + const ublas::vector& getOffset(void) {return offset;}; + + void setMaxAngle(const ublas::vector& max) {max_angle = max;}; + void setMinAngle(const ublas::vector& min) {min_angle = min;}; + const ublas::vector& getMaxAngle(void) {return max_angle;}; + const ublas::vector& getMinAngle(void) {return min_angle;}; + + void grabMarker(void); + void grabBall(void); + void openGrip(void); + + + void stop(void); + bool busy(void) { return /*!(control->isMoveCompleted()) || TBD*/ set_busy; }; + void park(void) { control->move(park_cmd); position = park_position; }; + void home(void); + + // mutators + static void degreesToRadians(ublas::vector& v); + static void radiansToDegrees(ublas::vector& v); + ublas::vector position; //!< Current position + +private: + unsigned int thetaToPwm(double theta, int offset = 0); + double pwmToTheta(unsigned int pwm, int offset = 0); + + SSC32Controller* control; + bool slave_theta3; //!< If true, theta3 is not directly commandable + ublas::vector offset; //!< Calibration offsets + ublas::vector max_angle; //!< Maximum joint limits + ublas::vector min_angle; //!< Minimum joint limits + + bool set_busy; //!< Report arm busy processing another command + + bool max_rate_set; //!< TRUE = use max rate, FALSE = no software limit + unsigned int max_rate; //!< max microsecond/second change in PWM pulse + + Command park_cmd; //!< saved command + ublas::vector park_position; //!< angles to command for park + //! position +}; + +#endif diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Command.cpp b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Command.cpp new file mode 100644 index 0000000..fc2d07a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Command.cpp @@ -0,0 +1,85 @@ +/* + * VisualServoing is a tutorial program for introducing students to + * robotics. + * + * Copyright 2009, 2010 Kevin Quigley and + * Marsette Vona + * + * VisualServoing is free software: you can redistribute it andor modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * VisualServoing 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * as the file COPYING along with VisualServoing. If not, see + * . + */ +#include "Command.hpp" + + +Command::Command(const char c) { + if (c != 0) cmd << c; +} + + +void Command::addMove(unsigned int servo, + unsigned int pwm, + unsigned int speed, + unsigned int time) { + cmd << "#" << servo + << "P" << pwm; + + if (speed != ANY_SPEED) + cmd << "S" << speed; + + if (time != NO_TIME) + minTime(time); +} + +void Command::minTime(unsigned int time) { + if (time == NO_TIME) { + //perror("Time commanded is not possible"); + return; + } + + cmd << "T" << time; +} + +void Command::reset(void) { + cmd.str(""); + cmd.clear(); +} + + +const Command& Command::release(unsigned int servo) { + static Command cmd; + cmd.reset(); + cmd.addMove(servo,0); // zero move releases power + return cmd; +} + +const Command& Command::release(std::list servos) { + static Command cmd; + cmd.reset(); + for (std::list::const_iterator c = servos.begin(); + c != servos.end(); + c++) { + cmd.addMove(*c,0); // zero move releases power + } + return cmd; +} + + +const Command& Command::releaseAll(unsigned int num_servos) { + static Command cmd; + cmd.reset(); + for (unsigned int i = 0; i < num_servos; i++) { + cmd.addMove(i,0); + } + return cmd; +} diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Command.hpp b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Command.hpp new file mode 100644 index 0000000..0cc7019 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Command.hpp @@ -0,0 +1,61 @@ +/* + * VisualServoing is a tutorial program for introducing students to + * robotics. + * + * Copyright 2009, 2010 Kevin Quigley and + * Marsette Vona + * + * VisualServoing is free software: you can redistribute it andor modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * VisualServoing 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * as the file COPYING along with VisualServoing. If not, see + * . + */ +#ifndef COMMAND_HPP +#define COMMAND_HPP + +#include +#include + +class Command { + +public: + static const unsigned int NO_TIME = 0; + static const unsigned int ANY_SPEED = 0; + + Command(const char c = 0); + + void addMove(unsigned int servo, + unsigned int pwm, + unsigned int speed = ANY_SPEED, + unsigned int time = NO_TIME); + + void minTime(unsigned int msec); + + void reset(void); + + operator const std::string() const { return cmd.str(); } + + static const Command& release(unsigned int servo); + static const Command& release(const std::list servos); + static const Command& releaseAll(unsigned int num_servos); + static const Command& queryMove(void); + static const Command& queryPosition(unsigned int servo); + +private: + static const char CR = '\r'; + static const char CANCEL = 0x1b; + + std::stringstream cmd; +}; + + +#endif diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Makefile b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Makefile new file mode 100644 index 0000000..679cd42 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/Makefile @@ -0,0 +1,14 @@ +# build for 32bit +MEXOPT += -maci +MEXOPT += -I/opt/local/include + +OTHER = ArmControl.cpp Command.cpp SSC32Controller.cpp + +build: crustcrawler.mexmaci + +crustcrawler.mexmaci: crustcrawler.cpp + echo $(PATH) + mex $(MEXOPT) crustcrawler.cpp $(OTHER) + +clean: + rm crustcrawler.mexmaci diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/SSC32Controller.cpp b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/SSC32Controller.cpp new file mode 100644 index 0000000..e1d18ea --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/SSC32Controller.cpp @@ -0,0 +1,175 @@ +/* + * VisualServoing is a tutorial program for introducing students to + * robotics. + * + * Copyright 2009, 2010 Kevin Quigley and + * Marsette Vona + * + * VisualServoing is free software: you can redistribute it andor modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * VisualServoing 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * as the file COPYING along with VisualServoing. If not, see + * . + */ +#include "SSC32Controller.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +SSC32Controller::SSC32Controller(const std::string& port_name, BAUD baud) { + + std::cerr << "opening port \"" << port_name << "\""<< std::endl; + port = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (port < 0) { + + perror(port_name.c_str()); + // exit(port); + + } else { + + std::cerr << "opened port \"" << port_name << "\""<< std::endl; + + tcgetattr(port, &oldtio); // save current serial port settings + + memset(&newtio, 0x00, sizeof(newtio)); + + // set baud rate + cfsetispeed(&newtio, baud); // input + cfsetospeed(&newtio, baud); // output + + // CRTSCTS/CNEW_RTSCTS: output hardware flow control + // set to NONE +#if defined(CRTSCTS) + newtio.c_cflag &= ~CRTSCTS; +#elif defined(CNEW_RTCTS) + newtio.c_cflag &= ~CNEW_RTSCTS; +#endif + + // CLOCAL : local connection, no modem contol + // CREAD : enable receiving characters + newtio.c_cflag |= CLOCAL | CREAD; + + // set parity 8N1 + newtio.c_cflag &= ~CSIZE; // data mask + newtio.c_cflag |= CS8; // 8 + newtio.c_cflag &= ~PARENB; // N + newtio.c_cflag &= ~CSTOPB; // 1 + + // use raw data, no processing or signals + newtio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + newtio.c_oflag &= ~OPOST; + + newtio.c_iflag |= ICRNL; // map CR to NL + + + newtio.c_oflag = 0; // raw output + + tcflush(port, TCIOFLUSH); // clear all data on line + tcsetattr(port, TCSANOW, &newtio); // set up port + } +} + +SSC32Controller::~SSC32Controller(void) { + if (port >= 0) { + std::cout << "closing port" << std::endl; + sleep(1); + tcsetattr(port, TCSANOW, &oldtio); // reset port to old settings + if (close(port) < 0) { + perror("error closing port"); + } + port = -1; + } +} + +void SSC32Controller::release(unsigned int servo) { + write(Command::release(servo)); +} + +void SSC32Controller::release(std::list servos) { + write(Command::release(servos)); +} + +void SSC32Controller::releaseAll(void) { + write(Command::releaseAll(NUM_SERVOS)); +} + +void SSC32Controller::move(const Command& cmd) { + write(cmd); +} + +void SSC32Controller::movePWM(unsigned int servo, + unsigned int pwm, unsigned int speed, + unsigned int time) { + cmd.reset(); + cmd.addMove(servo, pwm, speed); + cmd.minTime(time); + write(cmd); +} + +void SSC32Controller::movePWM(std::list servos, unsigned int time) { + cmd.reset(); + for (std::list::const_iterator c = servos.begin(); + c != servos.end(); + c++ ) { + cmd.addMove(c->servo, c->position, c->speed); + } + cmd.minTime(time); + write(cmd); +} + +/* +bool SSC32Controller::isMoveCompleted(void) { + + //write(Command::queryMove()); + + //TBD read + + return true; +} +*/ + + +/* +unsigned int SSC32Controller::getPosition(unsigned int servo) { + cmd.reset(); + + //cmd.queryPosition(servo); + + //write(cmd); + + //TBD read + + return 0; +} +*/ + +void SSC32Controller::write(const std::string cmd) { + printf("sending command: %s\n", cmd.c_str()); + if (port >= 0) { + int n = ::write(port, cmd.c_str(), cmd.length()); + if (n < 0) { + perror("error writing command"); + return; + } + char CR = '\r'; + if (::write(port, &CR, sizeof(CR)) != sizeof(CR)) + perror("error writing command terminator"); + else + printf("ok\n"); + } +} diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/SSC32Controller.hpp b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/SSC32Controller.hpp new file mode 100644 index 0000000..de19a52 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/SSC32Controller.hpp @@ -0,0 +1,89 @@ +/* + * VisualServoing is a tutorial program for introducing students to + * robotics. + * + * Copyright 2009, 2010 Kevin Quigley and + * Marsette Vona + * + * VisualServoing is free software: you can redistribute it andor modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * VisualServoing 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * as the file COPYING along with VisualServoing. If not, see + * . + */ +#ifndef SSC32CONTROLLER_HPP +#define SSC32CONTROLLER_HPP + +#include "Command.hpp" + +#include +#include +#include + + +class SSC32Controller { + +public: + static const unsigned int NUM_SERVOS = 32; + static const unsigned int NUM_BANKS = 4; + + struct SPS { + unsigned int servo; + unsigned int position; + unsigned int speed; + }; + + enum BAUD { + B_2400 = B2400, + B_9600 = B9600, + B_38_4K = B38400, + B_115_2K= B115200, + }; + + SSC32Controller(const std::string& port_name = "/dev/ttyS0", + //BAUD baud = B_9600); + BAUD baud = B_115_2K); + ~SSC32Controller(void); + + + unsigned int getMaxServos(void) { return NUM_SERVOS; } + + + /// movements + void release(unsigned int servo); + void release(std::list servos); + void releaseAll(void); + + /* bool isMoveCompleted(void); TBD */ + + /* unsigned int getPosition(unsigned int servo); TBD */ + + void move(const Command& cmd); + void movePWM(unsigned int servo, + unsigned int pwm, + unsigned int speed = Command::ANY_SPEED, + unsigned int time = Command::NO_TIME); + + void movePWM(std::list servos, unsigned int time = Command::NO_TIME); + + +private: + void write(const std::string s); + + int port; + Command cmd; + struct termios newtio; + struct termios oldtio; + +}; + + +#endif //#ifndef SSC32CONTROLLER_HPP diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/VisualServoing.cpp b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/VisualServoing.cpp new file mode 100644 index 0000000..0dbc5c4 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/VisualServoing.cpp @@ -0,0 +1,681 @@ +/* + * VisualServoing is a tutorial program for introducing students to + * robotics. + * + * Copyright 2009, 2010 Kevin Quigley and + * Marsette Vona + * + * VisualServoing is free software: you can redistribute it andor modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * VisualServoing 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * as the file COPYING along with VisualServoing. If not, see + * . + */ + +// system headers +#include +#include +#include +#include +#include + +// CTY arm project +#include "ArmControl.hpp" +#include "ArmGui.hpp" +#include "ArmGuiGTK.hpp" +#include "IK.hpp" +#include "ImageProcessing.hpp" +#include "Params.hpp" + +// OpenCV +#include +#include + +// Boost +#include +#include + +using namespace boost::numeric; + +#if RUN_THREADED +#include +#include +#endif + +// constants +#define FOCAL_LENGTH 481.0 // calc p65 //TBD - add calculation info +#define DIAMETER .038 //!< Diameter of ball in meters (measurexd) +#define MIN_TRACKING_RADIUS_PIXELS 2.0 //!< Minimum tracking radius required + +void mark_images(const ublas::vector& target, const CvSeq* circles, + const Params& params, Images& images); + +void calibrate_offsets(std::string& file, ublas::vector& offsets); +void update_gui_position (ArmControl& ctl, Params& params); + +void handler(int sig); +ArmControl* sig_ctl = 0; //!< Pointer to ArmControl for stopping arm + //! movement upon received signal. + + +/*! + * \Brief Starting function containing main control loop. + * Start up and configure all objects, spin off the GUI, and continue + * in main loop until told to stop. + */ +int main(int argc, char** argv) { + // set signal handling + struct sigaction action; + action.sa_handler = &handler; + + if (sigaction(SIGHUP, &action, NULL) < 0) + printf("Error setting action for SIGHUP\n"); + if (sigaction(SIGINT, &action, NULL) < 0) + printf("Error setting action for SIGINT\n"); + if (sigaction(SIGQUIT, &action, NULL) < 0) + printf("Error setting action for SIGQUIT\n"); + if (sigaction(SIGILL, &action, NULL) < 0) + printf("Error setting action for SIGILL\n"); + if (sigaction(SIGABRT, &action, NULL) < 0) + printf("Error setting action for SIGABRT\n"); + if (sigaction(SIGFPE, &action, NULL) < 0) + printf("Error setting action for SIGFPE\n"); + if (sigaction(SIGSEGV, &action, NULL) < 0) + printf("Error setting action for SIGSEGV\n"); + if (sigaction(SIGTERM, &action, NULL) < 0) + printf("Error setting action for SIGTERM\n"); + + Images images; + images.set = false; + images.bgr = 0; + images.filtered_bgr = 0; + images.filtered_hls = 0; + + Params params; + init_params(params); + CvSeq* circles = 0; + + unsigned int cameraID(0); + std::string port("/dev/ttyS0"); + std::string config_file; + + std::string flags = "hp:f:"; + int opt; + bool help = false; + while ((opt = getopt(argc, argv, flags.c_str())) > 0) { + switch (opt) { + case 'h': help = true; break; + case 'p': port = optarg; break; + case 'f': config_file = optarg; break; + default: break; + } + } + + if (help) { + printf("Visual Servo Arm Options:\n" + " -h Print this help menu\n" + " -f Use a calibration file to set joint offsets\n" + " -p Use an alternate serial port (default: /dev/ttyS0\n"); + exit(0); + } + + CvCapture* capture(0); + IplImage* frame(0); + + ImageProcessing ip; + + ublas::vector features(3); + ublas::vector delta_angles(3); + ublas::vector target_pos(3); + ublas::vector grab_target(3); + + target_pos(0) = 0.0; //x + target_pos(1) = 0.0; //y + target_pos(2) = 0.2; //Z + + grab_target(0) = 0.0; //x + grab_target(1) = 0.0; //y + grab_target(2) = 0.05; //Z + + // div by focal_length to normalize target x,y + ublas::vector target_pos_norm(target_pos); + target_pos_norm(0) /= FOCAL_LENGTH; + target_pos_norm(1) /= FOCAL_LENGTH; + + IK ik; + ik.setTarget(target_pos_norm); + ik.setLengths(0.0, .152, 0.122, 0.075); + ik.setV(.015, -.150, .25); //m, m, rad + + ArmGuiGTK* gui = ArmGuiGTK::instance(); + gui->update(images, params); + +#if RUN_THREADED + pthread_t guiTID; + switch (pthread_create(&guiTID, 0, ArmGui::threadRun, gui)) { + case EAGAIN: printf("Max threads reached\n"); return -1; + case EINVAL: printf("Invalid thread attributes\n"); return -1; + case EPERM: printf("Invalid permissions\n"); return -1; + default: break; + } +#endif + + SSC32Controller ssc(port); + ArmControl ctl(ssc); + sig_ctl = &ctl; + ctl.setRateLimit(500); + + ublas::vector off(ublas::zero_vector(NUM_JOINTS)); + calibrate_offsets(config_file, off); + ctl.setOffset(off); + + ublas::vector angle_limits(ublas::vector(NUM_JOINTS)); + + // max limits + angle_limits(0) = 3.0/8.0 * M_PI; + angle_limits(1) = M_PI_2; + angle_limits(2) = M_PI - .70; // off arm brace + angle_limits(3) = M_PI_2; + std::cout << "max limits: " << angle_limits << std::endl; + ctl.setMaxAngle(angle_limits); + + ArmControl::radiansToDegrees(angle_limits); + for (int i = 0; i < NUM_JOINTS; i++) + params.ctl.max_limits[i] = angle_limits(i); + params.limits_changed = true; + + // min limits + angle_limits(0) = -3.0/8.0 * M_PI; + angle_limits(1) = -M_PI_2 + 0.35; // off spring pedestal + // angle_limits(2) = 0; + angle_limits(2) = -50.0*2.0*M_PI/360.0; + angle_limits(3) = -M_PI_2; + ctl.setMinAngle(angle_limits); + std::cout << "min limits: " << angle_limits << std::endl; + + ArmControl::radiansToDegrees(angle_limits); + + for (int i = 0; i < NUM_JOINTS; i++) + params.ctl.min_limits[i] = angle_limits(i); + params.limits_changed = true; + + ctl.park(); + update_gui_position(ctl, params); + params.current_mode = PARK; + + + while (params.run) { //mainloop + + gui->update(images, params); + +#if !RUN_THREADED + gui->run(); +#endif + + if (!params.run) continue; //to next mainloop iteration + + if (params.gui.estop) { + params.gui.estop = false; + printf("ESTOP received\n"); + ctl.stop(); + if (params.current_mode != ESTOP) { + params.current_mode = ESTOP; + } + } + + // all activities respond to these new modes + switch (params.new_mode) { + case HOME: + params.new_mode = NONE; + printf("*** -> HOME\n"); + ctl.home(); + update_gui_position(ctl, params); + params.current_mode = READY; + break; + case PARK: + printf("park request\n"); + params.new_mode = NONE; + printf("*** -> PARK\n"); + ctl.park(); + update_gui_position(ctl, params); + params.current_mode = PARK; + break; + default: + break; + } + + // all activities respond to these current modes + switch (params.current_mode) { + case HOME: + printf("HOME->READY\n"); + params.current_mode = READY; + break; + case PARK: + // getting out of PARK handled above + usleep(10000); // 10ms + case BUSY: + printf("BUSY -> READY\n"); + if (!ctl.busy()) + params.current_mode = READY; + break; + default: + break; + } + + if (params.activity == KINEMATICS) { + + usleep(10000); // 10ms + + ctl.slaveWrist(false); + ublas::vector new_position(NUM_JOINTS); + + if (params.current_mode == READY) { + switch (params.new_mode) { + case MOVE: + params.new_mode = NONE; + printf("Moving\n"); + for (int i = 0; i < NUM_JOINTS; i++ ) + new_position(i) = params.gui.new_theta[i]; + ArmControl::degreesToRadians(new_position); + ctl.moveToPosition(new_position); + update_gui_position(ctl, params); + break; + + case DRAW: + params.new_mode = NONE; + printf("Drawing\n"); + if (params.ctl.holding_marker) { + //ctl.drawX(); + } else { + params.new_mode = ERROR; + params.error = "Must hold marker to draw."; + } + break; + + // end movement modes + case GRAB: + params.new_mode = NONE; + printf("Grab marker\n"); + if (!params.ctl.holding_marker) { + ctl.grabMarker(); + //sleep(1); + params.ctl.holding_marker = true; + } else { + printf("error set\n"); + params.error_set = true; + params.error = "Marker already held\n"; + } + break; + + case RELEASE: + params.new_mode = NONE; + printf("Release marker\n"); + if (params.ctl.holding_marker) { + ctl.openGrip(); + params.ctl.holding_marker = false; + } else { + params.error_set = true; + params.error = "Marker not being held\n"; + } + break; + + default: + break; + } + } + + // update param struct + + continue; //to next mainloop iteration + + } //end of kinematics + + // + // Setup code for Image Processing and Visual Servoing + // + + if (capture == 0) { + + capture = cvCreateCameraCapture(cameraID); + + if (capture == 0) { + printf("failed to init capture device\n"); + sleep(1); continue; //to next mainloop iteration + } + + printf("initialized capture device\n"); + printf("allocating images\n"); + + images.bgr = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_8U, 3); + +#if FLOAT_HLS + images.bgr32 = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_32F, 3); + images.hls = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_32F, 3); +#else + images.hls = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_8U, 3); +#endif + + images.filtered_bgr = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_8U, 1); + images.filtered_hls = cvCreateImage(IMAGE_SIZE, IPL_DEPTH_8U, 1); + + if (images.bgr == 0 || images.hls == 0 +#if FLOAT_HLS + || images.bgr32 == 0 +#endif + || images.filtered_bgr == 0 || images.filtered_hls == 0) { + params.current_mode = ERROR; + params.error = "Cannot create image holders"; + std::cout << params.error << std::endl; + params.run = false; + continue; //to next mainloop iteration + } + + //some images might be displayed before being initialized + + cvSet(images.bgr, cvScalar(0,0,0)); +#if FLOAT_HLS + cvSet(images.bgr32, cvScalar(0,0,0)); +#endif + cvSet(images.hls, cvScalar(0,0,0)); + cvSet(images.filtered_bgr, cvScalar(0)); + cvSet(images.filtered_hls, cvScalar(0)); + + images.set = true; + + } //capture was 0 + + // + // Image Processing + // + frame = cvQueryFrame(capture); + + if (frame == 0) { + params.current_mode = ERROR; + params.error = "Null frame"; + std::cout << params.error << std::endl; + params.run = false; + continue; //to next mainloop iteration + } + + cvResize(frame, images.bgr); + + ctl.slaveWrist(true); + + ip.filterImages(images, params); + + if (!params.gui.target_set) continue; + + if (params.activity == VS || + params.activity == IP) { + //find ball + + circles = ip.findBall(images, params); + + mark_images(target_pos, circles, params, images); + + } //find ball + + if (params.activity != VS) { + usleep(1000); + continue; //to next mainloop iteration + } + + // + // Visual Servoing code + // + + switch (params.new_mode) { + case GRAB: params.new_mode = NONE; ctl.grabBall(); break; + case RELEASE: params.new_mode = NONE; ctl.openGrip(); break; + default: break; + } + + printf("current_mode = %d\n", params.current_mode); + switch (params.current_mode) { + case READY: + printf("old: READY\t"); + switch (params.new_mode) { + case MOVE: + printf("new: MOVE\n"); + params.new_mode = NONE; + params.current_mode = MOVE; + break; + case PAUSE: + printf("new: PAUSE\n"); + params.new_mode = NONE; + params.current_mode = PAUSE; + continue; //to next mainloop iteration + default: + break; + } + break; + + case PAUSE: + printf("old: PAUSE\t"); + if (params.new_mode == MOVE) { + printf("new: MOVE\n"); + params.new_mode = NONE; + params.current_mode = MOVE; + break; + } + break; + + case MOVE: + printf("old: MOVE\t"); + if (params.new_mode == PAUSE) { + printf("new: PAUSE\n"); + params.new_mode = NONE; + //ctl.stop(); + params.current_mode = PAUSE; + continue; //to next mainloop iteration + } + break; + + default: + break; + } + + if (circles != 0 && circles->total > 0 && + params.gui.target_set && + (params.current_mode == MOVE || params.current_mode == GRAB)) { + ublas::vector features(3); + + float* p = (float*) cvGetSeqElem(circles, 0); + + printf("first circle at (%d,%d) radius %d\n", + cvRound(p[0]), cvRound(p[1]), cvRound(p[2])); + + features(0) = p[0]; features(1) = p[1]; features(2) = p[2]; + + if (features(2) >= MIN_TRACKING_RADIUS_PIXELS) { + + // rotate/translate to center origin, x left, y up + features(0) = (images.hls->width / 2.0) - features(0); // x + if (images.hls->origin == 0) // top left origin + features(1) = (images.hls->height / 2.0) - features(1); // y + + // normalize x & y + features(0) /= FOCAL_LENGTH; features(1) /= FOCAL_LENGTH; + + // circular approximation of Z + // Z = D*f / radius*2 + features(2) = DIAMETER * FOCAL_LENGTH / (features(2) * 2.0); + + printf("Norm features x,y = (%3f, %3f), Z = %3f\n", + features(0), features(1), features(2)); + printf("Norm target x,y = (%3f, %3f), Z = %3f\n", + target_pos_norm(0), target_pos_norm(1), target_pos_norm(2)); + + std::cout << "current angles: " << ctl.getCurrentAngles() << std::endl; + + bool dls = ik.damped_least_squares(features, ctl.getCurrentAngles(), + params, delta_angles); + + if (dls && params.current_mode != PARK) { + std::cout << "commanded angle deltas: " << delta_angles << std::endl; + ctl.moveDelta(delta_angles); + } + + } else { + std::cout << + "radius below tracking enable threshold " << + MIN_TRACKING_RADIUS_PIXELS; + } + } //tracking ball + + } //mainloop + +#if RUN_THREADED + switch (pthread_join(guiTID, 0)) { + case 0: break; // all ok + case EINVAL: + printf("pthread_join: Invalid thread id %d\n", (int) guiTID); break; + case ESRCH: + printf("pthread_join: Thread ID %d not found\n", (int) guiTID); break; + case EDEADLK: + printf("pthread_join: Deadlock detected\n"); break; + default: + break; + } +#endif + + if (images.set) { + printf("releasing images\n"); + cvReleaseImage(&(images.bgr)); + cvReleaseImage(&(images.hls)); + cvReleaseImage(&(images.filtered_hls)); + cvReleaseImage(&(images.filtered_bgr)); +#ifdef FLOAT_HLS + cvReleaseImage(&(images.bgr32)); +#endif + } + + if (gui != 0) { + printf("destroying gui\n"); + gui->destroy(); + gui = 0; + } + + if (capture != 0) { + printf("releasing capture device\n"); + cvReleaseCapture(&capture); + } + +} //main() + + +/*! + * \brief Markup images with circles and lines. + * Used for giving feedback to the user on the location of the visual + * servo target and where the ball is detected in the image. + * + * \param[in] target Cartesian coordinates of the target in [pixel, + * pixel, meter] units. + * \param[in] circles Sequence of detected circles (u,v,r) in pixels + * \param[in] params Params struct + * \param[in,out] images Images struct + */ +void mark_images(const ublas::vector& target, const CvSeq* circles, + const Params& params, Images& images) { + + // draw target cross + if (params.gui.target_set && params.activity == VS) { + // fl * D / Z = apparent diameter, so div by 2 to get apparent radius + double radius = (FOCAL_LENGTH * DIAMETER / target(2)) / 2.0; + + // rescale since target(x,y) was normalized using FOCAL_LENGTH + double ih = images.bgr->height/2.0; + double iw = images.bgr->width/2.0; + CvPoint v1 = cvPoint(cvRound(target(0) + iw ), + cvRound(target(1) + ih - radius)); // up + CvPoint v2 = cvPoint(cvRound(target(0) + iw ), + cvRound(target(1) + ih + radius)); // down + CvPoint h1 = cvPoint(cvRound(target(0) + iw - radius), + cvRound(target(1) + ih )); // left + CvPoint h2 = cvPoint(cvRound(target(0) + iw + radius), + cvRound(target(1) + ih )); // right + + // Draw target cross for sighting. + cvLine(images.bgr, h1, h2, CV_RGB(0x00, 0x00, 0xff)); + cvLine(images.bgr, v1, v2, CV_RGB(0x00, 0x00, 0xff)); + } + + int num_circles = /*params.activity == VS ? 1 :*/ circles->total; + + // draw the ball + for (int i = 0; i < num_circles; i++ ) { + float* p = (float*) cvGetSeqElem(circles, i); + CvPoint pt = cvPoint(cvRound(p[0]),cvRound(p[1])); + cvCircle(images.bgr, pt, cvRound(p[2]), CV_RGB(0xff, 0x00, 0x00)); + cvCircle(images.filtered_hls, pt, cvRound(p[2]), cvScalar(192)); //greyscale + //TBD mark filtered_bgr if using that to find the ball + } +} + +/*! + * \brief Uses calibration file to set offsets. + * Reads servo numbers and calibration positions from the provided + * file. Offsets are calculated from calibration position differences + * to ideal positions. + */ +void +calibrate_offsets(std::string& file, ublas::vector& offsets){ + if (file.empty()) { + offsets(Arm::ELBOW) = 400; + } else { + std::fstream input(file.c_str()); + int servo, val; + ublas::vector calibration_position(NUM_JOINTS); + calibration_position(Arm::GRIP) = 1350; + calibration_position(Arm::WRIST) = 1500; + calibration_position(Arm::ELBOW) = 1500; + calibration_position(Arm::SHOULDER) = 1500; + calibration_position(Arm::BASE) = 1500; + std::cout << "cal: " << calibration_position << std::endl; + std::cout << "grip: " << Arm::GRIP << std::endl; + while (!input.eof()) { + input >> std::skipws >> servo >> val; + printf("servo: %d, val: %d, cal: %g\t", + servo, val, calibration_position(servo)); + offsets[servo] = val - calibration_position(servo); + printf("offset: %g\n", offsets(servo)); + } + std::cout << "off: " << offsets << std::endl; + } +} + +/*! + * \brief Update params with current angles. + * Sets current angles of ctl in struct params to be picked up by GUI. + */ +void +update_gui_position (ArmControl& ctl, Params& params) { + ublas::vector current_theta(ctl.getCurrentAngles()); + ArmControl::radiansToDegrees(current_theta); + for (int i = 0; i < NUM_JOINTS; i++) { + params.ctl.current_theta[i] = current_theta(i); + } + params.position_changed = true; +} + +/*! + * \brief Signal handler function for stopping threads. + * + * \param[in] sig The received signal. + */ +void handler(int sig) { + if (sig_ctl == 0) { + printf("No control object for emergency shutdown!\n"); + } else { + sig_ctl->stop(); + } + exit(sig); +} diff --git a/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/crustcrawler.cpp b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/crustcrawler.cpp new file mode 100644 index 0000000..3500dfe --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/crustcrawler/crustcrawler.cpp @@ -0,0 +1,137 @@ +/*================================================================= + * crustcrawler.cpp + * + * This example is the C++ version of mexatexit.c. It demonstrates + * how to write strings to a data file using a C++ static class + * contructor. In this example, we do not need to use a mexatexit + * function to close the data file. This is because in C++ when you + * instantiate a static class constructor, the destructor for that + * static class gets called automatically when the MEX-file is cleared + * or exited. + * + * The input to the MEX-file is a string. You may continue calling + * the function with new strings to add to the data file + * matlab.data. The data file will not be closed until the MEX-file is + * cleared or MATLAB is exited. + + * This is a MEX-file for MATLAB. + * Copyright 1984-2006 The MathWorks, Inc. + * All rights reserved. + *=================================================================*/ + +/* $Revision: 1.2.6.2 $ */ + +#include +#include /* strlen */ +#include "mex.h" + +// CTY arm project +#include "ArmControl.hpp" + + +// Boost +#include + +using namespace boost::numeric; + +SSC32Controller *ssc; +ArmControl *ctl; + +/* + * crustcrawler('init', serialport, baudrate) open the link + * crustcrawler('stop') stop motion + * crustcrawler('park') park the robot + * crustcrawler('move', q, time) move to joint angles + * q = crustcrawler('get') get joint angles + */ + +void +mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) +{ + char *str; + + /* Check for proper number of input and output arguments */ + if (nrhs == 0) { + mexPrintf("Open connection to robot"); + + //SSC32Controller ssc(port); + ssc = new SSC32Controller("/dev/tty.usbserial-00001104", SSC32Controller::B_9600); + ctl = new ArmControl(*ssc); + + ctl->setRateLimit(500); + + ublas::vector angle_limits(ublas::vector(NUM_JOINTS)); + + // max limits + angle_limits(0) = 3.0/8.0 * M_PI; + angle_limits(1) = M_PI_2; + angle_limits(2) = M_PI - .70; // off arm brace + angle_limits(3) = M_PI_2; + mexPrintf("set max limits\n"); + ctl->setMaxAngle(angle_limits); + + // min limits + angle_limits(0) = -3.0/8.0 * M_PI; + angle_limits(1) = -M_PI_2 + 0.35; // off spring pedestal + // angle_limits(2) = 0; + angle_limits(2) = -50.0*2.0*M_PI/360.0; + angle_limits(3) = -M_PI_2; + ctl->setMinAngle(angle_limits); + mexPrintf("set min limits\n"); + + + } else if (nrhs > 0) { + if (!(mxIsChar(prhs[0]))) { + mexErrMsgTxt("Input must be of type string.\n."); + } + str=mxArrayToString(prhs[0]); + if (strcmp(str, "stop") == 0) { + mexPrintf("STOP\n"); + ctl->stop(); + } else if (strcmp(str, "park") == 0) { + mexPrintf("PARK\n"); + ctl->park(); + } else if (strcmp(str, "home") == 0) { + mexPrintf("HOME\n"); + ctl->home(); + } else if (strcmp(str, "move") == 0) { + if (nrhs != 3) + mexErrMsgTxt("Three arguments for move command\n."); + + ublas::vector new_position(NUM_JOINTS); + + if (mxGetNumberOfElements(prhs[1]) != 4) + mexErrMsgTxt("Joint vector must have 4 elements\n."); + + double *q = mxGetPr(prhs[1]); + double time = mxGetScalar(prhs[2]); + + for (int i = 0; i < 4; i++ ) + new_position(i) = q[i]; + ctl->moveToPosition(new_position, (int) time); + } else if (strcmp(str, "gripper") == 0) { + if (nrhs != 2) + mexErrMsgTxt("Two arguments for gripper command\n."); + if (mxGetScalar(prhs[1]) == 0) + ctl->grabMarker(); + else + ctl->openGrip(); + } else if (strcmp(str, "get") == 0) { + if (nlhs < 1) + mexErrMsgTxt("No output argument for return value\n."); + + mxArray *m = mxCreateDoubleMatrix(5, 1, mxREAL); + + double *q = mxGetPr(m); + for (int i = 0; i < 4; i++ ) + q[i] = ctl->position(i); + plhs[0] = m; + } + mxFree(str); + } + if (nlhs > 1){ + mexErrMsgTxt("Too many output arguments."); + } + + return; +} diff --git a/lwrserv/matlab/rvctools/robot/interfaces/dm.mat b/lwrserv/matlab/rvctools/robot/interfaces/dm.mat new file mode 100644 index 0000000..63f0c28 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/interfaces/dm.mat differ diff --git a/lwrserv/matlab/rvctools/robot/interfaces/pincher.m b/lwrserv/matlab/rvctools/robot/interfaces/pincher.m new file mode 100644 index 0000000..1132624 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/pincher.m @@ -0,0 +1,37 @@ + +% Copyright (C) 1993-2015, 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.petercorke.com +clear L +L(1) = Revolute('d', 40, 'alpha', -pi/2); +L(2) = Revolute('a', -105, 'alpha', pi, 'offset', pi/2); +L(3) = Revolute('a', -105); +L(4) = Revolute('a', -105); + +% Note alpha_2 = pi, needed to account for rotation axes of joints 3 and 4 having +% opposite sign to joint 2. +% +% s='Rz(q1) Tz(L1) Ry(q2) Tz(L2) Ry(q3) Tz(L3) Ry(q4) Tz(L4)' +% DHFactor(s) + + +arb = Arbotix('port', '/dev/tty.usbserial-A800JDPN', 'nservos', 5); + +px = RobotArm(L, arb, 'name', 'PhantomX', 'manufacturer', 'Trossen Robotics'); +qz = [0 0 0 0]; +px.tool = trotz(-pi/2) * trotx(pi/2); diff --git a/lwrserv/matlab/rvctools/robot/interfaces/server.m b/lwrserv/matlab/rvctools/robot/interfaces/server.m new file mode 100644 index 0000000..2f51912 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/server.m @@ -0,0 +1,72 @@ +% SERVER Write a message over the specified port +% +% Usage - server(message, output_port, number_of_retries) +function server(port, number_of_retries) + + import java.net.ServerSocket + import java.io.* + + if (nargin < 2) + number_of_retries = 2; % set to -1 for infinite + end + retry = 0; + + server_socket = []; + output_socket = []; + + port + + while true + + retry = retry + 1; + + try + if ((number_of_retries > 0) && (retry > number_of_retries)) + fprintf(1, 'Too many retries\n'); + break; + end + + fprintf(1, ['Try %d waiting for client to connect to this ' ... + 'host on port : %d\n'], retry, port); + + % wait for 1 second for client to connect server socket + server_socket = ServerSocket(port); + server_socket.setSoTimeout(10000); + + fprintf(1, 'Waiting for client to connect\n'); + + + output_socket = server_socket.accept; + + fprintf(1, 'Client connected\n'); + + input_stream = output_socket.getInputStream; + %d_output_stream = DataOutputStream(output_stream); + + for i=1:10 + % read data from the socket - wait a short time first + pause(0.5); + bytes_available = input_stream.available; + fprintf(1, 'Reading %d bytes\n', bytes_available); + end + + % clean up + server_socket.close; + output_socket.close; + break; + + catch me + me + if ~isempty(server_socket) + server_socket.close + end + + if ~isempty(output_socket) + output_socket.close + end + + % pause before retrying + pause(1); + end +end +end diff --git a/lwrserv/matlab/rvctools/robot/interfaces/ttest.m b/lwrserv/matlab/rvctools/robot/interfaces/ttest.m new file mode 100644 index 0000000..2b9aff2 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/interfaces/ttest.m @@ -0,0 +1,10 @@ +hh = []; +for i=1:200 + e = s.get(); + + th = [e.motionRoll e.motionPitch e.motionYaw] + R = rpy2r(th); + trplot(R) + hh = [hh; th]; + pause(0.2) +end diff --git a/lwrserv/matlab/rvctools/robot/joy2tr.m b/lwrserv/matlab/rvctools/robot/joy2tr.m new file mode 100644 index 0000000..fd9b131 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/joy2tr.m @@ -0,0 +1,96 @@ +%JOY2TR Update transform from joystick +% +% T = JOY2TR(T, OPTIONS) updates the SE(3) homogeneous transform (4x4) +% according to spatial velocities sourced from a connected joystick device. +% +% Options:: +% 'delay',D Pause for D seconds after reading (default 0.1) +% 'scale',S A 2-vector which scales joystick translational and +% rotational to rates (default [0.5m/s, 0.25rad/s]) +% 'world' Joystick motion is in the world frame +% 'tool' Joystick motion is in the tool frame (default) +% 'rotate',R Index of the button used to enable rotation (default 7) +% +% Notes:: +% - Joystick axes 0,1,3 map to X,Y,Z or R,P,Y motion. +% - A joystick button enables the mapping to translation OR rotation. +% - A 'delay' of zero means no pause +% - If 'delay' is non-zero 'scale' maps full scale to m/s or rad/s. +% - If 'delay' is zero 'scale' maps full scale to m/sample or rad/sample. +% +% See also joystick. + +% Copyright (C) 1993-2015, 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.petercorke.com + +function Tout = joy2tr(T, varargin) + + % parse the options + opt.delay = 0.1; + opt.rotate = 7; + opt.scale = [0.5 0.25]; + opt.frame = {'tool', 'world'}; + opt.min = 0.006; + + opt = tb_optparse(opt, varargin); + + % get the raw joystick output + [j,b] = joystick(); + + % update a 6-vector of translational and rotational velocities + vel = zeros(1,6); + if b(7) == 0 + % translation mode + vel(1:2) = j(1:2); + vel(3) = j(4); + else + % rotation mode + vel(4:5) = j(1:2); + vel(6) = j(4); + end + + % values below threshold are set to zero + vel(abs(vel) < opt.min) = 0; + + % apply scaling factors + scale = opt.scale; + + if opt.delay > 0 + % normalize scaling factors by time + pause(opt.delay); + scale = scale * opt.delay; + end + + vel(1:3) = vel(1:3) * scale(1); + vel(4:6) = vel(4:6) * scale(2); + + % compute the incremental motion + dT = transl(vel(1:3)) * rpy2tr(vel(4:6)); + + + % and apply it to the input transform + switch opt.frame + case 'tool' + Tout = T * dT; + case 'world' + Tout = dT * T; + end + + % normalize just to be safe + Tout = trnorm(Tout); diff --git a/lwrserv/matlab/rvctools/robot/joystick.m b/lwrserv/matlab/rvctools/robot/joystick.m new file mode 100644 index 0000000..e2095e1 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/joystick.m @@ -0,0 +1,34 @@ +%JOYSTICK Input from joystick +% +% J = JOYSTICK() returns a vector of joystick values in the range -1 to +1. +% +% [J,B] = JOYSTICK() as above but also returns a vector of button values, +% either 0 (not pressed) or 1 (pressed). +% +% Notes:: +% - This is a MEX file that uses SDL (www.libsdl.org) to interface to a standard +% gaming joystick. +% - The length of the vectors J and B depend on the capabilities of the +% joystick identified when it is first opened. +% +% See also joy2tr. + +% Copyright (C) 1993-2015, 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.petercorke.com +% joystick identified when it is first opened. diff --git a/lwrserv/matlab/rvctools/robot/jsingu.m b/lwrserv/matlab/rvctools/robot/jsingu.m new file mode 100644 index 0000000..e4eab29 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/jsingu.m @@ -0,0 +1,42 @@ +%JSINGU Show the linearly dependent joints in a Jacobian matrix +% +% JSINGU(J) displays the linear dependency of joints in a Jacobian matrix. +% This dependency indicates joint axes that are aligned and causes singularity. +% +% See also SerialLink.jacobn. + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function jsingu(J) + + % convert to row-echelon form + [R, jb] = rref(J); + + depcols = setdiff( 1:numcols(J), jb); + + fprintf('%d linearly dependent joints:\n', length(depcols)); + for d=depcols + fprintf(' q%d depends on: ', d) + for k=find(R(:,d)) + fprintf('q%d ', k); + end + fprintf('\n'); + end diff --git a/lwrserv/matlab/rvctools/robot/jtraj.m b/lwrserv/matlab/rvctools/robot/jtraj.m new file mode 100644 index 0000000..a87b5e9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/jtraj.m @@ -0,0 +1,84 @@ +%JTRAJ Compute a joint space trajectory between two points +% +% [Q,QD,QDD] = JTRAJ(Q0, QF, M) is a joint space trajectory Q (MxN) where the joint +% coordinates vary from Q0 (1xN) to QF (1xN). A quintic (5th order) polynomial is used +% with default zero boundary conditions for velocity and acceleration. +% Time is assumed to vary from 0 to 1 in M steps. Joint velocity and +% acceleration can be optionally returned as QD (MxN) and QDD (MxN) respectively. +% The trajectory Q, QD and QDD are MxN matrices, with one row per time step, +% and one column per joint. +% +% [Q,QD,QDD] = JTRAJ(Q0, QF, M, QD0, QDF) as above but also specifies initial +% and final joint velocity for the trajectory. +% +% [Q,QD,QDD] = JTRAJ(Q0, QF, T) as above but the trajectory length is defined +% by the length of the time vector T (Mx1). +% +% [Q,QD,QDD] = JTRAJ(Q0, QF, T, QD0, QDF) as above but specifies initial and +% final joint velocity for the trajectory and a time vector. +% +% See also CTRAJ, SerialLink.jtraj. + + +% 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 . + +function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1) + if length(tv) > 1 + tscal = max(tv); + t = tv(:)/tscal; + else + tscal = 1; + t = (0:(tv-1))'/(tv-1); % normalized time from 0 -> 1 + end + + q0 = q0(:); + q1 = q1(:); + + if nargin == 3 + qd0 = zeros(size(q0)); + qd1 = qd0; + elseif nargin == 5 + qd0 = qd0(:); + qd1 = qd1(:); + else + error('incorrect number of arguments') + end + + % compute the polynomial coefficients + A = 6*(q1 - q0) - 3*(qd1+qd0)*tscal; + B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal; + C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal; + E = qd0*tscal; % as the t vector has been normalized + F = q0; + + tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))]; + c = [A B C zeros(size(A)) E F]'; + + qt = tt*c; + + % compute optional velocity + if nargout >= 2 + c = [ zeros(size(A)) 5*A 4*B 3*C zeros(size(A)) E ]'; + qdt = tt*c/tscal; + end + + % compute optional acceleration + if nargout == 3 + c = [ zeros(size(A)) zeros(size(A)) 20*A 12*B 6*C zeros(size(A))]'; + qddt = tt*c/tscal^2; + end diff --git a/lwrserv/matlab/rvctools/robot/lspb.m b/lwrserv/matlab/rvctools/robot/lspb.m new file mode 100644 index 0000000..fc306d0 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/lspb.m @@ -0,0 +1,144 @@ +%LSPB Linear segment with parabolic blend +% +% [S,SD,SDD] = LSPB(S0, SF, M) is a scalar trajectory (Mx1) that varies +% smoothly from S0 to SF in M steps using a constant velocity segment and +% parabolic blends (a trapezoidal path). Velocity and acceleration can be +% optionally returned as SD (Mx1) and SDD (Mx1). +% +% [S,SD,SDD] = LSPB(S0, SF, M, V) as above but specifies the velocity of +% the linear segment which is normally computed automatically. +% +% [S,SD,SDD] = LSPB(S0, SF, T) as above but specifies the trajectory in +% terms of the length of the time vector T (Mx1). +% +% [S,SD,SDD] = LSPB(S0, SF, T, V) as above but specifies the velocity of +% the linear segment which is normally computed automatically and a time +% vector. +% +% Notes:: +% - If no output arguments are specified S, SD, and SDD are plotted. +% - For some values of V no solution is possible and an error is flagged. +% +% See also TPOLY, JTRAJ. + +% 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 . + +function [s,sd,sdd] = lspb(q0, q1, t, V) + + t0 = t; + if isscalar(t) + t = (0:t-1)'; + else + t = t(:); + end + + tf = max(t(:)); + + if nargin < 4 + % if velocity not specified, compute it + V = (q1-q0)/tf * 1.5; + else + if V < (q1-q0)/tf + error('V too small'); + elseif V > 2*(q1-q0)/tf + error('V too big'); + end + end + + if q0 == q1 + s = ones(size(t)) * q0; + sd = zeros(size(t)); + sdd = zeros(size(t)); + return + end + + tb = (q0 - q1 + V*tf)/V; + 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; + elseif tt <= (tf-tb) + % linear motion + p(i) = (q1+q0-V*tf)/2 + V*tt; + pd(i) = V; + pdd(i) = 0; + 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 diff --git a/lwrserv/matlab/rvctools/robot/makemap.m b/lwrserv/matlab/rvctools/robot/makemap.m new file mode 100644 index 0000000..8e4bace --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/makemap.m @@ -0,0 +1,151 @@ +%MAKEMAP Make an occupancy map +% +% map = makemap(N) is an occupancy grid map (NxN) created by a simple +% interactive editor. The map is initially unoccupied and obstacles can +% be added using geometric primitives. +% +% map = makemap() as above but N=128. +% +% map = makemap(map0) as above but the map is initialized from the +% occupancy grid map0, allowing obstacles to be added. +% +% With focus in the displayed figure window the following commands can +% be entered: +% left button click and drag to create a rectangle +% p draw polygon +% c draw circle +% u undo last action +% e erase map +% q leave editing mode and return map +% +% See also: DXForm, PRM, RRT. + +function world = makemap(Nw) + + if nargin < 1 + Nw = 128; + end + + if isscalar(Nw) + % initialize a new map + world = zeros(Nw, Nw); + else + % we were passed a map + world = Nw; + end + + imagesc(world); + set(gca, 'Ydir', 'normal'); + grid + binary_cmap = [1 1 1; 1 0 0]; + colormap(binary_cmap); + caxis([0 1]); + figure(gcf) + set(gcf, 'Name', 'makemap'); + + fprintf('makemap:\n'); + fprintf(' left button, click and drag to create a rectangle\n'); + fprintf(' or type the following letters in the figure window:\n'); + fprintf(' p - draw polygon\n'); + fprintf(' c - draw circle\n'); + fprintf(' e - erase map\n'); + fprintf(' u - undo last action\n'); + fprintf(' q - leave editing mode\n'); + + + while 1 + drawnow + + k = waitforbuttonpress; + if k == 1, + % key pressed + c = get(gcf, 'CurrentCharacter'); + switch c + case 'p' + fprintf('click a sequence of points, when done') + title('click a sequence of points, when done') + xy = ginput; + + fprintf('\r \r') + title('') + + xy = round(xy); + world_prev = world; + [X,Y] = meshgrid(1:Nw, 1:Nw); + world = world + inpolygon(X, Y, xy(:,1), xy(:,2)); + + case 'c' + fprintf('click a centre point') + title('click a centre point') + waitforbuttonpress; + point1 = get(gca,'CurrentPoint'); % button down detected + point1 = round(point1(1,1:2)); % extract x and y + hold on + h = plot(point1(1), point1(2), '+'); + drawnow + + fprintf('\rclick a circumference point') + title('click a circumference point') + waitforbuttonpress; + point2 = get(gca,'CurrentPoint'); % button up detected + point2 = round(point2(1,1:2)); + + fprintf('\r \r') + title('') + delete(h); + drawnow + hold off + + r = round(colnorm(point1-point2)); + c = kcircle(r); + + world_prev = world; + + % add the circle to the world + world(point1(2)-r:point1(2)+r,point1(1)-r:point1(1)+r) = ... + world(point1(2)-r:point1(2)+r,point1(1)-r:point1(1)+r) + c; + % ensure maximum value of occ.grid is 1 + world = min(world, 1); + + case 'e' + world_prev = world; + world = zeros(Nw, Nw); + + case 'u' + if ~isempty(world_prev) + world = world_prev; + end + + otherwise + break; % key pressed + end + else + % button pressed + point1 = get(gca,'CurrentPoint'); % button down detected + point2 = get(gca,'CurrentPoint'); % button up detected + + point1 = round(point1(1,1:2)); % extract x and y + point2 = round(point2(1,1:2)); + + x1 = max(1, point1(1)); + x2 = min(Nw, point2(1)); + y1 = max(1, point1(2)); + y2 = min(Nw, point2(2)); + + if x1 > x2 + t = x1; x1 = x2; x2 = t; + end + if y1 > y2 + t = y1; y1 = y2; y2 = t; + end + + world_prev = world; + world(y1:y2, x1:x2) = 1; + end + + % ensure maximum value of occ.grid is 1 + world = min(world, 1); + + % update the world display + set(gco, 'CData', world); + end diff --git a/lwrserv/matlab/rvctools/robot/mdl_3link3d.m b/lwrserv/matlab/rvctools/robot/mdl_3link3d.m new file mode 100644 index 0000000..3e71363 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_3link3d.m @@ -0,0 +1,23 @@ +% Copyright (C) 1993-2015, 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.petercorke.com +dh = [0 1 0 pi/2 + 0 0 2 0 + 0 0 3 0]; + +R3 = SerialLink(dh, 'name', '3link 3D', 'comment', 'Spong p106'); diff --git a/lwrserv/matlab/rvctools/robot/mdl_Fanuc10L.m b/lwrserv/matlab/rvctools/robot/mdl_Fanuc10L.m new file mode 100644 index 0000000..d8e8655 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_Fanuc10L.m @@ -0,0 +1,52 @@ +%MDL_FANUC10L Create kinematic model of Fanuc AM120iB/10L robot +% +% mdl_fanuc10L +% +% Script creates the workspace variable R which describes the +% kinematic characteristics of a Fanuc AM120iB/10L robot using standard +% DH conventions. +% +% Also defines the workspace vector: +% q0 mastering position. +% +% Author:: +% Wynand Swart, +% Mega Robots CC, P/O Box 8412, Pretoria, 0001, South Africa +% wynand.swart@gmail.com +% +% See also SerialLink, mdl_puma560akb, mdl_stanford, mdl_twolink. + +% 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 . + +%Cell: 073-1555-430 +%30 Sep 2007 +%Fanuc AM120iB/10L robot + +% theta d a alpha +L(1) = Link([0 0 0.15 -pi/2 0 ]); +L(2) = Link([0 0 0.77 pi 0 ]); +L(3) = Link([0 0 0.1 -pi/2 0 ]); +L(4) = Link([0 -0.96 0 pi/2 0 ]); +L(5) = Link([0 0 0 -pi/2 0 ]); +L(6) = Link([0 -0.1 0 0 0 ]); +%########################################################## +%Pose 0; At MASTERING position; +%########################################################## +q0 =[0 -pi/2 0 0 0 0]; +R=SerialLink(L, 'name', 'Fanuc AM120iB/10L'); +%########################################################## diff --git a/lwrserv/matlab/rvctools/robot/mdl_KR5.m b/lwrserv/matlab/rvctools/robot/mdl_KR5.m new file mode 100644 index 0000000..dc7827e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_KR5.m @@ -0,0 +1,44 @@ +%MDL_KR5 Create model of Kuka KR5 manipulator +% +% MDL_KR5 is a script that creates the workspace variable mico which +% describes the kinematic characteristics of a Kuka KR5 manipulator using +% standard DH conventions. +% +% Also define the workspace vectors: +% qk1 nominal working position 1 +% qk2 nominal working position 2 +% qk3 nominal working position 3 +% +% Notes:: +% - SI units of metres are used. +% - Includes 11.5cm tool in the z-direction +% +% Author:: +% - Gautam sinha +% Indian Institute of Technology, Kanpur. +% +% See also SerialLink, mdl_irb140, mdl_fanuc10l, mdl_motomanHP6, mdl_S4ABB2p8, mdl_puma560. + +% MODEL: Kuka, KR5, 6DOF, standard_DH + +%mdl_KR5 +%Define simplest line model for KUKA KR5 robot +%Contain DH parameters for KUKA KR5 robot +%All link lenghts and offsets are measured in cm +clear L +% theta d a alpha +L(1) = Link([0 0.4 0.18 pi/2]); +L(2) = Link([0 0.135 0.60 pi]); +L(3) = Link([0 0.135 0.12 -pi/2]); +L(4) = Link([0 0.62 0 pi/2]); +L(5) = Link([0 0 0 -pi/2]); +L(6) = Link([0 0 0 0]); + +KR5=SerialLink(L, 'name', 'Kuka KR5'); +KR5.tool=transl(0,0,0.115); +KR5.ikineType = 'kr5'; +KR5.model3d = 'KUKA/KR5_arc'; + +qk1=[pi/4 pi/3 pi/4 pi/6 pi/4 pi/6]; +qk2=[pi/4 pi/3 pi/6 pi/3 pi/4 pi/6]; +qk3=[pi/6 pi/3 pi/6 pi/3 pi/6 pi/3]; diff --git a/lwrserv/matlab/rvctools/robot/mdl_MotomanHP6.m b/lwrserv/matlab/rvctools/robot/mdl_MotomanHP6.m new file mode 100644 index 0000000..7daa9dd --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_MotomanHP6.m @@ -0,0 +1,53 @@ +%MDL_MotomanHP6 Create kinematic data of a Motoman HP6 manipulator +% +% mdl_motomanHP6 +% +% Script creates the workspace variable R which describes the +% kinematic characteristics of a Motoman HP6 manipulator +% using standard DH conventions. +% +% Also defines the workspace vector: +% q0 mastering position. +% +% Author: +% Wynand Swart, +% Mega Robots CC, P/O Box 8412, Pretoria, 0001, South Africa +% wynand.swart@gmail.com +% +% See also SerialLink, mdl_puma560akb, mdl_stanford, mdl_twolink. + +% 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 . + +%Cell: 073-1555-430 +%30 Sep 2007 +%Motoman HP robot + +%########################################################## +% theta d a alpha +L(1) = Link([ 0 0 0.15 -pi/2 0]); +L(2) = Link([ 0 0 0.57 -pi 0]); +L(3) = Link([ 0 0 0.155 -pi/2 0]); +L(4) = Link([ 0 -0.635 0 pi/2 0]); +L(5) = Link([ 0 0 0 -pi/2 0]); +L(6) = Link([ 0 -0.095 0 pi 0]); +%########################################################## +%Pose 0; At ZERO position +%########################################################## +q0 =[0 -pi/2 0 0 -pi/2 0]; +R=SerialLink(L, 'name', 'Motoman HP6'); +%########################################################## diff --git a/lwrserv/matlab/rvctools/robot/mdl_S4ABB2p8.m b/lwrserv/matlab/rvctools/robot/mdl_S4ABB2p8.m new file mode 100644 index 0000000..0867d2e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_S4ABB2p8.m @@ -0,0 +1,52 @@ +%MDL_S4ABB2p8 Create kinematic model of ABB S4 2.8robot +% +% mdl_s4abb2P8 +% +% Script creates the workspace variable R which describes the +% kinematic characteristics of an ABB S4 2.8 robot using standard +% DH conventions. +% +% Also defines the workspace vector: +% q0 mastering position. +% +% Author:: +% Wynand Swart, +% Mega Robots CC, P/O Box 8412, Pretoria, 0001, South Africa +% wynand.swart@gmail.com +% +% See also SerialLink, mdl_puma560akb, mdl_stanford, mdl_twolink. + +% 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 . + +%Cell: 073-1555-430 +%30 Sep 2007 +%S4 ABB 2.8 robot + +% theta d a alpha +L(1) = Link([ 0 0.9 0.188 -pi/2 0]); +L(2) = Link([ 0 0 0.95 0 0]); +L(3) = Link([ 0 0 0.225 -pi/2 0]); +L(4) = Link([ 0 1.705 0 pi/2 0]); +L(5) = Link([ 0 0 0 -pi/2 0]); +L(6) = Link([ 0 0.2 0 -pi/2 0]); +%########################################################## +%Pose 0; At SYNCHRONISATION position +%########################################################## +q0 = [0 -pi/2 0 0 0 -pi/2]; +R=SerialLink(L, 'name', 'S4 ABB 2.8'); +%########################################################## diff --git a/lwrserv/matlab/rvctools/robot/mdl_ball.m b/lwrserv/matlab/rvctools/robot/mdl_ball.m new file mode 100644 index 0000000..455cacc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_ball.m @@ -0,0 +1,59 @@ +%MDL_BALL Create model of a ball manipulator +% +% MDL_BALL creates the workspace variable ball which describes the +% kinematic characteristics of a serial link manipulator that folds into +% a ball shape. By default has 50 joints. +% +% MDL_BALL(N) as above but creates a manipulator with N joints. +% +% Also define the workspace vectors: +% q joint angle vector for default ball configuration +% +% Reference:: +% - "A divide and conquer articulated-body algorithm for parallel O(log(n)) +% calculation of rigid body dynamics, Part 2", +% Int. J. Robotics Research, 18(9), pp 876-892. +% +% Notes:: +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_puma560akb, mdl_stanford, mdl_twolink, mdl_coil. + +function mdl_ball(N) + + if nargin == 0 + N = 50; + end + + % create the links + for i=1:N + links(i) = Link([0 0 0.1, pi/2]); + q(i) = fract(i); + end + + % and build a serial link manipulator + r = SerialLink(links, 'name', 'ball'); + + % place the variables into the global workspace + if nargin == 0 + assignin('base', 'coil', r); + assignin('base', 'q', q); + end + + +end + + function f = fract(i) + theta1 = 1; + theta2 = -2/3; + + switch mod(i,3) + case 1 + f = theta1; + case 2 + f = theta2; + case 0 + f = fract(i/3); + end + end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/mdl_baxter.m b/lwrserv/matlab/rvctools/robot/mdl_baxter.m new file mode 100644 index 0000000..002b9f2 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_baxter.m @@ -0,0 +1,50 @@ +%MDL_BAXTER Kinematic model of Baxter dual-arm robot +% +% MDL_BAXTER is a script that creates the workspace variables LEFT and +% RIGHT which describes the kinematic characteristics of the two arms of a +% Rethink Robotics Baxter robot using standard DH conventions. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qd lower arm horizontal as per data sheet +% +% Notes:: +% - SI units of metres are used. +% +% References:: +% "Kinematics Modeling and Experimental Verification of Baxter Robot" +% Z. Ju, C. Yang, H. Ma, Chinese Control Conf, 2015. +% +% See also SerialLink, mdl_nao. + + +% MODEL: Baxter, Rethink Robotics, 7DOF, standard_DH + + +% th d a alpha + +L(1) = Revolute('d', 0.27, 'a', 0.069, 'alpha', -pi/2); +L(2) = Revolute('d', 0, 'a', 0, 'alpha', pi/2, 'offset', pi/2); +L(3) = Revolute('d', 0.102+0.262, 'a', 0.069, 'alpha', -pi/2); +L(4) = Revolute('d', 0, 'a', 0, 'alpha', pi/2); +L(5) = Revolute('d', 0.103+0.271, 'a', 0.010, 'alpha', -pi/2); +L(6) = Revolute('d', 0, 'a', 0, 'alpha', pi/2); +L(7) = Revolute('d', 0.28, 'a', 0, 'alpha', 0); + +left = SerialLink(L, 'name', 'Baxter LEFT'); +right = SerialLink(L, 'name', 'Baxter RIGHT'); + +left.base = transl(0.064614, 0.25858, 0.119)*rpy2tr(0, 0, pi/4); +right.base = transl(0.063534, -0.25966, 0.119)*rpy2tr(0, 0, -pi/4); + +% define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qstretch arm is stretched out in the X direction +% qn arm is at a nominal non-singular configuration +% +qz = [0 0 0 0 0 0 0]; % zero angles, L shaped pose +qr = [0 -pi/2 -pi/2 0 0 0 0]; % ready pose, arm up +qs = [0 0 -pi/2 0 0 0 0]; +qn = [0 pi/4 pi/2 0 pi/4 0 0]; diff --git a/lwrserv/matlab/rvctools/robot/mdl_coil.m b/lwrserv/matlab/rvctools/robot/mdl_coil.m new file mode 100644 index 0000000..cd381ce --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_coil.m @@ -0,0 +1,40 @@ +%MDL_COIL Create model of a coil manipulator +% +% MDL_COIL creates the workspace variable coil which describes the +% kinematic characteristics of a serial link manipulator that folds into +% a helix shape. By default has 50 joints. +% +% MDL_BALL(N) as above but creates a manipulator with N joints. +% +% Also define the workspace vectors: +% q joint angle vector for default helical configuration +% +% Reference:: +% - "A divide and conquer articulated-body algorithm for parallel O(log(n)) +% calculation of rigid body dynamics, Part 2", +% Int. J. Robotics Research, 18(9), pp 876-892. +% +% Notes:: +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_puma560akb, mdl_stanford, mdl_twolink, mdl_ball. +function mdl_coil(N) + + if nargin == 0 + N = 50; + end + + % create the links + for i=1:N + links(i) = Link('d', 0, 'a', 1/N, 'alpha', 5*pi/N); + end + + % and build a serial link manipulator + r = SerialLink(links, 'name', 'coil'); + + % place the variables into the global workspace + if nargin == 0 + assignin('base', 'coil', r); + assignin('base', 'q', 10*pi/N*ones(1,N)); + end \ No newline at end of file diff --git a/lwrserv/matlab/rvctools/robot/mdl_hyper2d.m b/lwrserv/matlab/rvctools/robot/mdl_hyper2d.m new file mode 100644 index 0000000..002fb8a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_hyper2d.m @@ -0,0 +1,72 @@ +%MDL_HYPER2D Create model of a hyper redundant planar manipulator +% +% MDL_HYPER2D creates the workspace variable h2d which describes the +% kinematic characteristics of a serial link manipulator with 10 joints +% which at zero angles is a straight line in the XY plane. +% +% MDL_HYPER2D(N) as above but creates a manipulator with N joints. +% +% Also define the workspace vectors: +% qz joint angle vector for zero angle configuration +% +% R = MDL_HYPER2D(N) functional form of the above, returns the SerialLink object. +% +% [R,QZ] = MDL_HYPER2D(N) as above but also returns a vector of zero joint angles. +% +% +% Notes:: +% - The manipulator in default pose is a straight line 1m long. +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_hyper3d, mdl_coil, mdl_ball, mdl_twolink. + +% MODEL: planar, hyper redundant, 10DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + +function [r,qq] = mdl_hyper2d(N) + + if nargin == 0 + N = 10; + end + + % create the links + for i=1:N + links(i) = Link([0 0 1/N, 0]); + q(i) = 0; + end + + % and build a serial link manipulator + robot = SerialLink(links, 'name', 'hyper2d'); + + % place the variables into the global workspace + if nargout == 0 + assignin('base', 'h2d', robot); + assignin('base', 'qz', q); + elseif nargout == 1 + r = robot; + elseif nargout == 2 + r = robot; + qq = q; + end + + +end diff --git a/lwrserv/matlab/rvctools/robot/mdl_hyper3d.m b/lwrserv/matlab/rvctools/robot/mdl_hyper3d.m new file mode 100644 index 0000000..a008aeb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_hyper3d.m @@ -0,0 +1,77 @@ +%MDL_HYPER3D Create model of a hyper redundant 3D manipulator +% +% MDL_HYPER3D is a script that creates the workspace variable h3d which +% describes the kinematic characteristics of a serial link manipulator with +% 10 joints which at zero angles is a straight line in the XY plane. +% +% MDL_HYPER3D(N) as above but creates a manipulator with N joints. +% +% Also define the workspace vectors: +% qz joint angle vector for zero angle configuration +% +% R = MDL_HYPER3D(N) functional form of the above, returns the SerialLink object. +% +% [R,QZ] = MDL_HYPER3D(N) as above but also returns a vector of zero joint angles. +% +% Notes:: +% - The manipulator in default pose is a straight line 1m long. +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_hyper2d, mdl_ball, mdl_coil. + +% MODEL: hyper redundant, 10DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + +function [r,qq] = mdl_hyper3d(N) + + if nargin == 0 + N = 10; + end + + % create the links + for i=1:N + if mod(i,2) == 0 + links(i) = Link([0 0 1/N, 0]); + else + links(i) = Link([0 0 1/N, pi/2]); + + end + q(i) = 0; + end + + % and build a serial link manipulator + robot = SerialLink(links, 'name', 'hyper3d', ... + 'plotopt', {'nojoints'}); + + % place the variables into the global workspace + if nargout == 0 + assignin('base', 'h3d', robot); + assignin('base', 'qz', q); + elseif nargout == 1 + r = robot; + elseif nargout == 2 + r = robot; + qq = q; + end + + +end diff --git a/lwrserv/matlab/rvctools/robot/mdl_irb140.m b/lwrserv/matlab/rvctools/robot/mdl_irb140.m new file mode 100644 index 0000000..2b8ffaf --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_irb140.m @@ -0,0 +1,83 @@ +%MDL_IRB140 Create model of ABB IRB 140 manipulator +% +% MDL_IRB140 is a script that creates the workspace variable robot which +% describes the kinematic characteristics of an ABB IRB 140 manipulator +% using standard DH conventions. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qd lower arm horizontal as per data sheet +% +% Reference:: +% - "IRB 140 data sheet", ABB Robotics. +% - "Utilizing the Functional Work Space Evaluation Tool for Assessing a +% System Design and Reconfiguration Alternatives" +% A. Djuric and R. J. Urbanic +% +% Notes:: +% - SI units of metres are used. +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_fanuc10l, mdl_m16, mdl_motormanHP6, mdl_S4ABB2p8, mdl_puma560. + +% MODEL: ABB, IRB140, 6DOF, standard_DH + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function r = mdl_irb140() + + deg = pi/180; + + % robot length values (metres) + d1 = 0.352; + a1 = 0.070; + a2 = 0.360; + d4 = 0.380; + d6 = 0.065; + + % DH parameter table + % theta d a alpha + dh = [0 d1 a1 -pi/2 + 0 0 a2 0 + 0 0 0 pi/2 + 0 d4 0 -pi/2 + 0 0 0 pi/2 + 0 d6 0 pi/2]; + + + % and build a serial link manipulator + + robot = SerialLink(dh, 'name', 'IRB 140', ... + 'manufacturer', 'ABB', 'ikine', 'nooffset'); + + % place the variables into the global workspace + if nargin == 1 + r = robot; + elseif nargin == 0 + assignin('base', 'irb140', robot); + assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles + assignin('base', 'qd', [0 -90 180 0 0 -90]*deg); % data sheet pose, horizontal + assignin('base', 'qr', [0 -90 90 0 90 -90]*deg); % ready pose, arm up + end +end + diff --git a/lwrserv/matlab/rvctools/robot/mdl_irb140_mdh.m b/lwrserv/matlab/rvctools/robot/mdl_irb140_mdh.m new file mode 100644 index 0000000..46deb91 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_irb140_mdh.m @@ -0,0 +1,94 @@ +%MDL_IR140 Create model of the ABB IRB 140 manipulator +% +% mdl_irb140_mod +% +% Script creates the workspace variable irb which describes the +% kinematic characteristics of an ABB IRB 140 manipulator using +% modified DH conventions. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% +% Reference:: +% - ABB IRB 140 data sheet +% - "THE MODELING OF A SIX DEGREE-OF-FREEDOM INDUSTRIAL ROBOT FOR +% THE PURPOSE OF EFFICIENT PATH PLANNING" +% Master of Science Thesis, Penn State U, May 2009 +% Tyler Carter +% +% See also SerialLink, mdl_irb140, mdl_puma560, mdl_stanford, mdl_twolink. +% +% Notes:: +% - SI units of metres are used. +% - The tool frame is in the centre of the tool flange. +% - Zero angle configuration has the upper arm vertical and lower arm +% horizontal. + +% MODEL: ABB, IRB140, 6DOF, modified_DH + +% Reference:: + + + +% Copyright (C) 1993-2015, 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.petercorke.com + +clear L + +% joint angle limits from +% A combined optimization method for solving the inverse kinematics problem... +% Wang & Chen +% IEEE Trans. RA 7(4) 1991 pp 489- +L(1) = Revolute('d', 0.352, 'a', 0, 'alpha', 0, 'offset', 0, 'modified'); +L(2) = Revolute('d', 0, 'a', 0.070, 'alpha', pi/2, 'offset', 0, 'modified'); +L(3) = Revolute('d', 0, 'a', 0.360, 'alpha', 0, 'offset', 0, 'modified'); +L(4) = Revolute('d', 0.380, 'a', 0, 'alpha', pi/2, 'offset', 0, 'modified'); +L(5) = Revolute('d', 0, 'a', 0, 'alpha', -pi/2, 'offset', 0, 'modified'); +L(6) = Revolute('d', 0, 'a', 0, 'alpha', pi/2, 'offset', 0, 'modified'); + +L(1).m = 34655.36e-3; +L(1).r = [27.87 43.12 -89.03]*1e-3; +L(1).I = [ + 512052539.74 1361335.88 51305020.72 + 1361335.88 464074688.59 70335556.04 + 51305020.72 70335556.04 462745526.12]*1e-9; + +L(2).m = 15994.59e-3; +L(2).r = [ 198.29 9.73 92.43]*1e03; +L(2).I = [ + 94817914.40 -3859712.77 37932017.01 + -3859712.77 328604163.24 -1088970.86 + 37932017.01 -1088970.86 277463004.88]*1e-9; + +L(3).m = 20862.05e-3; +L(3).r = [ -4.56 -79.96 -5.86]; +L(3).I = [ + 500060915.95 -1863252.17 934875.78 + -1863252.17 75152670.69 -15204130.09 + 934875.78 -15204130.09 515424754.34]*1e-9; + +irb = SerialLink(L, 'name', 'IRB 140', ... + 'manufacturer', 'ABB', 'comment', 'modified DH'); + +% +% some useful poses +% +qz = [0 0 0 0 0 0]; % zero angles, L shaped pose + +clear L diff --git a/lwrserv/matlab/rvctools/robot/mdl_jaco.m b/lwrserv/matlab/rvctools/robot/mdl_jaco.m new file mode 100644 index 0000000..56b2d21 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_jaco.m @@ -0,0 +1,91 @@ +%MDL_JACO Create model of Kinova Jaco manipulator +% +% MDL_JACO is a script that creates the workspace variable jaco which +% describes the kinematic characteristics of a Kinova Jaco manipulator +% using standard DH conventions. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% +% Reference:: +% - "DH Parameters of Jaco" Version 1.0.8, July 25, 2013. +% +% Notes:: +% - SI units of metres are used. +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_mico, mdl_puma560. + +% MODEL: Kinova, Jaco, 6DOF, standard_DH + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function r = mdl_jaco() + + deg = pi/180; + + % robot length values (metres) + D1 = 0.2755; + D2 = 0.4100; + D3 = 0.2073; + D4 = 0.0743; + D5 = 0.0743; + D6 = 0.1687; + e2 = 0.0098; + + % alternate parameters + aa = 30*deg; + ca = cos(aa); + sa = sin(aa); + c2a = cos(2*aa); + s2a = sin(2*aa); + d4b = D3 + sa/s2a*D4; + d5b = sa/s2a*D4 + sa/s2a*D5; + d6b = sa/s2a*D5 + D6; + + % and build a serial link manipulator + + % offsets from the table on page 4, "Mico" angles are the passed joint + % angles. "DH Algo" are the result after adding the joint angle offset. + + robot = SerialLink([ + Revolute('alpha', pi/2, 'a', 0, 'd', D1, 'flip') + Revolute('alpha', pi, 'a', D2, 'd', 0, 'offset', -pi/2) + Revolute('alpha', pi/2, 'a', 0, 'd', -e2, 'offset', pi/2) + Revolute('alpha', 2*aa, 'a', 0, 'd', -d4b) + Revolute('alpha', 2*aa, 'a', 0, 'd', -d5b, 'offset', -pi) + Revolute('alpha', pi, 'a', 0, 'd', -d6b, 'offset', 100*deg) + ], ... + 'name', 'Jaco', 'manufacturer', 'Kinova'); + + + + % place the variables into the global workspace + if nargin == 1 + r = robot; + elseif nargin == 0 + assignin('base', 'jaco', robot); + assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles + assignin('base', 'qr', [270 180 180 0 0 0]*deg); % vertical pose as per Fig 2 + end +end diff --git a/lwrserv/matlab/rvctools/robot/mdl_m16.m b/lwrserv/matlab/rvctools/robot/mdl_m16.m new file mode 100644 index 0000000..98ccf26 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_m16.m @@ -0,0 +1,84 @@ +%MDL_M16 Create model of Fanuc M16 manipulator +% +% MDL_M16 is a script that creates the workspace variable mico which +% describes the kinematic characteristics of a Fanuc M16 manipulator using +% standard DH conventions. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qd lower arm horizontal as per data sheet +% +% Reference:: +% - "Fanuc M-16iB data sheet", http://www.robots.com/fanuc/m-16ib. +% - "Utilizing the Functional Work Space Evaluation Tool for Assessing a +% System Design and Reconfiguration Alternatives" +% A. Djuric and R. J. Urbanic +% +% Notes:: +% - SI units of metres are used. +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_irb140, mdl_fanuc10l, mdl_motomanHP6, mdl_S4ABB2p8, mdl_puma560. + + +% MODEL: Fanuc, M16, 6DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + +function r = mdl_m16() + + deg = pi/180; + + % robot length values (metres) + d1 = 0.524; + a1 = 0.150; + a2 = 0.770; + a3 = -0.100; + d4 = 0.740; + d6 = 0.100; + + % DH parameter table + % theta d a alpha + dh = [0 d1 a1 -pi/2 + 0 0 a2 pi + 0 0 a3 pi/2 + 0 d4 0 -pi/2 + 0 0 0 pi/2 + 0 d6 0 pi]; + + + % and build a serial link manipulator + + robot = SerialLink(dh, 'name', 'M16', ... + 'manufacturer', 'Fanuc'); + + % place the variables into the global workspace + if nargin == 1 + r = robot; + elseif nargin == 0 + assignin('base', 'm16', robot); + assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles + assignin('base', 'qd', [0 -90 0 0 -180 180]*deg); % data sheet pose, horizontal + assignin('base', 'qr', [0 -90 90 0 -180 180]*deg); % ready pose, arm up + end +end + diff --git a/lwrserv/matlab/rvctools/robot/mdl_mico.m b/lwrserv/matlab/rvctools/robot/mdl_mico.m new file mode 100644 index 0000000..7295bfc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_mico.m @@ -0,0 +1,105 @@ +%MDL_MICO Create model of Kinova Mico manipulator +% +% MDL_MICO is a script that creates the workspace variable mico which +% describes the kinematic characteristics of a Kinova Mico manipulator +% using standard DH conventions. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% +% Reference:: +% - "DH Parameters of Mico" Version 1.0.1, August 05, 2013. +% Kinova +% +% Notes:: +% - SI units of metres are used. +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, Revolute, mdl_jaco, mdl_puma560, mdl_twolink. + +% MODEL: Kinova, Mico, 6DOF, standard_DH + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function r = mdl_mico() + + deg = pi/180; + + % robot length values (metres) page 4 + D1 = 0.2755; + D2 = 0.2900; + D3 = 0.1233; + D4 = 0.0741; + D5 = 0.0741; + D6 = 0.1600; + e2 = 0.0070; + + % alternate parameters + aa = 30*deg; + ca = cos(aa); + sa = sin(aa); + c2a = cos(2*aa); + s2a = sin(2*aa); + d4b = D3 + sa/s2a*D4; + d5b = sa/s2a*D4 + sa/s2a*D5; + d6b = sa/s2a*D5 + D6; + + + % and build a serial link manipulator + + % offsets from the table on page 4, "Mico" angles are the passed joint + % angles. "DH Algo" are the result after adding the joint angle offset. + + robot = SerialLink([ + Revolute('alpha', pi/2, 'a', 0, 'd', D1, 'flip') + Revolute('alpha', pi, 'a', D2, 'd', 0, 'offset', -pi/2) + Revolute('alpha', pi/2, 'a', 0, 'd', -e2, 'offset', pi/2) + Revolute('alpha', 2*aa, 'a', 0, 'd', -d4b) + Revolute('alpha', 2*aa, 'a', 0, 'd', -d5b, 'offset', -pi) + Revolute('alpha', pi, 'a', 0, 'd', -d6b, 'offset', pi/2) + ], ... + 'name', 'Mico', 'manufacturer', 'Kinova'); + + %{ + % MDH version, no test yet + robot = SerialLink([ + Revolute('alpha', 0, 'a', 0, 'd', D1, 'modified', 'flip') + Revolute('alpha', -pi/2, 'a', 0, 'd', 0, 'modified', 'offset', -pi/2) + Revolute('alpha', 0, 'a', D2, 'd', e2, 'modified', 'offset', pi/2) + Revolute('alpha', -pi/2, 'a', 0, 'd', d4b, 'modified') + Revolute('alpha', 2*aa, 'a', 0, 'd', d5b, 'modified', 'offset', -pi) + Revolute('alpha', 2*aa, 'a', 0, 'd', d6b, 'modified', 'offset', pi/2) + ], ... + 'name', 'Mico', 'manufacturer', 'Kinova'); + %} + + % place the variables into the global workspace + if nargin == 1 + r = robot; + elseif nargin == 0 + assignin('base', 'mico', robot); + assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles, arm up + assignin('base', 'qr', [270 180 180 0 0 180]*deg); % vertical pose as per Fig 2 + end +end + diff --git a/lwrserv/matlab/rvctools/robot/mdl_nao.m b/lwrserv/matlab/rvctools/robot/mdl_nao.m new file mode 100644 index 0000000..a7613c8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_nao.m @@ -0,0 +1,138 @@ +%MDL_NAO Create model of Aldebaran NAO humanoid robot +% +% MDL_NAO is a script that creates several workspace variables +% +% leftarm left-arm kinematics (4DOF) +% rightarm right-arm kinematics (4DOF) +% leftleg left-leg kinematics (6DOF) +% rightleg right-leg kinematics (6DOF) +% +% which are each SerialLink objects that describe the kinematic +% characteristics of the arms and legs of the NAO humanoid. +% +% Reference:: +% - "Forward and Inverse Kinematics for the NAO Humanoid Robot", +% Nikolaos Kofinas, +% Thesis, Technical University of Crete +% July 2012. +% - "Mechatronic design of NAO humanoid" +% David Gouaillier etal. +% IROS 2009, pp. 769-774. +% +% Notes:: +% - SI units of metres are used. +% - The base transform of arms and legs are constant with respect to the +% torso frame, which is assumed to be the constant value when the robot +% is upright. Clearly if the robot is walking these base transforms +% will be dynamic. +% - The first reference uses Modified DH notation, but doesn't explicitly +% mention this, and the parameter tables have the wrong column headings +% for Modified DH parameters. +% - TODO; add joint limits +% - TODO; add dynamic parameters +% +% See also SerialLink, Revolute. + +% MODEL: Aldebaran, NAO, humanoid, 4DOF, standard_DH + + +% Copyright (C) 1993-2015, 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.petercorke.com + +deg = pi/180; + +% NAO constants (in mm) +NeckOffsetZ = 126.50; +ShoulderOffsetY = 98.00; +ElbowOffsetY = 15.00; +UpperArmLength = 105.00; +LowerArmLength = 55.95; +ShoulderOffsetZ = 100.00; +HandOffsetX = 57.75; +HipOffsetZ = 85.00; +HipOffsetY = 50.00; +ThighLength = 100.00; +TibiaLength = 102.90; +FootHeight = 45.19; +HandOffsetZ = 12.31; + +% set some default plot options, base and shadow are not useful for a multi-arm plot +plotopts = {'nobase', 'noshadow'}; + + +leftarm = SerialLink( [ + Revolute('d', 0, 'alpha', -pi/2, 'a', 0, 'modified') + Revolute('d', 0, 'alpha', pi/2, 'a', 0, 'offset', -pi/2, 'modified') + Revolute('d', UpperArmLength, 'alpha', -pi/2, 'a', 0, 'modified') + Revolute('d', 0, 'alpha', pi/2, 'a', 0, 'modified') + ], ... + 'base', transl(0, ShoulderOffsetY+ElbowOffsetY, ShoulderOffsetZ), ... + 'tool', trotz(pi/2)*transl(HandOffsetX+LowerArmLength, 0, 0), ... + 'plotopt', plotopts, ... + 'name', 'left arm', 'manufacturer', 'Aldabaran'); + +rightarm = SerialLink( [ + Revolute('d', 0, 'alpha', -pi/2, 'a', 0, 'modified') + Revolute('d', 0, 'alpha', pi/2, 'a', 0, 'offset', pi/2, 'modified') + Revolute('d', -UpperArmLength, 'alpha', -pi/2, 'a', 0, 'modified') + Revolute('d', 0, 'alpha', pi/2, 'a', 0, 'modified') + ], ... + 'base', transl(0, -ShoulderOffsetY-ElbowOffsetY, ShoulderOffsetZ), ... + 'tool', trotz(pi/2)*transl(-HandOffsetX-LowerArmLength, 0, 0)*trotz(-pi), ... + 'plotopt', plotopts, ... + 'name', 'right arm', 'manufacturer', 'Aldabaran'); + + +leftleg = SerialLink( [ + Revolute('d', 0, 'alpha', -3*pi/4, 'a', 0, 'offset', -pi/2, 'modified') + Revolute('d', 0, 'alpha', -pi/2, 'a', 0, 'offset', pi/4, 'modified') + Revolute('d', 0, 'alpha', pi/2, 'a', 0, 'modified') + Revolute('d', 0, 'alpha', 0, 'a', -ThighLength, 'modified') + Revolute('d', 0, 'alpha', 0, 'a', -TibiaLength, 'modified') + Revolute('d', 0, 'alpha', -pi/2, 'a', 0, 'modified') + ], ... + 'base', transl(0, HipOffsetY, -HipOffsetZ), ... + 'tool', trotz(pi)*troty(-pi/2)*transl(0, 0, -FootHeight), ... + 'plotopt', plotopts, ... + 'name', 'left leg', 'manufacturer', 'Aldabaran'); + +rightleg = SerialLink( [ + Revolute('d', 0, 'alpha', -pi/4, 'a', 0, 'offset', -pi/2, 'modified') + Revolute('d', 0, 'alpha', -pi/2, 'a', 0, 'offset', -pi/4, 'modified') + Revolute('d', 0, 'alpha', pi/2, 'a', 0, 'modified') + Revolute('d', 0, 'alpha', 0, 'a', -ThighLength, 'modified') + Revolute('d', 0, 'alpha', 0, 'a', -TibiaLength, 'modified') + Revolute('d', 0, 'alpha', -pi/2, 'a', 0, 'modified') + ], ... + 'base', transl(0, -HipOffsetY, -HipOffsetZ), ... + 'tool', trotz(pi)*troty(-pi/2)*transl(0, 0, -FootHeight), ... + 'plotopt', plotopts, ... + 'name', 'right leg', 'manufacturer', 'Aldabaran'); + + +% plot the legs and arms in a nominal sized workspace +clf +leftleg.plot([0 0 0 0 0 0], 'workspace', 400*[-1 1 -1 1 -1 1]); +hold on +rightleg.plot([0 0 0 0 0 0], 'workspace', 400*[-1 1 -1 1 -1 1]); +leftarm.plot([0 0 0 0], 'workspace', 400*[-1 1 -1 1 -1 1]); +rightarm.plot([0 0 0 0], 'workspace', 400*[-1 1 -1 1 -1 1]); + + + diff --git a/lwrserv/matlab/rvctools/robot/mdl_offset3.m b/lwrserv/matlab/rvctools/robot/mdl_offset3.m new file mode 100644 index 0000000..5bf9b4a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_offset3.m @@ -0,0 +1,64 @@ +%MDL_OFFSET3 A minimalistic 3DOF robot arm with shoulder offset +% +% MDL_OFFSET3 is a script that creates the workspace variable off3 which +% describes the kinematic characteristics of a simple arm manipulator with +% a shoulder offset, using standard DH conventions. +% +% Somewhat like a Puma arm without the wrist. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% +% Notes:: +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_offset6, mdl_simple6, mdl_puma560. + +% MODEL: generic, 3DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + +function r = mdl_offset3() + + % robot length values (metres) + L1 = 1; + L2 = 1; + O1 = 0; + O2 = 0.2; + O3 = 0; + + % and build a serial link manipulator + + robot = SerialLink([ + Revolute('alpha', -pi/2, 'a', O1, 'd', 0) + Revolute('alpha', 0, 'a', L1, 'd', O2) + Revolute('alpha', pi/2, 'a', L2, 'd', O3) + ], ... + 'name', 'Offset3'); + + % place the variables into the global workspace + if nargout == 1 + r = robot; + elseif nargout == 0 + assignin('base', 'off3', robot); + assignin('base', 'qz', [0 0 0 ]); % zero angles, arm up + end +end diff --git a/lwrserv/matlab/rvctools/robot/mdl_offset6.m b/lwrserv/matlab/rvctools/robot/mdl_offset6.m new file mode 100644 index 0000000..c27e0d0 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_offset6.m @@ -0,0 +1,65 @@ +%MDL_OFFSET6 A minimalistic 6DOF robot arm with shoulder offset +% +% MDL_OFFSET6 is a script that creates the workspace variable off6 which +% describes the kinematic characteristics of a simple arm manipulator with +% a spherical wrist and a shoulder offset, using standard DH conventions. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% +% Notes:: +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_simple6, mdl_puma560, mdl_twolink. + +% MODEL: generic, 6DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + +function r = mdl_offset6() + + % robot length values (metres) + L1 = 1; + L2 = 1; + O1 = 0; + O2 = 0.2; + O3 = 0; + + % and build a serial link manipulator + + robot = SerialLink([ + Revolute('alpha', -pi/2, 'a', O1, 'd', 0) + Revolute('alpha', 0, 'a', L1, 'd', O2) + Revolute('alpha', pi/2, 'a', L2, 'd', O3) + Revolute('alpha', pi/2, 'a', 0, 'd', 0) + Revolute('alpha', -pi/2, 'a', 0, 'd', 0) + Revolute('alpha', 0, 'a', 0, 'd', 0) + ], ... + 'name', 'Offset6'); + + % place the variables into the global workspace + if nargout == 1 + r = robot; + elseif nargout == 0 + assignin('base', 'off6', robot); + assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles, arm up + end +end diff --git a/lwrserv/matlab/rvctools/robot/mdl_onelink.m b/lwrserv/matlab/rvctools/robot/mdl_onelink.m new file mode 100644 index 0000000..2c87963 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_onelink.m @@ -0,0 +1,50 @@ +%MDL_ONELINK Create model of a simple 1-link mechanism +% +% MDL_ONELINK is a script that creates the workspace variable tl which +% describes the kinematic and dynamic characteristics of a simple planar +% 1-link mechanism. +% +% Also defines the vector: +% qz corresponds to the zero joint angle configuration. +% +% Notes:: +% - SI units are used. +% - It is a planar mechanism operating in the XY (horizontal) plane and is +% therefore not affected by gravity. +% - Assume unit length links with all mass (unity) concentrated at the joints. +% +% References:: +% - Based on Fig 3-6 (p73) of Spong and Vidyasagar (1st edition). +% +% See also SerialLink, mdl_twolink, mdl_planar1. + +% MODEL: generic, planar, 1DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + + +a1 = 1; + +onelink = SerialLink([ + Revolute('d', 0, 'a', a1, 'alpha', 0, 'standard') + ], ... + 'name', 'one link'); +qz = [0]; +qn = [pi/6]; diff --git a/lwrserv/matlab/rvctools/robot/mdl_p8.m b/lwrserv/matlab/rvctools/robot/mdl_p8.m new file mode 100644 index 0000000..37fab8f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_p8.m @@ -0,0 +1,44 @@ +%MDL_P8 Create model of Puma robot on an XY base +% +% mdl_p8 +% +% Script creates the workspace variable p8 which is an 8-axis robot +% comprising a Puma 560 robot on an XY base. Joints 1 and 2 are the base, +% joints 3-8 are the robot arm. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qstretch arm is stretched out in the X direction +% qn arm is at a nominal non-singular configuration +% +% See also SerialLink, mdl_puma560. + +% 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 . + +% create the base +platform = SerialLink( [0 0 0 -pi/2 1; -pi/2 0 0 pi/2 1], ... + 'base', troty(pi/2), 'name', 'platform' ); + +% load the standard Puma +mdl_puma560; +% add the pedestal height to link 1 +p560.links(1).d = 30 * 0.0254; + +% compose the two robots +p8 = SerialLink( [platform, p560], 'name', 'P8'); diff --git a/lwrserv/matlab/rvctools/robot/mdl_phantomx.m b/lwrserv/matlab/rvctools/robot/mdl_phantomx.m new file mode 100644 index 0000000..3e4d4a0 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_phantomx.m @@ -0,0 +1,35 @@ +%MDL_PHANTOMX Create model of PhantomX pincher manipulator +% +% mdl_phantomx +% +% Script creates the workspace variable px which describes the +% kinematic characteristics of a PhantomX Pincher Robot, a 4 joint hobby +% class manipulator by Trossen Robotics. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% +% Notes:: +% - the x-axis is forward, and the z-axis is upwards. +% - uses standard DH conventions. +% - Tool centrepoint is middle of the fingertips. +% - all translational units in mm. +% +% Reference:: +% +% - http://www.trossenrobotics.com/productdocs/assemblyguides/phantomx-basic-robot-arm.html + +L(1) = Revolute('d', 40, 'alpha', -pi/2); +L(2) = Revolute('a', -105, 'alpha', pi, 'offset', pi/2); +L(3) = Revolute('a', -105); +L(4) = Revolute('a', -105); + +% Note alpha_2 = pi, needed to account for rotation axes of joints 3 and 4 having +% opposite sign to joint 2. +% +% s='Rz(q1) Tz(L1) Ry(q2) Tz(L2) Ry(q3) Tz(L3) Ry(q4) Tz(L4)' +% DHFactor(s) + +px = SerialLink(L, 'name', 'PhantomX', 'manufacturer', 'Trossen Robotics'); +qz = [0 0 0 0]; +px.tool = trotz(-pi/2) * trotx(pi/2); diff --git a/lwrserv/matlab/rvctools/robot/mdl_planar1.m b/lwrserv/matlab/rvctools/robot/mdl_planar1.m new file mode 100644 index 0000000..6b0b5c9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_planar1.m @@ -0,0 +1,44 @@ +%MDL_PLANAR1 Create model of a simple planar 1-link mechanism +% +% MDL_PLANAR1 is a script that creates the workspace variable p1 which +% describes the kinematic characteristics of a simple planar 1-link +% mechanism. +% +% Also defines the vector: +% qz corresponds to the zero joint angle configuration. +% +% Notes:: +% - Moves in the XY plane. +% - No dynamics in this model. +% +% See also SerialLink, mdl_onelink, mdl_planar2, mdl_planar3. + +% MODEL: generic, planar, 1DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + + +a1 = 1; + +p1 = SerialLink([ + Revolute('d', 0, 'a', a1, 'alpha', 0, 'standard') + ], ... + 'name', 'one link'); +qz = [0]; diff --git a/lwrserv/matlab/rvctools/robot/mdl_planar2.m b/lwrserv/matlab/rvctools/robot/mdl_planar2.m new file mode 100644 index 0000000..5a3ceb1 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_planar2.m @@ -0,0 +1,50 @@ +%MDL_PLANAR2 Create model of a simple planar 2-link mechanism +% +% MDL_PLANAR2 is a script that creates the workspace variable p2 which +% describes the kinematic characteristics of a simple planar 2-link +% mechanism. +% +% Also defines the vector: +% qz corresponds to the zero joint angle configuration. +% +% Also defines the vector: +% qz corresponds to the zero joint angle configuration. +% +% Notes:: +% - Moves in the XY plane. +% - No dynamics in this model. +% +% See also SerialLink, mdl_twolink, mdl_planar1, mdl_planar3. + + +% MODEL: generic, planar, 2DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + + +a1 = 1; +a2 = 1; + +p2 = SerialLink([ + Revolute('d', 0, 'a', a1, 'alpha', 0, 'standard') + Revolute('d', 0, 'a', a2, 'alpha', 0, 'standard') + ], ... + 'name', 'two link'); +qz = [0 0]; diff --git a/lwrserv/matlab/rvctools/robot/mdl_planar3.m b/lwrserv/matlab/rvctools/robot/mdl_planar3.m new file mode 100644 index 0000000..363e2e7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_planar3.m @@ -0,0 +1,49 @@ +%MDL_PLANAR3 Create model of a simple planar 3-link mechanism +% +% MDL_PLANAR2 is a script that creates the workspace variable p3 which +% describes the kinematic characteristics of a simple redundant planar +% 3-link mechanism. +% +% Also defines the vector: +% qz corresponds to the zero joint angle configuration. +% +% +% Notes:: +% - Moves in the XY plane. +% - No dynamics in this model. +% +% See also SerialLink, mdl_twolink, mdl_planar1, mdl_planar2. + +% MODEL: generic, planar, 3DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + + +a1 = 1; +a2 = 1; +a3 = 1; + +p3 = SerialLink([ + Revolute('d', 0, 'a', a1, 'alpha', 0, 'standard') + Revolute('d', 0, 'a', a2, 'alpha', 0, 'standard') + Revolute('d', 0, 'a', a3, 'alpha', 0, 'standard') + ], ... + 'name', 'three link'); +qz = [0 0 0]; diff --git a/lwrserv/matlab/rvctools/robot/mdl_puma560.m b/lwrserv/matlab/rvctools/robot/mdl_puma560.m new file mode 100644 index 0000000..b581510 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_puma560.m @@ -0,0 +1,148 @@ +%MDL_PUMA560 Create model of Puma 560 manipulator +% +% mdl_puma560 +% +% Script creates the workspace variable p560 which describes the +% kinematic and dynamic characteristics of a Unimation Puma 560 manipulator +% using standard DH conventions. +% The model includes armature inertia and gear ratios. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qstretch arm is stretched out in the X direction +% qn arm is at a nominal non-singular configuration +% +% Reference:: +% - "A search for consensus among model parameters reported for the PUMA 560 robot", +% P. Corke and B. Armstrong-Helouvry, +% Proc. IEEE Int. Conf. Robotics and Automation, (San Diego), +% pp. 1608-1613, May 1994. +% +% See also SerialRevolute, mdl_puma560akb, mdl_stanford, mdl_twolink. + +% +% Notes: +% - the value of m1 is given as 0 here. Armstrong found no value for it +% and it does not appear in the equation for tau1 after the substituion +% is made to inertia about link frame rather than COG frame. +% updated: +% 2/8/95 changed D3 to 150.05mm which is closer to data from Lee, AKB86 and Tarn +% fixed errors in COG for links 2 and 3 +% 29/1/91 to agree with data from Armstrong etal. Due to their use +% of modified D&H params, some of the offsets Ai, Di are +% offset, and for links 3-5 swap Y and Z axes. +% 14/2/91 to use Paul's value of link twist (alpha) to be consistant +% with ARCL. This is the -ve of Lee's values, which means the +% zero angle position is a righty for Paul, and lefty for Lee. +% Note that gravity load torque is the motor torque necessary +% to keep the joint static, and is thus -ve of the gravity +% caused torque. +% +% 8/95 fix bugs in COG data for Puma 560. This led to signficant errors in +% inertia of joint 1. +% $Log: not supported by cvs2svn $ +% Revision 1.4 2008/04/27 11:36:54 cor134 +% Add nominal (non singular) pose qn + + +% 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 . + +clear L +deg = pi/180; + +% joint angle limits from +% A combined optimization method for solving the inverse kinematics problem... +% Wang & Chen +% IEEE Trans. RA 7(4) 1991 pp 489- +L(1) = Revolute('d', 0, 'a', 0, 'alpha', pi/2, ... + 'I', [0, 0.35, 0, 0, 0, 0], ... + 'r', [0, 0, 0], ... + 'm', 0, ... + 'Jm', 200e-6, ... + 'G', -62.6111, ... + 'B', 1.48e-3, ... + 'Tc', [0.395 -0.435], ... + 'qlim', [-160 160]*deg ); + +L(2) = Revolute('d', 0, 'a', 0.4318, 'alpha', 0, ... + 'I', [0.13, 0.524, 0.539, 0, 0, 0], ... + 'r', [-0.3638, 0.006, 0.2275], ... + 'm', 17.4, ... + 'Jm', 200e-6, ... + 'G', 107.815, ... + 'B', .817e-3, ... + 'Tc', [0.126 -0.071], ... + 'qlim', [-45 225]*deg ); + +L(3) = Revolute('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2, ... + 'I', [0.066, 0.086, 0.0125, 0, 0, 0], ... + 'r', [-0.0203, -0.0141, 0.070], ... + 'm', 4.8, ... + 'Jm', 200e-6, ... + 'G', -53.7063, ... + 'B', 1.38e-3, ... + 'Tc', [0.132, -0.105], ... + 'qlim', [-225 45]*deg ); + +L(4) = Revolute('d', 0.4318, 'a', 0, 'alpha', pi/2, ... + 'I', [1.8e-3, 1.3e-3, 1.8e-3, 0, 0, 0], ... + 'r', [0, 0.019, 0], ... + 'm', 0.82, ... + 'Jm', 33e-6, ... + 'G', 76.0364, ... + 'B', 71.2e-6, ... + 'Tc', [11.2e-3, -16.9e-3], ... + 'qlim', [-110 170]*deg); + +L(5) = Revolute('d', 0, 'a', 0, 'alpha', -pi/2, ... + 'I', [0.3e-3, 0.4e-3, 0.3e-3, 0, 0, 0], ... + 'r', [0, 0, 0], ... + 'm', 0.34, ... + 'Jm', 33e-6, ... + 'G', 71.923, ... + 'B', 82.6e-6, ... + 'Tc', [9.26e-3, -14.5e-3], ... + 'qlim', [-100 100]*deg ); + + +L(6) = Revolute('d', 0, 'a', 0, 'alpha', 0, ... + 'I', [0.15e-3, 0.15e-3, 0.04e-3, 0, 0, 0], ... + 'r', [0, 0, 0.032], ... + 'm', 0.09, ... + 'Jm', 33e-6, ... + 'G', 76.686, ... + 'B', 36.7e-6, ... + 'Tc', [3.96e-3, -10.5e-3], ... + 'qlim', [-266 266]*deg ); + +p560 = SerialLink(L, 'name', 'Puma 560', ... + 'manufacturer', 'Unimation', 'comment', 'viscous friction; params of 8/95'); + + + +% +% some useful poses +% +qz = [0 0 0 0 0 0]; % zero angles, L shaped pose +qr = [0 pi/2 -pi/2 0 0 0]; % ready pose, arm up +qs = [0 0 -pi/2 0 0 0]; +qn=[0 pi/4 pi 0 pi/4 0]; + + +clear L diff --git a/lwrserv/matlab/rvctools/robot/mdl_puma560_3.m b/lwrserv/matlab/rvctools/robot/mdl_puma560_3.m new file mode 100644 index 0000000..7fae7de --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_puma560_3.m @@ -0,0 +1,107 @@ +%MDL_PUMA560 Create model of Puma 560 manipulator +% +% mdl_puma560_3 +% +% Script creates the workspace variable p560 which describes the +% kinematic and dynamic characteristics of a Unimation Puma 560 manipulator +% using standard DH conventions. +% The model includes armature inertia and gear ratios. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qstretch arm is stretched out in the X direction +% qn arm is at a nominal non-singular configuration +% +% Reference:: +% - "A search for consensus among model parameters reported for the PUMA 560 robot", +% P. Corke and B. Armstrong-Helouvry, +% Proc. IEEE Int. Conf. Robotics and Automation, (San Diego), +% pp. 1608-1613, May 1994. +% +% See also SerialLink, mdl_puma560akb, mdl_stanford, mdl_twolink. + +% +% Notes: +% - the value of m1 is given as 0 here. Armstrong found no value for it +% and it does not appear in the equation for tau1 after the substituion +% is made to inertia about link frame rather than COG frame. +% updated: +% 2/8/95 changed D3 to 150.05mm which is closer to data from Lee, AKB86 and Tarn +% fixed errors in COG for links 2 and 3 +% 29/1/91 to agree with data from Armstrong etal. Due to their use +% of modified D&H params, some of the offsets Ai, Di are +% offset, and for links 3-5 swap Y and Z axes. +% 14/2/91 to use Paul's value of link twist (alpha) to be consistant +% with ARCL. This is the -ve of Lee's values, which means the +% zero angle position is a righty for Paul, and lefty for Lee. +% Note that gravity load torque is the motor torque necessary +% to keep the joint static, and is thus -ve of the gravity +% caused torque. +% +% 8/95 fix bugs in COG data for Puma 560. This led to signficant errors in +% inertia of joint 1. +% $Log: not supported by cvs2svn $ +% Revision 1.4 2008/04/27 11:36:54 cor134 +% Add nominal (non singular) pose qn + + +% 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 . + +clear L + +L(1) = Link('d', 0, 'a', 0, 'alpha', pi/2, ... + 'I', [0, 0.35, 0, 0, 0, 0], ... + 'r', [0, 0, 0], ... + 'm', 0, ... + 'Jm', 200e-6, ... + 'G', -62.6111, ... + 'B', 1.48e-3, ... + 'Tc', [0.395 -0.435] ); + +L(2) = Link('d', 0, 'a', 0.4318, 'alpha', 0, ... + 'I', [0.13, 0.524, 0.539, 0, 0, 0], ... + 'r', [-0.3638, 0.006, 0.2275], ... + 'm', 17.4, ... + 'Jm', 200e-6, ... + 'G', 107.815, ... + 'B', .817e-3, ... + 'Tc', [0.126 -0.071] ); + +L(3) = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2, ... + 'I', [0.066, 0.086, 0.0125, 0, 0, 0], ... + 'r', [-0.0203, -0.0141, 0.070], ... + 'm', 4.8, ... + 'Jm', 200e-6, ... + 'G', -53.7063, ... + 'B', 1.38e-3, ... + 'Tc', [0.132, -0.105] ); + +p560 = SerialLink(L, 'name', 'Puma 560 3 axis', ... + 'manufacturer', 'Unimation', 'comment', 'viscous friction; params of 8/95'); + +% +% some, useful, poses +% +qz = [0, 0, 0, ]; % zero angles, L, shaped, pose +qr = [0, pi/2, -pi/2]; % ready pose, arm, up +qs = [0, 0, -pi/2]; +qn=[0, pi/4, pi]; + + +clear L diff --git a/lwrserv/matlab/rvctools/robot/mdl_puma560_3_sym.m b/lwrserv/matlab/rvctools/robot/mdl_puma560_3_sym.m new file mode 100644 index 0000000..d65f2ce --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_puma560_3_sym.m @@ -0,0 +1,139 @@ +%MDL_PUMA560 Create model of Puma 560 manipulator +% +% mdl_puma560 +% +% Script creates the workspace variable p560 which describes the +% kinematic and dynamic characteristics of a Unimation Puma 560 manipulator +% using standard DH conventions. +% The model includes armature inertia and gear ratios. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qstretch arm is stretched out in the X direction +% qn arm is at a nominal non-singular configuration +% +% Reference:: +% - "A search for consensus among model parameters reported for the PUMA 560 robot", +% P. Corke and B. Armstrong-Helouvry, +% Proc. IEEE Int. Conf. Robotics and Automation, (San Diego), +% pp. 1608-1613, May 1994. +% +% See also SerialLink, mdl_puma560akb, mdl_stanford, mdl_twolink. + +% +% Notes: +% - the value of m1 is given as 0 here. Armstrong found no value for it +% and it does not appear in the equation for tau1 after the substituion +% is made to inertia about link frame rather than COG frame. +% updated: +% 2/8/95 changed D3 to 150.05mm which is closer to data from Lee, AKB86 and Tarn +% fixed errors in COG for links 2 and 3 +% 29/1/91 to agree with data from Armstrong etal. Due to their use +% of modified D&H params, some of the offsets Ai, Di are +% offset, and for links 3-5 swap Y and Z axes. +% 14/2/91 to use Paul's value of link twist (alpha) to be consistant +% with ARCL. This is the -ve of Lee's values, which means the +% zero angle position is a righty for Paul, and lefty for Lee. +% Note that gravity load torque is the motor torque necessary +% to keep the joint static, and is thus -ve of the gravity +% caused torque. +% +% 8/95 fix bugs in COG data for Puma 560. This led to signficant errors in +% inertia of joint 1. +% $Log: not supported by cvs2svn $ +% Revision 1.4 2008/04/27 11:36:54 cor134 +% Add nominal (non singular) pose qn + + +% 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 . + +clear L +L(1) = Link('d', 0, 'a', 0, 'alpha', pi/2, ... + 'I', [0, 0.35, 0, 0, 0, 0], ... + 'r', [0, 0, 0], ... + 'm', 0, ... + 'Jm', 200e-6, ... + 'G', -62.6111, ... + 'B', 1.48e-3, ... + 'Tc', [0.395 -0.435],... + 'sym'); + +L(2) = Link('d', 0, 'a', 0.4318, 'alpha', 0, ... + 'I', [0.13, 0.524, 0.539, 0, 0, 0], ... + 'r', [-0.3638, 0.006, 0.2275], ... + 'm', 17.4, ... + 'Jm', 200e-6, ... + 'G', 107.815, ... + 'B', .817e-3, ... + 'Tc', [0.126 -0.071],... + 'sym'); + +L(3) = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2, ... + 'I', [0.066, 0.086, 0.0125, 0, 0, 0], ... + 'r', [-0.0203, -0.0141, 0.070], ... + 'm', 4.8, ... + 'Jm', 200e-6, ... + 'G', -53.7063, ... + 'B', 1.38e-3, ... + 'Tc', [0.132, -0.105],... + 'sym'); + +% L(4) = Link('d', 0.4318, 'a', 0, 'alpha', pi/2, ... +% 'I', [1.8e-3, 1.3e-3, 1.8e-3, 0, 0, 0], ... +% 'r', [0, 0.019, 0], ... +% 'm', 0.82, ... +% 'Jm', 33e-6, ... +% 'G', 76.0364, ... +% 'B', 71.2e-6, ... +% 'Tc', [11.2e-3, -16.9e-3] ); +% +% L(5) = Link('d', 0, 'a', 0, 'alpha', -pi/2, ... +% 'I', [0.3e-3, 0.4e-3, 0.3e-3, 0, 0, 0], ... +% 'r', [0, 0, 0], ... +% 'm', 0.34, ... +% 'Jm', 33e-6, ... +% 'G', 71.923, ... +% 'B', 82.6e-6, ... +% 'Tc', [9.26e-3, -14.5e-3] ); +% +% +% L(6) = Link('d', 0, 'a', 0, 'alpha', 0, ... +% 'I', [0.15e-3, 0.15e-3, 0.04e-3, 0, 0, 0], ... +% 'r', [0, 0, 0.032], ... +% 'm', 0.09, ... +% 'Jm', 33e-6, ... +% 'G', 76.686, ... +% 'B', 36.7e-6, ... +% 'Tc', [3.96e-3, -10.5e-3] ); + +p560 = SerialLink(L, 'name', 'Puma 560', ... + 'manufacturer', 'Unimation', 'comment', 'viscous friction; params of 8/95'); + + + +% +% some useful poses +% +qz = [0 0 0 0 0 0]; % zero angles, L shaped pose +qr = [0 pi/2 -pi/2 0 0 0]; % ready pose, arm up +qs = [0 0 -pi/2 0 0 0]; +qn=[0 pi/4 pi 0 pi/4 0]; + + +clear L diff --git a/lwrserv/matlab/rvctools/robot/mdl_puma560akb.m b/lwrserv/matlab/rvctools/robot/mdl_puma560akb.m new file mode 100644 index 0000000..b182802 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_puma560akb.m @@ -0,0 +1,100 @@ +%MDL_PUMA560AKB Create model of Puma 560 manipulator +% +% mdl_puma560akb +% +% Script creates the workspace variable p560m which describes the +% kinematic and dynamic characterstics of a Unimation Puma 560 manipulator +% modified DH conventions. +% +% Also defines the workspace vectors: +% qz zero joint angle configuration +% qr vertical 'READY' configuration +% qstretch arm is stretched out in the X direction +% +% References:: +% - "The Explicit Dynamic Model and Inertial Parameters of the Puma 560 Arm" +% Armstrong, Khatib and Burdick +% 1986 +% +% See also SerialLink, mdl_puma560, mdl_stanford, mdl_twolink. + + +% 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 . + +clear L +% theta d a alpha +L(1) = Link([ 0 0 0 0 0], 'mod'); +L(2) = Link([ 0 0.2435 0 -pi/2 0], 'mod'); +L(3) = Link([ 0 -0.0934 0.4318 0 0], 'mod'); +L(4) = Link([ 0 0.4331 -0.0203 pi/2 0], 'mod'); +L(5) = Link([ 0 0 0 -pi/2 0], 'mod'); +L(6) = Link([ 0 0 0 pi/2 0], 'mod'); + + +L(1).m = 0; +L(2).m = 17.4; +L(3).m = 4.8; +L(4).m = 0.82; +L(5).m = 0.34; +L(6).m = .09; + +% rx ry rz +L(1).r = [0 0 0 ]; +L(2).r = [0.068 0.006 -0.016]; +L(3).r = [0 -0.070 0.014 ]; +L(4).r = [0 0 -0.019]; +L(5).r = [0 0 0 ]; +L(6).r = [0 0 .032 ]; + +% Ixx Iyy Izz Ixy Iyz Ixz +L(1).I = [0 0 0.35 0 0 0]; +L(2).I = [.13 .524 .539 0 0 0]; +L(3).I = [.066 .0125 .066 0 0 0]; +L(4).I = [1.8e-3 1.8e-3 1.3e-3 0 0 0]; +L(5).I = [.3e-3 .3e-3 .4e-3 0 0 0]; +L(6).I = [.15e-3 .15e-3 .04e-3 0 0 0]; + +L(1).Jm = 291e-6; +L(2).Jm = 409e-6; +L(3).Jm = 299e-6; +L(4).Jm = 35e-6; +L(5).Jm = 35e-6; +L(6).Jm = 35e-6; + +L(1).G = -62.6111; +L(2).G = 107.815; +L(3).G = -53.7063; +L(4).G = 76.0364; +L(5).G = 71.923; +L(6).G = 76.686; + +% viscous friction (motor referenced) +% unknown + +% Coulomb friction (motor referenced) +% unknown + +% +% some useful poses +% +qz = [0 0 0 0 0 0]; % zero angles, L shaped pose +qr = [0 -pi/2 pi/2 0 0 0]; % ready pose, arm up +qstretch = [0 0 pi/2 0 0 0]; % horizontal along x-axis + +p560m = SerialLink(L, 'name', 'Puma560-AKB', 'manufacturer', 'Unimation', 'comment', 'AK&B'); +clear L diff --git a/lwrserv/matlab/rvctools/robot/mdl_quadcopter.m b/lwrserv/matlab/rvctools/robot/mdl_quadcopter.m new file mode 100644 index 0000000..f595702 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_quadcopter.m @@ -0,0 +1,112 @@ +%MDL_QUADCOPTER Dynamic parameters for a quadcopter. +% +% mdl_quadcopter +% +% Script creates the workspace variable quad which describes the +% dynamic characterstics of a quadcopter. +% +% Properties:: +% +% This is a structure with the following elements: +% +% J Flyer rotational inertia matrix (3x3) +% h Height of rotors above CoG (1x1) +% d Length of flyer arms (1x1) +% nb Number of blades per rotor (1x1) +% r Rotor radius (1x1) +% c Blade chord (1x1) +% e Flapping hinge offset (1x1) +% Mb Rotor blade mass (1x1) +% Mc Estimated hub clamp mass (1x1) +% ec Blade root clamp displacement (1x1) +% Ib Rotor blade rotational inertia (1x1) +% Ic Estimated root clamp inertia (1x1) +% mb Static blade moment (1x1) +% Ir Total rotor inertia (1x1) +% Ct Non-dim. thrust coefficient (1x1) +% Cq Non-dim. torque coefficient (1x1) +% sigma Rotor solidity ratio (1x1) +% thetat Blade tip angle (1x1) +% theta0 Blade root angle (1x1) +% theta1 Blade twist angle (1x1) +% theta75 3/4 blade angle (1x1) +% thetai Blade ideal root approximation (1x1) +% a Lift slope gradient (1x1) +% A Rotor disc area (1x1) +% gamma Lock number (1x1) +% +% +% References:: +% - Design, Construction and Control of a Large Quadrotor micro air vehicle. +% P.Pounds, PhD thesis, +% Australian National University, 2007. +% http://www.eng.yale.edu/pep5/P_Pounds_Thesis_2008.pdf +% +% See also sl_quadcopter. + +% 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 . + +quad.g = 9.81; % g Gravity 1x1 +quad.rho = 1.184; % rho Density of air 1x1 +quad.muv = 1.5e-5; % muv Viscosity of air 1x1 + +% Airframe +quad.M = 4; % M Mass 1x1 +Ixx = 0.082; +Iyy = 0.082; +Izz = 0.149;%0.160; +quad.J = diag([Ixx Iyy Izz]); % I Flyer rotational inertia matrix 3x3 + +quad.h = -0.007; % h Height of rotors above CoG 1x1 +quad.d = 0.315; % d Length of flyer arms 1x1 + +%Rotor +quad.nb = 2; % b Number of blades per rotor 1x1 +quad.r = 0.165; % r Rotor radius 1x1 + +quad.c = 0.018; % c Blade chord 1x1 + +quad.e = 0.0; % e Flapping hinge offset 1x1 +quad.Mb = 0.005; % Mb Rotor blade mass 1x1 +quad.Mc = 0.010; % Mc Estimated hub clamp mass 1x1 +quad.ec = 0.004; % ec Blade root clamp displacement 1x1 +quad.Ib = quad.Mb*(quad.r-quad.ec)^2/4 ; % Ib Rotor blade rotational inertia 1x1 +quad.Ic = quad.Mc*(quad.ec)^2/4; % Ic Estimated root clamp inertia 1x1 +quad.mb = quad.g*(quad.Mc*quad.ec/2+quad.Mb*quad.r/2); % mb Static blade moment 1x1 +quad.Ir = quad.nb*(quad.Ib+quad.Ic); % Ir Total rotor inertia 1x1 + +quad.Ct = 0.0048; % Ct Non-dim. thrust coefficient 1x1 +quad.Cq = quad.Ct*sqrt(quad.Ct/2); % Cq Non-dim. torque coefficient 1x1 + +quad.sigma = quad.c*quad.nb/(pi*quad.r); % sigma Rotor solidity ratio 1x1 +quad.thetat = 6.8*(pi/180); % thetat Blade tip angle 1x1 +quad.theta0 = 14.6*(pi/180); % theta0 Blade root angle 1x1 +quad.theta1 = quad.thetat - quad.theta0; % theta1 Blade twist angle 1x1 +quad.theta75 = quad.theta0 + 0.75*quad.theta1;% theta76 3/4 blade angle 1x1 +quad.thetai = quad.thetat*(quad.r/quad.e); % thetai Blade ideal root approximation 1x1 +quad.a = 5.5; % a Lift slope gradient 1x1 + +% derived constants +quad.A = pi*quad.r^2; % A Rotor disc area 1x1 +quad.gamma = quad.rho*quad.a*quad.c*quad.r^4/(quad.Ib+quad.Ic);% gamma Lock number 1x1 + +quad.b = quad.Ct*quad.rho*quad.A*quad.r^2; % T = b w^2 +quad.k = quad.Cq*quad.rho*quad.A*quad.r^3; % Q = k w^2 + +quad.verbose = false; + diff --git a/lwrserv/matlab/rvctools/robot/mdl_quadrotor.m b/lwrserv/matlab/rvctools/robot/mdl_quadrotor.m new file mode 100644 index 0000000..1099feb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_quadrotor.m @@ -0,0 +1,120 @@ +%MDL_QUADCOPTER Dynamic parameters for a quadrotor. +% +% MDL_QUADCOPTER is a script creates the workspace variable quad which +% describes the dynamic characterstics of a quadrotor flying robot. +% +% Properties:: +% +% This is a structure with the following elements: +% +% nrotors Number of rotors (1x1) +% J Flyer rotational inertia matrix (3x3) +% h Height of rotors above CoG (1x1) +% d Length of flyer arms (1x1) +% nb Number of blades per rotor (1x1) +% r Rotor radius (1x1) +% c Blade chord (1x1) +% e Flapping hinge offset (1x1) +% Mb Rotor blade mass (1x1) +% Mc Estimated hub clamp mass (1x1) +% ec Blade root clamp displacement (1x1) +% Ib Rotor blade rotational inertia (1x1) +% Ic Estimated root clamp inertia (1x1) +% mb Static blade moment (1x1) +% Ir Total rotor inertia (1x1) +% Ct Non-dim. thrust coefficient (1x1) +% Cq Non-dim. torque coefficient (1x1) +% sigma Rotor solidity ratio (1x1) +% thetat Blade tip angle (1x1) +% theta0 Blade root angle (1x1) +% theta1 Blade twist angle (1x1) +% theta75 3/4 blade angle (1x1) +% thetai Blade ideal root approximation (1x1) +% a Lift slope gradient (1x1) +% A Rotor disc area (1x1) +% gamma Lock number (1x1) +% +% +% Notes:: +% - SI units are used. +% +% References:: +% - Design, Construction and Control of a Large Quadrotor micro air vehicle. +% P.Pounds, PhD thesis, +% Australian National University, 2007. +% http://www.eng.yale.edu/pep5/P_Pounds_Thesis_2008.pdf +% - This is a heavy lift quadrotor +% +% See also sl_quadrotor. + +% MODEL: quadrotor + +% Copyright (C) 1993-2015, 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.petercorke.com + +quad.nrotors = 4; % 4 rotors +quad.g = 9.81; % g Gravity 1x1 +quad.rho = 1.184; % rho Density of air 1x1 +quad.muv = 1.5e-5; % muv Viscosity of air 1x1 + +% Airframe +quad.M = 4; % M Mass 1x1 +Ixx = 0.082; +Iyy = 0.082; +Izz = 0.149;%0.160; +quad.J = diag([Ixx Iyy Izz]); % I Flyer rotational inertia matrix 3x3 + +quad.h = -0.007; % h Height of rotors above CoG 1x1 +quad.d = 0.315; % d Length of flyer arms 1x1 + +%Rotor +quad.nb = 2; % b Number of blades per rotor 1x1 +quad.r = 0.165; % r Rotor radius 1x1 + +quad.c = 0.018; % c Blade chord 1x1 + +quad.e = 0.0; % e Flapping hinge offset 1x1 +quad.Mb = 0.005; % Mb Rotor blade mass 1x1 +quad.Mc = 0.010; % Mc Estimated hub clamp mass 1x1 +quad.ec = 0.004; % ec Blade root clamp displacement 1x1 +quad.Ib = quad.Mb*(quad.r-quad.ec)^2/4 ; % Ib Rotor blade rotational inertia 1x1 +quad.Ic = quad.Mc*(quad.ec)^2/4; % Ic Estimated root clamp inertia 1x1 +quad.mb = quad.g*(quad.Mc*quad.ec/2+quad.Mb*quad.r/2); % mb Static blade moment 1x1 +quad.Ir = quad.nb*(quad.Ib+quad.Ic); % Ir Total rotor inertia 1x1 + +quad.Ct = 0.0048; % Ct Non-dim. thrust coefficient 1x1 +quad.Cq = quad.Ct*sqrt(quad.Ct/2); % Cq Non-dim. torque coefficient 1x1 + +quad.sigma = quad.c*quad.nb/(pi*quad.r); % sigma Rotor solidity ratio 1x1 +quad.thetat = 6.8*(pi/180); % thetat Blade tip angle 1x1 +quad.theta0 = 14.6*(pi/180); % theta0 Blade root angle 1x1 +quad.theta1 = quad.thetat - quad.theta0; % theta1 Blade twist angle 1x1 +quad.theta75 = quad.theta0 + 0.75*quad.theta1;% theta76 3/4 blade angle 1x1 +quad.thetai = quad.thetat*(quad.r/quad.e); % thetai Blade ideal root approximation 1x1 +quad.a = 5.5; % a Lift slope gradient 1x1 + +% derived constants +quad.A = pi*quad.r^2; % A Rotor disc area 1x1 +quad.gamma = quad.rho*quad.a*quad.c*quad.r^4/(quad.Ib+quad.Ic);% gamma Lock number 1x1 + +quad.b = quad.Ct*quad.rho*quad.A*quad.r^2; % T = b w^2 +quad.k = quad.Cq*quad.rho*quad.A*quad.r^3; % Q = k w^2 + +quad.verbose = false; + diff --git a/lwrserv/matlab/rvctools/robot/mdl_simple6.m b/lwrserv/matlab/rvctools/robot/mdl_simple6.m new file mode 100644 index 0000000..777bf78 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_simple6.m @@ -0,0 +1,62 @@ +%MDL_SIMPLE6 A minimalistic 6DOF robot arm +% +% MDL_SIMPLE6 is a script creates the workspace variable s6 which describes +% the kinematic characteristics of a simple arm manipulator with a +% spherical wrist and no shoulder offset, using standard DH conventions. +% +% Also define the workspace vectors: +% qz zero joint angle configuration +% +% Notes:: +% - Unlike most other mdl_xxx scripts this one is actually a function that +% behaves like a script and writes to the global workspace. +% +% See also SerialLink, mdl_offset6, mdl_puma560. + +% MODEL: generic, 6DOF, standard_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + +function r = mdl_simple6() + + % robot length values (metres) + L1 = 1; + L2 = 1; + + % and build a serial link manipulator + + robot = SerialLink([ + Revolute('alpha', pi/2, 'a', 0, 'd', 0) + Revolute('alpha', 0, 'a', L1, 'd', 0) + Revolute('alpha', -pi/2, 'a', L2, 'd', 0) + Revolute('alpha', -pi/2, 'a', 0, 'd', 0) + Revolute('alpha', pi/2, 'a', 0, 'd', 0) + Revolute('alpha', 0, 'a', 0, 'd', 0) + ], ... + 'name', 'Simple6'); + + % place the variables into the global workspace + if nargout == 1 + r = robot; + elseif nargout == 0 + assignin('base', 's6', robot); + assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles, arm up + end +end diff --git a/lwrserv/matlab/rvctools/robot/mdl_stanford.m b/lwrserv/matlab/rvctools/robot/mdl_stanford.m new file mode 100644 index 0000000..8ebb40a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_stanford.m @@ -0,0 +1,88 @@ +%MDL_STANFORD Create model of Stanford arm +% +% mdl_stanford +% +% Script creates the workspace variable stanf which describes the +% kinematic and dynamic characteristics of the Stanford (Scheinman) arm. +% +% Also defines the vectors: +% qz zero joint angle configuration. +% +% Note:: +% - Gear ratios not currently known, though reflected armature inertia +% is known, so gear ratios are set to 1. +% +% References:: +% - Kinematic data from "Modelling, Trajectory calculation and Servoing of +% a computer controlled arm". Stanford AIM-177. Figure 2.3 +% - Dynamic data from "Robot manipulators: mathematics, programming and control" +% Paul 1981, Tables 6.4, 6.6 +% +% See also SerialLink, mdl_puma560, mdl_puma560akb, mdl_twolink. + + +% 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 . + +clear L +% th d a alpha +L(1) = Link([ 0 0.412 0 -pi/2 0]); +L(2) = Link([ 0 0.154 0 pi/2 0]); +L(3) = Link([ -pi/2 0 0 0 1]); +L(4) = Link([ 0 0 0 -pi/2 0]); +L(5) = Link([ 0 0 0 pi/2 0]); +L(6) = Link([ 0 0.263 0 0 0]); + +L(1).m = 9.29; +L(2).m = 5.01; +L(3).m = 4.25; +L(4).m = 1.08; +L(5).m = 0.630; +L(6).m = 0.51; + +L(1).Jm = 0.953; +L(2).Jm = 2.193; +L(3).Jm = 0.782; +L(4).Jm = 0.106; +L(5).Jm = 0.097; +L(6).Jm = 0.020; + +L(1).G = 1; +L(2).G = 1; +L(3).G = 1; +L(4).G = 1; +L(5).G = 1; +L(6).G = 1; + +L(1).I = [0.276 0.255 0.071 0 0 0]; +L(2).I = [0.108 0.018 0.100 0 0 0]; +L(3).I = [2.51 2.51 0.006 0 0 0 ]; +L(4).I = [0.002 0.001 0.001 0 0 0 ]; +L(5).I = [0.003 0.0004 0 0 0 0]; +L(6).I = [0.013 0.013 0.0003 0 0 0]; + +L(1).r = [0 0.0175 -0.1105]; +L(2).r = [0 -1.054 0]; +L(3).r = [0 0 -6.447]; +L(4).r = [0 0.092 -0.054]; +L(5).r = [0 0.566 0.003]; +L(6).r = [0 0 1.554]; + +qz = [0 0 0 0 0 0]; + +stanf = SerialLink(L, 'name', 'Stanford arm'); +stanf.plotopt = {'workspace', [-2 2 -2 2 -2 2]}; diff --git a/lwrserv/matlab/rvctools/robot/mdl_stanford_mdh.m b/lwrserv/matlab/rvctools/robot/mdl_stanford_mdh.m new file mode 100644 index 0000000..17653d7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_stanford_mdh.m @@ -0,0 +1,60 @@ +%MDL_STANFORD_MDH Create model of Stanford arm using MDH conventions +% +% mdl_stanford_mdh +% +% Script creates the workspace variable stanf which describes the +% kinematic and dynamic characteristics of the Stanford (Scheinman) arm +% using modified Denavit-Hartenberg parameters. +% +% Also defines the vectors: +% qz zero joint angle configuration. +% +% Notes:: +% - SI units are used. +% +% References:: +% - Kinematic data from "Modelling, Trajectory calculation and Servoing of +% a computer controlled arm". Stanford AIM-177. Figure 2.3 +% - Dynamic data from "Robot manipulators: mathematics, programming and control" +% Paul 1981, Tables 6.5, 6.6 +% +% See also SerialLink, mdl_puma560, mdl_puma560akb. + + +% MODEL: Stanford, Stanford arm, prismatic, 6DOF, modified_DH + + +% Copyright (C) 1993-2015, 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.petercorke.com + +clear L + +qz = [0 0 0 0 0 0]; + +stanf = SerialLink([ + RevoluteMDH('d', 0.412) + RevoluteMDH('d', 0.154, 'alpha', -pi/2) + PrismaticMDH('alpha', pi/2, 'qlim', [0.2032 0.9144]) + RevoluteMDH() + RevoluteMDH('alpha', -pi/2) + RevoluteMDH('d', 0.263, 'alpha', pi/2) + ], ... + 'name', 'Stanford arm MDH', ... + 'plotopt', {'workspace', [-2 2 -2 2 -2 2]} ... + ); diff --git a/lwrserv/matlab/rvctools/robot/mdl_twolink.m b/lwrserv/matlab/rvctools/robot/mdl_twolink.m new file mode 100644 index 0000000..6ce2755 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_twolink.m @@ -0,0 +1,65 @@ +%MDL_TWOLINK Create model of a simple 2-link mechanism +% +% mdl_twolink +% +% Script creates the workspace variable tl which describes the +% kinematic and dynamic characteristics of a simple planar 2-link mechanism. +% +% Also defines the vector: +% qz corresponds to the zero joint angle configuration. +% +% Notes:: +% - It is a planar mechanism operating in the XY (horizontal) plane and is +% therefore not affected by gravity. +% - Assume unit length links with all mass (unity) concentrated at the joints. +% +% References:: +% - Based on Fig 3-6 (p73) of Spong and Vidyasagar (1st edition). +% +% See also SerialLink, mdl_puma560, mdl_stanford. + + +% 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 . + +twolink_dh = [ +% theta d a alpha a sigma m rx ry rz Ixx Iyy Izz Ixy Iyz Ixz Jm G + 0 0 1 0 0 1 1 0 0 0 0 0 0 0 0 0 1 + 0 0 1 0 0 1 1 0 0 0 0 0 0 0 0 0 1 +]; + +a1 = 1; +a2 = 1; +% theta d a alpha +L(1) = Link([ 0 0 a1 0], 'standard'); +L(2) = Link([ 0 0 a2 0], 'standard'); +L(1).m = 1; +L(1).r = [-0.5 0 0]; +L(1).I = zeros(3,3); +L(1).G = 0; +L(1).Jm = 0; +L(1).B = 0; +L(2).m = 1; +L(2).r = [-0.5 0 0]; +L(2).I = zeros(3,3); +L(2).G = 0; +L(2).Jm = 0; +L(2).B = 0; +twolink = SerialLink(L, 'name', 'two link', ... + 'comment', 'from Spong, Hutchinson, Vidyasagar'); +qz = [0 0]; +qn = [pi/6, -pi/6]; diff --git a/lwrserv/matlab/rvctools/robot/mdl_twolink_mdh.m b/lwrserv/matlab/rvctools/robot/mdl_twolink_mdh.m new file mode 100644 index 0000000..810a6e9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mdl_twolink_mdh.m @@ -0,0 +1,56 @@ +%MDL_TWOLINK_MDH Create model of a 2-link mechanism using modified DH convention +% +% MDL_TWOLINK_MDH is a script that the workspace variable tl which +% describes the kinematic and dynamic characteristics of a simple planar +% 2-link mechanism using modified Denavit-Hartenberg conventions. +% +% Also defines the vector: +% qz corresponds to the zero joint angle configuration. +% +% Notes:: +% - SI units of metres are used. +% - It is a planar mechanism operating in the XY (horizontal) plane and is +% therefore not affected by gravity. +% +% References:: +% - Based on Fig 3.8 (p71) of Craig (3rd edition). +% +% See also SerialLink, mdl_onelink, mdl_twolink, mdl_planar2. + +% MODEL: generic, planar, 2DOF, modified_DH + +% Copyright (C) 1993-2015, 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.petercorke.com + + +a1 = 1; +a2 = 1; + +% for MDH parameters we need to implement the second link as a tool +% transform + +twolink = SerialLink([ + RevoluteMDH('d', 0, 'a', 0, 'alpha', 0) + RevoluteMDH('d', 0, 'a', a1, 'alpha', 0) + ], ... + 'tool', transl(a2, 0, 0), ... + 'name', 'two link', ... + 'comment', 'from Craig'); +qz = [0 0]; +qn = [pi/6, -pi/6]; diff --git a/lwrserv/matlab/rvctools/robot/mex/Makefile b/lwrserv/matlab/rvctools/robot/mex/Makefile new file mode 100644 index 0000000..fc6911f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/Makefile @@ -0,0 +1,11 @@ +rne: frne.c ne.c vmath.c + mex CFLAGS=-std=c99 frne.c ne.c vmath.c + +install: rne + for f in *.mex*; do cp $$f ../@SerialLink/rne.$${f##*.}; done + +uninstall: + rm ../@SerialLink/rne.mex* + +clean: + rm *.mex* *~ diff --git a/lwrserv/matlab/rvctools/robot/mex/README b/lwrserv/matlab/rvctools/robot/mex/README new file mode 100644 index 0000000..27d16bc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/README @@ -0,0 +1,64 @@ +A number of toolbox functions such as accel(), gravload(), coriolis(), inertia() +rely on multiple calls to the function rne(). The Simulink ROBOT block in turn +relies on accel(). Since rne() is an M-file things can run rather slowly. + +This directory contains a drop in replacment for rne.m which is a MEX file and +typically runs 10-20 times faster. + +BUILDING THE MEX FILE + +The mex file compries 3 files written in vanilla C with no other library +dependencies (apart from libm): + + frne.c + ne.c + vmath.c + +that must be compiled and linked together, resulting in a MEX file called +frne. The extension of the file depends on the operating system and architecture. + +For Linux or MacOS just run: + +% make + +which assumes that the mex utility is in your current path. This is typically +found in the bin subdirectory of your installed MATLAB. You do of course also +need a C compiler. + +For Windows: + +probably easier to run the script from inside MATLAB + +>> make + +INSTALLING THE MEX FILE + +1. Move the MEX file frne.xxx to the directory above. You can then invoke either +rne() or frne() and evaluate the speed improvement. The results should be exactly +the same. + +2. Move the MEX file frne.xxx to the @SerialLink directory, rename it rne.xxx, and +type "clear classes". The MEX file will now be used instead of the M-file, and thus +anything that calls rne() will use the MEX file and be faster. This applies to +inertia(), coriolis(), gravload(), and fdyn(). + +You can use the command "make install" to do this. + +NOTES ON PERFORMANCE + +The MEX file speedup is greater when invoked on multiple points, not just one. This +is because of startup overhead in extracting the kinematic and dynamic parameters +from the SerialArm object. + +LIMITATIONS + +- The MEX file does not support modified DH parameters. +- The MEX file does not support base or tool transforms. + +NOTES ON FILES + + frne.c MEX function main, handles different call formats, unpacks the SerialArm + object. + ne.c The actual recursive Newton-Euler code for standard and modified DH + parameters. + vmath.c A very simple vector and matrix (linalg) library. diff --git a/lwrserv/matlab/rvctools/robot/mex/TODO b/lwrserv/matlab/rvctools/robot/mex/TODO new file mode 100644 index 0000000..c910d90 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/TODO @@ -0,0 +1,2 @@ +verify prismatic joint for standard and modified DH +handle base xform diff --git a/lwrserv/matlab/rvctools/robot/mex/check.m b/lwrserv/matlab/rvctools/robot/mex/check.m new file mode 100644 index 0000000..25b05a7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/check.m @@ -0,0 +1,26 @@ +fprintf('***************************************************************\n') +fprintf('************************ Puma 560 *****************************\n') +fprintf('***************************************************************\n') +clear +mdl_puma560 +rdh = p560; +mdl_puma560akb +rmdh = p560m; + +if exist('rne') == 3 + error('need to remove rne.mex file from @SerialLink dir'); +end + +check1 + +fprintf('\n***************************************************************\n') +fprintf('********************** Stanford arm ***************************\n') +fprintf('***************************************************************\n') + +clear +mdl_stanford +stanfordm +rdh = stanf; +rdhm = stanm; + +check1 diff --git a/lwrserv/matlab/rvctools/robot/mex/check1.m b/lwrserv/matlab/rvctools/robot/mex/check1.m new file mode 100644 index 0000000..a61d3a7 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/check1.m @@ -0,0 +1,29 @@ +%CHECK script to compare M-file and MEX-file versions of RNE + +% load the model and remove non-linear friction +rdh = nofriction(rdh, 'coulomb'); +rmdh = nofriction(rdh, 'coulomb'); + +% number of trials +n = 10; + +fprintf('************************ normal case *****************************\n') +args = {}; +fprintf('DH: ') +check2(rdh, n, args); +fprintf('MDH: ') +check2(rmdh, n, args); + +fprintf('************************ no gravity *****************************\n') +args = {[0 0 0]}; +fprintf('DH: ') +check2(rdh, n, args); +fprintf('MDH: ') +check2(rmdh, n, args); + +fprintf('************************ ext force *****************************\n') +args = {[0 0 9.81], [10 10 10 10 10 10]'}; +fprintf('DH: ') +check2(rdh, n, args); +fprintf('MDH: ') +check2(rmdh, n, args); diff --git a/lwrserv/matlab/rvctools/robot/mex/check2.m b/lwrserv/matlab/rvctools/robot/mex/check2.m new file mode 100644 index 0000000..efe5eae --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/check2.m @@ -0,0 +1,23 @@ +%CHECK2 script to compare M-file and MEX-file versions of RNE + +function check2(robot, n, args) + robot = nofriction(robot, 'coulomb'); + + % create random points in state space + q = rand(n, 6); + qd = rand(n, 6); + qdd = rand(n, 6); + + % test M-file + tic; + tau = rne(robot, q, qd, qdd, args{:}); + t = toc; + + % test MEX-file + tic; + tau_f = frne(robot, q, qd, qdd, args{:}); + t_f = toc; + + % print comparative results + fprintf('Speedup is %10.0f, worst case error is %f\n', ... + t/t_f, max(max(abs(tau-tau_f)))); diff --git a/lwrserv/matlab/rvctools/robot/mex/frne.c b/lwrserv/matlab/rvctools/robot/mex/frne.c new file mode 100644 index 0000000..84a9729 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/frne.c @@ -0,0 +1,496 @@ +/** + * \file frne.c + * \author Peter Corke + * \brief MEX file body + * + * + * FRNE MEX file version of RNE.M + * + * TAU = FRNE(ROBOT, Q, QD, QDD) + * TAU = FRNE(ROBOT, [Q QD QDD]) + * + * where Q, QD and QDD are row vectors of the manipulator state; pos, + * vel, and accel. + * + * Returns the joint torque required to achieve the specified joint + * position, velocity and acceleration state. Gravity is taken + * from the robot object. + * + * TAU = RNE(ROBOT, Q, QD, QDD, GRAV) + * TAU = RNE(ROBOT, [Q QD QDD], GRAV) + * + * GRAV overrides the gravity vector in the robot object. + * + * An external force/moment acting on the end of the manipulator may + * also be specified by a 6-element vector [Fx Fy Fz Mx My Mz]. + * + * TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT) + * TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT) + * + */ + +/* + * Copyright (C) 1999-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 . + * + */ + +#include "mex.h" +#include + +#include "frne.h" + +/* +#define DEBUG +*/ + +/* Input Arguments */ +#define ROBOT_IN prhs[0] +#define A1_IN prhs[1] +#define A2_IN prhs[2] +#define A3_IN prhs[3] +#define A4_IN prhs[4] +#define A5_IN prhs[5] + +/* Output Arguments */ +#define TAU_OUT plhs[0] + +/* Some useful things */ +#define NUMROWS(x) mxGetM(x) +#define NUMCOLS(x) mxGetN(x) +#define NUMELS(x) (mxGetN(x)*mxGetM(x)) +#define POINTER(x) mxGetPr(x) + +/* forward defines */ +static void rot_mat (Link *l, double th, double d, DHType type); +static int mstruct_getfield_number(mxArray *m, char *field); +static int mstruct_getint(mxArray *m, int i, char *field); +static double mstruct_getreal(mxArray *m, int i, char *field); +static double * mstruct_getrealvect(mxArray *m, int i, char *field); +void error(char *s, ...); + + +/* default values for gravity and external load */ + +/** + * MEX function entry point. + */ +void +mexFunction( + int nlhs, + mxArray *plhs[], + int nrhs, + const mxArray *prhs[] +) { + double *q, *qd, *qdd; + double *tau; + unsigned int m,n; + int j, njoints, p, nq; + double *fext = NULL; + double *grav = NULL; + Robot robot; + mxArray *link0; + mxArray *mx_robot; + mxArray *mx_links; + static int first_time = 0; + + /* + fprintf(stderr, "Fast RNE: (c) Peter Corke 2002-2011\n"); + */ + + if ( !mxIsClass(ROBOT_IN, "SerialLink") ) + mexErrMsgTxt("frne: first argument is not a robot structure\n"); + + mx_robot = (mxArray *)ROBOT_IN; + + njoints = mstruct_getint(mx_robot, 0, "n"); + +/*********************************************************************** + * Handle the different calling formats. + * Setup pointers to q, qd and qdd inputs + ***********************************************************************/ + switch (nrhs) { + case 2: + /* + * TAU = RNE(ROBOT, [Q QD QDD]) + */ + if (NUMCOLS(A1_IN) != 3 * njoints) + mexErrMsgTxt("frne: too few cols in [Q QD QDD]"); + q = POINTER(A1_IN); + nq = NUMROWS(A1_IN); + qd = &q[njoints*nq]; + qdd = &q[2*njoints*nq]; + break; + + case 3: + /* + * TAU = RNE(ROBOT, [Q QD QDD], GRAV) + */ + if (NUMCOLS(A1_IN) != (3 * njoints)) + mexErrMsgTxt("frne: too few cols in [Q QD QDD]"); + q = POINTER(A1_IN); + nq = NUMROWS(A1_IN); + qd = &q[njoints*nq]; + qdd = &q[2*njoints*nq]; + + if (NUMELS(A2_IN) != 3) + mexErrMsgTxt("frne: gravity vector expected"); + grav = POINTER(A2_IN); + break; + + case 4: + /* + * TAU = RNE(ROBOT, Q, QD, QDD) + * TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT) + */ + if (NUMCOLS(A1_IN) == (3 * njoints)) { + q = POINTER(A1_IN); + nq = NUMROWS(A1_IN); + qd = &q[njoints*nq]; + qdd = &q[2*njoints*nq]; + + if (NUMELS(A2_IN) != 3) + mexErrMsgTxt("frne: gravity vector expected"); + grav = POINTER(A2_IN); + if (NUMELS(A3_IN) != 6) + mexErrMsgTxt("frne: Fext vector expected"); + fext = POINTER(A3_IN); + } else { + int nqd = NUMROWS(A2_IN), + nqdd = NUMROWS(A3_IN); + + nq = NUMROWS(A1_IN); + if ((nq != nqd) || (nqd != nqdd)) + mexErrMsgTxt("frne: Q QD QDD must be same length"); + if ( (NUMCOLS(A1_IN) != njoints) || + (NUMCOLS(A2_IN) != njoints) || + (NUMCOLS(A3_IN) != njoints) + ) + mexErrMsgTxt("frne: Q must have Naxis columns"); + q = POINTER(A1_IN); + qd = POINTER(A2_IN); + qdd = POINTER(A3_IN); + } + break; + + case 5: { + /* + * TAU = RNE(ROBOT, Q, QD, QDD, GRAV) + */ + int nqd = NUMROWS(A2_IN), + nqdd = NUMROWS(A3_IN); + + nq = NUMROWS(A1_IN); + if ((nq != nqd) || (nqd != nqdd)) + mexErrMsgTxt("frne: Q QD QDD must be same length"); + if ( (NUMCOLS(A1_IN) != njoints) || + (NUMCOLS(A2_IN) != njoints) || + (NUMCOLS(A3_IN) != njoints) + ) + mexErrMsgTxt("frne: Q must have Naxis columns"); + q = POINTER(A1_IN); + qd = POINTER(A2_IN); + qdd = POINTER(A3_IN); + if (NUMELS(A4_IN) != 3) + mexErrMsgTxt("frne: gravity vector expected"); + grav = POINTER(A4_IN); + break; + } + + case 6: { + /* + * TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT) + */ + int nqd = NUMROWS(A2_IN), + nqdd = NUMROWS(A3_IN); + + nq = NUMROWS(A1_IN); + if ((nq != nqd) || (nqd != nqdd)) + mexErrMsgTxt("frne: Q QD QDD must be same length"); + if ( (NUMCOLS(A1_IN) != njoints) || + (NUMCOLS(A2_IN) != njoints) || + (NUMCOLS(A3_IN) != njoints) + ) + mexErrMsgTxt("frne: Q must have Naxis columns"); + q = POINTER(A1_IN); + qd = POINTER(A2_IN); + qdd = POINTER(A3_IN); + if (NUMELS(A4_IN) != 3) + mexErrMsgTxt("frne: gravity vector expected"); + grav = POINTER(A4_IN); + if (NUMELS(A5_IN) != 6) + mexErrMsgTxt("frne: Fext vector expected"); + fext = POINTER(A5_IN); + break; + } + default: + mexErrMsgTxt("frne: wrong number of arguments."); + } + + /* + * fill out the robot structure + */ + robot.njoints = njoints; + + if (grav) + robot.gravity = (Vect *)grav; + else + robot.gravity = (Vect *)mxGetPr( mxGetProperty(mx_robot, (mwIndex)0, "gravity") ); + robot.dhtype = mstruct_getint(mx_robot, 0, "mdh"); + + /* build link structure */ + robot.links = (Link *)mxCalloc((mwSize) njoints, (mwSize) sizeof(Link)); + + +/*********************************************************************** + * Now we have to get pointers to data spread all across a cell-array + * of Matlab structures. + * + * Matlab structure elements can be found by name (slow) or by number (fast). + * We assume that since the link structures are all created by the same + * constructor, the index number for each element will be the same for all + * links. However we make no assumption about the numbers themselves. + ***********************************************************************/ + + /* get pointer to the first link structure */ + link0 = mxGetProperty(mx_robot, (mwIndex) 0, "links"); + if (link0 == NULL) + mexErrMsgTxt("couldnt find element link in robot object"); + + /* + * Elements of the link structure are: + * + * alpha: + * A: + * theta: + * D: + * offset: + * sigma: + * mdh: + * m: + * r: + * I: + * Jm: + * G: + * B: + * Tc: + */ + + if (first_time == 0) { + mexPrintf("Fast RNE: (c) Peter Corke 2002-2012\n"); + first_time = 1; + } + + /* + * copy data from the Link objects into the local links structure + * to save function calls later + */ + for (j=0; jalpha = mxGetScalar( mxGetProperty(links, (mwIndex) j, "alpha") ); + l->A = mxGetScalar( mxGetProperty(links, (mwIndex) j, "a") ); + l->theta = mxGetScalar( mxGetProperty(links, (mwIndex) j, "theta") ); + l->D = mxGetScalar( mxGetProperty(links, (mwIndex) j, "d") ); + l->sigma = mxGetScalar( mxGetProperty(links, (mwIndex) j, "sigma") ); + l->offset = mxGetScalar( mxGetProperty(links, (mwIndex) j, "offset") ); + l->m = mxGetScalar( mxGetProperty(links, (mwIndex) j, "m") ); + l->rbar = (Vect *)mxGetPr( mxGetProperty(links, (mwIndex) j, "r") ); + l->I = mxGetPr( mxGetProperty(links, (mwIndex) j, "I") ); + l->Jm = mxGetScalar( mxGetProperty(links, (mwIndex) j, "Jm") ); + l->G = mxGetScalar( mxGetProperty(links, (mwIndex) j, "G") ); + l->B = mxGetScalar( mxGetProperty(links, (mwIndex) j, "B") ); + l->Tc = mxGetPr( mxGetProperty(links, (mwIndex) j, "Tc") ); + } + + /* Create a matrix for the return argument */ + TAU_OUT = mxCreateDoubleMatrix((mwSize) nq, (mwSize) njoints, mxREAL); + tau = mxGetPr(TAU_OUT); + +#define MEL(x,R,C) (x[(R)+(C)*nq]) + + /* for each point in the input trajectory */ + for (p=0; psigma) { + case REVOLUTE: + rot_mat(l, MEL(q,p,j)+l->offset, l->D, robot.dhtype); + break; + case PRISMATIC: + rot_mat(l, l->theta, MEL(q,p,j)+l->offset, robot.dhtype); + break; + } +#ifdef DEBUG + rot_print("R", &l->R); + vect_print("p*", &l->r); +#endif + } + + newton_euler(&robot, &tau[p], &qd[p], &qdd[p], fext, nq); + + } + + mxFree(robot.links); +} + +/* + * Written by; + * + * Peter I. Corke + * CSIRO Division of Manufacturing Technology + * Preston, Melbourne. Australia. 3072. + * + * pic@mlb.dmt.csiro.au + * + * Permission to use and distribute is granted, provided that this message + * is retained, and due credit given when the results are incorporated in + * publised work. + * + */ + +/** + * Return the link rotation matrix and translation vector. + * + * @param l Link object for which R and p* are required. + * @param th Joint angle, overrides value in link object + * @param d Link extension, overrides value in link object + * @param type Kinematic convention. + */ +static void +rot_mat ( + Link *l, + double th, + double d, + DHType type +) { + double st, ct, sa, ca; + +#ifdef sun + sincos(th, &st, &ct); + sincos(l->alpha, &sa, &ca); +#else + st = sin(th); + ct = cos(th); + sa = sin(l->alpha); + ca = cos(l->alpha); +#endif + + switch (type) { +case STANDARD: + l->R.n.x = ct; l->R.o.x = -ca*st; l->R.a.x = sa*st; + l->R.n.y = st; l->R.o.y = ca*ct; l->R.a.y = -sa*ct; + l->R.n.z = 0.0; l->R.o.z = sa; l->R.a.z = ca; + + l->r.x = l->A; + l->r.y = d * sa; + l->r.z = d * ca; + break; +case MODIFIED: + l->R.n.x = ct; l->R.o.x = -st; l->R.a.x = 0.0; + l->R.n.y = st*ca; l->R.o.y = ca*ct; l->R.a.y = -sa; + l->R.n.z = st*sa; l->R.o.z = ct*sa; l->R.a.z = ca; + + l->r.x = l->A; + l->r.y = -d * sa; + l->r.z = d * ca; + break; + } +} + +/************************************************************************* + * Matlab structure access methods, get the field from joint i + *************************************************************************/ +static mxArray * +mstruct_get_element(mxArray *m, int j, char *field) +{ + mxArray *e; + + if ((e = mxGetProperty(m, (mwIndex)j, field)) != NULL) + return e; + else { + error("No such field as %s", field); + } +} + +static int +mstruct_getfield_number(mxArray *m, char *field) +{ + int f; + + if ((f = mxGetFieldNumber(m, field)) < 0) + error("no element %s in link structure"); + + return f; +} + +static int +mstruct_getint(mxArray *m, int i, char *field) +{ + mxArray *e; + + e = mstruct_get_element(m, i, field); + + return (int) mxGetScalar(e); +} + +static double +mstruct_getreal(mxArray *m, int i, char *field) +{ + mxArray *e; + + e = mstruct_get_element(m, i, field); + + return mxGetScalar(e); +} + +static double * +mstruct_getrealvect(mxArray *m, int i, char *field) +{ + mxArray *e; + + e = mstruct_get_element(m, i, field); + + return mxGetPr(e); +} + +#include + +/** + * Error message handler. Takes printf() style format string and variable + * arguments and sends resultant string to Matlab via \t mexErrMsgTxt(). + * + * @param s Error message string, \t printf() style. + */ +void +error(char *s, ...) +{ + char b[BUFSIZ]; + + va_list ap; + + va_start(ap, s); + + vsprintf(b, s, ap); + + mexErrMsgTxt(b); +} diff --git a/lwrserv/matlab/rvctools/robot/mex/frne.h b/lwrserv/matlab/rvctools/robot/mex/frne.h new file mode 100644 index 0000000..a214306 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/frne.h @@ -0,0 +1,108 @@ +/** + * \file frne.h + * \author Peter Corke + * \brief Definitions for MEX file + * + */ + +/* + * Copyright (C) 1999-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 . + * + */ + +#ifndef _rne_h_ +#define _rne_h_ + +#include + +#include "vmath.h" + +#define TRUE 1 +#define FALSE 0 + +/* + * Accessing information within a MATLAB structure is inconvenient and slow. + * To get around this we build our own robot and link data structures, and + * copy the information from the MATLAB objects once per call. If the call + * is for multiple states values then our efficiency becomes very high. + */ + +/* Robot kinematic convention */ +typedef + enum _dhtype { + STANDARD, + MODIFIED +} DHType; + +/* Link joint type */ +typedef + enum _axistype { + REVOLUTE = 0, + PRISMATIC = 1 +} Sigma; + +/* A robot link structure */ +typedef struct _link { + /********************************************************** + *************** kinematic parameters ********************* + **********************************************************/ + double alpha; /* link twist */ + double A; /* link offset */ + double D; /* link length */ + double theta; /* link rotation angle */ + double offset; /* link coordinate offset */ + int sigma; /* axis type; revolute or prismatic */ + + /********************************************************** + ***************** dynamic parameters ********************* + **********************************************************/ + + /**************** of links ********************************/ + Vect *rbar; /* centre of mass of link wrt link origin */ + double m; /* mass of link */ + double *I; /* inertia tensor of link wrt link origin */ + + /**************** of actuators *****************************/ + /* these parameters are motor referenced */ + double Jm; /* actuator inertia */ + double G; /* gear ratio */ + double B; /* actuator friction damping coefficient */ + double *Tc; /* actuator Coulomb friction coeffient */ + + /********************************************************** + **************** intermediate variables ****************** + **********************************************************/ + Vect r; /* distance of ith origin from i-1th wrt ith */ + Rot R; /* link rotation matrix */ + Vect omega; /* angular velocity */ + Vect omega_d; /* angular acceleration */ + Vect acc; /* acceleration */ + Vect abar; /* acceleration of centre of mass */ + Vect f; /* inter-link force */ + Vect n; /* inter-link moment */ +} Link; + +/* A robot */ +typedef struct _robot { + int njoints; /* number of joints */ + Vect *gravity; /* gravity vector */ + DHType dhtype; /* kinematic convention */ + Link *links; /* the links */ +} Robot; + +#endif diff --git a/lwrserv/matlab/rvctools/robot/mex/frne.mexmaci64 b/lwrserv/matlab/rvctools/robot/mex/frne.mexmaci64 new file mode 100644 index 0000000..81cfecf Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/mex/frne.mexmaci64 differ diff --git a/lwrserv/matlab/rvctools/robot/mex/frne.mexw32 b/lwrserv/matlab/rvctools/robot/mex/frne.mexw32 new file mode 100644 index 0000000..683e63b Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/mex/frne.mexw32 differ diff --git a/lwrserv/matlab/rvctools/robot/mex/make.m b/lwrserv/matlab/rvctools/robot/mex/make.m new file mode 100644 index 0000000..9fede4c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/make.m @@ -0,0 +1 @@ +mex CFLAGS=-std=c99 frne.c ne.c vmath.c diff --git a/lwrserv/matlab/rvctools/robot/mex/ne.c b/lwrserv/matlab/rvctools/robot/mex/ne.c new file mode 100644 index 0000000..61fddee --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/ne.c @@ -0,0 +1,473 @@ +/** + * \file ne.c + * \author Peter Corke + * \brief Compute the recursive Newton-Euler formulation + */ + +/* + * Copyright (C) 1999-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 . + * + */ + +/* + * Compute the inverse dynamics via the recursive Newton-Euler formulation + * + * Requires: qd current joint velocities + * qdd current joint accelerations + * f applied tip force or load + * grav the gravitational constant + * + * Returns: tau vector of bias torques + */ +#include "frne.h" +#include "vmath.h" + +/* +#define DEBUG +*/ + +/* + * Bunch of macros to make the main code easier to read. Dereference vectors + * from the Link structures for the manipulator. + * + * Note that they return pointers (except for M(j) which is a scalar) + */ +#undef N + +#define OMEGA(j) (&links[j].omega) /* angular velocity */ +#define OMEGADOT(j) (&links[j].omega_d) /* angular acceleration */ +#define ACC(j) (&links[j].acc) /* linear acceleration */ +#define ACC_COG(j) (&links[j].abar) /* linear acceln of COG */ + +#define f(j) (&links[j].f) /* force on link j due to link j-1 */ +#define n(j) (&links[j].n) /* torque on link j due to link j-1 */ + +#define ROT(j) (&links[j].R) /* link rotation matrix */ +#define M(j) (links[j].m) /* link mass */ +#define PSTAR(j) (&links[j].r) /* offset link i from link (j-1) */ +#define R_COG(j) (links[j].rbar) /* COG link j wrt link j */ +#define INERTIA(j) (links[j].I) /* inertia of link about COG */ + +/** + * Recursive Newton-Euler algorithm. + * + * @Note the parameter \p stride which is used to allow for input and output + * arrays which are 2-dimensional but in column-major (Matlab) order. We + * need to access rows from the arrays. + * + */ +void +newton_euler ( + Robot *robot, /*!< robot object */ + double *tau, /*!< returned joint torques */ + double *qd, /*!< joint velocities */ + double *qdd, /*!< joint accelerations */ + double *fext, /*!< external force on manipulator tip */ + int stride /*!< indexing stride for qd, qdd */ +) { + Vect t1, t2, t3, t4; + Vect qdv, qddv; + Vect F, N; + Vect z0 = {0.0, 0.0, 1.0}; + Vect zero = {0.0, 0.0, 0.0}; + Vect f_tip = {0.0, 0.0, 0.0}; + Vect n_tip = {0.0, 0.0, 0.0}; + register int j; + double t; + Link *links = robot->links; + + /* + * angular rate and acceleration vectors only have finite + * z-axis component + */ + qdv = qddv = zero; + + /* setup external force/moment vectors */ + if (fext) { + f_tip.x = fext[0]; + f_tip.y = fext[1]; + f_tip.z = fext[2]; + n_tip.x = fext[3]; + n_tip.y = fext[4]; + n_tip.z = fext[5]; + } + + +/****************************************************************************** + * forward recursion --the kinematics + ******************************************************************************/ + + if (robot->dhtype == MODIFIED) { + /* + * MODIFIED D&H CONVENTIONS + */ + for (j = 0; j < robot->njoints; j++) { + + /* create angular vector from scalar input */ + qdv.z = qd[j*stride]; + qddv.z = qdd[j*stride]; + + switch (links[j].sigma) { + case REVOLUTE: + /* + * calculate angular velocity of link j + */ + if (j == 0) + *OMEGA(j) = qdv; + else { + rot_trans_vect_mult (&t1, ROT(j), OMEGA(j-1)); + vect_add (OMEGA(j), &t1, &qdv); + } + + /* + * calculate angular acceleration of link j + */ + if (j == 0) + *OMEGADOT(j) = qddv; + else { + rot_trans_vect_mult (&t3, ROT(j), OMEGADOT(j-1)); + vect_cross (&t2, &t1, &qdv); + vect_add (&t1, &t2, &t3); + vect_add (OMEGADOT(j), &t1, &qddv); + } + + /* + * compute acc[j] + */ + if (j == 0) { + t1 = *robot->gravity; + } else { + vect_cross(&t1, OMEGA(j-1), PSTAR(j)); + vect_cross(&t2, OMEGA(j-1), &t1); + vect_cross(&t1, OMEGADOT(j-1), PSTAR(j)); + vect_add(&t1, &t1, &t2); + vect_add(&t1, &t1, ACC(j-1)); + } + rot_trans_vect_mult(ACC(j), ROT(j), &t1); + + break; + + case PRISMATIC: + /* + * calculate omega[j] + */ + if (j == 0) + *(OMEGA(j)) = qdv; + else + rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1)); + + /* + * calculate alpha[j] + */ + if (j == 0) + *(OMEGADOT(j)) = qddv; + else + rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1)); + + /* + * compute acc[j] + */ + if (j == 0) { + *ACC(j) = *robot->gravity; + } else { + vect_cross(&t1, OMEGADOT(j-1), PSTAR(j)); + + vect_cross(&t3, OMEGA(j-1), PSTAR(j)); + vect_cross(&t2, OMEGA(j-1), &t3); + vect_add(&t1, &t1, &t2); + vect_add(&t1, &t1, ACC(j-1)); + rot_trans_vect_mult(ACC(j), ROT(j), &t1); + + rot_trans_vect_mult(&t2, ROT(j), OMEGA(j-1)); + vect_cross(&t1, &t2, &qdv); + scal_mult(&t1, &t1, 2.0); + vect_add(ACC(j), ACC(j), &t1); + + vect_add(ACC(j), ACC(j), &qddv); + } + + break; + } + + /* + * compute abar[j] + */ + vect_cross(&t1, OMEGADOT(j), R_COG(j)); + vect_cross(&t2, OMEGA(j), R_COG(j)); + vect_cross(&t3, OMEGA(j), &t2); + vect_add(ACC_COG(j), &t1, &t3); + vect_add(ACC_COG(j), ACC_COG(j), ACC(j)); + +#ifdef DEBUG + vect_print("w", OMEGA(j)); + vect_print("wd", OMEGADOT(j)); + vect_print("acc", ACC(j)); + vect_print("abar", ACC_COG(j)); +#endif + } + } else { + /* + * STANDARD D&H CONVENTIONS + */ + for (j = 0; j < robot->njoints; j++) { + + /* create angular vector from scalar input */ + qdv.z = qd[j*stride]; + qddv.z = qdd[j*stride]; + + switch (links[j].sigma) { + case REVOLUTE: + /* + * calculate omega[j] + */ + if (j == 0) + t1 = qdv; + else + vect_add (&t1, OMEGA(j-1), &qdv); + rot_trans_vect_mult (OMEGA(j), ROT(j), &t1); + + /* + * calculate alpha[j] + */ + if (j == 0) + t3 = qddv; + else { + vect_add (&t1, OMEGADOT(j-1), &qddv); + vect_cross (&t2, OMEGA(j-1), &qdv); + vect_add (&t3, &t1, &t2); + } + rot_trans_vect_mult (OMEGADOT(j), ROT(j), &t3); + + /* + * compute acc[j] + */ + vect_cross(&t1, OMEGADOT(j), PSTAR(j)); + vect_cross(&t2, OMEGA(j), PSTAR(j)); + vect_cross(&t3, OMEGA(j), &t2); + vect_add(ACC(j), &t1, &t3); + if (j == 0) { + rot_trans_vect_mult(&t1, ROT(j), robot->gravity); + } else + rot_trans_vect_mult(&t1, ROT(j), ACC(j-1)); + vect_add(ACC(j), ACC(j), &t1); + break; + + case PRISMATIC: + /* + * calculate omega[j] + */ + if (j == 0) + *(OMEGA(j)) = zero; + else + rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1)); + + /* + * calculate alpha[j] + */ + if (j == 0) + *(OMEGADOT(j)) = zero; + else + rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1)); + + /* + * compute acc[j] + */ + if (j == 0) { + vect_add(&qddv, &qddv, robot->gravity); + rot_trans_vect_mult(ACC(j), ROT(j), &qddv); + } else { + vect_add(&t1, &qddv, ACC(j-1)); + rot_trans_vect_mult(ACC(j), ROT(j), &t1); + + } + + vect_cross(&t1, OMEGADOT(j), PSTAR(j)); + vect_add(ACC(j), ACC(j), &t1); + + rot_trans_vect_mult(&t1, ROT(j), &qdv); + vect_cross(&t2, OMEGA(j), &t1); + scal_mult(&t2, &t2, 2.0); + vect_add(ACC(j), ACC(j), &t2); + + vect_cross(&t2, OMEGA(j), PSTAR(j)); + vect_cross(&t3, OMEGA(j), &t2); + vect_add(ACC(j), ACC(j), &t3); + break; + } + /* + * compute abar[j] + */ + vect_cross(&t1, OMEGADOT(j), R_COG(j)); + vect_cross(&t2, OMEGA(j), R_COG(j)); + vect_cross(&t3, OMEGA(j), &t2); + vect_add(ACC_COG(j), &t1, &t3); + vect_add(ACC_COG(j), ACC_COG(j), ACC(j)); + +#ifdef DEBUG + vect_print("w", OMEGA(j)); + vect_print("wd", OMEGADOT(j)); + vect_print("acc", ACC(j)); + vect_print("abar", ACC_COG(j)); +#endif + } + } + +/****************************************************************************** + * backward recursion part --the kinetics + ******************************************************************************/ + + if (robot->dhtype == MODIFIED) { + /* + * MODIFIED D&H CONVENTIONS + */ + for (j = robot->njoints - 1; j >= 0; j--) { + + /* + * compute F[j] + */ + scal_mult (&F, ACC_COG(j), M(j)); + + /* + * compute f[j] + */ + if (j == (robot->njoints-1)) + t1 = f_tip; + else + rot_vect_mult (&t1, ROT(j+1), f(j+1)); + vect_add (f(j), &t1, &F); + + /* + * compute N[j] + */ + mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j)); + mat_vect_mult(&t3, INERTIA(j), OMEGA(j)); + vect_cross(&t4, OMEGA(j), &t3); + vect_add(&N, &t2, &t4); + + /* + * compute n[j] + */ + if (j == (robot->njoints-1)) + t1 = n_tip; + else { + rot_vect_mult(&t1, ROT(j+1), n(j+1)); + rot_vect_mult(&t4, ROT(j+1), f(j+1)); + vect_cross(&t3, PSTAR(j+1), &t4); + + vect_add(&t1, &t1, &t3); + } + + vect_cross(&t2, R_COG(j), &F); + vect_add(&t1, &t1, &t2); + vect_add(n(j), &t1, &N); + +#ifdef DEBUG + vect_print("f", f(j)); + vect_print("n", n(j)); +#endif + } + + } else { + /* + * STANDARD D&H CONVENTIONS + */ + for (j = robot->njoints - 1; j >= 0; j--) { + + /* + * compute f[j] + */ + scal_mult (&t4, ACC_COG(j), M(j)); + if (j != (robot->njoints-1)) { + rot_vect_mult (&t1, ROT(j+1), f(j+1)); + vect_add (f(j), &t4, &t1); + } else + vect_add (f(j), &t4, &f_tip); + + /* + * compute n[j] + */ + + /* cross(pstar+r,Fm(:,j)) */ + vect_add(&t2, PSTAR(j), R_COG(j)); + vect_cross(&t1, &t2, &t4); + + if (j != (robot->njoints-1)) { + /* cross(R'*pstar,f) */ + rot_trans_vect_mult(&t2, ROT(j+1), PSTAR(j)); + vect_cross(&t3, &t2, f(j+1)); + + /* nn += R*(nn + cross(R'*pstar,f)) */ + vect_add(&t3, &t3, n(j+1)); + rot_vect_mult(&t2, ROT(j+1), &t3); + vect_add(&t1, &t1, &t2); + } else { + /* cross(R'*pstar,f) */ + vect_cross(&t2, PSTAR(j), &f_tip); + + /* nn += R*(nn + cross(R'*pstar,f)) */ + vect_add(&t1, &t1, &t2); + vect_add(&t1, &t1, &n_tip); + } + + mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j)); + mat_vect_mult(&t3, INERTIA(j), OMEGA(j)); + vect_cross(&t4, OMEGA(j), &t3); + vect_add(&t2, &t2, &t4); + + vect_add(n(j), &t1, &t2); +#ifdef DEBUG + vect_print("f", f(j)); + vect_print("n", n(j)); +#endif + } + } + + /* + * Compute the torque total for each axis + * + */ + for (j=0; j < robot->njoints; j++) { + double t; + Link *l = &links[j]; + + if (robot->dhtype == MODIFIED) + t1 = z0; + else + rot_trans_vect_mult(&t1, ROT(j), &z0); + + switch (l->sigma) { + case REVOLUTE: + t = vect_dot(n(j), &t1); + break; + case PRISMATIC: + t = vect_dot(f(j), &t1); + break; + } + + /* + * add actuator dynamics and friction + */ + t += l->G * l->G * l->Jm * qdd[j*stride]; // inertia + t += l->G * l->G * l->B * qd[j*stride]; // viscous friction + t += fabs(l->G) * ( + (qd[j*stride] > 0 ? l->Tc[0] : 0.0) + // Coulomb friction + (qd[j*stride] < 0 ? l->Tc[1] : 0.0) + ); + tau[j*stride] = t; + } +} diff --git a/lwrserv/matlab/rvctools/robot/mex/prismatic.m b/lwrserv/matlab/rvctools/robot/mex/prismatic.m new file mode 100644 index 0000000..ded0796 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/prismatic.m @@ -0,0 +1,24 @@ +stanford +stanfordm + +% number of trials +n = 1; + +% create random points in state space +q = rand(n, 6); +qd = rand(n, 6); +qdd = rand(n, 6); + +jacobn1 = jacobn(stan,q); +jacobn2 = jacobn(stanm,q); +fprintf('Jacob N - Worst case error is %f\n', max(max(abs(jacobn1-jacobn2)))); + +jacob01 = jacob0(stan,q(1,:)); +jacob02 = jacob0(stanm,q(1,:)); +fprintf('Jacob 0 - Worst case error is %f\n', max(max(abs(jacob01-jacob02)))); + +tau1=rne(stan,q,qd,qdd); +tau2=rne(stanm,q,qd,qdd); + +% print comparative results +fprintf('Torques and forces - Worst case error is %f\n', max(max(abs(tau1-tau2)))); diff --git a/lwrserv/matlab/rvctools/robot/mex/stanford.m b/lwrserv/matlab/rvctools/robot/mex/stanford.m new file mode 100644 index 0000000..d9080b4 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/stanford.m @@ -0,0 +1,8 @@ +L{1}=link([-pi/2 0 0 0 0 1 0 1 0 1 1 1 0 0 0 291e-6 -62.6111]); +L{2}=link([ pi/2 0 0 1 0 1 0 -1 0 1 1 1 0 0 0 409e-6 107.815]); +L{3}=link([ 0 0 0 0 1 1 0 0 -1 1 1 1 0 0 0 299e-6 -53.7063]); +L{4}=link([-pi/2 0 0 0 0 1 0 1 0 7 0.5 11 9 3 2 291e-6 -62.6111]); +L{5}=link([ pi/2 0 0 0 0 1 0 1 0 3 2 4 5 -3 -6 409e-6 107.815]); +L{6}=link([ 0 0 0 1 0 1 0 0 -1 1 1 1 0 0 0 299e-6 -53.7063]); +stan=robot(L, 'Stanford'); +clear L diff --git a/lwrserv/matlab/rvctools/robot/mex/stanfordm.m b/lwrserv/matlab/rvctools/robot/mex/stanfordm.m new file mode 100644 index 0000000..07fab72 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/stanfordm.m @@ -0,0 +1,8 @@ +clear L +L(1) = Link([ 0 0 0 0 0 1 0 0 -1 1 1 1 0 0 0 291e-6 -62.6111], 'modified'); +L(2) = Link([ 0 1 0 -pi/2 0 1 0 0 -1 1 1 1 0 0 0 409e-6 107.815], 'modified'); +L(3) = Link([ 0 0 0 pi/2 1 1 0 0 -1 1 1 1 0 0 0 299e-6 -53.7063], 'modified'); +L(4) = Link([ 0 0 0 0 0 1 0 0 -1 7 11 0.5 2 -3 -9 291e-6 -62.6111], 'modified'); +L(5) = Link([ 0 0 0 -pi/2 0 1 0 0 1 3 4 2 6 3 5 409e-6 107.815], 'modified'); +L(6) = Link([ 0 1 0 pi/2 0 1 0 0 -1 1 1 1 0 0 0 299e-6 -53.7063], 'modified'); +stanm = SerialLink(L, 'name', 'stanford'); diff --git a/lwrserv/matlab/rvctools/robot/mex/vmath.c b/lwrserv/matlab/rvctools/robot/mex/vmath.c new file mode 100644 index 0000000..20b3904 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/vmath.c @@ -0,0 +1,179 @@ +/** + * \file vmath.c + * \author Peter Corke + * \brief Simple vector/matrix maths library. + */ + +/* + * Copyright (C) 1999-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 . + * + */ +#include "mex.h" +#include "vmath.h" + +/** + * Vector cross product. + * + * @param r Return vector. + * @param a Vector. + * @param b Vector. + */ +void +vect_cross (Vect *r, Vect *a, Vect *b) +{ + r->x = a->y*b->z - a->z*b->y; + r->y = a->z*b->x - a->x*b->z; + r->z = a->x*b->y - a->y*b->x; +} + +/** + * Vector cross product. + * + * @param a Vector. + * @param b Vector. + * @return Dot (inner) product. + */ +double +vect_dot (Vect *a, Vect *b) +{ + return a->x * b->x + a->y * b->y + a->z * b->z; +} + +/** + * Vector sum. + * + * @param r Return sum vector. + * @param a Vector. + * @param b Vector. + * + * @note Elementwise addition of two vectors. + */ +void +vect_add (Vect *r, Vect *a, Vect *b) +{ + r->x = a->x + b->x; + r->y = a->y + b->y; + r->z = a->z + b->z; +} + +/** + * Vector scalar product. + * + * @param r Return scaled vector. + * @param a Vector. + * @param s Scalar. + * + * @note Elementwise scaling of vector. + */ +void +scal_mult (Vect *r, Vect *a, double s) +{ + r->x = s*a->x; + r->y = s*a->y; + r->z = s*a->z; +} + +/** + * Matrix vector product. + * + * @param r Return rotated vector. + * @param m 3x3 rotation matrix. + * @param v Vector. + */ +void +rot_vect_mult (Vect *r, Rot *m, Vect *v) +{ + r->x = m->n.x*v->x + m->o.x*v->y + m->a.x*v->z; + r->y = m->n.y*v->x + m->o.y*v->y + m->a.y*v->z; + r->z = m->n.z*v->x + m->o.z*v->y + m->a.z*v->z; +} + +/** + * Matrix transpose vector product. + * + * @param r Return rotated vector. + * @param m 3x3 rotation matrix. + * @param v Vector. + * + * @note Multiplies \p v by transpose of \p m. + */ +void +rot_trans_vect_mult (Vect *r, Rot *m, Vect *v) +{ + r->x = m->n.x*v->x + m->n.y*v->y + m->n.z*v->z; + r->y = m->o.x*v->x + m->o.y*v->y + m->o.z*v->z; + r->z = m->a.x*v->x + m->a.y*v->y + m->a.z*v->z; +} + + +/** + * General matrix vector product. + * + * @param r Return vector. + * @param m 3x3 matrix. + * @param v Vector. + * + * @note Assumes matrix is organized in column major order. + */ +void +mat_vect_mult (Vect *r, double *m, Vect *v) +{ + r->x = m[0]*v->x + m[3]*v->y + m[6]*v->z; + r->y = m[1]*v->x + m[4]*v->y + m[7]*v->z; + r->z = m[2]*v->x + m[5]*v->y + m[8]*v->z; +} + +/** + * Print vector. + * + * @param s Identification string, printed first. + * @param v Vector + * + * Vector is printed on a single line, preceded by the string \p s. + */ +void +vect_print(char *s, Vect *v) +{ + int j; + + mexPrintf("%10s: ", s); + mexPrintf("%15.3f", v->x); + mexPrintf("%15.3f", v->y); + mexPrintf("%15.3f\n", v->z); +} + + +/** + * Print matrix. + * + * @param s Identification string, printed first. + * @param m Rotation matrix. + * + * Vector is printed on a single line, preceded by the string \p s. + */ +void +rot_print(char *s, Rot *m) +{ + int j; + + mexPrintf("%s:\n", s); + mexPrintf(" %15.3f%15.3f%15.3f\n", m->n.x, m->o.x, m->a.x); + mexPrintf(" %15.3f%15.3f%15.3f\n", m->n.y, m->o.y, m->a.y); + mexPrintf(" %15.3f%15.3f%15.3f\n", m->n.z, m->o.z, m->a.z); +} + diff --git a/lwrserv/matlab/rvctools/robot/mex/vmath.h b/lwrserv/matlab/rvctools/robot/mex/vmath.h new file mode 100644 index 0000000..eb38cd0 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mex/vmath.h @@ -0,0 +1,50 @@ +/** + * \file vmath.h + * \author Peter Corke + * \brief Simple vector/matrix maths library. + * + * \note All vectors and matrices are passed by reference. + */ + +/* + * Copyright (C) 1999-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 . + * + */ +#ifndef _vmath_h_ +#define _vmath_h_ +typedef struct vector { + double x, y, z; +} Vect; + +typedef struct matrix { + Vect n, o, a; +} Rot; + +typedef struct homogeneous_matrix { + Vect n, o, a, p; +} Transform; +void vect_cross (Vect *r, Vect *a, Vect *b); +double vect_dot (Vect *a, Vect *b); +void vect_add (Vect *r, Vect *a, Vect *b); +void scal_mult (Vect *r, Vect *a, double s); +void rot_vect_mult (Vect *r, Rot *m, Vect *v); +void rot_trans_vect_mult (Vect *r, Rot *m, Vect *v); +void mat_vect_mult (Vect *r, double *m, Vect *v); +void rot_print(char *s, Rot *m); +void vect_print(char *s, Vect *v); +#endif diff --git a/lwrserv/matlab/rvctools/robot/mstraj.m b/lwrserv/matlab/rvctools/robot/mstraj.m new file mode 100644 index 0000000..fe51533 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mstraj.m @@ -0,0 +1,203 @@ +%MSTRAJ Multi-segment multi-axis trajectory +% +% TRAJ = MSTRAJ(P, QDMAX, Q0, DT, TACC, OPTIONS) is a multi-segment trajectory (KxN) +% based on via points P (MxN) and axis velocity limits QDMAX (1xN). The +% path comprises linear segments with polynomial blends. The output +% trajectory matrix has one row per time step, and one column per axis. +% +% - P (MxN) is a matrix of via points, 1 row per via point, one column +% per axis. The last via point is the destination. +% - QDMAX (1xN) are axis velocity limits which cannot be exceeded, or +% - QDMAX (Mx1) are the durations for each of the M segments +% - Q0 (1xN) are the initial axis coordinates +% - DT is the time step +% - TACC (1x1) this acceleration time is applied to all segment transitions +% - TACC (1xM) acceleration time for each segment, TACC(i) is the acceleration +% time for the transition from segment i to segment i+1. TACC(1) is also +% the acceleration time at the start of segment 1. +% +% TRAJ = MSTRAJ(SEGMENTS, QDMAX, Q0, DT, TACC, QD0, QDF, OPTIONS) as above but +% additionally specifies the initial and final axis velocities (1xN). +% +% Options:: +% 'verbose' Show details. +% +% Notes:: +% - If no output arguments are specified the trajectory is plotted. +% - The path length K is a function of the number of via points, Q0, DT +% and TACC. +% - The final via point P(M,:) is the destination. +% - The motion has M segments from Q0 to P(1,:) to P(2,:) to P(M,:). +% - All axes reach their via points at the same time. +% - Can be used to create joint space trajectories where each axis is a joint +% coordinate. +% - Can be used to create Cartesian trajectories with the "axes" assigned +% to translation and orientation in RPY or Euler angle form. +% +% See also MSTRAJ, LSPB, CTRAJ. + + +% 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 . + +function [TG, taxis] = mstraj(segments, qdmax, tsegment, q, dt, Tacc, varargin) + + + ns = numrows(segments); + nj = numcols(segments); + + if ~isempty(qdmax) && ~isempty(tsegment) + error('Can only specify one of qdmax or tsegment'); + end + if isempty(qdmax) && isempty(tsegment) + error('Must specify one of qdmax or tsegment'); + end + + [opt,args] = tb_optparse([], varargin); + + if length(args) > 0 + qd0 = args{1}; + else + qd0 = zeros(1, nj); + end + if length(args) > 1 + qdf = args{2}; + else + qdf = zeros(1, nj); + end + + % set the initial conditions + q_prev = q; + qd_prev = qd0; + + clock = 0; % keep track of time + arrive = []; % record planned time of arrival at via points + + tg = []; + taxis = []; + + for seg=1:ns + if opt.verbose + fprintf('------------------- segment %d\n', seg); + end + + % set the blend time, just half an interval for the first segment + + if length(Tacc) > 1 + tacc = Tacc(seg); + else + tacc = Tacc; + end + + tacc = ceil(tacc/dt)*dt; + tacc2 = ceil(tacc/2/dt) * dt; + if seg == 1 + taccx = tacc2; + else + taccx = tacc; + end + + % estimate travel time + % could better estimate distance travelled during the blend + q_next = segments(seg,:); % current target + dq = q_next - q_prev; % total distance to move this segment + + %% probably should iterate over the next section to get qb right... + % while 1 + % qd_next = (qnextnext - qnext) + % tb = abs(qd_next - qd) ./ qddmax; + % qb = f(tb, max acceleration) + % dq = q_next - q_prev - qb + % tl = abs(dq) ./ qdmax; + + if ~isempty(qdmax) + % qdmax is specified, compute slowest axis + + qb = taccx * qdmax / 2; % distance moved during blend + tb = taccx; + + % convert to time + tl = abs(dq) ./ qdmax; + %tl = abs(dq - qb) ./ qdmax; + tl = ceil(tl/dt) * dt; + + % find the total time and slowest axis + tt = tb + tl; + [tseg,slowest] = max(tt); + taxis(seg,:) = tt; + + % best if there is some linear motion component + if tseg <= 2*tacc + tseg = 2 * tacc; + end + elseif ~isempty(tsegment) + % segment time specified, use that + tseg = tsegment(seg); + slowest = NaN; + end + + % log the planned arrival time + arrive(seg) = clock + tseg; + if seg > 1 + arrive(seg) = arrive(seg) + tacc2; + end + + if opt.verbose + fprintf('seg %d, slowest axis %d, time required %.4g\n', ... + seg, slowest, tseg); + end + + %% create the trajectories for this segment + + % linear velocity from qprev to qnext + qd = dq / tseg; + + % add the blend polynomial + qb = jtraj(q, q_prev+tacc2*qd, 0:dt:taccx, qd_prev, qd); + tg = [tg; qb(2:end,:)]; + + clock = clock + taccx; % update the clock + + % add the linear part, from tacc/2+dt to tseg-tacc/2 + for t=tacc2+dt:dt:tseg-tacc2 + s = t/tseg; + q = (1-s) * q_prev + s * q_next; % linear step + tg = [tg; q]; + clock = clock + dt; + end + + q_prev = q_next; % next target becomes previous target + qd_prev = qd; + end + % add the final blend + qb = jtraj(q, q_next, 0:dt:tacc2, qd_prev, qdf); + tg = [tg; qb(2:end,:)]; + + % plot a graph if no output argument + if nargout == 0 + t = (0:numrows(tg)-1)'*dt; + clf + plot(t, tg, '-o'); + hold on + plot(arrive, segments, 'bo', 'MarkerFaceColor', 'k'); + hold off + grid + xlabel('time'); + xaxis(t(1), t(end)) + else + TG = tg; + end diff --git a/lwrserv/matlab/rvctools/robot/mtraj.m b/lwrserv/matlab/rvctools/robot/mtraj.m new file mode 100644 index 0000000..8b22d97 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/mtraj.m @@ -0,0 +1,102 @@ +%MTRAJ Multi-axis trajectory between two points +% +% [Q,QD,QDD] = MTRAJ(TFUNC, Q0, QF, M) is a multi-axis trajectory (MxN) varying +% from state Q0 (1xN) to QF (1xN) according to the scalar trajectory function +% TFUNC in M steps. Joint velocity and acceleration can be optionally returned as +% QD (MxN) and QDD (MxN) respectively. The trajectory outputs have one row per +% time step, and one column per axis. +% +% The shape of the trajectory is given by the scalar trajectory function TFUNC +% [S,SD,SDD] = TFUNC(S0, SF, M); +% and possible values of TFUNC include @lspb for a trapezoidal trajectory, or +% @tpoly for a polynomial trajectory. +% +% [Q,QD,QDD] = MTRAJ(TFUNC, Q0, QF, T) as above but specifies the trajectory +% length in terms of the length of the time vector T (Mx1). +% +% Notes:: +% - If no output arguments are specified Q, QD, and QDD are plotted. +% - When TFUNC is @tpoly the result is functionally equivalent to JTRAJ except +% that no initial velocities can be specified. JTRAJ is computationally a little +% more efficient. +% +% See also JTRAJ, MSTRAJ, LSPB, TPOLY. + +% 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 . + +function [S,Sd,Sdd] = mtraj(tfunc, q0, qf, M) + + if ~isa(tfunc, 'function_handle') + error('first argument must be a function handle'); + end + + M0 = M; + if ~isscalar(M) + M = length(M); + end + if numcols(q0) ~= numcols(qf) + error('must be same number of columns in q0 and qf') + end + + s = zeros(M, numcols(q0)); + sd = zeros(M, numcols(q0)); + sdd = zeros(M, numcols(q0)); + + for i=1:numcols(q0) + % for each axis + [s(:,i),sd(:,i),sdd(:,i)] = tfunc(q0(i), qf(i), M); + end + +% - If no output arguments are specified S, SD, and SDD are plotted +% against time. + + switch nargout + case 0 + clf + + if isscalar(M0) + t = [1:M0]'; + else + t = M0; + end + subplot(311) + plot(t, s); grid; ylabel('s'); + + subplot(312) + plot(t, sd); grid; ylabel('sd'); + + subplot(313) + plot(t, sdd); grid; ylabel('sdd'); + if ~isscalar(M0) + xlabel('time') + else + for c=get(gcf, 'Children'); + set(c, 'XLim', [1 M0]); + end + end + shg + case 1 + S = s; + case 2 + S = s; + Sd = sd; + case 3 + S = s; + Sd = sd; + Sdd = sdd; + end diff --git a/lwrserv/matlab/rvctools/robot/oa2r.m b/lwrserv/matlab/rvctools/robot/oa2r.m new file mode 100644 index 0000000..5676c4d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/oa2r.m @@ -0,0 +1,42 @@ +%OA2R Convert orientation and approach vectors to rotation matrix +% +% R = OA2R(O, A) is a rotation matrix for the specified orientation and +% approach vectors (3x1) formed from 3 vectors such that R = [N O A] and +% N = O x A. +% +% Notes:: +% - The submatrix is guaranteed to be orthonormal so long as O and A +% are not parallel. +% - The vectors O and A are parallel to the Y- and Z-axes of the coordinate +% frame. +% +% See also RPY2R, EUL2R, OA2TR. + + +% 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 . + +function R = oa2r(o, a) + + if nargin < 2 || ~isvec(o) || ~isvec(a) + error('RTB:oa2r:badarg', 'bad arguments'); + end + + o = o(:); a = a(:); + n = cross(o, a); + o = cross(a, n); + R = [unit(n(:)) unit(o(:)) unit(a(:))]; diff --git a/lwrserv/matlab/rvctools/robot/oa2tr.m b/lwrserv/matlab/rvctools/robot/oa2tr.m new file mode 100644 index 0000000..1edd459 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/oa2tr.m @@ -0,0 +1,40 @@ +%OA2TR Convert orientation and approach vectors to homogeneous transformation +% +% T = OA2TR(O, A) is a homogeneous tranformation for the specified orientation +% and approach vectors (3x1) formed from 3 vectors such that R = [N O A] and +% N = O x A. +% +% Notes:: +% - The rotation submatrix is guaranteed to be orthonormal so long as O and A +% are not parallel. +% - The translational part is zero. +% - The vectors O and A are parallel to the Y- and Z-axes of the coordinate +% frame. +% +% See also RPY2TR, EUL2TR, OA2R. + + +% 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 . + +function r = oa2tr(o, a) + if nargin < 2 + error('RTB:oa2tr:badarg', 'bad arguments'); + end + n = cross(o, a); + o = cross(a, n); + r = [unit(n(:)) unit(o(:)) unit(a(:)) zeros(3,1); 0 0 0 1]; diff --git a/lwrserv/matlab/rvctools/robot/plot_vehicle.m b/lwrserv/matlab/rvctools/robot/plot_vehicle.m new file mode 100644 index 0000000..653b118 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/plot_vehicle.m @@ -0,0 +1,55 @@ +%plot_vehicle Plot ground vehicle pose +% +% plot_vehicle(X,OPTIONS) draw representation of ground robot as an +% oriented triangle with pose X (1x3) [x,y,theta] or X (3x3) as homogeneous +% transform in SE(2). +% +% Options:: +% 'scale',S Draw vehicle with length S x maximum axis dimension +% 'size',S Draw vehicle with length S +% +% See also Vehicle.plot. + +% TODO needs to work for 3D point + +function plot_vehicle(x, varargin) + + opt.scale = 1/60; + opt.size = []; + + [opt,args] = tb_optparse(opt, varargin); + + % get the current axes + a = axis; + + % compute the dimensions of the robot + if ~isempty(opt.size) + d = opt.size; + else + d = (a(2)+a(4) - a(1)-a(3)) * opt.scale; + end + + if numel(x) == 3 + % convert vector form of pose to SE(2) + T = se2(x(1), x(2), x(3)); + else + T = x; + end + + % draw it +% points = [ +% d 0 1 +% -d -0.6*d 1 +% -d 0.6*d 1 +% d 0 1]'; + + points = [ + d 0 + -d -0.6*d + -d 0.6*d]'; + + points = homtrans(T, points); + + plot_poly(points, args{:}); + + end diff --git a/lwrserv/matlab/rvctools/robot/plotbotopt.m b/lwrserv/matlab/rvctools/robot/plotbotopt.m new file mode 100644 index 0000000..c8df62e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/plotbotopt.m @@ -0,0 +1,27 @@ +%PLOTBOTOPT Define default options for robot plotting +% +% A user provided function that returns a cell array of default +% plot options for the SerialLink.plot method. +% +% See also SerialLink.plot. + + +% 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 . + +function options = plotbotopt + options = {'base' 'perspective'}; diff --git a/lwrserv/matlab/rvctools/robot/qplot.m b/lwrserv/matlab/rvctools/robot/qplot.m new file mode 100644 index 0000000..21dc32c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/qplot.m @@ -0,0 +1,45 @@ +%QPLOT Plot joint angles +% +% QPLOT(Q) is a convenience function to plot joint angle trajectories (Mx6) for +% a 6-axis robot, where each row represents one time step. +% +% The first three joints are shown as solid lines, the last three joints (wrist) +% are shown as dashed lines. A legend is also displayed. +% +% QPLOT(T, Q) as above but displays the joint angle trajectory versus time T (Mx1). +% +% See also JTRAJ, PLOT. + +% 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 . + +function qplot(t, q) + if nargin < 2 + q = t; + t = (1:numrows(q))'; + end + clf + hold on + plot(t, q(:,1:3)) + plot(t, q(:,4:6), '--') + grid on + xlabel('time') + ylabel('q') + legend('q1', 'q2', 'q3', 'q4', 'q5', 'q6'); + hold off + + xlim([t(1), t(end)]); diff --git a/lwrserv/matlab/rvctools/robot/r2t.m b/lwrserv/matlab/rvctools/robot/r2t.m new file mode 100644 index 0000000..546c025 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/r2t.m @@ -0,0 +1,57 @@ +%R2T Convert rotation matrix to a homogeneous transform +% +% T = R2T(R) is a homogeneous transform equivalent to an orthonormal +% rotation matrix R with a zero translational component. +% +% Notes:: +% - Works for T in either SE(2) or SE(3) +% - if R is 2x2 then T is 3x3, or +% - if R is 3x3 then T is 4x4. +% - Translational component is zero. +% - For a rotation matrix sequence returns a homogeneous transform +% sequence. +% +% See also T2R. + + +% 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 . + +function T = r2t(R) + + % check dimensions: R is SO(2) or SO(3) + d = size(R); + if d(1) ~= d(2) + error('matrix must be square'); + end + if ~any(d(1) == [2 3]) + error('argument is not a rotation matrix (sequence)'); + end + + Z = zeros(d(1),1); + B = [Z' 1]; + + if numel(d) == 2 + % single matrix case + T = [R Z; B]; + else + % matrix sequence case + T = zeros(4,4,d(3)); + for i=1:d(3) + T(:,:,i) = [R(:,:,i) Z; B]; + end + end diff --git a/lwrserv/matlab/rvctools/robot/robot.pdf b/lwrserv/matlab/rvctools/robot/robot.pdf new file mode 100644 index 0000000..cc6b86c Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/robot.pdf differ diff --git a/lwrserv/matlab/rvctools/robot/rot2.m b/lwrserv/matlab/rvctools/robot/rot2.m new file mode 100644 index 0000000..50a6876 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/rot2.m @@ -0,0 +1,42 @@ +%ROT2 SO(2) Rotation matrix +% +% R = ROT2(THETA) is an SO(2) rotation matrix representing a rotation of THETA +% radians. +% +% R = ROT2(THETA, 'deg') as above but THETA is in degrees. +% +% See also TROT2, ROTX, ROTY, ROTZ. + + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function R = rot2(t, deg) + + if nargin > 1 && strcmp(deg, 'deg') + t = t *pi/180; + end + + ct = cos(t); + st = sin(t); + R = [ + ct -st + st ct + ]; diff --git a/lwrserv/matlab/rvctools/robot/rotx.m b/lwrserv/matlab/rvctools/robot/rotx.m new file mode 100644 index 0000000..f592040 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/rotx.m @@ -0,0 +1,40 @@ +%ROTX Rotation about X axis +% +% R = ROTX(THETA) is a rotation matrix representing a rotation of THETA +% radians about the x-axis. +% +% R = ROTX(THETA, 'deg') as above but THETA is in degrees. +% +% See also ROTY, ROTZ, ANGVEC2R. + + +% 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 . + +function R = rotx(t, deg) + + if nargin > 1 && strcmp(deg, 'deg') + t = t *pi/180; + end + + ct = cos(t); + st = sin(t); + R = [ + 1 0 0 + 0 ct -st + 0 st ct + ]; diff --git a/lwrserv/matlab/rvctools/robot/roty.m b/lwrserv/matlab/rvctools/robot/roty.m new file mode 100644 index 0000000..b963343 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/roty.m @@ -0,0 +1,38 @@ +%ROTY Rotation about Y axis +% +% R = ROTY(THETA) is a rotation matrix representing a rotation of THETA +% radians about the y-axis. +% +% R = ROTY(THETA, 'deg') as above but THETA is in degrees. +% +% See also ROTX, ROTZ, ANGVEC2R. + + +% 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 . + +function R = roty(t, deg) + if nargin > 1 && strcmp(deg, 'deg') + t = t *pi/180; + end + ct = cos(t); + st = sin(t); + R = [ + ct 0 st + 0 1 0 + -st 0 ct + ]; diff --git a/lwrserv/matlab/rvctools/robot/rotz.m b/lwrserv/matlab/rvctools/robot/rotz.m new file mode 100644 index 0000000..e954432 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/rotz.m @@ -0,0 +1,39 @@ +%ROTZ Rotation about Z axis +% +% R = ROTZ(THETA) is a rotation matrix representing a rotation of THETA +% radians about the z-axis. +% +% R = ROTZ(THETA, 'deg') as above but THETA is in degrees. +% +% See also ROTX, ROTY, ANGVEC2R. + + +% 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 . + +function R = rotz(t, deg) + if nargin > 1 && strcmp(deg, 'deg') + t = t *pi/180; + end + + ct = cos(t); + st = sin(t); + R = [ + ct -st 0 + st ct 0 + 0 0 1 + ]; diff --git a/lwrserv/matlab/rvctools/robot/rpy2jac.m b/lwrserv/matlab/rvctools/robot/rpy2jac.m new file mode 100644 index 0000000..5e670c1 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/rpy2jac.m @@ -0,0 +1,47 @@ +%RPY2JAC Jacobian from RPY angle rates to angular velocity +% +% J = RPY2JAC(EUL) is a Jacobian matrix (3x3) that maps roll-pitch-yaw angle +% rates to angular velocity at the operating point RPY=[R,P,Y]. +% +% J = RPY2JAC(R, P, Y) as above but the roll-pitch-yaw angles are passed +% as separate arguments. +% +% Notes:: +% - Used in the creation of an analytical Jacobian. +% +% See also EUL2JAC, SerialLink.JACOBN. + + +% 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 . + +function J = rpy2jac(r, p, y) + + if length(r) == 3 + % rpy2jac([r,p,y]) + p = r(2); + y = r(3); + r = r(1); + elseif nargin ~= 3 + error('RTB:rpy2jac:badarg', 'bad arguments'); + end + J = [ + 1 0 sin(p) + 0 cos(r) -cos(p)*sin(r) + 0 sin(r) cos(p)*cos(r) + ]; + diff --git a/lwrserv/matlab/rvctools/robot/rpy2r.m b/lwrserv/matlab/rvctools/robot/rpy2r.m new file mode 100644 index 0000000..714ed77 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/rpy2r.m @@ -0,0 +1,88 @@ +%RPY2R Roll-pitch-yaw angles to rotation matrix +% +% R = RPY2R(RPY, OPTIONS) is an orthonormal rotation matrix equivalent to the +% specified roll, pitch, yaw angles which correspond to rotations about the +% X, Y, Z axes respectively. If RPY has multiple rows they are assumed to +% represent a trajectory and R is a three dimensional matrix, where the last index +% corresponds to the rows of RPY. +% +% R = RPY2R(ROLL, PITCH, YAW, OPTIONS) as above but the roll-pitch-yaw angles +% are passed as separate arguments. If ROLL, PITCH and YAW are column vectors +% they are assumed to represent a trajectory and R is a three dimensional matrix, +% where the last index corresponds to the rows of ROLL, PITCH, YAW. +% +% Options:: +% 'deg' Compute angles in degrees (radians default) +% 'zyx' Return solution for sequential rotations about Z, Y, X axes (Paul book) +% +% Note:: +% - In previous releases (<8) the angles corresponded to rotations about ZYX. Many +% texts (Paul, Spong) use the rotation order ZYX. This old behaviour can be enabled +% by passing the option 'zyx' +% +% See also TR2RPY, EUL2TR. + + +% 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 . + +function R = rpy2r(roll, varargin) + opt.zyx = false; + opt.deg = false; + [opt,args] = tb_optparse(opt, varargin); + + % unpack the arguments + if numcols(roll) == 3 + pitch = roll(:,2); + yaw = roll(:,3); + roll = roll(:,1); + elseif nargin >= 3 + pitch = args{1}; + yaw = args{2}; + else + error('bad arguments') + end + + % optionally convert from degrees + if opt.deg + d2r = pi/180.0; + roll = roll * d2r; + pitch = pitch * d2r; + yaw = yaw * d2r; + end + + if ~opt.zyx + % XYZ order + if numrows(roll) == 1 + R = rotx(roll) * roty(pitch) * rotz(yaw); + else + R = zeros(3,3,numrows(roll)); + for i=1:numrows(roll) + R(:,:,i) = rotx(roll(i)) * roty(pitch(i)) * rotz(yaw(i)); + end + end + else + % old ZYX order (as per Paul book) + if numrows(roll) == 1 + R = rotz(roll) * roty(pitch) * rotx(yaw); + else + R = zeros(3,3,numrows(roll)); + for i=1:numrows(roll) + R(:,:,i) = rotz(roll(i)) * roty(pitch(i)) * rotx(yaw(i)); + end + end + end diff --git a/lwrserv/matlab/rvctools/robot/rpy2tr.m b/lwrserv/matlab/rvctools/robot/rpy2tr.m new file mode 100644 index 0000000..ea33b2a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/rpy2tr.m @@ -0,0 +1,45 @@ +%RPY2TR Roll-pitch-yaw angles to homogeneous transform +% +% T = RPY2TR(RPY, OPTIONS) is a homogeneous transformation equivalent to the +% specified roll, pitch, yaw angles which correspond to rotations about the +% X, Y, Z axes respectively. If RPY has multiple rows they are assumed to +% represent a trajectory and T is a three dimensional matrix, where the last index +% corresponds to the rows of RPY. +% +% T = RPY2TR(ROLL, PITCH, YAW, OPTIONS) as above but the roll-pitch-yaw angles +% are passed as separate arguments. If ROLL, PITCH and YAW are column vectors +% they are assumed to represent a trajectory and T is a three dimensional matrix, +% where the last index corresponds to the rows of ROLL, PITCH, YAW. +% +% Options:: +% 'deg' Compute angles in degrees (radians default) +% 'zyx' Return solution for sequential rotations about Z, Y, X axes (Paul book) +% +% Note:: +% - In previous releases (<8) the angles corresponded to rotations about ZYX. Many +% texts (Paul, Spong) use the rotation order ZYX. This old behaviour can be enabled +% by passing the option 'zyx' +% +% See also TR2RPY, RPY2R, EUL2TR. + +% 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 . + +function T = rpy2tr(roll, varargin) + + R = rpy2r(roll, varargin{:}); + T = r2t(R); diff --git a/lwrserv/matlab/rvctools/robot/rt2tr.m b/lwrserv/matlab/rvctools/robot/rt2tr.m new file mode 100644 index 0000000..1be7f1d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/rt2tr.m @@ -0,0 +1,56 @@ +%RT2TR Convert rotation and translation to homogeneous transform +% +% TR = RT2TR(R, t) is a homogeneous transformation matrix (MxM) formed from an +% orthonormal rotation matrix R (NxN) and a translation vector t (Nx1) where +% M=N+1. +% +% For a sequence R (NxNxK) and t (kxN) results in a transform sequence (NxNxk). +% +% Notes:: +% - Works for R in SO(2) or SO(3) +% - If R is 2x2 and t is 2x1, then TR is 3x3 +% - If R is 3x3 and t is 3x1, then TR is 4x4 +% - The validity of R is not checked +% +% See also T2R, R2T, TR2RT. + +% 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 . + +function T = rt2tr(R, t) + if numcols(R) ~= numrows(R) + error('R must be square'); + end + if numrows(R) ~= numrows(t) + error('R and t must have the same number of rows'); + end + + if size(R,3) ~= numcols(t) + error('For sequence size(R,3) must equal size(t,2)'); + end + + if size(R,3) > 1 + Z = zeros(numcols(R),1); + B = [Z' 1]; + T = zeros(4,4,size(R,3)); + for i=1:size(R,3) + T(:,:,i) = [R(:,:,i) t(:,i); B]; + end + else + T = [R t; zeros(1,numcols(R)) 1]; + end + diff --git a/lwrserv/matlab/rvctools/robot/rtbdemo.m b/lwrserv/matlab/rvctools/robot/rtbdemo.m new file mode 100644 index 0000000..4760a19 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/rtbdemo.m @@ -0,0 +1,122 @@ +%RTBDEMO Robot toolbox demonstrations +% +% Displays popup menu of toolbox demonstration scripts that illustrate: +% - homogeneous transformations +% - trajectories +% - forward kinematics +% - inverse kinematics +% - robot animation +% - inverse dynamics +% - forward dynamics +% +% Notes:: +% - The scripts require the user to periodically hit in order to move +% through the explanation. +% - Set PAUSE OFF if you want the scripts to run completely automatically. + +% 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 . + +echo off +clear all +delete( get(0, 'Children') ); + +% find the path to the demos +if exist('rtbdemo', 'file') == 2 + tbpath = fileparts(which('tr2eul')); + demopath = fullfile(tbpath, 'demos'); +end + +opts = {'path', demopath}; + + +fprintf('------------------------------------------------------------\n'); +fprintf('Many of these demos print tutorial text and MATLAB commmands in the console window.\n'); +fprintf('Read the text and press to move on to the next command\n'); +fprintf('At the end of the tutorial/demo you can choose the next one from the graphical menu.\n'); +fprintf('------------------------------------------------------------\n'); + +while 1 + selection = menu('Robot Toolbox demonstrations', ... + 'General/Rotations', ... + 'General/Transformations', ... + 'General/Trajectory', ... + 'Arm/Robots', ... + 'Arm/Animation', ... + 'Arm/Forward kinematics', ... + 'Arm/Inverse kinematics', ... + 'Arm/Jacobians', ... + 'Arm/Inverse dynamics', ... + 'Arm/Forward dynamics', ... + 'Arm/Symbolic', ... + 'Arm/Code generation', ... + 'Mobile/driving to a pose', ... + 'Mobile/quadrotor', ... + 'Mobile/Braitenberg', ... + 'Mobile/Bug', ... + 'Mobile/D*', ... + 'Mobile/PRM', ... + 'Mobile/SLAM', ... + 'Mobile/Particle filter', ... + 'Exit'); + + switch selection + case 1 + runscript('rotation', opts{:}) + case 2 + runscript('trans', opts{:}) + case 3 + runscript('traj', opts{:}) + case 4 + runscript('robot', opts{:}) + case 5 + runscript('fkine', opts{:}) + case 6 + runscript('graphics', opts{:}) + case 7 + runscript('ikine', opts{:}) + case 8 + runscript('jacob', opts{:}) + case 9 + runscript('idyn', opts{:}) + case 10 + runscript('fdyn', opts{:}) + case 11 + runscript('symbolic', opts{:}) + case 12 + runscript('codegen', opts{:}) + case 13 + runscript('drivepose', opts{:}) + case 14 + runscript('quadrotor', opts{:}) + case 15 + runscript('braitnav', opts{:}) + case 16 + runscript('bugnav', opts{:}) + case 17 + runscript('dstarnav', opts{:}) + case 18 + runscript('prmnav', opts{:}) + case 19 + runscript('slam', opts{:}) + case 20 + runscript('particlefilt', opts{:}) + case 21 + delete( get(0, 'Children') ); + break; + end +end diff --git a/lwrserv/matlab/rvctools/robot/rtbdemo_gui.fig b/lwrserv/matlab/rvctools/robot/rtbdemo_gui.fig new file mode 100644 index 0000000..1eb9a58 Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/rtbdemo_gui.fig differ diff --git a/lwrserv/matlab/rvctools/robot/se2.m b/lwrserv/matlab/rvctools/robot/se2.m new file mode 100644 index 0000000..8062638 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/se2.m @@ -0,0 +1,59 @@ +%SE2 Create planar translation and rotation transformation +% +% T = SE2(X, Y, THETA) is a 3x3 homogeneous transformation SE(2) +% representing translation X and Y, and rotation THETA in the plane. +% +% T = SE2(XY) as above where XY=[X,Y] and rotation is zero +% +% T = SE2(XY, THETA) as above where XY=[X,Y] +% +% T = SE2(XYT) as above where XYT=[X,Y,THETA] +% +% See also TRPLOT2. + +% 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 . + +function t = se2(a, b, c) + + if length(a) == 3 + x = a(1); + y = a(2); + th = a(3); + elseif length(a) == 2 + x = a(1); + y = a(2); + if nargin < 2 + th = 0; + else + th = b; + end + else + x = a; + y = b; + if nargin < 3 + th = 0; + else + th = c; + end + end + + cth = cos(th); + sth = sin(th); + R = [cth -sth; sth cth]; + + t = [R [x; y]; 0 0 1]; diff --git a/lwrserv/matlab/rvctools/robot/se3.m b/lwrserv/matlab/rvctools/robot/se3.m new file mode 100644 index 0000000..1014dee --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/se3.m @@ -0,0 +1,31 @@ +%SE3 Lift SE(2) transform to SE(3) +% +% T3 = SE3(T2) returns a homogeneous transform (4x4) that represents +% the same X,Y translation and Z rotation as does T2 (3x3). +% +% See also SE2, TRANSL, ROTX. + + +% Copyright (C) 1993-2015, 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.petercorke.com +function T = se3(x) + + if all(size(x) == [3 3]) + T = [x(1:2,1:2) [0 0]' x(1:2,3); 0 0 1 0; 0 0 0 1]; + end diff --git a/lwrserv/matlab/rvctools/robot/skew.m b/lwrserv/matlab/rvctools/robot/skew.m new file mode 100644 index 0000000..a1cb8a6 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/skew.m @@ -0,0 +1,40 @@ +%SKEW Create skew-symmetric matrix +% +% S = SKEW(V) is a skew-symmetric matrix formed from V (3x1). +% +% | 0 -vz vy| +% | vz 0 -vx| +% |-vy vx 0 | +% +% See also VEX. + + +% 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 . + +function S = skew(v) + if isvec(v,3) + % SO(3) case + S = [ 0 -v(3) v(2) + v(3) 0 -v(1) + -v(2) v(1) 0]; + elseif isvec(v,1) + % SO(2) case + S = [0 -v; v 0]; + else + error('argument must be a 1- or 3-vector'); + end diff --git a/lwrserv/matlab/rvctools/robot/startup_rtb.m b/lwrserv/matlab/rvctools/robot/startup_rtb.m new file mode 100644 index 0000000..54f6d8b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/startup_rtb.m @@ -0,0 +1,21 @@ +%STARTUP_RVC Initialize MATLAB paths for Robotics Toolbox +% +% Adds demos, examples to the MATLAB path, and adds also to +% Java class path. +release = load('RELEASE'); +fprintf('- Robotics Toolbox for Matlab (release %.1f)\n', release) +tbpath = fileparts(which('Link')); +addpath( fullfile(tbpath, 'demos') ); +addpath( fullfile(tbpath, 'examples') ); +addpath( fullfile(tbpath, 'mex') ); +javaaddpath( fullfile(tbpath, 'DH.jar') ); +%currentversion = urlread('http://www.petercorke.com/RTB/currentversion.php'); +currentversion = '0'; +currentversion = str2double(currentversion); +%{ +if release ~= currentversion + fprintf('** Release %.1f now available\n\n', ... + currentversion); +end +%} +clear release currentversion diff --git a/lwrserv/matlab/rvctools/robot/t2r.m b/lwrserv/matlab/rvctools/robot/t2r.m new file mode 100644 index 0000000..c9bf9f9 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/t2r.m @@ -0,0 +1,55 @@ +%T2R Return rotational submatrix of a homogeneous transformation +% +% R = T2R(T) is the orthonormal rotation matrix component of homogeneous +% transformation matrix T: +% +% Notes:: +% - Works for T in SE(2) or SE(3) +% - If T is 4x4, then R is 3x3. +% - If T is 3x3, then R is 2x2. +% - The validity of rotational part is not checked +% - For a homogeneous transform sequence returns a rotation matrix sequence +% +% See also R2T, TR2RT, RT2TR. + + +% 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 . + +function R = t2r(T) + + % check dimensions: T is SE(2) or SE(3) + d = size(T); + if d(1) ~= d(2) + error('RTB:t2r:badarg', 'matrix must be square'); + end + if ~any(d(1) == [3 4]) + error('RTB:t2r:badarg', 'argument is not a homogeneous transform (sequence)'); + end + + n = d(1); % works for SE(2) or SE(3) + + if numel(d) == 2 + % single matrix case + R = T(1:n-1,1:n-1); + else + % matrix sequence case + R = zeros(3,3,d(3)); + for i=1:d(3) + R(:,:,i) = T(1:n-1,1:n-1,i); + end + end diff --git a/lwrserv/matlab/rvctools/robot/tpoly.m b/lwrserv/matlab/rvctools/robot/tpoly.m new file mode 100644 index 0000000..2fdff6d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tpoly.m @@ -0,0 +1,115 @@ +%TPOLY Generate scalar polynomial trajectory +% +% [S,SD,SDD] = TPOLY(S0, SF, M) is a scalar trajectory (Mx1) that varies +% smoothly from S0 to SF in M steps using a quintic (5th order) polynomial. +% Velocity and acceleration can be optionally returned as SD (Mx1) and SDD (Mx1). +% +% [S,SD,SDD] = TPOLY(S0, SF, T) as above but specifies the trajectory in +% terms of the length of the time vector T (Mx1). +% +% Notes:: +% - If no output arguments are specified S, SD, and SDD are plotted. + +% 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 . + +% [S,SD,SDD] = TPOLY(S0, SF, N, SD0, SDF) as above but specifies initial +% and final joint velocity for the trajectory. +% +% [S,SD,SDD] = TPOLY(S0, SF, T, SD0, SDF) as above but specifies initial +% and final joint velocity for the trajectory and time vector T. +% +% Notes:: +% - In all cases if no output arguments are specified S, SD, and SDD are plotted +% against time. +% +% See also LSPB, JTRAJ. + +function [s,sd,sdd] = tpoly(q0, qf, t, qd0, qdf) + + t0 = t; + if isscalar(t) + t = (0:t-1)'; + else + t = t(:); + end + if nargin < 4 + qd0 = 0; + end + if nargin < 5 + qdf = 0; + end + + tf = max(t); + + % solve for the polynomial coefficients using least squares + X = [ + 0 0 0 0 0 1 + tf^5 tf^4 tf^3 tf^2 tf 1 + 0 0 0 0 1 0 + 5*tf^4 4*tf^3 3*tf^2 2*tf 1 0 + 0 0 0 2 0 0 + 20*tf^3 12*tf^2 6*tf 2 0 0 + ]; + coeffs = (X \ [q0 qf qd0 qdf 0 0]')'; + + % coefficients of derivatives + coeffs_d = coeffs(1:5) .* (5:-1:1); + coeffs_dd = coeffs_d(1:4) .* (4:-1:1); + + % evaluate the polynomials + p = polyval(coeffs, t); + pd = polyval(coeffs_d, t); + pdd = polyval(coeffs_dd, t); + + 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) + plot(xt, p); grid; ylabel('s'); + + 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 diff --git a/lwrserv/matlab/rvctools/robot/tr2angvec.m b/lwrserv/matlab/rvctools/robot/tr2angvec.m new file mode 100644 index 0000000..8f2f1f8 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tr2angvec.m @@ -0,0 +1,104 @@ +%TR2ANGVEC Convert rotation matrix to angle-vector form +% +% [THETA,V] = TR2ANGVEC(R) converts an orthonormal rotation matrix R into a +% rotation of THETA (1x1) about the axis V (1x3). +% +% [THETA,V] = TR2ANGVEC(T) as above but uses the rotational part of the +% homogeneous transform T. +% +% If R (3x3xK) or T (4x4xK) represent a sequence then THETA (Kx1)is a vector +% of angles for corresponding elements of the sequence and V (Kx3) are the +% corresponding axes, one per row. +% +% Notes:: +% - If no output arguments are specified the result is displayed. +% - This algorithm is from Paul 1981, other solutions are possible using +% eigenvectors or Rodriguez formula. +% +% See also ANGVEC2R, ANGVEC2TR. + + +% 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 . + +function [theta, v] = tr2angvec(R) + + if ~isrot(R) + R = t2r(R); + end + + if size(R,3) > 1 + theta = zeros(size(R,3),1); + v = zeros(size(R,3),3); + for i=1:size(R,3) + [t,a] = tr2angvec(R(:,:,i)); + theta(i) = t; + v(i,:) = a; + end + return + end + + qs = sqrt(trace(R)+1)/2.0; + qs = min(qs, 1); + + kx = R(3,2) - R(2,3); % Oz - Ay + ky = R(1,3) - R(3,1); % Ax - Nz + kz = R(2,1) - R(1,2); % Ny - Ox + + if (R(1,1) >= R(2,2)) && (R(1,1) >= R(3,3)) + kx1 = R(1,1) - R(2,2) - R(3,3) + 1; % Nx - Oy - Az + 1 + ky1 = R(2,1) + R(1,2); % Ny + Ox + kz1 = R(3,1) + R(1,3); % Nz + Ax + add = (kx >= 0); + elseif (R(2,2) >= R(3,3)) + kx1 = R(2,1) + R(1,2); % Ny + Ox + ky1 = R(2,2) - R(1,1) - R(3,3) + 1; % Oy - Nx - Az + 1 + kz1 = R(3,2) + R(2,3); % Oz + Ay + add = (ky >= 0); + else + kx1 = R(3,1) + R(1,3); % Nz + Ax + ky1 = R(3,2) + R(2,3); % Oz + Ay + kz1 = R(3,3) - R(1,1) - R(2,2) + 1; % Az - Nx - Oy + 1 + add = (kz >= 0); + end + + if add + kx = kx + kx1; + ky = ky + ky1; + kz = kz + kz1; + else + kx = kx - kx1; + ky = ky - ky1; + kz = kz - kz1; + end + n = norm([kx ky kz]); + if n < eps + % for zero rotation case set arbitrary rotation axis and zero angle + v = [1 0 0]; + theta = 0; + else + theta = 2*acos(qs); + v = [kx ky kz] /n; % unit vector + if theta > pi + theta = pi - theta; + v = -v; + end + end + + if nargout == 0 + fprintf('Rotation: %f rad x [%f %f %f]\n', theta, v(1), v(2), v(3)); + end diff --git a/lwrserv/matlab/rvctools/robot/tr2delta.m b/lwrserv/matlab/rvctools/robot/tr2delta.m new file mode 100644 index 0000000..279c19c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tr2delta.m @@ -0,0 +1,54 @@ +%TR2DELTA Convert homogeneous transform to differential motion +% +% D = TR2DELTA(T0, T1) is the differential motion (6x1) corresponding to +% infinitessimal motion from pose T0 to T1 which are homogeneous +% transformations. D=(dx, dy, dz, dRx, dRy, dRz) and is an approximation +% to the average spatial velocity multiplied by time. +% +% D = TR2DELTA(T) is the differential motion corresponding to the +% infinitessimal relative pose T expressed as a homogeneous +% transformation. +% +% Notes:: +% - D is only an approximation to the motion T, and assumes +% that T0 ~ T1 or T ~ eye(4,4). +% +% See also DELTA2TR, SKEW. + + +% 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 . + +function delta = tr2delta(T0, T1) + if nargin == 1 + T1 = T0; + T0 = eye(4,4); + end + R0 = t2r(T0); R1 = t2r(T1); + % in world frame + delta = [ (T1(1:3,4)-T0(1:3,4)); vex( R1*R0' - eye(3,3)) ]; + % in T0 frame + %delta = [ R0'*(T1(1:3,4)-T0(1:3,4)); R0'*vex( R1*R0' - eye(3,3)) ]; + +% TODO HACK understand/fix this and update Chapter 2 +% delta = [ T1(1:3,4)-T0(1:3,4); +% 0.5*( cross(T0(1:3,1), T1(1:3,1)) + ... +% cross(T0(1:3,2), T1(1:3,2)) + ... +% cross(T0(1:3,3), T1(1:3,3)) ... +% )]; +end + diff --git a/lwrserv/matlab/rvctools/robot/tr2eul.m b/lwrserv/matlab/rvctools/robot/tr2eul.m new file mode 100644 index 0000000..c4a58bc --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tr2eul.m @@ -0,0 +1,80 @@ +%TR2EUL Convert homogeneous transform to Euler angles +% +% EUL = TR2EUL(T, OPTIONS) are the ZYZ Euler angles expressed as a row vector +% corresponding to the rotational part of a homogeneous transform T. +% The 3 angles EUL=[PHI,THETA,PSI] correspond to sequential rotations about +% the Z, Y and Z axes respectively. +% +% EUL = TR2EUL(R, OPTIONS) are the ZYZ Euler angles expressed as a row vector +% corresponding to the orthonormal rotation matrix R. +% +% If R or T represents a trajectory (has 3 dimensions), then each row of EUL +% corresponds to a step of the trajectory. +% +% Options:: +% 'deg' Compute angles in degrees (radians default) +% +% Notes:: +% - There is a singularity for the case where THETA=0 in which case PHI is arbitrarily +% set to zero and PSI is the sum (PHI+PSI). +% +% See also EUL2TR, TR2RPY. + + +% 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 . + +function euler = tr2eul(m, varargin) + + opt.deg = false; + opt = tb_optparse(opt, varargin); + + s = size(m); + if length(s) > 2 + euler = zeros(s(3), 3); + for i=1:s(3) + euler(i,:) = tr2eul(m(:,:,i)); + end + + if opt.deg + euler = euler * 180/pi; + end + return + end + + euler = zeros(1,3); + + % Method as per Paul, p 69. + % phi = atan2(ay, ax) + % Only positive phi is returned. + if abs(m(1,3)) < eps && abs(m(2,3)) < eps + % singularity + euler(1) = 0; + sp = 0; + cp = 1; + euler(2) = atan2(cp*m(1,3) + sp*m(2,3), m(3,3)); + euler(3) = atan2(-sp * m(1,1) + cp * m(2,1), -sp*m(1,2) + cp*m(2,2)); + else + euler(1) = atan2(m(2,3), m(1,3)); + sp = sin(euler(1)); + cp = cos(euler(1)); + euler(2) = atan2(cp*m(1,3) + sp*m(2,3), m(3,3)); + euler(3) = atan2(-sp * m(1,1) + cp * m(2,1), -sp*m(1,2) + cp*m(2,2)); + end + if opt.deg + euler = euler * 180/pi; + end diff --git a/lwrserv/matlab/rvctools/robot/tr2jac.m b/lwrserv/matlab/rvctools/robot/tr2jac.m new file mode 100644 index 0000000..ee4c598 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tr2jac.m @@ -0,0 +1,33 @@ +%TR2JAC Jacobian for differential motion +% +% J = TR2JAC(T) is a Jacobian matrix (6x6) that maps spatial velocity or +% differential motion from the world frame to the frame represented by +% the homogeneous transform T. +% +% See also WTRANS, TR2DELTA, DELTA2TR. + + +% 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 . + +function J = tr2jac(T) + + R = t2r(T); + J = [ + R' (skew(transl(T))*R)' + zeros(3,3) R' + ]; diff --git a/lwrserv/matlab/rvctools/robot/tr2rpy.m b/lwrserv/matlab/rvctools/robot/tr2rpy.m new file mode 100644 index 0000000..9a2849c --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tr2rpy.m @@ -0,0 +1,93 @@ +%TR2RPY Convert a homogeneous transform to roll-pitch-yaw angles +% +% RPY = TR2RPY(T, options) are the roll-pitch-yaw angles expressed as a row +% vector corresponding to the rotation part of a homogeneous transform T. +% The 3 angles RPY=[R,P,Y] correspond to sequential rotations about +% the X, Y and Z axes respectively. +% +% RPY = TR2RPY(R, options) are the roll-pitch-yaw angles expressed as a row +% vector corresponding to the orthonormal rotation matrix R. +% +% If R or T represents a trajectory (has 3 dimensions), then each row of RPY +% corresponds to a step of the trajectory. +% +% Options:: +% 'deg' Compute angles in degrees (radians default) +% 'zyx' Return solution for sequential rotations about Z, Y, X axes (Paul book) +% +% Notes:: +% - There is a singularity for the case where P=pi/2 in which case R is arbitrarily +% set to zero and Y is the sum (R+Y). +% - Note that textbooks (Paul, Spong) use the rotation order ZYX. +% +% See also rpy2tr, tr2eul. + + +% 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 . + +% TODO singularity for XYZ case, +function rpy = tr2rpy(m, varargin) + + opt.deg = false; + opt.zyx = false; + opt = tb_optparse(opt, varargin); + + s = size(m); + if length(s) > 2 + rpy = zeros(s(3), 3); + for i=1:s(3) + rpy(i,:) = tr2rpy(m(:,:,i), varargin{:}); + end + return + end + rpy = zeros(1,3); + + if ~opt.zyx + % XYZ order + if abs(m(3,3)) < eps && abs(m(2,3)) < eps + % singularity + rpy(1) = 0; % roll is zero + rpy(2) = atan2(m(1,3), m(3,3)); % pitch + rpy(3) = atan2(m(2,1), m(2,2)); % yaw is sum of roll+yaw + else + rpy(1) = atan2(-m(2,3), m(3,3)); % roll + % compute sin/cos of roll angle + sr = sin(rpy(1)); + cr = cos(rpy(1)); + rpy(2) = atan2(m(1,3), cr * m(3,3) - sr * m(2,3)); % pitch + rpy(3) = atan2(-m(1,2), m(1,1)); % yaw + end + else + % old ZYX order (as per Paul book) + if abs(m(1,1)) < eps && abs(m(2,1)) < eps + % singularity + rpy(1) = 0; % roll is zero + rpy(2) = atan2(-m(3,1), m(1,1)); % pitch + rpy(3) = atan2(-m(2,3), m(2,2)); % yaw is difference yaw-roll + else + rpy(1) = atan2(m(2,1), m(1,1)); + sp = sin(rpy(1)); + cp = cos(rpy(1)); + rpy(2) = atan2(-m(3,1), cp * m(1,1) + sp * m(2,1)); + rpy(3) = atan2(sp * m(1,3) - cp * m(2,3), cp*m(2,2) - sp*m(1,2)); + end + end + if opt.deg + rpy = rpy * 180/pi; + end +end diff --git a/lwrserv/matlab/rvctools/robot/tr2rt.m b/lwrserv/matlab/rvctools/robot/tr2rt.m new file mode 100644 index 0000000..d3d7991 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tr2rt.m @@ -0,0 +1,52 @@ +%TR2RT Convert homogeneous transform to rotation and translation +% +% [R,t] = TR2RT(TR) split a homogeneous transformation matrix (NxN) into an +% orthonormal rotation matrix R (MxM) and a translation vector t (Mx1), where +% N=M+1. +% +% A homogeneous transform sequence TR (NxNxK) is split into rotation matrix +% sequence R (MxMxK) and a translation sequence t (KxM). +% +% Notes:: +% - Works for TR in SE(2) or SE(3) +% - If TR is 4x4, then R is 3x3 and T is 3x1. +% - If TR is 3x3, then R is 2x2 and T is 2x1. +% - The validity of R is not checked. +% +% See also RT2TR, R2T, T2R. + +% 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 . + +function [R,t] = tr2rt(T) + if numcols(T) ~= numrows(T) + error('T must be square'); + end + + n = numcols(T); + + if size(T,3) > 1 + R = zeros(3,3,size(T,3)); + t = zeros(size(T,3), 3); + for i=1:size(T,3) + R(:,:,i) = T(1:n-1,1:n-1,i); + t(i,:) = T(1:n-1,n,i)'; + end + else + R = T(1:n-1,1:n-1); + t = T(1:n-1,n); + end diff --git a/lwrserv/matlab/rvctools/robot/tranimate.m b/lwrserv/matlab/rvctools/robot/tranimate.m new file mode 100644 index 0000000..d55e64e --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tranimate.m @@ -0,0 +1,139 @@ +%TRANIMATE Animate a coordinate frame +% +% TRANIMATE(P1, P2, OPTIONS) animates a 3D coordinate frame moving from pose P1 +% to pose P2. Poses P1 and P2 can be represented by: +% - homogeneous transformation matrices (4x4) +% - orthonormal rotation matrices (3x3) +% - Quaternion +% +% TRANIMATE(P, OPTIONS) animates a coordinate frame moving from the identity pose +% to the pose P represented by any of the types listed above. +% +% TRANIMATE(PSEQ, OPTIONS) animates a trajectory, where PSEQ is any of +% - homogeneous transformation matrix sequence (4x4xN) +% - orthonormal rotation matrix sequence (3x3xN) +% - Quaternion vector (Nx1) +% +% Options:: +% 'fps', fps Number of frames per second to display (default 10) +% 'nsteps', n The number of steps along the path (default 50) +% 'axis',A Axis bounds [xmin, xmax, ymin, ymax, zmin, zmax] +% 'movie',M Save frames as files in the folder M +% +% Notes:: +% - The 'movie' options saves frames as files NNNN.png. +% - When using 'movie' option ensure that the window is fully visible. +% - To convert frames to a movie use a command like: +% ffmpeg -r 10 -i %04d.png out.avi +% +% See also TRPLOT. + +% 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 . + +% TODO +% auto detect the axis scaling +function tranimate(P2, varargin) + + opt.fps = 10; + opt.nsteps = 50; + opt.axis = []; + opt.movie = []; + + [opt, args] = tb_optparse(opt, varargin); + + if ~isempty(opt.movie) + mkdir(opt.movie); + framenum = 1; + end + P1 = []; + + % convert quaternion and rotation matrix to hom transform + if isa(P2, 'Quaternion') + T2 = P2.T; % convert quaternion to transform + if ~isempty(args) && isa(args{1},'Quaternion') + T1 = T2; + Q2 = args{1}; + T2 = Q2.T; + args = args(2:end); + else + T1 = eye(4,4); + end + elseif isrot(P2) + T2 = r2t(P2); + if ~isempty(args) && isrot(args{1}) + T1 = T2; + T2 = r2t(args{1}); + args = args(2:end); + else + T1 = eye(4,4); + end + elseif ishomog(P2) + T2 = P2; + if ~isempty(args) && ishomog(args{1}) + T1 = T2; + T2 = args{1}; + args = args(2:end); + else + T1 = eye(4,4); + end + end + + % at this point + % T1 is the initial pose + % T2 is the final pose + % + % T2 may be a sequence + + if size(T2,3) > 1 + % tranimate(Ts) + % we were passed a homog sequence + if ~isempty(P1) + error('only 1 input argument if sequence specified'); + end + Ttraj = T2; + else + % tranimate(P1, P2) + % create a path between them + Ttraj = ctraj(T1, T2, opt.nsteps); + end + + if isempty(opt.axis) + % create axis limits automatically based on motion of frame origin + t = transl(Ttraj); + mn = min(t) - 1.5; % min value + length of axis + some + mx = max(t) + 1.5; % max value + length of axis + some + axlim = [mn; mx]; + axlim = axlim(:)'; + args = [args 'axis' axlim]; + end + + hg = trplot(eye(4,4), args{:}); % create a frame at the origin + + % animate it for all poses in the sequence + for i=1:size(Ttraj,3) + T = Ttraj(:,:,i); + trplot(hg, T); + + if ~isempty(opt.movie) + f = getframe; + imwrite(f.cdata, sprintf('%s/%04d.png', opt.movie, framenum)); + framenum = framenum+1; + end + + pause(1/opt.fps); + end diff --git a/lwrserv/matlab/rvctools/robot/transl.m b/lwrserv/matlab/rvctools/robot/transl.m new file mode 100644 index 0000000..ee313fb --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/transl.m @@ -0,0 +1,76 @@ +%TRANSL Create translational transform +% +% T = TRANSL(X, Y, Z) is a homogeneous transform representing a +% pure translation. +% +% T = TRANSL(P) is a homogeneous transform representing a translation or +% point P=[X,Y,Z]. If P (Mx3) it represents a sequence and T (4x4xM) +% is a sequence of homogenous transforms such that T(:,:,i) corresponds to +% the i'th row of P. +% +% P = TRANSL(T) is the translational part of a homogenous transform as a +% 3-element column vector. If T (4x4xM) is a homgoeneous transform sequence +% the rows of P (Mx3) are the translational component of the corresponding +% transform in the sequence. +% +% Notes:: +% - Somewhat unusually this function performs a function and its inverse. An +% historical anomaly. +% +% See also CTRAJ. + + +% 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 . + +function T = transl(x, y, z) + if nargin == 1 + if ishomog(x) + if ndims(x) == 3 + % transl(T) -> P, trajectory case + T = squeeze(x(1:3,4,:))'; + else + % transl(T) -> P + T = x(1:3,4); + end + elseif all(size(x) == [3 3]) + T = x(1:2,3); + elseif length(x) == 2 + % transl(P) -> T + t = x(:); + T = [eye(2) t(:); + 0 0 1]; + elseif length(x) == 3 + % transl(P) -> T + t = x(:); + T = [eye(3) t(:); + 0 0 0 1]; + else + % transl(P) -> T, trajectory case + n = numrows(x); + T = repmat(eye(4,4), [1 1 n]); + T(1:3,4,:) = x'; + end + elseif nargin == 2 + % transl(x,y) -> T + t = [x; y]; + T = rt2tr( eye(2), t); + elseif nargin == 3 + % transl(x,y,z) -> T + t = [x; y; z]; + T = rt2tr( eye(3), t); + end diff --git a/lwrserv/matlab/rvctools/robot/transl2.m b/lwrserv/matlab/rvctools/robot/transl2.m new file mode 100644 index 0000000..62d3643 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/transl2.m @@ -0,0 +1,74 @@ +%TRANSL2 Create or unpack an SE2 translational transform +% +% Create a translational transformation matrix:: +% +% T = TRANSL2(X, Y) is an SE2 homogeneous transform (3x3) representing a +% pure translation. +% +% T = TRANSL2(P) is a homogeneous transform representing a translation or +% point P=[X,Y]. If P (Mx2) it represents a sequence and T (3x3xM) is a +% sequence of homogenous transforms such that T(:,:,i) corresponds to the +% i'th row of P. +% +% Unpack the translational part of a transformation matrix:: +% +% P = TRANSL2(T) is the translational part of a homogeneous transform as a +% 2-element column vector. If T (3x3xM) is a homogeneous transform +% sequence the rows of P (Mx2) are the translational component of the +% corresponding transform in the sequence. +% +% Notes:: +% - Somewhat unusually this function performs a function and its inverse. An +% historical anomaly. +% +% See also TRANSL. + + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function T = transl2(x, y) + if nargin == 1 + if ishomog2(x) + if ndims(x) == 3 + % transl(T) -> P, trajectory case + T = squeeze(x(1:2,3,:))'; + else + % transl(T) -> P + T = x(1:2,3); + end + elseif all(size(x) == [3 3]) + T = x(1:2,3); + elseif length(x) == 2 + % transl(P) -> T + t = x(:); + T = [eye(2) t(:); + 0 0 1]; + else + % transl(P) -> T, trajectory case + n = numrows(x); + T = repmat(eye(3,3), [1 1 n]); + T(1:2,3,:) = x'; + end + elseif nargin == 2 + % transl(x,y) -> T + t = [x; y]; + T = rt2tr( eye(2), t); + end diff --git a/lwrserv/matlab/rvctools/robot/trchain.m b/lwrserv/matlab/rvctools/robot/trchain.m new file mode 100644 index 0000000..169c58b --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trchain.m @@ -0,0 +1,101 @@ +%TRCHAIN Chain 3D transforms from string +% +% T = TRCHAIN(S, Q) is a homogeneous transform (4x4) that results from +% compounding a number of elementary transformations defined by the string +% S. The string S comprises a number of tokens of the form X(ARG) where +% X is one of Tx, Ty, Tz, Rx, Ry, or Rz. ARG is the name of a variable in +% MATLAB workspace or qJ where J is an integer in the range 1 to N that +% selects the variable from the Jth column of the vector Q (1xN). +% +% For example: +% trchain('Rx(q1)Tx(a1)Ry(q2)Ty(a3)Rz(q3)', [1 2 3]) +% +% is equivalent to computing: +% trotx(1) * transl(a1,0,0) * troty(2) * transl(0,a3,0) * trotz(3) +% +% Notes:: +% - The string can contain spaces between elements or on either side of ARG. +% - Works for symbolic variables in the workspace and/or passed in via the +% vector Q. +% - For symbolic operations that involve use of the value pi, make sure you +% define it first in the workspace: pi = sym('pi'); +% +% +% See also trchain2, trotx, troty, trotz, transl, SerialLink.trchain, ETS. + +% Copyright (C) 1993-2015, 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.petercorke.com + + +function T = trchain(s, q) + + if isa(q, 'symfun') + q = formula(q); + end + % s = 'Rx(q1)Tx(a1)Ry(q2)Tx(a3)Rz(q3)Tx(a3)'; + + tokens = regexp(s, '\s*(?R.?|T.)\(\s*(?[A-Za-z\-][A-Za-z0-9+\-\*/]*)\s*\)\s*', 'names'); + + T = eye(4,4); + joint = 1; + + for token = tokens + + % get the argument for this transform element + if token.arg(1) == 'q' + % from the passed in vector q + + try + arg = q(joint); + catch + error('RTB:trchain:badarg', 'vector q has insufficient values'); + end + joint = joint+1; + else % or the workspace + + try + arg = evalin('base', token.arg); + catch + error('RTB:trchain:badarg', 'variable %s does not exist', token.arg); + end + end + + % now evaluate the element and update the transform chain + + switch token.op + case 'Rx' + T = T * trotx(arg); + case 'Ry' + T = T * troty(arg); + case 'Rz' + T = T * trotz(arg); + case 'Tx' + T = T * transl(arg, 0, 0); + case 'Ty' + T = T * transl(0, arg, 0); + case 'Tz' + T = T * transl(0, 0, arg); + otherwise + error('RTB:trchain:badarg', 'unknown operator %s', token.op); + end + end + + if isa(q, 'symfun') + T = formula(T); + end diff --git a/lwrserv/matlab/rvctools/robot/trchain2.m b/lwrserv/matlab/rvctools/robot/trchain2.m new file mode 100644 index 0000000..dbfb74f --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trchain2.m @@ -0,0 +1,94 @@ +%TRCHAIN2 Chain 2D transforms from string +% +% T = TRCHAIN2(S, Q) is a homogeneous transform (3x3) that results from +% compounding a number of elementary transformations defined by the string +% S. The string S comprises a number of tokens of the form X(ARG) where +% X is one of Tx, Ty or R. ARG is the name of a variable in +% MATLAB workspace or qJ where J is an integer in the range 1 to N that +% selects the variable from the Jth column of the vector Q (1xN). +% +% For example: +% trchain('R(q1)Tx(a1)R(q2)Ty(a3)R(q3)', [1 2 3]) +% +% is equivalent to computing: +% trot2(1) * transl2(a1,0) * trot2(2) * transl2(0,a3) * trot2(3) +% +% Notes:: +% - The string can contain spaces between elements or on either side of ARG. +% - Works for symbolic variables in the workspace and/or passed in via the +% vector Q. +% - For symbolic operations that involve use of the value pi, make sure you +% define it first in the workspace: pi = sym('pi'); +% +% See also trchain, trot2, transl2. + +% Copyright (C) 1993-2015, 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.petercorke.com + + +function T = trchain2(s, q) + + %s = 'R(q1)Tx(a1)R(q2)Tx(a3)R(q3)Tx(a3)'; + + tokens = regexp(s, '\s*(?R.?|T.)\(\s*(?[A-Za-z][A-Za-z0-9]*)\s*\)\s*', 'names'); + + if isa(q, 'symfun') + q = formula(q); + end + + T = eye(3,3); + joint = 1; + + for token = tokens + + % get the argument for this transform element + if token.arg(1) == 'q' + % from the passed in vector q + try + arg = q(joint); + catch + error('RTB:trchain2:badarg', 'vector q has insufficient values'); + end + joint = joint+1; + else + % or the workspace + try + arg = evalin('base', token.arg); + catch + error('RTB:trchain2:badarg', 'variable %s does not exist', token.arg); + end + end + + % now evaluate the element and update the transform chain + switch token.op + case 'R' + T = T * trot2(arg); + case 'Tx' + T = T * transl2(arg, 0); + case 'Ty' + T = T * transl2(0, arg); + otherwise + error('RTB:trchain2:badarg', 'unknown operator %s', token.op); + end + end + + if isa(q, 'symfun') + T = formula(T); + end + diff --git a/lwrserv/matlab/rvctools/robot/trinterp.m b/lwrserv/matlab/rvctools/robot/trinterp.m new file mode 100644 index 0000000..b0ea895 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trinterp.m @@ -0,0 +1,78 @@ +%TRINTERP Interpolate homogeneous transformations +% +% T = TRINTERP(T0, T1, S) is a homogeneous transform interpolation +% between T0 when S=0 to T1 when S=1. Rotation is interpolated using +% quaternion spherical linear interpolation. If S (Nx1) then T (4x4xN) +% is a sequence of homogeneous transforms corresponding to the interpolation +% values in S. +% +% T = TRINTERP(T, S) is a transform that varies from the identity matrix when +% S=0 to T when R=1. If S (Nx1) then T (4x4xN) is a sequence of homogeneous +% transforms corresponding to the interpolation values in S. +% +% See also CTRAJ, QUATERNION. + + +% 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 . + +function T = trinterp(A, B, C) + + if nargin == 3 + % TR = TRINTERP(T0, T1, r) + T0 = A; T1 = B; r = C; + + if length(r) > 1 + T = []; + for rr=r(:)' + TT = trinterp(T0, T1, rr); + T = cat(3, T, TT); + end + return; + end + + q0 = Quaternion(T0); + q1 = Quaternion(T1); + + p0 = transl(T0); + p1 = transl(T1); + + qr = q0.interp(q1, r); + pr = p0*(1-r) + r*p1; + elseif nargin == 2 + % TR = TRINTERP(T, r) + T0 = A; r = B; + + if length(r) > 1 + T = []; + for rr=r(:)' + TT = trinterp(T0, rr); + T = cat(3, T, TT); + end + return; + end + + q0 = Quaternion(T0); + p0 = transl(T0); + + qr = q0.scale(r); + pr = r*p0; + else + error('must be 2 or 3 arguments'); + end + T = rt2tr(qr.R, pr); + diff --git a/lwrserv/matlab/rvctools/robot/tripleangle.fig b/lwrserv/matlab/rvctools/robot/tripleangle.fig new file mode 100644 index 0000000..cfe770d Binary files /dev/null and b/lwrserv/matlab/rvctools/robot/tripleangle.fig differ diff --git a/lwrserv/matlab/rvctools/robot/tripleangle.m b/lwrserv/matlab/rvctools/robot/tripleangle.m new file mode 100644 index 0000000..196d9aa --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/tripleangle.m @@ -0,0 +1,297 @@ +function varargout = tripleangle(varargin) +% TRIPLEANGLE Visualize triple angle rotations +% +% TRIPLEANGLE, by itself, displays a simple GUI with three angle sliders +% and a set of axes showing three coordinate frames. The frames correspond +% to rotation after the first angle (red), the first and second angles (green) +% and all three angles (blue). +% +% TRIPLEANGLE(OPTIONS) as above but with options to select the rotation axes. +% +% Options:: +% 'rpy' Rotation about axes x, y, z (default) +% 'euler' Rotation about axes z, y, z +% 'ABC' Rotation about axes A, B, C where A,B,C are each one of x,y or z. +% +% Other options relevant to TRPLOT can be appended. +% +% Notes:: +% - All angles are displayed in units of degrees. +% +% See also trplot. + + + +% Last Modified by GUIDE v2.5 26-Jan-2013 15:28:14 + + +% Begin initialization code - DO NOT EDIT +gui_Singleton = 1; +gui_State = struct('gui_Name', mfilename, ... + 'gui_Singleton', gui_Singleton, ... + 'gui_OpeningFcn', @tripleangle_OpeningFcn, ... + 'gui_OutputFcn', @tripleangle_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 tripleang is made visible. +function tripleangle_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 tripleang (see VARARGIN) + +% Choose default command line output for tripleang +handles.output = hObject; + +figure(handles.figure1); + +opt.wait = false; +opt.which = {'rpy', 'euler'}; + +[opt,args] = tb_optparse(opt, varargin); + +if length(args) > 0 && ischar(args{1}) + s = args{1}; + try + set(handles.popupmenu1, 'Value', strfind('xyz', s(1))); + set(handles.popupmenu2, 'Value', strfind('xyz', s(2))); + set(handles.popupmenu3, 'Value', strfind('xyz', s(3))); + if length(args) > 1 + args = args{2:end}; + else + args = {}; + end + opt.which = 'none'; + catch me + % doesnt match a rotation string, go with 'rpy' + opt.which = 'rpy'; + + end +end +switch opt.which + case 'rpy' + set(handles.popupmenu1, 'Value', 1); + set(handles.popupmenu2, 'Value', 2); + set(handles.popupmenu3, 'Value', 3); + case 'euler' + set(handles.popupmenu1, 'Value', 3); + set(handles.popupmenu2, 'Value', 2); + set(handles.popupmenu3, 'Value', 3); +end + +% draw the axes +handles.h1 = trplot(eye(3,3), 'color', 'r', args{:}); +hold on +handles.h2 = trplot(eye(3,3), 'color', 'g', args{:}); +handles.h3 = trplot(eye(3,3), 'color', 'b', 'thick', 2, args{:}); + +% Update handles structure +guidata(hObject, handles); + +% set the initial value of text fields +set(handles.text1, 'String', sprintf('%.1f', get(handles.slider1, 'Value'))); +set(handles.text2, 'String', sprintf('%.1f', get(handles.slider2, 'Value'))); +set(handles.text3, 'String', sprintf('%.1f', get(handles.slider3, 'Value'))); + +% UIWAIT makes tripleang wait for user response (see UIRESUME) +if opt.wait + uiwait(handles.figure1); +end + + +% --- Outputs from this function are returned to the command line. +function varargout = tripleangle_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 selection change in popupmenu1. +function popupmenu1_Callback(hObject, eventdata, handles) +% hObject handle to popupmenu1 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: contents = cellstr(get(hObject,'String')) returns popupmenu1 contents as cell array +% contents{get(hObject,'Value')} returns selected item from popupmenu1 + + +% --- Executes during object creation, after setting all properties. +function popupmenu1_CreateFcn(hObject, eventdata, handles) +% hObject handle to popupmenu1 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: popupmenu controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +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 + +set(handles.text1, 'String', sprintf('%.1f', get(hObject, 'Value'))); +update(handles); + +% --- 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 + +set(handles.text2, 'String', sprintf('%.1f', get(hObject, 'Value'))); + +update(handles); + +% --- 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 + +set(handles.text3, 'String', sprintf('%.1f', get(hObject, 'Value'))); + +update(handles); + +% --- 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 selection change in popupmenu2. +function popupmenu2_Callback(hObject, eventdata, handles) +% hObject handle to popupmenu2 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: contents = cellstr(get(hObject,'String')) returns popupmenu2 contents as cell array +% contents{get(hObject,'Value')} returns selected item from popupmenu2 + + +% --- Executes during object creation, after setting all properties. +function popupmenu2_CreateFcn(hObject, eventdata, handles) +% hObject handle to popupmenu2 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: popupmenu controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + + +% --- Executes on selection change in popupmenu3. +function popupmenu3_Callback(hObject, eventdata, handles) +% hObject handle to popupmenu3 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: contents = cellstr(get(hObject,'String')) returns popupmenu3 contents as cell array +% contents{get(hObject,'Value')} returns selected item from popupmenu3 + + +% --- Executes during object creation, after setting all properties. +function popupmenu3_CreateFcn(hObject, eventdata, handles) +% hObject handle to popupmenu3 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: popupmenu controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + +function update(handles) + + % compute the three rotation matrices + + R1 = Rx( get(handles.slider1, 'Value'), get(handles.popupmenu1, 'Value')); + R2 = Rx( get(handles.slider2, 'Value'), get(handles.popupmenu2, 'Value')); + R3 = Rx( get(handles.slider3, 'Value'), get(handles.popupmenu3, 'Value')); + + + % display the three frames + trplot(handles.h1, R1); + trplot(handles.h2, R1*R2); + trplot(handles.h3, R1*R2*R3); + + +function R = Rx(theta, which) + theta = theta * pi/180; + + switch which + case 1 + R = rotx(theta); + case 2 + R = roty(theta); + case 3 + R = rotz(theta); + end + + diff --git a/lwrserv/matlab/rvctools/robot/trnorm.m b/lwrserv/matlab/rvctools/robot/trnorm.m new file mode 100644 index 0000000..548d89a --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trnorm.m @@ -0,0 +1,52 @@ +%TRNORM Normalize a homogeneous transform +% +% TN = TRNORM(T) is a normalized homogeneous transformation matrix in which +% the rotation submatrix R = [N,O,A] is guaranteed to be a proper orthogonal +% matrix. The O and A vectors are normalized and the normal vector is formed from +% N = O x A, and then we ensure that O and A are orthogonal by O = A x N. +% +% Notes:: +% - Used to prevent finite word length arithmetic causing transforms to +% become `unnormalized'. +% +% See also OA2TR. + + +% 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 . + +function r = trnorm(t) + + if ndims(t) == 3 + nd = size(t, 3); + r = zeros(4,4,nd); + for i=1:nd + r(:,:,i) = trnorm(t(:,:,i)); + end + return + end + + if all(size(t) == [4 4]) + n = cross(t(1:3,2), t(1:3,3)); % N = O x A + o = cross(t(1:3,3), n); % O = A x N + r = [unit(n) unit(t(1:3,2)) unit(t(1:3,3)) t(1:3,4); 0 0 0 1]; + elseif all(size(t) == [3 3]) + r = t; + else + error('RTB:trnorm:badarg', 'argument must be 3x3 or 4x4 hom xform'); + end + diff --git a/lwrserv/matlab/rvctools/robot/trot2.m b/lwrserv/matlab/rvctools/robot/trot2.m new file mode 100644 index 0000000..8063b93 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trot2.m @@ -0,0 +1,35 @@ +%TROT2 SE2 rotation matrix +% +% T = TROT2(THETA) is a homogeneous transformation (3x3) representing a rotation +% of THETA radians. +% +% T = TROT2(THETA, 'deg') as above but THETA is in degrees. +% +% Notes:: +% - Translational component is zero. +% +% See also ROT2, TRANSL2, TROTX, TROTY, TROTZ. + + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function T = trot2(t, varargin) + T = [rot2(t, varargin{:}) [0 0]'; 0 0 1]; diff --git a/lwrserv/matlab/rvctools/robot/trotx.m b/lwrserv/matlab/rvctools/robot/trotx.m new file mode 100644 index 0000000..cf86a30 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trotx.m @@ -0,0 +1,32 @@ +%TROTX Rotation about X axis +% +% T = TROTX(THETA) is a homogeneous transformation (4x4) representing a rotation +% radians about the x-axis. +% +% T = TROTX(THETA, 'deg') as above but THETA is in degrees. +% +% Notes:: +% - Translational component is zero. +% +% See also ROTX, TROTY, TROTZ. + + +% 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 . + +function T = trotx(t, varargin) + T = [rotx(t, varargin{:}) [0 0 0]'; 0 0 0 1]; diff --git a/lwrserv/matlab/rvctools/robot/troty.m b/lwrserv/matlab/rvctools/robot/troty.m new file mode 100644 index 0000000..d63c1f3 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/troty.m @@ -0,0 +1,32 @@ +%TROTY Rotation about Y axis +% +% T = TROTY(THETA) is a homogeneous transformation (4x4) representing a rotation +% radians about the y-axis. +% +% T = TROTY(THETA, 'deg') as above but THETA is in degrees. +% +% Notes:: +% - Translational component is zero. +% +% See also ROTY, TROTX, TROTZ. + + +% 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 . + +function T = troty(t, varargin) + T = [roty(t, varargin{:}) [0 0 0]'; 0 0 0 1]; diff --git a/lwrserv/matlab/rvctools/robot/trotz.m b/lwrserv/matlab/rvctools/robot/trotz.m new file mode 100644 index 0000000..7d0c529 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trotz.m @@ -0,0 +1,32 @@ +%TROTZ Rotation about Z axis +% +% T = TROTZ(THETA) is a homogeneous transformation (4x4) representing a rotation +% radians about the z-axis. +% +% T = TROTZ(THETA, 'deg') as above but THETA is in degrees. +% +% Notes:: +% - Translational component is zero. +% +% See also ROTZ, TROTX, TROTY. + + +% 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 . + +function T = trotz(t, varargin) + T = [rotz(t, varargin{:}) [0 0 0]'; 0 0 0 1]; diff --git a/lwrserv/matlab/rvctools/robot/trplot.m b/lwrserv/matlab/rvctools/robot/trplot.m new file mode 100644 index 0000000..6181507 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trplot.m @@ -0,0 +1,325 @@ +%TRPLOT Draw a coordinate frame +% +% TRPLOT(T, OPTIONS) draws a 3D coordinate frame represented by the homogeneous +% transform T (4x4). +% +% H = TRPLOT(T, OPTIONS) as above but returns a handle. +% +% TRPLOT(H, T) moves the coordinate frame described by the handle H to +% the pose T (4x4). +% +% TRPLOT(R, OPTIONS) draws a 3D coordinate frame represented by the orthonormal +% rotation matrix R (3x3). +% +% H = TRPLOT(R, OPTIONS) as above but returns a handle. +% +% TRPLOT(H, R) moves the coordinate frame described by the handle H to +% the orientation R. +% +% Options:: +% 'color',C The color to draw the axes, MATLAB colorspec C +% 'noaxes' Don't display axes on the plot +% 'axis',A Set dimensions of the MATLAB axes to A=[xmin xmax ymin ymax zmin zmax] +% 'frame',F The frame is named {F} and the subscript on the axis labels is F. +% 'text_opts', opt A cell array of MATLAB text properties +% 'handle',H Draw in the MATLAB axes specified by the axis handle H +% 'view',V Set plot view parameters V=[az el] angles, or 'auto' +% for view toward origin of coordinate frame +% 'arrow' Use arrows rather than line segments for the axes +% 'width', w Width of arrow tips (default 1) +% 'thick',t Thickness of lines (default 0.5) +% '3d' Plot in 3D using anaglyph graphics +% 'anaglyph',A Specify anaglyph colors for '3d' as 2 characters for +% left and right (default colors 'rc'): +% 'r' red +% 'g' green +% 'b' green +% 'c' cyan +% 'm' magenta +% 'dispar',D Disparity for 3d display (default 0.1) +% +% Examples:: +% +% trplot(T, 'frame', 'A') +% trplot(T, 'frame', 'A', 'color', 'b') +% trplot(T1, 'frame', 'A', 'text_opts', {'FontSize', 10, 'FontWeight', 'bold'}) +% +% h = trplot(T, 'frame', 'A', 'color', 'b'); +% trplot(h, T2); +% +% 3D anaglyph plot +% trplot(T, '3d'); +% +% Notes:: +% - The arrow option requires the third party package arrow3. +% - The handle H is an hgtransform object. +% - When using the form TRPLOT(H, ...) the axes are not rescaled. +% - The '3d' option requires that the plot is viewed with anaglyph glasses. +% - You cannot specify 'color' +% +% See also TRPLOT2, TRANIMATE. + + +% 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 . + +% TODO +% need to decide how to handle scaling +% what does hold on mean? don't touch scaling? + +function hout = trplot(T, varargin) + + if isscalar(T) && ishandle(T) + % trplot(H, T) + H = T; T = varargin{1}; + if isrot(T) + T = r2t(T); + end + set(H, 'Matrix', T); + + % for the 3D case retrieve the right hgtransform and set it + hg2 = get(H, 'UserData'); + if ~isempty(hg2) + set(hg2, 'Matrix', T); + end + + return; + end + + if size(T,3) > 1 + error('trplot cannot operate on a sequence'); + end + if ~ishomog(T) && ~isrot(T) + error('trplot operates only on transform (4x4) or rotation matrix (3x3)'); + end + + opt.color = []; + opt.axes = true; + opt.axis = []; + opt.frame = []; + opt.text_opts = []; + opt.view = []; + opt.width = 1; + opt.arrow = false; + opt.handle = []; + opt.anaglyph = 'rc'; + opt.d_3d = false; + opt.dispar = 0.1; + opt.thick = 0.5; + + opt = tb_optparse(opt, varargin); + + if ~isempty(opt.color) && opt.d_3d + error('cannot specify ''color'' and ''3d'', use ''anaglyph'' option'); + end + if isempty(opt.color) + opt.color = 'b'; + end + if isempty(opt.text_opts) + opt.text_opts = {}; + end + + if opt.d_3d + opt.color = ag_color(opt.anaglyph(1)); + end + + if isempty(opt.axis) + % determine some default axis dimensions + + % get the origin of the frame + if isrot(T) + c = [0 0 0]; % at zero for a rotation matrix + else + c = transl(T); + end + + d = 1.2; + opt.axis = [c(1)-d c(1)+d c(2)-d c(2)+d c(3)-d c(3)+d]; + + end + + % TODO: should do the 2D case as well + + if ~isempty(opt.handle) + hax = opt.handle; + hold(hax); + else + ih = ishold; + if ~ih + % if hold is not on, then clear the axes and set scaling + cla + if ~isempty(opt.axis) + axis(opt.axis); + end + daspect([1 1 1]); + + if opt.axes + xlabel( 'X'); + ylabel( 'Y'); + zlabel( 'Z'); + rotate3d on + end + new_plot = true; + end + hax = gca; + hold on + end + % hax is the handle for the axis we will work with, either new or + % passed by option 'handle' + + opt.text_opts = [opt.text_opts, 'Color', opt.color]; + + + hg = hgtransform('Parent', hax); + + + % trplot( Q.R, fmt, color); + if isrot(T) + T = r2t(T); + end + + % create unit vectors + o = [0 0 0]'; + x1 = [1 0 0]'; + y1 = [0 1 0]'; + z1 = [0 0 1]'; + + % draw the axes + + mstart = [o o o]'; + mend = [x1 y1 z1]'; + + if opt.arrow + % draw the 3 arrows + S = [opt.color num2str(opt.width)]; + ha = arrow3(mstart, mend, S); + for h=ha' + set(h, 'Parent', hg); + end + else + for i=1:3 + plot2([mstart(i,1:3); mend(i,1:3)], 'Color', opt.color, ... + 'LineWidth', opt.thick, ... + 'Parent', hg); + end + end + + % label the axes + if isempty(opt.frame) + fmt = '%c'; + else + fmt = sprintf('%%c_{%s}', opt.frame); + end + + % add the labels to each axis + h = text(x1(1), x1(2), x1(3), sprintf(fmt, 'X'), 'Parent', hg); + set(h, opt.text_opts{:}); + + h = text(y1(1), y1(2), y1(3), sprintf(fmt, 'Y'), 'Parent', hg); + set(h, opt.text_opts{:}); + + h = text(z1(1), z1(2), z1(3), sprintf(fmt, 'Z'), 'Parent', hg); + set(h, opt.text_opts{:}); + + % label the frame + if ~isempty(opt.frame) + h = text(o(1)-0.04*x1(1), o(2)-0.04*y1(2), o(3)-0.04*z1(3), ... + ['\{' opt.frame '\}'], 'Parent', hg); + set(h, 'VerticalAlignment', 'middle', ... + 'HorizontalAlignment', 'center', opt.text_opts{:}); + end + + if ~opt.axes + set(gca, 'visible', 'off'); + end + if ischar(opt.view) && strcmp(opt.view, 'auto') + cam = x1+y1+z1; + view(cam(1:3)); + elseif ~isempty(opt.view) + view(opt.view); + end + if isempty(opt.handle) && ~ih + grid on + hold off + end + + % now place the frame in the desired pose + set(hg, 'Matrix', T); + + + if opt.d_3d + % 3D display. The original axes are for the left eye, and we add + % another set of axes to the figure for the right eye view and + % displace its camera to the right of that of that for the left eye. + % Then we recursively call trplot() to create the right eye view. + + left = gca; + right = axes; + + % compute the offset in world coordinates + off = -t2r(view(left))'*[opt.dispar 0 0]'; + pos = get(left, 'CameraPosition'); + + set(right, 'CameraPosition', pos+off'); + set(right, 'CameraViewAngle', get(left, 'CameraViewAngle')); + set(right, 'CameraUpVector', get(left, 'CameraUpVector')); + target = get(left, 'CameraTarget'); + set(right, 'CameraTarget', target+off'); + + % set perspective projections + set(left, 'Projection', 'perspective'); + set(right, 'Projection', 'perspective'); + + % turn off axes for right view + set(right, 'Visible', 'Off'); + + % set color for right view + hg2 = trplot(T, 'color', ag_color(opt.anaglyph(2))); + + % the hgtransform for the right view is user data for the left + % view hgtransform, we need to change both when we rotate the + % frame. + set(hg, 'UserData', hg2); + end + + % optionally return the handle, for later modification of pose + if nargout > 0 + hout = hg; + end +end + +function out = ag_color(c) + +% map color character to an color triple, same as anaglyph.m + + % map single letter color codes to image planes + switch c + case 'r' + out = [1 0 0]; % red + case 'g' + out = [0 1 0]; % green + case 'b' + % blue + out = [0 0 1]; + case 'c' + out = [0 1 1]; % cyan + case 'm' + out = [1 0 1]; % magenta + case 'o' + out = [1 1 0]; % orange + end +end diff --git a/lwrserv/matlab/rvctools/robot/trplot2.m b/lwrserv/matlab/rvctools/robot/trplot2.m new file mode 100644 index 0000000..5919454 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trplot2.m @@ -0,0 +1,187 @@ +%TRPLOT2 Plot a planar transformation +% +% TRPLOT2(T, OPTIONS) draws a 2D coordinate frame represented by the SE(2) +% homogeneous transform T (3x3). +% +% H = TRPLOT2(T, OPTIONS) as above but returns a handle. +% +% TRPLOT2(H, T) moves the coordinate frame described by the handle H to +% the SE(2) pose T (3x3). +% +% Options:: +% 'axis',A Set dimensions of the MATLAB axes to A=[xmin xmax ymin ymax] +% 'color', c The color to draw the axes, MATLAB colorspec +% 'noaxes' Don't display axes on the plot +% 'frame',F The frame is named {F} and the subscript on the axis labels is F. +% 'text_opts', opt A cell array of Matlab text properties +% 'handle', h Draw in the MATLAB axes specified by h +% 'view',V Set plot view parameters V=[az el] angles, or 'auto' +% for view toward origin of coordinate frame +% 'arrow' Use arrows rather than line segments for the axes +% 'width', w Width of arrow tips +% +% Examples:: +% +% trplot(T, 'frame', 'A') +% trplot(T, 'frame', 'A', 'color', 'b') +% trplot(T1, 'frame', 'A', 'text_opts', {'FontSize', 10, 'FontWeight', 'bold'}) +% +% Notes:: +% - The arrow option requires the third party package arrow3. +% - Generally it is best to set the axis bounds +% +% See also TRPLOT. + + +% 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 . + +% 'frame', name name of the frame, used for axis subscripts and origin +% 'color', color Matlab color specificication for the frame and annotations +% 'noaxes' show the frame but no Matlab axes +% 'arrow' use the contributed arrow3 function to draw the frame axes +% 'width', width width of lines to draw if using arrow3 + +function hout = trplot2(T, varargin) + + if isscalar(T) && ishandle(T) + % trplot(H, T) + H = T; T = varargin{1}; + set(H, 'Matrix', se2t3(T)); + return; + end + + opt.color = 'b'; + opt.axes = true; + opt.axis = []; + opt.frame = []; + opt.text_opts = []; + opt.view = []; + opt.width = 1; + opt.arrow = false; + opt.handle = []; + + opt = tb_optparse(opt, varargin); + + if isempty(opt.text_opts) + opt.text_opts = {}; + end + if isempty(opt.axis) + if all(size(T) == [3 3]) || norm(transl(T)) < eps + c = transl(T); + d = 1.2; + opt.axis = [c(1)-d c(1)+d c(2)-d c(2)+d]; + end + end + + if ~isempty(opt.handle) + hax = opt.handle; + hold(hax); + else + ih = ishold; + if ~ih + % if hold is not on, then clear the axes and set scaling + cla + if ~isempty(opt.axis) + axis(opt.axis); + end + %axis equal + + if opt.axes + xlabel( 'X'); + ylabel( 'Y'); + end + end + hax = gca; + hold on + end + + % create unit vectors + o = [0 0 1]'; o = o(1:2); + x1 = [1 0 1]'; x1 = x1(1:2); + y1 = [0 1 1]'; y1 = y1(1:2); + + % draw the axes + + mstart = [o o]'; + mend = [x1 y1]'; + + hg = hgtransform('Parent', hax); + if opt.arrow + % draw the 2 arrows + S = [opt.color num2str(opt.width)]; + ha = arrow3(mstart, mend, S); + for h=ha' + set(h, 'Parent', hg); + end + else + for i=1:2 + plot2([mstart(i,1:2); mend(i,1:2)], 'Color', opt.color, 'Parent', hg); + end + end + + % label the axes + if isempty(opt.frame) + fmt = '%c'; + else + fmt = sprintf('%%c_{%s}', opt.frame); + end + + % add the labels to each axis + h = text(x1(1), x1(2), sprintf(fmt, 'X'), 'Parent', hg); + if ~isempty(opt.text_opts) + set(h, opt.text_opts{:}); + end + if opt.arrow + set(h, 'Parent', hg); + end + + h = text(y1(1), y1(2), sprintf(fmt, 'Y'), 'Parent', hg); + if ~isempty(opt.text_opts) + set(h, opt.text_opts{:}); + end + if opt.arrow + set(h, 'Parent', hg); + end + + % label the frame + if ~isempty(opt.frame) + h = text(o(1)-0.04*x1(1), o(2)-0.04*y1(2), ... + ['\{' opt.frame '\}'], 'Parent', hg); + set(h, 'VerticalAlignment', 'middle', ... + 'HorizontalAlignment', 'center', opt.text_opts{:}); + end + + if ~opt.axes + set(gca, 'visible', 'off'); + end + grid on + if ~ih + hold off + end + + % now place the frame in the desired pose + set(hg, 'Matrix', se2t3(T)); + + if nargout > 0 + hout = hg; + end +end + +function T3 = se2t3(T2) + T3 = [T2(1:2,1:2) zeros(2,1) T2(1:2,3); 0 0 1 0; 0 0 0 1]; +end diff --git a/lwrserv/matlab/rvctools/robot/trprint.m b/lwrserv/matlab/rvctools/robot/trprint.m new file mode 100644 index 0000000..e771368 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trprint.m @@ -0,0 +1,138 @@ +%TRPRINT Compact display of homogeneous transformation +% +% TRPRINT(T, OPTIONS) displays the homogoneous transform in a compact +% single-line format. If T is a homogeneous transform sequence then each +% element is printed on a separate line. +% +% S = TRPRINT(T, OPTIONS) as above but returns the string. +% +% TRPRINT T is the command line form of above, and displays in RPY format. +% +% Options:: +% 'rpy' display with rotation in roll/pitch/yaw angles (default) +% 'euler' display with rotation in ZYX Euler angles +% 'angvec' display with rotation in angle/vector format +% 'radian' display angle in radians (default is degrees) +% 'fmt', f use format string f for all numbers, (default %g) +% 'label',l display the text before the transform +% +% Examples:: +% >> trprint(T2) +% t = (0,0,0), RPY = (-122.704,65.4084,-8.11266) deg +% +% >> trprint(T1, 'label', 'A') +% A:t = (0,0,0), RPY = (-0,0,-0) deg +% +% See also TR2EUL, TR2RPY, TR2ANGVEC. + +% 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 . + +function out = trprint(T, varargin) + + if ischar(T) + % command form: trprint T + trprint( evalin('base', T) ); + return; + end + + opt.fmt = []; + opt.mode = {'rpy', 'euler', 'angvec'}; + opt.radian = false; + opt.label = ''; + + opt = tb_optparse(opt, varargin); + + s = ''; + + if size(T,3) == 1 + if isempty(opt.fmt) + opt.fmt = '%g'; + end + s = tr2s(T, opt); + else + if isempty(opt.fmt) + opt.fmt = '%8.2g'; + end + + for i=1:size(T,3) + % for each 4x4 transform in a possible 3D matrix + s = char(s, tr2s(T(:,:,i), opt) ); + end + end + + % if no output provided then display it + if nargout == 0 + disp(s); + else + out = s; + end +end + +function s = tr2s(T, opt) + % print the translational part if it exists + if ~isempty(opt.label) + s = sprintf('%8s: ', opt.label); + else + s = ''; + end + if ~isrot(T) + s = strcat(s, sprintf('t = (%s),', vec2s(opt.fmt, transl(T)'))); + end + + % print the angular part in various representations + switch (opt.mode) + case {'rpy', 'euler'} + % angle as a 3-vector + if strcmp(opt.mode, 'rpy') + ang = tr2rpy(T); + label = 'RPY'; + else + ang = tr2eul(T); + label = 'EUL'; + end + if opt.radian + s = strcat(s, ... + sprintf(' %s = (%s) rad', label, vec2s(opt.fmt, ang)) ); + else + s = strcat(s, ... + sprintf(' %s = (%s) deg', label, vec2s(opt.fmt, ang*180.0/pi)) ); + end + case 'angvec' + % as a vector and angle + [th,v] = tr2angvec(T); + if isnan(v(1)) + s = strcat(s, sprintf(' R = nil') ); + elseif opt.radian + s = strcat(s, sprintf(' R = (%sdeg | %s)', ... + sprintf(opt.fmt, th), vec2s(opt.fmt, v)) ); + else + s = strcat(s, sprintf(' R = (%sdeg | %s)', ... + sprintf(opt.fmt, th*180.0/pi), vec2s(opt.fmt,v)) ); + end + end +end + +function s = vec2s(fmt, v) + s = ''; + for i=1:length(v) + s = strcat(s, sprintf(fmt, v(i))); + if i ~= length(v) + s = strcat(s, ', '); + end + end +end diff --git a/lwrserv/matlab/rvctools/robot/trscale.m b/lwrserv/matlab/rvctools/robot/trscale.m new file mode 100644 index 0000000..6200c09 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/trscale.m @@ -0,0 +1,40 @@ +%TRSCALE Homogeneous transformation for pure scale +% +% T = TRSCALE(S) is a homogeneous transform (4x4) corresponding to a pure +% scale change. If S is a scalar the same scale factor is used for x,y,z, +% else it can be a 3-vector specifying scale in the x-, y- and +% z-directions. +% + + +% Copyright (C) 1993-2015, 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.petercorke.com + +function t = trscale(sx, sy, sz) + + if length(sx) > 1 + s = sx; + else + if nargin == 1 + s = [sx sx sx]; + else + s = [sx sy sz]; + end + end + t = r2t(diag(s)); diff --git a/lwrserv/matlab/rvctools/robot/unit.m b/lwrserv/matlab/rvctools/robot/unit.m new file mode 100644 index 0000000..3cb5434 --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/unit.m @@ -0,0 +1,32 @@ +%UNIT Unitize a vector +% +% VN = UNIT(V) is a unit vector parallel to V. +% +% Note:: +% - Reports error for the case where norm(V) is zero. + + +% 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 . + +function u = unit(v) + n = norm(v, 'fro'); + if n < eps + error('RTB:unit:zero_norm', 'vector has zero norm'); + end + + u = v / n; diff --git a/lwrserv/matlab/rvctools/robot/vex.m b/lwrserv/matlab/rvctools/robot/vex.m new file mode 100644 index 0000000..24936ee --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/vex.m @@ -0,0 +1,40 @@ +%VEX Convert skew-symmetric matrix to vector +% +% V = VEX(S) is the vector (3x1) which has the skew-symmetric matrix S (3x3) +% +% | 0 -vz vy| +% | vz 0 -vx| +% |-vy vx 0 | +% +% Notes:: +% - This is the inverse of the function SKEW(). +% - No checking is done to ensure that the matrix is actually skew-symmetric. +% - The function takes the mean of the two elements that correspond to each unique +% element of the matrix, ie. vx = 0.5*(S(3,2)-S(2,3)) +% +% See also SKEW. + + +% 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 . + +function v = vex(S) + if isrot(S) || ishomog(S) + v = 0.5*[S(3,2)-S(2,3); S(1,3)-S(3,1); S(2,1)-S(1,2)]; + else + error('argument must be a 3x3 matrix'); + end diff --git a/lwrserv/matlab/rvctools/robot/wtrans.m b/lwrserv/matlab/rvctools/robot/wtrans.m new file mode 100644 index 0000000..3456d3d --- /dev/null +++ b/lwrserv/matlab/rvctools/robot/wtrans.m @@ -0,0 +1,35 @@ +%WTRANS Transform a wrench between coordinate frames +% +% WT = WTRANS(T, W) is a wrench (6x1) in the frame represented by the homogeneous +% transform T (4x4) corresponding to the world frame wrench W (6x1). +% +% The wrenches W and WT are 6-vectors of the form [Fx Fy Fz Mx My Mz]. +% +% See also TR2DELTA, TR2JAC. + +% 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 . + +function Wt = wtrans(T, W) + + f = W(1:3); m = W(4:6); + k = cross(f, transl(T) ) + m; + + ft = t2r(T)' * f; + mt = t2r(T)' * k; + + Wt = [ft; mt]; diff --git a/lwrserv/matlab/rvctools/simulink/control.mdl b/lwrserv/matlab/rvctools/simulink/control.mdl new file mode 100644 index 0000000..7e48af5 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/control.mdl @@ -0,0 +1,2236 @@ +Library { + Name "control" + Version 8.0 + MdlSubVersion 0 + SavedCharacterEncoding "ISO-8859-1" + LibraryType "BlockLibrary" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [1125.0, 50.0, 1156.0, 860.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [0] + DockPosition "Left" + Width [50] + Height [50] + Filter [10] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Object { + $PropName "EditorsInfo" + $ObjectID 5 + $ClassName "Simulink.EditorInfo" + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [1122.0, 698.0] + ZoomFactor [1.0] + Offset [0.0, 0.0] + } + } + } + Created "Sun Apr 4 19:58:29 2010" + Creator "corkep" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jan 27 11:35:19 2013" + RTWModifiedTimeStamp 281187294 + ModelVersionFormat "1.%" + ConfigurationManager "None" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "all" + WideLines off + ShowLineDimensions off + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + RecordCoverage off + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 6 + Version "1.12.1" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 7 + Version "1.12.1" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 8 + Version "1.12.1" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 9 + Version "1.12.1" + Array { + Type "Cell" + Dimension 7 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 10 + Version "1.12.1" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "UseLocalSettings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + SFUndirectedBroadcastEventsDiag "warning" + SFTransitionActionBeforeConditionDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 11 + Version "1.12.1" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 12 + Version "1.12.1" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 13 + Version "1.12.1" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 14 + Version "1.12.1" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + PortableWordSizes off + GenerateErtSFunction off + CreateSILPILBlock "None" + CodeExecutionProfiling off + CodeExecutionProfileVariable "executionProfile" + CodeProfilingSaveOptions "SummaryOnly" + CodeProfilingInstrumentation off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 15 + Version "1.12.1" + Array { + Type "Cell" + Dimension 19 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + OperatorAnnotations off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InternalIdentifier "Classic" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 16 + Version "1.12.1" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface on + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 920, 197, 1800, 827 ] + } + PropName "ConfigurationSets" + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + MaskDefaults { + SelfModifiable "off" + IconFrame "on" + IconOpaque "on" + RunInitForIconRedraw "off" + IconRotate "none" + PortRotate "default" + IconUnits "autoscale" + } + MaskParameterDefaults { + Evaluate "on" + Tunable "on" + NeverSave "off" + Internal "off" + ReadOnly "off" + Enabled "on" + Visible "on" + ToolTip "on" + } + BlockParameterDefaults { + Block { + BlockType Delay + DelayLengthSource "Dialog" + DelayLength "2" + DelayLengthUpperLimit "100" + InitialConditionSource "Dialog" + InitialCondition "0.0" + ExternalReset "None" + PreventDirectFeedthrough off + DiagnosticForOutOfRangeDelayLength "None" + RemoveProtectionDelayLength off + InputProcessing "Elements as channels (sample based)" + UseCircularBuffer off + SampleTime "-1" + StateMustResolveToSignalObject off + CodeGenStateStorageClass "Auto" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Inport + Port "1" + OutputFunctionCall off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchInputForFeedbackSignals off + Interpolate on + } + Block { + BlockType Integrator + ExternalReset "none" + InitialConditionSource "internal" + InitialCondition "0" + LimitOutput off + UpperSaturationLimit "inf" + LowerSaturationLimit "-inf" + ShowSaturationPort off + ShowStatePort off + AbsoluteTolerance "auto" + IgnoreLimit off + ZeroCross on + ContinuousStateAttributes "''" + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Saturate + UpperLimitSource "Dialog" + UpperLimit "0.5" + LowerLimitSource "Dialog" + LowerLimit "-0.5" + LinearizeAsGain on + ZeroCross on + SampleTime "-1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + CheckFcnCallInpInsideContextMsg off + SystemSampleTime "-1" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + SFBlockType "NONE" + Variant off + GeneratePreprocessorConditionals off + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType TransferFcn + Numerator "[1]" + Denominator "[1 2 1]" + AbsoluteTolerance "auto" + ContinuousStateAttributes "''" + Realization "auto" + } + } + System { + Name "control" + Location [1125, 50, 2281, 910] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "191" + Block { + BlockType SubSystem + Name "vloop" + SID "24" + Ports [2, 4] + Position [110, 67, 160, 168] + ZOrder -9 + DropShadow on + LibraryVersion "*1.13" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 17 + $ClassName "Simulink.Mask" + Array { + Type "Simulink.MaskParameter" + Dimension 7 + Object { + $ObjectID 18 + Type "edit" + Name "J" + Prompt "Inertia (J)" + Value "372e-6" + } + Object { + $ObjectID 19 + Type "edit" + Name "B" + Prompt "Viscous friction coefficient (B)" + Value "817e-6" + } + Object { + $ObjectID 20 + Type "edit" + Name "Km" + Prompt "Motor torque constant" + Value "0.228" + } + Object { + $ObjectID 21 + Type "edit" + Name "tau_max" + Prompt "Torque limit (tau_max)" + Value "0.9" + } + Object { + $ObjectID 22 + Type "edit" + Name "Kv" + Prompt "Proportional gain (Kv)" + Value "1" + } + Object { + $ObjectID 23 + Type "edit" + Name "Ki" + Prompt "Integral gain (Ki)" + Value "5" + } + Object { + $ObjectID 24 + Type "edit" + Name "T" + Prompt "Sample interval (T)" + Value "0.001" + } + PropName "Parameters" + } + } + System { + Name "vloop" + Location [1199, 44, 2042, 510] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "qd*" + SID "175" + Position [25, 113, 55, 127] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tau_d" + SID "176" + Position [435, 33, 465, 47] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Integrator + Name "Integrator" + SID "177" + Ports [2, 1] + Position [225, 152, 255, 183] + ZOrder -3 + ExternalReset "level" + LimitOutput on + UpperSaturationLimit "1" + LowerSaturationLimit "-1" + } + Block { + BlockType Gain + Name "Ki" + SID "178" + Position [295, 150, 325, 180] + ZOrder -4 + Gain "Ki" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Km" + SID "179" + Position [400, 105, 430, 135] + ZOrder -5 + ForegroundColor "blue" + Gain "Km" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Kv" + SID "180" + Position [295, 105, 325, 135] + ZOrder -6 + Gain "Kv" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum" + SID "181" + Ports [2, 1] + Position [100, 110, 120, 130] + ZOrder -7 + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID "182" + Ports [2, 1] + Position [530, 110, 550, 130] + ZOrder -8 + ForegroundColor "blue" + ShowName off + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum2" + SID "183" + Ports [2, 1] + Position [350, 110, 370, 130] + ZOrder -9 + ShowName off + IconShape "round" + Inputs "|++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Delay + Name "control\ntime" + SID "184" + Ports [1, 1] + Position [160, 103, 185, 137] + ZOrder -10 + UserDataPersistent on + UserData "DataTag0" + InputPortMap "u0" + DelayLength "1" + InputProcessing "Inherited" + SampleTime "T" + } + Block { + BlockType TransferFcn + Name "motor" + SID "185" + Position [580, 101, 645, 139] + ZOrder -11 + ForegroundColor "blue" + Denominator "[J B]" + ContinuousStateAttributes "'qd'" + Port { + PortNumber 1 + Name "qd" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Saturate + Name "torque\nlimit" + SID "186" + Ports [1, 1] + Position [450, 105, 480, 135] + ZOrder -12 + ForegroundColor "blue" + InputPortMap "u0" + UpperLimit "tau_max" + LowerLimit "-tau_max" + } + Block { + BlockType Outport + Name "qd" + SID "187" + Position [720, 113, 750, 127] + ZOrder -13 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qd error" + SID "188" + Position [150, 33, 180, 47] + ZOrder -14 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "tau" + SID "189" + Position [720, 193, 750, 207] + ZOrder -15 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "integral" + SID "190" + Position [720, 253, 750, 267] + ZOrder -16 + Port "4" + IconDisplay "Port number" + } + Line { + SrcBlock "Ki" + SrcPort 1 + Points [30, 0] + DstBlock "Sum2" + DstPort 2 + } + Line { + SrcBlock "Integrator" + SrcPort 1 + Points [0, -5; 10, 0] + Branch { + Points [0, 95] + DstBlock "integral" + DstPort 1 + } + Branch { + DstBlock "Ki" + DstPort 1 + } + } + Line { + SrcBlock "Kv" + SrcPort 1 + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "Sum2" + SrcPort 1 + DstBlock "Km" + DstPort 1 + } + Line { + SrcBlock "Km" + SrcPort 1 + DstBlock "torque\nlimit" + DstPort 1 + } + Line { + Labels [0, 0] + SrcBlock "qd*" + SrcPort 1 + DstBlock "Sum" + DstPort 1 + } + Line { + SrcBlock "control\ntime" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "Integrator" + DstPort 1 + } + Branch { + DstBlock "Kv" + DstPort 1 + } + } + Line { + SrcBlock "torque\nlimit" + SrcPort 1 + Points [15, 0] + Branch { + Points [0, 80] + DstBlock "tau" + DstPort 1 + } + Branch { + DstBlock "Sum1" + DstPort 2 + } + } + Line { + SrcBlock "tau_d" + SrcPort 1 + Points [70, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + Name "qd" + Labels [0, 0] + SrcBlock "motor" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "qd" + DstPort 1 + } + Branch { + Points [0, 105; -560, 0] + DstBlock "Sum" + DstPort 2 + } + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [5, 0] + Branch { + Points [0, -80] + DstBlock "qd error" + DstPort 1 + } + Branch { + DstBlock "control\ntime" + DstPort 1 + } + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "motor" + DstPort 1 + } + Annotation { + SID "191" + Name "tau_m" + Position [505, 116] + ForegroundColor "blue" + } + } + } + Block { + BlockType SubSystem + Name "vloop_ff" + SID "41" + Ports [3, 3] + Position [265, 65, 315, 165] + ZOrder -2 + DropShadow on + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 25 + $ClassName "Simulink.Mask" + Array { + Type "Simulink.MaskParameter" + Dimension 7 + Object { + $ObjectID 26 + Type "edit" + Name "J" + Prompt "Inertia (J)" + Value "372e-6" + } + Object { + $ObjectID 27 + Type "edit" + Name "B" + Prompt "Viscous friction coefficient (B)" + Value "817e-6" + } + Object { + $ObjectID 28 + Type "edit" + Name "Km" + Prompt "Motor torque constant" + Value "0.228" + } + Object { + $ObjectID 29 + Type "edit" + Name "tau_max" + Prompt "Torque limit (tau_max)" + Value "0.9" + } + Object { + $ObjectID 30 + Type "edit" + Name "Kv" + Prompt "Proportional gain (Kv)" + Value "1" + } + Object { + $ObjectID 31 + Type "edit" + Name "Ki" + Prompt "Integral gain (Ki)" + Value "0" + } + Object { + $ObjectID 32 + Type "edit" + Name "T" + Prompt "Sample interval (T)" + Value "0.001" + } + PropName "Parameters" + } + } + System { + Name "vloop_ff" + Location [229, 359, 1129, 826] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "qd*" + SID "42" + Position [25, 113, 55, 127] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tau_d" + SID "43" + Position [480, 33, 510, 47] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tau_ff" + SID "59" + Position [25, 203, 55, 217] + ZOrder -3 + ForegroundColor "red" + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Integrator + Name "Integrator" + SID "45" + Ports [1, 1] + Position [225, 150, 255, 180] + ZOrder -4 + } + Block { + BlockType Gain + Name "Kff" + SID "58" + Position [295, 194, 345, 226] + ZOrder -5 + ForegroundColor "red" + Gain "1/Km" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Ki" + SID "44" + Position [295, 150, 325, 180] + ZOrder -6 + Gain "Ki" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Km" + SID "46" + Position [445, 105, 475, 135] + ZOrder -7 + ForegroundColor "blue" + Gain "Km" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Kv" + SID "47" + Position [295, 105, 325, 135] + ZOrder -8 + Gain "Kv" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum" + SID "48" + Ports [2, 1] + Position [100, 110, 120, 130] + ZOrder -9 + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID "49" + Ports [2, 1] + Position [575, 110, 595, 130] + ZOrder -10 + ForegroundColor "blue" + ShowName off + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum2" + SID "50" + Ports [2, 1] + Position [350, 110, 370, 130] + ZOrder -11 + ShowName off + IconShape "round" + Inputs "|++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum3" + SID "57" + Ports [2, 1] + Position [395, 110, 415, 130] + ZOrder -12 + ForegroundColor "red" + ShowName off + IconShape "round" + Inputs "|++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Delay + Name "control\ntime" + SID "51" + Ports [1, 1] + Position [160, 103, 185, 137] + ZOrder -13 + UserDataPersistent on + UserData "DataTag1" + InputPortMap "u0" + DelayLength "1" + InputProcessing "Inherited" + SampleTime "T" + } + Block { + BlockType TransferFcn + Name "motor" + SID "52" + Position [625, 101, 690, 139] + ZOrder -14 + ForegroundColor "blue" + Denominator "[J B]" + ContinuousStateAttributes "'qd'" + Port { + PortNumber 1 + Name "qd" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Saturate + Name "torque\nlimit" + SID "53" + Ports [1, 1] + Position [495, 105, 525, 135] + ZOrder -15 + ForegroundColor "blue" + InputPortMap "u0" + UpperLimit "tau_max" + LowerLimit "-tau_max" + } + Block { + BlockType Outport + Name "qd" + SID "54" + Position [765, 113, 795, 127] + ZOrder -16 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qd error" + SID "55" + Position [150, 33, 180, 47] + ZOrder -17 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "tau" + SID "56" + Position [765, 193, 795, 207] + ZOrder -18 + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "motor" + DstPort 1 + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [5, 0] + Branch { + DstBlock "control\ntime" + DstPort 1 + } + Branch { + Points [0, -80] + DstBlock "qd error" + DstPort 1 + } + } + Line { + Name "qd" + Labels [0, 0] + SrcBlock "motor" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "qd" + DstPort 1 + } + Branch { + Points [0, 130; -605, 0] + DstBlock "Sum" + DstPort 2 + } + } + Line { + SrcBlock "tau_d" + SrcPort 1 + Points [70, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "torque\nlimit" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "Sum1" + DstPort 2 + } + Branch { + Points [0, 80] + DstBlock "tau" + DstPort 1 + } + } + Line { + SrcBlock "control\ntime" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "Kv" + DstPort 1 + } + Branch { + DstBlock "Integrator" + DstPort 1 + } + } + Line { + Labels [0, 0] + SrcBlock "qd*" + SrcPort 1 + DstBlock "Sum" + DstPort 1 + } + Line { + SrcBlock "Km" + SrcPort 1 + DstBlock "torque\nlimit" + DstPort 1 + } + Line { + SrcBlock "Kv" + SrcPort 1 + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "Integrator" + SrcPort 1 + DstBlock "Ki" + DstPort 1 + } + Line { + SrcBlock "Ki" + SrcPort 1 + Points [30, 0] + DstBlock "Sum2" + DstPort 2 + } + Line { + SrcBlock "Sum2" + SrcPort 1 + DstBlock "Sum3" + DstPort 1 + } + Line { + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Km" + DstPort 1 + } + Line { + SrcBlock "Kff" + SrcPort 1 + Points [55, 0] + DstBlock "Sum3" + DstPort 2 + } + Line { + SrcBlock "tau_ff" + SrcPort 1 + DstBlock "Kff" + DstPort 1 + } + Annotation { + SID "79" + Name "tau_m" + Position [550, 116] + ForegroundColor "blue" + } + Annotation { + SID "80" + Name "feedforward path" + Position [182, 222] + ForegroundColor "red" + } + Annotation { + SID "81" + Name "feedback path" + Position [297, 262] + } + } + } + Block { + BlockType SubSystem + Name "vloop_voltage" + SID "163" + Ports [2, 4] + Position [405, 70, 465, 175] + ZOrder -3 + DropShadow on + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 33 + $ClassName "Simulink.Mask" + Array { + Type "Simulink.MaskParameter" + Dimension 10 + Object { + $ObjectID 34 + Type "edit" + Name "J" + Prompt "Inertia (K)" + Value "372e-6" + } + Object { + $ObjectID 35 + Type "edit" + Name "B" + Prompt "Viscous friction coefficient (B)" + Value "817e-6" + } + Object { + $ObjectID 36 + Type "edit" + Name "Km" + Prompt "Motor torque constant" + Value "0.228" + } + Object { + $ObjectID 37 + Type "edit" + Name "tau_max" + Prompt "Voltage limit (tau_max)" + Value "20" + } + Object { + $ObjectID 38 + Type "edit" + Name "Rm" + Prompt "Motor resistance (Rm)" + Value "0.1" + } + Object { + $ObjectID 39 + Type "edit" + Name "Lm" + Prompt "Motor inductance (Lm)" + Value "0.1" + } + Object { + $ObjectID 40 + Type "edit" + Name "Ka" + Prompt "Amplifier gain (Ka)" + Value "10" + } + Object { + $ObjectID 41 + Type "edit" + Name "Kv" + Prompt "Proportional gain (Kv)" + Value "1" + } + Object { + $ObjectID 42 + Type "edit" + Name "Ki" + Prompt "Integral gain (Ki)" + Value "1" + } + Object { + $ObjectID 43 + Type "edit" + Name "T" + Prompt "Sample interval (T)" + Value "0.001" + Evaluate "off" + } + PropName "Parameters" + } + } + System { + Name "vloop_voltage" + Location [978, 480, 2096, 933] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "qd*" + SID "165" + Position [25, 108, 55, 122] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tau_d" + SID "166" + Position [690, 28, 720, 42] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Gain + Name "Gain" + SID "171" + Position [725, 100, 755, 130] + ZOrder -8 + ShowName off + Gain "Km" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "tau" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Gain + Name "Gain1" + SID "172" + Position [740, 180, 770, 210] + ZOrder -8 + BlockMirror on + ShowName off + Gain "Km" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "back EMF" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Integrator + Name "Integrator" + SID "149" + Ports [1, 1] + Position [225, 145, 255, 175] + ZOrder -10 + } + Block { + BlockType Gain + Name "Ki" + SID "150" + Position [295, 145, 325, 175] + ZOrder -11 + Gain "Ki" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Kv" + SID "152" + Position [295, 100, 325, 130] + ZOrder -12 + Gain "Kv" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType TransferFcn + Name "Motor\nimpedance" + SID "170" + Position [610, 97, 670, 133] + ZOrder -9 + Denominator "[Lm Rm]" + Port { + PortNumber 1 + Name "motor\ncurrent" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Sum + Name "Sum" + SID "153" + Ports [2, 1] + Position [100, 105, 120, 125] + ZOrder -14 + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID "154" + Ports [2, 1] + Position [810, 105, 830, 125] + ZOrder -15 + ForegroundColor "blue" + ShowName off + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum2" + SID "155" + Ports [2, 1] + Position [350, 105, 370, 125] + ZOrder -16 + ShowName off + IconShape "round" + Inputs "|++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum3" + SID "173" + Ports [2, 1] + Position [540, 105, 560, 125] + ZOrder -18 + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "nett\nvoltage" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Delay + Name "control\ntime" + SID "156" + Ports [1, 1] + Position [160, 98, 185, 132] + ZOrder -19 + UserDataPersistent on + UserData "DataTag2" + InputPortMap "u0" + DelayLength "1" + InputProcessing "Inherited" + SampleTime "T" + } + Block { + BlockType TransferFcn + Name "motor" + SID "157" + Position [855, 96, 920, 134] + ZOrder -20 + ForegroundColor "blue" + Denominator "[J B]" + ContinuousStateAttributes "'qd'" + Port { + PortNumber 1 + Name "qd" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Gain + Name "voltage\namplifier\ngain" + SID "151" + Position [405, 100, 435, 130] + ZOrder -21 + ForegroundColor "blue" + Gain "Ka" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Saturate + Name "voltage\nlimit" + SID "158" + Ports [1, 1] + Position [460, 100, 490, 130] + ZOrder -22 + ForegroundColor "blue" + InputPortMap "u0" + UpperLimit "v_max" + LowerLimit "-v_max" + Port { + PortNumber 1 + Name "applied\nvoltage" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Outport + Name "qd error" + SID "164" + Position [150, 28, 180, 42] + ZOrder -23 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qd" + SID "167" + Position [990, 108, 1020, 122] + ZOrder -24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "tau" + SID "168" + Position [990, 163, 1020, 177] + ZOrder -25 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "integral" + SID "174" + Position [385, 188, 415, 202] + ZOrder -26 + Port "4" + IconDisplay "Port number" + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "motor" + DstPort 1 + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [5, 0] + Branch { + DstBlock "control\ntime" + DstPort 1 + } + Branch { + Points [0, -80] + DstBlock "qd error" + DstPort 1 + } + } + Line { + Name "qd" + Labels [0, 0] + SrcBlock "motor" + SrcPort 1 + Points [25, 0] + Branch { + DstBlock "qd" + DstPort 1 + } + Branch { + Points [0, 80] + Branch { + Points [0, 40; -840, 0] + DstBlock "Sum" + DstPort 2 + } + Branch { + DstBlock "Gain1" + DstPort 1 + } + } + } + Line { + SrcBlock "tau_d" + SrcPort 1 + Points [95, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "control\ntime" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "Kv" + DstPort 1 + } + Branch { + DstBlock "Integrator" + DstPort 1 + } + } + Line { + Labels [0, 0] + SrcBlock "qd*" + SrcPort 1 + DstBlock "Sum" + DstPort 1 + } + Line { + SrcBlock "Sum2" + SrcPort 1 + DstBlock "voltage\namplifier\ngain" + DstPort 1 + } + Line { + SrcBlock "Kv" + SrcPort 1 + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "Integrator" + SrcPort 1 + DstBlock "Ki" + DstPort 1 + } + Line { + SrcBlock "Ki" + SrcPort 1 + Points [30, 0] + Branch { + DstBlock "Sum2" + DstPort 2 + } + Branch { + Points [10, 0] + DstBlock "integral" + DstPort 1 + } + } + Line { + SrcBlock "voltage\namplifier\ngain" + SrcPort 1 + DstBlock "voltage\nlimit" + DstPort 1 + } + Line { + Name "applied\nvoltage" + Labels [0, 0] + SrcBlock "voltage\nlimit" + SrcPort 1 + DstBlock "Sum3" + DstPort 1 + } + Line { + Name "nett\nvoltage" + Labels [-1, 0] + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Motor\nimpedance" + DstPort 1 + } + Line { + Name "motor\ncurrent" + Labels [-1, 0] + SrcBlock "Motor\nimpedance" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + Name "tau" + Labels [0, 0] + SrcBlock "Gain" + SrcPort 1 + Points [30, 0] + Branch { + DstBlock "Sum1" + DstPort 2 + } + Branch { + Points [0, 55] + DstBlock "tau" + DstPort 1 + } + } + Line { + Name "back EMF" + Labels [0, 0] + SrcBlock "Gain1" + SrcPort 1 + Points [-185, 0] + DstBlock "Sum3" + DstPort 2 + } + } + } + Annotation { + SID "82" + Position [131, 168] + } + Annotation { + SID "83" + Position [131, 168] + } + Annotation { + SID "84" + Position [131, 168] + } + } +} +MatData { + NumRecords 3 + DataRecord { + Tag DataTag0 + Data " %)30 . B 8 ( @ % \" $ ! 0 % 0 $P $ 3 :&%S26" + "YH97)I=&5D3W!T:6]N #@ # & \" D\" !0 @ ! 0 $ @ ! $ " + } + DataRecord { + Tag DataTag1 + Data " %)30 . B 8 ( @ % \" $ ! 0 % 0 $P $ 3 :&%S26" + "YH97)I=&5D3W!T:6]N #@ # & \" D\" !0 @ ! 0 $ @ ! $ " + } + DataRecord { + Tag DataTag2 + Data " %)30 . B 8 ( @ % \" $ ! 0 % 0 $P $ 3 :&%S26" + "YH97)I=&5D3W!T:6]N #@ # & \" D\" !0 @ ! 0 $ @ ! $ " + } +} diff --git a/lwrserv/matlab/rvctools/simulink/ploop_test.mdl b/lwrserv/matlab/rvctools/simulink/ploop_test.mdl new file mode 100644 index 0000000..422f5cc --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/ploop_test.mdl @@ -0,0 +1,1269 @@ +Model { + Name "ploop_test" + Version 7.9 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 3 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q_err" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q*" + } + ParameterArgumentNames "" + ComputedModelVersion "1.10" + NumModelReferences 1 + ModelReference { + ModelRefBlockPath "ploop_test/ploop/vloop|vloop" + } + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + Created "Tue Jul 7 09:08:01 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Apr 29 16:03:26 2012" + RTWModifiedTimeStamp 257616203 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "all" + WideLines on + ShowLineDimensions off + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.12.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.12.0" + StartTime "0.0" + StopTime "1" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode15s" + SolverName "ode15s" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.12.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "SpecifiedOutputTimes" + OutputTimes "[0:0.001:1]'" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.12.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.12.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.12.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.12.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.12.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.12.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.12.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.12.0" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + CodeExecutionProfiling off + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface on + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Data Import//Export" + ConfigPrmDlgPosition [ 280, 124, 1160, 754 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Inport + Port "1" + OutputFunctionCall off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchInputForFeedbackSignals off + Interpolate on + } + Block { + BlockType Integrator + ExternalReset "none" + InitialConditionSource "internal" + InitialCondition "0" + LimitOutput off + UpperSaturationLimit "inf" + LowerSaturationLimit "-inf" + ShowSaturationPort off + ShowStatePort off + AbsoluteTolerance "auto" + IgnoreLimit off + ZeroCross on + ContinuousStateAttributes "''" + } + Block { + BlockType ModelReference + SimulationMode "Accelerator" + Variant off + GeneratePreprocessorConditionals off + CopyOfModelProtected off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + DataFormat "Array" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + CheckFcnCallInpInsideContextMsg off + SystemSampleTime "-1" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + SFBlockType "NONE" + Variant off + GeneratePreprocessorConditionals off + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Terminator + } + } + System { + Name "ploop_test" + Location [1148, 819, 1883, 1187] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "61" + Block { + BlockType Gain + Name "Gain" + SID "1" + Position [155, 240, 195, 270] + ShowName off + Gain "1/G" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Scope + Name "Scope" + SID "2" + Ports [1] + Position [440, 49, 470, 81] + Floating off + Location [89, 161, 413, 400] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + YMin "0" + YMax "55" + SaveName "ScopeData1" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Terminator + Name "Terminator" + SID "38" + Position [165, 155, 185, 175] + ShowName off + } + Block { + BlockType Reference + Name "lspb" + SID "41" + Ports [0, 3] + Position [95, 91, 135, 179] + BackgroundColor "red" + LibraryVersion "1.52" + SourceBlock "roblocks/Trajectory/lspb" + SourceType "" + q0 "0" + qf "0.5*G" + tmax "1" + } + Block { + BlockType SubSystem + Name "ploop" + SID "3" + Ports [3, 2] + Position [250, 87, 315, 183] + DropShadow on + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + System { + Name "ploop" + Location [572, 274, 1212, 489] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "q*" + SID "4" + Position [25, 78, 55, 92] + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "qd*" + SID "35" + Position [25, 123, 55, 137] + ForegroundColor "red" + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tau_d" + SID "5" + Position [25, 163, 55, 177] + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Integrator + Name "Integrator" + SID "6" + Ports [1, 1] + Position [415, 60, 445, 90] + ShowName off + } + Block { + BlockType Gain + Name "Kff" + SID "39" + Position [145, 115, 175, 145] + ForegroundColor "red" + Gain "0" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Kp" + SID "7" + Position [165, 70, 195, 100] + Gain "40" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum" + SID "8" + Ports [2, 1] + Position [100, 75, 120, 95] + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID "36" + Ports [2, 1] + Position [230, 75, 250, 95] + ShowName off + IconShape "round" + Inputs "|++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Terminator + Name "Terminator" + SID "9" + Position [365, 100, 385, 120] + ShowName off + } + Block { + BlockType Terminator + Name "Terminator1" + SID "10" + Position [365, 135, 385, 155] + ShowName off + } + Block { + BlockType ModelReference + Name "vloop" + SID "40" + Ports [2, 3] + Position [275, 56, 330, 164] + ModelNameDialog "vloop" + ModelReferenceVersion "1.8" + List { + ListType InputPortNames + port0 "qd*" + port1 "tau_d" + } + List { + ListType OutputPortNames + port0 "qd" + port1 "qd error" + port2 "tau" + } + List { + ListType OutputPortOutputBusAsStructs + port0 "off" + port1 "off" + port2 "off" + } + List { + ListType OutputPortSignalNames + port0 "qd" + port1 "" + port2 "" + } + List { + ListType InputPortLatchByCopyingInsideSignal + port0 "off" + port1 "off" + } + CopyOfModelName "vloop" + DefaultDataLogging off + List { + ListType InputPortLatchByDelayingOutsideSignal + port0 "off" + port1 "off" + } + } + Block { + BlockType Outport + Name "q" + SID "27" + Position [500, 68, 530, 82] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "q err" + SID "28" + Position [165, 28, 195, 42] + Port "2" + IconDisplay "Port number" + } + Line { + Labels [3, 0] + SrcBlock "tau_d" + SrcPort 1 + Points [190, 0; 0, -30] + DstBlock "vloop" + DstPort 2 + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "Kp" + DstPort 1 + } + Branch { + Points [0, -50] + DstBlock "q err" + DstPort 1 + } + } + Line { + SrcBlock "vloop" + SrcPort 1 + DstBlock "Integrator" + DstPort 1 + } + Line { + SrcBlock "Integrator" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "q" + DstPort 1 + } + Branch { + Points [0, 115; -360, 0] + DstBlock "Sum" + DstPort 2 + } + } + Line { + SrcBlock "q*" + SrcPort 1 + DstBlock "Sum" + DstPort 1 + } + Line { + SrcBlock "vloop" + SrcPort 2 + DstBlock "Terminator" + DstPort 1 + } + Line { + SrcBlock "vloop" + SrcPort 3 + DstBlock "Terminator1" + DstPort 1 + } + Line { + SrcBlock "Kp" + SrcPort 1 + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "vloop" + DstPort 1 + } + Line { + SrcBlock "qd*" + SrcPort 1 + DstBlock "Kff" + DstPort 1 + } + Line { + SrcBlock "Kff" + SrcPort 1 + Points [60, 0] + DstBlock "Sum1" + DstPort 2 + } + } + } + Block { + BlockType Scope + Name "q err" + SID "30" + Ports [1] + Position [455, 194, 485, 226] + Floating off + Location [529, 161, 853, 400] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + TimeRange "1" + YMin "-0.5" + YMax "8" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Constant + Name "tau_d" + SID "31" + Position [90, 240, 120, 270] + Value "20" + } + Block { + BlockType Outport + Name "q" + SID "32" + Position [505, 103, 535, 117] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "q_err" + SID "33" + Position [505, 153, 535, 167] + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "q*" + SID "37" + Position [270, 53, 300, 67] + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "ploop" + SrcPort 1 + Points [85, 0] + Branch { + DstBlock "q" + DstPort 1 + } + Branch { + Points [0, -45] + DstBlock "Scope" + DstPort 1 + } + } + Line { + SrcBlock "ploop" + SrcPort 2 + Points [95, 0] + Branch { + DstBlock "q_err" + DstPort 1 + } + Branch { + Points [0, 50] + DstBlock "q err" + DstPort 1 + } + } + Line { + SrcBlock "tau_d" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + SrcBlock "lspb" + SrcPort 1 + Points [55, 0] + Branch { + Points [0, -45] + DstBlock "q*" + DstPort 1 + } + Branch { + DstBlock "ploop" + DstPort 1 + } + } + Line { + SrcBlock "Gain" + SrcPort 1 + Points [15, 0; 0, -90] + DstBlock "ploop" + DstPort 3 + } + Line { + SrcBlock "lspb" + SrcPort 3 + DstBlock "Terminator" + DstPort 1 + } + Line { + SrcBlock "lspb" + SrcPort 2 + DstBlock "ploop" + DstPort 2 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/polar_sfunc.m b/lwrserv/matlab/rvctools/simulink/polar_sfunc.m new file mode 100644 index 0000000..c6bf205 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/polar_sfunc.m @@ -0,0 +1,161 @@ +function movepoint_sfunc(block) + + block.NumInputPorts = 1; + block.NumOutputPorts = 1; + + % Setup port properties to be inherited or dynamic + block.SetPreCompInpPortInfoToDynamic; + block.SetPreCompOutPortInfoToDynamic; + + % Override input port properties + block.InputPort(1).DatatypeID = 0; % double + block.InputPort(1).Complexity = 'Real'; + block.InputPort(1).Dimensions = 3; + block.InputPort(1).DirectFeedthrough = true; + + % Override output port properties + block.OutputPort(1).DatatypeID = 0; % double + block.OutputPort(1).Complexity = 'Real'; + block.OutputPort(1).Dimensions = 4; + + block.NumDialogPrms = 0; + + % Specify if Accelerator should use TLC or call back into + % M-file + block.SetAccelRunOnTLC(false); + + block.SimStateCompliance = 'DefaultSimState'; + + %block.RegBlockMethod('CheckParameters', @CheckPrms); + + %block.RegBlockMethod('SetInputPortSamplingMode', @SetInpPortFrameData); + + %block.RegBlockMethod('SetInputPortDimensions', @SetInpPortDims); + %block.RegBlockMethod('SetOutputPortDimensions', @SetOutPortDims); + %block.RegBlockMethod('SetInputPortDataType', @SetInpPortDataType); + %block.RegBlockMethod('SetOutputPortDataType', @SetOutPortDataType); + %block.RegBlockMethod('SetInputPortComplexSignal', @SetInpPortComplexSig); + %block.RegBlockMethod('SetOutputPortComplexSignal', @SetOutPortComplexSig); + + %% PostPropagationSetup: + %% Functionality : Setup work areas and state variables. Can + %% also register run-time methods here + block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup); + + %% Register methods called at run-time + %% ----------------------------------------------------------------- + + %% + %% ProcessParameters: + %% Functionality : Called in order to allow update of run-time + %% parameters + %block.RegBlockMethod('ProcessParameters', @ProcessPrms); + + %% + %% InitializeConditions: + %% Functionality : Called in order to initialize state and work + %% area values + %block.RegBlockMethod('InitializeConditions', @InitializeConditions); + + %% + %% Start: + %% Functionality : Called in order to initialize state and work + %% area values + block.RegBlockMethod('Start', @Start); + + %% + %% Outputs: + %% Functionality : Called to generate block outputs in + %% simulation step + block.RegBlockMethod('Outputs', @Outputs); + + %% + %% Update: + %% Functionality : Called to update discrete states + %% during simulation step + %% C-Mex counterpart: mdlUpdate + %% + %block.RegBlockMethod('Update', @Update); + + %% + %% Derivatives: + %% Functionality : Called to update derivatives of + %% continuous states during simulation step + %% C-Mex counterpart: mdlDerivatives + %% + %block.RegBlockMethod('Derivatives', @Derivatives); + + %% + %% Projection: + %% Functionality : Called to update projections during + %% simulation step + %% C-Mex counterpart: mdlProjections + %% + %block.RegBlockMethod('Projection', @Projection); + + %% + %% SimStatusChange: + %% Functionality : Called when simulation goes to pause mode + %% or continnues from pause mode + %% C-Mex counterpart: mdlSimStatusChange + %% + %block.RegBlockMethod('SimStatusChange', @SimStatusChange); + + %% + %% Terminate: + %% Functionality : Called at the end of simulation for cleanup + %% C-Mex counterpart: mdlTerminate + %% + %block.RegBlockMethod('Terminate', @Terminate); + + %block.RegBlockMethod('WriteRTW', @WriteRTW); +end + +function DoPostPropSetup(block) + block.NumDworks = 1; + block.Dwork(1).Name = 'direction'; + block.Dwork(1).Dimensions = 1; + block.Dwork(1).DatatypeID = 0; + block.Dwork(1).Complexity = 'Real'; + block.Dwork(1).UsedAsDiscState = false; +end + +function Start(block) + block.Dwork(1).Data = 0; +end + +function Outputs(block) + X = block.InputPort(1).Data; + + x = X(1); y = X(2); theta = X(3); + + d = sqrt(x^2 + y^2); + + if block.Dwork(1).Data == 0 + beta = -atan2(-y, -x); + alpha = -theta - beta; + fprintf('alpha %f, beta %f\n', alpha, beta); + % first time in simulation, choose the direction of travel + if (alpha > pi/2) || (alpha < -pi/2) + fprintf('going backwards\n'); + block.Dwork(1).Data = -1; + else + fprintf('going forwards\n'); + block.Dwork(1).Data = 1; + end + elseif block.Dwork(1).Data == -1 + beta = -atan2(y, x); + alpha = -theta - beta; + else + beta = -atan2(-y, -x); + alpha = -theta - beta; + end + if alpha > pi/2 + alpha = pi/2; + end + if alpha < -pi/2 + alpha = -pi/2; + end + + block.OutputPort(1).Data = [d alpha beta block.Dwork(1).Data]; +end diff --git a/lwrserv/matlab/rvctools/simulink/quadrotor_dynamics.m b/lwrserv/matlab/rvctools/simulink/quadrotor_dynamics.m new file mode 100644 index 0000000..317e960 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/quadrotor_dynamics.m @@ -0,0 +1,253 @@ + +function [sys,x0,str,ts] = quadrotor_dynamics(t,x,u,flag, quad) + % Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04 + % A simulation of idealised X-4 Flyer II flight dynamics. + % version 2.0 2005 modified to be compatible with latest version of Matlab + % version 3.0 2006 fixed rotation matrix problem + % version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring + + warning off MATLAB:divideByZero + + % New in version 2: + % - Generalised rotor thrust model + % - Rotor flapping model + % - Frame aerodynamic drag model + % - Frame aerodynamic surfaces model + % - Internal motor model + % - Much coolage + + % Version 1.3 + % - Rigid body dynamic model + % - Rotor gyroscopic model + % - External motor model + + %ARGUMENTS + % u Reference inputs 1x4 + % tele Enable telemetry (1 or 0) 1x1 + % crash Enable crash detection (1 or 0) 1x1 + % init Initial conditions 1x12 + + %INPUTS + % u = [N S E W] + % NSEW motor commands 1x4 + + %CONTINUOUS STATES + % z Position 3x1 (x,y,z) + % v Velocity 3x1 (xd,yd,zd) + % n Attitude 3x1 (Y,P,R) + % o Angular velocity 3x1 (wx,wy,wz) + % w Rotor angular velocity 4x1 + + %CONTINUOUS STATE MATRIX MAPPING + % x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4] + + %INITIAL CONDITIONS + z0 = [-1 0 -.15]; % z0 Position initial conditions 1x3 + n0 = [0 0 0]; % n0 Ang. position initial conditions 1x3 + v0 = [0 0 0]; % v0 Velocity Initial conditions 1x3 + o0 = [0 0 0]; % o0 Ang. velocity initial conditions 1x3 + init = [z0 n0 v0 o0]; + + %CONTINUOUS STATE EQUATIONS + % z` = v + % v` = g*e3 - (1/m)*T*R*e3 + % I*o` = -o X I*o + G + torq + % R = f(n) + % n` = inv(W)*o + + % Dispatch the flag. + % + + switch flag + case 0 + [sys,x0,str,ts]=mdlInitializeSizes(init, quad); % Initialization + case 1 + sys = mdlDerivatives(t,x,u, quad); % Calculate derivatives + case 3 + sys = mdlOutputs(t,x, quad); % Calculate outputs + case { 2, 4, 9 } % Unused flags + sys = []; + otherwise + error(['Unhandled flag = ',num2str(flag)]); % Error handling + end +end % End of flyer2dynamics + +%============================================================== +% mdlInitializeSizes +% Return the sizes, initial conditions, and sample times for the +% S-function. +%============================================================== +% +function [sys,x0,str,ts] = mdlInitializeSizes(init, quad) + % + % Call simsizes for a sizes structure, fill it in and convert it + % to a sizes array. + % + sizes = simsizes; + sizes.NumContStates = 12; + sizes.NumDiscStates = 0; + sizes.NumOutputs = 12; + sizes.NumInputs = 4; + sizes.DirFeedthrough = 0; + sizes.NumSampleTimes = 1; + sys = simsizes(sizes); + % + % Initialize the initial conditions. + x0 = init; + % + % str is an empty matrix. + str = []; + % + % Generic timesample + ts = [0 0]; + + if quad.verbose + disp(sprintf('t\t\tz1\t\tz2\t\tz3\t\tn1\t\tn2\t\tn3\t\tv1\t\tv2\t\tv3\t\to1\t\to2\t\to3\t\tw1\t\tw2\t\tw3\t\tw4\t\tu1\t\tu2\t\tu3\t\tu4')) + end +end % End of mdlInitializeSizes. + + +%============================================================== +% mdlDerivatives +% Calculate the state derivatives for the next timestep +%============================================================== +% +function sys = mdlDerivatives(t,x,u, quad) + global a1s b1s + + %CONSTANTS + %Cardinal Direction Indicies + N = 1; % N 'North' 1x1 + E = 2; % S 'South' 1x1 + S = 3; % E 'East' 1x1 + W = 4; % W 'West' 1x1 + + + D(:,1) = [quad.d;0;quad.h]; % Di Rotor hub displacements 1x3 + D(:,2) = [0;quad.d;quad.h]; + D(:,3) = [-quad.d;0;quad.h]; + D(:,4) = [0;-quad.d;quad.h]; + + %Body-fixed frame references + e1 = [1;0;0]; % ei Body fixed frame references 3x1 + e2 = [0;1;0]; + e3 = [0;0;1]; + + %EXTRACT STATES FROM U + w = u(1:4); + + %EXTRACT STATES FROM X + z = x(1:3); % position in {W} + n = x(4:6); % RPY angles {W} + v = x(7:9); % velocity in {W} + o = x(10:12); % angular velocity in {W} + + %PREPROCESS ROTATION AND WRONSKIAN MATRICIES + phi = n(1); % yaw + the = n(2); % pitch + psi = n(3); % roll + + % rotz(phi)*roty(the)*rotx(psi) + R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); %BBF > Inertial rotation matrix + cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi); + -sin(the) sin(psi)*cos(the) cos(psi)*cos(the)]; + + + %Manual Construction + % Q3 = [cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1]; % RZ %Rotation mappings + % Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; % RY + % Q1 = [1 0 0;0 cos(psi) -sin(psi);0 sin(psi) cos(psi)]; % RX + % R = Q3*Q2*Q1 %Rotation matrix + % + % RZ * RY * RX + iW = [0 sin(psi) cos(psi); %inverted Wronskian + 0 cos(psi)*cos(the) -sin(psi)*cos(the); + cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the); + + %ROTOR MODEL + for i=[N E S W] %for each rotor + %Relative motion + + Vr = cross(o,D(:,i)) + v; + mu = sqrt(sum(Vr(1:2).^2)) / (abs(w(i))*quad.r); %Magnitude of mu, planar components + lc = Vr(3) / (abs(w(i))*quad.r); %Non-dimensionalised normal inflow + li = mu; %Non-dimensionalised induced velocity approximation + alphas = atan2(lc,mu); + j = atan2(Vr(2),Vr(1)); %Sideslip azimuth relative to e1 (zero over nose) + J = [cos(j) -sin(j); + sin(j) cos(j)]; %BBF > mu sideslip rotation matrix + + %Flapping + beta = [((8/3*quad.theta0 + 2*quad.theta1)*mu - 2*(lc)*mu)/(1-mu^2/2); %Longitudinal flapping + 0;];%sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)]; %Lattitudinal flapping (note sign) + beta = J'*beta; %Rotate the beta flapping angles to longitudinal and lateral coordinates. + a1s(i) = beta(1) - 16/quad.gamma/abs(w(i)) * o(2); + b1s(i) = beta(2) - 16/quad.gamma/abs(w(i)) * o(1); + + %Forces and torques + T(:,i) = quad.Ct*quad.rho*quad.A*quad.r^2*w(i)^2 * [-cos(b1s(i))*sin(a1s(i)); sin(b1s(i));-cos(a1s(i))*cos(b1s(i))]; %Rotor thrust, linearised angle approximations + Q(:,i) = -quad.Cq*quad.rho*quad.A*quad.r^3*w(i)*abs(w(i)) * e3; %Rotor drag torque - note that this preserves w(i) direction sign + tau(:,i) = cross(T(:,i),D(:,i)); %Torque due to rotor thrust + end + + %RIGID BODY DYNAMIC MODEL + dz = v; + dn = iW*o; + + dv = quad.g*e3 + R*(1/quad.M)*sum(T,2); + do = inv(quad.J)*(cross(-o,quad.J*o) + sum(tau,2) + sum(Q,2)); %row sum of torques + sys = [dz;dn;dv;do]; %This is the state derivative vector +end % End of mdlDerivatives. + + +%============================================================== +% mdlOutputs +% Calculate the output vector for this timestep +%============================================================== +% +function sys = mdlOutputs(t,x, quad) + %CRASH DETECTION + if x(3)>0 + x(3) = 0; + if x(6) > 0 + x(6) = 0; + end + end + %if (x(3)>0)&(crash) + % error('CRASH!') + %end + + %TELEMETRY + if quad.verbose + disp(sprintf('%0.3f\t',t,x)) + end + + % compute output vector as a function of state vector + % z Position 3x1 (x,y,z) + % v Velocity 3x1 (xd,yd,zd) + % n Attitude 3x1 (Y,P,R) + % o Angular velocity 3x1 (Yd,Pd,Rd) + + n = x(4:6); % RPY angles + phi = n(1); % yaw + the = n(2); % pitch + psi = n(3); % roll + + + % rotz(phi)*roty(the)*rotx(psi) + R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); %BBF > Inertial rotation matrix + cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi); + -sin(the) sin(psi)*cos(the) cos(psi)*cos(the)]; + + iW = [0 sin(psi) cos(psi); %inverted Wronskian + 0 cos(psi)*cos(the) -sin(psi)*cos(the); + cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the); + + % return velocity in the body frame + sys = [ x(1:6); + inv(R)*x(7:9); % translational velocity mapped to body frame + iW*x(10:12)]; % RPY rates mapped to body frame + %sys = [x(1:6); iW*x(7:9); iW*x(10:12)]; + %sys = x; +end +% End of mdlOutputs. diff --git a/lwrserv/matlab/rvctools/simulink/quadrotor_plot.m b/lwrserv/matlab/rvctools/simulink/quadrotor_plot.m new file mode 100644 index 0000000..ee387a4 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/quadrotor_plot.m @@ -0,0 +1,195 @@ + + +% 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 . +function [sys,x0,str,ts] = flyerplot3(t,x,u,flag,s,plot,enable) +% Flyer plot, lovingly coded by Paul Pounds, first coded 17/4/02 +% version 2 2004 added scaling and ground display +% version 3 2010 improved rotor rendering and fixed mirroring bug +% +% Displays X-4 flyer position and attitude in a 3D plot. +% GREEN ROTOR POINTS NORTH +% BLUE ROTOR POINTS EAST + +% PARAMETERS +% s defines the plot size in meters +% swi controls flyer attitude plot; 1 = on, otherwise off. + +% INPUTS +% 1 Center X position +% 2 Center Y position +% 3 Center Z position +% 4 Yaw angle in rad +% 5 Pitch angle in rad +% 6 Roll angle in rad + +% OUTPUTS +% None +ts = [-1 0]; + +switch flag, + case 0 + [sys,x0,str,ts] = mdlInitializeSizes(ts,plot,enable); % Initialization + case 3 + sys = mdlOutputs(t,u,s,plot,enable); % Calculate outputs + case {1,2, 4, 9} % Unused flags + sys = []; + otherwise + error(['unhandled flag = ',num2str(flag)]); % Error handling +end + + +% Initialize +function [sys,x0,str,ts] = mdlInitializeSizes(ts,plot,enable) +% Call simsizes for a sizes structure, fill it in, and convert it +% to a sizes array. +sizes = simsizes; +sizes.NumContStates = 0; +sizes.NumDiscStates = 0; +sizes.NumOutputs = 0; +sizes.NumInputs = 6; +sizes.DirFeedthrough = 1; +sizes.NumSampleTimes = 1; +sys = simsizes(sizes); +x0 = []; +str = []; % Set str to an empty matrix. +ts = [0.05 0]; + +if enable == 1 + figure(plot); + clf; + %colordef(1,'none'); +end +% End of mdlInitializeSizes. + + +function sys = mdlOutputs(t,u,s,plot,enable) +global a1s b1s + + name = strcat('flyer_movie',num2str((t/0.125)*2+1),'r.bmp'); %odds + %name = strcat('flyer_movie',num2str(160-(t/0.125)*2),'l.bmp'); %evens + + +if size(a1s) == [0 0]; + a1s = [0 0 0 0]; + b1s = [0 0 0 0]; +end + +d = 0.315; %Hub displacement from COG +r = 0.165; %Rotor radius +N = 1; +S = 2; +E = 3; +W = 4; +D(:,N) = [d;0;0]; %displacements +D(:,S) = [-d;0;0]; +D(:,E) = [0;d;0]; +D(:,W) = [0;-d;0]; +C(:,N) = [s(1)/4;0;0]; %Attitude center displacements +C(:,S) = [-s(1)/4;0;0]; +C(:,E) = [0;s(1)/4;0]; +C(:,W) = [0;-s(1)/4;0]; + +if enable == 1 + %draw ground + figure(plot); + clf; + if length(s) == 1 + axis([-s s -s s 0 s]); + else + axis([-s(1) s(1) -s(1) s(1) 0 s(2)]) + s = s(1); + end + hold on; + + plot3([-s -s],[s -s],[0 0],'-b') + plot3([-s s],[s s],[0 0],'-b') + plot3([s -s],[-s -s],[0 0],'-b') + plot3([s s],[s -s],[0 0],'-b') + plot3([s -s],[-s s],[0 0],'-b') + plot3([-s s],[-s s],[0 0],'-b') + + %READ STATE + z = [u(1);u(2);u(3)]; + n = [u(4);u(5);u(6)]; + + %PREPROCESS ROTATION MATRIX + phi = n(1); %Euler angles + the = n(2); + psi = n(3); + + R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); %BBF > Inertial rotation matrix + cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi); + -sin(the) sin(psi)*cos(the) cos(psi)*cos(the)]; + + %Manual Construction + %Q3 = [cos(psi) -sin(psi) 0;sin(psi) cos(psi) 0;0 0 1]; %Rotation mappings + %Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; + %Q1 = [1 0 0;0 cos(phi) -sin(phi);0 sin(phi) cos(phi)]; + %R = Q3*Q2*Q1; %Rotation matrix + + %CALCULATE FLYER TIP POSITONS USING COORDINATE FRAME ROTATION + F = [1 0 0;0 -1 0;0 0 -1]; + + %Draw flyer rotors + t = [0:pi/8:2*pi]; + for j = 1:length(t) + circle(:,j) = [r*sin(t(j));r*cos(t(j));0]; + end + + for i = [N S E W] + hub(:,i) = F*(z + R*D(:,i)); %points in the inertial frame + + q = 1; %Flapping angle scaling for output display - makes it easier to see what flapping is occurring + Rr = [cos(q*a1s(i)) sin(q*b1s(i))*sin(q*a1s(i)) cos(q*b1s(i))*sin(q*a1s(i)); %Rotor > Plot frame + 0 cos(q*b1s(i)) -sin(q*b1s(i)); + -sin(q*a1s(i)) sin(q*b1s(i))*cos(q*a1s(i)) cos(q*b1s(i))*cos(q*a1s(i))]; + + tippath(:,:,i) = F*R*Rr*circle; + plot3([hub(1,i)+tippath(1,:,i)],[hub(2,i)+tippath(2,:,i)],[hub(3,i)+tippath(3,:,i)],'b-') + end + + + + + + + + + % %Draw flyer + plot3([hub(1,N) hub(1,S)],[hub(2,N) hub(2,S)],[hub(3,N) hub(3,S)],'-b') + plot3([hub(1,E) hub(1,W)],[hub(2,E) hub(2,W)],[hub(3,E) hub(3,W)],'-b') + plot3([hub(1,N)],[hub(2,N)],[hub(3,N)],'og') + plot3([hub(1,S)],[hub(2,S)],[hub(3,S)],'or') + plot3([hub(1,E)],[hub(2,E)],[hub(3,E)],'oc') + plot3([hub(1,W)],[hub(2,W)],[hub(3,W)],'or') + + + %Tracking lines + plot3([z(1) 0],[-z(2) 0],[0 0],'--k') + plot3([z(1)],[-z(2)],[0],'xk') +% plot3([-s -s],[s s],[-z(3) 0],'--r') +% plot3([-s],[s],[-z(3)],'xr') + xlabel('x'); + ylabel('y'); + zlabel('z (height above ground)'); +end + +%saveas(1,name); + +sys = []; +% End of mdlOutputs. diff --git a/lwrserv/matlab/rvctools/simulink/roblocks.mdl b/lwrserv/matlab/rvctools/simulink/roblocks.mdl new file mode 100644 index 0000000..aec3760 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/roblocks.mdl @@ -0,0 +1,6672 @@ +Library { + Name "roblocks" + Version 8.0 + MdlSubVersion 0 + SavedCharacterEncoding "ISO-8859-1" + LibraryType "BlockLibrary" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [618.0, 490.0, 563.0, 616.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [0] + DockPosition "Left" + Width [50] + Height [50] + Filter [9] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Object { + $PropName "EditorsInfo" + $ObjectID 5 + $ClassName "Simulink.EditorInfo" + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [529.0, 454.0] + ZoomFactor [1.0] + Offset [0.0, -5.0] + } + } + } + Created "Mon Feb 06 11:30:26 2012" + Creator "n6334687" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jan 27 11:27:27 2013" + RTWModifiedTimeStamp 281186779 + ModelVersionFormat "1.%" + ConfigurationManager "None" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "none" + WideLines off + ShowLineDimensions off + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks on + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + RecordCoverage off + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 6 + Version "1.12.1" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 7 + Version "1.12.1" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 8 + Version "1.12.1" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 9 + Version "1.12.1" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 10 + Version "1.12.1" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "UseLocalSettings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorLevel1" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + SFUndirectedBroadcastEventsDiag "warning" + SFTransitionActionBeforeConditionDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 11 + Version "1.12.1" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 12 + Version "1.12.1" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 13 + Version "1.12.1" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 14 + Version "1.12.1" + Array { + Type "Cell" + Dimension 7 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + Cell "GenerateSLWebview" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + PortableWordSizes off + GenerateErtSFunction off + CreateSILPILBlock "None" + CodeExecutionProfiling off + CodeExecutionProfileVariable "executionProfile" + CodeProfilingSaveOptions "SummaryOnly" + CodeProfilingInstrumentation off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 15 + Version "1.12.1" + Array { + Type "Cell" + Dimension 21 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "InsertPolySpaceComments" + Cell "SFDataObjDesc" + Cell "MATLABFcnDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + OperatorAnnotations off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InternalIdentifier "Classic" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 16 + Version "1.12.1" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + Cell "PortableWordSizes" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface on + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 400, 210, 1280, 840 ] + } + PropName "ConfigurationSets" + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + MaskDefaults { + SelfModifiable "off" + IconFrame "on" + IconOpaque "on" + RunInitForIconRedraw "off" + IconRotate "none" + PortRotate "default" + IconUnits "autoscale" + } + MaskParameterDefaults { + Evaluate "on" + Tunable "on" + NeverSave "off" + Internal "off" + ReadOnly "off" + Enabled "on" + Visible "on" + ToolTip "on" + } + BlockParameterDefaults { + Block { + BlockType BusCreator + Inputs "4" + DisplayOption "none" + OutDataTypeStr "Inherit: auto" + NonVirtualBus off + } + Block { + BlockType BusSelector + OutputSignals "signal1,signal2,signal3" + OutputAsBus off + } + Block { + BlockType Clock + DisplayTime off + Decimation "10" + } + Block { + BlockType Concatenate + NumInputs "2" + Mode "Vector" + ConcatenateDimension "1" + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Demux + Outputs "4" + DisplayOption "none" + BusSelectionMode off + } + Block { + BlockType Fcn + Expr "sin(u[1])" + SampleTime "-1" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Inport + Port "1" + OutputFunctionCall off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchInputForFeedbackSignals off + Interpolate on + } + Block { + BlockType Integrator + ExternalReset "none" + InitialConditionSource "internal" + InitialCondition "0" + LimitOutput off + UpperSaturationLimit "inf" + LowerSaturationLimit "-inf" + ShowSaturationPort off + ShowStatePort off + AbsoluteTolerance "auto" + IgnoreLimit off + ZeroCross on + ContinuousStateAttributes "''" + } + Block { + BlockType MATLABFcn + MATLABFcn "sin" + OutputDimensions "-1" + OutputSignalType "auto" + Output1D on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Product + Inputs "2" + Multiplication "Element-wise(.*)" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Zero" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType RateLimiter + RisingSlewLimit "1" + FallingSlewLimit "-1" + SampleTimeMode "continuous" + InitialCondition "0" + LinearizeAsGain on + } + Block { + BlockType Reshape + OutputDimensionality "1-D array" + OutputDimensions "[1,1]" + } + Block { + BlockType S-Function + FunctionName "system" + SFunctionModules "''" + PortCounts "[]" + SFunctionDeploymentMode off + } + Block { + BlockType Saturate + UpperLimitSource "Dialog" + UpperLimit "0.5" + LowerLimitSource "Dialog" + LowerLimit "-0.5" + LinearizeAsGain on + ZeroCross on + SampleTime "-1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + CheckFcnCallInpInsideContextMsg off + SystemSampleTime "-1" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + SFBlockType "NONE" + Variant off + GeneratePreprocessorConditionals off + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Trigonometry + Operator "sin" + ApproximationMethod "None" + NumberOfIterations "11" + OutputSignalType "auto" + SampleTime "-1" + } + Block { + BlockType UnitDelay + InitialCondition "0" + InputProcessing "Inherited" + SampleTime "1" + StateMustResolveToSignalObject off + CodeGenStateStorageClass "Auto" + HasFrameUpgradeWarning on + } + } + System { + Name "roblocks" + Location [618, 490, 1181, 1106] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 212 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "648" + Block { + BlockType SubSystem + Name "Dynamics" + SID "5" + Ports [] + Position [152, 199, 260, 241] + ZOrder -1 + BackgroundColor "darkGreen" + ShowName off + LibraryVersion "1.225" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 17 + $ClassName "Simulink.Mask" + Display "color('darkBlue')\ndisp('Dynamics')" + IconUnits "pixels" + } + System { + Name "Dynamics" + Location [1072, 55, 1491, 783] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType SubSystem + Name "Bicycle" + SID "51" + Ports [2, 3] + Position [115, 207, 175, 283] + ZOrder -1 + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 18 + $ClassName "Simulink.Mask" + Description "Kino-dynamic model of bicycle vehicle" + Help "Kino-dynamic model of bicycle vehicle, also known as Ackermann vehicle\nmodel.\n\nParameters::\n" + " \nAcceleration (1x1) Vehicle acceleration limit [m/s2]\nVelocity limit (1x1) Vehicle speed limit [m/s]\nSteer" + "ing limits (1x1) Steering angle limit [rad]\nInitial state (1x3) Initial state (x, y, theta)\nHandbrake (1x1) Veh" + "icle cannot move if non-zero\n \nInputs::\n \nv (1x1) velocity demand [m/s]\ngamma (1x1) steering angle [r" + "ad]\n \nOutputs::\n \nx (1x1) x-coordinate in world frame [m]\ny (1x1) x-coordinate in world frame [m]\nth" + "eta (1x1) heading angle in world frame [rad]\n\n \nNotes::\n \n- Steering limit is symmetric between -limi" + "t and +limit\n- Same dynamic model as the Toolbox class Vehicle.\n\n" + Array { + Type "Simulink.MaskParameter" + Dimension 6 + Object { + $ObjectID 19 + Type "edit" + Name "accel" + Prompt "Acceleration" + Value "1" + } + Object { + $ObjectID 20 + Type "edit" + Name "vlim" + Prompt "Velocity limit" + Value "1" + } + Object { + $ObjectID 21 + Type "edit" + Name "alim" + Prompt "Steering limits" + Value "2" + } + Object { + $ObjectID 22 + Type "edit" + Name "L" + Prompt "Wheel base" + Value "0" + } + Object { + $ObjectID 23 + Type "edit" + Name "x0" + Prompt "Initial state (x, y, theta)" + Value "[0, 0, 0]" + } + Object { + $ObjectID 24 + Type "edit" + Name "handbrake" + Prompt "Handbrake" + Value "0" + } + PropName "Parameters" + } + } + System { + Name "Bicycle" + Location [63, 384, 735, 831] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "243" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "v" + SID "51:2" + Position [25, 43, 55, 57] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "gamma" + SID "51:3" + Position [25, 158, 55, 172] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant" + SID "51:222" + Position [320, 210, 395, 240] + ZOrder -3 + ShowName off + Value "handbrake" + } + Block { + BlockType Demux + Name "Demux" + SID "51:4" + Ports [1, 3] + Position [525, 104, 530, 166] + ZOrder -4 + ShowName off + Outputs "3" + DisplayOption "bar" + } + Block { + BlockType Fcn + Name "Fcn" + SID "51:226" + Position [135, 150, 195, 180] + ZOrder -5 + ShowName off + Expr "tan(u)/L" + } + Block { + BlockType Integrator + Name "Integrator" + SID "51:227" + Ports [2, 1] + Position [440, 113, 470, 157] + ZOrder -6 + ExternalReset "level" + InitialCondition "x0" + } + Block { + BlockType Mux + Name "Mux" + SID "51:6" + Ports [3, 1] + Position [310, 81, 315, 149] + ZOrder -7 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Product + Name "Product" + SID "51:7" + Ports [2, 1] + Position [365, 107, 395, 138] + ZOrder -8 + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Trigonometry + Name "Trigonometric\nFunction" + SID "51:8" + Ports [1, 1] + Position [240, 80, 270, 110] + ZOrder -9 + ShowName off + Operator "cos" + } + Block { + BlockType Trigonometry + Name "Trigonometric\nFunction2" + SID "51:9" + Ports [1, 1] + Position [240, 125, 270, 155] + ZOrder -10 + ShowName off + } + Block { + BlockType RateLimiter + Name "acceleration\nlimit" + SID "51:10" + Position [150, 35, 180, 65] + ZOrder -11 + RisingSlewLimit "accel" + FallingSlewLimit "-accel" + SampleTimeMode "inherited" + } + Block { + BlockType Saturate + Name "steering\nangle\nlimit" + SID "51:11" + Ports [1, 1] + Position [80, 150, 110, 180] + ZOrder -12 + InputPortMap "u0" + UpperLimit "alim" + LowerLimit "-alim" + } + Block { + BlockType Saturate + Name "vel\nlimit" + SID "51:12" + Ports [1, 1] + Position [90, 35, 120, 65] + ZOrder -13 + InputPortMap "u0" + UpperLimit "vlim" + LowerLimit "-vlim" + } + Block { + BlockType Outport + Name "x" + SID "51:13" + Position [570, 108, 600, 122] + ZOrder -14 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "y" + SID "51:14" + Position [570, 148, 600, 162] + ZOrder -15 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "theta" + SID "51:15" + Position [570, 193, 600, 207] + ZOrder -16 + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "Constant" + SrcPort 1 + Points [5, 0; 0, -80] + DstBlock "Integrator" + DstPort 2 + } + Line { + SrcBlock "steering\nangle\nlimit" + SrcPort 1 + DstBlock "Fcn" + DstPort 1 + } + Line { + SrcBlock "Trigonometric\nFunction2" + SrcPort 1 + Points [5, 0; 0, -25] + DstBlock "Mux" + DstPort 2 + } + Line { + SrcBlock "vel\nlimit" + SrcPort 1 + DstBlock "acceleration\nlimit" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + SrcBlock "v" + SrcPort 1 + DstBlock "vel\nlimit" + DstPort 1 + } + Line { + SrcBlock "gamma" + SrcPort 1 + DstBlock "steering\nangle\nlimit" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 2 + Points [20, 0] + DstBlock "y" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 1 + DstBlock "x" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 3 + Points [5, 0; 0, 45] + Branch { + DstBlock "theta" + DstPort 1 + } + Branch { + Points [-325, 0; 0, -60] + Branch { + DstBlock "Trigonometric\nFunction2" + DstPort 1 + } + Branch { + Points [0, -45] + DstBlock "Trigonometric\nFunction" + DstPort 1 + } + } + } + Line { + SrcBlock "Integrator" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "Product" + SrcPort 1 + DstBlock "Integrator" + DstPort 1 + } + Line { + SrcBlock "acceleration\nlimit" + SrcPort 1 + Points [155, 0; 0, 80] + DstBlock "Product" + DstPort 2 + } + Line { + SrcBlock "Trigonometric\nFunction" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "Fcn" + SrcPort 1 + Points [95, 0] + DstBlock "Mux" + DstPort 3 + } + Annotation { + SID "51:228" + Name "Bicycle kinematic model for mobile robot" + Position [368, 29] + } + } + } + Block { + BlockType SubSystem + Name "Control Mixer" + SID "52" + Ports [4, 5] + Position [115, 330, 175, 470] + ZOrder -2 + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 25 + $ClassName "Simulink.Mask" + Description "Quadrotor control mixer" + Help "Determine rotor speeds based on desired torques and thrust.\n \nInputs::\n \ntau pitch (1x1) d" + "esired pitching torque [Nm]\ntau roll (1x1) desired rolling torque [Nm]\ntau yaw (1x1) desired yawing torque" + " [Nm]\nT (1x1) desired thrust [N]\n \nOutputs::\n \nw1 (1x1) angular velocity of rotor 1 [rad]\nw" + "2 (1x1) angular velocity of rotor 2 [rad]\nw3 (1x1) angular velocity of rotor 3 [rad]\nw4 (1x1) angular velocity " + "of rotor 4 [rad]\n \nNotes::\n \n- Assumes the rotor labelling convention of RVC Fig 4.16.\n- The quadroto" + "r is underactuated and has only 4 inputs.\n" + } + System { + Name "Control Mixer" + Location [397, 175, 1302, 734] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "249" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "tau pitch" + SID "52:156" + Position [15, 78, 45, 92] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tau roll" + SID "52:157" + Position [15, 123, 45, 137] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tau yaw" + SID "52:158" + Position [15, 243, 45, 257] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "T" + SID "52:159" + Position [15, 283, 45, 297] + ZOrder -4 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Gain + Name "Gain1" + SID "52:160" + Position [595, 205, 625, 235] + ZOrder -5 + ShowName off + Gain "-1" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Gain2" + SID "52:161" + Position [595, 115, 625, 145] + ZOrder -6 + ShowName off + Gain "-1" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Mux + Name "Mux" + SID "52:162" + Ports [4, 1] + Position [480, 276, 485, 314] + ZOrder -7 + ShowName off + DisplayOption "bar" + } + Block { + BlockType Saturate + Name "Saturation1" + SID "52:163" + Ports [1, 1] + Position [520, 115, 550, 145] + ZOrder -8 + ShowName off + InputPortMap "u0" + UpperLimit "1000" + LowerLimit "700" + } + Block { + BlockType Saturate + Name "Saturation2" + SID "52:164" + Ports [1, 1] + Position [520, 70, 550, 100] + ZOrder -9 + ShowName off + InputPortMap "u0" + UpperLimit "1000" + LowerLimit "700" + } + Block { + BlockType Saturate + Name "Saturation3" + SID "52:165" + Ports [1, 1] + Position [520, 160, 550, 190] + ZOrder -10 + ShowName off + InputPortMap "u0" + UpperLimit "1000" + LowerLimit "700" + } + Block { + BlockType Saturate + Name "Saturation4" + SID "52:166" + Ports [1, 1] + Position [520, 205, 550, 235] + ZOrder -11 + ShowName off + InputPortMap "u0" + UpperLimit "1000" + LowerLimit "700" + } + Block { + BlockType Sum + Name "Sum1" + SID "52:167" + Ports [2, 1] + Position [240, 75, 260, 95] + ZOrder -12 + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Sum + Name "Sum10" + SID "52:168" + Ports [2, 1] + Position [320, 120, 340, 140] + ZOrder -13 + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Sum + Name "Sum11" + SID "52:169" + Ports [2, 1] + Position [345, 165, 365, 185] + ZOrder -14 + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Sum + Name "Sum12" + SID "52:170" + Ports [2, 1] + Position [370, 210, 390, 230] + ZOrder -15 + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Sum + Name "Sum3" + SID "52:171" + Ports [2, 1] + Position [215, 120, 235, 140] + ZOrder -16 + ShowName off + IconShape "round" + Inputs "|--" + } + Block { + BlockType Sum + Name "Sum5" + SID "52:172" + Ports [2, 1] + Position [190, 165, 210, 185] + ZOrder -17 + ShowName off + IconShape "round" + Inputs "|-+" + } + Block { + BlockType Sum + Name "Sum7" + SID "52:173" + Ports [2, 1] + Position [165, 210, 185, 230] + ZOrder -18 + ShowName off + IconShape "round" + Inputs "|+-" + } + Block { + BlockType Sum + Name "Sum9" + SID "52:174" + Ports [2, 1] + Position [290, 75, 310, 95] + ZOrder -19 + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Outport + Name "w1" + SID "52:175" + Position [650, 78, 680, 92] + ZOrder -20 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "w2" + SID "52:176" + Position [650, 123, 680, 137] + ZOrder -21 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "w3" + SID "52:177" + Position [650, 168, 680, 182] + ZOrder -22 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "w4" + SID "52:178" + Position [650, 213, 680, 227] + ZOrder -23 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "all" + SID "52:179" + Position [535, 288, 565, 302] + ZOrder -24 + Port "5" + IconDisplay "Port number" + } + Line { + SrcBlock "Gain1" + SrcPort 1 + DstBlock "w4" + DstPort 1 + } + Line { + SrcBlock "Gain2" + SrcPort 1 + DstBlock "w2" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "all" + DstPort 1 + } + Line { + SrcBlock "tau roll" + SrcPort 1 + Points [35, 0] + Branch { + Points [0, 90] + DstBlock "Sum7" + DstPort 1 + } + Branch { + DstBlock "Sum3" + DstPort 1 + } + } + Line { + SrcBlock "T" + SrcPort 1 + Points [250, 0] + Branch { + DstBlock "Sum9" + DstPort 2 + } + Branch { + Points [30, 0] + Branch { + DstBlock "Sum10" + DstPort 2 + } + Branch { + Points [35, 0] + Branch { + Points [-10, 0] + DstBlock "Sum11" + DstPort 2 + } + Branch { + Points [15, 0] + DstBlock "Sum12" + DstPort 2 + } + } + } + } + Line { + SrcBlock "Sum12" + SrcPort 1 + Points [10, 0] + Branch { + Points [0, 90] + DstBlock "Mux" + DstPort 4 + } + Branch { + DstBlock "Saturation4" + DstPort 1 + } + } + Line { + SrcBlock "Sum11" + SrcPort 1 + Points [45, 0] + Branch { + Points [0, 125] + DstBlock "Mux" + DstPort 3 + } + Branch { + DstBlock "Saturation3" + DstPort 1 + } + } + Line { + SrcBlock "Sum10" + SrcPort 1 + Points [85, 0] + Branch { + Points [0, 160] + DstBlock "Mux" + DstPort 2 + } + Branch { + DstBlock "Saturation1" + DstPort 1 + } + } + Line { + SrcBlock "Sum9" + SrcPort 1 + Points [130, 0] + Branch { + Points [0, 195] + DstBlock "Mux" + DstPort 1 + } + Branch { + DstBlock "Saturation2" + DstPort 1 + } + } + Line { + SrcBlock "Sum7" + SrcPort 1 + DstBlock "Sum12" + DstPort 1 + } + Line { + SrcBlock "Sum5" + SrcPort 1 + DstBlock "Sum11" + DstPort 1 + } + Line { + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Sum10" + DstPort 1 + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "Sum9" + DstPort 1 + } + Line { + SrcBlock "tau pitch" + SrcPort 1 + Points [50, 0] + Branch { + Points [0, 90] + DstBlock "Sum5" + DstPort 1 + } + Branch { + DstBlock "Sum1" + DstPort 1 + } + } + Line { + SrcBlock "Saturation4" + SrcPort 1 + DstBlock "Gain1" + DstPort 1 + } + Line { + SrcBlock "Saturation3" + SrcPort 1 + DstBlock "w3" + DstPort 1 + } + Line { + SrcBlock "Saturation1" + SrcPort 1 + DstBlock "Gain2" + DstPort 1 + } + Line { + SrcBlock "Saturation2" + SrcPort 1 + DstBlock "w1" + DstPort 1 + } + Line { + SrcBlock "tau yaw" + SrcPort 1 + Points [125, 0] + Branch { + Points [25, 0] + Branch { + Points [25, 0] + Branch { + Points [25, 0] + DstBlock "Sum1" + DstPort 2 + } + Branch { + DstBlock "Sum3" + DstPort 2 + } + } + Branch { + DstBlock "Sum5" + DstPort 2 + } + } + Branch { + DstBlock "Sum7" + DstPort 2 + } + } + } + } + Block { + BlockType SubSystem + Name "Quadrotor" + SID "53" + Ports [4, 1] + Position [230, 332, 290, 443] + ZOrder -3 + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 26 + $ClassName "Simulink.Mask" + Description "Quadrotor dynamics" + Help "Integrate spatial velocity to update pose represented as a homogeneous\ntransformation.\n \nParamet" + "ers::\n \nq (struct) A structure containing quadrotor dynamics\n\nThe elements of q are:\n\n J Flyer" + " rotational inertia matrix (3x3)\n h Height of rotors above CoG (1x1)\n d Length of flyer arms (1" + "x1)\n nb Number of blades per rotor (1x1)\n r Rotor radius (1x1)\n c Blade chord (1x1)\n e" + " Flapping hinge offset (1x1)\n Mb Rotor blade mass (1x1)\n Mc Estimated hub clamp mass (1x1" + ")\n ec Blade root clamp displacement (1x1)\n Ib Rotor blade rotational inertia (1x1)\n Ic Es" + "timated root clamp inertia (1x1)\n mb Static blade moment (1x1)\n Ir Total rotor inertia (1x1)\n Ct" + " Non-dim. thrust coefficient (1x1)\n Cq Non-dim. torque coefficient (1x1)\n sigma Rotor solidit" + "y ratio (1x1)\n thetat Blade tip angle (1x1)\n theta0 Blade root angle (1x1)\n theta1 Blade twist angle " + "(1x1)\n theta75 3/4 blade angle (1x1)\n thetai Blade ideal root approximation (1x1)\n a Lift slope g" + "radient (1x1)\n A Rotor disc area (1x1)\n gamma Lock number (1x1)\n\n \nInputs::\n \nw1 (1x1) " + "angular velocity of rotor 1 [rad]\nw2 (1x1) angular velocity of rotor 2 [rad]\nw3 (1x1) angular velocity of rotor" + " 3 [rad]\nw4 (1x1) angular velocity of rotor 4 [rad]\n\nOutputs::\n \nX (1x12) state vector (x, y, z, thr," + " thp, thy, xd, yd, zd, thrd, thpd, thyd)\n\n(x,y,z) is the position in the world frame [m]\n(thr, thp, thy) the r" + "oll/pitch/yaw orientation in the world frame [rad]\n(xd,yd,zd) is the velocity in the body frame [m]\n(thrd,thpd," + "thyd) is the roll/pitch/yaw rates in the body frame [rad/s]\n \nNotes::\n \n- See also mdl_quadrotor which" + " defines parameters for the ANU X4 flyer.\n" + Object { + $PropName "Parameters" + $ObjectID 27 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "quad" + Prompt "Quadrotor params" + Value "0" + } + } + System { + Name "Quadrotor" + Location [26, 172, 508, 726] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "234" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "w 1" + SID "53:181" + Position [25, 128, 55, 142] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w 2" + SID "53:182" + Position [25, 158, 55, 172] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w 3" + SID "53:183" + Position [25, 188, 55, 202] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "w 4" + SID "53:184" + Position [25, 218, 55, 232] + ZOrder -4 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType BusCreator + Name "Bus\nCreator" + SID "53:185" + Ports [12, 1] + Position [305, 17, 310, 338] + ZOrder -5 + ShowName off + Inputs "'X','Y','Z','yaw','pitch','roll','dx','dy','dz','dyaw','dpitch','droll'" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux" + SID "53:186" + Ports [1, 12] + Position [230, 37, 235, 318] + ZOrder -6 + BackgroundColor "black" + ShowName off + Outputs "12" + Port { + PortNumber 1 + Name "X" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 2 + Name "Y" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 3 + Name "Z" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 4 + Name "yaw" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 5 + Name "pitch" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 6 + Name "roll" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 7 + Name "dx" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 8 + Name "dy" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 9 + Name "dz" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 10 + Name "dyaw" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 11 + Name "dpitch" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 12 + Name "droll" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType S-Function + Name "Dynamics" + SID "53:187" + Ports [1, 1] + Position [110, 165, 195, 195] + ZOrder -7 + FunctionName "quadrotor_dynamics" + Parameters "quad" + EnableBusSupport off + } + Block { + BlockType Mux + Name "Mux" + SID "53:188" + Ports [4, 1] + Position [80, 119, 85, 241] + ZOrder -8 + ShowName off + DisplayOption "bar" + } + Block { + BlockType Outport + Name "X" + SID "53:189" + Position [345, 173, 375, 187] + ZOrder -9 + IconDisplay "Port number" + } + Line { + Name "droll" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 12 + DstBlock "Bus\nCreator" + DstPort 12 + } + Line { + Name "dpitch" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 11 + DstBlock "Bus\nCreator" + DstPort 11 + } + Line { + Name "dyaw" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 10 + DstBlock "Bus\nCreator" + DstPort 10 + } + Line { + SrcBlock "Dynamics" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "Bus\nCreator" + SrcPort 1 + DstBlock "X" + DstPort 1 + } + Line { + Name "dy" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 8 + DstBlock "Bus\nCreator" + DstPort 8 + } + Line { + Name "roll" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 6 + DstBlock "Bus\nCreator" + DstPort 6 + } + Line { + Name "Y" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 2 + DstBlock "Bus\nCreator" + DstPort 2 + } + Line { + Name "dz" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 9 + DstBlock "Bus\nCreator" + DstPort 9 + } + Line { + Name "dx" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 7 + DstBlock "Bus\nCreator" + DstPort 7 + } + Line { + SrcBlock "w 4" + SrcPort 1 + DstBlock "Mux" + DstPort 4 + } + Line { + SrcBlock "w 3" + SrcPort 1 + DstBlock "Mux" + DstPort 3 + } + Line { + SrcBlock "w 2" + SrcPort 1 + DstBlock "Mux" + DstPort 2 + } + Line { + SrcBlock "w 1" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + Name "pitch" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 5 + DstBlock "Bus\nCreator" + DstPort 5 + } + Line { + Name "yaw" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 4 + DstBlock "Bus\nCreator" + DstPort 4 + } + Line { + Name "Z" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 3 + DstBlock "Bus\nCreator" + DstPort 3 + } + Line { + Name "X" + Labels [0, 0] + SrcBlock "Demux" + SrcPort 1 + DstBlock "Bus\nCreator" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "Dynamics" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "RNE" + SID "54" + Ports [3, 1] + Position [230, 91, 290, 169] + ZOrder -4 + BackgroundColor "red" + LibraryVersion "*1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 28 + $ClassName "Simulink.Mask" + Description "Rigid-body inverse dynamics." + Help "Given a robot dynamic model and joint angle, rate and acceleration \ndetermine the required joint torq" + "ue. \n \nParameters::\n \nrobot (SerialLink) a dynamic model of an N-link serial link manipulator\ngravit" + "y (1x3) IGNORED Gravitational acceleration in world frame [m/s2]\n \nInputs::\n \nq (1xN) joint coordinat" + "es\nqd (1xN) joint coordinate velocity\nqdd (1xN) joint coordinate acceleration\n\n \nOutputs::\n \nQ (1xN" + ") the generalized joint force (torque or force) required to achieve\nthe motion state (q, qd, qdd).\n \nNotes:" + ":\n \n- Required for computed and inverse torque control schemes.\n- The gravity parameter is ignored.\n- Imp" + "lemented using the recursive Newton-Euler scheme (RNE).\n- A wrapper for the Toolbox method rne().\n\n" + Initialization "if isa(robot, 'SerialLink')\n n = robot.n\n rname = robot.name;\nelse\n n = 0;\n " + "rname = '';\nend" + Display "port_label('output', 1, 'Q')\nport_label('input', 1, 'q')\nport_label('input', 2, 'qd')\nport_label" + "('input', 3, 'qdd')\ntext(0.9, 0.9, rname, 'horizontalAlignment', 'right')\n" + IconUnits "normalized" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 29 + Type "edit" + Name "robot" + Prompt "Robot object" + Value "robot" + } + Object { + $ObjectID 30 + Type "edit" + Name "grav" + Prompt "Gravity" + Value "[0 0 9.81]" + } + PropName "Parameters" + } + } + System { + Name "RNE" + Location [1276, 176, 1808, 505] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "245" + SIDPrevWatermark "245" + Block { + BlockType Inport + Name "q" + SID "54:240" + Position [20, 33, 50, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "qd" + SID "54:241" + Position [20, 73, 50, 87] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "qdd" + SID "54:242" + Position [15, 113, 45, 127] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "54:243" + Ports [1, 1] + Position [120, 66, 295, 94] + ZOrder -4 + MATLABFcn "rne(robot, u(1:n)', u(n+1:2*n)', u(2*n+1:3*n)')" + OutputDimensions "robot.n" + } + Block { + BlockType Mux + Name "Mux" + SID "54:244" + Ports [3, 1] + Position [85, 21, 90, 139] + ZOrder -5 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Outport + Name "Q" + SID "54:245" + Position [370, 73, 400, 87] + ZOrder -6 + IconDisplay "Port number" + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "q" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "qd" + SrcPort 1 + DstBlock "Mux" + DstPort 2 + } + Line { + SrcBlock "qdd" + SrcPort 1 + DstBlock "Mux" + DstPort 3 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "Q" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Robot" + SID "55" + Ports [1, 3] + Position [105, 92, 180, 168] + ZOrder -5 + BackgroundColor "red" + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 31 + $ClassName "Simulink.Mask" + Description "Rigid-body dynamic model of robot." + Help "Simulates the forward (direct) rigid-body dynamics of the robot described \nby the given robot object." + " For given applied joint torque/force it computes\nthe instaneous joint coordinates, velocities and acclerations" + ".\n\n \nParameters::\n \nrobot (SerialLink) a dynamic model of an N-link serial link manipulator\nq0 (1xN)" + " Initial joint coordinates\n \nInputs::\n \nQ (1xN) the generalized joint force (torque or force) require" + "d to achieve\nthe motion state (q, qd, qdd).\n \nOutputs::\n\nq (1xN) joint coordinates\nqd (1xN) joint co" + "ordinate velocity\nqdd (1xN) joint coordinate acceleration\n\n \nNotes::\n \n- Required to simulate robot " + "rigid body motion.\n- Implemented using the recursive Newton-Euler scheme (RNE).\n- A wrapper for the Toolbox met" + "hod accel() which in turn depends on the\n method rne().\n\n" + Initialization "if isa(robot, 'SerialLink')\n rname = robot.name;\n %fprintf('**robot name is %s\\n', r" + "name);\nelse\n rname = '';\n n = 0\nend\n" + Display "port_label('input', 1, 'Q')\nport_label('output', 1, 'q')\nport_label('output', 2, 'qd')\nport_labe" + "l('output', 3, 'qdd')\ntext(0.1, 0.9, rname, 'horizontalAlignment', 'left')\n" + IconUnits "normalized" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 32 + Type "edit" + Name "robot" + Prompt "Robot object" + Value "robot" + } + Object { + $ObjectID 33 + Type "edit" + Name "q0" + Prompt "Initial state" + Value "[0 0 0 0 0 0]" + } + PropName "Parameters" + } + } + System { + Name "Robot" + Location [242, 300, 814, 706] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "248" + SIDPrevWatermark "232" + Block { + BlockType Inport + Name "Q" + SID "55:225" + Position [20, 98, 50, 112] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Integrator + Name "Integrator" + SID "55:246" + Ports [1, 1] + Position [265, 65, 295, 95] + ZOrder -2 + ShowName off + Port { + PortNumber 1 + Name "qd" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Integrator + Name "Integrator1" + SID "55:247" + Ports [1, 1] + Position [345, 65, 375, 95] + ZOrder -3 + ShowName off + InitialCondition "q0" + } + Block { + BlockType Mux + Name "Mux" + SID "55:248" + Ports [3, 1] + Position [105, 44, 115, 116] + ZOrder -4 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType S-Function + Name "S-Function" + SID "55:229" + Ports [1, 1] + Position [150, 65, 210, 95] + ZOrder -5 + FunctionName "slaccel" + Parameters "robot" + EnableBusSupport off + Port { + PortNumber 1 + Name "qdd" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Outport + Name "q" + SID "55:230" + Position [440, 73, 470, 87] + ZOrder -6 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qd" + SID "55:231" + Position [440, 128, 470, 142] + ZOrder -7 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qdd" + SID "55:232" + Position [440, 173, 470, 187] + ZOrder -8 + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "Integrator1" + SrcPort 1 + Points [25, 0] + Branch { + Points [0, -60; -325, 0; 0, 35] + DstBlock "Mux" + DstPort 1 + } + Branch { + DstBlock "q" + DstPort 1 + } + } + Line { + SrcBlock "Q" + SrcPort 1 + DstBlock "Mux" + DstPort 3 + } + Line { + Name "qd" + Labels [0, 0] + SrcBlock "Integrator" + SrcPort 1 + Points [30, 0] + Branch { + Points [0, -50; -285, 0; 0, 50] + DstBlock "Mux" + DstPort 2 + } + Branch { + DstBlock "Integrator1" + DstPort 1 + } + Branch { + Points [0, 55] + DstBlock "qd" + DstPort 1 + } + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + Line { + Name "qdd" + Labels [0, 0] + SrcBlock "S-Function" + SrcPort 1 + Points [35, 0] + Branch { + DstBlock "Integrator" + DstPort 1 + } + Branch { + Points [0, 100] + DstBlock "qdd" + DstPort 1 + } + } + } + } + Annotation { + SID "145" + Name "\nDynamics \n " + Position [47, 27] + BackgroundColor "cyan" + FontSize 18 + } + Annotation { + SID "146" + Name "Arm-type robot" + Position [55, 100] + ForegroundColor "orange" + } + Annotation { + SID "147" + Name "Quadrotor" + Position [41, 336] + ForegroundColor "orange" + } + Annotation { + SID "148" + Name "Mobile robot" + Position [49, 214] + ForegroundColor "orange" + } + Annotation { + SID "149" + Name "Robotics Toolbox for MATLAB" + Position [116, 17] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + } + } + Block { + BlockType SubSystem + Name "Kinematics" + SID "249" + Ports [] + Position [154, 128, 260, 170] + ZOrder -2 + BackgroundColor "darkGreen" + ShowName off + LibraryVersion "*1.3" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 34 + $ClassName "Simulink.Mask" + Display "color('darkBlue')\ndisp('Kinematics')" + IconUnits "pixels" + } + System { + Name "Kinematics" + Location [618, 490, 1181, 1106] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "654" + SIDPrevWatermark "14" + Block { + BlockType SubSystem + Name "fkine" + SID "249:22" + Ports [1, 1] + Position [80, 84, 145, 146] + ZOrder -1 + BackgroundColor "red" + LibraryVersion "*1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 35 + $ClassName "Simulink.Mask" + Description "Robot forward kinematics." + Help "Compute homogeneous transform for robot end-effector given joint coordinates\n\n\nParameters::\n" + " \nRobot (SerialLink class) The N-axis robot object for computing forward kinematics.\n \nInputs::\n \nq " + "(1xN) joint coordinates\n \nOutputs::\n \nT (4x4) the pose of the end effector as a homogeneous transforma" + "tion\n \nNotes::\n \n- A wrapper for the Toolbox method fkine().\n" + Display "port_label('input', 1, 'q')\nport_label('output', 1, 'T')\nif ~isempty(robot)\n text(0.5, 0.2, r" + "obot.name, 'horizontalAlignment', 'center')\nend\n" + IconUnits "normalized" + Object { + $PropName "Parameters" + $ObjectID 36 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "robot" + Prompt "Robot object" + Value "robot" + } + } + System { + Name "fkine" + Location [495, 403, 899, 730] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "q" + SID "249:22:69" + Position [55, 68, 85, 82] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "249:22:70" + Ports [1, 1] + Position [120, 60, 180, 90] + ZOrder -2 + MATLABFcn "fkine(robot,u)" + OutputDimensions "[4 4]" + Output1D off + } + Block { + BlockType Outport + Name "T" + SID "249:22:71" + Position [225, 68, 255, 82] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "T" + DstPort 1 + } + Line { + SrcBlock "q" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "ijacob" + SID "249:23" + Ports [1, 1] + Position [285, 185, 340, 255] + ZOrder -2 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 37 + $ClassName "Simulink.Mask" + Description "Inverse Jacobian\n" + Help "Inverse of Jacobian matrix which maps spatial velocity to joint coordinate\nrates.\n\n\nInputs::\n \nJ (6x6) the Jacobian matrix\n \nOutputs::\n \nJi (6x6) the inverse Jacobian matrix\n \nNotes::\n \n- A wrapper for the MATLAB function inv().\n- Only valid for square Jacobians (6-axis robots)\n" + Display "text(0.4, 0.7, '\\bf\\fontsize{16}J', 'texmode', 'on', 'verticalAlignment', 'top', 'horizontalAlign" + "ment', 'left')\ntext(0.6, 0.65, '\\fontsize{10}-1', 'texmode', 'on','verticalAlignment', 'bottom', 'horizontalAli" + "gnment', 'left')\nport_label('input', 1, 'J');\nport_label('output', 1, 'Ji')\n" + IconUnits "normalized" + } + System { + Name "ijacob" + Location [198, 365, 602, 692] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "J" + SID "249:23:73" + Position [20, 33, 50, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "inverse\njacobian" + SID "249:23:74" + Ports [1, 1] + Position [110, 25, 170, 55] + ZOrder -2 + MATLABFcn "inv(u)" + Output1D off + } + Block { + BlockType Outport + Name "Ji" + SID "249:23:75" + Position [220, 33, 250, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "J" + SrcPort 1 + DstBlock "inverse\njacobian" + DstPort 1 + } + Line { + SrcBlock "inverse\njacobian" + SrcPort 1 + DstBlock "Ji" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "ikine" + SID "249:250" + Ports [1, 1] + Position [290, 83, 340, 147] + ZOrder 3 + BackgroundColor "red" + LibraryVersion "1.56" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 38 + $ClassName "Simulink.Mask" + Description "Generalized numerical inverse kinematics" + Help "Compute joint coordinates for robot given end-effector pose.\n\n\nParameters::\n \nRobot (Seria" + "lLink class) The N-axis robot object for computing forward kinematics.\nInitial q (1xN) Initial joint " + "coordinates for solution\nMask vector (1x6) Mask vector for underactuated manipulators.\nPseudo inverse (" + "logical) Enable option 'pinv'\nOptions Option value list to be passed to ikine()\n\nInputs:" + ":\n \nT (4x4) the pose of the end effector as a homogeneous transformation\n\n \nOutputs::\n\nq (1xN) " + "joint coordinates\n \nNotes::\n \n- A wrapper for the Toolbox method ikine().\n" + Initialization "opt = options;\nif pinv\n opt = [opt 'pinv'];\nend\ndisp(opt)\n" + Array { + Type "Simulink.MaskParameter" + Dimension 5 + Object { + $ObjectID 39 + Type "edit" + Name "robot" + Prompt "Robot object" + Value "p560" + } + Object { + $ObjectID 40 + Type "edit" + Name "q0" + Prompt "Initial q" + Value "qz" + } + Object { + $ObjectID 41 + Type "edit" + Name "M" + Prompt "Mask vector" + Value "[1 1 1 1 1 1]" + } + Object { + $ObjectID 42 + Type "checkbox" + Name "pinv" + Prompt "Pseudo-inverse (pinv)" + Value "off" + Callback "\n" + } + Object { + $ObjectID 43 + Type "edit" + Name "options" + Prompt "Options (cell array)" + Value "{}" + Tunable "off" + } + PropName "Parameters" + } + } + System { + Name "ikine" + Location [885, 595, 1448, 1211] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "T" + SID "249:72" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "ikine" + SID "249:251" + Ports [1, 1] + Position [80, 25, 150, 55] + ZOrder -2 + MATLABFcn "robot.ikine(u, q0, M, opt{:})" + } + Block { + BlockType Outport + Name "q" + SID "249:252" + Position [175, 33, 205, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "T" + SrcPort 1 + DstBlock "ikine" + DstPort 1 + } + Line { + SrcBlock "ikine" + SrcPort 1 + DstBlock "q" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "ikine6s" + SID "249:66" + Ports [1, 1] + Position [190, 84, 240, 146] + ZOrder -4 + BackgroundColor "red" + LibraryVersion "1.51" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 44 + $ClassName "Simulink.Mask" + Description "Inverse kinematics for 6-axis spherical robot" + Help "Compute joint coordinates for robot given end-effector pose.\n\n\nParameters::\n \nRobot (Seria" + "lLink class) The N-axis robot object for computing forward kinematics.\nConfiguration Configuration " + "string comprised of 'lrudnf'\n \nInputs::\n \nT (4x4) the pose of the end effector as a homogeneous transf" + "ormation\n\n \nOutputs::\n\nq (1xN) joint coordinates\n \nNotes::\n \n- A wrapper for the Toolbox m" + "ethod ikine6s().\n" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 45 + Type "edit" + Name "robot" + Prompt "Robot object" + Value "p560" + } + Object { + $ObjectID 46 + Type "edit" + Name "conf" + Prompt "Configuration string" + Value "'luf'" + } + PropName "Parameters" + } + } + System { + Name "ikine6s" + Location [1429, 718, 1699, 978] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "T" + SID "249:67" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "ikine6s" + SID "249:65" + Ports [1, 1] + Position [80, 25, 150, 55] + ZOrder -2 + MATLABFcn "robot.ikine6s(u,conf)" + } + Block { + BlockType Outport + Name "q" + SID "249:68" + Position [175, 33, 205, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "T" + SrcPort 1 + DstBlock "ikine6s" + DstPort 1 + } + Line { + SrcBlock "ikine6s" + SrcPort 1 + DstBlock "q" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "jacob0\n" + SID "249:24" + Ports [1, 1] + Position [80, 186, 140, 254] + ZOrder -5 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 47 + $ClassName "Simulink.Mask" + Description "Robot Jacobian in world coordinates" + Help "Jacobian matrix relating joint coordinate rates to end-effector spatial\nvelocity expressed in the wor" + "ld frame.\n\nParameters::\n \nRobot (SerialLink class) The N-axis robot object for which the Jacobian is\n" + " computed.\n \nInputs::\n \nq (1xN) joint coordinates\n \nOutputs::\n \nJ " + "(6xN) the Jacobian matrix\n \nNotes::\n \n- A wrapper for the Toolbox method jacob0().\n" + Initialization "n=robot.n;\n" + Display "text(0.4, 0.7, '\\bf\\fontsize{16}J', 'texmode', 'on', 'verticalAlignment', 'top', 'horizontalAlign" + "ment', 'left')\ntext(0.45, 0.65, '\\fontsize{10}0', 'texmode', 'on','verticalAlignment', 'bottom', 'horizontalAli" + "gnment', 'right')\nif ~isempty(robot)\n text(0.5, 0.2, robot.name, 'horizontalAlignment', 'center')\nend\npor" + "t_label('input', 1, 'q')\nport_label('output', 1, 'J')" + IconUnits "normalized" + Object { + $PropName "Parameters" + $ObjectID 48 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "robot" + Prompt "Robot object" + Value "robot" + } + } + System { + Name "jacob0\n" + Location [542, 430, 946, 757] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "q" + SID "249:24:77" + Position [20, 33, 50, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "249:24:78" + Ports [1, 1] + Position [100, 25, 160, 55] + ZOrder -2 + MATLABFcn "jacob0(robot,u)" + OutputDimensions "[6 n]" + Output1D off + } + Block { + BlockType Outport + Name "J" + SID "249:24:79" + Position [220, 33, 250, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "q" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "J" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "jacobn" + SID "249:25" + Ports [1, 1] + Position [180, 186, 235, 254] + ZOrder -6 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 49 + $ClassName "Simulink.Mask" + Description "Robot Jacobian in end-effector coordinates" + Help "Jacobian matrix relating joint coordinate rates to end-effector spatial\nvelocity expressed in the end" + "-effector frame.\n\nParameters::\n \nRobot (SerialLink class) The N-axis robot object for which the Jacob" + "ian is\n computed.\n \nInputs::\n \nq (1xN) joint coordinates\n \nOutputs::\n \nJ (6xN) the Jacobian matrix\n \nNotes::\n \n- A wrapper for the Toolbox method jacobn().\n" + Initialization "n=robot.n;" + Display "text(0.4, 0.7, '\\bf\\fontsize{16}J', 'texmode', 'on', 'verticalAlignment', 'top', 'horizontalAlign" + "ment', 'left')\ntext(0.45, 0.65, '\\fontsize{10}N', 'texmode', 'on','verticalAlignment', 'bottom', 'horizontalAli" + "gnment', 'right')\nif ~isempty(robot)\n text(0.5, 0.2, robot.name, 'horizontalAlignment', 'center')\nend\npor" + "t_label('input', 1, 'q')\nport_label('output', 1, 'J')" + IconUnits "normalized" + Object { + $PropName "Parameters" + $ObjectID 50 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "robot" + Prompt "Robot object" + Value "robot" + } + } + System { + Name "jacobn" + Location [129, 393, 533, 720] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "q" + SID "249:25:81" + Position [20, 33, 50, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "249:25:82" + Ports [1, 1] + Position [100, 25, 160, 55] + ZOrder -2 + MATLABFcn "jacobn(robot,u)" + OutputDimensions "[6 n]" + Output1D off + } + Block { + BlockType Outport + Name "J" + SID "249:25:83" + Position [220, 33, 250, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "q" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "J" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "pose integral" + SID "249:58" + Ports [1, 1] + Position [180, 293, 250, 347] + ZOrder -16 + BackgroundColor "red" + DropShadow on + LibraryVersion "1.55" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 51 + $ClassName "Simulink.Mask" + Description "Integrate spatial velocity" + Help "Integrate spatial velocity to update pose represented as a homogeneous\ntransformation.\n\nParamete" + "rs::\n\nT0 (4x4) The initial pose\ndt (1x1) The sample interval [s]\n\nInputs::\n\nv (1x6) spatial v" + "elocity (xd yd zd wx wy wz)\n\nOutputs::\n\nT (4x4) the integrated pose as a homogeneous transformation\n\n" + "Notes::\n\n- Contains a unit delay of dt\n- The transform is kept normalized using trnorm().\n" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 52 + Type "edit" + Name "T0" + Prompt "Initial pose (hom transform)" + Value "transl(0,0,1)*trotz(1.5)" + } + Object { + $ObjectID 53 + Type "edit" + Name "dt" + Prompt "Sample interval (s)" + Value "0.02" + } + PropName "Parameters" + } + } + System { + Name "pose integral" + Location [877, 322, 1440, 938] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "v" + SID "249:59" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn3" + SID "249:60" + Ports [1, 1] + Position [80, 25, 140, 55] + ZOrder -2 + MATLABFcn "delta2tr(u*dt)" + Output1D off + } + Block { + BlockType Product + Name "Product" + SID "249:61" + Ports [2, 1] + Position [195, 29, 225, 76] + ZOrder -3 + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType UnitDelay + Name "Unit Delay" + SID "249:62" + Position [345, 33, 380, 67] + ZOrder -4 + InitialCondition "T0" + SampleTime "dt" + } + Block { + BlockType MATLABFcn + Name "trnorm" + SID "249:63" + Ports [1, 1] + Position [250, 35, 310, 65] + ZOrder -5 + MATLABFcn "trnorm(u)" + Output1D off + } + Block { + BlockType Outport + Name "T" + SID "249:64" + Position [435, 43, 465, 57] + ZOrder -6 + IconDisplay "Port number" + } + Line { + SrcBlock "v" + SrcPort 1 + DstBlock "MATLAB Fcn3" + DstPort 1 + } + Line { + SrcBlock "trnorm" + SrcPort 1 + DstBlock "Unit Delay" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn3" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + SrcBlock "Unit Delay" + SrcPort 1 + Points [30, 0] + Branch { + DstBlock "T" + DstPort 1 + } + Branch { + Points [0, 50; -245, 0; 0, -35] + DstBlock "Product" + DstPort 2 + } + } + Line { + SrcBlock "Product" + SrcPort 1 + Points [0, -5] + DstBlock "trnorm" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "tr2diff" + SID "249:26" + Ports [2, 1] + Position [80, 290, 125, 345] + ZOrder -8 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 54 + $ClassName "Simulink.Mask" + Description "Compute differential motion " + Help "Compute the differential motion (as a 6-vector) between two poses given as\nhomogeneous transformation" + "s.\n \nInputs::\n \nT1 (4x4) initial pose as a homogeneous transformation \nT2 (4x4) final pose as a homog" + "eneous transformation \n \nOutputs::\n \ndx (1x6) the differential motion (dx dy dz dwx dwy dwz)\n \nNo" + "tes::\n\n- Only valid for infinitessimal displacements.\n- dx is the integral of a spatial velocity over time" + ".\n- A wrapper for the Toolbox function tr2delta()." + Display "port_label('input', 1, 'T1')\nport_label('input', 2, 'T2')\nport_label('output', 1, 'dx')\n\n" + IconUnits "normalized" + Object { + $PropName "Parameters" + $ObjectID 55 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "robot" + Prompt "Robot object" + Value "robot" + } + } + System { + Name "tr2diff" + Location [160, 386, 564, 713] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "T1" + SID "249:26:111" + Position [45, 38, 75, 52] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "T2" + SID "249:26:112" + Position [45, 93, 75, 107] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "249:26:113" + Ports [1, 1] + Position [215, 40, 275, 70] + ZOrder -3 + MATLABFcn "tr2delta(reshape(u(1:16),4,4), reshape(u(17:32),4,4))" + OutputDimensions "6" + } + Block { + BlockType Mux + Name "Mux" + SID "249:26:114" + Ports [2, 1] + Position [185, 36, 190, 74] + ZOrder -4 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Reshape + Name "Reshape" + SID "249:26:115" + Ports [1, 1] + Position [115, 33, 145, 57] + ZOrder -5 + } + Block { + BlockType Reshape + Name "Reshape1" + SID "249:26:116" + Ports [1, 1] + Position [125, 88, 155, 112] + ZOrder -6 + } + Block { + BlockType Outport + Name "dx" + SID "249:26:117" + Position [300, 48, 330, 62] + ZOrder -7 + IconDisplay "Port number" + } + Line { + SrcBlock "Reshape1" + SrcPort 1 + Points [10, 0] + DstBlock "Mux" + DstPort 2 + } + Line { + SrcBlock "Reshape" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "T2" + SrcPort 1 + DstBlock "Reshape1" + DstPort 1 + } + Line { + SrcBlock "T1" + SrcPort 1 + DstBlock "Reshape" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "dx" + DstPort 1 + } + } + } + Annotation { + SID "249:256" + Name "\nKinematics \n " + Position [52, 22] + BackgroundColor "cyan" + FontSize 18 + } + Annotation { + SID "249:255" + Name "Robotics Toolbox for MATLAB" + Position [116, 17] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + Annotation { + SID "249:76" + Name "Forward\nkinematics" + Position [8, 96] + HorizontalAlignment "left" + ForegroundColor "orange" + } + Annotation { + SID "249:254" + Name "Jacobian" + Position [31, 191] + ForegroundColor "orange" + } + Annotation { + SID "249:253" + Name "Spatial\nvelocity" + Position [14, 306] + HorizontalAlignment "left" + ForegroundColor "orange" + } + } + } + Block { + BlockType SubSystem + Name "Robot Graphics" + SID "16" + Ports [] + Position [15, 60, 122, 103] + ZOrder -3 + BackgroundColor "darkGreen" + ShowName off + LibraryVersion "*1.3" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 56 + $ClassName "Simulink.Mask" + Display "color('darkBlue')\ndisp('Robot Graphics')" + IconUnits "pixels" + } + System { + Name "Robot Graphics" + Location [25, 44, 441, 456] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "193" + SIDPrevWatermark "16" + Block { + BlockType SubSystem + Name "Quadrotor plot" + SID "16:18" + Ports [1] + Position [195, 102, 225, 148] + ZOrder -1 + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 57 + $ClassName "Simulink.Mask" + Description "Animate quadrotor." + Help "This block graphically displays a quadrotor (flying robot).\n\nA new figure is created and A graphical" + " model of a quadrotor is animated \nto a pose specified by the state vector input to the block. \n\nThe state ve" + "ctor is [x y z roll pitch yaw] in world coordinates and the\nangles are in radians.\n\nNotes:\n- The dimensions o" + "f the model are currently fixed in the code and cannot\n be set via a parameter." + Object { + $PropName "Parameters" + $ObjectID 58 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "x" + Prompt "State vector" + Value "0" + } + } + System { + Name "Quadrotor plot" + Location [502, 208, 901, 651] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "233" + SIDPrevWatermark "230" + Block { + BlockType Inport + Name "X" + SID "16:18:191" + Position [50, 113, 80, 127] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType BusSelector + Name "Bus\nSelector1" + SID "16:18:192" + Ports [1, 1] + Position [110, 101, 115, 139] + ZOrder -2 + ShowName off + OutputSignals "X,Y,Z,yaw,pitch,roll" + OutputAsBus on + } + Block { + BlockType S-Function + Name "Plotter" + SID "16:18:193" + Ports [1] + Position [155, 105, 215, 135] + ZOrder -3 + FunctionName "quadrotor_plot" + Parameters "[2,10], 1, 1" + EnableBusSupport off + } + Line { + SrcBlock "Bus\nSelector1" + SrcPort 1 + DstBlock "Plotter" + DstPort 1 + } + Line { + SrcBlock "X" + SrcPort 1 + DstBlock "Bus\nSelector1" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "plot" + SID "16:17" + Ports [1] + Position [55, 97, 135, 153] + ZOrder -2 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 59 + $ClassName "Simulink.Mask" + Description "Graphical robot display." + Help "This block graphically displays the passed SerialLink class object.\n\nA new figure is created and the" + " robot is animated (using SerialLink.plot)\nto a pose specified by the joint coordinate vector input to the block" + " whose\nwidth must match the number of robot joints.\n\nIf the Hold checkbox is ticked then a \"hold\" command is" + " issued at initialization\ntime allowing multiple robots to be displayed in the one plot." + SelfModifiable "on" + Display "port_label('input', 1, 'q')\nif ~isempty(robot)\n text(0.5, 0.2, robot.name, 'horizontalAlignme" + "nt', 'center')\nend\n" + IconUnits "normalized" + Array { + Type "Simulink.MaskParameter" + Dimension 3 + Object { + $ObjectID 60 + Type "edit" + Name "robot" + Prompt "Robot object" + Value "robot" + Tunable "off" + Callback "\n" + } + Object { + $ObjectID 61 + Type "edit" + Name "fps" + Prompt "Update rate (fps)" + Value "25" + } + Object { + $ObjectID 62 + Type "checkbox" + Name "holdplot" + Prompt "Hold" + Value "on" + } + PropName "Parameters" + } + } + System { + Name "plot" + Location [795, 127, 1100, 387] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "230" + SIDPrevWatermark "230" + Block { + BlockType Inport + Name "q" + SID "16:17:94" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "16:17:95" + Ports [1] + Position [120, 25, 180, 55] + ZOrder -2 + FunctionName "slplotbot" + Parameters "robot,fps,holdplot" + EnableBusSupport off + } + Line { + SrcBlock "q" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + } + } + Annotation { + SID "16:19" + Name "\nRobot\nGraphics \n " + Position [1, 32] + HorizontalAlignment "left" + BackgroundColor "cyan" + FontSize 18 + } + Annotation { + SID "16:20" + Name "Robotics Toolbox for MATLAB" + Position [96, 17] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + } + } + Block { + BlockType SubSystem + Name "Trajectory" + SID "257" + Ports [] + Position [153, 60, 260, 102] + ZOrder -4 + BackgroundColor "darkGreen" + ShowName off + LibraryVersion "1.225" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 63 + $ClassName "Simulink.Mask" + Display "color('darkBlue')\ndisp('Trajectory')" + } + System { + Name "Trajectory" + Location [1070, 44, 1487, 522] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType SubSystem + Name "Circle" + SID "56" + Ports [0, 1] + Position [220, 85, 260, 145] + ZOrder -1 + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 64 + $ClassName "Simulink.Mask" + Description "Generate circular trajectory" + Help "Generate circular trajectory in the xy-plane and centred at the origin.\n\nParameters::\n\nradi" + "us The radius of the circle [m]\nfreq The frequency of motion [rev/s]\n\nOutput::\n\nxy (1x2) a vect" + "or of xy-coordinates at the current simulation time step.\n\nNotes::\n- Motion is at constant velocity aro" + "und the circle, there is an infinite\n initial acceleration.\n- The motion continues for the whole simulation." + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 65 + Type "edit" + Name "radius" + Prompt "Radius" + Value "1" + } + Object { + $ObjectID 66 + Type "edit" + Name "freq" + Prompt "Frequency (rev/s)" + Value "0.2" + } + PropName "Parameters" + } + } + System { + Name "Circle" + Location [8, 129, 540, 556] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "264" + SIDPrevWatermark "225" + Block { + BlockType Clock + Name "Clock" + SID "56:258" + Position [25, 35, 45, 55] + ZOrder -1 + } + Block { + BlockType Mux + Name "Mux" + SID "56:259" + Ports [2, 1] + Position [230, 29, 240, 91] + ZOrder -2 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Product + Name "Product1" + SID "56:260" + Ports [2, 1] + Position [280, 52, 310, 83] + ZOrder -3 + ShowName off + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Trigonometry + Name "Trigonometric\nFunction1" + SID "56:261" + Ports [1, 1] + Position [155, 30, 185, 60] + ZOrder -4 + ShowName off + Operator "cos" + } + Block { + BlockType Trigonometry + Name "Trigonometric\nFunction3" + SID "56:21" + Ports [1, 1] + Position [155, 90, 185, 120] + ZOrder -5 + ShowName off + } + Block { + BlockType Gain + Name "freq" + SID "56:262" + Position [70, 30, 95, 60] + ZOrder -6 + Gain "freq*2*pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "radius" + SID "56:263" + Position [185, 185, 215, 215] + ZOrder -7 + Value "radius" + } + Block { + BlockType Outport + Name "xy" + SID "56:264" + Position [335, 63, 365, 77] + ZOrder -8 + IconDisplay "Port number" + } + Line { + SrcBlock "Trigonometric\nFunction3" + SrcPort 1 + Points [25, 0] + DstBlock "Mux" + DstPort 2 + } + Line { + SrcBlock "Trigonometric\nFunction1" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "Product1" + DstPort 1 + } + Line { + SrcBlock "radius" + SrcPort 1 + Points [45, 0] + DstBlock "Product1" + DstPort 2 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "freq" + DstPort 1 + } + Line { + SrcBlock "Product1" + SrcPort 1 + DstBlock "xy" + DstPort 1 + } + Line { + SrcBlock "freq" + SrcPort 1 + Points [25, 0] + Branch { + DstBlock "Trigonometric\nFunction1" + DstPort 1 + } + Branch { + Points [0, 60] + DstBlock "Trigonometric\nFunction3" + DstPort 1 + } + } + } + } + Block { + BlockType SubSystem + Name "angdiff" + SID "57" + Ports [2, 1] + Position [55, 222, 75, 273] + ZOrder -2 + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 67 + $ClassName "Simulink.Mask" + Description "Difference of angles" + Help "A difference block for angles and the result is guaranteed to be in the\ninterval [-pi +pi).\n\nNot" + "es::\n- Is a wrapper for the Toolbox function angdiff." + } + System { + Name "angdiff" + Location [57, 112, 447, 485] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "269" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "+" + SID "57:265" + Position [35, 73, 65, 87] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "-" + SID "57:266" + Position [35, 113, 65, 127] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "57:267" + Ports [1, 1] + Position [175, 75, 235, 105] + ZOrder -3 + MATLABFcn "angdiff(u(1), u(2))" + } + Block { + BlockType Mux + Name "Mux" + SID "57:268" + Ports [2, 1] + Position [125, 71, 130, 109] + ZOrder -4 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Outport + Name "d" + SID "57:269" + Position [275, 83, 305, 97] + ZOrder -5 + IconDisplay "Port number" + } + Line { + SrcBlock "-" + SrcPort 1 + Points [40, 0] + DstBlock "Mux" + DstPort 2 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "d" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "+" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "jtraj" + SID "270" + Ports [0, 3] + Position [45, 84, 80, 156] + ZOrder -3 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 68 + $ClassName "Simulink.Mask" + Description "Joint interpolated trajectory." + Help "Generate quintic polynomial to move from initial to final joint angles as specified. Initial and fina" + "l velocity are assumed to be zero.\n\nParameters::\n\nq0 (1xN) initial joint coordinates\nqf (1xN) final j" + "oint coordinates\ntmax (1x1) the time for the trajectory to complete\n\nInitial and final velocity are assumed to" + " be zero.\n\nOutputs::\n\nq (1xN) vector of joint coordinates at current simulation time\nqd (1xN) vector " + "of joint coordinate velocity at current simulation time\nqdd (1xN) vector of joint coordinate acceleration at cur" + "rent simulation time\n\nNotes::\n\n- If simulation time exceeds tmax then q is clipped to qf.\n- A wrapper" + " on the Toolbox function jtraj.\n- The piecewise constant output of jtraj is interpolated to the current\n simul" + "ation time.\n\n" + Initialization "n = length(q0);\nif length(q0) ~= length(qf) ,\n error('q0 and qf must be same length')\nend\n" + "t=[0:100]'/100*tmax;\n[q,qd,qdd] = jtraj(q0, qf, t);" + Display "port_label('output', 1, 'q')\nport_label('output', 2, 'qd')\nport_label('output', 3, 'qdd')" + IconUnits "normalized" + Array { + Type "Simulink.MaskParameter" + Dimension 4 + Object { + $ObjectID 69 + Type "edit" + Name "q0" + Prompt "q0" + Value "[0 0 0 0 0 0]" + } + Object { + $ObjectID 70 + Type "edit" + Name "qf" + Prompt "qf" + Value "[pi/4 pi/2 -pi/2 0 0 0]" + } + Object { + $ObjectID 71 + Type "edit" + Name "tmax" + Prompt "tmax (s)" + Value "10" + } + Object { + $ObjectID 72 + Type "edit" + Name "ts" + Prompt "ts (s)" + Value "0.02" + } + PropName "Parameters" + } + } + System { + Name "jtraj" + Location [779, 370, 1183, 739] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Clock + Name "Clock" + SID "270:85" + Position [20, 30, 40, 50] + ZOrder -1 + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "270:86" + Ports [1, 1] + Position [105, 25, 165, 55] + ZOrder -2 + MATLABFcn "interp1(t,q,u)" + OutputDimensions "n" + SampleTime "ts" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn1" + SID "270:87" + Ports [1, 1] + Position [110, 75, 170, 105] + ZOrder -3 + MATLABFcn "interp1(t,qd,u)" + OutputDimensions "n" + SampleTime "ts" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn2" + SID "270:88" + Ports [1, 1] + Position [110, 125, 170, 155] + ZOrder -4 + MATLABFcn "interp1(t,qdd,u)" + OutputDimensions "n" + SampleTime "ts" + } + Block { + BlockType Outport + Name "q" + SID "270:89" + Position [220, 33, 250, 47] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qd" + SID "270:90" + Position [220, 83, 250, 97] + ZOrder -6 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qdd" + SID "270:91" + Position [220, 133, 250, 147] + ZOrder -7 + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "MATLAB Fcn2" + SrcPort 1 + DstBlock "qdd" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + Points [30, 0] + Branch { + Points [0, 50] + Branch { + Points [0, 50] + DstBlock "MATLAB Fcn2" + DstPort 1 + } + Branch { + DstBlock "MATLAB Fcn1" + DstPort 1 + } + } + Branch { + DstBlock "MATLAB Fcn" + DstPort 1 + } + } + Line { + SrcBlock "MATLAB Fcn1" + SrcPort 1 + DstBlock "qd" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "q" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "lspb" + SID "271" + Ports [0, 3] + Position [130, 84, 165, 156] + ZOrder -4 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 73 + $ClassName "Simulink.Mask" + Description "Scalar trajectory." + Help "Generate linear segment with polynomial blend to move from initial to final\n joint angles as specifie" + "d.\n\nParameters::\n\nq0 (1x1) initial joint coordinate\nqf (1x1) final joint coordinate\ntmax (1x1) the t" + "ime for the trajectory to complete\n\nInitial and final velocity are assumed to be zero.\n\nOutputs::\n\nq" + " (1x1) joint coordinates at current simulation time\nqd (1x1) joint coordinate velocity at current simulation tim" + "e\nqdd (1xN) joint coordinate acceleration at current simulation time\n\nNotes::\n\n- If simulation time e" + "xceeds tmax then q is clipped to qf.\n- A wrapper on the Toolbox function lspb.\n- The piecewise constant output " + "of jtraj is interpolated to the current simulation time." + Initialization "n = length(q0);\nif length(q0) ~= length(qf) ,\n error('q0 and qf must be same length')\nend\n" + "t=[0:1000]'/1000*tmax;\n[q,qd,qdd] = lspb(q0, qf, t);" + Array { + Type "Simulink.MaskParameter" + Dimension 3 + Object { + $ObjectID 74 + Type "edit" + Name "q0" + Prompt "q0" + Value "0" + } + Object { + $ObjectID 75 + Type "edit" + Name "qf" + Prompt "qf" + Value "0" + } + Object { + $ObjectID 76 + Type "edit" + Name "tmax" + Prompt "tmax" + Value "0" + } + PropName "Parameters" + } + } + System { + Name "lspb" + Location [567, 540, 831, 863] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "80" + SIDHighWatermark "595" + SIDPrevWatermark "595" + Block { + BlockType Clock + Name "Clock" + SID "271:585" + Position [25, 30, 45, 50] + ZOrder -1 + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "271:586" + Ports [1, 1] + Position [110, 25, 170, 55] + ZOrder -2 + MATLABFcn "interp1(t,q,u)" + OutputDimensions "n" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn1" + SID "271:587" + Ports [1, 1] + Position [115, 75, 175, 105] + ZOrder -3 + MATLABFcn "interp1(t,qd,u)" + OutputDimensions "n" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn2" + SID "271:588" + Ports [1, 1] + Position [115, 125, 175, 155] + ZOrder -4 + MATLABFcn "interp1(t,qdd,u)" + OutputDimensions "n" + } + Block { + BlockType Outport + Name "q" + SID "271:593" + Position [225, 33, 255, 47] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qd" + SID "271:594" + Position [225, 83, 255, 97] + ZOrder -6 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qdd" + SID "271:595" + Position [225, 133, 255, 147] + ZOrder -7 + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "q" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn1" + SrcPort 1 + DstBlock "qd" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + Points [30, 0] + Branch { + DstBlock "MATLAB Fcn" + DstPort 1 + } + Branch { + Points [0, 50] + Branch { + DstBlock "MATLAB Fcn1" + DstPort 1 + } + Branch { + Points [0, 50] + DstBlock "MATLAB Fcn2" + DstPort 1 + } + } + } + Line { + SrcBlock "MATLAB Fcn2" + SrcPort 1 + DstBlock "qdd" + DstPort 1 + } + } + } + Annotation { + SID "150" + Name "\nTrajectory \n " + Position [52, 22] + BackgroundColor "cyan" + FontSize 18 + } + Annotation { + SID "151" + Name "Robotics Toolbox for MATLAB" + Position [111, 12] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + } + } + Block { + BlockType SubSystem + Name "Transform" + SID "35" + Ports [] + Position [15, 199, 123, 242] + ZOrder -5 + BackgroundColor "darkGreen" + ShowName off + LibraryVersion "1.225" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 77 + $ClassName "Simulink.Mask" + Display "color('darkBlue')\ndisp('Transform')" + } + System { + Name "Transform" + Location [1828, 909, 2246, 1363] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType SubSystem + Name "Inverse" + SID "596" + Ports [1, 1] + Position [230, 90, 270, 150] + ZOrder -1 + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 78 + $ClassName "Simulink.Mask" + Description "Inverse of homogeneous transformation" + Help "Inverse of homogeneous transformation\n \n \nInputs::\n \nT (4x4) Incoming homogeneous transfor" + "m\n \nOutputs::\n \ninvT (4x4) Outgoing homogeneous transformation T^(-1)\n\nNotes::\n- Efficient i" + "mplementation that uses transpose of rotation submatrix.\n" + } + System { + Name "Inverse" + Location [1212, 444, 1472, 704] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "T" + SID "596:219" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "596:217" + Ports [1, 1] + Position [80, 25, 140, 55] + ZOrder -2 + MATLABFcn "inv(u)" + OutputDimensions "[4,4]" + Output1D off + } + Block { + BlockType Outport + Name "invT" + SID "596:220" + Position [165, 33, 195, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "invT" + DstPort 1 + } + Line { + SrcBlock "T" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Post multiply" + SID "597" + Ports [1, 1] + Position [130, 90, 170, 150] + ZOrder -2 + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 79 + $ClassName "Simulink.Mask" + Description "Post-multiply incoming transform by a constant transformation" + Help "Post-multiply homogeneous transform signal by a constant transform.\n \nParameters::\n \nTP (4x" + "4) The transform to post-multiply by\n \nInputs::\n \nT1 (4x4) Incoming homogeneous transform\n \nOutp" + "uts::\n \nT (4x4) Outgoing homogeneous transformation is T1*TP\n\n\n" + Object { + $PropName "Parameters" + $ObjectID 80 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "T2" + Prompt "TP" + Value "0" + } + } + System { + Name "Post multiply" + Location [824, 420, 1160, 748] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "T1" + SID "597:215" + Position [25, 38, 55, 52] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant" + SID "597:221" + Position [25, 85, 55, 115] + ZOrder -2 + Value "T2" + } + Block { + BlockType Product + Name "Product" + SID "597:212" + Ports [2, 1] + Position [85, 29, 115, 96] + ZOrder -3 + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "T" + SID "597:216" + Position [160, 58, 190, 72] + ZOrder -4 + IconDisplay "Port number" + } + Line { + SrcBlock "T1" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + SrcBlock "Constant" + SrcPort 1 + Points [0, -20] + DstBlock "Product" + DstPort 2 + } + Line { + SrcBlock "Product" + SrcPort 1 + DstBlock "T" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Pre multiply" + SID "598" + Ports [1, 1] + Position [30, 90, 70, 150] + ZOrder -3 + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 81 + $ClassName "Simulink.Mask" + Description "Pre-multiply homogeneous transform by constant transform." + Help "Pre-multiply homogeneous transform signal by a constant transform.\n \nParameters::\n \nTP (4x4" + ") The transform to pre-multiply by\n \nInputs::\n \nT2 (4x4) Incoming homogeneous transform\n \nOutput" + "s::\n \nT (4x4) Outgoing homogeneous transformation is TP*T2\n\n\n" + Object { + $PropName "Parameters" + $ObjectID 82 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "T" + Prompt "T1" + Value "0" + } + } + System { + Name "Pre multiply" + Location [42, 44, 399, 405] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "T2" + SID "598:208" + Position [25, 83, 55, 97] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant" + SID "598:202" + Position [25, 30, 55, 60] + ZOrder -2 + Value "T" + VectorParams1D off + } + Block { + BlockType Product + Name "Product" + SID "598:203" + Ports [2, 1] + Position [85, 29, 115, 96] + ZOrder -3 + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "T" + SID "598:207" + Position [160, 58, 190, 72] + ZOrder -4 + IconDisplay "Port number" + } + Line { + SrcBlock "T2" + SrcPort 1 + Points [0, -10] + DstBlock "Product" + DstPort 2 + } + Line { + SrcBlock "Product" + SrcPort 1 + DstBlock "T" + DstPort 1 + } + Line { + SrcBlock "Constant" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + } + } + Annotation { + SID "152" + Name "\nTransform \n " + Position [52, 32] + BackgroundColor "cyan" + FontSize 18 + } + Annotation { + SID "153" + Name "Robotics Toolbox for MATLAB" + Position [121, 17] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + } + } + Block { + BlockType SubSystem + Name "Transform Conversion" + SID "599" + Ports [] + Position [15, 129, 123, 172] + ZOrder -6 + BackgroundColor "darkGreen" + ShowName off + LibraryVersion "1.225" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 83 + $ClassName "Simulink.Mask" + Display "color('darkBlue')\ndisp('Transform Conversion')" + } + System { + Name "Transform Conversion" + Location [1827, 485, 2246, 1035] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType SubSystem + Name "T2eul" + SID "600" + Ports [1, 3] + Position [265, 242, 300, 298] + ZOrder -1 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 84 + $ClassName "Simulink.Mask" + Description "Convert transform to Euler angles." + Help "Convert homogeneous transformation to Euler angles.\n \n \nInputs::\n \nT (4x4) the attitude as" + " a homogeneous transformation\n\nOutputs::\n\nphi (1x1) first Euler angle [rad]\ntheta (1x1) second Euler " + "angle [rad]\npsi (1x1) third Euler angle [rad]\n \nNotes::\n \n- Axis rotation order is z-y-z (aerospace c" + "onvention).\n- Wrapper for the Toolbox function tr2eul().\n\n" + IconUnits "normalized" + } + System { + Name "T2eul" + Location [129, 393, 533, 720] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "601" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "T" + SID "600:601" + Position [30, 63, 60, 77] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name "Demux" + SID "600:36" + Ports [1, 3] + Position [225, 13, 230, 127] + ZOrder -2 + BackgroundColor "black" + ShowName off + Outputs "3" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "600:37" + Ports [1, 1] + Position [105, 55, 165, 85] + ZOrder -3 + MATLABFcn "tr2eul(u)" + OutputDimensions "3" + } + Block { + BlockType Outport + Name "a" + SID "600:38" + Position [265, 23, 295, 37] + ZOrder -4 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "b" + SID "600:39" + Position [255, 63, 285, 77] + ZOrder -5 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "c" + SID "600:40" + Position [255, 103, 285, 117] + ZOrder -6 + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "T" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 1 + DstBlock "a" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 2 + DstBlock "b" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 3 + DstBlock "c" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "T2rpy" + SID "602" + Ports [1, 3] + Position [155, 243, 195, 297] + ZOrder -2 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 85 + $ClassName "Simulink.Mask" + Description "Convert transform to roll/pitch/yaw angles." + Help "Convert homogeneous transformation to roll/pitch/yaw angles.\n \n \nInputs::\n\nT (4x4) the att" + "itude as a homogeneous transformation\n\nOutputs::\n \nroll (1x1) roll angle [rad]\npitch (1x1) pitch angl" + "e [rad]\nyaw (1x1) yaw angle [rad]\n \nNotes::\n \n- Axis rotation order is x-y-z (aerospace convention).\n" + "- Wrapper for the Toolbox function tr2rpy().\n\n" + IconUnits "normalized" + } + System { + Name "T2rpy" + Location [129, 393, 533, 720] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "T" + SID "602:42" + Position [30, 63, 60, 77] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name "Demux" + SID "602:43" + Ports [1, 3] + Position [225, 13, 230, 127] + ZOrder -2 + BackgroundColor "black" + ShowName off + Outputs "3" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "602:44" + Ports [1, 1] + Position [105, 55, 165, 85] + ZOrder -3 + MATLABFcn "tr2rpy(u)" + OutputDimensions "3" + } + Block { + BlockType Outport + Name "roll" + SID "602:45" + Position [265, 23, 295, 37] + ZOrder -4 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "pitch" + SID "602:46" + Position [255, 63, 285, 77] + ZOrder -5 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "yaw" + SID "602:47" + Position [255, 103, 285, 117] + ZOrder -6 + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "T" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 1 + DstBlock "roll" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 2 + DstBlock "pitch" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 3 + DstBlock "yaw" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "T2xyz" + SID "603" + Ports [1, 3] + Position [55, 242, 90, 298] + ZOrder -3 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 86 + $ClassName "Simulink.Mask" + Description "Convert transform to xyz scalars" + Help "Convert homogeneous transformation to position scalars.\n\n\nInputs::\n\nT (4x4) a translation " + "represented as a homogeneous transformation\n\nOutputs::\n\nx (1x1) x-coordinate\ny (1x1) y-coordinate\nz " + "(1x1) z-coordinate\n\nNotes::\n\n- A wrapper for the Toolbox function transl().\n" + IconUnits "normalized" + } + System { + Name "T2xyz" + Location [129, 393, 533, 720] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "607" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "T" + SID "603:49" + Position [30, 63, 60, 77] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Demux + Name "Demux" + SID "603:50" + Ports [1, 3] + Position [225, 13, 230, 127] + ZOrder -2 + BackgroundColor "black" + ShowName off + Outputs "3" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "603:604" + Ports [1, 1] + Position [105, 55, 165, 85] + ZOrder -3 + MATLABFcn "transl(u)" + OutputDimensions "3" + } + Block { + BlockType Outport + Name "x" + SID "603:605" + Position [265, 23, 295, 37] + ZOrder -4 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "y" + SID "603:606" + Position [255, 63, 285, 77] + ZOrder -5 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "z" + SID "603:607" + Position [255, 103, 285, 117] + ZOrder -6 + Port "3" + IconDisplay "Port number" + } + Line { + SrcBlock "Demux" + SrcPort 3 + DstBlock "z" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 2 + DstBlock "y" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 1 + DstBlock "x" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "T" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "eul2T" + SID "608" + Ports [3, 1] + Position [265, 122, 300, 178] + ZOrder -4 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 87 + $ClassName "Simulink.Mask" + Description "Convert Euler angles to transform." + Help "Convert Euler angles to homogeneous transformation.\n \n \nInputs::\n \nphi (1x1) first Euler a" + "ngle [rad]\ntheta (1x1) second Euler angle [rad]\npsi (1x1) third Euler angle [rad]\n\n\nOutputs::\n \nT (" + "4x4) the attitude as a homogeneous transformation\n \nNotes::\n \n- Axis rotation order is z-y-z (aerospac" + "e convention).\n- Wrapper for the Toolbox function eul2tr().\n\n" + IconUnits "normalized" + } + System { + Name "eul2T" + Location [129, 393, 533, 720] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "614" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "a" + SID "608:609" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "b" + SID "608:610" + Position [25, 68, 55, 82] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "c" + SID "608:611" + Position [25, 103, 55, 117] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "608:612" + Ports [1, 1] + Position [120, 60, 180, 90] + ZOrder -4 + MATLABFcn "eul2tr(u)" + OutputDimensions "[4 4]" + Output1D off + } + Block { + BlockType Mux + Name "Mux" + SID "608:613" + Ports [3, 1] + Position [85, 20, 90, 130] + ZOrder -5 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Outport + Name "T" + SID "608:614" + Position [225, 68, 255, 82] + ZOrder -6 + IconDisplay "Port number" + } + Line { + SrcBlock "b" + SrcPort 1 + DstBlock "Mux" + DstPort 2 + } + Line { + SrcBlock "a" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "T" + DstPort 1 + } + Line { + SrcBlock "c" + SrcPort 1 + DstBlock "Mux" + DstPort 3 + } + } + } + Block { + BlockType SubSystem + Name "rpy2T" + SID "615" + Ports [3, 1] + Position [155, 122, 195, 178] + ZOrder -5 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 88 + $ClassName "Simulink.Mask" + Description "Convert roll/pitch/yaw angles to transform." + Help "Convert roll/pitch/yaw angles to homogeneous transformation.\n \n \nInputs::\n \nroll (1x1) rol" + "l angle [rad]\npitch (1x1) pitch angle [rad]\nyaw (1x1) yaw angle [rad]\n\n\nOutputs::\n \nT (4x4) the att" + "itude as a homogeneous transformation\n \nNotes::\n \n- Axis rotation order is x-y-z (aerospace convention" + ").\n- Wrapper for the Toolbox function rpy2tr().\n\n" + IconUnits "normalized" + } + System { + Name "rpy2T" + Location [129, 393, 533, 720] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "roll" + SID "615:104" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "pitch" + SID "615:105" + Position [25, 68, 55, 82] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "yaw" + SID "615:106" + Position [25, 103, 55, 117] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "615:107" + Ports [1, 1] + Position [120, 60, 180, 90] + ZOrder -4 + MATLABFcn "rpy2tr(u)" + OutputDimensions "[4 4]" + Output1D off + } + Block { + BlockType Mux + Name "Mux" + SID "615:108" + Ports [3, 1] + Position [85, 20, 90, 130] + ZOrder -5 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Outport + Name "T" + SID "615:109" + Position [225, 68, 255, 82] + ZOrder -6 + IconDisplay "Port number" + } + Line { + SrcBlock "pitch" + SrcPort 1 + DstBlock "Mux" + DstPort 2 + } + Line { + SrcBlock "roll" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "T" + DstPort 1 + } + Line { + SrcBlock "yaw" + SrcPort 1 + DstBlock "Mux" + DstPort 3 + } + } + } + Block { + BlockType SubSystem + Name "xyz2T" + SID "616" + Ports [3, 1] + Position [55, 121, 90, 179] + ZOrder -6 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 89 + $ClassName "Simulink.Mask" + Description "Convert xyz scalars to transform." + Help "Convert position scalars to a homogeneous transformation.\n\n\nInputs::\n\nx (1x1) x-coordinate" + "\ny (1x1) y-coordinate\nz (1x1) z-coordinate\n\n\nOutputs::\n\nT (4x4) a translation represented as a homo" + "geneous transformation\n\nNotes::\n\n- A wrapper for the Toolbox function transl().\n" + SelfModifiable "on" + IconUnits "normalized" + } + System { + Name "xyz2T" + Location [129, 393, 533, 720] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "x" + SID "616:119" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "y" + SID "616:120" + Position [25, 68, 55, 82] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "z" + SID "616:121" + Position [25, 103, 55, 117] + ZOrder -3 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "616:122" + Ports [1, 1] + Position [120, 60, 180, 90] + ZOrder -4 + MATLABFcn "transl(u)" + OutputDimensions "[4 4]" + Output1D off + } + Block { + BlockType Mux + Name "Mux" + SID "616:123" + Ports [3, 1] + Position [85, 20, 90, 130] + ZOrder -5 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Outport + Name "T" + SID "616:124" + Position [225, 68, 255, 82] + ZOrder -6 + IconDisplay "Port number" + } + Line { + SrcBlock "z" + SrcPort 1 + DstBlock "Mux" + DstPort 3 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "T" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "x" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "y" + SrcPort 1 + DstBlock "Mux" + DstPort 2 + } + } + } + Annotation { + SID "154" + Name "\nTransform\nconversion \n " + Position [2, 32] + HorizontalAlignment "left" + BackgroundColor "cyan" + FontSize 18 + } + Annotation { + SID "155" + Name "Robotics Toolbox for MATLAB" + Position [121, 17] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + Annotation { + SID "619" + Name "Translation" + Position [76, 91] + ForegroundColor "orange" + } + Annotation { + SID "618" + Name "roll/pitch/yaw\nangles" + Position [181, 96] + ForegroundColor "orange" + } + Annotation { + SID "617" + Name "Euler angles" + Position [286, 91] + ForegroundColor "orange" + } + } + } + Block { + BlockType SubSystem + Name "Vision" + SID "41" + Ports [] + Position [15, 318, 122, 361] + ZOrder -7 + BackgroundColor "lightBlue" + ShowName off + LibraryVersion "1.225" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 90 + $ClassName "Simulink.Mask" + Display "color('Black')\ndisp('Vision')" + } + System { + Name "Vision" + Location [1736, 168, 2155, 937] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType SubSystem + Name "Pose estimation" + SID "620" + Ports [1, 1] + Position [90, 462, 160, 518] + ZOrder -1 + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 91 + $ClassName "Simulink.Mask" + Description "Pose estimation" + Help "Estimate pose of an object based on known 3D points with respect to object\ncoordinate frame, a camera" + " model, and observed object point projections.\n\nParameters::\n \nCamera (Camera) The camera object, sub" + "class of Camera class\nPoints (3xN) The 3D world points in object coordinate frame, one per column\n \nInp" + "uts::\n \nPoints (2xN) The 2D image-plane points, one per column, corresponding\n to the column" + "s of Points.\n \nOutputs::\n \nT (4x4) the estimated object pose as a homogeneous transformation\n \nNo" + "tes::\n \n- A wrapper for the Toolbox method estpose()." + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 92 + Type "edit" + Name "cam" + Prompt "Camera" + Value "0" + } + Object { + $ObjectID 93 + Type "edit" + Name "P" + Prompt "Points (3xN)" + Value "0" + } + PropName "Parameters" + } + } + System { + Name "Pose estimation" + Location [891, 566, 1200, 884] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "p" + SID "620:200" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "620:198" + Ports [1, 1] + Position [80, 25, 140, 55] + ZOrder -2 + MATLABFcn "cam.estpose(P, u)" + OutputDimensions "[4,4]" + Output1D off + } + Block { + BlockType Outport + Name "T" + SID "620:201" + Position [165, 33, 195, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "T" + DstPort 1 + } + Line { + SrcBlock "p" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "camera" + SID "621" + Ports [1, 1] + Position [95, 106, 175, 184] + ZOrder -2 + BackgroundColor "lightBlue" + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 94 + $ClassName "Simulink.Mask" + Description "Camera model" + Help "A generalized camera model that projects a set of world points to the image\nplane.\n\n\nParameters" + "::\n \nCamera (Camera) The camera object, subclass of Camera class\nPoints (3xN) The 3D world points, on" + "e per column\n \nInputs::\n \nT (4x4) the camera pose as a homogeneous transformation\n\n \nOutputs::\n \np (2xN) projection of world points, one per column, corresponding to columns\n of the parameter Poi" + "nts.\n \nNotes::\n \n- Accepts any camera object derived from the abstract Camera superclass." + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 95 + Type "edit" + Name "cam" + Prompt "Camera" + Value "" + } + Object { + $ObjectID 96 + Type "edit" + Name "points" + Prompt "Points (3xN)" + Value "" + } + PropName "Parameters" + } + } + System { + Name "camera" + Location [1795, 1139, 2142, 1423] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "T" + SID "621:133" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "621:134" + Ports [1, 1] + Position [110, 25, 170, 55] + ZOrder -2 + MATLABFcn "cam.plot(points, 'Tcam', u)" + OutputSignalType "real" + Output1D off + } + Block { + BlockType Outport + Name "p" + SID "621:135" + Position [230, 33, 260, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "T" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "p" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "camera2" + SID "622" + Ports [2, 1] + Position [245, 105, 330, 185] + ZOrder -3 + BackgroundColor "lightBlue" + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 97 + $ClassName "Simulink.Mask" + Description "Camera model for moving object" + Help "A generalized camera model that projects a set of world points to the image\nplane.\n\n\nParameters" + "::\n \nCamera (Camera) The camera object, subclass of Camera class\nPoints (3xN) The 3D world points, on" + "e per column, with respect to the\n object coordinate frame.\n \nInputs::\n \nT (4x4) t" + "he camera pose as a homogeneous transformation\nTobj (4x4) the pose of the object coordinate frame\n \nOutputs" + "::\n \np (2xN) projection of world points, one per column, corresponding to columns\n of the parameter" + " Points.\n \nNotes::\n\n- This block allows for the object to move, as well as the camera.\n- Accepts any " + "camera object derived from the abstract Camera superclass." + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 98 + Type "edit" + Name "cam" + Prompt "Camera" + Value "0" + } + Object { + $ObjectID 99 + Type "edit" + Name "points" + Prompt "Points (3xN)" + Value "0" + } + PropName "Parameters" + } + } + System { + Name "camera2" + Location [481, 649, 911, 1026] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "627" + SIDPrevWatermark "257" + Block { + BlockType Inport + Name "T" + SID "622:623" + Position [15, 53, 45, 67] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Tobj" + SID "622:624" + Position [15, 103, 45, 117] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "622:625" + Ports [1, 1] + Position [230, 55, 290, 85] + ZOrder -3 + MATLABFcn "cam.plot(points, 'Tcam', u(:,:,1), 'Tobj', u(:,:,2), 'drawnow')" + OutputDimensions "[2,numcols(points)]" + OutputSignalType "real" + Output1D off + } + Block { + BlockType Concatenate + Name "Matrix\nConcatenate" + SID "622:626" + Ports [2, 1] + Position [135, 49, 190, 91] + ZOrder -4 + Mode "Multidimensional array" + ConcatenateDimension "3" + } + Block { + BlockType Outport + Name "p" + SID "622:627" + Position [325, 63, 355, 77] + ZOrder -5 + IconDisplay "Port number" + } + Line { + SrcBlock "T" + SrcPort 1 + DstBlock "Matrix\nConcatenate" + DstPort 1 + } + Line { + SrcBlock "Tobj" + SrcPort 1 + Points [70, 0] + DstBlock "Matrix\nConcatenate" + DstPort 2 + } + Line { + SrcBlock "Matrix\nConcatenate" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "p" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "image\nJacobian" + SID "628" + Ports [1, 1] + Position [95, 249, 165, 301] + ZOrder -4 + BackgroundColor "lightBlue" + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 100 + $ClassName "Simulink.Mask" + Description "Image Jacobian" + Help "Jacobian matrix relating image plane point velocity to camera spatial\nvelocity expressed in the world" + " frame.\n\nParameters::\n \nCamera (Camera) The camera object, subclass of Camera class\nz (1x1) " + " Point depth, assumed same for all points\nz (1xN) Point depth, unique for each point\n\nInputs::" + "\n\nPoints (2xN) The 2D image-plane points, one per column\n\n \nOutputs::\n \nJ (2Nx6) the image Jaco" + "bian matrix\n \nNotes::\n\n- For multiple points the Jacobian is the \"stack\" of the 2x6 point feature\n " + " Jacobians.\n- A wrapper for the Toolbox method visjac_p().\n" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 101 + Type "edit" + Name "cam" + Prompt "Camera" + Value "" + } + Object { + $ObjectID 102 + Type "edit" + Name "z" + Prompt "Assumed depth (m)" + Value "" + } + PropName "Parameters" + } + } + System { + Name "image\nJacobian" + Location [97, 117, 446, 421] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "225" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "p" + SID "628:137" + Position [25, 33, 55, 47] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "628:138" + Ports [1, 1] + Position [105, 25, 165, 55] + ZOrder -2 + MATLABFcn "cam.visjac_p(u, z)" + Output1D off + } + Block { + BlockType Outport + Name "J" + SID "628:139" + Position [210, 33, 240, 47] + ZOrder -3 + IconDisplay "Port number" + } + Line { + SrcBlock "p" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "J" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "invJac" + SID "629" + Ports [2, 2] + Position [95, 360, 135, 420] + ZOrder -5 + BackgroundColor "red" + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 103 + $ClassName "Simulink.Mask" + Description "Inverse image Jacobian" + Help "Computes camera velocity based on the inverse image Jacobian and image \nplane error.\n\n\nInputs::" + "\n \nJ (6x6) the image Jacobian matrix\ne (6x1) the image plane error (du1, dv1, du2, dv2, ...)\n \nOutput" + "s::\n \nvdot (1x6) the required camera spatial velocity\ncond (1x1) the condition of the image Jacobian\n \n<" + "b>Notes::\n \n- A wrapper for the MATLAB function inv().\n- Only valid for square Jacobians (3 image points)\n" + } + System { + Name "invJac" + Location [1215, 581, 1799, 1020] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "636" + SIDPrevWatermark "271" + Block { + BlockType Inport + Name "J" + SID "629:630" + Position [25, 103, 55, 117] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "e" + SID "629:631" + Position [25, 163, 55, 177] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn2" + SID "629:632" + Ports [1, 1] + Position [185, 25, 245, 55] + ZOrder -3 + ShowName off + MATLABFcn "cond(u)" + Output1D off + } + Block { + BlockType Product + Name "Product1" + SID "629:633" + Ports [2, 1] + Position [240, 95, 275, 155] + ZOrder -4 + BackgroundColor "lightBlue" + ShowName off + Multiplication "Matrix(*)" + } + Block { + BlockType MATLABFcn + Name "inv" + SID "629:634" + Ports [1, 1] + Position [120, 95, 180, 125] + ZOrder -5 + BackgroundColor "lightBlue" + MATLABFcn "pinv(u)" + Output1D off + } + Block { + BlockType Outport + Name "vdot" + SID "629:635" + Position [315, 118, 345, 132] + ZOrder -6 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "cond" + SID "629:636" + Position [315, 33, 345, 47] + ZOrder -7 + Port "2" + IconDisplay "Port number" + } + Line { + SrcBlock "Product1" + SrcPort 1 + DstBlock "vdot" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn2" + SrcPort 1 + DstBlock "cond" + DstPort 1 + } + Line { + SrcBlock "e" + SrcPort 1 + Points [140, 0; 0, -30] + DstBlock "Product1" + DstPort 2 + } + Line { + SrcBlock "inv" + SrcPort 1 + DstBlock "Product1" + DstPort 1 + } + Line { + SrcBlock "J" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "inv" + DstPort 1 + } + Branch { + Points [0, -70] + DstBlock "MATLAB Fcn2" + DstPort 1 + } + } + } + } + Block { + BlockType SubSystem + Name "pinvJac" + SID "637" + Ports [2, 2] + Position [175, 360, 215, 420] + ZOrder -6 + BackgroundColor "red" + DropShadow on + LibraryVersion "1.61" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Object { + $PropName "MaskObject" + $ObjectID 104 + $ClassName "Simulink.Mask" + Description "Inverse image Jacobian" + Help "Computes camera velocity based on the pseudo-inverse of the image Jacobian \nand image plane error for" + " more than 3 points.\n\n\nInputs::\n \nJ (2Nx6) the image Jacobian matrix\ne (2Nx1) the image plane error " + "(du1, dv1, du2, dv2, ...)\n \nOutputs::\n \nvdot (1x6) the required camera spatial velocity\ncond (1x1) th" + "e condition of the image Jacobian\n \nNotes::\n \n- A wrapper for the MATLAB function pinv().\n- Valid for" + " non-square Jacobians (>=3 image points)\n" + } + System { + Name "pinvJac" + Location [391, 253, 848, 623] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + SIDHighWatermark "640" + SIDPrevWatermark "225" + Block { + BlockType Inport + Name "J" + SID "637:141" + Position [25, 103, 55, 117] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "e" + SID "637:142" + Position [25, 163, 55, 177] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn2" + SID "637:143" + Ports [1, 1] + Position [185, 25, 245, 55] + ZOrder -3 + ShowName off + MATLABFcn "cond(u)" + Output1D off + } + Block { + BlockType Product + Name "Product1" + SID "637:144" + Ports [2, 1] + Position [240, 95, 275, 155] + ZOrder -4 + BackgroundColor "lightBlue" + ShowName off + Multiplication "Matrix(*)" + } + Block { + BlockType MATLABFcn + Name "pinv" + SID "637:638" + Ports [1, 1] + Position [120, 95, 180, 125] + ZOrder -5 + BackgroundColor "lightBlue" + MATLABFcn "pinv(u)" + Output1D off + } + Block { + BlockType Outport + Name "vdot" + SID "637:639" + Position [315, 118, 345, 132] + ZOrder -6 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "cond" + SID "637:640" + Position [315, 33, 345, 47] + ZOrder -7 + Port "2" + IconDisplay "Port number" + } + Line { + SrcBlock "Product1" + SrcPort 1 + DstBlock "vdot" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn2" + SrcPort 1 + DstBlock "cond" + DstPort 1 + } + Line { + SrcBlock "e" + SrcPort 1 + Points [140, 0; 0, -30] + DstBlock "Product1" + DstPort 2 + } + Line { + SrcBlock "pinv" + SrcPort 1 + DstBlock "Product1" + DstPort 1 + } + Line { + SrcBlock "J" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "pinv" + DstPort 1 + } + Branch { + Points [0, -70] + DstBlock "MATLAB Fcn2" + DstPort 1 + } + } + } + } + Annotation { + SID "645" + Name "\nMachine Vision \n " + Position [72, 32] + BackgroundColor "cyan" + FontSize 18 + } + Annotation { + SID "644" + Name "Cameras" + Position [17, 111] + HorizontalAlignment "left" + ForegroundColor "orange" + } + Annotation { + SID "643" + Name "Image\nJacobian" + Position [12, 266] + HorizontalAlignment "left" + ForegroundColor "orange" + } + Annotation { + SID "642" + Name "Pose\nestimation" + Position [12, 476] + HorizontalAlignment "left" + ForegroundColor "orange" + } + Annotation { + SID "641" + Name "Machine Vision Toolbox for MATLAB" + Position [151, 17] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + } + } + Annotation { + SID "648" + Name "Robotics Toolbox for MATLAB\n(release 9)" + Position [21, 22] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + Annotation { + SID "647" + Name "Machine Vision Toolbox for Matlab\n(release 3)" + Position [21, 277] + HorizontalAlignment "left" + FontSize 12 + FontWeight "bold" + } + Annotation { + SID "646" + Name "Copyright (c) 2002-2011 Peter Corke" + Position [190, 395] + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sensorfield.m b/lwrserv/matlab/rvctools/simulink/sensorfield.m new file mode 100644 index 0000000..81e6c27 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sensorfield.m @@ -0,0 +1,3 @@ +function sensor = sensorfield(x, y) + xc = 60; yc = 90; + sensor = 200./((x-xc).^2 + (y-yc).^2 + 200); diff --git a/lwrserv/matlab/rvctools/simulink/sl_braitenberg.mdl b/lwrserv/matlab/rvctools/simulink/sl_braitenberg.mdl new file mode 100644 index 0000000..dd5bee2 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_braitenberg.mdl @@ -0,0 +1,1270 @@ +Model { + Name "sl_braitenberg" + Version 7.9 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.16" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + Created "Thu Jul 30 10:56:41 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jul 22 14:25:05 2012" + RTWModifiedTimeStamp 264867848 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "all" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.12.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.12.0" + StartTime "0.0" + StopTime "200" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.12.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput off + SaveState on + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.12.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.12.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.12.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.12.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.12.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.12.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.12.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.12.0" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + CodeExecutionProfiling off + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface on + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 479, 34, 1359, 664 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Inport + Port "1" + OutputFunctionCall off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchInputForFeedbackSignals off + Interpolate on + } + Block { + BlockType MATLABFcn + MATLABFcn "sin" + OutputDimensions "-1" + OutputSignalType "auto" + Output1D on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + DataFormat "Array" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + CheckFcnCallInpInsideContextMsg off + SystemSampleTime "-1" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + SFBlockType "NONE" + Variant off + GeneratePreprocessorConditionals off + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + } + System { + Name "sl_braitenberg" + Location [1371, 69, 2023, 396] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "25" + Block { + BlockType Reference + Name "Bicycle" + SID "19" + Ports [2, 3] + Position [65, 93, 125, 167] + DropShadow on + LibraryVersion "1.53" + SourceBlock "roblocks/Dynamics/Bicycle" + SourceType "" + accel "1" + vlim "1" + alim "2" + L "1" + x0 "[5, 5, 0]" + handbrake "0" + } + Block { + BlockType Constant + Name "Constant" + SID "21" + Position [400, 83, 425, 107] + ShowName off + Value "2" + } + Block { + BlockType Gain + Name "Gain" + SID "22" + Position [280, 255, 310, 285] + BlockMirror on + Gain "5" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType SubSystem + Name "Left sensor" + SID "4" + Ports [1, 1] + Position [280, 177, 365, 223] + ForegroundColor "green" + BackgroundColor "green" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + MaskPromptString "Sensor position x|Sensor position y|Sensor function" + MaskStyleString "edit,edit,edit" + MaskVariables "dx=@1;dy=@2;sensorfunc=@3;" + MaskTunableValueString "on,on,on" + MaskCallbackString "||" + MaskEnableString "on,on,on" + MaskVisibilityString "on,on,on" + MaskToolTipString "on,on,on" + MaskIconFrame on + MaskIconOpaque on + MaskIconRotate "none" + MaskPortRotate "default" + MaskIconUnits "autoscale" + MaskValueString "0|-2|@sensorfield" + System { + Name "Left sensor" + Location [1115, 553, 1700, 746] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "Robot\npose" + SID "5" + Position [20, 33, 50, 47] + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "6" + Ports [1, 1] + Position [95, 25, 155, 55] + MATLABFcn "se2(u)*[dx; dy; 1]" + Port { + PortNumber 1 + Name "position of the sensor\nin the world" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn1" + SID "7" + Ports [1, 1] + Position [280, 25, 340, 55] + MATLABFcn "sensorfunc(u(1), u(2));" + OutputDimensions "1" + } + Block { + BlockType Outport + Name "Sensor" + SID "8" + Position [490, 33, 520, 47] + IconDisplay "Port number" + } + Line { + SrcBlock "Robot\npose" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + Name "position of the sensor\nin the world" + Labels [0, 0] + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "MATLAB Fcn1" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn1" + SrcPort 1 + DstBlock "Sensor" + DstPort 1 + } + Annotation { + SID "24" + Name "Model a sensor that is rigidly attached to the robot.\nFrom robot pose (x,y, theta) we compute the positi" + "on of\nthe sensor at offset (dx,dy) and evaluate the value\nof the sensor function at that position. " + Position [93, 129] + HorizontalAlignment "left" + } + } + } + Block { + BlockType Mux + Name "Mux" + SID "9" + Ports [3, 1] + Position [195, 91, 205, 169] + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType SubSystem + Name "Right sensor" + SID "10" + Ports [1, 1] + Position [280, 107, 365, 153] + ForegroundColor "green" + BackgroundColor "green" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + MaskPromptString "Sensor position x|Sensor position y|Sensor function" + MaskStyleString "edit,edit,edit" + MaskVariables "dx=@1;dy=@2;sensorfunc=@3;" + MaskTunableValueString "on,on,on" + MaskCallbackString "||" + MaskEnableString "on,on,on" + MaskVisibilityString "on,on,on" + MaskToolTipString "on,on,on" + MaskIconFrame on + MaskIconOpaque on + MaskIconRotate "none" + MaskPortRotate "default" + MaskIconUnits "autoscale" + MaskValueString "0|2|@sensorfield" + System { + Name "Right sensor" + Location [1920, 537, 2505, 730] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "Robot\npose" + SID "11" + Position [20, 33, 50, 47] + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "12" + Ports [1, 1] + Position [95, 25, 155, 55] + MATLABFcn "se2(u)*[dx; dy; 1]" + Port { + PortNumber 1 + Name "position of the sensor\nin the world" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn1" + SID "13" + Ports [1, 1] + Position [280, 25, 340, 55] + MATLABFcn "sensorfunc(u(1), u(2));" + OutputDimensions "1" + } + Block { + BlockType Outport + Name "Sensor" + SID "14" + Position [490, 33, 520, 47] + IconDisplay "Port number" + } + Line { + SrcBlock "Robot\npose" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + Name "position of the sensor\nin the world" + Labels [0, 0] + SrcBlock "MATLAB Fcn" + SrcPort 1 + DstBlock "MATLAB Fcn1" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn1" + SrcPort 1 + DstBlock "Sensor" + DstPort 1 + } + Annotation { + SID "25" + Name "Model a sensor that is rigidly attached to the robot.\nFrom robot pose (x,y, theta) we compute the positi" + "on of\nthe sensor at offset (dx,dy) and evaluate the value\nof the sensor function at that position. " + Position [93, 129] + HorizontalAlignment "left" + } + } + } + Block { + BlockType Sum + Name "Sum" + SID "16" + Ports [2, 1] + Position [480, 160, 500, 180] + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID "20" + Ports [3, 1] + Position [440, 120, 460, 140] + ShowName off + IconShape "round" + Inputs "+--" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "XY position" + SID "17" + Ports [2] + Position [195, 15, 225, 50] + LibraryVersion "1.260" + SourceBlock "simulink/Sinks/XY Graph" + SourceType "XY scope." + xmin "0" + xmax "100" + ymin "0" + ymax "100" + st "-1" + } + Block { + BlockType Scope + Name "speed" + SID "23" + Ports [1] + Position [335, 19, 365, 51] + Floating off + Location [1351, 499, 1675, 738] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + YMin "0.035" + YMax "0.0675" + SaveName "ScopeData1" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Scope + Name "steer" + SID "15" + Ports [1] + Position [570, 184, 600, 216] + Floating off + Location [1351, 499, 1675, 738] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + YMin "0.035" + YMax "0.0675" + DataFormat "StructureWithTime" + SampleTime "0" + } + Line { + SrcBlock "Bicycle" + SrcPort 1 + Points [25, 0] + Branch { + DstBlock "Mux" + DstPort 1 + } + Branch { + Points [0, -80] + DstBlock "XY position" + DstPort 1 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 2 + Points [35, 0] + Branch { + DstBlock "Mux" + DstPort 2 + } + Branch { + Points [0, -90] + DstBlock "XY position" + DstPort 2 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 3 + DstBlock "Mux" + DstPort 3 + } + Line { + SrcBlock "Mux" + SrcPort 1 + Points [30, 0] + Branch { + DstBlock "Right sensor" + DstPort 1 + } + Branch { + Points [0, 70] + DstBlock "Left sensor" + DstPort 1 + } + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [20, 0; 0, 30; 0, 0] + Branch { + DstBlock "steer" + DstPort 1 + } + Branch { + Points [0, 70] + DstBlock "Gain" + DstPort 1 + } + } + Line { + SrcBlock "Right sensor" + SrcPort 1 + Points [25, 0] + Branch { + Points [0, 40] + DstBlock "Sum" + DstPort 1 + } + Branch { + DstBlock "Sum1" + DstPort 2 + } + } + Line { + SrcBlock "Left sensor" + SrcPort 1 + Points [80, 0] + Branch { + Points [40, 0] + DstBlock "Sum" + DstPort 2 + } + Branch { + DstBlock "Sum1" + DstPort 3 + } + } + Line { + SrcBlock "Sum1" + SrcPort 1 + Points [25, 0; 0, -55; -185, 0] + Branch { + Points [-280, 0; 0, 40] + DstBlock "Bicycle" + DstPort 1 + } + Branch { + Points [0, -40] + DstBlock "speed" + DstPort 1 + } + } + Line { + SrcBlock "Constant" + SrcPort 1 + Points [20, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "Gain" + SrcPort 1 + Points [-250, 0; 0, -120] + DstBlock "Bicycle" + DstPort 2 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_ctorque.mdl b/lwrserv/matlab/rvctools/simulink/sl_ctorque.mdl new file mode 100644 index 0000000..3a2dc3c --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_ctorque.mdl @@ -0,0 +1,986 @@ +Model { + Name "sl_ctorque" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 2 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "err" + } + ParameterArgumentNames "" + ComputedModelVersion "1.37" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + PreLoadFcn "puma560\np560 = nofriction(p560);" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Tue Jan 8 12:29:23 2002" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Wed Jun 20 20:33:12 2012" + RTWModifiedTimeStamp 261836570 + ModelVersionFormat "1.%" + ConfigurationManager "none" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip on + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "oneshot" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect off + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "10" + AbsTol "auto" + FixedStep "0.01" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "0.05" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "DisableAll" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + BlockReduction off + BooleanDataType off + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero on + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "none" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "TryResolveAllWithWarning" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "warning" + MultiTaskCondExecSysMsg "none" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "None" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown on + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime on + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant off + ParMdlRefBuildCompliant on + CompOptLevelCompliant off + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging off + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 289, 139, 1191, 761 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Clock + DisplayTime off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Terminator + } + Block { + BlockType ZeroOrderHold + SampleTime "1" + } + } + System { + Name "sl_ctorque" + Location [56, 55, 895, 337] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "43" + Block { + BlockType Clock + Name "Clock" + SID "30" + Position [35, 20, 85, 40] + DisplayTime on + Decimation "10" + } + Block { + BlockType Gain + Name "Kd" + SID "2" + Position [600, 90, 630, 120] + ForegroundColor "gray" + } + Block { + BlockType Gain + Name "Kp" + SID "3" + Position [570, 45, 600, 75] + ForegroundColor "gray" + Gain "20" + } + Block { + BlockType Reference + Name "RNE" + SID "34" + Ports [3, 1] + Position [175, 147, 250, 223] + BackgroundColor "red" + LibraryVersion "1.52" + SourceBlock "roblocks/Dynamics/RNE" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560.nofriction()" + grav "[0 0 9.81]" + } + Block { + BlockType Reference + Name "Robot" + SID "35" + Ports [1, 3] + Position [395, 147, 470, 223] + BackgroundColor "red" + DropShadow on + LibraryVersion "1.52" + SourceBlock "roblocks/Dynamics/Robot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560.nofriction()" + q0 "[0 0 0 0 0 0]" + } + Block { + BlockType Sum + Name "Sum" + SID "7" + Ports [2, 1] + Position [510, 50, 530, 70] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "|+-" + } + Block { + BlockType Sum + Name "Sum1" + SID "8" + Ports [2, 1] + Position [540, 95, 560, 115] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "|+-" + } + Block { + BlockType Sum + Name "Sum2" + SID "9" + Ports [2, 1] + Position [635, 50, 655, 70] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Sum + Name "Sum3" + SID "40" + Ports [2, 1] + Position [115, 200, 135, 220] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Terminator + Name "Terminator1" + SID "11" + Position [495, 200, 515, 220] + ShowName off + } + Block { + BlockType Terminator + Name "Terminator2" + SID "36" + Position [105, 20, 125, 40] + ShowName off + } + Block { + BlockType ZeroOrderHold + Name "Tfb" + SID "41" + Position [320, 170, 355, 200] + SampleTime "0.01" + } + Block { + BlockType Reference + Name "jtraj" + SID "38" + Ports [0, 3] + Position [45, 149, 80, 221] + BackgroundColor "red" + LibraryVersion "1.52" + SourceBlock "roblocks/Trajectory/jtraj" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + q0 "[0 0 0 0 0 0]" + qf "[pi/4 pi/2 -pi/2 0 0 0]" + tmax "10" + ts "-1" + } + Block { + BlockType Reference + Name "plot" + SID "39" + Ports [1] + Position [700, 132, 780, 188] + BackgroundColor "red" + LibraryVersion "1.52" + SourceBlock "roblocks/Robot Graphics/plot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + fps "25" + } + Block { + BlockType Outport + Name "q" + SID "28" + Position [625, 173, 655, 187] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "err" + SID "31" + Position [570, 18, 600, 32] + Port "2" + IconDisplay "Port number" + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "Kp" + DstPort 1 + } + Branch { + Points [0, -35] + DstBlock "err" + DstPort 1 + } + } + Line { + SrcBlock "Kp" + SrcPort 1 + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "Robot" + SrcPort 1 + Points [45, 0] + Branch { + DstBlock "Sum" + DstPort 2 + } + Branch { + Points [85, 0] + Branch { + Points [0, 20] + DstBlock "q" + DstPort 1 + } + Branch { + DstBlock "plot" + DstPort 1 + } + } + } + Line { + SrcBlock "Robot" + SrcPort 2 + Points [75, 0] + DstBlock "Sum1" + DstPort 2 + } + Line { + SrcBlock "RNE" + SrcPort 1 + DstBlock "Tfb" + DstPort 1 + } + Line { + SrcBlock "jtraj" + SrcPort 1 + Points [20, 0] + Branch { + Points [0, -100] + DstBlock "Sum" + DstPort 1 + } + Branch { + DstBlock "RNE" + DstPort 1 + } + } + Line { + SrcBlock "jtraj" + SrcPort 2 + Points [35, 0] + Branch { + Points [0, -80] + DstBlock "Sum1" + DstPort 1 + } + Branch { + DstBlock "RNE" + DstPort 2 + } + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "Kd" + DstPort 1 + } + Line { + SrcBlock "Kd" + SrcPort 1 + Points [10, 0] + DstBlock "Sum2" + DstPort 2 + } + Line { + SrcBlock "jtraj" + SrcPort 3 + DstBlock "Sum3" + DstPort 1 + } + Line { + SrcBlock "Robot" + SrcPort 3 + DstBlock "Terminator1" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "Terminator2" + DstPort 1 + } + Line { + SrcBlock "Sum3" + SrcPort 1 + DstBlock "RNE" + DstPort 3 + } + Line { + SrcBlock "Sum2" + SrcPort 1 + Points [20, 0; 0, 185; -555, 0] + DstBlock "Sum3" + DstPort 2 + } + Line { + SrcBlock "Tfb" + SrcPort 1 + DstBlock "Robot" + DstPort 1 + } + Annotation { + Name "trajectory\n(demand)" + Position [57, 245] + ForegroundColor "blue" + } + Annotation { + Name "robot state\n(actual)" + Position [574, 204] + ForegroundColor "orange" + } + Annotation { + Name "desired acceleration" + Position [625, 256] + ForegroundColor "gray" + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_ctorque2.mdl b/lwrserv/matlab/rvctools/simulink/sl_ctorque2.mdl new file mode 100644 index 0000000..2c49d6a --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_ctorque2.mdl @@ -0,0 +1,1285 @@ +Model { + Name "sl_ctorque2" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 2 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "err" + } + ParameterArgumentNames "" + ComputedModelVersion "1.37" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + PreLoadFcn "puma560\np560 = nofriction(p560);" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Tue Jan 8 12:29:23 2002" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sat Apr 28 13:32:14 2012" + RTWModifiedTimeStamp 257519084 + ModelVersionFormat "1.%" + ConfigurationManager "none" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip on + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "oneshot" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect off + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "0.01" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + BlockReduction off + BooleanDataType off + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero on + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "none" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "TryResolveAllWithWarning" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "warning" + MultiTaskCondExecSysMsg "none" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "None" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown on + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime on + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant off + ParMdlRefBuildCompliant on + CompOptLevelCompliant off + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging off + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 786, 727, 1777, 1357 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Clock + DisplayTime off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Inport + Port "1" + OutputFunctionCall off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchInputForFeedbackSignals off + Interpolate on + } + Block { + BlockType MATLABFcn + MATLABFcn "sin" + OutputDimensions "-1" + OutputSignalType "auto" + Output1D on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Product + Inputs "2" + Multiplication "Element-wise(.*)" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Zero" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + CheckFcnCallInpInsideContextMsg off + SystemSampleTime "-1" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + Variant off + GeneratePreprocessorConditionals off + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Terminator + } + Block { + BlockType ZeroOrderHold + SampleTime "1" + } + } + System { + Name "sl_ctorque2" + Location [1260, 51, 2097, 347] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "57" + Block { + BlockType Clock + Name "Clock" + SID "30" + Position [35, 20, 85, 40] + DisplayTime on + Decimation "10" + } + Block { + BlockType Gain + Name "Kd" + SID "2" + Position [600, 90, 630, 120] + ForegroundColor "gray" + } + Block { + BlockType Gain + Name "Kp" + SID "3" + Position [570, 45, 600, 75] + ForegroundColor "gray" + Gain "100" + } + Block { + BlockType SubSystem + Name "RNE approx" + SID "53" + Ports [3, 1] + Position [195, 146, 245, 224] + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + MaskPromptString "Robot|Sample interval (s)" + MaskStyleString "edit,edit" + MaskVariables "robot=@1;ts=@2;" + MaskTunableValueString "on,on" + MaskCallbackString "|" + MaskEnableString "on,on" + MaskVisibilityString "on,on" + MaskToolTipString "on,on" + MaskIconFrame on + MaskIconOpaque on + MaskIconRotate "none" + MaskPortRotate "default" + MaskIconUnits "autoscale" + MaskValueString "p560|0.05" + System { + Name "RNE approx" + Location [1613, 413, 2387, 710] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "q" + SID "54" + Position [25, 83, 55, 97] + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "qd" + SID "56" + Position [25, 173, 55, 187] + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "qdd" + SID "57" + Position [25, 248, 55, 262] + Port "3" + IconDisplay "Port number" + } + Block { + BlockType MATLABFcn + Name "Coriolis" + SID "51" + Ports [1, 1] + Position [270, 156, 410, 184] + MATLABFcn "robot.coriolis(u')" + OutputDimensions "[robot.n robot.n]" + Output1D off + } + Block { + BlockType Mux + Name "Mux" + SID "41" + Ports [2, 1] + Position [200, 151, 205, 189] + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Product + Name "Product" + SID "42" + Ports [2, 1] + Position [545, 32, 590, 63] + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Product + Name "Product1" + SID "43" + Ports [2, 1] + Position [460, 162, 500, 193] + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum3" + SID "47" + Ports [3, 1] + Position [635, 80, 655, 100] + ShowName off + IconShape "round" + Inputs "+++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType ZeroOrderHold + Name "Zero-Order\nHold" + SID "49" + Position [100, 75, 135, 105] + SampleTime "ts" + } + Block { + BlockType ZeroOrderHold + Name "Zero-Order\nHold1" + SID "50" + Position [100, 165, 135, 195] + SampleTime "ts" + } + Block { + BlockType MATLABFcn + Name "gravity" + SID "40" + Ports [1, 1] + Position [270, 76, 410, 104] + MATLABFcn "robot.gravload(u')'" + Output1D off + } + Block { + BlockType MATLABFcn + Name "inertia" + SID "52" + Ports [1, 1] + Position [270, 26, 410, 54] + MATLABFcn "robot.inertia(u')" + OutputDimensions "[robot.n robot.n]" + Output1D off + } + Block { + BlockType Outport + Name "Q" + SID "55" + Position [710, 83, 740, 97] + IconDisplay "Port number" + } + Line { + SrcBlock "q" + SrcPort 1 + DstBlock "Zero-Order\nHold" + DstPort 1 + } + Line { + SrcBlock "qd" + SrcPort 1 + DstBlock "Zero-Order\nHold1" + DstPort 1 + } + Line { + SrcBlock "Zero-Order\nHold" + SrcPort 1 + Points [35, 0] + Branch { + DstBlock "gravity" + DstPort 1 + } + Branch { + Points [0, 70] + DstBlock "Mux" + DstPort 1 + } + Branch { + Points [0, -50] + DstBlock "inertia" + DstPort 1 + } + } + Line { + SrcBlock "Zero-Order\nHold1" + SrcPort 1 + Points [30, 0] + Branch { + DstBlock "Mux" + DstPort 2 + } + Branch { + Points [0, 40; 275, 0] + DstBlock "Product1" + DstPort 2 + } + } + Line { + SrcBlock "inertia" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "Coriolis" + DstPort 1 + } + Line { + SrcBlock "Coriolis" + SrcPort 1 + DstBlock "Product1" + DstPort 1 + } + Line { + SrcBlock "Product1" + SrcPort 1 + Points [140, 0] + DstBlock "Sum3" + DstPort 3 + } + Line { + SrcBlock "Product" + SrcPort 1 + Points [50, 0] + DstBlock "Sum3" + DstPort 1 + } + Line { + SrcBlock "gravity" + SrcPort 1 + DstBlock "Sum3" + DstPort 2 + } + Line { + SrcBlock "qdd" + SrcPort 1 + Points [470, 0] + DstBlock "Product" + DstPort 2 + } + Line { + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Q" + DstPort 1 + } + } + } + Block { + BlockType Reference + Name "Robot" + SID "35" + Ports [1, 3] + Position [395, 147, 470, 223] + BackgroundColor "red" + DropShadow on + LibraryVersion "1.51" + SourceBlock "roblocks/Dynamics/Robot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560.nofriction()" + q0 "[0 0 0 0 0 0]" + } + Block { + BlockType Sum + Name "Sum" + SID "7" + Ports [2, 1] + Position [510, 50, 530, 70] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "|+-" + } + Block { + BlockType Sum + Name "Sum1" + SID "8" + Ports [2, 1] + Position [540, 95, 560, 115] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "|+-" + } + Block { + BlockType Sum + Name "Sum2" + SID "9" + Ports [2, 1] + Position [635, 50, 655, 70] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Terminator + Name "Terminator" + SID "10" + Position [105, 200, 125, 220] + ShowName off + } + Block { + BlockType Terminator + Name "Terminator1" + SID "11" + Position [495, 200, 515, 220] + ShowName off + } + Block { + BlockType Terminator + Name "Terminator2" + SID "36" + Position [105, 20, 125, 40] + ShowName off + } + Block { + BlockType ZeroOrderHold + Name "Tfb" + SID "27" + Position [295, 170, 330, 200] + SampleTime "0.01" + } + Block { + BlockType Reference + Name "jtraj" + SID "38" + Ports [0, 3] + Position [45, 149, 80, 221] + BackgroundColor "red" + LibraryVersion "1.51" + SourceBlock "roblocks/Trajectory/jtraj" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + q0 "[0 0 0 0 0 0]" + qf "[pi/4 pi/2 -pi/2 0 0 0]" + tmax "10" + ts "0.02" + } + Block { + BlockType Reference + Name "plot" + SID "39" + Ports [1] + Position [700, 132, 780, 188] + BackgroundColor "red" + LibraryVersion "1.51" + SourceBlock "roblocks/Robot Graphics/plot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + fps "25" + } + Block { + BlockType Outport + Name "q" + SID "28" + Position [625, 173, 655, 187] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "err" + SID "31" + Position [570, 18, 600, 32] + Port "2" + IconDisplay "Port number" + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "Kp" + DstPort 1 + } + Branch { + Points [0, -35] + DstBlock "err" + DstPort 1 + } + } + Line { + SrcBlock "Kp" + SrcPort 1 + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "Robot" + SrcPort 1 + Points [45, 0] + Branch { + DstBlock "Sum" + DstPort 2 + } + Branch { + Points [85, 0] + Branch { + Points [0, 20] + DstBlock "q" + DstPort 1 + } + Branch { + DstBlock "plot" + DstPort 1 + } + } + } + Line { + SrcBlock "Robot" + SrcPort 2 + Points [75, 0] + DstBlock "Sum1" + DstPort 2 + } + Line { + SrcBlock "Sum2" + SrcPort 1 + Points [10, 0; 0, 190; -510, 0; 0, -40] + DstBlock "RNE approx" + DstPort 3 + } + Line { + SrcBlock "RNE approx" + SrcPort 1 + DstBlock "Tfb" + DstPort 1 + } + Line { + SrcBlock "jtraj" + SrcPort 1 + Points [20, 0] + Branch { + Points [0, -100] + DstBlock "Sum" + DstPort 1 + } + Branch { + DstBlock "RNE approx" + DstPort 1 + } + } + Line { + SrcBlock "jtraj" + SrcPort 2 + Points [35, 0] + Branch { + Points [0, -80] + DstBlock "Sum1" + DstPort 1 + } + Branch { + DstBlock "RNE approx" + DstPort 2 + } + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "Kd" + DstPort 1 + } + Line { + SrcBlock "Kd" + SrcPort 1 + Points [10, 0] + DstBlock "Sum2" + DstPort 2 + } + Line { + SrcBlock "jtraj" + SrcPort 3 + DstBlock "Terminator" + DstPort 1 + } + Line { + SrcBlock "Robot" + SrcPort 3 + DstBlock "Terminator1" + DstPort 1 + } + Line { + SrcBlock "Tfb" + SrcPort 1 + DstBlock "Robot" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "Terminator2" + DstPort 1 + } + Annotation { + Name "trajectory\n(demand)" + Position [57, 245] + ForegroundColor "blue" + } + Annotation { + Name "robot state\n(actual)" + Position [574, 204] + ForegroundColor "orange" + } + Annotation { + Name "desired acceleration" + Position [625, 256] + ForegroundColor "gray" + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_driveline.mdl b/lwrserv/matlab/rvctools/simulink/sl_driveline.mdl new file mode 100644 index 0000000..1228baa --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_driveline.mdl @@ -0,0 +1,1032 @@ +Model { + Name "sl_driveline" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 1 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "xy" + } + ParameterArgumentNames "" + ComputedModelVersion "1.15" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Thu Jul 30 10:56:41 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jul 15 20:32:34 2012" + RTWModifiedTimeStamp 264285088 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions off + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 256, 146, 1136, 776 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Fcn + Expr "sin(u[1])" + SampleTime "-1" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + } + System { + Name "sl_driveline" + Location [1782, 322, 2611, 624] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "34" + Block { + BlockType Reference + Name "Bicycle" + SID "32" + Ports [2, 3] + Position [450, 123, 510, 197] + DropShadow on + LibraryVersion "1.53" + SourceBlock "roblocks/Dynamics/Bicycle" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + accel "1" + vlim "1" + alim "1" + L "1" + x0 "x0" + handbrake "0" + } + Block { + BlockType Constant + Name "Constant" + SID "15" + Position [80, 147, 175, 183] + ShowName off + Value "atan2(-L(1),L(2))" + } + Block { + BlockType Constant + Name "Constant1" + SID "16" + Position [370, 90, 400, 120] + VectorParams1D off + SampleTime "0.02" + } + Block { + BlockType Gain + Name "Gain" + SID "17" + Position [295, 88, 335, 122] + Gain "0.5" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Kh" + SID "18" + Position [275, 165, 305, 195] + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Mux + Name "Mux3" + SID "19" + Ports [2, 1] + Position [585, 131, 590, 169] + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Sum + Name "Sum" + SID "20" + Ports [2, 1] + Position [335, 170, 355, 190] + ShowName off + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "XY" + SID "21" + Ports [2] + Position [570, 80, 600, 115] + LibraryVersion "1.236" + SourceBlock "simulink/Sinks/XY Graph" + SourceType "XY scope." + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + xmin "0" + xmax "10" + ymin "0" + ymax "10" + st "-1" + } + Block { + BlockType Reference + Name "angdiff" + SID "34" + Ports [2, 1] + Position [210, 152, 230, 203] + LibraryVersion "1.53" + SourceBlock "roblocks/Trajectory/angdiff" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + } + Block { + BlockType Scope + Name "d" + SID "28" + Ports [1] + Position [295, 14, 325, 46] + Floating off + Location [1111, 577, 1435, 816] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + SaveName "ScopeData1" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Fcn + Name "distance from line" + SID "29" + Position [80, 88, 175, 122] + Expr "(u(1)*L(1)+u(2)*L(2)+L(3))/sqrt(L(1)^2+L(2)^2)" + Port { + PortNumber 1 + Name "d" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Scope + Name "theta" + SID "30" + Ports [1] + Position [630, 214, 660, 246] + Floating off + Location [817, 565, 1141, 804] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Outport + Name "xy" + SID "31" + Position [660, 53, 690, 67] + IconDisplay "Port number" + } + Line { + SrcBlock "Mux3" + SrcPort 1 + Points [25, 0; 0, -90] + Branch { + DstBlock "xy" + DstPort 1 + } + Branch { + Points [-555, 0] + DstBlock "distance from line" + DstPort 1 + } + } + Line { + SrcBlock "Constant" + SrcPort 1 + DstBlock "angdiff" + DstPort 1 + } + Line { + SrcBlock "angdiff" + SrcPort 1 + DstBlock "Kh" + DstPort 1 + } + Line { + SrcBlock "Kh" + SrcPort 1 + DstBlock "Sum" + DstPort 2 + } + Line { + SrcBlock "Constant1" + SrcPort 1 + Points [20, 0; 0, 40] + DstBlock "Bicycle" + DstPort 1 + } + Line { + SrcBlock "Sum" + SrcPort 1 + DstBlock "Bicycle" + DstPort 2 + } + Line { + Name "d" + Labels [0, 1] + SrcBlock "distance from line" + SrcPort 1 + Points [95, 0] + Branch { + DstBlock "Gain" + DstPort 1 + } + Branch { + Points [0, -75] + DstBlock "d" + DstPort 1 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 1 + Points [0, 5; 15, 0] + Branch { + DstBlock "Mux3" + DstPort 1 + } + Branch { + Points [0, -50] + DstBlock "XY" + DstPort 1 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 2 + Points [25, 0] + Branch { + DstBlock "Mux3" + DstPort 2 + } + Branch { + Points [0, -55] + DstBlock "XY" + DstPort 2 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 3 + Points [0, -5; 25, 0] + Branch { + Points [25, 0; 0, 50] + DstBlock "theta" + DstPort 1 + } + Branch { + Points [0, 50; -345, 0] + DstBlock "angdiff" + DstPort 2 + } + } + Line { + SrcBlock "Gain" + SrcPort 1 + Points [5, 0] + DstBlock "Sum" + DstPort 1 + } + Annotation { + Name "slope of line" + Position [118, 193] + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_drivepoint.mdl b/lwrserv/matlab/rvctools/simulink/sl_drivepoint.mdl new file mode 100644 index 0000000..b76c63e --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_drivepoint.mdl @@ -0,0 +1,1057 @@ +Model { + Name "sl_drivepoint" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 2 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "xy" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + SignalName "theta" + Name "theta " + } + ParameterArgumentNames "" + ComputedModelVersion "1.19" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Thu Jul 30 10:56:41 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jul 15 20:25:25 2012" + RTWModifiedTimeStamp 264284681 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions off + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "15.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Diagnostics" + ConfigPrmDlgPosition [ 234, 146, 1114, 776 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Fcn + Expr "sin(u[1])" + SampleTime "-1" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + } + System { + Name "sl_drivepoint" + Location [1214, 531, 2043, 833] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "22" + Block { + BlockType Reference + Name "Bicycle" + SID "20" + Ports [2, 3] + Position [545, 133, 605, 207] + DropShadow on + LibraryVersion "1.53" + SourceBlock "roblocks/Dynamics/Bicycle" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + accel "1" + vlim "1" + alim "1" + L "1" + x0 "x0" + handbrake "0" + Port { + PortNumber 3 + Name "theta" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Gain + Name "Gain" + SID "2" + Position [430, 115, 460, 145] + Gain "0.5" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "Goal" + SID "3" + Position [70, 75, 100, 105] + Value "xg" + } + Block { + BlockType Gain + Name "Kh" + SID "4" + Position [430, 175, 460, 205] + Gain "4" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Mux + Name "Mux3" + SID "5" + Ports [2, 1] + Position [685, 134, 690, 181] + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Sum + Name "Sum" + SID "6" + Ports [2, 1] + Position [135, 80, 155, 100] + ShowName off + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "XY" + SID "7" + Ports [2] + Position [670, 80, 700, 115] + LibraryVersion "1.236" + SourceBlock "simulink/Sinks/XY Graph" + SourceType "XY scope." + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + xmin "0" + xmax "10" + ymin "0" + ymax "10" + st "-1" + } + Block { + BlockType Reference + Name "angdiff" + SID "22" + Ports [2, 1] + Position [370, 162, 390, 213] + LibraryVersion "1.53" + SourceBlock "roblocks/Trajectory/angdiff" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + } + Block { + BlockType Fcn + Name "steering" + SID "14" + Position [205, 159, 340, 191] + Expr "atan2(u(2), u(1))" + } + Block { + BlockType Scope + Name "theta" + SID "15" + Ports [1] + Position [730, 199, 760, 231] + Floating off + Location [475, 553, 799, 792] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Fcn + Name "throttle" + SID "16" + Position [205, 72, 340, 108] + Expr "sqrt(u(1)^2+u(2)^2)" + } + Block { + BlockType Scope + Name "v" + SID "17" + Ports [1] + Position [510, 79, 540, 111] + Floating off + Location [1083, 75, 1407, 314] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + SaveName "ScopeData1" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Outport + Name "xy" + SID "18" + Position [760, 43, 790, 57] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "theta " + SID "19" + Position [695, 253, 725, 267] + Port "2" + IconDisplay "Port number" + } + Line { + SrcBlock "Bicycle" + SrcPort 1 + Points [25, 0] + Branch { + DstBlock "Mux3" + DstPort 1 + } + Branch { + Points [0, -55] + DstBlock "XY" + DstPort 1 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 2 + Points [35, 0] + Branch { + DstBlock "Mux3" + DstPort 2 + } + Branch { + Points [0, -65] + DstBlock "XY" + DstPort 2 + } + } + Line { + SrcBlock "Mux3" + SrcPort 1 + Points [25, 0; 0, -110] + Branch { + DstBlock "xy" + DstPort 1 + } + Branch { + Points [-575, 0] + DstBlock "Sum" + DstPort 1 + } + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "throttle" + DstPort 1 + } + Branch { + Points [0, 85] + DstBlock "steering" + DstPort 1 + } + } + Line { + SrcBlock "throttle" + SrcPort 1 + Points [60, 0; 0, 40] + DstBlock "Gain" + DstPort 1 + } + Line { + SrcBlock "steering" + SrcPort 1 + DstBlock "angdiff" + DstPort 1 + } + Line { + Name "theta" + Labels [0, 0] + SrcBlock "Bicycle" + SrcPort 3 + Points [35, 0; 0, 20] + Branch { + Points [30, 0] + Branch { + DstBlock "theta" + DstPort 1 + } + Branch { + Points [0, 45] + DstBlock "theta " + DstPort 1 + } + } + Branch { + Labels [2, 0] + Points [0, 30; -290, 0] + DstBlock "angdiff" + DstPort 2 + } + } + Line { + SrcBlock "angdiff" + SrcPort 1 + DstBlock "Kh" + DstPort 1 + } + Line { + SrcBlock "Gain" + SrcPort 1 + Points [15, 0] + Branch { + Points [0, -35] + DstBlock "v" + DstPort 1 + } + Branch { + Points [50, 0] + DstBlock "Bicycle" + DstPort 1 + } + } + Line { + SrcBlock "Goal" + SrcPort 1 + DstBlock "Sum" + DstPort 2 + } + Line { + SrcBlock "Kh" + SrcPort 1 + DstBlock "Bicycle" + DstPort 2 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_drivepose.mdl b/lwrserv/matlab/rvctools/simulink/sl_drivepose.mdl new file mode 100644 index 0000000..200eeda --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_drivepose.mdl @@ -0,0 +1,1390 @@ +Model { + Name "sl_drivepose" + Version 7.9 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 1 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "Out1" + } + ParameterArgumentNames "" + ComputedModelVersion "1.29" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + Created "Thu Jul 30 10:56:41 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jul 22 14:51:27 2012" + RTWModifiedTimeStamp 264869477 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions off + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.12.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.12.0" + StartTime "0.0" + StopTime "20.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "0.05" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.12.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState on + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.12.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.12.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.12.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.12.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.12.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.12.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.12.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.12.0" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + CodeExecutionProfiling off + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface on + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 203, 82, 1104, 704 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Abs + ZeroCross on + SampleTime "-1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Demux + Outputs "4" + DisplayOption "none" + BusSelectionMode off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Inport + Port "1" + OutputFunctionCall off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchInputForFeedbackSignals off + Interpolate on + } + Block { + BlockType M-S-Function + FunctionName "matlabfile" + DisplayMFileStacktrace on + } + Block { + BlockType MATLABFcn + MATLABFcn "sin" + OutputDimensions "-1" + OutputSignalType "auto" + Output1D on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Product + Inputs "2" + Multiplication "Element-wise(.*)" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Zero" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + DataFormat "Array" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType Stop + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + CheckFcnCallInpInsideContextMsg off + SystemSampleTime "-1" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + SFBlockType "NONE" + Variant off + GeneratePreprocessorConditionals off + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + } + System { + Name "sl_drivepose" + Location [1161, 813, 2040, 1097] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "107" + Block { + BlockType Abs + Name "Abs" + SID "107" + Position [220, 190, 250, 220] + BlockMirror on + ShowName off + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "Bicycle" + SID "41" + Ports [2, 3] + Position [310, 68, 370, 142] + DropShadow on + LibraryVersion "1.53" + SourceBlock "roblocks/Dynamics/Bicycle" + SourceType "" + accel "1" + vlim "1" + alim "1" + L "1" + x0 "x0" + handbrake "0" + } + Block { + BlockType Product + Name "Divide" + SID "103" + Ports [3, 1] + Position [220, 119, 250, 171] + ShowName off + Inputs "**/" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType Mux + Name "Mux3" + SID "18" + Ports [3, 1] + Position [465, 70, 470, 140] + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Product + Name "Product" + SID "83" + Ports [2, 1] + Position [220, 72, 250, 103] + ShowName off + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Stop + Name "Stop \nSimulation" + SID "95" + Position [785, 47, 820, 83] + ShowName off + } + Block { + BlockType Sum + Name "Sum" + SID "20" + Ports [2, 1] + Position [130, 120, 150, 140] + ShowName off + IconShape "round" + Inputs "|++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID "92" + Ports [2, 1] + Position [535, 95, 555, 115] + ShowName off + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum2" + SID "99" + Ports [2, 1] + Position [755, 105, 775, 125] + ShowName off + IconShape "round" + Inputs "|++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "XY" + SID "22" + Ports [2] + Position [455, 165, 485, 200] + LibraryVersion "1.260" + SourceBlock "simulink/Sinks/XY Graph" + SourceType "XY scope." + xmin "0" + xmax "10" + ymin "0" + ymax "10" + st "-1" + } + Block { + BlockType Scope + Name "alpha" + SID "105" + Ports [1] + Position [660, 124, 690, 156] + Floating off + Location [182, 439, 506, 678] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Scope + Name "beta" + SID "104" + Ports [1] + Position [725, 189, 755, 221] + Floating off + Location [513, 443, 837, 682] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + SaveName "ScopeData2" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Constant + Name "desired heading" + SID "100" + Position [720, 141, 755, 169] + Value "xg(3)" + } + Block { + BlockType Constant + Name "desired pose" + SID "93" + Position [480, 41, 535, 69] + Value "[xg(1:2) 0]" + } + Block { + BlockType Gain + Name "kalpha" + SID "17" + Position [70, 115, 100, 145] + Gain "8" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "kbeta" + SID "60" + Position [70, 165, 100, 195] + Gain "-3" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "krho" + SID "58" + Position [145, 80, 175, 110] + Gain "3" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType MATLABFcn + Name "there yet?" + SID "96" + Ports [1, 1] + Position [680, 50, 740, 80] + MATLABFcn "double(abs(u)<0.08)" + OutputSignalType "real" + } + Block { + BlockType SubSystem + Name "to polar" + SID "84" + Ports [1, 4] + Position [575, 54, 615, 151] + DropShadow on + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + Port { + PortNumber 4 + Name "direction of motion: +1 or -1" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + System { + Name "to polar" + Location [419, 275, 1124, 603] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Inport + Name "q" + SID "85" + Position [25, 88, 55, 102] + IconDisplay "Port number" + } + Block { + BlockType Demux + Name "Demux" + SID "86" + Ports [1, 4] + Position [270, 58, 275, 127] + ShowName off + DisplayOption "bar" + } + Block { + BlockType M-S-Function + Name "Level-2 M-file\nS-Function" + SID "87" + Ports [1, 1] + Position [140, 79, 210, 111] + FunctionName "polar_sfunc" + } + Block { + BlockType Outport + Name "rho" + SID "88" + Position [535, 33, 565, 47] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "alpha" + SID "89" + Position [535, 78, 565, 92] + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "beta" + SID "90" + Position [535, 123, 565, 137] + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "direction" + SID "91" + Position [535, 178, 565, 192] + Port "4" + IconDisplay "Port number" + } + Line { + SrcBlock "Demux" + SrcPort 3 + Points [170, 0; 0, 30] + DstBlock "beta" + DstPort 1 + } + Line { + SrcBlock "q" + SrcPort 1 + DstBlock "Level-2 M-file\nS-Function" + DstPort 1 + } + Line { + SrcBlock "Level-2 M-file\nS-Function" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 1 + Points [120, 0; 0, -30] + DstBlock "rho" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 2 + DstBlock "alpha" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 4 + Points [120, 0; 0, 70] + DstBlock "direction" + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "Out1" + SID "37" + Position [510, 158, 540, 172] + IconDisplay "Port number" + } + Line { + SrcBlock "Mux3" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "Out1" + DstPort 1 + } + Branch { + DstBlock "Sum1" + DstPort 2 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 1 + Points [40, 0] + Branch { + DstBlock "Mux3" + DstPort 1 + } + Branch { + Points [0, 95] + DstBlock "XY" + DstPort 1 + } + } + Line { + Labels [0, 0] + SrcBlock "Bicycle" + SrcPort 2 + Points [50, 0] + Branch { + DstBlock "Mux3" + DstPort 2 + } + Branch { + Points [0, 85] + DstBlock "XY" + DstPort 2 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 3 + DstBlock "Mux3" + DstPort 3 + } + Line { + SrcBlock "to polar" + SrcPort 2 + Points [25, 0; 0, 50] + Branch { + Points [0, 95; -595, 0; 0, -105] + DstBlock "kalpha" + DstPort 1 + } + Branch { + DstBlock "alpha" + DstPort 1 + } + } + Line { + SrcBlock "kalpha" + SrcPort 1 + DstBlock "Sum" + DstPort 1 + } + Line { + SrcBlock "kbeta" + SrcPort 1 + Points [35, 0] + DstBlock "Sum" + DstPort 2 + } + Line { + SrcBlock "Product" + SrcPort 1 + Points [25, 0] + Branch { + DstBlock "Bicycle" + DstPort 1 + } + Branch { + Points [0, 115] + DstBlock "Abs" + DstPort 1 + } + } + Line { + SrcBlock "krho" + SrcPort 1 + DstBlock "Product" + DstPort 2 + } + Line { + SrcBlock "to polar" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "there yet?" + DstPort 1 + } + Branch { + Labels [2, 0] + Points [0, -45; -510, 0] + DstBlock "krho" + DstPort 1 + } + } + Line { + Name "direction of motion: +1 or -1" + Labels [3, 0] + SrcBlock "to polar" + SrcPort 4 + Points [10, 0; 0, -111; -440, 0; 0, 51] + Branch { + DstBlock "Product" + DstPort 1 + } + Branch { + Points [0, 65] + DstBlock "Divide" + DstPort 2 + } + } + Line { + SrcBlock "desired pose" + SrcPort 1 + Points [5, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "to polar" + DstPort 1 + } + Line { + SrcBlock "Sum" + SrcPort 1 + DstBlock "Divide" + DstPort 1 + } + Line { + SrcBlock "there yet?" + SrcPort 1 + DstBlock "Stop \nSimulation" + DstPort 1 + } + Line { + SrcBlock "to polar" + SrcPort 3 + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "Sum2" + SrcPort 1 + Points [20, 0; 0, 130; -120, 0] + Branch { + Points [-640, 0; 0, -65] + DstBlock "kbeta" + DstPort 1 + } + Branch { + Points [0, -40] + DstBlock "beta" + DstPort 1 + } + } + Line { + SrcBlock "desired heading" + SrcPort 1 + Points [5, 0] + DstBlock "Sum2" + DstPort 2 + } + Line { + SrcBlock "Divide" + SrcPort 1 + Points [10, 0; 0, -20] + DstBlock "Bicycle" + DstPort 2 + } + Line { + SrcBlock "Abs" + SrcPort 1 + Points [-20, 0; 0, -45] + DstBlock "Divide" + DstPort 3 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_fforward.mdl b/lwrserv/matlab/rvctools/simulink/sl_fforward.mdl new file mode 100644 index 0000000..b1beae5 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_fforward.mdl @@ -0,0 +1,1113 @@ +Model { + Name "sl_fforward" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 4 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "err" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "tau_ff" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + SignalName "feedback\ntorque" + Name "tau_fb" + } + ParameterArgumentNames "" + ComputedModelVersion "1.4" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Mon Dec 13 07:52:47 2010" + Creator "corkep" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sat Apr 28 16:48:51 2012" + RTWModifiedTimeStamp 257532525 + ModelVersionFormat "1.%" + ConfigurationManager "None" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + Array { + Type "Cell" + Dimension 7 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorLevel1" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + Array { + Type "Cell" + Dimension 19 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 273, 428, 1153, 1058 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Clock + DisplayTime off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Terminator + } + Block { + BlockType ZeroOrderHold + SampleTime "1" + } + } + System { + Name "sl_fforward" + Location [762, 179, 1530, 472] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "20" + Block { + BlockType Clock + Name "Clock" + SID "1" + Position [30, 15, 80, 35] + DisplayTime on + Decimation "10" + } + Block { + BlockType Gain + Name "Kd" + SID "2" + Position [210, 115, 240, 145] + ForegroundColor "gray" + } + Block { + BlockType Gain + Name "Kp" + SID "3" + Position [210, 65, 240, 95] + ForegroundColor "gray" + Gain "100" + } + Block { + BlockType Reference + Name "RNE" + SID "4" + Ports [3, 1] + Position [210, 166, 295, 234] + BackgroundColor "red" + LibraryVersion "1.52" + SourceBlock "roblocks/Dynamics/RNE" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560.nofriction()" + grav "[0 0 9.81]" + } + Block { + BlockType Reference + Name "Robot" + SID "5" + Ports [1, 3] + Position [460, 41, 545, 119] + BackgroundColor "red" + DropShadow on + LibraryVersion "1.52" + SourceBlock "roblocks/Dynamics/Robot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + q0 "[0 0 0 0 0 0]" + } + Block { + BlockType Sum + Name "Sum" + SID "6" + Ports [2, 1] + Position [135, 70, 155, 90] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "-+|" + } + Block { + BlockType Sum + Name "Sum1" + SID "7" + Ports [2, 1] + Position [160, 120, 180, 140] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "-+|" + } + Block { + BlockType Sum + Name "Sum2" + SID "8" + Ports [2, 1] + Position [265, 70, 285, 90] + ForegroundColor "gray" + ShowName off + IconShape "round" + Inputs "|++" + Port { + PortNumber 1 + Name "feedback\ntorque" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Sum + Name "Sum3" + SID "9" + Ports [2, 1] + Position [410, 70, 430, 90] + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Terminator + Name "Terminator" + SID "10" + Position [580, 95, 600, 115] + ShowName off + } + Block { + BlockType Terminator + Name "Terminator1" + SID "20" + Position [100, 15, 120, 35] + ShowName off + } + Block { + BlockType ZeroOrderHold + Name "Tfb" + SID "11" + Position [350, 65, 385, 95] + SampleTime "0.01" + } + Block { + BlockType ZeroOrderHold + Name "Tff" + SID "12" + Position [345, 185, 380, 215] + SampleTime "0.01" + } + Block { + BlockType Reference + Name "jtraj" + SID "19" + Ports [0, 3] + Position [30, 69, 65, 141] + BackgroundColor "red" + LibraryVersion "1.52" + SourceBlock "roblocks/Trajectory/jtraj" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + q0 "[0 0 0 0 0 0]" + qf "[pi/4 pi/2 -pi/2 0 0 0]" + tmax "10" + ts "0.02" + } + Block { + BlockType Reference + Name "plot" + SID "14" + Ports [1] + Position [655, 27, 735, 83] + BackgroundColor "red" + LibraryVersion "1.52" + SourceBlock "roblocks/Robot Graphics/plot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + fps "25" + } + Block { + BlockType Outport + Name "q" + SID "15" + Position [610, 63, 640, 77] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "err" + SID "16" + Position [215, 38, 245, 52] + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "tau_ff" + SID "17" + Position [445, 193, 475, 207] + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "tau_fb" + SID "18" + Position [315, 38, 345, 52] + Port "4" + IconDisplay "Port number" + } + Line { + SrcBlock "Kd" + SrcPort 1 + Points [30, 0] + DstBlock "Sum2" + DstPort 2 + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "Kp" + DstPort 1 + } + Branch { + Points [0, -35] + DstBlock "err" + DstPort 1 + } + } + Line { + SrcBlock "Kp" + SrcPort 1 + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "Kd" + DstPort 1 + } + Line { + Name "feedback\ntorque" + SrcBlock "Sum2" + SrcPort 1 + Points [5, 0] + Branch { + Labels [-1, 0] + DstBlock "Tfb" + DstPort 1 + } + Branch { + Points [0, -35] + DstBlock "tau_fb" + DstPort 1 + } + } + Line { + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Robot" + DstPort 1 + } + Line { + SrcBlock "Robot" + SrcPort 1 + Points [40, 0] + Branch { + DstBlock "plot" + DstPort 1 + } + Branch { + Points [0, -40; -445, 0] + DstBlock "Sum" + DstPort 1 + } + Branch { + Points [0, 15] + DstBlock "q" + DstPort 1 + } + } + Line { + SrcBlock "Robot" + SrcPort 2 + Points [25, 0; 0, -55; -405, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "jtraj" + SrcPort 1 + Points [35, 0] + Branch { + Points [0, 100] + DstBlock "RNE" + DstPort 1 + } + Branch { + DstBlock "Sum" + DstPort 2 + } + } + Line { + SrcBlock "jtraj" + SrcPort 2 + Points [25, 0; 0, 25; 0, 0] + Branch { + DstBlock "Sum1" + DstPort 2 + } + Branch { + Points [0, 70] + DstBlock "RNE" + DstPort 2 + } + } + Line { + SrcBlock "jtraj" + SrcPort 3 + Points [15, 0; 0, 90] + DstBlock "RNE" + DstPort 3 + } + Line { + SrcBlock "Robot" + SrcPort 3 + DstBlock "Terminator" + DstPort 1 + } + Line { + SrcBlock "Tff" + SrcPort 1 + Points [35, 0] + Branch { + DstBlock "Sum3" + DstPort 2 + } + Branch { + DstBlock "tau_ff" + DstPort 1 + } + } + Line { + SrcBlock "Tfb" + SrcPort 1 + DstBlock "Sum3" + DstPort 1 + } + Line { + SrcBlock "RNE" + SrcPort 1 + DstBlock "Tff" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "Terminator1" + DstPort 1 + } + Annotation { + Name "trajectory\n(demand)" + Position [117, 240] + ForegroundColor "blue" + } + Annotation { + Name "robot state\n(actual)" + Position [579, 139] + ForegroundColor "orange" + } + Annotation { + Name "feedforward torque" + Position [304, 261] + ForegroundColor "magenta" + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_flex.mdl b/lwrserv/matlab/rvctools/simulink/sl_flex.mdl new file mode 100644 index 0000000..624ff8f --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_flex.mdl @@ -0,0 +1,982 @@ +Model { + Name "sl_flex" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 1 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q" + } + ParameterArgumentNames "" + ComputedModelVersion "1.29" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + PreLoadFcn "twolink" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Tue Jan 8 12:29:23 2002" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Wed Apr 25 09:45:55 2012" + RTWModifiedTimeStamp 257247943 + ModelVersionFormat "1.%" + ConfigurationManager "none" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip on + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "oneshot" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect off + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "4" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + BlockReduction off + BooleanDataType off + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero on + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "none" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "TryResolveAllWithWarning" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "warning" + MultiTaskCondExecSysMsg "none" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "None" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown on + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime on + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant off + ParMdlRefBuildCompliant on + CompOptLevelCompliant off + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging off + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 384, 120, 1264, 750 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Derivative + LinearizePole "inf" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType RateLimiter + RisingSlewLimit "1" + FallingSlewLimit "-1" + SampleTimeMode "continuous" + InitialCondition "0" + LinearizeAsGain on + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType Step + Time "1" + Before "0" + After "1" + SampleTime "-1" + VectorParams1D on + ZeroCross on + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Terminator + } + } + System { + Name "sl_flex" + Location [607, 267, 1411, 514] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "20" + Block { + BlockType Gain + Name "B" + SID "1" + Position [305, 115, 335, 145] + ForegroundColor "blue" + Gain "20" + } + Block { + BlockType Constant + Name "Constant" + SID "3" + Position [55, 85, 85, 115] + Value "0" + } + Block { + BlockType Derivative + Name "Derivative" + SID "4" + Position [255, 115, 285, 145] + ForegroundColor "blue" + } + Block { + BlockType Gain + Name "K" + SID "5" + Position [300, 59, 335, 91] + ForegroundColor "blue" + Gain "100" + } + Block { + BlockType Mux + Name "Mux1" + SID "8" + Ports [2, 1] + Position [160, 25, 165, 125] + ShowName off + Inputs "2" + DisplayOption "bar" + Port { + PortNumber 1 + Name "motor\nposition" + PropagatedSignals ", " + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType RateLimiter + Name "Rate Limiter" + SID "9" + Position [110, 34, 140, 66] + RisingSlewLimit "50" + FallingSlewLimit "-50" + } + Block { + BlockType Reference + Name "Robot" + SID "10" + Ports [1, 3] + Position [385, 39, 500, 111] + BackgroundColor "red" + DropShadow on + LibraryVersion "1.39" + SourceBlock "roblocks/Dynamics/Robot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "twolink" + q0 "[0 0]" + } + Block { + BlockType Scope + Name "Scope" + SID "11" + Ports [1] + Position [655, 99, 685, 131] + Floating off + Location [157, 190, 481, 429] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + YMin "-0.5" + YMax "1.5" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Step + Name "Step" + SID "12" + Position [55, 35, 85, 65] + SampleTime "0" + } + Block { + BlockType Sum + Name "Sum" + SID "13" + Ports [2, 1] + Position [205, 65, 225, 85] + ShowName off + IconShape "round" + Inputs "-+|" + } + Block { + BlockType Sum + Name "Sum1" + SID "14" + Ports [2, 1] + Position [350, 65, 370, 85] + ForegroundColor "blue" + ShowName off + IconShape "round" + Inputs "|++" + } + Block { + BlockType Terminator + Name "Terminator" + SID "15" + Position [520, 90, 540, 110] + ShowName off + } + Block { + BlockType Terminator + Name "Terminator1" + SID "20" + Position [520, 65, 540, 85] + ShowName off + } + Block { + BlockType Reference + Name "plot" + SID "18" + Ports [1] + Position [605, 22, 685, 78] + BackgroundColor "red" + LibraryVersion "1.39" + SourceBlock "roblocks/Robot Graphics/plot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "twolink" + } + Block { + BlockType Outport + Name "q" + SID "19" + Position [590, 153, 620, 167] + IconDisplay "Port number" + } + Line { + SrcBlock "Constant" + SrcPort 1 + DstBlock "Mux1" + DstPort 2 + } + Line { + SrcBlock "Step" + SrcPort 1 + DstBlock "Rate Limiter" + DstPort 1 + } + Line { + SrcBlock "Rate Limiter" + SrcPort 1 + DstBlock "Mux1" + DstPort 1 + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [5, 0] + Branch { + DstBlock "K" + DstPort 1 + } + Branch { + Points [0, 55] + DstBlock "Derivative" + DstPort 1 + } + } + Line { + SrcBlock "Robot" + SrcPort 1 + Points [55, 0] + Branch { + Points [0, -25; -345, 0] + DstBlock "Sum" + DstPort 1 + } + Branch { + DstBlock "plot" + DstPort 1 + } + Branch { + Points [0, 65] + Branch { + DstBlock "Scope" + DstPort 1 + } + Branch { + Points [0, 45] + DstBlock "q" + DstPort 1 + } + } + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "Robot" + DstPort 1 + } + Line { + SrcBlock "K" + SrcPort 1 + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "B" + SrcPort 1 + Points [20, 0] + DstBlock "Sum1" + DstPort 2 + } + Line { + SrcBlock "Derivative" + SrcPort 1 + DstBlock "B" + DstPort 1 + } + Line { + Name "motor\nposition" + Labels [0, 0] + SrcBlock "Mux1" + SrcPort 1 + DstBlock "Sum" + DstPort 2 + } + Line { + SrcBlock "Robot" + SrcPort 3 + DstBlock "Terminator" + DstPort 1 + } + Line { + SrcBlock "Robot" + SrcPort 2 + DstBlock "Terminator1" + DstPort 1 + } + Annotation { + Name "load position" + Position [258, 32] + } + Annotation { + Name "transmission comprises\nspring + damper" + Position [281, 178] + ForegroundColor "blue" + } + Annotation { + Name "assume the motor\nis infinitely \"stiff\"" + Position [109, 148] + } + Annotation { + Name "q1" + Position [40, 54] + } + Annotation { + Name "q2" + Position [39, 101] + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_jspace.mdl b/lwrserv/matlab/rvctools/simulink/sl_jspace.mdl new file mode 100644 index 0000000..90bfb45 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_jspace.mdl @@ -0,0 +1,776 @@ +Model { + Name "sl_jspace" + Version 7.6 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.26" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Tue Jan 8 12:29:23 2002" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "n6334687" + ModifiedDateFormat "%" + LastModifiedDate "Mon Feb 06 16:36:59 2012" + RTWModifiedTimeStamp 250447015 + ModelVersionFormat "1.%" + ConfigurationManager "none" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip on + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "oneshot" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect off + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock off + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.10.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.10.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.10.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "sigsOut" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.10.0" + Array { + Type "Cell" + Dimension 7 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction off + BooleanDataType off + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.10.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "none" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "TryResolveAllWithWarning" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "warning" + MultiTaskCondExecSysMsg "none" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + InitInArrayFormatMsg "warning" + StrictBusMsg "None" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.10.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown on + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.10.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.10.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.10.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime on + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.10.0" + Array { + Type "Cell" + Dimension 19 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.10.0" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition " [ 920, 197, 1800, 827 ] " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + } + System { + Name "sl_jspace" + Location [2035, 406, 2499, 616] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "6" + Block { + BlockType Reference + Name "T2xyz" + SID "1" + Ports [1, 3] + Position [275, 112, 325, 168] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Transform Conversion/T2xyz" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + } + Block { + BlockType Reference + Name "XY Graph" + SID "2" + Ports [2] + Position [390, 108, 430, 152] + LibraryVersion "1.225" + SourceBlock "simulink/Sinks/XY Graph" + SourceType "XY scope." + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + xmin "-1" + xmax "1" + ymin "-1" + ymax "1" + st "-1" + } + Block { + BlockType Reference + Name "fkine" + SID "3" + Ports [1, 1] + Position [185, 112, 235, 168] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Kinematics/fkine" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + } + Block { + BlockType Reference + Name "jtraj" + SID "6" + Ports [0, 3] + Position [70, 39, 105, 111] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Trajectory/jtraj" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + q0 "[0 0 0 0 0 0]" + qf "[pi/4 pi/2 -pi/2 0 0 0]" + tmax "10" + } + Block { + BlockType Reference + Name "plot" + SID "5" + Ports [1] + Position [215, 22, 295, 78] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Robot Graphics/plot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + } + Line { + SrcBlock "jtraj" + SrcPort 1 + Points [60, 0] + Branch { + DstBlock "plot" + DstPort 1 + } + Branch { + DstBlock "fkine" + DstPort 1 + } + } + Line { + SrcBlock "fkine" + SrcPort 1 + DstBlock "T2xyz" + DstPort 1 + } + Line { + SrcBlock "T2xyz" + SrcPort 1 + DstBlock "XY Graph" + DstPort 1 + } + Line { + SrcBlock "T2xyz" + SrcPort 2 + DstBlock "XY Graph" + DstPort 2 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_lanechange.mdl b/lwrserv/matlab/rvctools/simulink/sl_lanechange.mdl new file mode 100644 index 0000000..c4091cd --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_lanechange.mdl @@ -0,0 +1,859 @@ +Model { + Name "sl_lanechange" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 2 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "x" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "alpha" + } + ParameterArgumentNames "" + ComputedModelVersion "1.9" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Fri Jul 31 10:54:59 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jul 15 20:09:56 2012" + RTWModifiedTimeStamp 264283772 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions off + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Data Import//Export" + ConfigPrmDlgPosition [ 311, 123, 1191, 753 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + } + System { + Name "sl_lanechange" + Location [465, 371, 975, 595] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "21" + Block { + BlockType Reference + Name "Bicycle" + SID "21" + Ports [2, 3] + Position [265, 68, 325, 142] + DropShadow on + LibraryVersion "1.53" + SourceBlock "roblocks/Dynamics/Bicycle" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + accel "1" + vlim "1" + alim "2" + L "1" + x0 "[0, 0, 0]" + handbrake "0" + } + Block { + BlockType Mux + Name "Mux3" + SID "15" + Ports [3, 1] + Position [405, 70, 410, 140] + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Reference + Name "Steering\nangle" + SID "16" + Ports [0, 1] + Position [110, 109, 155, 141] + LibraryVersion "1.236" + SourceBlock "simulink/Sources/Repeating\nSequence\nStair" + SourceType "Repeating Sequence Stair" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + OutValues "[0 0 0 0.5 0 -0.5 0 0 0 0]" + tsamp "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "double" + OutputDataTypeScalingMode "Specify via dialog" + OutDataType "double" + ConRadixGroup "Best Precision: Vector-wise" + OutScaling "2^-12" + LockScale off + } + Block { + BlockType Reference + Name "XY" + SID "17" + Ports [2] + Position [390, 15, 420, 50] + LibraryVersion "1.236" + SourceBlock "simulink/Sinks/XY Graph" + SourceType "XY scope." + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + xmin "0" + xmax "10" + ymin "0" + ymax "10" + st "-1" + } + Block { + BlockType Constant + Name "speed" + SID "18" + Position [185, 75, 215, 105] + VectorParams1D off + SampleTime "0.02" + } + Block { + BlockType Outport + Name "x" + SID "19" + Position [450, 98, 480, 112] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "alpha" + SID "20" + Position [215, 138, 245, 152] + Port "2" + IconDisplay "Port number" + } + Line { + SrcBlock "Steering\nangle" + SrcPort 1 + Points [40, 0] + Branch { + DstBlock "alpha" + DstPort 1 + } + Branch { + DstBlock "Bicycle" + DstPort 2 + } + } + Line { + SrcBlock "speed" + SrcPort 1 + DstBlock "Bicycle" + DstPort 1 + } + Line { + Labels [1, 0] + SrcBlock "Bicycle" + SrcPort 3 + DstBlock "Mux3" + DstPort 3 + } + Line { + SrcBlock "Bicycle" + SrcPort 1 + Points [20, 0] + Branch { + DstBlock "Mux3" + DstPort 1 + } + Branch { + Points [0, -55] + DstBlock "XY" + DstPort 1 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 2 + Points [35, 0] + Branch { + DstBlock "Mux3" + DstPort 2 + } + Branch { + Points [0, -65] + DstBlock "XY" + DstPort 2 + } + } + Line { + SrcBlock "Mux3" + SrcPort 1 + DstBlock "x" + DstPort 1 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_pursuit.mdl b/lwrserv/matlab/rvctools/simulink/sl_pursuit.mdl new file mode 100644 index 0000000..1db3683 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_pursuit.mdl @@ -0,0 +1,1162 @@ +Model { + Name "sl_pursuit" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 4 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "xy" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "theta" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "goal" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "v" + } + ParameterArgumentNames "" + ComputedModelVersion "1.16" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Thu Jul 30 10:56:41 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jul 15 20:58:18 2012" + RTWModifiedTimeStamp 264286354 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions off + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "15" + AbsTol "auto" + FixedStep "0.2" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "0.1" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 280, 126, 1160, 756 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Fcn + Expr "sin(u[1])" + SampleTime "-1" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + } + System { + Name "sl_pursuit" + Location [1376, 113, 2205, 415] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "97" + ReportName "simulink-default.rpt" + SIDHighWatermark "51" + Block { + BlockType Reference + Name "Bicycle" + SID "49" + Ports [2, 3] + Position [555, 132, 615, 208] + DropShadow on + LibraryVersion "1.53" + SourceBlock "roblocks/Dynamics/Bicycle" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + accel "1" + vlim "2" + alim "1" + L "1" + x0 "[0, 0, 0]" + handbrake "0" + } + Block { + BlockType Reference + Name "Circle" + SID "47" + Ports [0, 1] + Position [40, 60, 80, 120] + LibraryVersion "1.53" + SourceBlock "roblocks/Trajectory/Circle" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + radius "1" + freq "0.2" + } + Block { + BlockType Fcn + Name "Fcn" + SID "50" + Position [195, 250, 255, 280] + Expr "sqrt(u(1)^2+u(2)^2)" + } + Block { + BlockType Gain + Name "Kh" + SID "24" + Position [475, 175, 505, 205] + Gain "5" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Mux + Name "Mux3" + SID "25" + Ports [2, 1] + Position [685, 136, 690, 174] + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Reference + Name "PID Controller" + SID "26" + Ports [1, 1] + Position [425, 115, 455, 145] + LibraryVersion "1.11" + SourceBlock "simulink_need_slupdate/PID Controller" + SourceType "PID Controller" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + P "1" + I "0.5" + D "0" + } + Block { + BlockType Scope + Name "Scope" + SID "51" + Ports [1] + Position [290, 249, 320, 281] + Floating off + Location [1356, 592, 1680, 831] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + SaveName "ScopeData3" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Sum + Name "Sum" + SID "27" + Ports [2, 1] + Position [135, 80, 155, 100] + ShowName off + IconShape "round" + Inputs "-+|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "XY" + SID "28" + Ports [2] + Position [670, 80, 700, 115] + LibraryVersion "1.236" + SourceBlock "simulink/Sinks/XY Graph" + SourceType "XY scope." + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + xmin "-2" + xmax "2" + ymin "-2" + ymax "2" + st "-1" + } + Block { + BlockType Reference + Name "angdiff" + SID "48" + Ports [2, 1] + Position [370, 162, 390, 213] + LibraryVersion "1.53" + SourceBlock "roblocks/Trajectory/angdiff" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + } + Block { + BlockType Scope + Name "steer angle" + SID "43" + Ports [1] + Position [555, 244, 585, 276] + Floating off + Location [1016, 567, 1340, 806] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + SaveName "ScopeData2" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Fcn + Name "steering" + SID "35" + Position [205, 159, 340, 191] + Expr "atan2(u(2), u(1))" + } + Block { + BlockType Scope + Name "theta " + SID "36" + Ports [1] + Position [730, 199, 760, 231] + Floating off + Location [475, 553, 799, 792] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Fcn + Name "throttle" + SID "37" + Position [205, 72, 340, 108] + Expr "sqrt(u(1)^2+u(2)^2)-0.1" + } + Block { + BlockType Scope + Name "velocity" + SID "38" + Ports [1] + Position [510, 79, 540, 111] + Floating off + Location [683, 553, 1007, 792] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + SaveName "ScopeData1" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Outport + Name "xy" + SID "39" + Position [760, 38, 790, 52] + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "theta" + SID "40" + Position [695, 253, 725, 267] + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "goal" + SID "41" + Position [120, 133, 150, 147] + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "v" + SID "46" + Position [495, 148, 525, 162] + Port "4" + IconDisplay "Port number" + } + Line { + SrcBlock "Bicycle" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "Mux3" + DstPort 1 + } + Branch { + Points [0, -55] + DstBlock "XY" + DstPort 1 + } + } + Line { + SrcBlock "Bicycle" + SrcPort 2 + Points [25, 0; 0, -5] + Branch { + DstBlock "Mux3" + DstPort 2 + } + Branch { + Points [0, -60] + DstBlock "XY" + DstPort 2 + } + } + Line { + SrcBlock "Circle" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "Sum" + DstPort 2 + } + Branch { + Points [0, 50] + DstBlock "goal" + DstPort 1 + } + } + Line { + SrcBlock "Mux3" + SrcPort 1 + Points [25, 0; 0, -110] + Branch { + Points [-575, 0] + DstBlock "Sum" + DstPort 1 + } + Branch { + DstBlock "xy" + DstPort 1 + } + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "throttle" + DstPort 1 + } + Branch { + Points [0, 85] + Branch { + DstBlock "steering" + DstPort 1 + } + Branch { + Points [0, 90] + DstBlock "Fcn" + DstPort 1 + } + } + } + Line { + SrcBlock "throttle" + SrcPort 1 + Points [45, 0; 0, 40] + DstBlock "PID Controller" + DstPort 1 + } + Line { + SrcBlock "steering" + SrcPort 1 + DstBlock "angdiff" + DstPort 1 + } + Line { + SrcBlock "Bicycle" + SrcPort 3 + Points [25, 0; 0, 20] + Branch { + Points [30, 0] + Branch { + DstBlock "theta " + DstPort 1 + } + Branch { + Points [0, 45] + DstBlock "theta" + DstPort 1 + } + } + Branch { + Points [0, 25; -290, 0] + DstBlock "angdiff" + DstPort 2 + } + } + Line { + SrcBlock "angdiff" + SrcPort 1 + DstBlock "Kh" + DstPort 1 + } + Line { + SrcBlock "Kh" + SrcPort 1 + Points [15, 0] + Branch { + Points [0, 70] + DstBlock "steer angle" + DstPort 1 + } + Branch { + DstBlock "Bicycle" + DstPort 2 + } + } + Line { + SrcBlock "PID Controller" + SrcPort 1 + Points [20, 0] + Branch { + Points [0, -35] + DstBlock "velocity" + DstPort 1 + } + Branch { + DstBlock "v" + DstPort 1 + } + Branch { + Points [60, 0] + DstBlock "Bicycle" + DstPort 1 + } + } + Line { + SrcBlock "Fcn" + SrcPort 1 + DstBlock "Scope" + DstPort 1 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_quadcopter.mdl b/lwrserv/matlab/rvctools/simulink/sl_quadcopter.mdl new file mode 100644 index 0000000..65e7ae4 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_quadcopter.mdl @@ -0,0 +1,683 @@ +Model { + Name "sl_quadcopter" + Version 7.9 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.1" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + Created "Sat Oct 20 16:06:10 2012" + Creator "corkep" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sat Oct 20 16:07:09 2012" + RTWModifiedTimeStamp 272650002 + ModelVersionFormat "1.%" + ConfigurationManager "None" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "disabled" + WideLines off + ShowLineDimensions off + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + Object { + $PropName "DataLoggingOverride" + $ObjectID 1 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "sl_quadcopter" + signals_ [] + overrideMode_ [0U] + Array { + Type "Cell" + Dimension 1 + Cell "sl_quadcopter" + PropName "logAsSpecifiedByModels_" + } + Array { + Type "Cell" + Dimension 1 + Cell [] + PropName "logAsSpecifiedByModelsSSIDs_" + } + } + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 2 + Version "1.12.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 3 + Version "1.12.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 4 + Version "1.12.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 5 + Version "1.12.0" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 6 + Version "1.12.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Enable All" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorLevel1" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 7 + Version "1.12.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 8 + Version "1.12.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 9 + Version "1.12.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 10 + Version "1.12.0" + Array { + Type "Cell" + Dimension 9 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + Cell "GenerateWebview" + Cell "GenerateCodeMetricsReport" + Cell "GenerateCodeReplacementReport" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 11 + Version "1.12.0" + Array { + Type "Cell" + Dimension 21 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "InsertPolySpaceComments" + Cell "SFDataObjDesc" + Cell "MATLABFcnDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 12 + Version "1.12.0" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + Cell "PortableWordSizes" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + CodeExecutionProfiling off + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns on + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 840, 405, 1720, 1035 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 2 + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + } + System { + Name "sl_quadcopter" + Location [1354, 285, 1934, 545] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "1" + Annotation { + SID "1" + Name "Please use sl_quadrotor instead" + Position [224, 133] + FontSize 24 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_quadcopter_vs.mdl b/lwrserv/matlab/rvctools/simulink/sl_quadcopter_vs.mdl new file mode 100644 index 0000000..8ba71b4 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_quadcopter_vs.mdl @@ -0,0 +1,682 @@ +Model { + Name "sl_quadcopter_vs" + Version 7.9 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.2" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + Created "Sat Oct 20 16:06:10 2012" + Creator "corkep" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sat Oct 20 16:08:29 2012" + RTWModifiedTimeStamp 272650100 + ModelVersionFormat "1.%" + ConfigurationManager "None" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "disabled" + WideLines off + ShowLineDimensions off + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + Object { + $PropName "DataLoggingOverride" + $ObjectID 1 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "sl_quadcopter" + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "sl_quadcopter" + PropName "logAsSpecifiedByModels_" + } + Array { + Type "Cell" + Dimension 1 + Cell [] + PropName "logAsSpecifiedByModelsSSIDs_" + } + } + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 2 + Version "1.12.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 3 + Version "1.12.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 4 + Version "1.12.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 5 + Version "1.12.0" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 6 + Version "1.12.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Enable All" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorLevel1" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 7 + Version "1.12.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 8 + Version "1.12.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 9 + Version "1.12.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 10 + Version "1.12.0" + Array { + Type "Cell" + Dimension 9 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + Cell "GenerateWebview" + Cell "GenerateCodeMetricsReport" + Cell "GenerateCodeReplacementReport" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 11 + Version "1.12.0" + Array { + Type "Cell" + Dimension 21 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "InsertPolySpaceComments" + Cell "SFDataObjDesc" + Cell "MATLABFcnDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 12 + Version "1.12.0" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + Cell "PortableWordSizes" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + CodeExecutionProfiling off + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns on + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 840, 405, 1720, 1035 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 2 + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + } + System { + Name "sl_quadcopter_vs" + Location [1354, 307, 1934, 567] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "1" + Annotation { + SID "1" + Name "Please use sl_quadrotor_vs instead" + Position [224, 133] + FontSize 24 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_quadrotor.mdl b/lwrserv/matlab/rvctools/simulink/sl_quadrotor.mdl new file mode 100644 index 0000000..5345e6c --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_quadrotor.mdl @@ -0,0 +1,1910 @@ +Model { + Name "sl_quadrotor" + Version 8.0 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.23" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + InitFcn "% load parameters of the default quadrotor\ndisp('Quadrotor parameters loaded by InitFcn')\nmdl_quadcopt" + "er" + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [363.0, 44.0, 1013.0, 745.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [0] + DockPosition "Left" + Width [50] + Height [50] + Filter [8] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Object { + $PropName "EditorsInfo" + $ObjectID 5 + $ClassName "Simulink.EditorInfo" + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [979.0, 571.0] + ZoomFactor [1.0] + Offset [0.0, 0.0] + } + } + } + Created "Sat Mar 20 12:13:00 2010" + Creator "corkep" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sat Jan 12 13:37:18 2013" + RTWModifiedTimeStamp 270302600 + ModelVersionFormat "1.%" + ConfigurationManager "None" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + Object { + $PropName "DataLoggingOverride" + $ObjectID 6 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "sl_quadrotor" + Array { + Type "Cell" + Dimension 1 + Cell "sl_quadrotor" + PropName "logAsSpecifiedByModels_" + } + Array { + Type "Cell" + Dimension 1 + Cell "" + PropName "logAsSpecifiedByModelsSSIDs_" + } + } + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 7 + Version "1.12.1" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 8 + Version "1.12.1" + StartTime "0.0" + StopTime "20" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "0.1" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 9 + Version "1.12.1" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 10 + Version "1.12.1" + Array { + Type "Cell" + Dimension 7 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 11 + Version "1.12.1" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "UseLocalSettings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + SFUndirectedBroadcastEventsDiag "warning" + SFTransitionActionBeforeConditionDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 12 + Version "1.12.1" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 13 + Version "1.12.1" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 14 + Version "1.12.1" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 15 + Version "1.12.1" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + PortableWordSizes off + GenerateErtSFunction off + CreateSILPILBlock "None" + CodeExecutionProfiling off + CodeExecutionProfileVariable "executionProfile" + CodeProfilingSaveOptions "SummaryOnly" + CodeProfilingInstrumentation off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 16 + Version "1.12.1" + Array { + Type "Cell" + Dimension 19 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + OperatorAnnotations off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InternalIdentifier "Classic" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 17 + Version "1.12.1" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface on + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 1496, 110, 2376, 740 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 7 + } + Object { + $PropName "DataTransfer" + $ObjectID 18 + $ClassName "Simulink.GlobalDataTransfer" + DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" + DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" + DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" + DefaultExtrapolationMethodBetweenContTasks "None" + AutoInsertRateTranBlk [0] + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + MaskDefaults { + SelfModifiable "off" + IconFrame "on" + IconOpaque "on" + RunInitForIconRedraw "off" + IconRotate "none" + PortRotate "default" + IconUnits "autoscale" + } + MaskParameterDefaults { + Evaluate "on" + Tunable "on" + NeverSave "off" + Internal "off" + ReadOnly "off" + Enabled "on" + Visible "on" + ToolTip "on" + } + BlockParameterDefaults { + Block { + BlockType BusSelector + OutputSignals "signal1,signal2,signal3" + OutputAsBus off + } + Block { + BlockType Clock + DisplayTime off + Decimation "10" + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Demux + Outputs "4" + DisplayOption "none" + BusSelectionMode off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType MATLABFcn + MATLABFcn "sin" + OutputDimensions "-1" + OutputSignalType "auto" + Output1D on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Product + Inputs "2" + Multiplication "Element-wise(.*)" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Zero" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + DataFormat "Array" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType Sin + SineType "Time based" + TimeSource "Use simulation time" + Amplitude "1" + Bias "0" + Frequency "1" + Phase "0" + Samples "10" + Offset "0" + SampleTime "-1" + VectorParams1D on + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType ToWorkspace + VariableName "simulink_output" + MaxDataPoints "1000" + Decimation "1" + SampleTime "0" + SaveFormat "Array" + FixptAsFi off + NumInputs "1" + } + } + System { + Name "sl_quadrotor" + Location [363, 44, 1376, 789] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "105" + Block { + BlockType BusSelector + Name "Bus\nSelector1" + SID "1" + Ports [1, 1] + Position [125, 71, 130, 109] + ZOrder -1 + ForegroundColor "orange" + ShowName off + OutputSignals "X,Y" + OutputAsBus on + Port { + PortNumber 1 + Name "xy" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector2" + SID "2" + Ports [1, 1] + Position [125, 121, 130, 159] + ZOrder -2 + ForegroundColor "orange" + ShowName off + OutputSignals "dx,dy" + OutputAsBus on + Port { + PortNumber 1 + Name "xy rates" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector3" + SID "3" + Ports [1, 1] + Position [435, 156, 440, 194] + ZOrder -3 + ForegroundColor "blue" + ShowName off + OutputSignals "pitch,roll" + OutputAsBus on + Port { + PortNumber 1 + Name "pitch/roll" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector4" + SID "4" + Ports [1, 1] + Position [435, 206, 440, 244] + ZOrder -4 + ForegroundColor "blue" + ShowName off + OutputSignals "dpitch,droll" + OutputAsBus on + Port { + PortNumber 1 + Name "pitch/roll rates" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector5" + SID "5" + Ports [1, 2] + Position [125, 308, 130, 377] + ZOrder -5 + ShowName off + OutputSignals "yaw,dyaw" + Port { + PortNumber 1 + Name "" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 2 + Name "" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector6" + SID "6" + Ports [1, 2] + Position [130, 438, 135, 507] + ZOrder -6 + ForegroundColor "red" + ShowName off + OutputSignals "Z,dz" + Port { + PortNumber 1 + Name "" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 2 + Name "" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Clock + Name "Clock" + SID "70" + Position [700, 409, 745, 431] + ZOrder -7 + BackgroundColor "yellow" + ShowName off + DisplayTime on + } + Block { + BlockType Reference + Name "Control Mixer" + SID "93" + Ports [4, 5] + Position [700, 45, 760, 355] + ZOrder -8 + DropShadow on + LibraryVersion "1.55" + SourceBlock "roblocks/Dynamics/Control Mixer" + SourceType "" + } + Block { + BlockType Gain + Name "D_pr" + SID "28" + Position [515, 210, 545, 240] + ZOrder -9 + ForegroundColor "blue" + Gain ".1" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "D_xy" + SID "29" + Position [185, 125, 215, 155] + ZOrder -10 + ForegroundColor "orange" + Gain "2" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "D_yaw" + SID "30" + Position [220, 345, 250, 375] + ZOrder -11 + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "D_z" + SID "31" + Position [220, 475, 250, 505] + ZOrder -12 + ForegroundColor "red" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Demux + Name "Demux" + SID "32" + Ports [1, 2] + Position [665, 61, 670, 99] + ZOrder -13 + ForegroundColor "blue" + ShowName off + Outputs "2" + DisplayOption "bar" + } + Block { + BlockType Mux + Name "Mux" + SID "71" + Ports [3, 1] + Position [840, 410, 845, 470] + ZOrder -14 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Mux + Name "Mux1" + SID "35" + Ports [2, 1] + Position [100, 21, 105, 59] + ZOrder -15 + ShowName off + Inputs "2" + DisplayOption "bar" + Port { + PortNumber 1 + Name "xy*" + PropagatedSignals ", " + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Gain + Name "P_pr" + SID "36" + Position [605, 65, 635, 95] + ZOrder -16 + ForegroundColor "blue" + Gain "-400" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "P_xy" + SID "37" + Position [390, 65, 420, 95] + ZOrder -17 + ForegroundColor "orange" + Gain "0.1*[-1, 1]" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "pitch/roll\ndmd" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Gain + Name "P_yaw" + SID "38" + Position [340, 310, 370, 340] + ZOrder -18 + Gain "-100" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "P_z" + SID "39" + Position [340, 440, 370, 470] + ZOrder -19 + ForegroundColor "red" + Gain "-40" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Product + Name "Product" + SID "89" + Ports [2, 1] + Position [275, 54, 305, 101] + ZOrder -20 + ForegroundColor "green" + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "Quadrotor" + SID "85" + Ports [4, 1] + Position [810, 47, 870, 293] + ZOrder -21 + DropShadow on + LibraryVersion "1.55" + SourceBlock "roblocks/Dynamics/Quadrotor" + SourceType "" + quad "quad" + } + Block { + BlockType Reference + Name "Quadrotor plot" + SID "91" + Ports [1] + Position [650, 422, 680, 468] + ZOrder -22 + DropShadow on + LibraryVersion "1.55" + SourceBlock "roblocks/Robot Graphics/Quadrotor plot" + SourceType "" + x "0" + } + Block { + BlockType MATLABFcn + Name "RZ^T" + SID "78" + Ports [1, 1] + Position [160, 235, 220, 265] + ZOrder -23 + MATLABFcn "inv([cos(u) -sin(u); sin(u) cos(u)])" + OutputDimensions "[2 2]" + Output1D off + } + Block { + BlockType Scope + Name "Scope" + SID "79" + Ports [1] + Position [285, 379, 315, 411] + ZOrder -24 + Floating off + Location [73, 568, 397, 807] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + YMin "-0.04" + YMax "0.045" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Sum + Name "Sum1" + SID "54" + Ports [2, 1] + Position [220, 315, 240, 335] + ZOrder -25 + ShowName off + IconShape "round" + Inputs "+-|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum15" + SID "55" + Ports [2, 1] + Position [275, 445, 295, 465] + ZOrder -26 + ForegroundColor "red" + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum16" + SID "56" + Ports [2, 1] + Position [560, 70, 580, 90] + ZOrder -27 + ForegroundColor "blue" + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum17" + SID "57" + Ports [2, 1] + Position [185, 80, 205, 100] + ZOrder -28 + ForegroundColor "orange" + ShowName off + IconShape "round" + Inputs "+-|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "xy error" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Sum + Name "Sum18" + SID "58" + Ports [2, 1] + Position [345, 70, 365, 90] + ZOrder -29 + ForegroundColor "orange" + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum19" + SID "59" + Ports [2, 1] + Position [485, 70, 505, 90] + ZOrder -30 + ForegroundColor "blue" + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "pitch/roll\nerror" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Sum + Name "Sum2" + SID "60" + Ports [2, 1] + Position [265, 315, 285, 335] + ZOrder -31 + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum3" + SID "61" + Ports [2, 1] + Position [220, 445, 240, 465] + ZOrder -32 + ForegroundColor "red" + ShowName off + IconShape "round" + Inputs "+-|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum4" + SID "62" + Ports [2, 1] + Position [415, 445, 435, 465] + ZOrder -33 + ForegroundColor "red" + DropShadow on + ShowName off + IconShape "round" + Inputs "++|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "69" + Ports [1] + Position [870, 425, 920, 455] + ZOrder -34 + ShowName off + VariableName "result" + MaxDataPoints "inf" + SampleTime "-1" + } + Block { + BlockType ToWorkspace + Name "To Workspace1" + SID "72" + Ports [1] + Position [535, 465, 595, 495] + ZOrder -35 + ShowName off + VariableName "T" + MaxDataPoints "inf" + SampleTime "-1" + } + Block { + BlockType Constant + Name "w0" + SID "63" + Position [380, 380, 410, 410] + ZOrder -36 + ForegroundColor "red" + DropShadow on + Value "sqrt(quad.M*quad.g/4/quad.b)" + SampleTime "0" + } + Block { + BlockType Sin + Name "x*" + SID "64" + Ports [0, 1] + Position [25, 15, 55, 45] + ZOrder -37 + Frequency "2*pi*.125" + Phase "-pi/2" + SampleTime "0" + } + Block { + BlockType Sin + Name "y*" + SID "65" + Ports [0, 1] + Position [25, 65, 55, 95] + ZOrder -38 + Frequency "2*pi*.125" + SampleTime "0" + } + Block { + BlockType Reference + Name "yaw*" + SID "84" + Ports [0, 1] + Position [25, 280, 55, 310] + ZOrder -39 + LibraryVersion "1.281" + SourceBlock "simulink/Sources/Ramp" + SourceType "Ramp" + slope "0.2" + start "2" + X0 "0" + VectorParams1D on + } + Block { + BlockType Constant + Name "z*" + SID "68" + Position [25, 405, 55, 435] + ZOrder -40 + Value "-4" + SampleTime "0" + } + Line { + SrcBlock "Control Mixer" + SrcPort 1 + DstBlock "Quadrotor" + DstPort 1 + } + Line { + SrcBlock "Control Mixer" + SrcPort 4 + DstBlock "Quadrotor" + DstPort 4 + } + Line { + SrcBlock "w0" + SrcPort 1 + Points [10, 0] + DstBlock "Sum4" + DstPort 1 + } + Line { + SrcBlock "Sum15" + SrcPort 1 + DstBlock "P_z" + DstPort 1 + } + Line { + SrcBlock "P_z" + SrcPort 1 + DstBlock "Sum4" + DstPort 2 + } + Line { + SrcBlock "Quadrotor" + SrcPort 1 + Points [55, 0; 0, 365; -130, 0] + Branch { + Points [-180, 0] + Branch { + Points [-530, 0; 0, -60] + Branch { + DstBlock "Bus\nSelector6" + DstPort 1 + } + Branch { + Points [0, -130] + Branch { + Points [0, -120] + Branch { + DstBlock "Bus\nSelector4" + DstPort 1 + } + Branch { + Points [0, -50] + Branch { + DstBlock "Bus\nSelector3" + DstPort 1 + } + Branch { + Points [0, -35] + Branch { + Points [0, -50] + DstBlock "Bus\nSelector1" + DstPort 1 + } + Branch { + DstBlock "Bus\nSelector2" + DstPort 1 + } + } + } + } + Branch { + DstBlock "Bus\nSelector5" + DstPort 1 + } + } + } + Branch { + Points [0, -90] + DstBlock "Quadrotor plot" + DstPort 1 + } + } + Branch { + Points [0, -95] + DstBlock "Mux" + DstPort 2 + } + } + Line { + SrcBlock "x*" + SrcPort 1 + DstBlock "Mux1" + DstPort 1 + } + Line { + SrcBlock "y*" + SrcPort 1 + Points [5, 0; 0, -30] + DstBlock "Mux1" + DstPort 2 + } + Line { + Name "xy" + Labels [0, 0] + SrcBlock "Bus\nSelector1" + SrcPort 1 + DstBlock "Sum17" + DstPort 2 + } + Line { + Name "xy*" + Labels [0, 0] + SrcBlock "Mux1" + SrcPort 1 + Points [85, 0] + DstBlock "Sum17" + DstPort 1 + } + Line { + Name "xy rates" + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 1 + DstBlock "D_xy" + DstPort 1 + } + Line { + SrcBlock "D_xy" + SrcPort 1 + Points [135, 0] + DstBlock "Sum18" + DstPort 2 + } + Line { + Name "pitch/roll\ndmd" + Labels [0, 0] + SrcBlock "P_xy" + SrcPort 1 + DstBlock "Sum19" + DstPort 1 + } + Line { + Name "pitch/roll" + Labels [0, 0] + SrcBlock "Bus\nSelector3" + SrcPort 1 + Points [50, 0] + DstBlock "Sum19" + DstPort 2 + } + Line { + Name "pitch/roll rates" + Labels [0, 0] + SrcBlock "Bus\nSelector4" + SrcPort 1 + DstBlock "D_pr" + DstPort 1 + } + Line { + Name "pitch/roll\nerror" + Labels [0, 0] + SrcBlock "Sum19" + SrcPort 1 + DstBlock "Sum16" + DstPort 1 + } + Line { + SrcBlock "Sum16" + SrcPort 1 + DstBlock "P_pr" + DstPort 1 + } + Line { + SrcBlock "P_pr" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "P_yaw" + SrcPort 1 + Points [270, 0; 0, -85] + DstBlock "Control Mixer" + DstPort 3 + } + Line { + Name "" + Labels [0, 0] + SrcBlock "Bus\nSelector5" + SrcPort 1 + Points [10, 0] + Branch { + DstBlock "RZ^T" + DstPort 1 + } + Branch { + Points [15, 0] + Branch { + DstBlock "Sum1" + DstPort 2 + } + Branch { + Points [0, 70] + DstBlock "Scope" + DstPort 1 + } + } + } + Line { + SrcBlock "Sum2" + SrcPort 1 + DstBlock "P_yaw" + DstPort 1 + } + Line { + Name "" + Labels [0, 0] + SrcBlock "Bus\nSelector6" + SrcPort 1 + DstBlock "Sum3" + DstPort 2 + } + Line { + SrcBlock "z*" + SrcPort 1 + Points [170, 0] + DstBlock "Sum3" + DstPort 1 + } + Line { + Name "" + Labels [0, 0] + SrcBlock "Bus\nSelector5" + SrcPort 2 + DstBlock "D_yaw" + DstPort 1 + } + Line { + SrcBlock "Sum3" + SrcPort 1 + DstBlock "Sum15" + DstPort 1 + } + Line { + Name "" + Labels [0, 0] + SrcBlock "Bus\nSelector6" + SrcPort 2 + DstBlock "D_z" + DstPort 1 + } + Line { + SrcBlock "D_z" + SrcPort 1 + Points [30, 0] + DstBlock "Sum15" + DstPort 2 + } + Line { + SrcBlock "D_pr" + SrcPort 1 + Points [20, 0] + DstBlock "Sum16" + DstPort 2 + } + Line { + SrcBlock "Demux" + SrcPort 2 + Points [0, 75] + DstBlock "Control Mixer" + DstPort 2 + } + Line { + SrcBlock "D_yaw" + SrcPort 1 + Points [20, 0] + DstBlock "Sum2" + DstPort 2 + } + Line { + SrcBlock "Sum4" + SrcPort 1 + Points [75, 0] + Branch { + Points [0, 25] + DstBlock "To Workspace1" + DstPort 1 + } + Branch { + Labels [3, 0] + Points [65, 0; 0, -110; 90, 0; 0, -30] + DstBlock "Control Mixer" + DstPort 4 + } + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "To Workspace" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + Labels [2, 0] + SrcBlock "Control Mixer" + SrcPort 5 + Points [20, 0; 0, 140] + DstBlock "Mux" + DstPort 3 + } + Line { + SrcBlock "Sum18" + SrcPort 1 + DstBlock "P_xy" + DstPort 1 + } + Line { + SrcBlock "yaw*" + SrcPort 1 + Points [170, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "Product" + SrcPort 1 + DstBlock "Sum18" + DstPort 1 + } + Line { + Name "xy error" + Labels [0, 0] + SrcBlock "Sum17" + SrcPort 1 + DstBlock "Product" + DstPort 2 + } + Line { + SrcBlock "RZ^T" + SrcPort 1 + Points [25, 0; 0, -185] + DstBlock "Product" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 1 + Points [5, 0; 0, 20] + DstBlock "Control Mixer" + DstPort 1 + } + Line { + SrcBlock "Control Mixer" + SrcPort 3 + DstBlock "Quadrotor" + DstPort 3 + } + Line { + SrcBlock "Control Mixer" + SrcPort 2 + DstBlock "Quadrotor" + DstPort 2 + } + Annotation { + SID "94" + Name " " + Position [470, 40] + } + Annotation { + SID "95" + Name "state vector" + Position [446, 523] + } + Annotation { + SID "96" + Name "gravity feedforward" + Position [442, 475] + } + Annotation { + SID "97" + Name "attitude rate\nloop" + Position [582, 135] + HorizontalAlignment "left" + } + Annotation { + SID "98" + Name "attitude\nloop" + Position [504, 135] + HorizontalAlignment "left" + } + Annotation { + SID "99" + Name "position rate\nloop" + Position [367, 130] + HorizontalAlignment "left" + } + Annotation { + SID "100" + Name "position\nloop" + Position [242, 50] + HorizontalAlignment "left" + } + Annotation { + SID "101" + Name "yaw rate\nloop" + Position [287, 365] + HorizontalAlignment "left" + } + Annotation { + SID "102" + Name "yaw angle\nloop" + Position [242, 300] + HorizontalAlignment "left" + } + Annotation { + SID "103" + Name "height rate\nloop" + Position [292, 470] + HorizontalAlignment "left" + } + Annotation { + SID "104" + Name "height \nloop" + Position [242, 430] + HorizontalAlignment "left" + } + Annotation { + SID "105" + Name "state" + Position [938, 140] + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_quadrotor_vs.mdl b/lwrserv/matlab/rvctools/simulink/sl_quadrotor_vs.mdl new file mode 100644 index 0000000..350071c --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_quadrotor_vs.mdl @@ -0,0 +1,1990 @@ +Model { + Name "sl_quadcopter_vs" + Version 7.7 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.13" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + InitFcn "fprintf('----- creating workspace variables: camera, quad, P, pstar\\n');\n% create a spherical camera\n" + "camera = SphericalCamera('name', 'quad-vs');\n\n% create a grid of points on the ground\nP = mkgrid(2, 4);\n\n% set " + "the desired view of these points, defines the final pose\n% of the quad\npstar = camera.project(P, 'Tcam', transl(0," + " 0, -5)*trotz(pi/2));\n\n% load a dynamic model of the quadrotor\nmdl_quadcopter" + Created "Thu Nov 4 20:52:52 2010" + Creator "corkep" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Wed Apr 25 21:17:41 2012" + RTWModifiedTimeStamp 257288787 + ModelVersionFormat "1.%" + ConfigurationManager "None" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.11.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.11.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.11.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.11.0" + Array { + Type "Cell" + Dimension 7 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + AccelParallelForEachSubsystem on + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.11.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorLevel1" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.11.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.11.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.11.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.11.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + GenerateCodeMetricsReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.11.0" + Array { + Type "Cell" + Dimension 19 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.11.0" + Array { + Type "Cell" + Dimension 16 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + FunctionExecutionProfile off + CodeExecutionProfiling off + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 920, 197, 1800, 827 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType BusSelector + OutputAsBus off + } + Block { + BlockType Clock + DisplayTime off + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Demux + Outputs "4" + DisplayOption "none" + BusSelectionMode off + } + Block { + BlockType Display + Format "short" + Decimation "10" + Floating off + SampleTime "-1" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType MATLABFcn + MATLABFcn "sin" + OutputDimensions "-1" + OutputSignalType "auto" + Output1D on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType ToWorkspace + VariableName "simulink_output" + MaxDataPoints "1000" + Decimation "1" + SampleTime "0" + FixptAsFi off + NumInputs "1" + } + } + System { + Name "sl_quadcopter_vs" + Location [685, 189, 1715, 1039] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "328" + Block { + BlockType MATLABFcn + Name "-> SE3" + SID "162" + Ports [1, 1] + Position [285, 545, 345, 575] + MATLABFcn "transl(u(1:3))*rpy2tr(u(4:6)')" + OutputDimensions "[4 4]" + Output1D off + } + Block { + BlockType BusSelector + Name "Bus\nSelector1" + SID "172" + Ports [1, 1] + Position [165, 591, 170, 639] + ShowName off + OutputSignals "Z" + OutputAsBus on + } + Block { + BlockType BusSelector + Name "Bus\nSelector2" + SID "246" + Ports [1, 1] + Position [195, 56, 200, 94] + ForegroundColor "orange" + ShowName off + OutputSignals "X,Y" + OutputAsBus on + Port { + PortNumber 1 + Name "xy" + PropagatedSignals "X, Y" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector3" + SID "247" + Ports [1, 1] + Position [195, 116, 200, 154] + ForegroundColor "orange" + ShowName off + OutputSignals "dx,dy" + OutputAsBus on + Port { + PortNumber 1 + Name "xy rates" + PropagatedSignals "dx, dy" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector4" + SID "248" + Ports [1, 1] + Position [405, 151, 410, 189] + ForegroundColor "blue" + ShowName off + OutputSignals "pitch,roll" + OutputAsBus on + Port { + PortNumber 1 + Name "pitch/roll" + PropagatedSignals "pitch, roll" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector5" + SID "249" + Ports [1, 1] + Position [405, 201, 410, 239] + ForegroundColor "blue" + ShowName off + OutputSignals "dpitch,droll" + OutputAsBus on + Port { + PortNumber 1 + Name "pitch/roll rates" + PropagatedSignals "dpitch, droll" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector6" + SID "250" + Ports [1, 1] + Position [195, 293, 200, 337] + ShowName off + OutputSignals "dyaw" + Port { + PortNumber 1 + Name "" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector7" + SID "161" + Ports [1, 1] + Position [165, 536, 170, 584] + ShowName off + OutputSignals "X,Y,Z,roll,pitch,yaw" + OutputAsBus on + Port { + PortNumber 1 + PropagatedSignals "X, Y, Z, roll, pitch, yaw" + ShowPropagatedSignals "on" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType BusSelector + Name "Bus\nSelector8" + SID "251" + Ports [1, 1] + Position [200, 398, 205, 452] + ForegroundColor "red" + ShowName off + OutputSignals "dz" + Port { + PortNumber 1 + Name "" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Scope + Name "Cartesian velocity" + SID "144" + Ports [1] + Position [260, 674, 290, 706] + Floating off + Location [1808, 549, 2594, 996] + Open off + NumInputPorts "1" + ZoomMode "yonly" + List { + ListType AxesTitles + axes1 "%" + } + TimeRange "20" + YMin "-0.4" + YMax "1.9" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Display + Name "Cartesian velocity dmd" + SID "145" + Ports [1] + Position [55, 650, 140, 730] + BlockMirror on + Format "short_e" + Decimation "1" + Lockdown off + } + Block { + BlockType Clock + Name "Clock" + SID "252" + Position [710, 399, 755, 421] + BackgroundColor "yellow" + ShowName off + DisplayTime on + Decimation "10" + } + Block { + BlockType Reference + Name "Control Mixer" + SID "324" + Ports [4, 5] + Position [680, 18, 755, 362] + DropShadow on + LibraryVersion "1.46" + SourceBlock "roblocks/Dynamics/Control Mixer" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + } + Block { + BlockType Gain + Name "D_pr" + SID "276" + Position [485, 205, 515, 235] + ForegroundColor "blue" + Gain ".1" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "D_xy" + SID "277" + Position [275, 120, 305, 150] + ForegroundColor "orange" + Gain "2" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "D_yaw" + SID "278" + Position [250, 300, 280, 330] + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "D_z" + SID "279" + Position [250, 410, 280, 440] + ForegroundColor "red" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Demux + Name "Demux" + SID "280" + Ports [1, 2] + Position [635, 56, 640, 94] + ForegroundColor "blue" + ShowName off + Outputs "2" + DisplayOption "bar" + } + Block { + BlockType Demux + Name "Demux1" + SID "165" + Ports [1, 4] + Position [70, 16, 75, 109] + ShowName off + DisplayOption "bar" + Port { + PortNumber 1 + Name "vx" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 2 + Name "vy" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 3 + Name "vz" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + Port { + PortNumber 4 + Name "wz" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Gain + Name "Gain" + SID "169" + Position [815, 635, 845, 665] + BlockRotation 270 + BlockMirror on + NamePlacement "alternate" + Gain "-0.3" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType MATLABFcn + Name "Jxyz" + SID "146" + Ports [1, 1] + Position [645, 545, 705, 575] + MATLABFcn "u(:,[1 2 3 6])" + Output1D off + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn" + SID "147" + Ports [1, 1] + Position [530, 645, 590, 675] + BackgroundColor "lightBlue" + ShowName off + MATLABFcn "angdiff( u(:) )" + Output1D off + Port { + PortNumber 1 + Name "feature error" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType MATLABFcn + Name "MATLAB Fcn1" + SID "148" + Ports [1, 1] + Position [705, 695, 765, 725] + ShowName off + MATLABFcn "norm(u)" + } + Block { + BlockType Mux + Name "Mux" + SID "283" + Ports [3, 1] + Position [840, 400, 845, 460] + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Mux + Name "Mux2" + SID "166" + Ports [2, 1] + Position [115, 13, 120, 62] + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Gain + Name "P_pr" + SID "285" + Position [575, 60, 605, 90] + ForegroundColor "blue" + Gain "-400" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "P_xy" + SID "286" + Position [380, 60, 410, 90] + ForegroundColor "orange" + Gain "0.1*diag([-1, 1])" + Multiplication "Matrix(K*u)" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "pitch/roll\ndmd" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Gain + Name "P_yaw" + SID "287" + Position [370, 265, 400, 295] + Gain "-100" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "P_z" + SID "288" + Position [370, 375, 400, 405] + ForegroundColor "red" + Gain "-40" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "Quadrotor" + SID "323" + Ports [4, 1] + Position [840, 13, 905, 297] + DropShadow on + LibraryVersion "1.46" + SourceBlock "roblocks/Dynamics/Quadrotor" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + quad "quad" + } + Block { + BlockType Reference + Name "Quadrotor plot" + SID "326" + Ports [1] + Position [645, 402, 675, 448] + DropShadow on + LibraryVersion "1.46" + SourceBlock "roblocks/Robot Graphics/Quadrotor plot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + x "0" + } + Block { + BlockType Sum + Name "Sum" + SID "150" + Ports [2, 1] + Position [475, 650, 495, 670] + BackgroundColor "lightBlue" + ShowName off + IconShape "round" + Inputs "+-|" + } + Block { + BlockType Sum + Name "Sum15" + SID "307" + Ports [2, 1] + Position [305, 380, 325, 400] + ForegroundColor "red" + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum16" + SID "308" + Ports [2, 1] + Position [530, 65, 550, 85] + ForegroundColor "blue" + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum17" + SID "309" + Ports [2, 1] + Position [250, 65, 270, 85] + ForegroundColor "orange" + ShowName off + IconShape "round" + Inputs "+-|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "xy error" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Sum + Name "Sum18" + SID "310" + Ports [2, 1] + Position [325, 65, 345, 85] + ForegroundColor "orange" + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum19" + SID "311" + Ports [2, 1] + Position [455, 65, 475, 85] + ForegroundColor "blue" + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "pitch/roll\nerror" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Sum + Name "Sum2" + SID "312" + Ports [2, 1] + Position [295, 270, 315, 290] + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum4" + SID "314" + Ports [2, 1] + Position [445, 380, 465, 400] + ForegroundColor "red" + DropShadow on + ShowName off + IconShape "round" + Inputs "++|" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "315" + Ports [1] + Position [870, 415, 930, 445] + ShowName off + VariableName "quad" + MaxDataPoints "inf" + SampleTime "-1" + SaveFormat "Array" + } + Block { + BlockType Display + Name "Z" + SID "171" + Ports [1] + Position [210, 600, 300, 630] + BackgroundColor "yellow" + Decimation "1" + Lockdown off + } + Block { + BlockType Reference + Name "camera" + SID "151" + Ports [1, 1] + Position [385, 521, 465, 599] + BackgroundColor "lightBlue" + DropShadow on + LibraryVersion "1.46" + SourceBlock "roblocks/Vision/camera" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + cam "camera" + points "P" + } + Block { + BlockType Constant + Name "desired\nimage plane\ncoordinates" + SID "152" + Position [320, 634, 435, 686] + BackgroundColor "green" + Value "pstar" + VectorParams1D off + } + Block { + BlockType Display + Name "feature error" + SID "153" + Ports [1] + Position [490, 716, 580, 814] + BlockMirror on + Format "bank" + Decimation "1" + Lockdown off + } + Block { + BlockType Display + Name "feature error\nnorm" + SID "154" + Ports [1] + Position [875, 695, 935, 725] + Decimation "1" + Lockdown off + } + Block { + BlockType Scope + Name "feature error2" + SID "155" + Ports [1] + Position [750, 749, 780, 781] + Floating off + Location [1829, 49, 2615, 496] + Open off + NumInputPorts "1" + ZoomMode "yonly" + List { + ListType AxesTitles + axes1 "%" + } + TimeRange "20" + YMin "-150" + YMax "150" + SaveName "ScopeData1" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Reference + Name "image\nJacobian" + SID "327" + Ports [1, 1] + Position [535, 534, 605, 586] + BackgroundColor "lightBlue" + DropShadow on + LibraryVersion "1.46" + SourceBlock "roblocks/Vision/image\nJacobian" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + cam "camera" + z "1" + } + Block { + BlockType Reference + Name "pinvJac" + SID "328" + Ports [2, 2] + Position [740, 545, 780, 605] + BackgroundColor "red" + DropShadow on + LibraryVersion "1.46" + SourceBlock "roblocks/Vision/pinvJac" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + } + Block { + BlockType Display + Name "visjac\ncondition" + SID "157" + Ports [1] + Position [875, 575, 935, 605] + Decimation "1" + Lockdown off + } + Block { + BlockType Constant + Name "weight" + SID "317" + Position [410, 315, 440, 345] + ForegroundColor "red" + DropShadow on + Value "861" + SampleTime "0" + } + Line { + SrcBlock "camera" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "Sum" + DstPort 1 + } + Branch { + DstBlock "image\nJacobian" + DstPort 1 + } + } + Line { + SrcBlock "Sum" + SrcPort 1 + DstBlock "MATLAB Fcn" + DstPort 1 + } + Line { + SrcBlock "desired\nimage plane\ncoordinates" + SrcPort 1 + DstBlock "Sum" + DstPort 2 + } + Line { + Name "feature error" + Labels [0, 0] + SrcBlock "MATLAB Fcn" + SrcPort 1 + Points [50, 0] + Branch { + Points [0, 50] + Branch { + DstBlock "MATLAB Fcn1" + DstPort 1 + } + Branch { + Points [0, 0; 0, 55] + Branch { + Labels [1, 0] + DstBlock "feature error2" + DstPort 1 + } + Branch { + DstBlock "feature error" + DstPort 1 + } + } + } + Branch { + Labels [1, 1] + Points [0, -70] + DstBlock "pinvJac" + DstPort 2 + } + } + Line { + Labels [0, 0] + SrcBlock "image\nJacobian" + SrcPort 1 + DstBlock "Jxyz" + DstPort 1 + } + Line { + Labels [0, 0] + SrcBlock "Jxyz" + SrcPort 1 + DstBlock "pinvJac" + DstPort 1 + } + Line { + SrcBlock "-> SE3" + SrcPort 1 + DstBlock "camera" + DstPort 1 + } + Line { + Labels [0, 0] + SrcBlock "Bus\nSelector7" + SrcPort 1 + DstBlock "-> SE3" + DstPort 1 + } + Line { + SrcBlock "MATLAB Fcn1" + SrcPort 1 + DstBlock "feature error\nnorm" + DstPort 1 + } + Line { + Name "vx" + Labels [0, 0] + SrcBlock "Demux1" + SrcPort 1 + DstBlock "Mux2" + DstPort 1 + } + Line { + Name "vy" + Labels [0, 0] + SrcBlock "Demux1" + SrcPort 2 + DstBlock "Mux2" + DstPort 2 + } + Line { + SrcBlock "Gain" + SrcPort 1 + Points [0, 160; -635, 0] + Branch { + Points [0, -140] + Branch { + DstBlock "Cartesian velocity dmd" + DstPort 1 + } + Branch { + DstBlock "Cartesian velocity" + DstPort 1 + } + } + Branch { + Points [-170, 0; 0, -765] + DstBlock "Demux1" + DstPort 1 + } + } + Line { + SrcBlock "Bus\nSelector1" + SrcPort 1 + DstBlock "Z" + DstPort 1 + } + Line { + SrcBlock "Control Mixer" + SrcPort 1 + DstBlock "Quadrotor" + DstPort 1 + } + Line { + SrcBlock "Control Mixer" + SrcPort 3 + DstBlock "Quadrotor" + DstPort 3 + } + Line { + SrcBlock "Control Mixer" + SrcPort 4 + DstBlock "Quadrotor" + DstPort 4 + } + Line { + SrcBlock "weight" + SrcPort 1 + Points [10, 0] + DstBlock "Sum4" + DstPort 1 + } + Line { + SrcBlock "Sum15" + SrcPort 1 + DstBlock "P_z" + DstPort 1 + } + Line { + SrcBlock "P_z" + SrcPort 1 + DstBlock "Sum4" + DstPort 2 + } + Line { + SrcBlock "Quadrotor" + SrcPort 1 + Points [30, 0; 0, 350; -220, 0; 0, -45] + Branch { + Points [0, 45; -160, 0; 0, 0] + Branch { + Points [-445, 0] + Branch { + Points [0, -80] + Branch { + DstBlock "Bus\nSelector8" + DstPort 1 + } + Branch { + Points [0, -110] + Branch { + Points [0, -95] + Branch { + DstBlock "Bus\nSelector5" + DstPort 1 + } + Branch { + Points [0, -50] + Branch { + DstBlock "Bus\nSelector4" + DstPort 1 + } + Branch { + Points [0, -35] + Branch { + Points [0, -60] + DstBlock "Bus\nSelector2" + DstPort 1 + } + Branch { + DstBlock "Bus\nSelector3" + DstPort 1 + } + } + } + } + Branch { + DstBlock "Bus\nSelector6" + DstPort 1 + } + } + } + Branch { + Points [0, 55] + Branch { + Points [0, 55] + DstBlock "Bus\nSelector1" + DstPort 1 + } + Branch { + DstBlock "Bus\nSelector7" + DstPort 1 + } + } + } + Branch { + Points [0, -80] + DstBlock "Quadrotor plot" + DstPort 1 + } + } + Branch { + Points [0, -30] + DstBlock "Mux" + DstPort 2 + } + } + Line { + Name "xy" + Labels [0, 0] + SrcBlock "Bus\nSelector2" + SrcPort 1 + DstBlock "Sum17" + DstPort 2 + } + Line { + Name "xy error" + Labels [0, 0] + SrcBlock "Sum17" + SrcPort 1 + DstBlock "Sum18" + DstPort 1 + } + Line { + Name "xy rates" + Labels [0, 0] + SrcBlock "Bus\nSelector3" + SrcPort 1 + DstBlock "D_xy" + DstPort 1 + } + Line { + SrcBlock "D_xy" + SrcPort 1 + Points [25, 0] + DstBlock "Sum18" + DstPort 2 + } + Line { + Name "pitch/roll\ndmd" + Labels [0, 0] + SrcBlock "P_xy" + SrcPort 1 + DstBlock "Sum19" + DstPort 1 + } + Line { + Name "pitch/roll" + Labels [0, 0] + SrcBlock "Bus\nSelector4" + SrcPort 1 + Points [50, 0] + DstBlock "Sum19" + DstPort 2 + } + Line { + Name "pitch/roll rates" + Labels [0, 0] + SrcBlock "Bus\nSelector5" + SrcPort 1 + DstBlock "D_pr" + DstPort 1 + } + Line { + Name "pitch/roll\nerror" + Labels [0, 0] + SrcBlock "Sum19" + SrcPort 1 + DstBlock "Sum16" + DstPort 1 + } + Line { + SrcBlock "Sum16" + SrcPort 1 + DstBlock "P_pr" + DstPort 1 + } + Line { + SrcBlock "P_pr" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "P_yaw" + SrcPort 1 + Points [210, 0; 0, -45] + DstBlock "Control Mixer" + DstPort 3 + } + Line { + SrcBlock "Sum2" + SrcPort 1 + DstBlock "P_yaw" + DstPort 1 + } + Line { + Name "" + Labels [0, 0] + SrcBlock "Bus\nSelector6" + SrcPort 1 + DstBlock "D_yaw" + DstPort 1 + } + Line { + Name "" + Labels [0, 0] + SrcBlock "Bus\nSelector8" + SrcPort 1 + DstBlock "D_z" + DstPort 1 + } + Line { + SrcBlock "D_z" + SrcPort 1 + Points [30, 0] + DstBlock "Sum15" + DstPort 2 + } + Line { + SrcBlock "D_pr" + SrcPort 1 + Points [20, 0] + DstBlock "Sum16" + DstPort 2 + } + Line { + SrcBlock "Demux" + SrcPort 2 + Points [0, 65] + DstBlock "Control Mixer" + DstPort 2 + } + Line { + SrcBlock "Demux" + SrcPort 1 + DstBlock "Control Mixer" + DstPort 1 + } + Line { + SrcBlock "D_yaw" + SrcPort 1 + Points [20, 0] + DstBlock "Sum2" + DstPort 2 + } + Line { + SrcBlock "Sum4" + SrcPort 1 + Points [135, 0; 0, -70] + DstBlock "Control Mixer" + DstPort 4 + } + Line { + SrcBlock "Control Mixer" + SrcPort 2 + DstBlock "Quadrotor" + DstPort 2 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "To Workspace" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "Control Mixer" + SrcPort 5 + Points [35, 0; 0, 120] + DstBlock "Mux" + DstPort 3 + } + Line { + SrcBlock "Sum18" + SrcPort 1 + DstBlock "P_xy" + DstPort 1 + } + Line { + SrcBlock "Mux2" + SrcPort 1 + Points [135, 0] + DstBlock "Sum17" + DstPort 1 + } + Line { + Name "vz" + Labels [0, 0] + SrcBlock "Demux1" + SrcPort 3 + Points [10, 0; 0, 315] + DstBlock "Sum15" + DstPort 1 + } + Line { + Name "wz" + Labels [0, 0] + SrcBlock "Demux1" + SrcPort 4 + Points [20, 0; 0, 180] + DstBlock "Sum2" + DstPort 1 + } + Line { + SrcBlock "pinvJac" + SrcPort 1 + Points [45, 0] + DstBlock "Gain" + DstPort 1 + } + Line { + SrcBlock "pinvJac" + SrcPort 2 + DstBlock "visjac\ncondition" + DstPort 1 + } + Annotation { + Position [70, 592] + } + Annotation { + Position [70, 592] + } + Annotation { + Position [70, 592] + } + Annotation { + Name " " + Position [440, 35] + } + Annotation { + Name "state" + Position [876, 493] + } + Annotation { + Name "gravity feedforward" + Position [472, 415] + } + Annotation { + Name "attitude rate\nloop" + Position [552, 130] + HorizontalAlignment "left" + } + Annotation { + Name "attitude\nloop" + Position [474, 130] + HorizontalAlignment "left" + } + Annotation { + Name "position rate\nloop" + Position [352, 125] + HorizontalAlignment "left" + } + Annotation { + Name "position\nloop" + Position [272, 45] + HorizontalAlignment "left" + } + Annotation { + Name "yaw rate\nloop" + Position [317, 320] + HorizontalAlignment "left" + } + Annotation { + Name "height rate\nloop" + Position [322, 435] + HorizontalAlignment "left" + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_rrmc.mdl b/lwrserv/matlab/rvctools/simulink/sl_rrmc.mdl new file mode 100644 index 0000000..81c3c4a --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_rrmc.mdl @@ -0,0 +1,912 @@ +Model { + Name "sl_rrmc" + Version 7.6 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 1 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q" + } + ParameterArgumentNames "" + ComputedModelVersion "1.15" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Thu Aug 10 00:20:35 2006" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "n6334687" + ModifiedDateFormat "%" + LastModifiedDate "Mon Feb 06 16:47:25 2012" + RTWModifiedTimeStamp 250447635 + ModelVersionFormat "1.%" + ConfigurationManager "None" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock off + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.10.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.10.0" + StartTime "0.0" + StopTime "5.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.10.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "sigsOut" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.10.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.10.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "none" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "TryResolveAllWithWarning" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "warning" + MultiTaskCondExecSysMsg "none" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + InitInArrayFormatMsg "warning" + StrictBusMsg "None" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.10.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown on + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.10.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.10.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.10.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime on + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.10.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.10.0" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Data Import//Export" + ConfigPrmDlgPosition " [ 512, 171, 1392, 801 ] " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType DiscreteIntegrator + IntegratorMethod "Integration: Forward Euler" + gainval "1.0" + ExternalReset "none" + InitialConditionSource "internal" + InitialCondition "0" + InitialConditionMode "State and output" + SampleTime "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit via internal rule" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow off + LimitOutput off + UpperSaturationLimit "inf" + LowerSaturationLimit "-inf" + ShowSaturationPort off + ShowStatePort off + IgnoreLimit off + StateMustResolveToSignalObject off + RTWStateStorageClass "Auto" + } + Block { + BlockType MATLABFcn + MATLABFcn "sin" + OutputDimensions "-1" + OutputSignalType "auto" + Output1D on + SampleTime "-1" + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Product + Inputs "2" + Multiplication "Element-wise(.*)" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Zero" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Reshape + OutputDimensionality "1-D array" + OutputDimensions "[1,1]" + } + } + System { + Name "sl_rrmc" + Location [79, 196, 772, 457] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "22" + Block { + BlockType DiscreteIntegrator + Name "Joint \nservo" + SID "3" + Ports [1, 1] + Position [435, 70, 470, 110] + IntegratorMethod "Integration: Forward Euler" + ExternalReset "none" + InitialConditionSource "internal" + InitialCondition "qn'" + SampleTime "0.05" + ICPrevOutput "DiscIntNeverNeededParam" + ICPrevScaledInput "DiscIntNeverNeededParam" + Port { + PortNumber 1 + Name "q*" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Product + Name "Product" + SID "5" + Ports [2, 1] + Position [345, 61, 390, 119] + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + RndMeth "Floor" + Port { + PortNumber 1 + Name "qd*" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Reshape + Name "convert to\nrow vector" + SID "19" + Ports [1, 1] + Position [570, 148, 610, 172] + OutputDimensionality "Customize" + OutputDimensions "6" + } + Block { + BlockType Constant + Name "desired Cartesian velocity" + SID "1" + Position [150, 142, 265, 178] + Value "[0 0.1 0 0 0 0]'" + VectorParams1D off + Port { + PortNumber 1 + Name "v*" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType MATLABFcn + Name "inv" + SID "22" + Ports [1, 1] + Position [215, 60, 275, 90] + MATLABFcn "inv" + Output1D off + Port { + PortNumber 1 + Name "invJ" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Reference + Name "jacob0\n" + SID "15" + Ports [1, 1] + Position [95, 40, 160, 110] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Kinematics/jacob0\n" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + Port { + PortNumber 1 + Name "J" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Reference + Name "plot" + SID "16" + Ports [1] + Position [580, 62, 660, 118] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Robot Graphics/plot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + } + Block { + BlockType Outport + Name "q" + SID "17" + Position [630, 153, 660, 167] + IconDisplay "Port number" + } + Line { + Name "J" + Labels [0, 0] + SrcBlock "jacob0\n" + SrcPort 1 + DstBlock "inv" + DstPort 1 + } + Line { + Name "qd*" + Labels [1, 1] + SrcBlock "Product" + SrcPort 1 + DstBlock "Joint \nservo" + DstPort 1 + } + Line { + Name "q*" + Labels [0, 0] + SrcBlock "Joint \nservo" + SrcPort 1 + Points [30, 0] + Branch { + Points [0, 120; -450, 0; 0, -135] + DstBlock "jacob0\n" + DstPort 1 + } + Branch { + Points [25, 0] + Branch { + DstBlock "plot" + DstPort 1 + } + Branch { + Points [0, 70] + DstBlock "convert to\nrow vector" + DstPort 1 + } + } + } + Line { + Name "invJ" + Labels [1, 0] + SrcBlock "inv" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + Name "v*" + Labels [3, 0] + SrcBlock "desired Cartesian velocity" + SrcPort 1 + Points [30, 0; 0, -55] + DstBlock "Product" + DstPort 2 + } + Line { + SrcBlock "convert to\nrow vector" + SrcPort 1 + DstBlock "q" + DstPort 1 + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_rrmc2.mdl b/lwrserv/matlab/rvctools/simulink/sl_rrmc2.mdl new file mode 100644 index 0000000..50b4b92 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_rrmc2.mdl @@ -0,0 +1,1306 @@ +Model { + Name "sl_rrmc2" + Version 7.6 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.20" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + PreLoadFcn "fprintf('*** Loading Puma560 model to workspace\\n');\nmdl_puma560" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Sat Jan 26 02:15:13 2002" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "n6334687" + ModifiedDateFormat "%" + LastModifiedDate "Mon Feb 06 16:49:47 2012" + RTWModifiedTimeStamp 250447782 + ModelVersionFormat "1.%" + ConfigurationManager "none" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip on + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "oneshot" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect off + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.10.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.10.0" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.10.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.10.0" + BlockReduction off + BooleanDataType off + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero on + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.10.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "none" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "TryResolveAllWithWarning" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "warning" + MultiTaskCondExecSysMsg "none" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + InitInArrayFormatMsg "warning" + StrictBusMsg "None" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.10.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown on + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.10.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.10.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.10.0" + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime on + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.10.0" + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.10.0" + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant off + ParMdlRefBuildCompliant on + CompOptLevelCompliant off + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging off + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition " [ 792, 367, 1672, 997 ] " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Clock + DisplayTime off + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Demux + Outputs "4" + DisplayOption "none" + BusSelectionMode off + } + Block { + BlockType DiscreteIntegrator + IntegratorMethod "Integration: Forward Euler" + gainval "1.0" + ExternalReset "none" + InitialConditionSource "internal" + InitialCondition "0" + InitialConditionMode "State and output" + SampleTime "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit via internal rule" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow off + LimitOutput off + UpperSaturationLimit "inf" + LowerSaturationLimit "-inf" + ShowSaturationPort off + ShowStatePort off + IgnoreLimit off + StateMustResolveToSignalObject off + RTWStateStorageClass "Auto" + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType MATLABFcn + MATLABFcn "sin" + OutputDimensions "-1" + OutputSignalType "auto" + Output1D on + SampleTime "-1" + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Product + Inputs "2" + Multiplication "Element-wise(.*)" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Zero" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + CheckFcnCallInpInsideContextMsg off + SystemSampleTime "-1" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + Variant off + GeneratePreprocessorConditionals off + } + Block { + BlockType ToWorkspace + VariableName "simulink_output" + MaxDataPoints "1000" + Decimation "1" + SampleTime "0" + FixptAsFi off + } + Block { + BlockType Trigonometry + Operator "sin" + ApproximationMethod "None" + NumberOfIterations "11" + OutputSignalType "auto" + SampleTime "-1" + } + } + System { + Name "sl_rrmc2" + Location [467, 232, 1143, 648] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "29" + Block { + BlockType SubSystem + Name "Circle" + SID "1" + Ports [0, 1] + Position [35, 224, 65, 286] + ForegroundColor "blue" + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + RTWSystemCode "Auto" + FunctionWithSeparateData off + Opaque off + RequestExecContextInheritance off + MaskHideContents off + MaskPromptString "Radius|Frequency (rev/s)" + MaskStyleString "edit,edit" + MaskVariables "radius=@1;freq=@2;" + MaskTunableValueString "on,on" + MaskCallbackString "|" + MaskEnableString "on,on" + MaskVisibilityString "on,on" + MaskToolTipString "on,on" + MaskIconFrame on + MaskIconOpaque on + MaskIconRotate "none" + MaskPortRotate "default" + MaskIconUnits "autoscale" + MaskValueString "0.05|0.2" + System { + Name "Circle" + Location [717, 253, 1152, 478] + Open off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + Block { + BlockType Clock + Name "Clock" + SID "2" + Position [25, 35, 45, 55] + Decimation "10" + } + Block { + BlockType Mux + Name "Mux" + SID "3" + Ports [2, 1] + Position [230, 19, 235, 126] + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Product + Name "Product1" + SID "4" + Ports [2, 1] + Position [280, 62, 310, 93] + ShowName off + Multiplication "Matrix(*)" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Trigonometry + Name "Trigonometric\nFunction1" + SID "5" + Ports [1, 1] + Position [155, 30, 185, 60] + ShowName off + Operator "cos" + } + Block { + BlockType Trigonometry + Name "Trigonometric\nFunction3" + SID "6" + Ports [1, 1] + Position [155, 80, 185, 110] + ShowName off + } + Block { + BlockType Gain + Name "freq" + SID "7" + Position [70, 30, 95, 60] + Gain "freq*2*pi" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Constant + Name "radius" + SID "8" + Position [170, 150, 200, 180] + Value "radius" + } + Block { + BlockType Outport + Name "xy" + SID "9" + Position [335, 73, 365, 87] + IconDisplay "Port number" + } + Line { + SrcBlock "freq" + SrcPort 1 + Points [25, 0] + Branch { + Points [0, 50] + DstBlock "Trigonometric\nFunction3" + DstPort 1 + } + Branch { + DstBlock "Trigonometric\nFunction1" + DstPort 1 + } + } + Line { + SrcBlock "Product1" + SrcPort 1 + DstBlock "xy" + DstPort 1 + } + Line { + SrcBlock "Clock" + SrcPort 1 + DstBlock "freq" + DstPort 1 + } + Line { + SrcBlock "radius" + SrcPort 1 + Points [55, 0; 0, -80] + DstBlock "Product1" + DstPort 2 + } + Line { + SrcBlock "Mux" + SrcPort 1 + DstBlock "Product1" + DstPort 1 + } + Line { + SrcBlock "Trigonometric\nFunction1" + SrcPort 1 + DstBlock "Mux" + DstPort 1 + } + Line { + SrcBlock "Trigonometric\nFunction3" + SrcPort 1 + DstBlock "Mux" + DstPort 2 + } + Annotation { + Name "omega" + Position [125, 37] + } + } + } + Block { + BlockType Constant + Name "Constant" + SID "10" + Position [70, 305, 100, 335] + ForegroundColor "blue" + ShowName off + Value "0" + } + Block { + BlockType Demux + Name "Demux" + SID "11" + Ports [1, 2] + Position [95, 236, 100, 274] + ForegroundColor "blue" + ShowName off + Outputs "2" + DisplayOption "bar" + } + Block { + BlockType Gain + Name "Gain" + SID "13" + Position [460, 69, 500, 101] + ShowName off + Gain "5" + } + Block { + BlockType Product + Name "Product" + SID "14" + Ports [2, 1] + Position [400, 55, 435, 115] + ShowName off + Multiplication "Matrix(*)" + } + Block { + BlockType Product + Name "Product1" + SID "23" + Ports [2, 1] + Position [215, 220, 250, 280] + ShowName off + Multiplication "Matrix(*)" + } + Block { + BlockType Reference + Name "T2xyz" + SID "15" + Ports [1, 3] + Position [450, 177, 500, 233] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Transform Conversion/T2xyz" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + } + Block { + BlockType ToWorkspace + Name "To Workspace" + SID "25" + Ports [1] + Position [350, 315, 410, 345] + NamePlacement "alternate" + ShowName off + VariableName "T" + MaxDataPoints "inf" + SampleTime "-1" + SaveFormat "Array" + } + Block { + BlockType ToWorkspace + Name "To Workspace1" + SID "28" + Ports [1] + Position [385, 135, 445, 165] + ShowName off + VariableName "v" + MaxDataPoints "inf" + SampleTime "-1" + SaveFormat "Array" + } + Block { + BlockType ToWorkspace + Name "To Workspace2" + SID "29" + Ports [1] + Position [625, 68, 655, 102] + NamePlacement "alternate" + ShowName off + VariableName "q" + MaxDataPoints "inf" + SampleTime "-1" + SaveFormat "Array" + } + Block { + BlockType Reference + Name "XY Graph" + SID "16" + Ports [2] + Position [535, 176, 565, 214] + LibraryVersion "1.225" + SourceBlock "simulink/Sinks/XY Graph" + SourceType "XY scope." + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + xmin "0.5" + xmax "0.7" + ymin "-0.25" + ymax "-0.05" + st "-1" + } + Block { + BlockType Constant + Name "circle centre" + SID "24" + Position [140, 170, 170, 200] + Value "transl( transl(p560.fkine(qn)) )" + } + Block { + BlockType Reference + Name "fkine" + SID "17" + Ports [1, 1] + Position [480, 267, 530, 323] + BlockMirror on + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Kinematics/fkine" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + } + Block { + BlockType MATLABFcn + Name "inv" + SID "26" + Ports [1, 1] + Position [275, 54, 330, 86] + MATLABFcn "inv" + Output1D off + Port { + PortNumber 1 + Name "invJ" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Reference + Name "jacob0\n" + SID "19" + Ports [1, 1] + Position [170, 35, 235, 105] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Kinematics/jacob0\n" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + Port { + PortNumber 1 + Name "J" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType DiscreteIntegrator + Name "joint\nservo" + SID "12" + Ports [1, 1] + Position [525, 65, 560, 105] + IntegratorMethod "Integration: Forward Euler" + ExternalReset "none" + InitialConditionSource "internal" + InitialCondition "qn'" + SampleTime "0.05" + ICPrevOutput "DiscIntNeverNeededParam" + ICPrevScaledInput "DiscIntNeverNeededParam" + Port { + PortNumber 1 + Name "q" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Reference + Name "tr2diff" + SID "21" + Ports [2, 1] + Position [300, 207, 350, 263] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Kinematics/tr2diff" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + Port { + PortNumber 1 + Name "v*" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Reference + Name "xyz2T" + SID "22" + Ports [3, 1] + Position [130, 238, 180, 292] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Transform Conversion/xyz2T" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + } + Line { + Name "q" + Labels [0, 0] + SrcBlock "joint\nservo" + SrcPort 1 + Points [30, 0] + Branch { + Labels [2, 0] + Points [0, -75; -450, 0; 0, 60] + DstBlock "jacob0\n" + DstPort 1 + } + Branch { + Points [0, 0] + Branch { + Points [0, 210] + DstBlock "fkine" + DstPort 1 + } + Branch { + DstBlock "To Workspace2" + DstPort 1 + } + } + } + Line { + SrcBlock "T2xyz" + SrcPort 1 + DstBlock "XY Graph" + DstPort 1 + } + Line { + SrcBlock "T2xyz" + SrcPort 2 + DstBlock "XY Graph" + DstPort 2 + } + Line { + Name "v*" + Labels [0, 0] + SrcBlock "tr2diff" + SrcPort 1 + Points [10, 0; 0, -85] + Branch { + Points [0, -50] + DstBlock "Product" + DstPort 2 + } + Branch { + DstBlock "To Workspace1" + DstPort 1 + } + } + Line { + SrcBlock "Demux" + SrcPort 1 + DstBlock "xyz2T" + DstPort 1 + } + Line { + Name "J" + Labels [0, 0] + SrcBlock "jacob0\n" + SrcPort 1 + DstBlock "inv" + DstPort 1 + } + Line { + SrcBlock "Product" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + Name "invJ" + Labels [1, 0] + SrcBlock "inv" + SrcPort 1 + DstBlock "Product" + DstPort 1 + } + Line { + SrcBlock "Gain" + SrcPort 1 + DstBlock "joint\nservo" + DstPort 1 + } + Line { + SrcBlock "Demux" + SrcPort 2 + DstBlock "xyz2T" + DstPort 2 + } + Line { + SrcBlock "Circle" + SrcPort 1 + DstBlock "Demux" + DstPort 1 + } + Line { + SrcBlock "Constant" + SrcPort 1 + Points [10, 0] + DstBlock "xyz2T" + DstPort 3 + } + Line { + SrcBlock "fkine" + SrcPort 1 + Points [-70, 0] + Branch { + Points [0, -90] + DstBlock "T2xyz" + DstPort 1 + } + Branch { + Points [-90, 0] + Branch { + Points [0, 35] + DstBlock "To Workspace" + DstPort 1 + } + Branch { + Points [-35, 0; 0, -75] + DstBlock "tr2diff" + DstPort 1 + } + } + } + Line { + SrcBlock "xyz2T" + SrcPort 1 + DstBlock "Product1" + DstPort 2 + } + Line { + SrcBlock "circle centre" + SrcPort 1 + Points [20, 0; 0, 50] + DstBlock "Product1" + DstPort 1 + } + Line { + SrcBlock "Product1" + SrcPort 1 + DstBlock "tr2diff" + DstPort 2 + } + Annotation { + Name "Cartesian circle" + Position [161, 353] + ForegroundColor "blue" + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/sl_ztorque.mdl b/lwrserv/matlab/rvctools/simulink/sl_ztorque.mdl new file mode 100644 index 0000000..08d56dd --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/sl_ztorque.mdl @@ -0,0 +1,716 @@ +Model { + Name "sl_ztorque" + Version 7.6 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 1 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "q" + } + ParameterArgumentNames "" + ComputedModelVersion "1.24" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + Created "Tue Jan 8 12:29:23 2002" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "n6334687" + ModifiedDateFormat "%" + LastModifiedDate "Mon Feb 06 16:50:21 2012" + RTWModifiedTimeStamp 250447816 + ModelVersionFormat "1.%" + ConfigurationManager "none" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "user" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip on + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "oneshot" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect off + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.10.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.10.0" + StartTime "0.0" + StopTime "1" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.10.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SaveOutput on + SaveState off + SignalLogging off + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs on + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.10.0" + BlockReduction off + BooleanDataType off + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero on + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.10.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "none" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "TryResolveAllWithWarning" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "warning" + MultiTaskCondExecSysMsg "none" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + InitInArrayFormatMsg "warning" + StrictBusMsg "None" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.10.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown on + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.10.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.10.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.10.0" + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime on + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateSLWebview off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.10.0" + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.10.0" + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant off + ParMdlRefBuildCompliant off + CompOptLevelCompliant off + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging off + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Data Import//Export" + ConfigPrmDlgPosition " [ 774, 145, 1654, 775 ] " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Terminator + } + } + System { + Name "sl_ztorque" + Location [127, 82, 627, 342] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "10" + Block { + BlockType Reference + Name "Robot" + SID "4" + Ports [1, 3] + Position [115, 59, 230, 131] + BackgroundColor "red" + DropShadow on + LibraryVersion "1.28" + SourceBlock "roblocks/Dynamics/Robot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + q0 "[0 0 0 0 0 0]" + } + Block { + BlockType Terminator + Name "Terminator" + SID "5" + Position [255, 110, 275, 130] + ShowName off + } + Block { + BlockType Terminator + Name "Terminator1" + SID "9" + Position [255, 85, 275, 105] + ShowName off + } + Block { + BlockType Constant + Name "Zero\ntorque" + SID "7" + Position [10, 78, 75, 112] + Value "[0 0 0 0 0 0]'" + } + Block { + BlockType Reference + Name "plot" + SID "8" + Ports [1] + Position [350, 42, 430, 98] + BackgroundColor "red" + LibraryVersion "1.28" + SourceBlock "roblocks/Robot Graphics/plot" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + GeneratePreprocessorConditionals off + robot "p560" + } + Block { + BlockType Outport + Name "q" + SID "10" + Position [320, 118, 350, 132] + IconDisplay "Port number" + } + Line { + SrcBlock "Zero\ntorque" + SrcPort 1 + DstBlock "Robot" + DstPort 1 + } + Line { + SrcBlock "Robot" + SrcPort 1 + Points [65, 0] + Branch { + DstBlock "plot" + DstPort 1 + } + Branch { + Points [0, 55] + DstBlock "q" + DstPort 1 + } + } + Line { + SrcBlock "Robot" + SrcPort 3 + DstBlock "Terminator" + DstPort 1 + } + Line { + SrcBlock "Robot" + SrcPort 2 + DstBlock "Terminator1" + DstPort 1 + } + Annotation { + Name "Puma560 collapsing under gravity" + Position [159, 21] + FontSize 18 + FontWeight "bold" + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/slaccel.m b/lwrserv/matlab/rvctools/simulink/slaccel.m new file mode 100644 index 0000000..ef32701 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/slaccel.m @@ -0,0 +1,73 @@ +%SLACCEL S-function for robot acceleration +% +% This is the S-function for computing robot acceleration. It assumes input +% data u to be the vector [q qd tau]. +% +% Implemented as an S-function to get around vector sizing problem with +% Simulink 4. + +function [sys, x0, str, ts] = slaccel(t, x, u, flag, robot) + switch flag, + + case 0 + % initialize the robot graphics + [sys,x0,str,ts] = mdlInitializeSizes(robot); % Init + + case {3} + % come here to calculate derivitives + + % first check that the torque vector is sensible + if length(u) ~= (3*robot.n) + error('RTB:slaccel:badarg', 'Input vector is length %d, should be %d', length(u), 3*robot.n); + end + if ~isreal(u) + error('RTB:slaccel:badarg', 'Input vector is complex, should be real'); + end + + sys = robot.accel(u(:)'); + case {1, 2, 4, 9} + sys = []; + end +% +%============================================================================= +% mdlInitializeSizes +% Return the sizes, initial conditions, and sample times for the S-function. +%============================================================================= +% +function [sys,x0,str,ts]=mdlInitializeSizes(robot) + +% +% call simsizes for a sizes structure, fill it in and convert it to a +% sizes array. +% +% Note that in this example, the values are hard coded. This is not a +% recommended practice as the characteristics of the block are typically +% defined by the S-function parameters. +% +sizes = simsizes; + +sizes.NumContStates = 0; +sizes.NumDiscStates = 0; +sizes.NumOutputs = robot.n; +sizes.NumInputs = 3*robot.n; +sizes.DirFeedthrough = 1; +sizes.NumSampleTimes = 1; % at least one sample time is needed + +sys = simsizes(sizes); + +% +% initialize the initial conditions +% +x0 = []; + +% +% str is always an empty matrix +% +str = []; + +% +% initialize the array of sample times +% +ts = [0 0]; + +% end mdlInitializeSizes diff --git a/lwrserv/matlab/rvctools/simulink/slplotbot.m b/lwrserv/matlab/rvctools/simulink/slplotbot.m new file mode 100644 index 0000000..6dec266 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/slplotbot.m @@ -0,0 +1,75 @@ +%SLPLOTBOT S-function for robot animation +% +% This is the S-function for animating the robot. It assumes input +% data u to be the joint angles q. +% +% Implemented as an S-function so as to update display at the end of +% each Simulink major integration step. + +function [sys,x0,str,ts] = splotbot(t,x,u,flag, robot, fps, holdplot) + switch flag, + + case 0 + % initialize the robot graphics + [sys,x0,str,ts] = mdlInitializeSizes(fps); % Init + if ~isempty(robot) + robot.plot(zeros(1, robot.n), 'delay', 0, 'noraise') + end + if holdplot + hold on + end + + case 3 + % come here on update + if ~isempty(u) + %fprintf('--slplotbot: t=%f\n', t); + robot.animate(u'); + drawnow + end + ret = []; + case {1, 2, 4, 9} + ret = []; + end +% +%============================================================================= +% mdlInitializeSizes +% Return the sizes, initial conditions, and sample times for the S-function. +%============================================================================= +% +function [sys,x0,str,ts]=mdlInitializeSizes(fps) + +% +% call simsizes for a sizes structure, fill it in and convert it to a +% sizes array. +% +% Note that in this example, the values are hard coded. This is not a +% recommended practice as the characteristics of the block are typically +% defined by the S-function parameters. +% +sizes = simsizes; + +sizes.NumContStates = 0; +sizes.NumDiscStates = 0; +sizes.NumOutputs = 0; +sizes.NumInputs = -1; +sizes.DirFeedthrough = 1; +sizes.NumSampleTimes = 1; % at least one sample time is needed + +sys = simsizes(sizes); + +% +% initialize the initial conditions +% +x0 = []; + +% +% str is always an empty matrix +% +str = []; + +% +% initialize the array of sample times +% +ts = [-1 0]; %[1.0/fps]; + +% end mdlInitializeSizes diff --git a/lwrserv/matlab/rvctools/simulink/vloop.mdl b/lwrserv/matlab/rvctools/simulink/vloop.mdl new file mode 100644 index 0000000..befc279 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/vloop.mdl @@ -0,0 +1,980 @@ +Model { + Name "vloop" + Version 7.4 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 2 + Inport { + Name "qd*" + } + Inport { + Name "tau_d" + } + NumRootOutports 3 + Outport { + BusObject "" + BusOutputAsStruct "off" + SignalName "qd" + Name "qd" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "qd error" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "tau" + } + ParameterArgumentNames "" + ComputedModelVersion "1.8" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + InitFcn "G = 107.815;" + Created "Mon Jul 6 21:30:05 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Apr 4 15:37:49 2010" + RTWModifiedTimeStamp 192296259 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "none" + WideLines off + ShowLineDimensions off + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.6.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.6.0" + StartTime "0.0" + StopTime "1" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + Solver "ode15s" + SolverName "ode15s" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.6.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SaveOutput on + SaveState off + SignalLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.6.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + EnforceIntegerDowncast on + ExpressionFolding on + BooleansAsBitfields off + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.6.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + StrictBusMsg "Warning" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.6.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.6.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + ModelReferenceNumInstancesAllowed "Multi" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.6.0" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.6.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.6.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + SimulinkBlockComments on + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.6.0" + Array { + Type "Cell" + Dimension 16 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "SupportNonFinite" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + Cell "PortableWordSizes" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Data Import//Export" + ConfigPrmDlgPosition " [ 560, 391, 1540, 1025 ] " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParameterDataTypeMode "Same as input" + ParameterDataType "fixdt(1,16,0)" + ParameterScalingMode "Best Precision: Matrix-wise" + ParameterScaling "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeMode "Same as input" + OutDataType "fixdt(1,16,0)" + OutScaling "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Inport + Port "1" + UseBusObject off + BusObject "BusObject" + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + OutMin "[]" + OutMax "[]" + DataType "auto" + OutDataType "fixdt(1,16,0)" + OutScaling "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchByCopyingInsideSignal off + Interpolate on + } + Block { + BlockType Outport + Port "1" + UseBusObject off + BusObject "BusObject" + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + OutMin "[]" + OutMax "[]" + DataType "auto" + OutDataType "fixdt(1,16,0)" + OutScaling "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Saturate + UpperLimit "0.5" + LowerLimit "-0.5" + LinearizeAsGain on + ZeroCross on + SampleTime "-1" + OutMin "[]" + OutMax "[]" + OutDataTypeMode "Same as input" + OutDataType "fixdt(1,16,0)" + OutScaling "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeMode "Same as first input" + OutDataType "fixdt(1,16,0)" + OutScaling "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType TransferFcn + Numerator "[1]" + Denominator "[1 2 1]" + AbsoluteTolerance "auto" + ContinuousStateAttributes "''" + Realization "auto" + } + } + System { + Name "vloop" + Location [15, 77, 768, 348] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark 32 + Block { + BlockType Inport + Name "qd*" + SID 20 + Position [85, 108, 115, 122] + IconDisplay "Port number" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Block { + BlockType Inport + Name "tau_d" + SID 21 + Position [335, 28, 365, 42] + Port "2" + IconDisplay "Port number" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Block { + BlockType Gain + Name "Gain" + SID 22 + Position [400, 20, 440, 50] + ShowName off + Gain "1/G" + ParameterDataTypeMode "Inherit via internal rule" + ParameterDataType "fixdt(1, 16)" + ParameterScaling "2^0" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeMode "Inherit via internal rule" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Km" + SID 23 + Position [320, 100, 350, 130] + Gain "0.228" + ParameterDataTypeMode "Inherit via internal rule" + ParameterDataType "fixdt(1, 16)" + ParameterScaling "2^0" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeMode "Inherit via internal rule" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Gain + Name "Kv" + SID 24 + Position [270, 100, 300, 130] + ParameterDataTypeMode "Inherit via internal rule" + ParameterDataType "fixdt(1, 16)" + ParameterScaling "2^0" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeMode "Inherit via internal rule" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum" + SID 25 + Ports [2, 1] + Position [160, 105, 180, 125] + ShowName off + IconShape "round" + Inputs "|+-" + InputSameDT off + OutDataTypeMode "Inherit via internal rule" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Sum + Name "Sum1" + SID 26 + Ports [2, 1] + Position [450, 105, 470, 125] + ShowName off + IconShape "round" + Inputs "++|" + InputSameDT off + OutDataTypeMode "Inherit via internal rule" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Reference + Name "control\ntime" + SID 27 + Ports [1, 1] + Position [220, 98, 245, 132] + LibraryVersion "1.762" + SourceBlock "simulink/Discrete/Integer Delay" + SourceType "Integer Delay" + vinit "0.0" + samptime "0.001" + NumDelays "1" + } + Block { + BlockType TransferFcn + Name "motor" + SID 28 + Position [500, 96, 565, 134] + Denominator "[372e-6 0.817e-3]" + ContinuousStateAttributes "'qd'" + Port { + PortNumber 1 + Name "qd" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Saturate + Name "torque\nlimit" + SID 29 + Position [370, 100, 400, 130] + UpperLimit "0.9" + LowerLimit "-0.9" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Block { + BlockType Outport + Name "qd" + SID 30 + Position [640, 108, 670, 122] + IconDisplay "Port number" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Block { + BlockType Outport + Name "qd error" + SID 31 + Position [210, 28, 240, 42] + Port "2" + IconDisplay "Port number" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Block { + BlockType Outport + Name "tau" + SID 32 + Position [640, 188, 670, 202] + Port "3" + IconDisplay "Port number" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Line { + SrcBlock "Gain" + SrcPort 1 + Points [15, 0] + DstBlock "Sum1" + DstPort 1 + } + Line { + SrcBlock "Sum1" + SrcPort 1 + DstBlock "motor" + DstPort 1 + } + Line { + SrcBlock "Km" + SrcPort 1 + DstBlock "torque\nlimit" + DstPort 1 + } + Line { + SrcBlock "Kv" + SrcPort 1 + DstBlock "Km" + DstPort 1 + } + Line { + SrcBlock "Sum" + SrcPort 1 + Points [5, 0] + Branch { + DstBlock "control\ntime" + DstPort 1 + } + Branch { + Points [0, -80] + DstBlock "qd error" + DstPort 1 + } + } + Line { + Name "qd" + Labels [0, 0] + SrcBlock "motor" + SrcPort 1 + Points [20, 0] + Branch { + Points [0, 55; -420, 0] + DstBlock "Sum" + DstPort 2 + } + Branch { + DstBlock "qd" + DstPort 1 + } + } + Line { + SrcBlock "tau_d" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + SrcBlock "torque\nlimit" + SrcPort 1 + Points [15, 0] + Branch { + DstBlock "Sum1" + DstPort 2 + } + Branch { + Points [0, 80] + DstBlock "tau" + DstPort 1 + } + } + Line { + SrcBlock "control\ntime" + SrcPort 1 + DstBlock "Kv" + DstPort 1 + } + Line { + Labels [0, 0] + SrcBlock "qd*" + SrcPort 1 + DstBlock "Sum" + DstPort 1 + } + Annotation { + Name "tau_m" + Position [425, 111] + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/vloop_test.mdl b/lwrserv/matlab/rvctools/simulink/vloop_test.mdl new file mode 100644 index 0000000..72d5024 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/vloop_test.mdl @@ -0,0 +1,1143 @@ +Model { + Name "vloop_test" + Version 8.0 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 2 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "qd" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + SignalName "qd*" + Name "qd*" + } + ParameterArgumentNames "" + ComputedModelVersion "1.13" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + InitFcn "G = 107.815;" + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [1154.0, 551.0, 749.0, 475.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [0] + DockPosition "Left" + Width [50] + Height [50] + Filter [10] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Object { + $PropName "EditorsInfo" + $ObjectID 5 + $ClassName "Simulink.EditorInfo" + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [715.0, 301.0] + ZoomFactor [1.0] + Offset [0.0, 0.0] + } + } + } + Created "Tue Jul 7 06:47:01 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun Jan 27 11:35:31 2013" + RTWModifiedTimeStamp 281187295 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "all" + WideLines on + ShowLineDimensions off + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + Object { + $PropName "DataLoggingOverride" + $ObjectID 6 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "vloop_test" + Array { + Type "Cell" + Dimension 1 + Cell "vloop_test" + PropName "logAsSpecifiedByModels_" + } + Array { + Type "Cell" + Dimension 1 + Cell "" + PropName "logAsSpecifiedByModelsSSIDs_" + } + } + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 7 + Version "1.12.1" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 8 + Version "1.12.1" + StartTime "0.0" + StopTime "1.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode15s" + SolverName "ode15s" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 9 + Version "1.12.1" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 10 + Version "1.12.1" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + UseFloatMulNetSlope off + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + ParallelExecutionInRapidAccelerator on + } + Simulink.DebuggingCC { + $ObjectID 11 + Version "1.12.1" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "UseLocalSettings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "warning" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + SFUndirectedBroadcastEventsDiag "warning" + SFTransitionActionBeforeConditionDiag "warning" + } + Simulink.HardwareCC { + $ObjectID 12 + Version "1.12.1" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 13 + Version "1.12.1" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 14 + Version "1.12.1" + SimCustomInitializer "if ~exist(tau_d)\n tau_d = 0\nend" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 15 + Version "1.12.1" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + PortableWordSizes off + GenerateErtSFunction off + CreateSILPILBlock "None" + CodeExecutionProfiling off + CodeExecutionProfileVariable "executionProfile" + CodeProfilingSaveOptions "SummaryOnly" + CodeProfilingInstrumentation off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 16 + Version "1.12.1" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + OperatorAnnotations off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InternalIdentifier "Classic" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 17 + Version "1.12.1" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "PortableWordSizes" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + CodeReplacementLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + GRTInterface on + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Simulation Target/Custom Code" + ConfigPrmDlgPosition [ 280, 124, 1160, 754 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 7 + } + Object { + $PropName "DataTransfer" + $ObjectID 18 + $ClassName "Simulink.GlobalDataTransfer" + DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" + DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" + DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" + DefaultExtrapolationMethodBetweenContTasks "None" + AutoInsertRateTranBlk [0] + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + MaskDefaults { + SelfModifiable "off" + IconFrame "on" + IconOpaque "on" + RunInitForIconRedraw "off" + IconRotate "none" + PortRotate "default" + IconUnits "autoscale" + } + MaskParameterDefaults { + Evaluate "on" + Tunable "on" + NeverSave "off" + Internal "off" + ReadOnly "off" + Enabled "on" + Visible "on" + ToolTip "on" + } + BlockParameterDefaults { + Block { + BlockType BusCreator + Inputs "4" + DisplayOption "none" + OutDataTypeStr "Inherit: auto" + NonVirtualBus off + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + DataFormat "Array" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType SignalGenerator + WaveForm "sine" + TimeSource "Use simulation time" + Amplitude "1" + Frequency "1" + Units "Hertz" + VectorParams1D on + } + } + System { + Name "vloop_test" + Location [1154, 551, 1903, 1026] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark "36" + Block { + BlockType BusCreator + Name "Bus\nCreator" + SID "1" + Ports [2, 1] + Position [535, 27, 540, 98] + ZOrder -1 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Gain + Name "Gain" + SID "2" + Position [180, 170, 220, 200] + ZOrder -2 + ForegroundColor "red" + ShowName off + Gain "1/G" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType SignalGenerator + Name "Signal\nGenerator" + SID "24" + Ports [0, 1] + Position [110, 75, 140, 105] + ZOrder -3 + WaveForm "square" + Amplitude "50" + Frequency "2" + Port { + PortNumber 1 + Name "qd*" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Scope + Name "integral" + SID "36" + Ports [1] + Position [575, 244, 605, 276] + ZOrder -4 + Floating off + Location [695, 1467, 1019, 1706] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + TimeRange "1" + YMin "-0.1" + YMax "0.175" + SaveName "ScopeData3" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Scope + Name "qd error" + SID "4" + Ports [1] + Position [575, 114, 605, 146] + ZOrder -5 + Floating off + Location [118, 1175, 442, 1414] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + TimeRange "1" + YMin "0" + YMax "0.7" + SaveName "ScopeData2" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Scope + Name "qd*/qd" + SID "5" + Ports [1] + Position [575, 49, 605, 81] + ZOrder -6 + Floating off + Location [212, 1467, 536, 1706] + Open off + NumInputPorts "1" + ZoomMode "xonly" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + TimeRange "1" + YMin "0" + YMax "90" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Scope + Name "tau" + SID "6" + Ports [1] + Position [575, 184, 605, 216] + ZOrder -7 + Floating off + Location [815, 1445, 1139, 1684] + Open off + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.5 0.5 0.5]" + AxesColor "[0 0 0]" + AxesTickColor "[1 1 1]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + ShowLegends off + TimeRange "1" + YMin "-0.1" + YMax "0.175" + SaveName "ScopeData1" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Constant + Name "tau_d" + SID "7" + Position [110, 170, 140, 200] + ZOrder -8 + ForegroundColor "red" + Value "0" + } + Block { + BlockType Reference + Name "vloop" + SID "32" + Ports [2, 4] + Position [315, 69, 365, 191] + ZOrder -9 + DropShadow on + LibraryVersion "1.18" + SourceBlock "control/vloop" + SourceType "" + J "372e-6" + B "817e-6" + Km "0.228" + tau_max "0.9" + Kv "1" + Ki "5" + T "0.001" + } + Block { + BlockType Outport + Name "qd" + SID "22" + Position [460, 93, 490, 107] + ZOrder -10 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "qd*" + SID "23" + Position [255, 103, 285, 117] + ZOrder -11 + Port "2" + IconDisplay "Port number" + } + Line { + Name "qd*" + Labels [0, 0] + SrcBlock "Signal\nGenerator" + SrcPort 1 + Points [95, 0] + Branch { + DstBlock "qd*" + DstPort 1 + } + Branch { + Points [50, 0] + Branch { + Points [0, -45] + DstBlock "Bus\nCreator" + DstPort 1 + } + Branch { + Points [10, 0] + DstBlock "vloop" + DstPort 1 + } + } + } + Line { + SrcBlock "Bus\nCreator" + SrcPort 1 + DstBlock "qd*/qd" + DstPort 1 + } + Line { + SrcBlock "tau_d" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + SrcBlock "vloop" + SrcPort 1 + Points [0, -5; 60, 0] + Branch { + DstBlock "Bus\nCreator" + DstPort 2 + } + Branch { + Points [0, 20] + DstBlock "qd" + DstPort 1 + } + } + Line { + SrcBlock "vloop" + SrcPort 2 + Points [190, 0] + DstBlock "qd error" + DstPort 1 + } + Line { + SrcBlock "vloop" + SrcPort 3 + Points [190, 0] + DstBlock "tau" + DstPort 1 + } + Line { + Labels [3, 0] + SrcBlock "Gain" + SrcPort 1 + Points [10, 0; 0, -25] + DstBlock "vloop" + DstPort 2 + } + Line { + SrcBlock "vloop" + SrcPort 4 + Points [95, 0; 0, 85] + DstBlock "integral" + DstPort 1 + } + Annotation { + SID "33" + Name "disturbance torque input" + Position [166, 227] + ForegroundColor "red" + } + Annotation { + SID "35" + Name "vloop parameters for\nPuma560 shoulder joint\nwith link inertia\nRVC p.205" + Position [326, 239] + } + } +} diff --git a/lwrserv/matlab/rvctools/simulink/vloop_test2.mdl b/lwrserv/matlab/rvctools/simulink/vloop_test2.mdl new file mode 100644 index 0000000..47a7597 --- /dev/null +++ b/lwrserv/matlab/rvctools/simulink/vloop_test2.mdl @@ -0,0 +1,950 @@ +Model { + Name "vloop_test2" + Version 7.5 + MdlSubVersion 0 + GraphicalInterface { + NumRootInports 0 + NumRootOutports 2 + Outport { + BusObject "" + BusOutputAsStruct "off" + Name "qd" + } + Outport { + BusObject "" + BusOutputAsStruct "off" + SignalName "qd*" + Name "qd*" + } + ParameterArgumentNames "" + ComputedModelVersion "1.10" + NumModelReferences 0 + NumTestPointedSignals 0 + } + SavedCharacterEncoding "ISO-8859-1" + SaveDefaultBlockParams on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + MaxMDLFileLineLength 120 + InitFcn "G = 107.815;" + Created "Tue Jul 7 06:47:01 2009" + Creator "pic" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "corkep" + ModifiedDateFormat "%" + LastModifiedDate "Sun May 30 08:57:46 2010" + RTWModifiedTimeStamp 197109807 + ModelVersionFormat "1.%" + ConfigurationManager "customverctrl" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "all" + WideLines on + ShowLineDimensions off + ShowPortDataTypes off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + ExecutionContextIcon off + ShowLinearizationAnnotations on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dw" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + covSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable off + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + BufferReuse on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 1 + Version "1.10.0" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 2 + Version "1.10.0" + StartTime "0.0" + StopTime "1.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "Auto" + Solver "ode15s" + SolverName "ode15s" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 3 + Version "1.10.0" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 4 + Version "1.10.0" + Array { + Type "Cell" + Dimension 6 + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseIntDivNetSlope off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + StrengthReduction off + EnforceIntegerDowncast on + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 2147483647 + FoldNonRolledExpr on + LocalBlockOutputs on + RollThreshold 5 + SystemCodeInlineAuto off + StateBitsets off + DataBitsets off + UseTempVars off + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + } + Simulink.DebuggingCC { + $ObjectID 5 + Version "1.10.0" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + DiscreteInheritContinuousMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "Use local settings" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + EnableOverflowDetection off + ModelReferenceIOMsg "none" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + ModelReferenceCSMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + StrictBusMsg "Warning" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + LoggingUnavailableSignals "error" + BlockIODiagnostic "none" + } + Simulink.HardwareCC { + $ObjectID 6 + Version "1.10.0" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetShiftRightIntArith on + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetTypeEmulationWarnSuppressLevel 0 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 7 + Version "1.10.0" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 8 + Version "1.10.0" + SimCustomInitializer "if ~exist(tau_d)\n tau_d = 0\nend" + SFSimEnableDebug on + SFSimOverflowDetection on + SFSimEcho on + SimBlas on + SimCtrlC on + SimExtrinsic on + SimIntegrity on + SimUseLocalCustomCode off + SimBuildMode "sf_incremental_build" + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 9 + Version "1.10.0" + Array { + Type "Cell" + Dimension 6 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + TemplateMakefile "grt_default_tmf" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + ProcessScriptMode "Default" + ConfigurationMode "Optimized" + ConfigAtBuild off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + IncludeHyperlinkInReport off + LaunchReport off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + IncludeERTFirstTime off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateCodeInfo off + RTWCompilerOptimization "Off" + CheckMdlBeforeBuild "Off" + CustomRebuildMode "OnUpdate" + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 10 + Version "1.10.0" + Array { + Type "Cell" + Dimension 17 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "SFDataObjDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + SimulinkBlockComments on + EnableCustomComments off + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 11 + Version "1.10.0" + Array { + Type "Cell" + Dimension 16 + Cell "IncludeMdlTerminateFcn" + Cell "CombineOutputUpdateFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "MultiInstanceERTCode" + Cell "PurelyIntegerCode" + Cell "SupportNonFinite" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + Cell "PortableWordSizes" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + TargetFunctionLibrary "ANSI_C" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTCodeCoverageTool "None" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + IsPILTarget off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + EnableShiftOperators on + ParenthesesLevel "Nominal" + PortableWordSizes off + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant off + AutosarCompliant off + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + GenerateASAP2 off + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Simulation Target/Custom Code" + ConfigPrmDlgPosition " [ 280, 124, 1160, 754 ] " + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 1 + } + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + BlockParameterDefaults { + Block { + BlockType BusCreator + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeMode "Inherit from 'Constant value'" + OutDataType "fixdt(1,16,0)" + ConRadixGroup "Use specified scaling" + OutScaling "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParameterDataTypeMode "Same as input" + ParameterDataType "fixdt(1,16,0)" + ParameterScalingMode "Best Precision: Matrix-wise" + ParameterScaling "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeMode "Same as input" + OutDataType "fixdt(1,16,0)" + OutScaling "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Outport + Port "1" + UseBusObject off + BusObject "BusObject" + BusOutputAsStruct off + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + OutMin "[]" + OutMax "[]" + DataType "auto" + OutDataType "fixdt(1,16,0)" + OutScaling "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + SignalType "auto" + SamplingMode "auto" + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + } + Block { + BlockType Scope + ModelBased off + TickLabels "OneTimeTick" + ZoomMode "on" + Grid "on" + TimeRange "auto" + YMin "-5" + YMax "5" + SaveToWorkspace off + SaveName "ScopeData" + LimitDataPoints on + MaxDataPoints "5000" + Decimation "1" + SampleInput off + SampleTime "-1" + } + Block { + BlockType SignalGenerator + WaveForm "sine" + TimeSource "Use simulation time" + Amplitude "1" + Frequency "1" + Units "Hertz" + VectorParams1D on + } + } + System { + Name "vloop_test2" + Location [713, 239, 1422, 536] + Open on + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "portrait" + PaperPositionMode "auto" + PaperType "A4" + PaperUnits "centimeters" + TiledPaperMargins [1.270000, 1.270000, 1.270000, 1.270000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "100" + ReportName "simulink-default.rpt" + SIDHighWatermark 33 + Block { + BlockType BusCreator + Name "Bus\nCreator" + SID 1 + Ports [2, 1] + Position [535, 27, 540, 98] + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Gain + Name "Gain" + SID 2 + Position [180, 150, 220, 180] + ForegroundColor "red" + ShowName off + Gain "1/G" + ParameterDataTypeMode "Inherit via internal rule" + ParameterDataType "fixdt(1, 16)" + ParameterScaling "2^0" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeMode "Inherit via internal rule" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType SignalGenerator + Name "Signal\nGenerator" + SID 24 + Ports [0, 1] + Position [110, 65, 140, 95] + WaveForm "sawtooth" + Frequency "2" + Port { + PortNumber 1 + Name "qd*" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Scope + Name "qd error" + SID 4 + Ports [1] + Position [575, 114, 605, 146] + Floating off + Location [64, 557, 388, 796] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + TimeRange "1" + YMin "0" + YMax "0.7" + SaveName "ScopeData2" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Scope + Name "qd*/qd" + SID 5 + Ports [1] + Position [575, 49, 605, 81] + Floating off + Location [41, 448, 365, 687] + Open on + NumInputPorts "1" + ZoomMode "xonly" + List { + ListType AxesTitles + axes1 "%" + } + TimeRange "1" + YMin "0" + YMax "90" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Scope + Name "tau" + SID 6 + Ports [1] + Position [575, 184, 605, 216] + Floating off + Location [142, 586, 466, 825] + Open on + NumInputPorts "1" + List { + ListType AxesTitles + axes1 "%" + } + TimeRange "1" + YMin "-0.1" + YMax "0.175" + SaveName "ScopeData1" + DataFormat "StructureWithTime" + SampleTime "0" + } + Block { + BlockType Constant + Name "tau_d" + SID 7 + Position [110, 150, 140, 180] + ForegroundColor "red" + Value "0" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Block { + BlockType Reference + Name "vloop_ff" + SID 33 + Ports [3, 3] + Position [315, 64, 375, 166] + DropShadow on + LibraryVersion "1.11" + SourceBlock "control/vloop_ff" + SourceType "" + ShowPortLabels "FromPortIcon" + SystemSampleTime "-1" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + J "372e-6" + B "817e-6" + Km "0.228" + tau_max "0.9" + Kv "1" + Ki "0" + T "0.001" + } + Block { + BlockType Outport + Name "qd" + SID 22 + Position [460, 93, 490, 107] + IconDisplay "Port number" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Block { + BlockType Outport + Name "qd*" + SID 23 + Position [230, 43, 260, 57] + Port "2" + IconDisplay "Port number" + OutDataType "fixdt(1, 16)" + OutScaling "2^0" + } + Line { + Name "qd*" + Labels [0, 0] + SrcBlock "Signal\nGenerator" + SrcPort 1 + Points [60, 0] + Branch { + Points [0, -30] + DstBlock "qd*" + DstPort 1 + } + Branch { + Points [80, 0] + Branch { + Points [0, -35] + DstBlock "Bus\nCreator" + DstPort 1 + } + Branch { + DstBlock "vloop_ff" + DstPort 1 + } + } + } + Line { + SrcBlock "Bus\nCreator" + SrcPort 1 + DstBlock "qd*/qd" + DstPort 1 + } + Line { + SrcBlock "tau_d" + SrcPort 1 + DstBlock "Gain" + DstPort 1 + } + Line { + SrcBlock "vloop_ff" + SrcPort 1 + Points [50, 0] + Branch { + DstBlock "Bus\nCreator" + DstPort 2 + } + Branch { + Points [0, 20] + DstBlock "qd" + DstPort 1 + } + } + Line { + SrcBlock "vloop_ff" + SrcPort 2 + Points [180, 0] + DstBlock "qd error" + DstPort 1 + } + Line { + SrcBlock "vloop_ff" + SrcPort 3 + Points [180, 0] + DstBlock "tau" + DstPort 1 + } + Line { + SrcBlock "Gain" + SrcPort 1 + Points [35, 0; 0, -50] + DstBlock "vloop_ff" + DstPort 2 + } + Annotation { + Name "disturbance torque input" + Position [166, 207] + ForegroundColor "red" + } + } +} diff --git a/lwrserv/matlab/rvctools/startup_rvc.m b/lwrserv/matlab/rvctools/startup_rvc.m new file mode 100644 index 0000000..2d43fa2 --- /dev/null +++ b/lwrserv/matlab/rvctools/startup_rvc.m @@ -0,0 +1,24 @@ +disp('Robotics, Vision & Control: (c) Peter Corke 1992-2011 http://www.petercorke.com') +tb = false; +rvcpath = fileparts( mfilename('fullpath') ); + +robotpath = fullfile(rvcpath, 'robot'); +if exist(robotpath,'dir') + addpath(robotpath); + tb = true; + startup_rtb +end + +visionpath = fullfile(rvcpath, 'vision'); +if exist(visionpath,'dir') + addpath(visionpath); + tb = true; + startup_mvtb +end + +if tb + addpath(fullfile(rvcpath, 'common')); + addpath(fullfile(rvcpath, 'simulink')); +end + +clear tb rvcpath robotpath visionpath diff --git a/lwrserv/matlab/setup.m b/lwrserv/matlab/setup.m new file mode 100644 index 0000000..b7baad7 --- /dev/null +++ b/lwrserv/matlab/setup.m @@ -0,0 +1,11 @@ +clear all; +close all; + +% load paths +addpath('gui'); +addpath('kinematics'); +addpath('rvctools'); +addpath('motion'); + +% robotic vision toolbox +startup_rvc \ No newline at end of file diff --git a/lwrserv/matlab/start_gui.m b/lwrserv/matlab/start_gui.m new file mode 100644 index 0000000..c3596e6 --- /dev/null +++ b/lwrserv/matlab/start_gui.m @@ -0,0 +1,7 @@ +clear all; +close all; +addpath('gui'); +addpath('kinematics'); +addpath('rvctools'); +startup_rvc +gui \ No newline at end of file diff --git a/lwrserv/matlab/start_motion.m b/lwrserv/matlab/start_motion.m new file mode 100644 index 0000000..90c72d1 --- /dev/null +++ b/lwrserv/matlab/start_motion.m @@ -0,0 +1,6 @@ +clear all +close all +addpath('gui'); +addpath('kinematics'); +addpath('rvctools'); +addpath('motion'); \ No newline at end of file