diff --git a/@DQ/mtimes.m b/@DQ/mtimes.m index d530da7d..ae75ba33 100644 --- a/@DQ/mtimes.m +++ b/@DQ/mtimes.m @@ -25,10 +25,10 @@ % Bruno Vihena Adorno - adorno@ufmg.br function res = mtimes(a,b) - if ~isa(a,'DQ'); + if ~isa(a,'DQ') a = DQ(a); end - if ~isa(b,'DQ'); + if ~isa(b,'DQ') b = DQ(b); end primary = QuaternionMultiplication(a.q(1:4),b.q(1:4)); diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 884c399a..d7356871 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -69,7 +69,7 @@ end methods - function obj = DQ_SerialManipulator(A,type) + function obj = DQ_SerialManipulator(A,convention) if nargin == 0 error('Input: matrix whose columns contain the DH parameters') end @@ -90,7 +90,7 @@ if nargin==1 obj.convention='standard'; else - obj.convention=type; + obj.convention=convention; end %For visualisation diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m new file mode 100644 index 00000000..e906bcc5 --- /dev/null +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -0,0 +1,246 @@ +% Concrete class that extends the DQ_SerialManipulator using the +% Denavit-Hartenberg parameters (DH) +% +% Usage: robot = DQ_SerialManipulatorDH(A) +% - 'A' is a 4 x n matrix containing the Denavit-Hartenberg parameters +% (n is the number of links) +% A = [theta1 ... thetan; +% d1 ... dn; +% a1 ... an; +% alpha1 ... alphan; +% type1 ... typen] +% where type is the actuation type, either DQ_SerialManipulatorDH.JOINT_ROTATIONAL +% or DQ_SerialManipulatorDH.JOINT_PRISMATIC +% - The only accepted convention in this subclass is the 'standard' DH +% convention. +% +% If the joint is of type JOINT_ROTATIONAL, then the first row of A will +% have the joint offsets. If the joint is of type JOINT_PRISMATIC, then the +% second row of A will have the joints offsets. +% +% DQ_SerialManipulatorDH Methods (Concrete): +% get_dim_configuration_space - Return the dimension of the configuration space. +% fkm - Compute the forward kinematics while taking into account base and end-effector's rigid transformations. +% plot - Plots the serial manipulator. +% pose_jacobian - Compute the pose Jacobian while taking into account base's and end-effector's rigid transformations. +% pose_jacobian_derivative - Compute the time derivative of the pose Jacobian. +% raw_fkm - Compute the FKM without taking into account base's and end-effector's rigid transformations. +% raw_pose_jacobian - Compute the pose Jacobian without taking into account base's and end-effector's rigid transformations. +% 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 DQ Robotics Developers +% +% This file is part of DQ Robotics. +% +% DQ Robotics is free software: you can redistribute it and/or modify +% it under the terms of the GNU Lesser General Public License as published +% by the Free Software Foundation, either version 3 of the License, or +% (at your option) any later version. +% +% DQ Robotics is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU Lesser General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public License +% along with DQ Robotics. If not, see . +% +% DQ Robotics website: dqrobotics.github.io +% +% Contributors to this file: +% Murilo M. Marinho - murilo@nml.t.u-tokyo.ac.jp + +classdef DQ_SerialManipulatorDH < DQ_SerialManipulator + properties + type + end + + properties (Constant) + % Joints that can be actuated + % Rotational joint + JOINT_ROTATIONAL = 1; + % Prismatic joint + JOINT_PRISMATIC = 2; + end + + methods + function obj = DQ_SerialManipulatorDH(A,convention) + % These are initialized in the constructor of + % DQ_SerialManipulator + %obj.convention = convention; + %obj.n_links = size(A,2); + %obj.theta = A(1,:); + %obj.d = A(2,:); + %obj.a = A(3,:); + %obj.alpha = A(4,:); + obj = obj@DQ_SerialManipulator(A(1:4,:),convention); + + if nargin == 0 + error('Input: matrix whose columns contain the DH parameters') + end + + if(size(A,1) ~= 5) + error('Input: Invalid DH matrix. It should have 5 rows.') + end + + % Add type + obj.type = A(5,:); + end + + function x = raw_fkm(obj,q,to_ith_link) + % RAW_FKM(q) calculates the forward kinematic model and + % returns the dual quaternion corresponding to the + % last joint (the displacements due to the base and the effector + % are not taken into account). + % + % 'q' is the vector of joint variables + % 'to_ith_link' defines until which link the raw_fkm will be + % calculated. + % + % This is an auxiliary function to be used mainly with the + % Jacobian function. + if nargin == 3 + n = to_ith_link; + else + n = obj.n_links; + end + + x = DQ(1); + + for i=1:n + x = x*dh2dq(obj,q(i),i); + end + end + + function x = fkm(obj,q,to_ith_link) + % FKM(q) calculates the forward kinematic model and + % returns the dual quaternion corresponding to the + % end-effector pose. This function takes into account the + % displacement due to the base's and effector's poses. + % + % 'q' is the vector of joint variables + % 'to_ith_link' defines up to which link the fkm will be + % calculated. If to_ith_link corresponds to the last link, + % the method DOES NOT take into account the transformation + % given by set_effector. If you want to take into account + % that transformation, use FKM(q) instead. + + if nargin == 3 + x = obj.reference_frame*obj.raw_fkm(q, to_ith_link); %Takes into account the base displacement + else + x = obj.reference_frame*obj.raw_fkm(q)*obj.effector; + end + end + + function dq = dh2dq(obj,q,ith) + % For a given link's Extended DH parameters, calculate the correspondent dual + % quaternion + % Usage: dq = dh2dq(q,ith), where + % q: joint value + % ith: link number + + if nargin ~= 3 + error('Wrong number of arguments. The parameters are joint value and the correspondent link') + end + + %The unoptimized standard dh2dq calculation is commented below + %if obj.type(ith) == obj.JOINT_ROTATIONAL + % % If joint is rotational + % h1 = cos((obj.theta(ith)+q)/2.0)+DQ.k*sin((obj.theta(ith)+q)/2.0); + % h2 = 1 + DQ.E*0.5*obj.d(ith)*DQ.k; + %else + % % If joint is prismatic + % h1 = cos(obj.theta(ith)/2.0)+DQ.k*sin(obj.theta(ith)/2.0); + % h2 = 1 + DQ.E*0.5*(obj.d(ith)+q)*DQ.k; + %end + %h3 = 1 + DQ.E*0.5*obj.a(ith)*DQ.i; + %h4 = cos(obj.alpha(ith)/2.0)+DQ.i*sin(obj.alpha(ith)/2.0); + %dq = h1*h2*h3*h4; + + % The optimized standard dh2dq calculation + % Store half angles and displacements + half_theta = obj.theta(ith)/2.0; + d = obj.d(ith); + a = obj.a(ith); + half_alpha = obj.alpha(ith)/2.0; + + % Add the effect of the joint value + if obj.type(ith) == obj.JOINT_ROTATIONAL + % If joint is rotational + half_theta = half_theta + (q/2.0); + else + % If joint is prismatic + d = d + q; + end + + % Pre-calculate cosines and sines + sine_of_half_theta = sin(half_theta); + cosine_of_half_theta = cos(half_theta); + sine_of_half_alpha = sin(half_alpha); + cosine_of_half_alpha = cos(half_alpha); + + % Return the optimized standard dh2dq calculation + dq = DQ([ + cosine_of_half_alpha*cosine_of_half_theta + + sine_of_half_alpha*cosine_of_half_theta + + sine_of_half_alpha*sine_of_half_theta + + cosine_of_half_alpha*sine_of_half_theta + + -(a*sine_of_half_alpha*cosine_of_half_theta) /2.0... + - (d*cosine_of_half_alpha*sine_of_half_theta)/2.0 + + (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0... + - (d*sine_of_half_alpha*sine_of_half_theta )/2.0 + + (a*cosine_of_half_alpha*sine_of_half_theta) /2.0... + + (d*sine_of_half_alpha*cosine_of_half_theta)/2.0 + + (d*cosine_of_half_alpha*cosine_of_half_theta)/2.0... + - (a*sine_of_half_alpha*sine_of_half_theta )/2.0 + ]); + end + + function w = get_w(obj,ith) + if obj.type(ith) == obj.JOINT_ROTATIONAL + w = DQ.k; + else + w = DQ.E*DQ.k; + end + end + + function J = raw_pose_jacobian(obj,q,to_ith_link) + % RAW_POSE_JACOBIAN(q) returns the Jacobian that satisfies + % vec(x_dot) = J * q_dot, where x = fkm(q) and q is the + % vector of joint variables. + % + % RAW_POSE_JACOBIAN(q,ith) returns the Jacobian that + % satisfies vec(x_ith_dot) = J * q_dot(1:ith), where + % x_ith = fkm(q, ith), that is, the fkm up to the i-th link. + % + % This function does not take into account any base or + % end-effector displacements and should be used mostly + % internally in DQ_kinematics + + if nargin < 3 + to_ith_link = obj.n_links; + end + x_effector = obj.raw_fkm(q,to_ith_link); + + x = DQ(1); + J = zeros(8,to_ith_link); + + for i = 0:to_ith_link-1 + w = obj.get_w(i+1); + z = 0.5*Ad(x,w); + x = x*obj.dh2dq(q(i+1),i+1); + j = z * x_effector; + J(:,i+1) = vec8(j); + end + end + + end +end \ No newline at end of file diff --git a/robots/Ax18ManipulatorRobot.m b/robots/Ax18ManipulatorRobot.m index 1a5521b8..9ddf9c09 100644 --- a/robots/Ax18ManipulatorRobot.m +++ b/robots/Ax18ManipulatorRobot.m @@ -28,14 +28,16 @@ ax18_DH_d = [0.167 0 0 0.1225 0]; % d ax18_DH_a = [0 0.159 0.02225 0 0.170]; % a ax18_DH_alpha = [-pi/2 0 -pi/2 -pi/2 0]; % alpha + ax18_DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,5); ax18_DH_matrix = [ ax18_DH_theta; ax18_DH_d; ax18_DH_a; ax18_DH_alpha; + ax18_DH_type; ]; % D&H parameters matrix for the arm model - ax = DQ_SerialManipulator(ax18_DH_matrix,'standard'); % Defines robot model using dual quaternions + ax = DQ_SerialManipulatorDH(ax18_DH_matrix,'standard'); % Defines robot model using dual quaternions end end end \ No newline at end of file diff --git a/robots/BarrettWamArmRobot.m b/robots/BarrettWamArmRobot.m index 692ce8d1..80a8c014 100644 --- a/robots/BarrettWamArmRobot.m +++ b/robots/BarrettWamArmRobot.m @@ -29,12 +29,14 @@ wam_DH_d = [0, 0, 0.55, 0, 0.3, 0, 0.0609]; wam_DH_a = [0, 0, 0.045, -0.045, 0, 0, 0]; wam_DH_alpha = pi*[-0.5, 0.5, -0.5, 0.5, -0.5, 0.5, 0]; + wam_DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,7); wam_DH_matrix = [wam_DH_theta; wam_DH_d; wam_DH_a; - wam_DH_alpha]; + wam_DH_alpha; + wam_DH_type]; - wam = DQ_SerialManipulator(wam_DH_matrix,'standard'); + wam = DQ_SerialManipulatorDH(wam_DH_matrix,'standard'); end end end \ No newline at end of file diff --git a/robots/KukaLwr4Robot.m b/robots/KukaLwr4Robot.m index 80f78d11..759130f8 100644 --- a/robots/KukaLwr4Robot.m +++ b/robots/KukaLwr4Robot.m @@ -32,12 +32,14 @@ kuka_DH_d = [0.310, 0, 0.4, 0, 0.39, 0, 0]; kuka_DH_a = [0, 0, 0, 0, 0, 0, 0]; kuka_DH_alpha = [pi/2, -pi/2, -pi/2, pi/2, pi/2, -pi/2, 0]; + kuka_DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,7); kuka_DH_matrix = [kuka_DH_theta; kuka_DH_d; kuka_DH_a; - kuka_DH_alpha]; + kuka_DH_alpha; + kuka_DH_type]; - ret = DQ_SerialManipulator(kuka_DH_matrix,'standard'); + ret = DQ_SerialManipulatorDH(kuka_DH_matrix,'standard'); end end end diff --git a/robots/KukaYoubot.m b/robots/KukaYoubot.m index 96b34f9a..a664701e 100644 --- a/robots/KukaYoubot.m +++ b/robots/KukaYoubot.m @@ -40,12 +40,14 @@ arm_DH_d = [ 0.147, 0, 0, 0, 0.218]; arm_DH_a = [ 0, 0.155, 0.135, 0, 0]; arm_DH_alpha = [pi2, 0, 0, pi2, 0]; + arm_DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,5); arm_DH_matrix = [arm_DH_theta; arm_DH_d; arm_DH_a; - arm_DH_alpha]; + arm_DH_alpha; + arm_DH_type]; - arm = DQ_SerialManipulator(arm_DH_matrix,'standard'); + arm = DQ_SerialManipulatorDH(arm_DH_matrix,'standard'); base = DQ_HolonomicBase(); include_namespace_dq