From 9b3bb7a8716c8160cc4e811b039a30dd104e3c04 Mon Sep 17 00:00:00 2001 From: xiaoniaogangsi Date: Tue, 24 Jun 2025 23:34:37 +0800 Subject: [PATCH] Fixed bugs in ETS2/ETS3 when displaying prismatic joints Added improved handling and validation for prismatic and revolute joint limits in ETS2 and ETS3 teach and plot methods. The teach panel now uses correct joint limits and scaling, and prismatic joint visualization is fixed to reflect actual joint values. Correspondingly, RTBPlot is updated to support new teach panel arguments and correct joint scaling. --- toolbox/ETS2.m | 86 ++++++++++++++++++++++++++++------ toolbox/ETS3.m | 114 +++++++++++++++++++++++++++++++++++++--------- toolbox/RTBPlot.m | 42 +++++++++++------ 3 files changed, 192 insertions(+), 50 deletions(-) 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{:});