From 0f1d7d3f5cfc1a843249ed0f02b4d2e6b8c6d926 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Fri, 27 Mar 2020 17:43:55 +0900 Subject: [PATCH 01/15] Adding the DQ_SerialManipulator_DH class --- robot_modeling/DQ_SerialManipulator_DH.m | 232 +++++++++++++++++++++++ robots/Ax18ManipulatorRobot.m | 4 +- robots/BarrettWamArmRobot.m | 6 +- robots/KukaLwr4Robot.m | 6 +- robots/KukaYoubot.m | 6 +- 5 files changed, 247 insertions(+), 7 deletions(-) create mode 100644 robot_modeling/DQ_SerialManipulator_DH.m diff --git a/robot_modeling/DQ_SerialManipulator_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m new file mode 100644 index 00000000..ae33d7af --- /dev/null +++ b/robot_modeling/DQ_SerialManipulator_DH.m @@ -0,0 +1,232 @@ +% Concrete class that extends the DQ_SerialManipulator using the +% Denavit-Hartenberg parameters (DH) +% +% Usage: robot = DQ_SerialManipulator_DH(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_SerialManipulator_DH.JOINT_ROTATIONAL +% or DQ_SerialManipulator_DH.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_SerialManipulator_DH 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_SerialManipulator_DH < 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_SerialManipulator_DH(A,convention) + % These are initialized in the constructor of + % DQ_SerialManipulator + %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,convention); + if nargin == 0 + error('Input: matrix whose columns contain the Extended DH parameters') + end + + if(size(A,1) ~= 5) + error('Input: Invalid extended DH matrix. It should have 5 rows.') + end + + 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 + + theta_ = obj.theta(ith); + d_ = obj.d(ith); + a_ = obj.a(ith); + alpha_ = obj.alpha(ith); + + % If joint is rotational + if obj.type(ith) == obj.JOINT_ROTATIONAL + h1 = cos((theta_+q)/2.0)+DQ.k*sin((theta_+q)/2.0); + h2 = 1 + DQ.E*0.5*d_*DQ.k; + % If joint is prismatic + else + h1 = cos(theta_/2.0)+DQ.k*sin(theta_/2.0); + h2 = 1 + DQ.E*0.5*(d_+q)*DQ.k; + end + h3 = 1 + DQ.E*0.5*a_*DQ.i; + h4 = cos(alpha_/2.0)+DQ.i*sin(alpha_/2.0); + + dq = h1*h2*h3*h4; + end + + function dq_dot = dh2dq_dot(obj,q,ith) + % For a given link's Extended DH parameters, calculate the + % correspondent dual quaternion derivative of that joint's + % pose transformation with respect to its joint value. + % Usage: dq = dh2dq_dot(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 + + theta_ = obj.theta(ith); + d_ = obj.d(ith); + a_ = obj.a(ith); + alpha_ = obj.alpha(ith); + + % If joint is rotational + if obj.type(ith) == obj.JOINT_ROTATIONAL + h1 = 0.5*(-sin((theta_+q)/2.0)+DQ.k*cos((theta_+q)/2.0)); + h2 = 1 + DQ.E*0.5*d_*DQ.k; + % If joint is prismatic + else + h1 = cos(theta_/2.0)+DQ.k*sin(theta_/2.0); + h2 = DQ.E*0.5*DQ.k; + end + h3 = 1 + DQ.E*0.5*a_*DQ.k; + h4 = cos(alpha_/2.0)+DQ.i*sin(alpha_/2.0); + + dq_dot = h1*h2*h3*h4; + 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 + + n = obj.n_links; + J = zeros(8,n); + + for i = 1:to_ith_link + xi = DQ(1); + for j = 1:to_ith_link + if i==j + xi = xi*obj.dh2dq_dot(q(j),j); + else + xi = xi*obj.dh2dq(q(j),j); + end + end + J(:,i) = vec8(xi); + end + + end + + end +end \ No newline at end of file diff --git a/robots/Ax18ManipulatorRobot.m b/robots/Ax18ManipulatorRobot.m index 1a5521b8..d45ba7c0 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_SerialManipulator_DH.JOINT_ROTATIONAL,1,6); 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_SerialManipulator_DH(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..04c8a7ed 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_SerialManipulator_DH.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_SerialManipulator_DH(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..fa6c8a0c 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_SerialManipulator_DH.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_SerialManipulator_DH(kuka_DH_matrix,'standard'); end end end diff --git a/robots/KukaYoubot.m b/robots/KukaYoubot.m index d40819fd..8cc6e126 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_SerialManipulator_DH.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_SerialManipulator_DH(arm_DH_matrix,'standard'); base = DQ_HolonomicBase(); include_namespace_dq From 48542f75cd105b7fa15caa75b3dbd2526794fa9b Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Fri, 27 Mar 2020 23:41:32 +0900 Subject: [PATCH 02/15] Fixing a few bugs regarding a weird interaction with the dummy parameter in DQ_SerialManipulator --- robot_modeling/DQ_SerialManipulator.m | 4 ++-- robot_modeling/DQ_SerialManipulator_DH.m | 12 ++++++++++-- robots/Ax18ManipulatorRobot.m | 2 +- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 45aac348..9b5dc409 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -73,7 +73,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 @@ -104,7 +104,7 @@ if nargin==1 obj.convention='standard'; else - obj.convention=type; + obj.convention=convention; end %For visualisation diff --git a/robot_modeling/DQ_SerialManipulator_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m index ae33d7af..2eca3fd9 100644 --- a/robot_modeling/DQ_SerialManipulator_DH.m +++ b/robot_modeling/DQ_SerialManipulator_DH.m @@ -68,20 +68,28 @@ function obj = DQ_SerialManipulator_DH(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.dummy = A(5,:); + obj = obj@DQ_SerialManipulator(A,convention); if nargin == 0 - error('Input: matrix whose columns contain the Extended DH parameters') + error('Input: matrix whose columns contain the DH parameters') end if(size(A,1) ~= 5) - error('Input: Invalid extended DH matrix. It should have 5 rows.') + error('Input: Invalid DH matrix. It should have 5 rows.') end + % Remove dummy joints + obj.dummy = zeros(1,obj.n_links); + obj.n_dummy = 0; + + % Add type obj.type = A(5,:); end diff --git a/robots/Ax18ManipulatorRobot.m b/robots/Ax18ManipulatorRobot.m index d45ba7c0..a6a8e64f 100644 --- a/robots/Ax18ManipulatorRobot.m +++ b/robots/Ax18ManipulatorRobot.m @@ -28,7 +28,7 @@ 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_SerialManipulator_DH.JOINT_ROTATIONAL,1,6); + ax18_DH_type = repmat(DQ_SerialManipulator_DH.JOINT_ROTATIONAL,1,5); ax18_DH_matrix = [ ax18_DH_theta; ax18_DH_d; From c1f9f3f2955356bf1dabe219e216b3d258c74de6 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Sun, 29 Mar 2020 17:16:47 +0900 Subject: [PATCH 03/15] Fixed a bug in the DQ_SerialManipulator_DQ that caused the Rraw_jacobian(q,to_ith_link) to return a Jacobian of the wrong size --- robot_modeling/DQ_SerialManipulator_DH.m | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m index 2eca3fd9..8641e34b 100644 --- a/robot_modeling/DQ_SerialManipulator_DH.m +++ b/robot_modeling/DQ_SerialManipulator_DH.m @@ -219,8 +219,7 @@ to_ith_link = obj.n_links; end - n = obj.n_links; - J = zeros(8,n); + J = zeros(8,to_ith_link); for i = 1:to_ith_link xi = DQ(1); From 96649859bac9947cbd9347d8a17ad040c15c4e8d Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Sun, 29 Mar 2020 18:25:18 +0900 Subject: [PATCH 04/15] Fixed a bug in pose_jacobian() of DQ_SerialManipulator_DH() causing it to generate the wrong Jacobian when a is nonzero --- examples/manipulator/wam_kinematic_control.m | 78 ++++++++++++-------- robot_modeling/DQ_SerialManipulator_DH.m | 4 +- 2 files changed, 51 insertions(+), 31 deletions(-) diff --git a/examples/manipulator/wam_kinematic_control.m b/examples/manipulator/wam_kinematic_control.m index 79ad25e4..ae3c1834 100644 --- a/examples/manipulator/wam_kinematic_control.m +++ b/examples/manipulator/wam_kinematic_control.m @@ -1,37 +1,57 @@ function wam_kinematic_control() +load('wam_kinematic_control.mat','store_pose_jacobian','store_fkm','xd'); - %Create a new DQ_kinematics object with the WAM arm standard Denavit-Hartenberg parameters - wam = BarrettWamArmRobot.kinematics(); +%Create a new DQ_kinematics object with the WAM arm standard Denavit-Hartenberg parameters +wam = BarrettWamArmRobot.kinematics(); - %Initial configuration - q =[-2.65 -0.604 -0.46 2.68 1.35 1.61 -3.03]'; - %Final configuration - qd = [-2.0 -0.304 -0.3 2.3 1.0 1.0 -2.03]'; +%Initial configuration +q =[-2.65 -0.604 -0.46 2.68 1.35 1.61 -3.03]'; +%Final configuration +qd = [-2.0 -0.304 -0.3 2.3 1.0 1.0 -2.03]'; - epsilon = 0.001; %The error must be bellow this value in order to stop the robot - gain = 0.1; %Gain of the controllers +epsilon = 0.001; %The error must be bellow this value in order to stop the robot +gain = 0.1; %Gain of the controllers - xd = wam.fkm(qd); %Desired end-effector's pose - - figure; - axis equal; - plot(wam, q); - - grid off; - view(-0,0) - hold on; - - plot(wam, q); - - disp('Performing standard kinematic control using dual quaternion coordinates'); +if xd~=wam.fkm(qd) + error('Error in xd') +end +xd = wam.fkm(qd); %Desired end-effector's pose + +figure; +axis equal; +plot(wam, q); + +grid off; +view(-0,0) +hold on; + +plot(wam, q); + +disp('Performing standard kinematic control using dual quaternion coordinates'); +err = epsilon+1; +%store_fkm = {}; +%store_pose_jacobian = {}; +i=1; +while norm(err) > epsilon + jacob = wam.pose_jacobian(q); + if jacob~=store_pose_jacobian{i} + jacob + store_pose_jacobian{i} + error(['Error in pose_jacobian at i=' int2str(i)]) + end xm = wam.fkm(q); - error = epsilon+1; - while norm(error) > epsilon - jacob = wam.pose_jacobian(q); - xm = wam.fkm(q); - error = vec8(xd-xm); - q = q+pinv(jacob)*gain*error; - plot(wam, q'); - drawnow; + if xm~=store_fkm{i} + error(['Error in fkm at i=' int2str(i)]) end + %jacob = wam.pose_jacobian(q); + %store_pose_jacobian{i} = jacob; + %xm = wam.fkm(q); + %store_fkm{i} = xm; + err = vec8(xd-xm); + q = q+pinv(jacob)*gain*err; + plot(wam, q'); + drawnow; + i=i+1; +end +%save('wam_kinematic_control.mat') end diff --git a/robot_modeling/DQ_SerialManipulator_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m index 8641e34b..9ece82dc 100644 --- a/robot_modeling/DQ_SerialManipulator_DH.m +++ b/robot_modeling/DQ_SerialManipulator_DH.m @@ -189,14 +189,14 @@ % If joint is rotational if obj.type(ith) == obj.JOINT_ROTATIONAL - h1 = 0.5*(-sin((theta_+q)/2.0)+DQ.k*cos((theta_+q)/2.0)); + h1 = 0.5*( -sin( (theta_+q) /2.0) + DQ.k*cos( (theta_+q) /2.0) ); h2 = 1 + DQ.E*0.5*d_*DQ.k; % If joint is prismatic else h1 = cos(theta_/2.0)+DQ.k*sin(theta_/2.0); h2 = DQ.E*0.5*DQ.k; end - h3 = 1 + DQ.E*0.5*a_*DQ.k; + h3 = 1 + DQ.E*0.5*a_*DQ.i; h4 = cos(alpha_/2.0)+DQ.i*sin(alpha_/2.0); dq_dot = h1*h2*h3*h4; From 3af71b60505967f101b7e02b054edd57330dc98a Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Mon, 30 Mar 2020 10:13:51 +0900 Subject: [PATCH 05/15] Evaluating possible optimizations for DQ_SerialManipulator_DH --- @DQ/mtimes.m | 4 +- robot_modeling/DQ_SerialManipulator_DH.m | 105 ++++++++++++++++------- 2 files changed, 76 insertions(+), 33 deletions(-) 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_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m index 9ece82dc..aafe2d34 100644 --- a/robot_modeling/DQ_SerialManipulator_DH.m +++ b/robot_modeling/DQ_SerialManipulator_DH.m @@ -54,6 +54,7 @@ classdef DQ_SerialManipulator_DH < DQ_SerialManipulator properties type + parameters_stored end properties (Constant) @@ -85,13 +86,18 @@ error('Input: Invalid DH matrix. It should have 5 rows.') end - % Remove dummy joints + % Remove dummy joints obj.dummy = zeros(1,obj.n_links); obj.n_dummy = 0; % Add type obj.type = A(5,:); + % Flag that the parameters have changed + obj.parameters_stored = cell(obj.n_links,5); + for i=1:obj.n_links + obj.parameters_stored{i,1}=true; + end end function x = raw_fkm(obj,q,to_ith_link) @@ -150,24 +156,46 @@ error('Wrong number of arguments. The parameters are joint value and the correspondent link') end - theta_ = obj.theta(ith); - d_ = obj.d(ith); - a_ = obj.a(ith); - alpha_ = obj.alpha(ith); - - % If joint is rotational - if obj.type(ith) == obj.JOINT_ROTATIONAL - h1 = cos((theta_+q)/2.0)+DQ.k*sin((theta_+q)/2.0); - h2 = 1 + DQ.E*0.5*d_*DQ.k; - % If joint is prismatic + if obj.parameters_stored{ith,1} + + 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); + + obj.parameters_stored{ith,1} = false; + obj.parameters_stored{ith,2} = h1; + obj.parameters_stored{ith,3} = h2; + obj.parameters_stored{ith,4} = h3; + obj.parameters_stored{ith,5} = h4; + + dq = h1*h2*h3*h4; else - h1 = cos(theta_/2.0)+DQ.k*sin(theta_/2.0); - h2 = 1 + DQ.E*0.5*(d_+q)*DQ.k; + 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); + dq = h1*... + obj.parameters_stored{ith,3}*... + obj.parameters_stored{ith,4}*... + obj.parameters_stored{ith,5}; + + else + % If joint is prismatic + h2 = 1 + DQ.E*0.5*(obj.d(ith)+q)*DQ.k; + dq = obj.parameters_stored{ith,2}*... + h2*... + obj.parameters_stored{ith,4}*... + obj.parameters_stored{ith,5}; + end end - h3 = 1 + DQ.E*0.5*a_*DQ.i; - h4 = cos(alpha_/2.0)+DQ.i*sin(alpha_/2.0); - dq = h1*h2*h3*h4; end function dq_dot = dh2dq_dot(obj,q,ith) @@ -182,24 +210,39 @@ error('Wrong number of arguments. The parameters are joint value and the correspondent link') end - theta_ = obj.theta(ith); - d_ = obj.d(ith); - a_ = obj.a(ith); - alpha_ = obj.alpha(ith); - - % If joint is rotational - if obj.type(ith) == obj.JOINT_ROTATIONAL - h1 = 0.5*( -sin( (theta_+q) /2.0) + DQ.k*cos( (theta_+q) /2.0) ); - h2 = 1 + DQ.E*0.5*d_*DQ.k; - % If joint is prismatic + if obj.parameters_stored{ith,1} + if obj.type(ith) == obj.JOINT_ROTATIONAL + % If joint is rotational + h1 = 0.5*( -sin( (obj.theta(ith)+q) /2.0) + DQ.k*cos( (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 = DQ.E*0.5*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_dot = h1*h2*h3*h4; else - h1 = cos(theta_/2.0)+DQ.k*sin(theta_/2.0); - h2 = DQ.E*0.5*DQ.k; + + if obj.type(ith) == obj.JOINT_ROTATIONAL + % If joint is rotational + h1_dot = 0.5*( -sin( (obj.theta(ith)+q) /2.0) + DQ.k*cos( (obj.theta(ith)+q) /2.0) ); + dq_dot = h1_dot*... + obj.parameters_stored{ith,3}*... + obj.parameters_stored{ith,4}*... + obj.parameters_stored{ith,5}; + else + % If joint is prismatic + h2_dot = DQ.E*0.5*DQ.k; + dq_dot = obj.parameters_stored{ith,2}*... + h2_dot*... + obj.parameters_stored{ith,4}*... + obj.parameters_stored{ith,5}; + end end - h3 = 1 + DQ.E*0.5*a_*DQ.i; - h4 = cos(alpha_/2.0)+DQ.i*sin(alpha_/2.0); - dq_dot = h1*h2*h3*h4; end function J = raw_pose_jacobian(obj,q,to_ith_link) From b556255aec2381d154c1347d57bb1931331b8e71 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Mon, 30 Mar 2020 12:02:48 +0900 Subject: [PATCH 06/15] Updated DQ_SerialManipulator_DH with a more sustainable optimization --- robot_modeling/DQ_SerialManipulator_DH.m | 205 +++++++++++++---------- 1 file changed, 120 insertions(+), 85 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m index aafe2d34..786bf737 100644 --- a/robot_modeling/DQ_SerialManipulator_DH.m +++ b/robot_modeling/DQ_SerialManipulator_DH.m @@ -54,7 +54,6 @@ classdef DQ_SerialManipulator_DH < DQ_SerialManipulator properties type - parameters_stored end properties (Constant) @@ -93,11 +92,7 @@ % Add type obj.type = A(5,:); - % Flag that the parameters have changed - obj.parameters_stored = cell(obj.n_links,5); - for i=1:obj.n_links - obj.parameters_stored{i,1}=true; - end + end function x = raw_fkm(obj,q,to_ith_link) @@ -156,46 +151,52 @@ error('Wrong number of arguments. The parameters are joint value and the correspondent link') end - if obj.parameters_stored{ith,1} - - 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); - - obj.parameters_stored{ith,1} = false; - obj.parameters_stored{ith,2} = h1; - obj.parameters_stored{ith,3} = h2; - obj.parameters_stored{ith,4} = h3; - obj.parameters_stored{ith,5} = h4; - - dq = h1*h2*h3*h4; + %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 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); - dq = h1*... - obj.parameters_stored{ith,3}*... - obj.parameters_stored{ith,4}*... - obj.parameters_stored{ith,5}; - - else - % If joint is prismatic - h2 = 1 + DQ.E*0.5*(obj.d(ith)+q)*DQ.k; - dq = obj.parameters_stored{ith,2}*... - h2*... - obj.parameters_stored{ith,4}*... - obj.parameters_stored{ith,5}; - end + % 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(0); + dq.q(1) = cosine_of_half_alpha*cosine_of_half_theta; + dq.q(2) = sine_of_half_alpha*cosine_of_half_theta; + dq.q(3) = sine_of_half_alpha*sine_of_half_theta; + dq.q(4) = cosine_of_half_alpha*sine_of_half_theta; + dq.q(5) = -(a*sine_of_half_alpha*cosine_of_half_theta) /2.0 - (d*cosine_of_half_alpha*sine_of_half_theta)/2.0; + dq.q(6) = (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (d*sine_of_half_alpha*sine_of_half_theta )/2.0; + dq.q(7) = (a*cosine_of_half_alpha*sine_of_half_theta) /2.0 + (d*sine_of_half_alpha*cosine_of_half_theta)/2.0; + dq.q(8) = (d*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/2.0; end function dq_dot = dh2dq_dot(obj,q,ith) @@ -210,39 +211,62 @@ error('Wrong number of arguments. The parameters are joint value and the correspondent link') end - if obj.parameters_stored{ith,1} - if obj.type(ith) == obj.JOINT_ROTATIONAL - % If joint is rotational - h1 = 0.5*( -sin( (obj.theta(ith)+q) /2.0) + DQ.k*cos( (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 = DQ.E*0.5*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_dot = h1*h2*h3*h4; - else - - if obj.type(ith) == obj.JOINT_ROTATIONAL - % If joint is rotational - h1_dot = 0.5*( -sin( (obj.theta(ith)+q) /2.0) + DQ.k*cos( (obj.theta(ith)+q) /2.0) ); - dq_dot = h1_dot*... - obj.parameters_stored{ith,3}*... - obj.parameters_stored{ith,4}*... - obj.parameters_stored{ith,5}; - else - % If joint is prismatic - h2_dot = DQ.E*0.5*DQ.k; - dq_dot = obj.parameters_stored{ith,2}*... - h2_dot*... - obj.parameters_stored{ith,4}*... - obj.parameters_stored{ith,5}; - end + %The unoptimized standard dh2dq_dot calculation is commented below + %if obj.type(ith) == obj.JOINT_ROTATIONAL + % % If joint is rotational + % h1 = 0.5*( -sin( (obj.theta(ith)+q) /2.0) + DQ.k*cos( (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 = DQ.E*0.5*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_dot = h1*h2*h3*h4; + + % The optimized standard dh2dq_dot 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); 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 dh2dq_dot calculation + dq_dot = DQ(0); + if obj.type(ith) == obj.JOINT_ROTATIONAL + % If joint is rotational + dq_dot.q(1) = -(cosine_of_half_alpha*sine_of_half_theta )/2.0; + dq_dot.q(2) = -(sine_of_half_alpha*sine_of_half_theta )/2.0; + dq_dot.q(3) = (sine_of_half_alpha*cosine_of_half_theta )/2.0; + dq_dot.q(4) = (cosine_of_half_alpha*cosine_of_half_theta )/2.0; + dq_dot.q(5) = (a*sine_of_half_alpha*sine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*cosine_of_half_theta)/4.0; + dq_dot.q(6) = -(a*cosine_of_half_alpha*sine_of_half_theta )/4.0 - (d*sine_of_half_alpha*cosine_of_half_theta )/4.0; + dq_dot.q(7) = (a*cosine_of_half_alpha*cosine_of_half_theta)/4.0 - (d*sine_of_half_alpha*sine_of_half_theta )/4.0; + dq_dot.q(8) = -(a*sine_of_half_alpha*cosine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*sine_of_half_theta )/4.0; + else + % If joint is prismatic + dq_dot.q(1) = cosine_of_half_alpha*cosine_of_half_theta; + dq_dot.q(2) = sine_of_half_alpha*cosine_of_half_theta; + dq_dot.q(3) = sine_of_half_alpha*sine_of_half_theta; + dq_dot.q(4) = cosine_of_half_alpha*sine_of_half_theta; + dq_dot.q(5) = -(cosine_of_half_alpha*sine_of_half_theta )/2.0 - (a*sine_of_half_alpha*cosine_of_half_theta)/2.0; + dq_dot.q(6) = (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (sine_of_half_alpha*sine_of_half_theta )/2.0; + dq_dot.q(7) = (sine_of_half_alpha*cosine_of_half_theta )/2.0 + (a*cosine_of_half_alpha*sine_of_half_theta)/2.0; + dq_dot.q(8) = (cosine_of_half_alpha*cosine_of_half_theta )/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/2.0; + end end function J = raw_pose_jacobian(obj,q,to_ith_link) @@ -264,18 +288,29 @@ J = zeros(8,to_ith_link); - for i = 1:to_ith_link - xi = DQ(1); - for j = 1:to_ith_link - if i==j - xi = xi*obj.dh2dq_dot(q(j),j); - else - xi = xi*obj.dh2dq(q(j),j); - end - end - J(:,i) = vec8(xi); - end + %The unoptimized raw_jacobian calculation is commented below + %for i = 1:to_ith_link + % xi = DQ(1); + % for j = 1:i + % if i==j + % xi = xi*obj.dh2dq_dot(q(j),j); + % else + % xi = xi*obj.dh2dq(q(j),j); + % end + % end + % J(:,i) = vec8(xi); + %end + %The optimized raw_jacobian calculation + x_forward = DQ(1); + x_backward = obj.raw_fkm(q,to_ith_link); + for i = 1:to_ith_link + x_i_to_i_plus_one = obj.dh2dq(q(i),i); + x_backward = conj(x_i_to_i_plus_one)*x_backward; + x_ith_dot = x_forward*obj.dh2dq_dot(q(i),i)*x_backward; + x_forward = x_forward*x_i_to_i_plus_one; + J(:,i) = vec8(x_ith_dot); + end end end From c56976a92ee0340e108fd2a6297dbdb9bee3d9ce Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Mon, 30 Mar 2020 12:21:28 +0900 Subject: [PATCH 07/15] Reverting changes to example --- examples/manipulator/wam_kinematic_control.m | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/examples/manipulator/wam_kinematic_control.m b/examples/manipulator/wam_kinematic_control.m index ae3c1834..c1f57f16 100644 --- a/examples/manipulator/wam_kinematic_control.m +++ b/examples/manipulator/wam_kinematic_control.m @@ -1,6 +1,4 @@ function wam_kinematic_control() -load('wam_kinematic_control.mat','store_pose_jacobian','store_fkm','xd'); - %Create a new DQ_kinematics object with the WAM arm standard Denavit-Hartenberg parameters wam = BarrettWamArmRobot.kinematics(); @@ -12,9 +10,6 @@ function wam_kinematic_control() epsilon = 0.001; %The error must be bellow this value in order to stop the robot gain = 0.1; %Gain of the controllers -if xd~=wam.fkm(qd) - error('Error in xd') -end xd = wam.fkm(qd); %Desired end-effector's pose figure; @@ -29,29 +24,14 @@ function wam_kinematic_control() disp('Performing standard kinematic control using dual quaternion coordinates'); err = epsilon+1; -%store_fkm = {}; -%store_pose_jacobian = {}; i=1; while norm(err) > epsilon jacob = wam.pose_jacobian(q); - if jacob~=store_pose_jacobian{i} - jacob - store_pose_jacobian{i} - error(['Error in pose_jacobian at i=' int2str(i)]) - end xm = wam.fkm(q); - if xm~=store_fkm{i} - error(['Error in fkm at i=' int2str(i)]) - end - %jacob = wam.pose_jacobian(q); - %store_pose_jacobian{i} = jacob; - %xm = wam.fkm(q); - %store_fkm{i} = xm; err = vec8(xd-xm); q = q+pinv(jacob)*gain*err; plot(wam, q'); drawnow; i=i+1; end -%save('wam_kinematic_control.mat') end From 06834eb08368d59a4277ae83c4f475725fab3d8d Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Mon, 30 Mar 2020 12:47:40 +0900 Subject: [PATCH 08/15] Further optimized dh2dq and dq2dq_dot in DQ_SerialManipulator_DH --- robot_modeling/DQ_SerialManipulator_DH.m | 56 +++++++++++++----------- 1 file changed, 30 insertions(+), 26 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m index 786bf737..cd14032d 100644 --- a/robot_modeling/DQ_SerialManipulator_DH.m +++ b/robot_modeling/DQ_SerialManipulator_DH.m @@ -188,15 +188,16 @@ cosine_of_half_alpha = cos(half_alpha); % Return the optimized standard dh2dq calculation - dq = DQ(0); - dq.q(1) = cosine_of_half_alpha*cosine_of_half_theta; - dq.q(2) = sine_of_half_alpha*cosine_of_half_theta; - dq.q(3) = sine_of_half_alpha*sine_of_half_theta; - dq.q(4) = cosine_of_half_alpha*sine_of_half_theta; - dq.q(5) = -(a*sine_of_half_alpha*cosine_of_half_theta) /2.0 - (d*cosine_of_half_alpha*sine_of_half_theta)/2.0; - dq.q(6) = (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (d*sine_of_half_alpha*sine_of_half_theta )/2.0; - dq.q(7) = (a*cosine_of_half_alpha*sine_of_half_theta) /2.0 + (d*sine_of_half_alpha*cosine_of_half_theta)/2.0; - dq.q(8) = (d*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/2.0; + 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 dq_dot = dh2dq_dot(obj,q,ith) @@ -245,27 +246,30 @@ cosine_of_half_alpha = cos(half_alpha); % Return the optimized dh2dq_dot calculation - dq_dot = DQ(0); if obj.type(ith) == obj.JOINT_ROTATIONAL % If joint is rotational - dq_dot.q(1) = -(cosine_of_half_alpha*sine_of_half_theta )/2.0; - dq_dot.q(2) = -(sine_of_half_alpha*sine_of_half_theta )/2.0; - dq_dot.q(3) = (sine_of_half_alpha*cosine_of_half_theta )/2.0; - dq_dot.q(4) = (cosine_of_half_alpha*cosine_of_half_theta )/2.0; - dq_dot.q(5) = (a*sine_of_half_alpha*sine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*cosine_of_half_theta)/4.0; - dq_dot.q(6) = -(a*cosine_of_half_alpha*sine_of_half_theta )/4.0 - (d*sine_of_half_alpha*cosine_of_half_theta )/4.0; - dq_dot.q(7) = (a*cosine_of_half_alpha*cosine_of_half_theta)/4.0 - (d*sine_of_half_alpha*sine_of_half_theta )/4.0; - dq_dot.q(8) = -(a*sine_of_half_alpha*cosine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*sine_of_half_theta )/4.0; + dq_dot = DQ([ + -(cosine_of_half_alpha*sine_of_half_theta )/2.0 + -(sine_of_half_alpha*sine_of_half_theta )/2.0 + (sine_of_half_alpha*cosine_of_half_theta )/2.0 + (cosine_of_half_alpha*cosine_of_half_theta )/2.0 + (a*sine_of_half_alpha*sine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*cosine_of_half_theta)/4.0 + -(a*cosine_of_half_alpha*sine_of_half_theta )/4.0 - (d*sine_of_half_alpha*cosine_of_half_theta )/4.0 + (a*cosine_of_half_alpha*cosine_of_half_theta)/4.0 - (d*sine_of_half_alpha*sine_of_half_theta )/4.0 + -(a*sine_of_half_alpha*cosine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*sine_of_half_theta )/4.0 + ]); else % If joint is prismatic - dq_dot.q(1) = cosine_of_half_alpha*cosine_of_half_theta; - dq_dot.q(2) = sine_of_half_alpha*cosine_of_half_theta; - dq_dot.q(3) = sine_of_half_alpha*sine_of_half_theta; - dq_dot.q(4) = cosine_of_half_alpha*sine_of_half_theta; - dq_dot.q(5) = -(cosine_of_half_alpha*sine_of_half_theta )/2.0 - (a*sine_of_half_alpha*cosine_of_half_theta)/2.0; - dq_dot.q(6) = (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (sine_of_half_alpha*sine_of_half_theta )/2.0; - dq_dot.q(7) = (sine_of_half_alpha*cosine_of_half_theta )/2.0 + (a*cosine_of_half_alpha*sine_of_half_theta)/2.0; - dq_dot.q(8) = (cosine_of_half_alpha*cosine_of_half_theta )/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/2.0; + dq_dot = 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 + -(cosine_of_half_alpha*sine_of_half_theta )/2.0 - (a*sine_of_half_alpha*cosine_of_half_theta)/2.0 + (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (sine_of_half_alpha*sine_of_half_theta )/2.0 + (sine_of_half_alpha*cosine_of_half_theta )/2.0 + (a*cosine_of_half_alpha*sine_of_half_theta)/2.0 + (cosine_of_half_alpha*cosine_of_half_theta )/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/2.0 + ]); end end From d3d80e0e29c549ab7127b124bd6cab356a11d026 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Tue, 31 Mar 2020 19:16:40 +0900 Subject: [PATCH 09/15] Fixed a bug in the dh2dq_dot method of DQ_SerialManipulator_DH causing the joint derivatives to be wrong for prismatic joints --- robot_modeling/DQ_SerialManipulator_DH.m | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m index cd14032d..3d28003d 100644 --- a/robot_modeling/DQ_SerialManipulator_DH.m +++ b/robot_modeling/DQ_SerialManipulator_DH.m @@ -261,15 +261,15 @@ else % If joint is prismatic dq_dot = 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 - -(cosine_of_half_alpha*sine_of_half_theta )/2.0 - (a*sine_of_half_alpha*cosine_of_half_theta)/2.0 - (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (sine_of_half_alpha*sine_of_half_theta )/2.0 - (sine_of_half_alpha*cosine_of_half_theta )/2.0 + (a*cosine_of_half_alpha*sine_of_half_theta)/2.0 - (cosine_of_half_alpha*cosine_of_half_theta )/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/2.0 - ]); + 0; + 0; + 0; + 0; + -(cosine_of_half_alpha*sine_of_half_theta)/2.0; + -(sine_of_half_alpha*sine_of_half_theta)/2.0; + (sine_of_half_alpha*cosine_of_half_theta)/2.0; + (cosine_of_half_alpha*cosine_of_half_theta)/2.0; + ]); end end From a388302b1bc54eb2871eb0554c040c9ec2d523a8 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Thu, 2 Apr 2020 11:03:09 +0900 Subject: [PATCH 10/15] Renamed DQ_SerialManipulator_DH to DQ_SerialManipulatorDH --- robot_modeling/DQ_SerialManipulator_DH.m | 321 ----------------------- robots/Ax18ManipulatorRobot.m | 4 +- robots/BarrettWamArmRobot.m | 4 +- robots/KukaLwr4Robot.m | 4 +- robots/KukaYoubot.m | 4 +- 5 files changed, 8 insertions(+), 329 deletions(-) delete mode 100644 robot_modeling/DQ_SerialManipulator_DH.m diff --git a/robot_modeling/DQ_SerialManipulator_DH.m b/robot_modeling/DQ_SerialManipulator_DH.m deleted file mode 100644 index 3d28003d..00000000 --- a/robot_modeling/DQ_SerialManipulator_DH.m +++ /dev/null @@ -1,321 +0,0 @@ -% Concrete class that extends the DQ_SerialManipulator using the -% Denavit-Hartenberg parameters (DH) -% -% Usage: robot = DQ_SerialManipulator_DH(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_SerialManipulator_DH.JOINT_ROTATIONAL -% or DQ_SerialManipulator_DH.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_SerialManipulator_DH 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_SerialManipulator_DH < 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_SerialManipulator_DH(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.dummy = A(5,:); - - obj = obj@DQ_SerialManipulator(A,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 - - % Remove dummy joints - obj.dummy = zeros(1,obj.n_links); - obj.n_dummy = 0; - - % 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 dq_dot = dh2dq_dot(obj,q,ith) - % For a given link's Extended DH parameters, calculate the - % correspondent dual quaternion derivative of that joint's - % pose transformation with respect to its joint value. - % Usage: dq = dh2dq_dot(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_dot calculation is commented below - %if obj.type(ith) == obj.JOINT_ROTATIONAL - % % If joint is rotational - % h1 = 0.5*( -sin( (obj.theta(ith)+q) /2.0) + DQ.k*cos( (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 = DQ.E*0.5*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_dot = h1*h2*h3*h4; - - % The optimized standard dh2dq_dot 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); - 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 dh2dq_dot calculation - if obj.type(ith) == obj.JOINT_ROTATIONAL - % If joint is rotational - dq_dot = DQ([ - -(cosine_of_half_alpha*sine_of_half_theta )/2.0 - -(sine_of_half_alpha*sine_of_half_theta )/2.0 - (sine_of_half_alpha*cosine_of_half_theta )/2.0 - (cosine_of_half_alpha*cosine_of_half_theta )/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*cosine_of_half_theta)/4.0 - -(a*cosine_of_half_alpha*sine_of_half_theta )/4.0 - (d*sine_of_half_alpha*cosine_of_half_theta )/4.0 - (a*cosine_of_half_alpha*cosine_of_half_theta)/4.0 - (d*sine_of_half_alpha*sine_of_half_theta )/4.0 - -(a*sine_of_half_alpha*cosine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*sine_of_half_theta )/4.0 - ]); - else - % If joint is prismatic - dq_dot = DQ([ - 0; - 0; - 0; - 0; - -(cosine_of_half_alpha*sine_of_half_theta)/2.0; - -(sine_of_half_alpha*sine_of_half_theta)/2.0; - (sine_of_half_alpha*cosine_of_half_theta)/2.0; - (cosine_of_half_alpha*cosine_of_half_theta)/2.0; - ]); - 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 - - J = zeros(8,to_ith_link); - - %The unoptimized raw_jacobian calculation is commented below - %for i = 1:to_ith_link - % xi = DQ(1); - % for j = 1:i - % if i==j - % xi = xi*obj.dh2dq_dot(q(j),j); - % else - % xi = xi*obj.dh2dq(q(j),j); - % end - % end - % J(:,i) = vec8(xi); - %end - - %The optimized raw_jacobian calculation - x_forward = DQ(1); - x_backward = obj.raw_fkm(q,to_ith_link); - for i = 1:to_ith_link - x_i_to_i_plus_one = obj.dh2dq(q(i),i); - x_backward = conj(x_i_to_i_plus_one)*x_backward; - x_ith_dot = x_forward*obj.dh2dq_dot(q(i),i)*x_backward; - x_forward = x_forward*x_i_to_i_plus_one; - J(:,i) = vec8(x_ith_dot); - end - end - - end -end \ No newline at end of file diff --git a/robots/Ax18ManipulatorRobot.m b/robots/Ax18ManipulatorRobot.m index a6a8e64f..9ddf9c09 100644 --- a/robots/Ax18ManipulatorRobot.m +++ b/robots/Ax18ManipulatorRobot.m @@ -28,7 +28,7 @@ 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_SerialManipulator_DH.JOINT_ROTATIONAL,1,5); + ax18_DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,5); ax18_DH_matrix = [ ax18_DH_theta; ax18_DH_d; @@ -37,7 +37,7 @@ ax18_DH_type; ]; % D&H parameters matrix for the arm model - ax = DQ_SerialManipulator_DH(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 04c8a7ed..80a8c014 100644 --- a/robots/BarrettWamArmRobot.m +++ b/robots/BarrettWamArmRobot.m @@ -29,14 +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_SerialManipulator_DH.JOINT_ROTATIONAL,1,7); + 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_type]; - wam = DQ_SerialManipulator_DH(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 fa6c8a0c..759130f8 100644 --- a/robots/KukaLwr4Robot.m +++ b/robots/KukaLwr4Robot.m @@ -32,14 +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_SerialManipulator_DH.JOINT_ROTATIONAL,1,7); + 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_type]; - ret = DQ_SerialManipulator_DH(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 8cc6e126..7528ef6f 100644 --- a/robots/KukaYoubot.m +++ b/robots/KukaYoubot.m @@ -40,14 +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_SerialManipulator_DH.JOINT_ROTATIONAL,1,5); + 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_type]; - arm = DQ_SerialManipulator_DH(arm_DH_matrix,'standard'); + arm = DQ_SerialManipulatorDH(arm_DH_matrix,'standard'); base = DQ_HolonomicBase(); include_namespace_dq From fafd4dad68804ffd1f0c3ab99ed315bf35adfda6 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Thu, 9 Apr 2020 11:08:01 +0900 Subject: [PATCH 11/15] Added missing file --- robot_modeling/DQ_SerialManipulatorDH.m | 321 ++++++++++++++++++++++++ 1 file changed, 321 insertions(+) create mode 100644 robot_modeling/DQ_SerialManipulatorDH.m diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m new file mode 100644 index 00000000..fd83818f --- /dev/null +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -0,0 +1,321 @@ +% 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.dummy = A(5,:); + + obj = obj@DQ_SerialManipulator(A,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 + + % Remove dummy joints + obj.dummy = zeros(1,obj.n_links); + obj.n_dummy = 0; + + % 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 dq_dot = dh2dq_dot(obj,q,ith) + % For a given link's Extended DH parameters, calculate the + % correspondent dual quaternion derivative of that joint's + % pose transformation with respect to its joint value. + % Usage: dq = dh2dq_dot(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_dot calculation is commented below + %if obj.type(ith) == obj.JOINT_ROTATIONAL + % % If joint is rotational + % h1 = 0.5*( -sin( (obj.theta(ith)+q) /2.0) + DQ.k*cos( (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 = DQ.E*0.5*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_dot = h1*h2*h3*h4; + + % The optimized standard dh2dq_dot 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); + 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 dh2dq_dot calculation + if obj.type(ith) == obj.JOINT_ROTATIONAL + % If joint is rotational + dq_dot = DQ([ + -(cosine_of_half_alpha*sine_of_half_theta )/2.0 + -(sine_of_half_alpha*sine_of_half_theta )/2.0 + (sine_of_half_alpha*cosine_of_half_theta )/2.0 + (cosine_of_half_alpha*cosine_of_half_theta )/2.0 + (a*sine_of_half_alpha*sine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*cosine_of_half_theta)/4.0 + -(a*cosine_of_half_alpha*sine_of_half_theta )/4.0 - (d*sine_of_half_alpha*cosine_of_half_theta )/4.0 + (a*cosine_of_half_alpha*cosine_of_half_theta)/4.0 - (d*sine_of_half_alpha*sine_of_half_theta )/4.0 + -(a*sine_of_half_alpha*cosine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*sine_of_half_theta )/4.0 + ]); + else + % If joint is prismatic + dq_dot = DQ([ + 0 + 0 + 0 + 0 + -(cosine_of_half_alpha*sine_of_half_theta)/2.0 + -(sine_of_half_alpha*sine_of_half_theta)/2.0 + (sine_of_half_alpha*cosine_of_half_theta)/2.0 + (cosine_of_half_alpha*cosine_of_half_theta)/2.0; + ]); + 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 + + J = zeros(8,to_ith_link); + + %The unoptimized raw_jacobian calculation is commented below + %for i = 1:to_ith_link + % xi = DQ(1); + % for j = 1:i + % if i==j + % xi = xi*obj.dh2dq_dot(q(j),j); + % else + % xi = xi*obj.dh2dq(q(j),j); + % end + % end + % J(:,i) = vec8(xi); + %end + + %The optimized raw_jacobian calculation + x_forward = DQ(1); + x_backward = obj.raw_fkm(q,to_ith_link); + for i = 1:to_ith_link + x_i_to_i_plus_one = obj.dh2dq(q(i),i); + x_backward = conj(x_i_to_i_plus_one)*x_backward; + x_ith_dot = x_forward*obj.dh2dq_dot(q(i),i)*x_backward; + x_forward = x_forward*x_i_to_i_plus_one; + J(:,i) = vec8(x_ith_dot); + end + end + + end +end \ No newline at end of file From 0946348b70e42618f7d7ae2b0549e2789100e22c Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Thu, 23 Apr 2020 15:25:23 +0900 Subject: [PATCH 12/15] Fixed DQ_SerialManipulatorDH.m to no longer consider dummy joints. --- robot_modeling/DQ_SerialManipulatorDH.m | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index fd83818f..fcd8be7b 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -75,8 +75,8 @@ %obj.a = A(3,:); %obj.alpha = A(4,:); %obj.dummy = A(5,:); + obj = obj@DQ_SerialManipulator(A(1:4,:),convention); - obj = obj@DQ_SerialManipulator(A,convention); if nargin == 0 error('Input: matrix whose columns contain the DH parameters') end @@ -85,14 +85,8 @@ error('Input: Invalid DH matrix. It should have 5 rows.') end - % Remove dummy joints - obj.dummy = zeros(1,obj.n_links); - obj.n_dummy = 0; - % Add type obj.type = A(5,:); - - end function x = raw_fkm(obj,q,to_ith_link) From 958603690ce1be854cd2ec0c5e02a08ae8aba0ea Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Tue, 28 Apr 2020 13:23:40 +0900 Subject: [PATCH 13/15] Fixing the issues found during the review. Still needs testing --- robot_modeling/DQ_SerialManipulatorDH.m | 127 ++++++------------------ 1 file changed, 29 insertions(+), 98 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index fcd8be7b..979054c6 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -74,7 +74,6 @@ %obj.d = A(2,:); %obj.a = A(3,:); %obj.alpha = A(4,:); - %obj.dummy = A(5,:); obj = obj@DQ_SerialManipulator(A(1:4,:),convention); if nargin == 0 @@ -184,96 +183,42 @@ % 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 + + -(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 dq_dot = dh2dq_dot(obj,q,ith) - % For a given link's Extended DH parameters, calculate the - % correspondent dual quaternion derivative of that joint's - % pose transformation with respect to its joint value. - % Usage: dq = dh2dq_dot(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_dot calculation is commented below - %if obj.type(ith) == obj.JOINT_ROTATIONAL - % % If joint is rotational - % h1 = 0.5*( -sin( (obj.theta(ith)+q) /2.0) + DQ.k*cos( (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 = DQ.E*0.5*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_dot = h1*h2*h3*h4; - - % The optimized standard dh2dq_dot 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); - 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 dh2dq_dot calculation + function w = get_w(obj,ith) if obj.type(ith) == obj.JOINT_ROTATIONAL - % If joint is rotational - dq_dot = DQ([ - -(cosine_of_half_alpha*sine_of_half_theta )/2.0 - -(sine_of_half_alpha*sine_of_half_theta )/2.0 - (sine_of_half_alpha*cosine_of_half_theta )/2.0 - (cosine_of_half_alpha*cosine_of_half_theta )/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*cosine_of_half_theta)/4.0 - -(a*cosine_of_half_alpha*sine_of_half_theta )/4.0 - (d*sine_of_half_alpha*cosine_of_half_theta )/4.0 - (a*cosine_of_half_alpha*cosine_of_half_theta)/4.0 - (d*sine_of_half_alpha*sine_of_half_theta )/4.0 - -(a*sine_of_half_alpha*cosine_of_half_theta )/4.0 - (d*cosine_of_half_alpha*sine_of_half_theta )/4.0 - ]); + w = DQ.k; else - % If joint is prismatic - dq_dot = DQ([ - 0 - 0 - 0 - 0 - -(cosine_of_half_alpha*sine_of_half_theta)/2.0 - -(sine_of_half_alpha*sine_of_half_theta)/2.0 - (sine_of_half_alpha*cosine_of_half_theta)/2.0 - (cosine_of_half_alpha*cosine_of_half_theta)/2.0; - ]); + w = DQ(1); 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 + % 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 + % 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 @@ -283,31 +228,17 @@ if nargin < 3 to_ith_link = obj.n_links; end + x_effector = obj.raw_fkm(q); + x = DQ(1); J = zeros(8,to_ith_link); - %The unoptimized raw_jacobian calculation is commented below - %for i = 1:to_ith_link - % xi = DQ(1); - % for j = 1:i - % if i==j - % xi = xi*obj.dh2dq_dot(q(j),j); - % else - % xi = xi*obj.dh2dq(q(j),j); - % end - % end - % J(:,i) = vec8(xi); - %end - - %The optimized raw_jacobian calculation - x_forward = DQ(1); - x_backward = obj.raw_fkm(q,to_ith_link); - for i = 1:to_ith_link - x_i_to_i_plus_one = obj.dh2dq(q(i),i); - x_backward = conj(x_i_to_i_plus_one)*x_backward; - x_ith_dot = x_forward*obj.dh2dq_dot(q(i),i)*x_backward; - x_forward = x_forward*x_i_to_i_plus_one; - J(:,i) = vec8(x_ith_dot); + for i = 0:to_ith_link-1 + w = obj.get_w(i+1); + z = 0.5*x*w*x'; + x = x*obj.dh2dq(q(i+1),i+1); + j = z * x_effector; + J(:,i+1) = vec8(j); end end From f5aa70ac6a0a676557543e2bf7c418ab05c47326 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Tue, 28 Apr 2020 14:33:14 +0900 Subject: [PATCH 14/15] Fixing a typo related to the prismatic joints --- robot_modeling/DQ_SerialManipulatorDH.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 979054c6..f0cebf76 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -208,7 +208,7 @@ if obj.type(ith) == obj.JOINT_ROTATIONAL w = DQ.k; else - w = DQ(1); + w = DQ.E*DQ.k; end end From 004fad67b1a8db6f4a76d2190e79912f7cde4ab1 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Tue, 28 Apr 2020 16:02:45 +0900 Subject: [PATCH 15/15] Using Ad() explicitly in raw_pose_jacobian of DQ_SerialManipulatorDH. --- robot_modeling/DQ_SerialManipulatorDH.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index f0cebf76..e906bcc5 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -228,14 +228,14 @@ if nargin < 3 to_ith_link = obj.n_links; end - x_effector = obj.raw_fkm(q); + 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*x*w*x'; + z = 0.5*Ad(x,w); x = x*obj.dh2dq(q(i+1),i+1); j = z * x_effector; J(:,i+1) = vec8(j);