Skip to content

This commit fixes the bug #76 #84

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

Merged
bvadorno merged 5 commits into from
Feb 2, 2023
Merged

This commit fixes the bug #76 #84

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
99f9e8e
This commit fixes the bug #76
juanjqo Jan 6, 2023
51e67ca
This commit fixes the method get_supported_joint_types().
juanjqo Feb 2, 2023
b3ac4bf
[DQ_SerialManipulator] Fixed comments as requested by Bruno.
juanjqo Feb 2, 2023
a648fe8
This commit updated the year of the copyright.
juanjqo Feb 2, 2023
559a5b6
[DQ_SerialManipulator.m] Fixed comment
juanjqo Feb 2, 2023
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
72 changes: 70 additions & 2 deletions robot_modeling/DQ_SerialManipulator.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
%
% See also DQ_Kinematics.

% (C) Copyright 2011-2022 DQ Robotics Developers
% (C) Copyright 2011-2023 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
Expand Down Expand Up @@ -51,6 +51,7 @@
% mainly in the plot function.
handle
n_links;
joint_types;
end

methods (Abstract, Access = protected)
Expand All @@ -73,9 +74,50 @@
% Human-Robot Collaboration' by Bruno Adorno.
% Usage: w = get_w(ith), where
% ith: link number
w = get_w(obj,ith) ;
w = get_w(obj,ith) ;
end

methods (Abstract, Static, Access = protected)
% This method returns the supported joint types.
st = get_supported_joint_types();
end

methods (Access = protected)
function check_joint_types(obj)
% CHECK_JOINT_TYPES() throws an exception if the joint types
% are different from the supported joints.
types = obj.get_joint_types();
supported_types = obj.get_supported_joint_types();
n = size(types, 2);
k = size(supported_types, 2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create a string containing the valid type of joints.
msg = 'Unsupported joint types. Use valid joint types: ';
for i=1:k
msg_type = ' DQ_JointType.' + string(supported_types(i));
if i==k
ps = '. ';
else
ps = ', ';
end
msg = msg + msg_type + ps;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
match = false;
for j=1:k
if types(i) == supported_types(j)
match = true;
break;
end
end
if match == false
error(msg);
end
end
end
end

methods
function obj = DQ_SerialManipulator()
obj.reference_frame = DQ(1); %Default base's pose
Expand All @@ -98,6 +140,32 @@ function set_effector(obj,effector)
% SET_EFFECTOR(effector) sets the pose of the effector
obj.effector = DQ(effector);
end

function set_joint_types(obj, joint_types)
% SET_JOINT_TYPES(joint_types) sets the joint types.
% 'joint_types' is a vector containing the joint types.
obj.joint_types = joint_types;
obj.check_joint_types();
end

function set_joint_type(obj, joint_type, ith_joint)
% SET_JOINT_TYPE(joint_type, ith_joint) sets the joint type of the ith
% joint.
% 'joint_type' type of joint to be set.
% 'ith_joint' ith joint to be set.
obj.joint_types(ith_joint) = joint_type;
obj.check_joint_types();
end

function ret = get_joint_types(obj)
Copy link
Member

@bvadorno bvadorno Feb 1, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Change it according to my comment for the set_joint_type method.

% GET_JOINT_TYPES() returns a vector containing the joint types.
ret = obj.joint_types;
end

function ret = get_joint_type(obj, ith_joint)
% GET_JOINT_TYPE(ith_joint) returns the joint type of the ith joint.
ret = obj.joint_types(ith_joint);
end

function x = fkm(obj,q, ith)
% FKM(q) calculates the forward kinematic model and
Expand Down
16 changes: 11 additions & 5 deletions robot_modeling/DQ_SerialManipulatorDH.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
% set_effector - Set an arbitrary end-effector rigid transformation with respect to the last frame in the kinematic chain.
% See also DQ_SerialManipulator.

% (C) Copyright 2020-2022 DQ Robotics Developers
% (C) Copyright 2020-2023 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
Expand Down Expand Up @@ -71,7 +71,6 @@
classdef DQ_SerialManipulatorDH < DQ_SerialManipulator
properties
theta,d,a,alpha;
type
end

properties (Constant)
Expand Down Expand Up @@ -106,7 +105,7 @@
a = obj.a(ith);
half_alpha = obj.alpha(ith)/2.0;
% Add the effect of the joint value
if obj.type(ith) == DQ_JointType.REVOLUTE
if obj.get_joint_type(ith) == DQ_JointType.REVOLUTE
% If joint is revolute
half_theta = half_theta + (q/2.0);
else
Expand Down Expand Up @@ -141,7 +140,7 @@
% Human-Robot Collaboration' by Bruno Adorno.
% Usage: w = get_w(ith), where
% ith: link number
if obj.type(ith) == DQ_JointType.REVOLUTE
if obj.get_joint_type(ith) == DQ_JointType.REVOLUTE
w = DQ.k;
else
% see Table 1 of "Dynamics of Mobile Manipulators using Dual Quaternion Algebra."
Expand All @@ -151,6 +150,13 @@
end
end
end

methods (Static, Access = protected)
function ret = get_supported_joint_types()
% This method returns the supported joint types.
ret = [DQ_JointType.REVOLUTE, DQ_JointType.PRISMATIC];
end
end

methods
function obj = DQ_SerialManipulatorDH(A, convention)
Expand Down Expand Up @@ -189,7 +195,7 @@
obj.d = A(2,:);
obj.a = A(3,:);
obj.alpha = A(4,:);
obj.type = A(5,:);
obj.set_joint_types(A(5,:));
end


Expand Down
21 changes: 14 additions & 7 deletions robot_modeling/DQ_SerialManipulatorMDH.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
% set_effector - Set an arbitrary end-effector rigid transformation with respect to the last frame in the kinematic chain.
% See also DQ_SerialManipulator.

% (C) Copyright 2020-2022 DQ Robotics Developers
% (C) Copyright 2020-2023 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
Expand Down Expand Up @@ -72,7 +72,6 @@
classdef DQ_SerialManipulatorMDH < DQ_SerialManipulator
properties
theta,d,a,alpha;
type
end

properties (Constant)
Expand Down Expand Up @@ -107,7 +106,7 @@
half_alpha = obj.alpha(ith)/2.0;

% Add the effect of the joint value
if obj.type(ith) == DQ_JointType.REVOLUTE
if obj.get_joint_type(ith) == DQ_JointType.REVOLUTE
% If joint is revolute
half_theta = half_theta + (q/2.0);
else
Expand Down Expand Up @@ -142,15 +141,23 @@
% Usage: w = get_w(ith), where
% ith: link number

if obj.type(ith) == DQ_JointType.REVOLUTE
if obj.get_joint_type(ith) == DQ_JointType.REVOLUTE
w = -DQ.j*sin(obj.alpha(ith))+ DQ.k*cos(obj.alpha(ith))...
-DQ.E*obj.a(ith)*(DQ.j*cos(obj.alpha(ith)) + DQ.k*sin(obj.alpha(ith)));
else % if joint is PRISMATIC
w = DQ.E*(cos(obj.alpha(ith))*DQ.k - sin(obj.alpha(ith))*DQ.j);
end
end

end
end

methods (Static, Access = protected)
% This method returns the supported joint types.
function ret = get_supported_joint_types()
% This method returns the supported joint types.
ret = [DQ_JointType.REVOLUTE, DQ_JointType.PRISMATIC];
end
end

methods
function obj = DQ_SerialManipulatorMDH(A)
% These are initialized in the constructor of
Expand Down Expand Up @@ -183,7 +190,7 @@
obj.d = A(2,:);
obj.a = A(3,:);
obj.alpha = A(4,:);
obj.type = A(5,:);
obj.set_joint_types(A(5,:));
end

end
Expand Down