diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 55832c70..c1e69f01 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -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. % @@ -51,6 +51,7 @@ % mainly in the plot function. handle n_links; + joint_types; end methods (Abstract, Access = protected) @@ -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 @@ -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) + % 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 diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 1c0246f7..06d9a943 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -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. % @@ -71,7 +71,6 @@ classdef DQ_SerialManipulatorDH < DQ_SerialManipulator properties theta,d,a,alpha; - type end properties (Constant) @@ -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 @@ -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." @@ -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) @@ -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 diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m index 8f096006..ca276952 100644 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -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. % @@ -72,7 +72,6 @@ classdef DQ_SerialManipulatorMDH < DQ_SerialManipulator properties theta,d,a,alpha; - type end properties (Constant) @@ -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 @@ -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 @@ -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