diff --git a/2drobot.gif b/2drobot.gif new file mode 100644 index 0000000..88bfaf9 Binary files /dev/null and b/2drobot.gif differ diff --git a/3drobot.gif b/3drobot.gif new file mode 100644 index 0000000..6f3974f Binary files /dev/null and b/3drobot.gif differ diff --git a/README.md b/README.md index 6e57da0..bd0170c 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ # Modern Robotics: Mechanics, Planning, and Control # Code Library -This repository contains the code library accompanying [_Modern Robotics: -Mechanics, Planning, and Control_](http://modernrobotics.org) (Kevin Lynch -and Frank Park, Cambridge University Press 2017). The +This repository contains the code library accompanying [_Modern Robotics: +Mechanics, Planning, and Control_](http://modernrobotics.org) (Kevin Lynch +and Frank Park, Cambridge University Press 2017). The [user manual](/doc/MRlib.pdf) is in the doc directory. The functions are available in: @@ -25,3 +25,19 @@ Some libraries built on ours: * [tf_rbdl](https://github.com/junhyeokahn/tf_rbdl#tf_rbdl), which refactors the Python version using the package `tensorflow`. Any contribution is welcomed but the maintenance team for this library here doesn't vouch for the reliability of those projects. + +### Plotting +Currently limited to only MATLAB +#### Basic Plotting +Basic plotting/animation functionality for Modern Robotics Package (MatLab) +2D example +

+ +

+ +

Fig.1 - Left: 2D robot, Right - Spatial Robot
+

+ +In order to run above examples, use the template in ```main_2drobot.m``` and ```main_3drobot.m``` + +#### Multi Robot Plotting diff --git a/plotting/.DS_Store b/plotting/.DS_Store new file mode 100644 index 0000000..cbdd810 Binary files /dev/null and b/plotting/.DS_Store differ diff --git a/plotting/animateBaseExp.m b/plotting/animateBaseExp.m new file mode 100644 index 0000000..4d4a58f --- /dev/null +++ b/plotting/animateBaseExp.m @@ -0,0 +1,201 @@ +function animateBaseExp(t, x, base, robotParam, Td, prob, heatMap, P) +%animate(t,x,P) +% +%FUNCTION: +% Animate is used to animate a system with state x at the times in t. +% +%INPUTS: +% t = [1xM] vector of times, Must be monotonic: t(k) < t(k+1) +% x = [NxM] matrix of states, corresponding to times in t +% P = animation parameter struct, with fields: +% .plotFunc = @(t,x) = function handle to create a plot +% t = a scalar time +% x = [Nx1] state vector +% .speed = scalar multiple of time, for playback speed +% .figNum = (optional) figure number for plotting. Default = 1000. +% .verbose = set to false to prevent printing details. Default = true; +% +%OUTPUTS: +% Animation based on data in t and x. +% +%NOTES: +% +% Keyboard commands during simulation: +% +% 'space' - toggle pause +% +% 'r' - reset animation +% +% 'uparrow' - go faster +% +% 'downarrow' - go slower +% +% 'rightarrow' - jump forward by 5 frames +% +% 'leftarrow' - jump backward by 5 frames +% +% 'esc' - quit animation +% +% + +if ~isfield(P,'figNum') + P.figNum=1000; %Default to figure 1000 +end +if ~isfield(P,'verbose') + P.verbose = true; +end +if ~isfield(P,'frameRate') + P.targetFrameRate = 10; +end + +% Animation call-back variables: +IS_PAUSED = false; +VERBOSE = P.verbose; +SPEED = P.speed; +QUIT = false; +START_TIME = t(1); +SIM_TIME = START_TIME; + +% Set up the figure, and attach keyboard events. +% fig = figure(P.figNum); clf(fig); +% fig.Position = [400 400 1080 1080]; +ax = gcf; +set(ax,'KeyPressFcn',@keyDownListener) + +tic; %Start a timer +timeBuffer(1:3) = toc; +myVideo = VideoWriter('myVideoFile','Motion JPEG AVI'); %open video file +myVideo.FrameRate = 10; %can adjust this, 5 - 10 works well for me +% open(myVideo) +filename = P.filename; +init = 0; +while SIM_TIME < t(end); + init = init + 1; + clf(ax) + + % plot the heat map + surf(heatMap.xx, heatMap.yy, heatMap.z-0.5); + hold on + target = heatMap.desPoses; + plot3(target(1,:), target(2,:), 0.5*ones(1,size(target,2)), '.', 'Color', [0 0 0], 'MarkerSize', 20) + + + %Interpolate to get the new point: + xNow = interp1(t',x',SIM_TIME,'linear','extrap')'; + + % handle in the case of the base is fixed + if (size(base,2) == 1) + xBaseNow = base; + else + xBaseNow = interp1(t', base', SIM_TIME, 'linear', 'extrap')'; + end + + %Call the plot command + feval(P.plotFunc,SIM_TIME,xNow,xBaseNow); + drawnow; + pause(0.005); + + if ~isempty(Td) + probNow = [probNow interp1(t',prob',SIM_TIME,'linear','extrap')']; + + %Plot the desired trajectory + fkDesired = [fkDesired interp1(t',Td',SIM_TIME,'linear','extrap')']; + plot3(fkDesired(1,:), fkDesired(2,:), fkDesired(3,:), 'g.'); + + %Plot the current trajectory + fk = FKinSpace(robotParam.M, robotParam.Slist, xNow); + fkCurrentStore = [fkCurrentStore fk(1:3,4)]; + + for k = 1:numel(probNow) + plot3(fkCurrentStore(1,k), fkCurrentStore(2,k), fkCurrentStore(3,k), 'k.', 'MarkerSize', probNow(k)); + end + end + + %Set up targets for timing + dtReal = 0.2*(timeBuffer(1) - timeBuffer(3)); + if IS_PAUSED + dtSim = 0; + else + dtSim = SPEED*dtReal; + end + SIM_TIME = SIM_TIME + dtSim; + + %Record the frame rate: + timeBuffer(3) = timeBuffer(2); + timeBuffer(2) = timeBuffer(1); + timeBuffer(1) = toc; + + frame = getframe(gcf); %get frame + im = frame2im(frame); + [imind,cm] = rgb2ind(im,256); + if (init == 1) + imwrite(imind,cm,filename,'gif', 'Loopcount',inf); + else + imwrite(imind,cm,filename,'gif','DelayTime',0, 'WriteMode','append'); + end +% writeVideo(myVideo, frame); + + + % Check exit conditions: + if QUIT + break + end + + +end + +% close(myVideo) +%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% +% Graphics call-back functions % +%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% + + + function keyDownListener(~,event) + switch event.Key + case 'space' + IS_PAUSED = ~IS_PAUSED; + if VERBOSE + if IS_PAUSED + fprintf('--> animation paused...'); + else + fprintf(' resumed! \n'); + end + end + case 'r' + SIM_TIME = START_TIME; + if VERBOSE + disp('--> restarting animation'); + end + case 'uparrow' + SPEED = 2*SPEED; + if VERBOSE + fprintf('--> speed set to %3.3f x real time\n',SPEED); + end + case 'downarrow' + SPEED = SPEED/2; + if VERBOSE + fprintf('--> speed set to %3.3f x real time\n',SPEED); + end + case 'rightarrow' + timeSkip = 5*SPEED*dtReal; + SIM_TIME = SIM_TIME + timeSkip; + if VERBOSE + fprintf('--> skipping forward by %3.3f seconds\n',timeSkip); + end + case 'leftarrow' + timeSkip = 5*SPEED*dtReal; + SIM_TIME = SIM_TIME - timeSkip; + if VERBOSE + fprintf('--> skipping backward by %3.3f seconds\n',timeSkip); + end + case 'escape' + QUIT = true; + if VERBOSE + disp('--> animation aborted'); + end + otherwise + end + end + + +end %animate.m diff --git a/plotting/animateExp.m b/plotting/animateExp.m new file mode 100644 index 0000000..baec98a --- /dev/null +++ b/plotting/animateExp.m @@ -0,0 +1,179 @@ +function animateExp(t,x, robotParam, P) +%animate(t,x,P) +% +%FUNCTION: +% Animate is used to animate a system with state x at the times in t. +% +%INPUTS: +% t = [1xM] vector of times, Must be monotonic: t(k) < t(k+1) +% x = [NxM] matrix of states, corresponding to times in t +% P = animation parameter struct, with fields: +% .plotFunc = @(t,x) = function handle to create a plot +% t = a scalar time +% x = [Nx1] state vector +% .speed = scalar multiple of time, for playback speed +% .figNum = (optional) figure number for plotting. Default = 1000. +% .verbose = set to false to prevent printing details. Default = true; +% +%OUTPUTS: +% Animation based on data in t and x. +% +%NOTES: +% +% Keyboard commands during simulation: +% +% 'space' - toggle pause +% +% 'r' - reset animation +% +% 'uparrow' - go faster +% +% 'downarrow' - go slower +% +% 'rightarrow' - jump forward by 5 frames +% +% 'leftarrow' - jump backward by 5 frames +% +% 'esc' - quit animation +% +% + +if ~isfield(P,'figNum') + P.figNum=1000; %Default to figure 1000 +end +if ~isfield(P,'verbose') + P.verbose = true; +end +if ~isfield(P,'frameRate') + P.targetFrameRate = 10; +end + +% Animation call-back variables: +IS_PAUSED = false; +VERBOSE = P.verbose; +SPEED = P.speed; +QUIT = false; +START_TIME = t(1); +SIM_TIME = START_TIME; + +% Set up the figure, and attach keyboard events. +fig = figure(P.figNum); clf(fig); +fig.Position = [400 400 1080 1080]; +set(fig,'KeyPressFcn',@keyDownListener) + +tic; %Start a timer +timeBuffer(1:3) = toc; +myVideo = VideoWriter('myVideoFile','Motion JPEG AVI'); %open video file +myVideo.FrameRate = 10; %can adjust this, 5 - 10 works well for me +% open(myVideo) +filename = P.filename; +init = 0; +fkCurrentStore = []; fkDesired = []; probNow = []; + +while SIM_TIME < t(end) + init = init + 1; + clf(fig) + %Interpolate to get the new point: + xNow = interp1(t',x',SIM_TIME,'linear','extrap')'; + + %Call the plot command + feval(P.plotFunc,SIM_TIME,xNow); + drawnow; + pause(0.005); + + %Plot the current trajectory + fk = FKinSpace(robotParam.M, robotParam.Slist, xNow); + fkCurrentStore = [fkCurrentStore fk(1:3,4)]; + + for k = 1:numel(probNow) + plot3(fkCurrentStore(1,k), fkCurrentStore(2,k), fkCurrentStore(3,k), 'k.', 'MarkerSize', probNow(k)); + end + + + %Set up targets for timing + dtReal = 0.2*(timeBuffer(1) - timeBuffer(3)); + if IS_PAUSED + dtSim = 0; + else + dtSim = SPEED*dtReal; + end + SIM_TIME = SIM_TIME + dtSim; + + %Record the frame rate: + timeBuffer(3) = timeBuffer(2); + timeBuffer(2) = timeBuffer(1); + timeBuffer(1) = toc; + + frame = getframe(gcf); %get frame + im = frame2im(frame); + [imind,cm] = rgb2ind(im,256); + if (init == 1) + imwrite(imind,cm,filename,'gif', 'Loopcount',inf); + else + imwrite(imind,cm,filename,'gif','DelayTime',0, 'WriteMode','append'); + end + + + % Check exit conditions: + if QUIT + break + end + + +end + +% close(myVideo) +%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% +% Graphics call-back functions % +%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% + + + function keyDownListener(~,event) + switch event.Key + case 'space' + IS_PAUSED = ~IS_PAUSED; + if VERBOSE + if IS_PAUSED + fprintf('--> animation paused...'); + else + fprintf(' resumed! \n'); + end + end + case 'r' + SIM_TIME = START_TIME; + if VERBOSE + disp('--> restarting animation'); + end + case 'uparrow' + SPEED = 2*SPEED; + if VERBOSE + fprintf('--> speed set to %3.3f x real time\n',SPEED); + end + case 'downarrow' + SPEED = SPEED/2; + if VERBOSE + fprintf('--> speed set to %3.3f x real time\n',SPEED); + end + case 'rightarrow' + timeSkip = 5*SPEED*dtReal; + SIM_TIME = SIM_TIME + timeSkip; + if VERBOSE + fprintf('--> skipping forward by %3.3f seconds\n',timeSkip); + end + case 'leftarrow' + timeSkip = 5*SPEED*dtReal; + SIM_TIME = SIM_TIME - timeSkip; + if VERBOSE + fprintf('--> skipping backward by %3.3f seconds\n',timeSkip); + end + case 'escape' + QUIT = true; + if VERBOSE + disp('--> animation aborted'); + end + otherwise + end + end + + +end %animate.m diff --git a/plotting/animateMultiRobotExp.m b/plotting/animateMultiRobotExp.m new file mode 100644 index 0000000..e25939b --- /dev/null +++ b/plotting/animateMultiRobotExp.m @@ -0,0 +1,183 @@ +function animateMultiRobotExp(t, x, base, manipObject, P) +%animate(t,x,P) +% +%FUNCTION: +% Animate is used to animate a system with state x at the times in t. +% +%INPUTS: +% t = [1xM] vector of times, Must be monotonic: t(k) < t(k+1) +% x = [NxM] matrix of states, corresponding to times in t +% P = animation parameter struct, with fields: +% .plotFunc = @(t,x) = function handle to create a plot +% t = a scalar time +% x = [Nx1] state vector +% .speed = scalar multiple of time, for playback speed +% .figNum = (optional) figure number for plotting. Default = 1000. +% .verbose = set to false to prevent printing details. Default = true; +% +%OUTPUTS: +% Animation based on data in t and x. +% +%NOTES: +% +% Keyboard commands during simulation: +% +% 'space' - toggle pause +% +% 'r' - reset animation +% +% 'uparrow' - go faster +% +% 'downarrow' - go slower +% +% 'rightarrow' - jump forward by 5 frames +% +% 'leftarrow' - jump backward by 5 frames +% +% 'esc' - quit animation +% +% + +if ~isfield(P,'figNum') + P.figNum=1000; %Default to figure 1000 +end +if ~isfield(P,'verbose') + P.verbose = true; +end +if ~isfield(P,'frameRate') + P.targetFrameRate = 10; +end + +% Animation call-back variables: +IS_PAUSED = false; +VERBOSE = P.verbose; +SPEED = P.speed; +QUIT = false; +START_TIME = t(1); +SIM_TIME = START_TIME; + +% Set up the figure, and attach keyboard events. +fig = figure(P.figNum); clf(fig); +fig.Position = [400 400 1080 1080]; +set(fig,'KeyPressFcn',@keyDownListener) + +tic; %Start a timer +timeBuffer(1:3) = toc; +myVideo = VideoWriter('myVideoFile','Motion JPEG AVI'); %open video file +myVideo.FrameRate = 10; %can adjust this, 5 - 10 works well for me +% open(myVideo) +filename = P.filename; +init = 0; +n_robots = numel(x); +xNow = cell(1,2); +xBaseNow = cell(1,2); +while SIM_TIME < t(end); + init = init + 1; + clf(fig) + %Interpolate to get the new point: + for i = 1:n_robots + xNow{i} = interp1(t',x{i}',SIM_TIME,'linear','extrap')'; + + if (size(base{i},2) == 1) + xBaseNow{i} = base{i}; + else + xBaseNow{i} = interp1(t',base{i}',SIM_TIME,'linear','extrap')'; + end + end + + % manipulation object + objectNow = interp1(t',manipObject',SIM_TIME,'linear','extrap')'; + + %Call the plot command + feval(P.plotFunc,SIM_TIME,xNow,xBaseNow,objectNow); + drawnow; + pause(0.005); + + %Set up targets for timing + dtReal = 0.2*(timeBuffer(1) - timeBuffer(3)); + if IS_PAUSED + dtSim = 0; + else + dtSim = SPEED*dtReal; + end + SIM_TIME = SIM_TIME + dtSim; + + %Record the frame rate: + timeBuffer(3) = timeBuffer(2); + timeBuffer(2) = timeBuffer(1); + timeBuffer(1) = toc; + + frame = getframe(gcf); %get frame + im = frame2im(frame); + [imind,cm] = rgb2ind(im,256); + if (init == 1) + imwrite(imind,cm,filename,'gif', 'Loopcount',inf); + else + imwrite(imind,cm,filename,'gif','DelayTime',0, 'WriteMode','append'); + end +% writeVideo(myVideo, frame); + + + % Check exit conditions: + if QUIT + break + end + + +end + +% close(myVideo) +%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% +% Graphics call-back functions % +%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% + + + function keyDownListener(~,event) + switch event.Key + case 'space' + IS_PAUSED = ~IS_PAUSED; + if VERBOSE + if IS_PAUSED + fprintf('--> animation paused...'); + else + fprintf(' resumed! \n'); + end + end + case 'r' + SIM_TIME = START_TIME; + if VERBOSE + disp('--> restarting animation'); + end + case 'uparrow' + SPEED = 2*SPEED; + if VERBOSE + fprintf('--> speed set to %3.3f x real time\n',SPEED); + end + case 'downarrow' + SPEED = SPEED/2; + if VERBOSE + fprintf('--> speed set to %3.3f x real time\n',SPEED); + end + case 'rightarrow' + timeSkip = 5*SPEED*dtReal; + SIM_TIME = SIM_TIME + timeSkip; + if VERBOSE + fprintf('--> skipping forward by %3.3f seconds\n',timeSkip); + end + case 'leftarrow' + timeSkip = 5*SPEED*dtReal; + SIM_TIME = SIM_TIME - timeSkip; + if VERBOSE + fprintf('--> skipping backward by %3.3f seconds\n',timeSkip); + end + case 'escape' + QUIT = true; + if VERBOSE + disp('--> animation aborted'); + end + otherwise + end + end + + +end %animate.m diff --git a/plotting/animateRobot2D.m b/plotting/animateRobot2D.m new file mode 100644 index 0000000..b6881f0 --- /dev/null +++ b/plotting/animateRobot2D.m @@ -0,0 +1,27 @@ +function animateRobot2D(x, q, param) + %% animation + n = 3; + W = [0 0 0;0 0 0;1 1 1]; + param.W = W; + param.L = reshape(x(1:6), [], 3)'; + param.L = [param.L zeros(3,1)]; + param.L = param.L'; + + Base = eye(4); + + param.Base = Base; + + h = 0; + S = []; + for i = 1:n + Si = ScrewToAxis(param.L(:,i), param.W(:,i), h); + S = [S Si]; + end + param.Slist = S; + + param.M = [1 0 0 x(7);0 1 0 x(8);0 0 1 0;0 0 0 1]; + param.ndofs = n; + param.view = 2; + + draw2DRobot(q, param) +end \ No newline at end of file diff --git a/plotting/animateRobotBase2D.m b/plotting/animateRobotBase2D.m new file mode 100644 index 0000000..9323b9d --- /dev/null +++ b/plotting/animateRobotBase2D.m @@ -0,0 +1,4 @@ +function animateRobotBase2D(q, base, Td, prob, heatMap, param) + %% animation + draw2DBaseRobot(q, base, Td, prob, heatMap, param) +end \ No newline at end of file diff --git a/plotting/animateRobotRedundant2D.m b/plotting/animateRobotRedundant2D.m new file mode 100644 index 0000000..b1dd1f7 --- /dev/null +++ b/plotting/animateRobotRedundant2D.m @@ -0,0 +1,9 @@ +function animateRobotRedundant2D(q, Td, prob, param) + %% animation + Base = eye(4); + Base(3,4) = 0; + + param.Base = Base; + + draw2DRobot(q, Td, prob, param) +end \ No newline at end of file diff --git a/plotting/draw2DBaseRobot.m b/plotting/draw2DBaseRobot.m new file mode 100644 index 0000000..df0138e --- /dev/null +++ b/plotting/draw2DBaseRobot.m @@ -0,0 +1,11 @@ +function draw2DBaseRobot(q, base, Td, prob, heatMap, param) +param.view = 2; + +t = linspace(0,100,size(q,2)); + +Anim.speed = 5.25; +Anim.plotFunc = @(t,q, b)( drawExpBaseRobot(q, b, param) ); +Anim.verbose = true; +Anim.filename = 'robot2DBaseRedundant.gif'; +animateBaseExp(t, q, base, param, Td, prob, heatMap, Anim); +end \ No newline at end of file diff --git a/plotting/draw2DRobot.m b/plotting/draw2DRobot.m new file mode 100644 index 0000000..9698aff --- /dev/null +++ b/plotting/draw2DRobot.m @@ -0,0 +1,11 @@ +function draw2DRobot(q, param) + +t = linspace(0,100,size(q,2)); + +Anim.speed = 5.25; +Anim.plotFunc = @(t,q)( drawExpRobot(q, param) ); +Anim.verbose = true; +Anim.filename = 'robot2DRedundant.gif'; +animateExp(t, q, param, Anim); + +end \ No newline at end of file diff --git a/plotting/draw7DofRobot.m b/plotting/draw7DofRobot.m new file mode 100644 index 0000000..44cf3af --- /dev/null +++ b/plotting/draw7DofRobot.m @@ -0,0 +1,70 @@ +function draw7DofRobot(q, Bases, W, L) +close all; +n = 7; + +W = [0 0 0 0 0 0 0; 0 -1 0 1 0 -1 0;1 0 1 0 1 0 1]; +L = [0 0 0 0 0 0 0;0 0 0 0 0 0 0;0.3600 0.3600 0.3600 0.7800 0.7800 1.1800 1.4210]; + +h = 0; +S = []; +for i = 1:n + Si = ScrewToAxis(L(:,i), W(:,i), h); + S = [S Si]; +end + +b2 = eye(4); +b2(2,4) = -0.3; b2(1,4) = 0.1; + +Base = eye(4); + +param.Base = Base; +param.W = W; +param.L = L; +param.Slist = S; +param.M = [1 0 0 0;0 1 0 0;0 0 1 1.421;0 0 0 1]; +param.view = 3; +param.qmin = (2/3)*[-pi -pi -pi -pi -pi -pi -pi]'; +param.qmax = (2/3)*[pi pi pi pi pi pi pi]'; + +% drawExpRobot(4*rand(7,1)-2, param) +n = size(W,2); + +nData = 100; +t = linspace(0,10,nData); + +q0 = 2*rand(7,1)-1; +T0 = FKinSpace(param.M, param.Slist, q0); +Tn = eye(4); +Tn(1,4) = 0.1; +Tn(2,4) = 0.05; +Tn(3,4) = 0.9; +cartTraj = CartesianTrajectory(T0, Tn , 10, nData, 3); +q = []; +fk_updated = zeros(3, nData); +fk_actual = zeros(3, nData); + +for i = 1:nData + q_ = IKinSpace3D(param.Slist, param.M, cartTraj{i}, q0, 0.01, 0.01, param); + % q_ = projectJointSpace(q_, param); + fk_ = FKinSpace(param.M, param.Slist, q_); + fk_actual(:,i) = cartTraj{i}(1:3,4); + fk_updated(:,i) = fk_(1:3,4); + q = [q q_]; +end + +Anim.filename = '7dof.gif'; +Anim.speed = 2; +Anim.plotFunc = @(t,q)( drawExpRobot(q,param) ); +Anim.verbose = true; +animateExp(t,q,Anim); + +hold on +plot3(fk_updated(1,:), fk_updated(2,:), fk_updated(3,:), 'r*') +plot3(fk_actual(1,:), fk_actual(2,:), fk_actual(3,:), 'k*') + +figure(2) +for i = 1:7 + plot(q(i,:)) + hold on +end +end \ No newline at end of file diff --git a/plotting/drawDualRobot.m b/plotting/drawDualRobot.m new file mode 100644 index 0000000..91ee6f4 --- /dev/null +++ b/plotting/drawDualRobot.m @@ -0,0 +1,51 @@ +function drawDualRobot(q, Bases) + close all; + n = 7; + + param = KUKA_Model(); + + nData = 100; + t = linspace(0,10,nData); + + q0 = 2*rand(7,1)-1; + T0 = FKinSpace(param.M, param.Slist, q0); + Tn = eye(4); + Tn(1,4) = 0.1; + Tn(2,4) = 0.05; + Tn(3,4) = 0.9; + cartTraj = CartesianTrajectory(T0, Tn , 10, nData, 3); + q = []; + fk_updated = zeros(3, nData); + fk_actual = zeros(3, nData); + + for i = 1:nData + q_ = IKinSpace3D(param.Slist, param.M, cartTraj{i}, q0, 0.01, 0.01, param); + % q_ = projectJointSpace(q_, param); + fk_ = FKinSpace(param.M, param.Slist, q_); + fk_actual(:,i) = cartTraj{i}(1:3,4); + fk_updated(:,i) = fk_(1:3,4); + q = [q q_]; + end + qDual = {q,q}; + T1 = eye(4); T2 = eye(4); + T1(1,4) = 0.4; + T2(1,4) = -0.4; + baseDual = {T1, T2}; + paramDual = {param, param}; + + Anim.filename = 'dual_dof.gif'; + Anim.speed = 2; + Anim.plotFunc = @(t,q)( drawMultiRobots(qDual,baseDual, paramDual) ); + Anim.verbose = true; + animateExp(t,q,Anim); + + hold on + plot3(fk_updated(1,:), fk_updated(2,:), fk_updated(3,:), 'r*') + plot3(fk_actual(1,:), fk_actual(2,:), fk_actual(3,:), 'k*') + + figure(2) + for i = 1:7 + plot(q(i,:)) + hold on + end +end \ No newline at end of file diff --git a/plotting/drawDualRobot2D.m b/plotting/drawDualRobot2D.m new file mode 100644 index 0000000..9b30238 --- /dev/null +++ b/plotting/drawDualRobot2D.m @@ -0,0 +1,35 @@ +function drawDualRobot2D() + close all; + n = 5; + + param = Planer5DOF(); + object.position = [-0.4;1.5]; + object.orientation = 0; + [t, qDual, base, objectTrajectory] = dualRobotManipulationTrajectory(object); + + baseDual = {zeros(6,1), zeros(6,1)}; + baseDual{1}(1) = -0.4; + baseDual{2}(1) = 0.4; + baseDual{1}(4:6) = rotm2eul(base{1}(1:3,1:3)); + baseDual{2}(4:6) = rotm2eul(base{2}(1:3,1:3)); + + param.view = 2; + + paramDual = {param, param, object}; + + Anim.filename = 'dual_dof.gif'; + Anim.speed = 2; + Anim.plotFunc = @(t,q,base,object)( drawMultiRobots(q, base, object, paramDual) ); + Anim.verbose = true; + animateMultiRobotExp(t,qDual,baseDual,objectTrajectory,Anim); + + % hold on + % plot3(fk_updated(1,:), fk_updated(2,:), fk_updated(3,:), 'r*') + % plot3(fk_actual(1,:), fk_actual(2,:), fk_actual(3,:), 'k*') + % + % figure(2) + % for i = 1:5 + % plot(q(i,:)) + % hold on + % end +end \ No newline at end of file diff --git a/plotting/drawExpBaseRobot.m b/plotting/drawExpBaseRobot.m new file mode 100644 index 0000000..e442655 --- /dev/null +++ b/plotting/drawExpBaseRobot.m @@ -0,0 +1,119 @@ +function drawExpBaseRobot(q, base, p) + +W = p.W; +L = p.L; +Base = eye(4); +Base(1:3, 1:3) = eul2rotm(base(4:6)'); +Base(1:3, 4) = base(1:3); + +nDoF = size(W,2); +set(gcf,'color','w'); + +% Colors: +colorGround = [118,62,12]/255; +colorStance = [200,60,60]/255; +colorAxis = [0,0,0]/255; + +% Set up the figure +hold off; +% xBnd = [-1,1] * 2.0; +% yBnd = [-1,1] * 2.0; +% zBnd = [-1,1] * 2.0; + +xBnd = [-1,1] * 2.5; +yBnd = [-1,1] * 2.5; +zBnd = [-0.5,1] * 2.4; + +% Plot the ground: +% plot(xBnd,[0,0],'LineWidth',6,'Color',colorGround); + +hold on; +T0 = Base; +x0 = T0(1:3,4); +w0 = [0 0 1]'; +L = [L;ones(1,nDoF)]; +L = T0 * L; +L = L(1:3, :); +xE = L(:,1); + + + +plot3(T0(1,4), T0(2,4), T0(3,4),'b.','MarkerSize',100); +% Plot the base +A1 = x0; +A2 = x0 - w0*0.1; +plot3([A1(1) A2(1)], [A1(2) A2(2)], [A1(3) A2(3)], 'LineWidth',15,'Color',colorGround) + +view(p.view) + +axang = [w0' q(1)]; +rotm = axang2rotm(axang); +rotmAxis = eye(3); + +% Plot the links: +plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'LineWidth',6,'Color',colorStance); + + +x0 = xE; + +for i = 2:nDoF + + w = W(:,i-1); + axang = [w' q(i-1)]; + rotm = axang2rotm(axang); + rotmAxis = rotmAxis * rotm; + + l = rotmAxis * (L(:,i)-L(:,i-1)); + xE = x0 + l; + % xC = x0 + l/2; + % plot3(xC(1), xC(2), xC(3),'k.','MarkerSize',30); + + % Plot the links: + plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'LineWidth',10,'Color',colorStance); + + % Format the axis: + axis([xBnd,yBnd,zBnd]); + % axis equal; + % axis off; + + % Plot the joints: + plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'k.','MarkerSize',60); + + % Plot the joint axis + A1 = x0 + rotmAxis*w*0.1; + A2 = x0 - rotmAxis*w*0.1; + plot3([A1(1) A2(1)], [A1(2) A2(2)], [A1(3) A2(3)], 'LineWidth',3,'Color',colorAxis) + + % update the parameters + x0 = xE; + + % plot the manipulability ellipsoid + J = JacobianSpace(p.Slist, q); + fk = FKinSpace(p.M, p.Slist, q); + +end + w = W(:,end); + axang = [w' q(end)]; + rotm = axang2rotm(axang); + rotmAxis = rotmAxis * rotm; + + % Plot the joint axis + A1 = x0 + rotmAxis*w*0.1; + A2 = x0 - rotmAxis*w*0.1; + plot3([A1(1) A2(1)], [A1(2) A2(2)], [A1(3) A2(3)], 'LineWidth',3,'Color',colorAxis) + + + % plot the tool length + l = rotmAxis * (T0(1:3,1:3)*p.M(1:3,4)+T0(1:3,4)-L(:,end)); + xE = x0 + l; + + % Plot the links: + plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'LineWidth',10,'Color',colorStance); + plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'k.','MarkerSize',40); + plot3([xE(1)], [xE(2)], [xE(3)],'s','MarkerSize',15, ... + 'MarkerFaceColor',[1 .6 .6]); + + fk = FKinSpace(p.M, p.Slist, q); + + +end \ No newline at end of file diff --git a/plotting/drawExpRobot.m b/plotting/drawExpRobot.m new file mode 100644 index 0000000..be051a4 --- /dev/null +++ b/plotting/drawExpRobot.m @@ -0,0 +1,130 @@ +function drawExpRobot(q, p) +% drawRobot(q,p) +% +% This function draws the robot with configuration q and parameters p +% +% INPUTS: +% q = [5, 1] = column vector of a single robot configuration +% p = parameter struct +% +% close all +W = p.W; +L = p.L; +Base = p.Base; +Base(3,4) = 0; + +nDoF = p.ndofs; +set(gcf,'color','w'); + +% Colors: +colorGround = [118,62,12]/255; +colorStance = [200,60,60]/255; +colorAxis = [0,0,0]/255; + +% Set up the figure +% hold off; +% xBnd = [-1,1] * 2.0; +% yBnd = [-1,1] * 2.0; +% zBnd = [-1,1] * 2.0; + +xBnd = [-1,1] * 2.5; +yBnd = [-1,1] * 2.5; +zBnd = [0,1] * 2; + +% Plot the ground: +% plot(xBnd,[0,0],'LineWidth',6,'Color',colorGround); + +hold on; +T0 = Base; +x0 = T0(1:3,4); +w0 = [0 0 1]'; +L = [L;ones(1,nDoF)]; +L = Base * L; +L = L(1:3, :); +xE = L(:,1); + + + +plot3(0, 0, 0,'b.','MarkerSize',40); +% Plot the base +A1 = x0; +A2 = x0 - w0*0.1; +plot3([A1(1) A2(1)], [A1(2) A2(2)], [A1(3) A2(3)], 'LineWidth',8,'Color',colorGround) + +view(p.view) + +axang = [w0' q(1)]; +rotm = axang2rotm(axang); +rotmAxis = eye(3); + +% Plot the links: +plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'LineWidth',6,'Color',colorStance); + + +x0 = xE; + +for i = 2:nDoF + + w = W(:,i-1); + axang = [w' q(i-1)]; + rotm = axang2rotm(axang); + rotmAxis = rotmAxis * rotm; + + l = rotmAxis * (L(:,i)-L(:,i-1)); + xE = x0 + l; + % xC = x0 + l/2; + % plot3(xC(1), xC(2), xC(3),'k.','MarkerSize',30); + + % Plot the links: + plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'LineWidth',10,'Color',colorStance); + + + % Format the axis: + axis([xBnd,yBnd,zBnd]); + % axis equal; + % axis off; + + % Plot the joints: + plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'k.','MarkerSize',60); + + % Plot the joint axis + A1 = x0 + rotmAxis*w*0.1; + A2 = x0 - rotmAxis*w*0.1; + plot3([A1(1) A2(1)], [A1(2) A2(2)], [A1(3) A2(3)], 'LineWidth',3,'Color',colorAxis) + + + % update the parameters + x0 = xE; + + % plot the manipulability ellipsoid + J = JacobianSpace(p.Slist, q); + fk = FKinSpace(p.M, p.Slist, q); + + +end + w = W(:,end); + axang = [w' q(end)]; + rotm = axang2rotm(axang); + rotmAxis = rotmAxis * rotm; + + % Plot the joint axis + A1 = x0 + rotmAxis*w*0.1; + A2 = x0 - rotmAxis*w*0.1; + plot3([A1(1) A2(1)], [A1(2) A2(2)], [A1(3) A2(3)], 'LineWidth',3,'Color',colorAxis) + + + % plot the tool length + l = rotmAxis * (p.M(1:3,4)-L(:,end)); + xE = x0 + l; + xE(3) = xE(3); + + % Plot the links: + plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'LineWidth',10,'Color',colorStance); + plot3([x0(1) xE(1)], [x0(2) xE(2)], [x0(3) xE(3)],'k.','MarkerSize',40); + plot3([xE(1)], [xE(2)], [xE(3)],'s','MarkerSize',15, ... + 'MarkerFaceColor',[1 .6 .6]); + + fk = FKinSpace(p.M, p.Slist, q); + % plot the trajectory + +end \ No newline at end of file diff --git a/plotting/drawManipulationObject.m b/plotting/drawManipulationObject.m new file mode 100644 index 0000000..d6a6329 --- /dev/null +++ b/plotting/drawManipulationObject.m @@ -0,0 +1,59 @@ +function drawManipulationObject(p) +% drawRobot(q,p) +% +% This function draws the robot with configuration q and parameters p +% +% INPUTS: +% q = [5, 1] = column vector of a single robot configuration +% p = parameter struct +% + +position = p(1:2); +orientation = p(3); +scale = 0.2; + +xHR = [-0.5 0.5 0.5 -0.5]/10 + scale; +yHR = [0.5 0.5 -0.5 -0.5]/10; + +xHL = [-0.5 0.5 0.5 -0.5]/10 - scale; +yHL = [0.5 0.5 -0.5 -0.5]/10; + +t = linspace(0,2*pi,50); +xy = scale * [cos(t);sin(t)]; + + +R = rotz(orientation); +xy = R(1:2,1:2) * xy + position; + +x = xy(1,:); +y = xy(2,:); +z = zeros(1,numel(t)); + +xyHR = R(1:2,1:2) * [xHR;yHR] + position; +xyHL = R(1:2,1:2) * [xHL;yHL] + position; + +set(gcf,'color','w'); + +% Colors: +colorGround = [0,0,0]/255; + +xBnd = [-1,1] * 1.4; +yBnd = [-0.25,1] * 2.5; +zBnd = [-0.0,1] * 2.4; + + +%% Plot the links: +plot3(x, y,z,'LineWidth',6,'Color',colorGround); +view(2) + +ax = gca; +hold on +patch(ax,xyHR(1,:),xyHR(2,:),'black') +patch(ax,xyHL(1,:),xyHL(2,:),'black') + +axis([xBnd,yBnd,zBnd]); + +% axis equal +% axis off + +end \ No newline at end of file diff --git a/plotting/drawMultiRobots.m b/plotting/drawMultiRobots.m new file mode 100644 index 0000000..5c3d6c8 --- /dev/null +++ b/plotting/drawMultiRobots.m @@ -0,0 +1,15 @@ +function drawMultiRobots(q, base, manipulationObject, p) + n_robots = numel(q); + for i = 1:n_robots + drawExpBaseRobot(q{i},base{i},p{i}) + end + + drawManipulationObject(manipulationObject) + + xHR = [-0.5 0.5 0.5 -0.5]; + yHR = [0.25 0.25 -0.25 -0.25]; + + ax = gca; + patch(ax,xHR,yHR,'black') + axis off +end diff --git a/plotting/kukaTestTrajectories.gif b/plotting/kukaTestTrajectories.gif new file mode 100644 index 0000000..6f3974f Binary files /dev/null and b/plotting/kukaTestTrajectories.gif differ diff --git a/plotting/main_2Drobot.m b/plotting/main_2Drobot.m new file mode 100644 index 0000000..4e200af --- /dev/null +++ b/plotting/main_2Drobot.m @@ -0,0 +1,7 @@ + +% robot definition +xvec = [0. 0. 0.5 0.0 1.0 0.0 1.5 0]; + +qStore = [0 0 0; pi/3 pi/4 0]'; + +animateRobot2D(xvec, qStore, param) diff --git a/plotting/main_3Drobot.m b/plotting/main_3Drobot.m new file mode 100644 index 0000000..810ea90 --- /dev/null +++ b/plotting/main_3Drobot.m @@ -0,0 +1,16 @@ + +addpath('robots/') + +% robot definition +params = kukar820_model(); + +qStore = [0 0 0 0 0 0 0; pi/3 pi/4 0 pi/6 0 pi/4 0]'; + +t = linspace(0,10, size(qStore,2)); + +Anim.filename = 'kukaTestTrajectories.gif'; +Anim.speed = 2; +Anim.plotFunc = @(t,q)(drawExpRobot(q,params) ); +Anim.verbose = true; +animateExp(t, qStore, params, Anim); + diff --git a/plotting/robot2DRedundant.gif b/plotting/robot2DRedundant.gif new file mode 100644 index 0000000..52d001f Binary files /dev/null and b/plotting/robot2DRedundant.gif differ diff --git a/plotting/robotEXP.m b/plotting/robotEXP.m new file mode 100644 index 0000000..f2241c0 --- /dev/null +++ b/plotting/robotEXP.m @@ -0,0 +1,25 @@ +function [M, Slist, W, L] = robotEXP(x) + + %% manipulator twists + ndofs = numel(x)/2 - 1; + h = 0; + W = []; + Slist = []; + L = []; + for i = 1:ndofs + wi = [0;0;1]; qi = [x(2*i-1);x(2*i);0]; + W = [W wi]; + L = [L qi]; + + Si = ScrewToAxis(qi,wi, h); + Slist = [Slist Si]; + end + + M = eye(4); + M(1,4) = x(end-1); + M(2,4) = x(end); + + +end + + diff --git a/plotting/robots/Planer3DOF.m b/plotting/robots/Planer3DOF.m new file mode 100644 index 0000000..2ba4d69 --- /dev/null +++ b/plotting/robots/Planer3DOF.m @@ -0,0 +1,16 @@ +function param = Planer3DOF() + + xvec = [0. 0. 0.5 0.0 1.0 0.0 1.5 0]; + + % robot definition + [M, Slist, W, L] = robotEXP(xvec); + param.M = M; + param.Slist = Slist; + param.W = W; + param.L = L; + param.Base = eye(4); + param.qmin = 1.5*[-(1/6)*pi -pi/2 -pi/2]'; + param.qmax = 1.5*[(1/6)*pi pi/2 pi/2]'; + param.ndofs = 3; + +end \ No newline at end of file diff --git a/plotting/robots/Planer5DOF.m b/plotting/robots/Planer5DOF.m new file mode 100644 index 0000000..48e7798 --- /dev/null +++ b/plotting/robots/Planer5DOF.m @@ -0,0 +1,18 @@ +function param = Planer5DOF() + + xvec = [0. 0. 0.5 0.0 1.0 0.0 1.5 0 2 0 2.5 0]; + + % robot definition + [M, Slist, W, L] = robotEXP(xvec); + param.M = M; + param.Slist = Slist; + param.W = W; + param.L = L; + param.ndofs = 5; + param.Base = eye(4); + param.qmin = 1.5*[-(1/6)*pi -pi/2 -pi/2 -pi/2 -pi/2]'; + param.qmax = 1.5*[(1/6)*pi pi/2 pi/2 pi/2 pi/2]'; + param.ndofs = 5; + param.view = 2; + +end \ No newline at end of file diff --git a/plotting/robots/Spatial7DOF.m b/plotting/robots/Spatial7DOF.m new file mode 100644 index 0000000..e38cad9 --- /dev/null +++ b/plotting/robots/Spatial7DOF.m @@ -0,0 +1,25 @@ +function param = Spatial7DOF() + + W = [0 0 0 0 0 0 0; 0 -1 0 1 0 -1 0;1 0 1 0 1 0 1]; + L = [0 0 0 0 0 0 0;0 0 0 0 0 0 0;0.3600 0.3600 0.3600 0.7800 0.7800 1.1800 1.4210]; + + h = 0; + S = []; + for i = 1:7 + Si = ScrewToAxis(L(:,i), W(:,i), h); + S = [S Si]; + end + + Base = eye(4); + + param.Base = Base; + param.W = W; + param.L = L; + param.Slist = S; + param.M = [1 0 0 0;0 1 0 0;0 0 1 1.421;0 0 0 1]; + param.view = 3; + param.qmin = [-(3.9/4)*pi -(3/4)*pi -(3.9/4)*pi -(3/4)*pi -(3.9/4)*pi -(3/4)*pi -(3.9/4)*pi]'; + param.qmax = [(3.9/4)*pi (3/4)*pi (3.9/4)*pi (3/4)*pi (3.9/4)*pi (3/4)*pi (3.9/4)*pi]'; + param.ndofs = 7; + +end \ No newline at end of file diff --git a/plotting/robots/YUMI_Model.m b/plotting/robots/YUMI_Model.m new file mode 100644 index 0000000..df7e8bc --- /dev/null +++ b/plotting/robots/YUMI_Model.m @@ -0,0 +1,35 @@ +function param = YUMI_Model() + + n = 7; + + W = [0 0 0 0 1 0 1; ... + 0 -1 0 -1 0 -1 0; ... + 1 0 1 0 0 0 0]; + + L = [0 0 0 40.5 40.5 305.5 341.5;... + 0 -30 0 0 0 0 0;... + 0 166 166 417.5 458 431 458]/1000; + + h = 0; + S = []; + for i = 1:n + Si = ScrewToAxis(L(:,i), W(:,i), h); + S = [S Si]; + end + + Base = eye(4); + + param.Base = Base; + param.W = W; + param.L = L; + param.Slist = S; + param.M = [0 0 1 341.5/1000;... + 0 1 0 0;... + -1 0 0 458/1000;... + 0 0 0 1]; + + param.view = 3; + param.qmin = deg2rad([-168.5 -143.5 -123.5 -290 -88 -229 -168.5]'); + param.qmax = deg2rad([168.5 43.5 80 290 138 229 168.5]'); + param.ndofs = 7; +end \ No newline at end of file diff --git a/plotting/robots/kukar820_model.m b/plotting/robots/kukar820_model.m new file mode 100644 index 0000000..ecad6ab --- /dev/null +++ b/plotting/robots/kukar820_model.m @@ -0,0 +1,34 @@ +function param = kukar820_model() + + n = 7; + + W = [0 0 0 0 0 0 0; ... + 0 -1 0 1 0 -1 0; ... + 1 0 1 0 1 0 1]; + L = [0 0 0 0 0 0 0;... + 0 0 0 0 0 0 0;... + 0.3600 0.3600 0.3600 0.7800 0.7800 1.1800 1.4210]; + + h = 0; + S = []; + for i = 1:n + Si = ScrewToAxis(L(:,i), W(:,i), h); + S = [S Si]; + end + + b2 = eye(4); + b2(2,4) = -0.3; b2(1,4) = 0.1; + + Base = eye(4); + + param.Base = Base; + param.W = W; + param.L = L; + param.Slist = S; + param.M = [1 0 0 0;0 1 0 0;0 0 1 1.421;0 0 0 1]; + + param.view = 3; + param.qmin = (3/4)*[-pi -pi -pi -pi -pi -pi -pi]'; + param.qmax = (3/4)*[pi pi pi pi pi pi pi]'; + param.ndofs = 7; +end \ No newline at end of file diff --git a/robot2DRedundant.gif b/robot2DRedundant.gif new file mode 100644 index 0000000..f5ea0b3 Binary files /dev/null and b/robot2DRedundant.gif differ