Skip to content

Fixed bugs in ETS2/ETS3 when displaying prismatic joints #19

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fixed bugs in ETS2/ETS3 when displaying prismatic joints #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
86 changes: 71 additions & 15 deletions toolbox/ETS2.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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::
Expand All @@ -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.

Expand All @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down
114 changes: 93 additions & 21 deletions toolbox/ETS3.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
%
Expand Down Expand Up @@ -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::
Expand All @@ -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.

Expand All @@ -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
Expand Down Expand Up @@ -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);

Expand All @@ -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
Expand Down Expand Up @@ -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'
Expand Down
Loading