diff --git a/toolbox/ETS2.m b/toolbox/ETS2.m index c824fd7..d669fed 100644 --- a/toolbox/ETS2.m +++ b/toolbox/ETS2.m @@ -184,6 +184,14 @@ % parameter is of the form 'qN' and it controls a translation. v = isjoint(ets) && (ets.what(1) == 'T'); end + + function v = isrevolute(ets) + %ETS2.isrevolute Test if transform is revolute joint + % + % E.isrevolute is true if the transform element is a joint, that is, its + % parameter is of the form 'qN' and it controls a rotation. + v = isjoint(ets) && (ets.what(1) == 'R'); + end function k = find(ets, j) %ETS2.find Find joints in transform sequence @@ -342,9 +350,6 @@ function teach(robot, varargin) % ETS.teach(Q, OPTIONS) as above but the robot joint angles are set to Q (1xN). % % Options:: - % 'eul' Display tool orientation in Euler angles (default) - % 'rpy' Display tool orientation in roll/pitch/yaw angles - % 'approach' Display tool orientation as approach vector (z-axis) % '[no]deg' Display angles in degrees (default true) % % GUI:: @@ -356,6 +361,7 @@ function teach(robot, varargin) % set then for % - a revolute joint they are assumed to be [-pi, +pi] % - a prismatic joint they are assumed unknown and an error occurs. + % - The tool orientation is expressed using Yaw angle. % % See also ETS2.plot. @@ -368,7 +374,7 @@ function teach(robot, varargin) %---- handle options opt.deg = true; - opt.orientation = {'rpy', 'eul', 'approach'}; + opt.orientation = {'eul', 'rpy', 'approach'}; opt.d_2d = true; opt.callback = []; opt.vellipse = false; @@ -385,16 +391,65 @@ function teach(robot, varargin) if nargin == 1 q = zeros(1,robot.n); + % Set the default values for prismatic to the mean value + % of its largest and smallest allowed value. + for i = 1:length(robot) + e = robot(i); + if isprismatic(e) + q(e.qvar) = mean(e.qlim); + end + % Check if there is an joint angle limit for the + % revolute joint, if so, and the range does not include + % zero, choose the mean value of its largest and + % smallest allowed value. Else, keep 0 as the default. + if isrevolute(e) && ~isempty(e.qlim) + if e.qlim(1) > 0 || e.qlim(2) < 0 + q(e.qvar) = mean(e.qlim); + end + end + end else q = varargin{1}; + % If the joint parameters are specified, check if the inputs + % are legal. + for i = 1:length(robot) + e = robot(i); + if isprismatic(e) + if q(e.qvar) < e.qlim(1) || q(e.qvar) > e.qlim(2) + error("Prismatic Joint Parameter Out of Range!") + end + if q(e.qvar) == 0 + error("Prismatic Joint Parameter Should Not Be Zero!") + % Set q(e.qvar) to zero will cause 'Matrix' + % property value to be invalid in the scaling + % step of ETS2/animate. + elseif q(e.qvar) < 0 + error("Prismatic Joint Parameter Should Not Be Negative!") + end + end + if isrevolute(e) + if ~isempty(e.qlim) + if q(e.qvar) < e.qlim(1) || q(e.qvar) > e.qlim(2) + error("Prismatic Joint Parameter Out of Range!") + end + else % If qlim not set, use [-pi, +pi] + if q(e.qvar) < -pi || q(e.qvar) > pi + error("Revolute Joint Parameter Out of Range!") + end + end + end + end end - robot.plot(q, args{2:end}); + + % Save a copy for the initial q, used for scaling + q_initial = q(:,:); + robot.plot(q, q_initial, args{2:end}); - RTBPlot.install_teach_panel('ETS2', robot, q, opt); + RTBPlot.install_teach_panel('ETS2', robot, q, q_initial, opt); end - function plot(ets, qq, varargin) + function plot(ets, qq, q_initial, varargin) %ETS2.plot Graphical display and animation % % ETS.plot(Q, options) displays a graphical animation of a robot based on @@ -543,10 +598,10 @@ function plot(ets, qq, varargin) % enable mouse-based 3D rotation rotate3d on - ets.animate(qq); + ets.animate(qq, q_initial); end - function animate(~, qq) + function animate(~, qq, q_initial) handles = findobj('Tag', 'ETS2'); h = handles.UserData; opt = h.opt; @@ -568,11 +623,12 @@ function animate(~, qq) if isprismatic(e) % for prismatic joints, scale the box + original = q_initial(e.qvar); switch e.what case 'Tx' - set(h.pjoint(e.qvar), 'Matrix', diag([q(e.qvar) 1 1 1])); + set(h.pjoint(e.qvar), 'Matrix', diag([q(e.qvar)/original 1 1 1])); case 'Ty' - set(h.pjoint(e.qvar), 'Matrix', diag([1 q(e.qvar) 1 1])); + set(h.pjoint(e.qvar), 'Matrix', diag([1 q(e.qvar)/original 1 1])); end end end @@ -667,11 +723,11 @@ function animate(~, qq) % it's a joint element: revolute or prismatic switch e.what case 'Tx' - h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', diag([q(e.qvar) 1 1 1])); - RTBPlot.box('x', opt.jointdiam*s, [0 1], opt.pjointcolor, [], 'Parent', h.pjoint(i)); + h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', [1 0 0 q(e.qvar); 0 1 0 0; 0 0 1 0; 0 0 0 1]); + RTBPlot.box('x', opt.jointdiam*s, [0 q(e.qvar)], opt.pjointcolor, [], 'Parent', h.pjoint(e.qvar)); case 'Ty' - h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', diag([1 q(e.qvar) 1 1])); - RTBPlot.box('y', opt.jointdiam*s, [0 1], opt.pjointcolor, [], 'Parent', h.pjoint(i)); + h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', [1 0 0 0; 0 1 0 q(e.qvar); 0 0 1 0; 0 0 0 1]); + RTBPlot.box('y', opt.jointdiam*s, [0 q(e.qvar)], opt.pjointcolor, [], 'Parent', h.pjoint(e.qvar)); case 'Rz' RTBPlot.cyl('z', opt.jointdiam*s, opt.jointlen*s*[-1 1], opt.jointcolor, [], 'Parent', h.element(i)); end diff --git a/toolbox/ETS3.m b/toolbox/ETS3.m index cfef790..882b1a1 100644 --- a/toolbox/ETS3.m +++ b/toolbox/ETS3.m @@ -170,6 +170,14 @@ v = isjoint(ets) && (ets.what(1) == 'T'); end + function v = isrevolute(ets) + %ETS2.isrevolute Test if transform is revolute joint + % + % E.isrevolute is true if the transform element is a joint, that is, its + % parameter is of the form 'qN' and it controls a rotation. + v = isjoint(ets) && (ets.what(1) == 'R'); + end + function k = find(ets, j) %ETS3.find Find joints in transform sequence % @@ -330,9 +338,10 @@ function teach(robot, varargin) % ETS.teach(Q, OPTIONS) as above but the robot joint angles are set to Q (1xN). % % Options:: - % 'eul' Display tool orientation in Euler angles (default) - % 'rpy' Display tool orientation in roll/pitch/yaw angles - % 'approach' Display tool orientation as approach vector (z-axis) + % orientation= + % - 'eul' Display tool orientation in Euler angles (default) + % - 'rpy' Display tool orientation in roll/pitch/yaw angles + % - 'approach' Display tool orientation as approach vector (z-axis) % '[no]deg' Display angles in degrees (default true) % % GUI:: @@ -344,6 +353,10 @@ function teach(robot, varargin) % set then for % - a revolute joint they are assumed to be [-pi, +pi] % - a prismatic joint they are assumed unknown and an error occurs. + % - For orientation option, you can directly write one of 'eul', 'rpy' or + % 'approach', optionally, you can use the option name 'orientation' before them, + % or (after R2021a) use 'orientation=xxx' format, but put it at the end + % of the option argument list. % % See also ETS3.plot. @@ -356,28 +369,83 @@ function teach(robot, varargin) %---- handle options opt.deg = true; - opt.orientation = {'rpy', 'eul', 'approach'}; + % opt.orientation = {'rpy', 'eul', 'approach'}; + opt.orientation = {'eul', 'rpy', 'approach'}; % By default we use Euler angles opt.d_2d = false; opt.callback = []; opt = tb_optparse(opt, varargin); if nargin == 1 q = zeros(1,robot.n); - elseif nargin == 2 - q = varargin{1}; - varargin = {}; + % Set the default values for prismatic to the mean value + % of its largest and smallest allowed value. + for i = 1:length(robot) + e = robot(i); + if isprismatic(e) + q(e.qvar) = mean(e.qlim); + end + % Check if there is an joint angle limit for the + % revolute joint, if so, and the range does not include + % zero, choose the mean value of its largest and + % smallest allowed value. Else, keep 0 as the default. + if isrevolute(e) && ~isempty(e.qlim) + if e.qlim(1) > 0 || e.qlim(2) < 0 + q(e.qvar) = mean(e.qlim); + end + end + end else q = varargin{1}; - varargin = varargin{2:end}; + % Exclude q and delete the orientation option from + % varargin, so that the rest options can be sent to + % the function "robot.plot". + varargin = varargin(2:end); % Use () to mantain cell structure + orientation_dict = {'orientation', 'rpy', 'eul', 'approach'}; + remove_idx = cellfun(@(x) (ischar(x) || isstring(x)) && any(strcmp(x, orientation_dict)), varargin); + varargin(remove_idx) = []; + + % If the joint parameters are specified, check if the inputs + % are legal. + for i = 1:length(robot) + e = robot(i); + if isprismatic(e) + if q(e.qvar) < e.qlim(1) || q(e.qvar) > e.qlim(2) + error("Prismatic Joint Parameter Out of Range!") + end + if q(e.qvar) == 0 + error("Prismatic Joint Parameter Should Not Be Zero!") + % Set q(e.qvar) to zero will cause 'Matrix' + % property value to be invalid in the scaling + % step of ETS3/animate. + elseif q(e.qvar) < 0 + error("Prismatic Joint Parameter Should Not Be Negative!") + end + end + if isrevolute(e) + if ~isempty(e.qlim) + if q(e.qvar) < e.qlim(1) || q(e.qvar) > e.qlim(2) + error("Prismatic Joint Parameter Out of Range!") + end + else % If qlim not set, use [-pi, +pi] + if q(e.qvar) < -pi || q(e.qvar) > pi + error("Revolute Joint Parameter Out of Range!") + end + end + end + end end + + % Save a copy for the initial q, used for scaling + q_initial = q(:,:); + if isempty(findobj('Tag', 'ETS3')) - robot.plot(q, varargin{:}) + robot.plot(q, q_initial, varargin{:}) end - RTBPlot.install_teach_panel('ETS3', robot, q, opt); + RTBPlot.install_teach_panel('ETS3', robot, q, q_initial, opt); end - function plot(ets, qq, varargin) + function plot(ets, qq, q_initial, varargin) %ETS3.plot Graphical display and animation % % ETS.plot(Q, options) displays a graphical animation of a robot based on @@ -530,10 +598,10 @@ function plot(ets, qq, varargin) % enable mouse-based 3D rotation rotate3d on - ets.animate(qq); + ets.animate(qq, q_initial); end - function animate(ets, qq) + function animate(ets, qq, q_initial) assert(numel(qq) == ets.n, 'RTB:ETS3:badarg', 'Joint angles should have %d columns', ets.n,2); @@ -556,11 +624,15 @@ function animate(ets, qq) set(h.element(i), 'Matrix', T.tform); if isprismatic(e) + % for prismatic joints, scale the box + original = q_initial(e.qvar); switch e.what case 'Tx' - set(h.pjoint(e.qvar), 'Matrix', diag([q(e.qvar) 1 1 1])); + set(h.pjoint(e.qvar), 'Matrix', diag([q(e.qvar)/original 1 1 1])); case 'Ty' - set(h.pjoint(e.qvar), 'Matrix', diag([1 q(e.qvar) 1 1])); + set(h.pjoint(e.qvar), 'Matrix', diag([1 q(e.qvar)/original 1 1])); + case 'Tz' + set(h.pjoint(e.qvar), 'Matrix', diag([1 1 q(e.qvar)/original 1])); end end end @@ -651,14 +723,14 @@ function animate(ets, qq) % it's a joint element: revolute or prismatic switch e.what case 'Tx' - h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', diag([q(e.qvar) 1 1 1])); - RTBPlot.box('x', opt.jointdiam*s, [0 1], opt.pjointcolor, [], 'Parent', h.pjoint(i)); + h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', [1 0 0 q(e.qvar); 0 1 0 0; 0 0 1 0; 0 0 0 1]); + RTBPlot.box('x', opt.jointdiam*s, [0 q(e.qvar)], opt.pjointcolor, [], 'Parent', h.pjoint(e.qvar)); case 'Ty' - h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', diag([1 q(e.qvar) 1 1])); - RTBPlot.box('y', opt.jointdiam*s, [0 1], opt.pjointcolor, [], 'Parent', h.pjoint(i)); + h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', [1 0 0 0; 0 1 0 q(e.qvar); 0 0 1 0; 0 0 0 1]); + RTBPlot.box('y', opt.jointdiam*s, [0 q(e.qvar)], opt.pjointcolor, [], 'Parent', h.pjoint(e.qvar)); case 'Tz' - h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', diag([1 1 q(e.qvar) 1])); - RTBPlot.box('z', opt.jointdiam*s, [0 1], opt.pjointcolor, [], 'Parent', h.pjoint(i)); + h.pjoint(e.qvar) = hgtransform('Tag', 'prismatic', 'Parent', h.element(i), 'Matrix', [1 0 0 0; 0 1 0 0; 0 0 1 q(e.qvar); 0 0 0 1]); + RTBPlot.box('z', opt.jointdiam*s, [0 q(e.qvar)], opt.pjointcolor, [], 'Parent', h.pjoint(e.qvar)); case 'Rx' RTBPlot.cyl('x', opt.jointdiam*s, opt.jointlen*s*[-1 1], opt.jointcolor, [], 'Parent', h.element(i)); case 'Ry' diff --git a/toolbox/RTBPlot.m b/toolbox/RTBPlot.m index 1c77e36..dd275b7 100644 --- a/toolbox/RTBPlot.m +++ b/toolbox/RTBPlot.m @@ -6,8 +6,8 @@ methods (Static) - function th = install_teach_panel(name, robot, q, opt) - + function th = install_teach_panel(name, robot, q, q_initial, opt) + % install the teach panel for the robot %------------------------------- % parameters for teach panel @@ -109,21 +109,33 @@ else % for an ETS* - for i=1:robot.n - if robot(i).isprismatic - qlim(i,:) = [0 2*robot(i).param]; %#ok - else - qlim(i,:) = pi*[-1 1]; %#ok + % Set the joint limits + qlim = zeros(robot.n, 2); + for i=1:length(robot) + e = robot(i); + if e.isjoint + if e.isprismatic + qlim(e.qvar,:) = [e.qlim(1) e.qlim(2)]; + elseif e.isrevolute + if ~isempty(e.qlim) + qlim(e.qvar,:) = [e.qlim(1) e.qlim(2)]; + else + qlim(e.qvar,:) = pi*[-1 1]; + end + end end end % set up scale factor, from actual limits in radians/metres to display units qscale = ones(robot.n,1); - for j=1:robot.n - if ~robot(i).isprismatic && opt.deg - qscale(j) = 180/pi; - else - qscale(j) = 1; + for j=1:length(robot) + e = robot(j); + if e.isjoint + if ~e.isprismatic && opt.deg + qscale(e.qvar) = 180/pi; + else + qscale(e.qvar) = 1; + end end end end @@ -134,6 +146,8 @@ teachhandles.orientation = opt.orientation; teachhandles.opt = opt; + teachhandles.q_initial = q_initial; + %---- now make the sliders n = robot.n; for j=1:n @@ -429,7 +443,7 @@ function teach_callback(src, name, j, teachhandles) end % update all robots of this name - animate(teachhandles.robot, info.q); + animate(teachhandles.robot, info.q, teachhandles.q_initial); % compute the robot tool pose @@ -579,7 +593,7 @@ function draw_shape(ax, r, extent, color, offset, theta, varargin) z = z + offset(3); % walls of the shape - surf(x,y,z, 'FaceColor', color, 'EdgeColor', 'none', varargin{:}) + surf(x,y,z, 'FaceColor', color, 'EdgeColor', 'none', varargin{:}); % put the ends on patch(x', y', z', color, 'EdgeColor', 'none', varargin{:});