From daeef13a8a2f8ef1cb34f76e4fb7d9e3724d48b4 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 16:51:55 +0900 Subject: [PATCH 01/74] [CONTRIBUTING.md] Added the contributing file. --- CONTRIBUTING.md | 69 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) create mode 100644 CONTRIBUTING.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..3f953d3f --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,69 @@ +# Welcome contributors to the DQ Robotics! + +We are happy about your interest in our project. Thank you for your time. DQ Robotics is a standalone open-source library and your contributions are always welcome! + +This is a set of guidelines for contribuiting to [DQ Robotics](https://dqrobotics.github.io/). + +# Workflow + +- Fork the [master branch of the dqrobotics/matlab](https://github.com/dqrobotics/matlab). +- Propose your modifications and open a draft pull request. +- Your modifications will be tested automatically by Github actions. Specifically, Github actions runs the tests of [matlab-tests](https://github.com/dqrobotics/matlab-tests), which executes all the examples of [matlab-examples](https://github.com/dqrobotics/matlab-examples). +- Once your draft pull request passes all the tests, you can switch the status to pull request. (More details [here](https://github.blog/2019-02-14-introducing-draft-pull-requests/)). + +![Untitled Diagram](https://user-images.githubusercontent.com/23158313/150255548-b4475747-444e-4530-9b20-def1655bb4f9.png) + +## Case 1 (Common cases) + +- If your modifications pass all tests, a designated member of our team will review all the changes and they will accept them after all necessary adjustments if any. + +## Case 2 (Very rare cases) + +- In some cases, your modifications would fail some tests because of incompatibility with the current version of [matlab-tests](https://github.com/dqrobotics/matlab-tests) and/or [matlab-examples](https://github.com/dqrobotics/matlab-examples). In thoses cases, you would propose changes in [matlab-tests](https://github.com/dqrobotics/matlab-tests) and [matlab-examples](https://github.com/dqrobotics/matlab-examples) to make them compatible with your new version of the dqrobotics/matlab. +- A designated member of our team will review all the changes proposed in both [matlab-tests](https://github.com/dqrobotics/matlab-tests) and [matlab-examples](https://github.com/dqrobotics/matlab-examples). They will accept the modifications in the master branch after all necessary adjustments. At this point, it is expected that your pull request passes all the tests in the master branch but fails in the release branch. + +![master_and_release](https://user-images.githubusercontent.com/23158313/150379489-cabc85bb-dbe4-41be-a405-7b254a36092a.png) + +- Then, they will test your dqrobotics/matlab pull request making all necessary adjustments until your pull request of dqrobotics/matlab passes all the tests. After that, they will review deeply your modifications. +- Finally, your modifications will be accepted in the master branch. + +# Example + +## Fork the [dqrobotics/matlab](https://github.com/dqrobotics/matlab) in your Github account + +![fork_master](https://user-images.githubusercontent.com/23158313/149602838-133f6c09-2e16-418e-8ab6-47fb36a91056.gif) + +## Clone the forked repository + +For instance, if your forked matlab respository is https://github.com/juanjqo/matlab, then + +Type in your terminal: + +- `git clone https://github.com/juanjqo/matlab.git` + +![git_clone](https://user-images.githubusercontent.com/23158313/149603381-78732b55-2794-4be9-9a12-b7062d0649b5.gif) + +## Make your modifications + +Now, you can modify the code with your contributions. +(In this specific example, as shown in the GIF, I modified the CONTRIBUTING.md file) + +![modifications](https://user-images.githubusercontent.com/23158313/149604028-915d9325-e52a-4378-ba58-17b7fe1a7a81.gif) + +## Add, commit and push your changes + +Please indicate in your commit's message the file that was modified using brackets. For instance, if you modified the class DQ_Serialmanipulator, then you would do the following: + +- `git commit -m "[DQ_SerialManipulator] your_message_explaining_the modification."` + +![add_commit_push](https://user-images.githubusercontent.com/23158313/149603960-d69a8202-a3b1-4af5-a2d8-e1197cc26a81.gif) + +## Open a draft pull request (More details [here](https://github.blog/2019-02-14-introducing-draft-pull-requests/)) + +Now, your draft pull request will be tested by Github actions automatically. + +![pull_request](https://user-images.githubusercontent.com/23158313/149604338-52f3ba35-ef25-440a-8bc8-75194c32130e.gif) + +If your pull request fails the tests, don't worry!, you will see where your code is not working. Pick your pull request in https://github.com/dqrobotics/matlab/pulls. Then, at the end of the page, click on 'Details'. + +![failed_check](https://user-images.githubusercontent.com/23158313/149604965-677f783f-64af-4120-966a-0461c85f9418.gif) From 2f700df94ca2858d8ab83b0725b17f9df7c77b67 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Sat, 3 Dec 2022 16:57:50 +0900 Subject: [PATCH 02/74] [CONTRIBUTING.md] Updated the file. Updated the file with last Bruno's recommendations. --- CONTRIBUTING.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 3f953d3f..cc5c4888 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -7,7 +7,10 @@ This is a set of guidelines for contribuiting to [DQ Robotics](https://dqrobotic # Workflow - Fork the [master branch of the dqrobotics/matlab](https://github.com/dqrobotics/matlab). -- Propose your modifications and open a draft pull request. +- Propose your modifications and open a draft pull request. Keep in mind the following recomentations: + - Propose individual changes (several changes of the same type are allowed on the same pull request). + - Do not unnecessarily change any internal implementation that is working correctly without prior approval. + - Include a clear and concise rationale behind each pull request. - Your modifications will be tested automatically by Github actions. Specifically, Github actions runs the tests of [matlab-tests](https://github.com/dqrobotics/matlab-tests), which executes all the examples of [matlab-examples](https://github.com/dqrobotics/matlab-examples). - Once your draft pull request passes all the tests, you can switch the status to pull request. (More details [here](https://github.blog/2019-02-14-introducing-draft-pull-requests/)). From 594decdc97d535f6ecb893ad3c6dd7924060ab84 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 19:28:29 +0900 Subject: [PATCH 03/74] [DQ_Kinematics.m] Added the dim_configuration_space protected property. --- robot_modeling/DQ_Kinematics.m | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/robot_modeling/DQ_Kinematics.m b/robot_modeling/DQ_Kinematics.m index d526b1aa..795ec3bf 100644 --- a/robot_modeling/DQ_Kinematics.m +++ b/robot_modeling/DQ_Kinematics.m @@ -73,7 +73,9 @@ % Frame used to determine the robot physical location base_frame; % Robot configuration vector - q + q + % Dimension of the robot configuration space + dim_configuration_space; end methods From 0ac286f78c862c1c155dfab2b670cfc8baa8068e Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 19:56:37 +0900 Subject: [PATCH 04/74] [DQ_SerialManipulatorDH] Added the properties theta,a, d, alpha and updated the constructor of the class. --- robot_modeling/DQ_SerialManipulatorDH.m | 53 ++++++++++++++++++------- 1 file changed, 39 insertions(+), 14 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index e906bcc5..4c6a3a2f 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -53,6 +53,7 @@ classdef DQ_SerialManipulatorDH < DQ_SerialManipulator properties + theta,d,a,alpha; type end @@ -65,27 +66,51 @@ 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); + function obj = DQ_SerialManipulatorDH(A, convention) + % Constructor of the DQ_SerialManipulatorDH class. + % 'A' is a matrix 5 x number_of_joints that represent the DH parameters of the robot. + % + % Example: Consider the Stanford manipulator. Using the DH parameters, we have: + % (See Table 3.4 from Robot Modeling and Control Second Edition, Spong, Mark W. + % Hutchinson, Seth M., Vidyasagar) + % + % robot_dh = [ 0, 0, 0, 0, 0, 0; % theta + % 0,d2,d3,0,0,d6; % d + % 0, 0, 0, 0, 0, 0; % a + % -pi/2, pi/2, 0, -pi/2, pi/2,0; % alpha + % 0,0,1,0,0,0;] % Type of joints. The joints are rotational, except the third joint, which is prismatic. + % StandfordManipulator = DQ_SerialManipulatorDH(robot_dh); + + str = ['DQ_SerialManipulatorDH(A), where ' ... + 'A = [theta1 ... thetan; ' ... + ' d1 ... dn; ' ... + ' a1 ... an; ' ... + ' alpha1 ... alphan; ' ... + ' type1 ... typen]']; + if nargin == 0 - error('Input: matrix whose columns contain the DH parameters') + error(['Input: matrix whose columns contain the DH parameters' ... + ' and type of joints. Example: ' str]) + end + + if nargin == 2 + warning(['DQ_SerialManipulatorDH(A, convention) is deprecated.' ... + ' Please use DQ_SerialManipulatorDH(A) instead.']); end if(size(A,1) ~= 5) error('Input: Invalid DH matrix. It should have 5 rows.') end - - % Add type - obj.type = A(5,:); + + obj = obj@DQ_SerialManipulator(size(A,2)); + + % Add theta, d, a, alpha and type + obj.theta = A(1,:); + obj.d = A(2,:); + obj.a = A(3,:); + obj.alpha = A(4,:); + obj.type = A(5,:); end function x = raw_fkm(obj,q,to_ith_link) From 765e2f0d1862965defe96ed6c5f958f2fed5f757 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 20:03:37 +0900 Subject: [PATCH 05/74] [DQ_SerialManipulatorDH.m] Updated the documentation. --- robot_modeling/DQ_SerialManipulatorDH.m | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 4c6a3a2f..2fdc7cc1 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -2,7 +2,7 @@ % Denavit-Hartenberg parameters (DH) % % Usage: robot = DQ_SerialManipulatorDH(A) -% - 'A' is a 4 x n matrix containing the Denavit-Hartenberg parameters +% - 'A' is a 5 x n matrix containing the Denavit-Hartenberg parameters % (n is the number of links) % A = [theta1 ... thetan; % d1 ... dn; @@ -67,19 +67,9 @@ methods function obj = DQ_SerialManipulatorDH(A, convention) - % Constructor of the DQ_SerialManipulatorDH class. - % 'A' is a matrix 5 x number_of_joints that represent the DH parameters of the robot. - % - % Example: Consider the Stanford manipulator. Using the DH parameters, we have: - % (See Table 3.4 from Robot Modeling and Control Second Edition, Spong, Mark W. - % Hutchinson, Seth M., Vidyasagar) - % - % robot_dh = [ 0, 0, 0, 0, 0, 0; % theta - % 0,d2,d3,0,0,d6; % d - % 0, 0, 0, 0, 0, 0; % a - % -pi/2, pi/2, 0, -pi/2, pi/2,0; % alpha - % 0,0,1,0,0,0;] % Type of joints. The joints are rotational, except the third joint, which is prismatic. - % StandfordManipulator = DQ_SerialManipulatorDH(robot_dh); + % These are initialized in the constructor of + % DQ_SerialManipulator + % obj.dim_configuration_space = dim_configuration_space; str = ['DQ_SerialManipulatorDH(A), where ' ... 'A = [theta1 ... thetan; ' ... From 73c1693e6f7bf1e981f16d6c7ea44fc4936a617f Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 20:29:53 +0900 Subject: [PATCH 06/74] [DQ_SerialManipulator.m] Defined the class as an abstract class and updated the constructor. --- robot_modeling/DQ_SerialManipulator.m | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index d7356871..269ea9ab 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -50,10 +50,10 @@ % confusion, specially among those trying to learn the library. The % affected methods are: FKM and Jacobian. -classdef DQ_SerialManipulator < DQ_Kinematics +classdef (Abstract) DQ_SerialManipulator < DQ_Kinematics properties - theta,d,a,alpha; - convention; + %theta,d,a,alpha; + %convention; effector; % Properties for the plot function @@ -69,20 +69,22 @@ end methods - function obj = DQ_SerialManipulator(A,convention) + function obj = DQ_SerialManipulator(dim_configuration_space) if nargin == 0 error('Input: matrix whose columns contain the DH parameters') end - obj.n_links = size(A,2); - obj.theta = A(1,:); - obj.d = A(2,:); - obj.a = A(3,:); - obj.alpha = A(4,:); +% obj.n_links = size(A,2); +% obj.theta = A(1,:); +% obj.d = A(2,:); +% obj.a = A(3,:); +% obj.alpha = A(4,:); obj.reference_frame = DQ(1); %Default base's pose obj.base_frame = DQ(1); obj.effector = DQ(1); %Default effector's pose + obj.dim_configuration_space = dim_configuration_space; + obj.n_links = dim_configuration_space; % Define a unique robot name obj.name = sprintf('%f',rand(1)); From 2157e3b833f0203b08c113ef511ed08df4b9917d Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 20:56:56 +0900 Subject: [PATCH 07/74] [DQ_SerialManipulator.m] Defined abstract methods. Removed raw_fkm. --- robot_modeling/DQ_SerialManipulator.m | 61 ++++++++++++++------------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 269ea9ab..13f3f9dd 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -53,7 +53,7 @@ classdef (Abstract) DQ_SerialManipulator < DQ_Kinematics properties %theta,d,a,alpha; - %convention; + convention; effector; % Properties for the plot function @@ -67,6 +67,35 @@ handle n_links; end + + methods (Abstract) + % 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 + J = raw_pose_jacobian(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. + pose = raw_fkm(obj,q, to_ith_link); + end methods function obj = DQ_SerialManipulator(dim_configuration_space) @@ -88,7 +117,7 @@ % Define a unique robot name obj.name = sprintf('%f',rand(1)); - + if nargin==1 obj.convention='standard'; else @@ -111,34 +140,6 @@ function set_effector(obj,effector) obj.effector = DQ(effector); end - function x = raw_fkm(obj,q, ith) - % 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 - % - % This is an auxiliary function to be used mainly with the - % Jacobian function. - - if nargin == 3 - n = ith; - else - n = obj.n_links; - end - - if length(q) ~= obj.n_links - error('Incorrect number of joint variables'); - end - - x = DQ(1); - - for i=1:n - x = x*dh2dq(obj,q(i),i); - end - end - function x = fkm(obj,q, ith) % FKM(q) calculates the forward kinematic model and % returns the dual quaternion corresponding to the From 22ea22dddaafd403fb685a081d8350d17584b523 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 20:58:09 +0900 Subject: [PATCH 08/74] [DQ_SerialManipulatorDH.m] Removed fkm, which is now in DQ_SerialManipulator. --- robot_modeling/DQ_SerialManipulatorDH.m | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 2fdc7cc1..96c68d59 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -128,25 +128,6 @@ 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 From a4c3396e6ece306d81360169b2520053ac3abd65 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 22:02:52 +0900 Subject: [PATCH 09/74] [DQ_SerialManipulator.m] Removed raw_pose_jacobian_derivative. --- robot_modeling/DQ_SerialManipulator.m | 80 +++++++++++++-------------- 1 file changed, 40 insertions(+), 40 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 13f3f9dd..e0c69132 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -220,46 +220,46 @@ function set_effector(obj,effector) p(8) = h(4)*h(8)-h(3)*h(7)-h(2)*h(6)+h(1)*h(5); end - function J = raw_pose_jacobian(obj,q,ith) - % 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 - n = ith; - x_effector = obj.raw_fkm(q,ith); - else - n = obj.n_links; - x_effector = obj.raw_fkm(q); - end - - x = DQ(1); - J = zeros(8,n); - - for i = 0:n-1 - % Use the standard DH convention - if strcmp(obj.convention,'standard') - z = DQ(obj.get_z(x.q)); - else % Use the modified DH convention - w = DQ([0,0,-sin(obj.alpha(i+1)),cos(obj.alpha(i+1)),0, ... - 0,-obj.a(i+1)*cos(obj.alpha(i+1)), ... - -obj.a(i+1)*sin(obj.alpha(i+1))] ); - z = 0.5*x*w*x'; - end - - x = x*obj.dh2dq(q(i+1),i+1); - j = z * x_effector; - J(:,i+1) = vec8(j); - end - end +% function J = raw_pose_jacobian(obj,q,ith) +% % 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 +% n = ith; +% x_effector = obj.raw_fkm(q,ith); +% else +% n = obj.n_links; +% x_effector = obj.raw_fkm(q); +% end +% +% x = DQ(1); +% J = zeros(8,n); +% +% for i = 0:n-1 +% % Use the standard DH convention +% if strcmp(obj.convention,'standard') +% z = DQ(obj.get_z(x.q)); +% else % Use the modified DH convention +% w = DQ([0,0,-sin(obj.alpha(i+1)),cos(obj.alpha(i+1)),0, ... +% 0,-obj.a(i+1)*cos(obj.alpha(i+1)), ... +% -obj.a(i+1)*sin(obj.alpha(i+1))] ); +% z = 0.5*x*w*x'; +% end +% +% x = x*obj.dh2dq(q(i+1),i+1); +% j = z * x_effector; +% J(:,i+1) = vec8(j); +% end +% end function J = pose_jacobian(obj, q, ith) % POSE_JACOBIAN(q) returns the Jacobian that satisfies From 2ad3a6910879408bfa780d4f8f4db5b99b61e244 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 22:03:40 +0900 Subject: [PATCH 10/74] [DQ_SerialManipulator.m] Removed raw_pose_jacobian_derivative. --- robot_modeling/DQ_SerialManipulator.m | 43 +-------------------------- 1 file changed, 1 insertion(+), 42 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index e0c69132..64334bc5 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -218,48 +218,7 @@ function set_effector(obj,effector) p(6) = h(2)*h(8)+h(6)*h(4)+h(1)*h(7)+h(5)*h(3); p(7) = h(3)*h(8)+h(7)*h(4)-h(1)*h(6)-h(5)*h(2); p(8) = h(4)*h(8)-h(3)*h(7)-h(2)*h(6)+h(1)*h(5); - end - -% function J = raw_pose_jacobian(obj,q,ith) -% % 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 -% n = ith; -% x_effector = obj.raw_fkm(q,ith); -% else -% n = obj.n_links; -% x_effector = obj.raw_fkm(q); -% end -% -% x = DQ(1); -% J = zeros(8,n); -% -% for i = 0:n-1 -% % Use the standard DH convention -% if strcmp(obj.convention,'standard') -% z = DQ(obj.get_z(x.q)); -% else % Use the modified DH convention -% w = DQ([0,0,-sin(obj.alpha(i+1)),cos(obj.alpha(i+1)),0, ... -% 0,-obj.a(i+1)*cos(obj.alpha(i+1)), ... -% -obj.a(i+1)*sin(obj.alpha(i+1))] ); -% z = 0.5*x*w*x'; -% end -% -% x = x*obj.dh2dq(q(i+1),i+1); -% j = z * x_effector; -% J(:,i+1) = vec8(j); -% end -% end + end function J = pose_jacobian(obj, q, ith) % POSE_JACOBIAN(q) returns the Jacobian that satisfies From e2bed8eee71669ea0885a8c6853edad67d80a6a2 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 22:05:05 +0900 Subject: [PATCH 11/74] [DQ_SerialManipulatorDH.m] Added the pose_jacobian_derivative. --- robot_modeling/DQ_SerialManipulatorDH.m | 52 +++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 96c68d59..e897da2b 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -237,6 +237,58 @@ J(:,i+1) = vec8(j); end end + + function J_dot = pose_jacobian_derivative(obj,q,q_dot, to_ith_link) + % POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian + % time derivative. + % + % POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first + % to_ith_link columns of the Jacobian time derivative. + % This function does not take into account any base or + % end-effector displacements. + % obj.check_q_vec(q); + % obj.check_q_vec(q_dot); + + if nargin == 4 + n = to_ith_link; + x_effector = obj.raw_fkm(q,to_ith_link); + J = obj.raw_pose_jacobian(q,to_ith_link); + vec_x_effector_dot = J*q_dot(1:to_ith_link); + else + n = obj.dim_configuration_space; + % obj.check_to_ith_link(n); + x_effector = obj.raw_fkm(q); + J = obj.raw_pose_jacobian(q); + vec_x_effector_dot = J*q_dot; + end + + x = DQ(1); + J_dot = zeros(8,n); + + for i = 0:n-1 + % Use the standard DH convention + + w = obj.get_w(i+1); %w = DQ.k; + %z = DQ(obj.get_z(x.q)); + z = 0.5*x*w*conj(x); + + % When i = 0 and length(theta) = 1, theta(1,i) returns + % a 1 x 0 vector, differently from the expected + % behavior, which is to return a 0 x 1 matrix. + % Therefore, we have to deal with the case i = 0 + % explictly. + if i ~= 0 + vec_zdot = 0.5*(haminus8(w*x') + ... + hamiplus8(x*w)*DQ.C8) * ... + obj.raw_pose_jacobian(q,i)*q_dot(1:i); + else + vec_zdot = zeros(8,1); + end + J_dot(:,i+1) = haminus8(x_effector)*vec_zdot +... + hamiplus8(z)*vec_x_effector_dot; + x = x*obj.dh2dq(q(i+1),i+1); + end + end end end \ No newline at end of file From 3aa7aca3831f3f9ed8b447cd47f47cd42a5c41c7 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 22:06:43 +0900 Subject: [PATCH 12/74] [DQ_Kinematics.m] Removed changes in DQ_Kinematics --- robot_modeling/DQ_Kinematics.m | 2 -- 1 file changed, 2 deletions(-) diff --git a/robot_modeling/DQ_Kinematics.m b/robot_modeling/DQ_Kinematics.m index 795ec3bf..9355592c 100644 --- a/robot_modeling/DQ_Kinematics.m +++ b/robot_modeling/DQ_Kinematics.m @@ -74,8 +74,6 @@ base_frame; % Robot configuration vector q - % Dimension of the robot configuration space - dim_configuration_space; end methods From 44f8bb97426aaef46c4e12f7a2547d1bb04f27dc Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 22:10:19 +0900 Subject: [PATCH 13/74] [DQ_SerialManipulator.m] Removed dim_configuration_space to keep using the n_links property. --- robot_modeling/DQ_SerialManipulator.m | 1 - 1 file changed, 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 64334bc5..0869671c 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -112,7 +112,6 @@ obj.reference_frame = DQ(1); %Default base's pose obj.base_frame = DQ(1); obj.effector = DQ(1); %Default effector's pose - obj.dim_configuration_space = dim_configuration_space; obj.n_links = dim_configuration_space; % Define a unique robot name From 0239e7102284a724929267525b768671ea53c313 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 22:10:52 +0900 Subject: [PATCH 14/74] [DQ_SerialManipulatorDH.m] Removed dim_configuration_space to keep using the n_links property. --- 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 e897da2b..83a4441d 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -255,7 +255,7 @@ J = obj.raw_pose_jacobian(q,to_ith_link); vec_x_effector_dot = J*q_dot(1:to_ith_link); else - n = obj.dim_configuration_space; + n = obj.n_links; % obj.check_to_ith_link(n); x_effector = obj.raw_fkm(q); J = obj.raw_pose_jacobian(q); From c65a7408b2ad32e1bb4fb33f5e5a9773c262db23 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 22:20:53 +0900 Subject: [PATCH 15/74] [ComauSmartSixRobot.m] Updated the robot definition. --- robots/ComauSmartSixRobot.m | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/robots/ComauSmartSixRobot.m b/robots/ComauSmartSixRobot.m index 71935483..39820bcc 100644 --- a/robots/ComauSmartSixRobot.m +++ b/robots/ComauSmartSixRobot.m @@ -32,13 +32,15 @@ comau_DH_d = [-0.45, 0, 0, -0.64707, 0, -0.095]; comau_DH_a = [0, 0.150, 0.590, 0.13, 0, 0]; comau_DH_alpha = [pi, pi/2, pi, -pi/2, -pi/2, pi/2]; + comau_DH_type = [1,1,1,1,1,1]; comau_DH_matrix = [comau_DH_theta; comau_DH_d; comau_DH_a; - comau_DH_alpha]; + comau_DH_alpha + comau_DH_type]; - comau = DQ_SerialManipulator(comau_DH_matrix, 'modified'); + comau = DQ_SerialManipulatorDH(comau_DH_matrix); % There is a final transformation for the end-effector given by % a rotation of pi around the local x-axis followed by a From 960f323230a88e101942775b77e5c301449eb772 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 23:39:27 +0900 Subject: [PATCH 16/74] [DQ_SerialManipulator.m] Defined the raw_pose_jacobian_derivative as an abstract method and defined its concrete version. --- robot_modeling/DQ_SerialManipulator.m | 72 ++++++++------------------- 1 file changed, 22 insertions(+), 50 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 0869671c..a9d83ebb 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -51,8 +51,7 @@ % affected methods are: FKM and Jacobian. classdef (Abstract) DQ_SerialManipulator < DQ_Kinematics - properties - %theta,d,a,alpha; + properties convention; effector; @@ -81,7 +80,16 @@ % end-effector displacements and should be used mostly % internally in DQ_kinematics J = raw_pose_jacobian(obj, q,to_ith_link); - + + + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian + % time derivative. + % + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot, to_ith_link) returns the first + % to_ith_link columns of the Jacobian time derivative. + % This function does not take into account any base or + % end-effector displacements. + J_dot = raw_pose_jacobian_derivative(obj, q, q_dot, to_ith_link); % RAW_FKM(q) calculates the forward kinematic model and % returns the dual quaternion corresponding to the @@ -103,12 +111,6 @@ error('Input: matrix whose columns contain the DH parameters') end -% obj.n_links = size(A,2); -% obj.theta = A(1,:); -% obj.d = A(2,:); -% obj.a = A(3,:); -% obj.alpha = A(4,:); - obj.reference_frame = DQ(1); %Default base's pose obj.base_frame = DQ(1); obj.effector = DQ(1); %Default effector's pose @@ -250,48 +252,18 @@ function set_effector(obj,effector) % This function does not take into account any base or % end-effector displacements. - if nargin == 4 - n = ith; - x_effector = obj.raw_fkm(q,ith); - J = obj.raw_pose_jacobian(q,ith); - vec_x_effector_dot = J*q_dot(1:ith); + if nargin == 4 && ith < obj.n_links + % If the Jacobian is not related to the mapping between the + % end-effector velocities and the joint velocities, it takes + % into account only the base displacement + J_dot = hamiplus8(obj.reference_frame)*obj.raw_pose_jacobian_derivative(... + q, q_dot, ith); else - n = obj.n_links; - x_effector = obj.raw_fkm(q); - J = obj.raw_pose_jacobian(q); - vec_x_effector_dot = J*q_dot; - end - - x = DQ(1); - J_dot = zeros(8,n); - - for i = 0:n-1 - % Use the standard DH convention - if strcmp(obj.convention,'standard') - w = DQ.k; - z = DQ(obj.get_z(x.q)); - else % Use the modified DH convention - w = DQ([0,0,-sin(obj.alpha(i+1)),cos(obj.alpha(i+1)),0, ... - 0,-obj.a(i+1)*cos(obj.alpha(i+1)),... - -obj.a(i+1)*sin(obj.alpha(i+1))] ); - z = 0.5*x*w*x'; - end - - % When i = 0 and length(theta) = 1, theta(1,i) returns - % a 1 x 0 vector, differently from the expected - % behavior, which is to return a 0 x 1 matrix. - % Therefore, we have to deal with the case i = 0 - % explictly. - if i ~= 0 - vec_zdot = 0.5*(haminus8(w*x') + ... - hamiplus8(x*w)*DQ.C8) * ... - obj.raw_pose_jacobian(q,i)*q_dot(1:i); - else - vec_zdot = zeros(8,1); - end - J_dot(:,i+1) = haminus8(x_effector)*vec_zdot +... - hamiplus8(z)*vec_x_effector_dot; - x = x*obj.dh2dq(q(i+1),i+1); + % Otherwise, it the Jacobian is related to the + % end-effector velocity, it takes into account both base + % and end-effector (constant) displacements. + J_dot = hamiplus8(obj.reference_frame)*haminus8(obj.effector)*... + obj.raw_pose_jacobian_derivative(q, q_dot); end end From 10e01b18cc4d083f7e50001cad1855f987ef2efe Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 23:41:49 +0900 Subject: [PATCH 17/74] [DQ_SerialManipulatorDH.m] Modified the name of the method to raw_pose_jacobian_derivative. --- robot_modeling/DQ_SerialManipulatorDH.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 83a4441d..67e9686f 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -238,11 +238,11 @@ end end - function J_dot = pose_jacobian_derivative(obj,q,q_dot, to_ith_link) - % POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian + function J_dot = raw_pose_jacobian_derivative(obj,q,q_dot, to_ith_link) + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian % time derivative. % - % POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first % to_ith_link columns of the Jacobian time derivative. % This function does not take into account any base or % end-effector displacements. From e928764e6c5420679bcbed258915b29d77acc404 Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 23:43:04 +0900 Subject: [PATCH 18/74] [DQ_SerialManipulatorMDH.m] Added a concrete class to model robots using the modified DH convention. --- robot_modeling/DQ_SerialManipulatorMDH.m | 311 +++++++++++++++++++++++ 1 file changed, 311 insertions(+) create mode 100644 robot_modeling/DQ_SerialManipulatorMDH.m diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m new file mode 100644 index 00000000..ebcfdc02 --- /dev/null +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -0,0 +1,311 @@ +% Concrete class that extends the DQ_SerialManipulator using the Modified +% Denavit-Hartenberg parameters (MDH) +% +% Usage: robot = DQ_SerialManipulatorMDH(A) +% - 'A' is a 5 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.REVOLUTE +% or DQ_SerialManipulatorDH.PRISMATIC +% - The only accepted convention in this subclass is the 'standard' DH +% convention. +% +% If the joint is of type REVOLUTE, 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_dh_parameters_theta - Returns the vector containing the theta parameters of the MDH table. +% get_dh_parameters_d - Returns the vector containing the d parameters of the MDH table. +% get_dh_parameters_a - Returns the vector containing the a parameters of the MDH table. +% get_dh_parameters_alpha - Returns the vector containing the alpha parameters of the MDH table. +% get_joint_types - Returns the joint type, which can be either REVOLUTE or PRISMATIC. +% 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. + + +% (C) Copyright 2022 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: +% Bruno Vihena Adorno - bruno.adorno@manchester.ac.uk +% Murilo M. Marinho - murilo@nml.t.u-tokyo.ac.jp +% Juan Jose Quiroz Omana - juanjqo@g.ecc.u-tokyo.ac.jp + +classdef DQ_SerialManipulatorMDH < DQ_SerialManipulator + + + properties (Access = protected) + mdh_matrix_; + + end + + properties (Constant) + % Joints that can be actuated + % Revolute joint + JOINT_ROTATIONAL = 1; + % Prismatic joint + JOINT_PRISMATIC = 2; + end + + methods (Access = protected) + function w = get_w(obj,ith) + % This method returns the term 'w' related with the time derivative of + % the unit dual quaternion pose using the Modified DH convention. + % See. eq (2.32) of 'Two-arm Manipulation: From Manipulators to Enhanced + % Human-Robot Collaboration' by Bruno Adorno. + % Usage: w = get_w(ith), where + % ith: link number + joint_type = obj.mdh_matrix_(5,ith); + alpha = obj.mdh_matrix_(4,ith); + a = obj.mdh_matrix_(3,ith); + + if joint_type == obj.JOINT_ROTATIONAL + w = -DQ.j*sin(alpha)+ DQ.k*cos(alpha)... + -DQ.E*a*(DQ.j*cos(alpha) + DQ.k*sin(alpha)); + else + w = DQ.E*(cos(alpha)*DQ.k - sin(alpha)*DQ.j); + end + end + + function dq = mdh2dq(obj,q,ith) + % For a given link's Extended MDH 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 + + + half_theta = obj.mdh_matrix_(1,ith)/2.0; + d = obj.mdh_matrix_(2,ith); + a = obj.mdh_matrix_(3,ith); + half_alpha = obj.mdh_matrix_(4,ith)/2.0; + joint_type = obj.mdh_matrix_(5,ith); + + % Add the effect of the joint value + if joint_type == obj.JOINT_ROTATIONAL + % If joint is revolute + 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); + + d2 = d/2; + a2 = a/2; + h(1) = cosine_of_half_alpha*cosine_of_half_theta; + h(2) = sine_of_half_alpha*cosine_of_half_theta; + h(3) = -sine_of_half_alpha*sine_of_half_theta; + h(4) = cosine_of_half_alpha*sine_of_half_theta; + h(5) = -a2*h(2) - d2*h(4); + h(6) = a2*h(1) - d2*-h(3); + h(7) = -a2*h(4) - d2*h(2); + h(8) = d2*h(1) - a2*-h(3); + dq = DQ(h); + end + end + + methods + function obj = DQ_SerialManipulatorMDH(A) + % These are initialized in the constructor of + % DQ_SerialManipulator + str = ['DQ_SerialManipulatorMDH(A), where ' ... + 'A = [theta1 ... thetan; ' ... + ' d1 ... dn; ' ... + ' a1 ... an; ' ... + ' alpha1 ... alphan; ' ... + ' type1 ... typen]']; + if nargin == 0 + error(['Input: matrix whose columns contain the MDH parameters' ... + ' and type of joints. Example: ' str]) + end + + if(size(A,1) ~= 5) + error(['Input: Invalid DH matrix. It should have 5 rows.' ... + ' Example: ' str]) + end + + obj = obj@DQ_SerialManipulator(size(A,2)); + obj.mdh_matrix_ = A; + + end + +% Methods that will be added in future versions but with different +% names and/or signatures. + +% function th = get_dh_parameters_theta(obj) +% %GET_DH_PARAMETERS_THETA() Returns the vector containing the theta parameters of the MDH table. +% th = obj.mdh_matrix_(1,:); +% end +% +% function ds = get_dh_parameters_d(obj) +% % GET_DH_PARAMETERS_D() Returns the vector containing the d parameters of the MDH table. +% ds = obj.mdh_matrix_(2,:); +% end +% +% function as = get_dh_parameters_a(obj) +% % GET_DH_PARAMETERS_A() Returns the vector containing the a parameters of the MDH table. +% as = obj.mdh_matrix_(3,:); +% end +% +% function alphas = get_dh_parameters_alpha(obj) +% % GET_DH_PARAMETERS_ALPHA() Returns the vector containing the alpha parameters of the MDH table. +% alphas = obj.mdh_matrix_(4,:); +% end +% +% function types = get_joint_types(obj) +% % GET_JOINT_TYPES() Returns the joint type, which can be either REVOLUTE or PRISMATIC. +% types = obj.mdh_matrix_(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. + + %obj.check_q_vec(q); + + if nargin == 3 + n = to_ith_link; + else + n = obj.n_links; + end + + %obj.check_to_ith_link(n); + x = DQ(1); + + for i=1:n + x = x*mdh2dq(obj,q(i),i); + 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 + % obj.check_q_vec(q); + + if nargin == 3 + n = to_ith_link; + else + n = obj.n_links; + end + + %obj.check_to_ith_link(n); + x_effector = obj.raw_fkm(q,n); + + x = DQ(1); + J = zeros(8,n); + for i = 1:n + w = obj.get_w(i); + z = 0.5*Ad(x,w); + x = x*obj.mdh2dq(q(i),i); + j = z * x_effector; + J(:,i) = vec8(j); + end + + end + + % TODO: + % This method is not defined in the DQ_Kinematics superclass + % we need to fix it in the future. + function J_dot = raw_pose_jacobian_derivative(obj,q,q_dot, to_ith_link) + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian + % time derivative. + % + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first + % to_ith_link columns of the Jacobian time derivative. + % This function does not take into account any base or + % end-effector displacements. + %obj.check_q_vec(q); + %obj.check_q_vec(q_dot); + + if nargin == 4 + n = to_ith_link; + x_effector = obj.raw_fkm(q,to_ith_link); + J = obj.raw_pose_jacobian(q,to_ith_link); + vec_x_effector_dot = J*q_dot(1:to_ith_link); + else + n = obj.n_links; + %obj.check_to_ith_link(n); + x_effector = obj.raw_fkm(q); + J = obj.raw_pose_jacobian(q); + vec_x_effector_dot = J*q_dot; + end + + + x = DQ(1); + J_dot = zeros(8,n); + + for i = 0:n-1 + % Use the standard DH convention + w = obj.get_w(i+1); + z = 0.5*x*w*conj(x); + + % When i = 0 and length(theta) = 1, theta(1,i) returns + % a 1 x 0 vector, differently from the expected + % behavior, which is to return a 0 x 1 matrix. + % Therefore, we have to deal with the case i = 0 + % explictly. + if i ~= 0 + vec_zdot = 0.5*(haminus8(w*x') + ... + hamiplus8(x*w)*DQ.C8) * ... + obj.raw_pose_jacobian(q,i)*q_dot(1:i); + else + vec_zdot = zeros(8,1); + end + J_dot(:,i+1) = haminus8(x_effector)*vec_zdot +... + hamiplus8(z)*vec_x_effector_dot; + x = x*obj.mdh2dq(q(i+1),i+1); + end + end + + end +end \ No newline at end of file From 91e710643be4bb0ddbb2efdf97b6f568945627aa Mon Sep 17 00:00:00 2001 From: Juancho Date: Sat, 3 Dec 2022 23:54:54 +0900 Subject: [PATCH 19/74] [DQ_Kinematics.h] Fixed missing semi-colon. --- robot_modeling/DQ_Kinematics.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_Kinematics.m b/robot_modeling/DQ_Kinematics.m index 9355592c..f774233e 100644 --- a/robot_modeling/DQ_Kinematics.m +++ b/robot_modeling/DQ_Kinematics.m @@ -73,7 +73,7 @@ % Frame used to determine the robot physical location base_frame; % Robot configuration vector - q + q; end methods From ce99e1d183b6bacb96c1d71a4dfc1702be2f7ce8 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sat, 3 Dec 2022 23:58:01 +0900 Subject: [PATCH 20/74] [DQ_SerialManipulator.m] the Fixed error message of the constructor. --- robot_modeling/DQ_SerialManipulator.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index a9d83ebb..740af7ab 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -108,7 +108,7 @@ methods function obj = DQ_SerialManipulator(dim_configuration_space) if nargin == 0 - error('Input: matrix whose columns contain the DH parameters') + error('Input: dimension of the configuration space') end obj.reference_frame = DQ(1); %Default base's pose From 80f5254c7cf19c6e6e3eca9ae1da91c1a7647b9c Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 00:26:26 +0900 Subject: [PATCH 21/74] [DQ_SerialManipulator.m] Added comments in some lines. --- robot_modeling/DQ_SerialManipulator.m | 102 ++++++++++++++------------ 1 file changed, 54 insertions(+), 48 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 740af7ab..8509f590 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -161,54 +161,56 @@ function set_effector(obj,effector) x = obj.reference_frame*obj.raw_fkm(q)*obj.effector; end end - - function dq = dh2dq(obj,theta,i) - % For a given link's DH parameters, calculate the correspondent dual - % quaternion - % Usage: dq = dh2dq(theta,i), where - % theta: joint angle - % i: link number - - if nargin ~= 3 - error('Wrong number of arguments. The parameters are theta and the correspondent link') - end - d = obj.d(i); - a = obj.a(i); - alpha = obj.alpha(i); - - if strcmp(obj.convention,'standard') - h(1) = cos((theta+obj.theta(i))/2)*cos(alpha/2); - h(2) = cos((theta+obj.theta(i))/2)*sin(alpha/2); - h(3) = sin((theta+obj.theta(i))/2)*sin(alpha/2); - h(4) = sin((theta+obj.theta(i))/2)*cos(alpha/2); - d2 = d/2; - a2 = a/2; - - - h(5) = -d2*h(4)-a2*h(2); - h(6) = -d2*h(3)+a2*h(1); - h(7) = d2*h(2)+a2*h(4); - h(8) = d2*h(1)-a2*h(3); - else - h1 = cos((theta+obj.theta(i))/2)*cos(alpha/2); - h2 = cos((theta+obj.theta(i))/2)*sin(alpha/2); - h3 = sin((theta+obj.theta(i))/2)*sin(alpha/2); - h4 = sin((theta+obj.theta(i))/2)*cos(alpha/2); - h(1) = h1; - h(2) = h2; - h(3) = -h3; - h(4) = h4; - d2 = d/2; - a2 = a/2; - - h(5) = -d2*h4-a2*h2; - h(6) = -d2*h3+a2*h1; - h(7) = -(d2*h2+a2*h4); - h(8) = d2*h1-a2*h3; - end - - dq = DQ(h); - end + +% This method is not required in the abstrac class. +% +% function dq = dh2dq(obj,theta,i) +% % For a given link's DH parameters, calculate the correspondent dual +% % quaternion +% % Usage: dq = dh2dq(theta,i), where +% % theta: joint angle +% % i: link number +% +% if nargin ~= 3 +% error('Wrong number of arguments. The parameters are theta and the correspondent link') +% end +% d = obj.d(i); +% a = obj.a(i); +% alpha = obj.alpha(i); +% +% if strcmp(obj.convention,'standard') +% h(1) = cos((theta+obj.theta(i))/2)*cos(alpha/2); +% h(2) = cos((theta+obj.theta(i))/2)*sin(alpha/2); +% h(3) = sin((theta+obj.theta(i))/2)*sin(alpha/2); +% h(4) = sin((theta+obj.theta(i))/2)*cos(alpha/2); +% d2 = d/2; +% a2 = a/2; +% +% +% h(5) = -d2*h(4)-a2*h(2); +% h(6) = -d2*h(3)+a2*h(1); +% h(7) = d2*h(2)+a2*h(4); +% h(8) = d2*h(1)-a2*h(3); +% else +% h1 = cos((theta+obj.theta(i))/2)*cos(alpha/2); +% h2 = cos((theta+obj.theta(i))/2)*sin(alpha/2); +% h3 = sin((theta+obj.theta(i))/2)*sin(alpha/2); +% h4 = sin((theta+obj.theta(i))/2)*cos(alpha/2); +% h(1) = h1; +% h(2) = h2; +% h(3) = -h3; +% h(4) = h4; +% d2 = d/2; +% a2 = a/2; +% +% h(5) = -d2*h4-a2*h2; +% h(6) = -d2*h3+a2*h1; +% h(7) = -(d2*h2+a2*h4); +% h(8) = d2*h1-a2*h3; +% end +% +% dq = DQ(h); +% end function p = get_z(~,h) p(1) = 0; @@ -702,6 +704,10 @@ function update_robot(robot, q) for i=1:robot.n_links % Since the maximum reaching distance are given by the link offset % and link length, we add them. + + % TODO + % This part is strange because it assumes we are using the + % DH parametrization. We need to fix it in future versions. reach = reach + abs(robot.a(i)) + abs(robot.d(i)); end o.workspace = [-reach reach -reach reach -reach reach]; From 60e8414bedf76378a0a756782845f6a71b8eb67c Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 00:28:39 +0900 Subject: [PATCH 22/74] [DQ_SerialManipulatorDH.m] Protected the methods get_w and dh2dq, as in C++. --- robot_modeling/DQ_SerialManipulatorDH.m | 141 +++++++++++++----------- 1 file changed, 76 insertions(+), 65 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 67e9686f..d28d4f56 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -64,71 +64,8 @@ % Prismatic joint JOINT_PRISMATIC = 2; end - - methods - function obj = DQ_SerialManipulatorDH(A, convention) - % These are initialized in the constructor of - % DQ_SerialManipulator - % obj.dim_configuration_space = dim_configuration_space; - str = ['DQ_SerialManipulatorDH(A), where ' ... - 'A = [theta1 ... thetan; ' ... - ' d1 ... dn; ' ... - ' a1 ... an; ' ... - ' alpha1 ... alphan; ' ... - ' type1 ... typen]']; - - - if nargin == 0 - error(['Input: matrix whose columns contain the DH parameters' ... - ' and type of joints. Example: ' str]) - end - - if nargin == 2 - warning(['DQ_SerialManipulatorDH(A, convention) is deprecated.' ... - ' Please use DQ_SerialManipulatorDH(A) instead.']); - end - - if(size(A,1) ~= 5) - error('Input: Invalid DH matrix. It should have 5 rows.') - end - - obj = obj@DQ_SerialManipulator(size(A,2)); - - % Add theta, d, a, alpha and type - obj.theta = A(1,:); - obj.d = A(2,:); - obj.a = A(3,:); - obj.alpha = A(4,:); - 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 - - + methods (Access = protected) function dq = dh2dq(obj,q,ith) % For a given link's Extended DH parameters, calculate the correspondent dual % quaternion @@ -200,13 +137,87 @@ ]); end - function w = get_w(obj,ith) + function w = get_w(obj,ith) + % This method returns the term 'w' related with the time derivative of + % the unit dual quaternion pose using the Standard DH convention. + % See. eq (2.32) of 'Two-arm Manipulation: From Manipulators to Enhanced + % Human-Robot Collaboration' by Bruno Adorno. + % Usage: w = get_w(ith), where + % ith: link number if obj.type(ith) == obj.JOINT_ROTATIONAL w = DQ.k; else + % see Table 1 of "Dynamics of Mobile Manipulators using Dual Quaternion Algebra." + % by Silva, F. F. A., Quiroz-Omaña, J. J., and Adorno, B. V. (April 12, 2022). + % ASME. J. Mechanisms Robotics. doi: https://doi.org/10.1115/1.4054320 w = DQ.E*DQ.k; end end + end + + methods + function obj = DQ_SerialManipulatorDH(A, convention) + % These are initialized in the constructor of + % DQ_SerialManipulator + % obj.dim_configuration_space = dim_configuration_space; + + str = ['DQ_SerialManipulatorDH(A), where ' ... + 'A = [theta1 ... thetan; ' ... + ' d1 ... dn; ' ... + ' a1 ... an; ' ... + ' alpha1 ... alphan; ' ... + ' type1 ... typen]']; + + + if nargin == 0 + error(['Input: matrix whose columns contain the DH parameters' ... + ' and type of joints. Example: ' str]) + end + + if nargin == 2 + warning(['DQ_SerialManipulatorDH(A, convention) is deprecated.' ... + ' Please use DQ_SerialManipulatorDH(A) instead.']); + end + + if(size(A,1) ~= 5) + error('Input: Invalid DH matrix. It should have 5 rows.') + end + + obj = obj@DQ_SerialManipulator(size(A,2)); + + % Add theta, d, a, alpha and type + obj.theta = A(1,:); + obj.d = A(2,:); + obj.a = A(3,:); + obj.alpha = A(4,:); + 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 J = raw_pose_jacobian(obj,q,to_ith_link) % RAW_POSE_JACOBIAN(q) returns the Jacobian that satisfies From 926ee75c7f80dab5cce3337900d1ac478e8f1b75 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 00:44:31 +0900 Subject: [PATCH 23/74] [DQ_SerialManipulatorDH.m] Added minimal changes in the dh2dq method. --- robot_modeling/DQ_SerialManipulatorDH.m | 49 ++++++------------------- 1 file changed, 12 insertions(+), 37 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index d28d4f56..7bf05879 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -77,21 +77,6 @@ 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); @@ -113,28 +98,18 @@ 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 - ]); + % Return the standard dh2dq calculation + d2 = d/2; + a2 = a/2; + h(1) = cosine_of_half_alpha*cosine_of_half_theta; + h(2) = sine_of_half_alpha*cosine_of_half_theta; + h(3) = sine_of_half_alpha*sine_of_half_theta; + h(4) = cosine_of_half_alpha*sine_of_half_theta; + h(5) = -a2*h(2) - d2*h(4); + h(6) = a2*h(1) - d2*h(3); + h(7) = a2*h(4) + d2*h(2); + h(8) = d2*h(1) - a2*h(3); + dq = DQ(h); end function w = get_w(obj,ith) From b85a81ac33dc18ad9f5844bc4f2ead022c618875 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 00:56:12 +0900 Subject: [PATCH 24/74] [DQ_SerialManipulator.m] Removed the convention property and some comments. --- robot_modeling/DQ_SerialManipulator.m | 59 +-------------------------- 1 file changed, 1 insertion(+), 58 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 8509f590..9648bceb 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -51,8 +51,7 @@ % affected methods are: FKM and Jacobian. classdef (Abstract) DQ_SerialManipulator < DQ_Kinematics - properties - convention; + properties effector; % Properties for the plot function @@ -118,12 +117,6 @@ % Define a unique robot name obj.name = sprintf('%f',rand(1)); - - if nargin==1 - obj.convention='standard'; - else - obj.convention=convention; - end %For visualisation obj.lineopt = {'Color', 'black', 'Linewidth', 2}; @@ -161,56 +154,6 @@ function set_effector(obj,effector) x = obj.reference_frame*obj.raw_fkm(q)*obj.effector; end end - -% This method is not required in the abstrac class. -% -% function dq = dh2dq(obj,theta,i) -% % For a given link's DH parameters, calculate the correspondent dual -% % quaternion -% % Usage: dq = dh2dq(theta,i), where -% % theta: joint angle -% % i: link number -% -% if nargin ~= 3 -% error('Wrong number of arguments. The parameters are theta and the correspondent link') -% end -% d = obj.d(i); -% a = obj.a(i); -% alpha = obj.alpha(i); -% -% if strcmp(obj.convention,'standard') -% h(1) = cos((theta+obj.theta(i))/2)*cos(alpha/2); -% h(2) = cos((theta+obj.theta(i))/2)*sin(alpha/2); -% h(3) = sin((theta+obj.theta(i))/2)*sin(alpha/2); -% h(4) = sin((theta+obj.theta(i))/2)*cos(alpha/2); -% d2 = d/2; -% a2 = a/2; -% -% -% h(5) = -d2*h(4)-a2*h(2); -% h(6) = -d2*h(3)+a2*h(1); -% h(7) = d2*h(2)+a2*h(4); -% h(8) = d2*h(1)-a2*h(3); -% else -% h1 = cos((theta+obj.theta(i))/2)*cos(alpha/2); -% h2 = cos((theta+obj.theta(i))/2)*sin(alpha/2); -% h3 = sin((theta+obj.theta(i))/2)*sin(alpha/2); -% h4 = sin((theta+obj.theta(i))/2)*cos(alpha/2); -% h(1) = h1; -% h(2) = h2; -% h(3) = -h3; -% h(4) = h4; -% d2 = d/2; -% a2 = a/2; -% -% h(5) = -d2*h4-a2*h2; -% h(6) = -d2*h3+a2*h1; -% h(7) = -(d2*h2+a2*h4); -% h(8) = d2*h1-a2*h3; -% end -% -% dq = DQ(h); -% end function p = get_z(~,h) p(1) = 0; From c28c98ba8e66f5aaaca2b1c3ea6c063339b3ecae Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 01:06:49 +0900 Subject: [PATCH 25/74] [DQ_SerialManipulator.m] Updated the year of the copyright --- robot_modeling/DQ_SerialManipulator.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 9648bceb..67855089 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -24,7 +24,7 @@ % set_effector - Set an arbitrary end-effector rigid transformation with respect to the last frame in the kinematic chain. % See also DQ_Kinematics. -% (C) Copyright 2011-2020 DQ Robotics Developers +% (C) Copyright 2011-2022 DQ Robotics Developers % % This file is part of DQ Robotics. % From 31bd43477a7eb686cb1fb989598518d97c057527 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 01:07:16 +0900 Subject: [PATCH 26/74] [DQ_SerialManipulatorDH.m] Updated the copyright. --- 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 7bf05879..741c8b73 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -29,7 +29,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 DQ Robotics Developers +% (C) Copyright 2020-2022 DQ Robotics Developers % % This file is part of DQ Robotics. % From 0aea5a4eed6a95f9387a173e2a491a9874d87ff1 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 12:47:05 +0900 Subject: [PATCH 27/74] [DQ_SerialManipulatorMDH] Removed the class. I will add it in a future PR. --- robot_modeling/DQ_SerialManipulatorMDH.m | 311 ----------------------- 1 file changed, 311 deletions(-) delete mode 100644 robot_modeling/DQ_SerialManipulatorMDH.m diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m deleted file mode 100644 index ebcfdc02..00000000 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ /dev/null @@ -1,311 +0,0 @@ -% Concrete class that extends the DQ_SerialManipulator using the Modified -% Denavit-Hartenberg parameters (MDH) -% -% Usage: robot = DQ_SerialManipulatorMDH(A) -% - 'A' is a 5 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.REVOLUTE -% or DQ_SerialManipulatorDH.PRISMATIC -% - The only accepted convention in this subclass is the 'standard' DH -% convention. -% -% If the joint is of type REVOLUTE, 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_dh_parameters_theta - Returns the vector containing the theta parameters of the MDH table. -% get_dh_parameters_d - Returns the vector containing the d parameters of the MDH table. -% get_dh_parameters_a - Returns the vector containing the a parameters of the MDH table. -% get_dh_parameters_alpha - Returns the vector containing the alpha parameters of the MDH table. -% get_joint_types - Returns the joint type, which can be either REVOLUTE or PRISMATIC. -% 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. - - -% (C) Copyright 2022 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: -% Bruno Vihena Adorno - bruno.adorno@manchester.ac.uk -% Murilo M. Marinho - murilo@nml.t.u-tokyo.ac.jp -% Juan Jose Quiroz Omana - juanjqo@g.ecc.u-tokyo.ac.jp - -classdef DQ_SerialManipulatorMDH < DQ_SerialManipulator - - - properties (Access = protected) - mdh_matrix_; - - end - - properties (Constant) - % Joints that can be actuated - % Revolute joint - JOINT_ROTATIONAL = 1; - % Prismatic joint - JOINT_PRISMATIC = 2; - end - - methods (Access = protected) - function w = get_w(obj,ith) - % This method returns the term 'w' related with the time derivative of - % the unit dual quaternion pose using the Modified DH convention. - % See. eq (2.32) of 'Two-arm Manipulation: From Manipulators to Enhanced - % Human-Robot Collaboration' by Bruno Adorno. - % Usage: w = get_w(ith), where - % ith: link number - joint_type = obj.mdh_matrix_(5,ith); - alpha = obj.mdh_matrix_(4,ith); - a = obj.mdh_matrix_(3,ith); - - if joint_type == obj.JOINT_ROTATIONAL - w = -DQ.j*sin(alpha)+ DQ.k*cos(alpha)... - -DQ.E*a*(DQ.j*cos(alpha) + DQ.k*sin(alpha)); - else - w = DQ.E*(cos(alpha)*DQ.k - sin(alpha)*DQ.j); - end - end - - function dq = mdh2dq(obj,q,ith) - % For a given link's Extended MDH 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 - - - half_theta = obj.mdh_matrix_(1,ith)/2.0; - d = obj.mdh_matrix_(2,ith); - a = obj.mdh_matrix_(3,ith); - half_alpha = obj.mdh_matrix_(4,ith)/2.0; - joint_type = obj.mdh_matrix_(5,ith); - - % Add the effect of the joint value - if joint_type == obj.JOINT_ROTATIONAL - % If joint is revolute - 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); - - d2 = d/2; - a2 = a/2; - h(1) = cosine_of_half_alpha*cosine_of_half_theta; - h(2) = sine_of_half_alpha*cosine_of_half_theta; - h(3) = -sine_of_half_alpha*sine_of_half_theta; - h(4) = cosine_of_half_alpha*sine_of_half_theta; - h(5) = -a2*h(2) - d2*h(4); - h(6) = a2*h(1) - d2*-h(3); - h(7) = -a2*h(4) - d2*h(2); - h(8) = d2*h(1) - a2*-h(3); - dq = DQ(h); - end - end - - methods - function obj = DQ_SerialManipulatorMDH(A) - % These are initialized in the constructor of - % DQ_SerialManipulator - str = ['DQ_SerialManipulatorMDH(A), where ' ... - 'A = [theta1 ... thetan; ' ... - ' d1 ... dn; ' ... - ' a1 ... an; ' ... - ' alpha1 ... alphan; ' ... - ' type1 ... typen]']; - if nargin == 0 - error(['Input: matrix whose columns contain the MDH parameters' ... - ' and type of joints. Example: ' str]) - end - - if(size(A,1) ~= 5) - error(['Input: Invalid DH matrix. It should have 5 rows.' ... - ' Example: ' str]) - end - - obj = obj@DQ_SerialManipulator(size(A,2)); - obj.mdh_matrix_ = A; - - end - -% Methods that will be added in future versions but with different -% names and/or signatures. - -% function th = get_dh_parameters_theta(obj) -% %GET_DH_PARAMETERS_THETA() Returns the vector containing the theta parameters of the MDH table. -% th = obj.mdh_matrix_(1,:); -% end -% -% function ds = get_dh_parameters_d(obj) -% % GET_DH_PARAMETERS_D() Returns the vector containing the d parameters of the MDH table. -% ds = obj.mdh_matrix_(2,:); -% end -% -% function as = get_dh_parameters_a(obj) -% % GET_DH_PARAMETERS_A() Returns the vector containing the a parameters of the MDH table. -% as = obj.mdh_matrix_(3,:); -% end -% -% function alphas = get_dh_parameters_alpha(obj) -% % GET_DH_PARAMETERS_ALPHA() Returns the vector containing the alpha parameters of the MDH table. -% alphas = obj.mdh_matrix_(4,:); -% end -% -% function types = get_joint_types(obj) -% % GET_JOINT_TYPES() Returns the joint type, which can be either REVOLUTE or PRISMATIC. -% types = obj.mdh_matrix_(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. - - %obj.check_q_vec(q); - - if nargin == 3 - n = to_ith_link; - else - n = obj.n_links; - end - - %obj.check_to_ith_link(n); - x = DQ(1); - - for i=1:n - x = x*mdh2dq(obj,q(i),i); - 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 - % obj.check_q_vec(q); - - if nargin == 3 - n = to_ith_link; - else - n = obj.n_links; - end - - %obj.check_to_ith_link(n); - x_effector = obj.raw_fkm(q,n); - - x = DQ(1); - J = zeros(8,n); - for i = 1:n - w = obj.get_w(i); - z = 0.5*Ad(x,w); - x = x*obj.mdh2dq(q(i),i); - j = z * x_effector; - J(:,i) = vec8(j); - end - - end - - % TODO: - % This method is not defined in the DQ_Kinematics superclass - % we need to fix it in the future. - function J_dot = raw_pose_jacobian_derivative(obj,q,q_dot, to_ith_link) - % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian - % time derivative. - % - % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first - % to_ith_link columns of the Jacobian time derivative. - % This function does not take into account any base or - % end-effector displacements. - %obj.check_q_vec(q); - %obj.check_q_vec(q_dot); - - if nargin == 4 - n = to_ith_link; - x_effector = obj.raw_fkm(q,to_ith_link); - J = obj.raw_pose_jacobian(q,to_ith_link); - vec_x_effector_dot = J*q_dot(1:to_ith_link); - else - n = obj.n_links; - %obj.check_to_ith_link(n); - x_effector = obj.raw_fkm(q); - J = obj.raw_pose_jacobian(q); - vec_x_effector_dot = J*q_dot; - end - - - x = DQ(1); - J_dot = zeros(8,n); - - for i = 0:n-1 - % Use the standard DH convention - w = obj.get_w(i+1); - z = 0.5*x*w*conj(x); - - % When i = 0 and length(theta) = 1, theta(1,i) returns - % a 1 x 0 vector, differently from the expected - % behavior, which is to return a 0 x 1 matrix. - % Therefore, we have to deal with the case i = 0 - % explictly. - if i ~= 0 - vec_zdot = 0.5*(haminus8(w*x') + ... - hamiplus8(x*w)*DQ.C8) * ... - obj.raw_pose_jacobian(q,i)*q_dot(1:i); - else - vec_zdot = zeros(8,1); - end - J_dot(:,i+1) = haminus8(x_effector)*vec_zdot +... - hamiplus8(z)*vec_x_effector_dot; - x = x*obj.mdh2dq(q(i+1),i+1); - end - end - - end -end \ No newline at end of file From d3fdb388090d35b2a8c51f0c5d7c846ab25c6020 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 14:32:34 +0900 Subject: [PATCH 28/74] [DQ_SerialManipulator.m] Updated the documentation. --- robot_modeling/DQ_SerialManipulator.m | 25 +++++++++++-------------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 67855089..398b8ccb 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -1,17 +1,8 @@ -% Concrete class that defines serial manipulators. +% Abstract class that defines serial manipulators. % -% Usage: robot = DQ_SerialManipulator(A,convention) -% - '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] -% - 'convention' is the convention used for the D-H parameters. Accepted -% values are 'standard' and 'modified' -% -% The first row of 'A' contains the joint offsets. More specifically, -% theta_i is the offset for the i-th joint. +% Usage: robot = DQ_SerialManipulator(dim_configuration_space) +% - 'dim_configuration_space' is the dimension of the +% configuration space. % % DQ_SerialManipulator Methods (Concrete): % get_dim_configuration_space - Return the dimension of the configuration space. @@ -19,9 +10,15 @@ % 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. +% set_effector - Set an arbitrary end-effector rigid transformation with respect to the last frame in the kinematic chain. +% +% +% DQ_SerialManipulator Methods (Abstract): % 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. +% raw_pose_jacobian_derivative - Compute the pose Jacobian derivative without taking into account base's and end-effector's rigid transformations. +% +% % See also DQ_Kinematics. % (C) Copyright 2011-2022 DQ Robotics Developers From 5348e17a255626eb391806b24196cb6ae8bbba4b Mon Sep 17 00:00:00 2001 From: Bruno Vilhena Adorno Date: Sun, 4 Dec 2022 12:05:24 +0000 Subject: [PATCH 29/74] Update CONTRIBUTING.md - Fixed grammar problems across the file. - Added more information about longer commits. --- CONTRIBUTING.md | 46 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 35 insertions(+), 11 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index cc5c4888..7e072cd7 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,33 +1,35 @@ -# Welcome contributors to the DQ Robotics! +# Welcome, contributors to DQ Robotics! -We are happy about your interest in our project. Thank you for your time. DQ Robotics is a standalone open-source library and your contributions are always welcome! +We are happy about your interest in our project! DQ Robotics is a standalone, **fully moderated** open-source library, and contributions are always welcome! -This is a set of guidelines for contribuiting to [DQ Robotics](https://dqrobotics.github.io/). +If your proposed modifications change current functionality, you **must** first discuss them with the developers team and ensure they are approved. Otherwise, it will be rejected without review. + +This is a set of guidelines for contributing to [DQ Robotics](https://dqrobotics.github.io/). # Workflow -- Fork the [master branch of the dqrobotics/matlab](https://github.com/dqrobotics/matlab). -- Propose your modifications and open a draft pull request. Keep in mind the following recomentations: +- Fork the [master branch of dqrobotics/matlab](https://github.com/dqrobotics/matlab). +- Propose your modifications and open a draft pull request. Keep in mind the following **requirements**: - Propose individual changes (several changes of the same type are allowed on the same pull request). - Do not unnecessarily change any internal implementation that is working correctly without prior approval. - Include a clear and concise rationale behind each pull request. -- Your modifications will be tested automatically by Github actions. Specifically, Github actions runs the tests of [matlab-tests](https://github.com/dqrobotics/matlab-tests), which executes all the examples of [matlab-examples](https://github.com/dqrobotics/matlab-examples). +- Your modifications will be tested automatically through Github actions. Github actions run the tests of [matlab-tests](https://github.com/dqrobotics/matlab-tests), which execute all the examples of [matlab-examples](https://github.com/dqrobotics/matlab-examples). - Once your draft pull request passes all the tests, you can switch the status to pull request. (More details [here](https://github.blog/2019-02-14-introducing-draft-pull-requests/)). ![Untitled Diagram](https://user-images.githubusercontent.com/23158313/150255548-b4475747-444e-4530-9b20-def1655bb4f9.png) ## Case 1 (Common cases) -- If your modifications pass all tests, a designated member of our team will review all the changes and they will accept them after all necessary adjustments if any. +- If your modifications pass all tests, a designated member of our team will review all the changes and they will accept them after all necessary adjustments, if any. ## Case 2 (Very rare cases) -- In some cases, your modifications would fail some tests because of incompatibility with the current version of [matlab-tests](https://github.com/dqrobotics/matlab-tests) and/or [matlab-examples](https://github.com/dqrobotics/matlab-examples). In thoses cases, you would propose changes in [matlab-tests](https://github.com/dqrobotics/matlab-tests) and [matlab-examples](https://github.com/dqrobotics/matlab-examples) to make them compatible with your new version of the dqrobotics/matlab. +- In some cases, your modifications would fail some tests because of incompatibility with the current version of [matlab-tests](https://github.com/dqrobotics/matlab-tests) and/or [matlab-examples](https://github.com/dqrobotics/matlab-examples). In thoses cases, you must propose changes in [matlab-tests](https://github.com/dqrobotics/matlab-tests) and [matlab-examples](https://github.com/dqrobotics/matlab-examples) to make them compatible with your new version of the dqrobotics/matlab. - A designated member of our team will review all the changes proposed in both [matlab-tests](https://github.com/dqrobotics/matlab-tests) and [matlab-examples](https://github.com/dqrobotics/matlab-examples). They will accept the modifications in the master branch after all necessary adjustments. At this point, it is expected that your pull request passes all the tests in the master branch but fails in the release branch. ![master_and_release](https://user-images.githubusercontent.com/23158313/150379489-cabc85bb-dbe4-41be-a405-7b254a36092a.png) -- Then, they will test your dqrobotics/matlab pull request making all necessary adjustments until your pull request of dqrobotics/matlab passes all the tests. After that, they will review deeply your modifications. +- After your dqrobotics/matlab pull request is adjusted accordingly and passes all the tests, your proposed modifications will be scrutinized to ensure they follow the coding style and development philosophy, are technically correct, and add value to the current implementation. - Finally, your modifications will be accepted in the master branch. # Example @@ -49,18 +51,40 @@ Type in your terminal: ## Make your modifications Now, you can modify the code with your contributions. -(In this specific example, as shown in the GIF, I modified the CONTRIBUTING.md file) +(In this specific example, as shown in the animated figure below, the CONTRIBUTING.md file is modified.) ![modifications](https://user-images.githubusercontent.com/23158313/149604028-915d9325-e52a-4378-ba58-17b7fe1a7a81.gif) ## Add, commit and push your changes -Please indicate in your commit's message the file that was modified using brackets. For instance, if you modified the class DQ_Serialmanipulator, then you would do the following: +Please indicate in your commit message, using brackets, the modified file. For instance, if you modified the class `DQ_SerialManipulator`, then you would do the following: - `git commit -m "[DQ_SerialManipulator] your_message_explaining_the modification."` ![add_commit_push](https://user-images.githubusercontent.com/23158313/149603960-d69a8202-a3b1-4af5-a2d8-e1197cc26a81.gif) +However, if your explanation is longer or affects more than one file, you must write a more meaninful commit using a text editor, and hence **would not** use the `-m` option. + +For example, this is how a longer commit would be written: +``` +1: This commit solves issue X affecting `DQ_CLASS_A` and all its subclasses. +2: +3: [DQ_CLASS_A] Deleted variable X and included method Y to ensure correctness of model Z. +4: More specifically, Z was returning the transformation from frame A to frame B, instead +5: of frame B to frame A, as described in Eq. (X) of paper [full reference here]. This has +6: been fixed. +7: +8: [DQ_CLASS_A_SUBCLASS_C] Changed method Z to comply with the modification made in +9: DQ_CLASS_A + +``` + + + + + + + ## Open a draft pull request (More details [here](https://github.blog/2019-02-14-introducing-draft-pull-requests/)) Now, your draft pull request will be tested by Github actions automatically. From be5d9755fcdfea3511b1efbb0f2da89e06ee3ccc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 5 Dec 2022 10:01:33 +0900 Subject: [PATCH 30/74] [CONTRIBUTING.md] Fixed typos pointed out by Bruno in the contributing diagram --- CONTRIBUTING.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 7e072cd7..1a866122 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -16,7 +16,8 @@ This is a set of guidelines for contributing to [DQ Robotics](https://dqrobotics - Your modifications will be tested automatically through Github actions. Github actions run the tests of [matlab-tests](https://github.com/dqrobotics/matlab-tests), which execute all the examples of [matlab-examples](https://github.com/dqrobotics/matlab-examples). - Once your draft pull request passes all the tests, you can switch the status to pull request. (More details [here](https://github.blog/2019-02-14-introducing-draft-pull-requests/)). -![Untitled Diagram](https://user-images.githubusercontent.com/23158313/150255548-b4475747-444e-4530-9b20-def1655bb4f9.png) +drawing + ## Case 1 (Common cases) From a9372a3b231b7cab18652b0e0edb1e43977399c1 Mon Sep 17 00:00:00 2001 From: Juancho Date: Mon, 5 Dec 2022 10:11:52 +0900 Subject: [PATCH 31/74] [DQ_Kinematics.m] Updated the copyright and the Bruno's email. --- robot_modeling/DQ_Kinematics.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot_modeling/DQ_Kinematics.m b/robot_modeling/DQ_Kinematics.m index f774233e..82b04250 100644 --- a/robot_modeling/DQ_Kinematics.m +++ b/robot_modeling/DQ_Kinematics.m @@ -36,7 +36,7 @@ % translation_jacobian - Compute the translation Jacobian. % See also DQ_SerialManipulator, DQ_MobileBase, DQ_CooperativeDualTaskSpace. -% (C) Copyright 2011-2019 DQ Robotics Developers +% (C) Copyright 2011-2022 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -56,7 +56,7 @@ % DQ Robotics website: dqrobotics.github.io % % Contributors to this file: -% Bruno Vihena Adorno - adorno@ufmg.br +% Bruno Vihena Adorno - adorno@ieee.org classdef DQ_Kinematics < handle % DQ_Kinematics inherits the HANDLE superclass to avoid unnecessary copies From 7ceb2e5664587705ad8f297de2a3c4858c04cc4b Mon Sep 17 00:00:00 2001 From: Juancho Date: Mon, 5 Dec 2022 10:15:47 +0900 Subject: [PATCH 32/74] [DQ_SerialManipulator.m] Removed TODO comments and updated the Bruno's email. --- robot_modeling/DQ_SerialManipulator.m | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 398b8ccb..3e1b4eab 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -41,11 +41,7 @@ % DQ Robotics website: dqrobotics.github.io % % Contributors to this file: -% Bruno Vihena Adorno - adorno@ufmg.br - -% TODO: Remove the virtual joints. Instead of helping, they cause a lot of -% confusion, specially among those trying to learn the library. The -% affected methods are: FKM and Jacobian. +% Bruno Vihena Adorno - adorno@ieee.org classdef (Abstract) DQ_SerialManipulator < DQ_Kinematics properties From 2295c75882f430dd747c6f6b91156ae08cfc1dd5 Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 13:39:14 +0900 Subject: [PATCH 33/74] [DQ_SerialManipulator.m] Fixed comment according to Bruno's suggestion --- robot_modeling/DQ_SerialManipulator.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 3e1b4eab..58c6ac46 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -193,7 +193,7 @@ function set_effector(obj,effector) if nargin == 4 && ith < obj.n_links % If the Jacobian is not related to the mapping between the % end-effector velocities and the joint velocities, it takes - % into account only the base displacement + % into account only the constant base displacement J_dot = hamiplus8(obj.reference_frame)*obj.raw_pose_jacobian_derivative(... q, q_dot, ith); else From f3ddafb3996d59ef296dd1dfc4e743788c453c57 Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 13:42:20 +0900 Subject: [PATCH 34/74] [DQ_SerialManipulator.m] Fixed comment according to Bruno's suggestion. --- robot_modeling/DQ_SerialManipulator.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 58c6ac46..09f9b014 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -642,7 +642,7 @@ function update_robot(robot, q) % and link length, we add them. % TODO - % This part is strange because it assumes we are using the + % This part of the code assumes we are using the % DH parametrization. We need to fix it in future versions. reach = reach + abs(robot.a(i)) + abs(robot.d(i)); end From 1bcff4a4b99b737a1fa9c12470d413b2c26e2184 Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 14:05:49 +0900 Subject: [PATCH 35/74] [DQ_SerialManipulatorDH.m] Fixed error message according to Bruno's suggestion. --- 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 741c8b73..973f58b2 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -155,7 +155,7 @@ end if(size(A,1) ~= 5) - error('Input: Invalid DH matrix. It should have 5 rows.') + error('Input: Invalid DH matrix. It must have 5 rows.') end obj = obj@DQ_SerialManipulator(size(A,2)); From 91c5b5504275d6e9ff364e7326943a57e7e901fa Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 15:51:40 +0900 Subject: [PATCH 36/74] [DQ_SerialManipulatorDH.m] Added modifications according to Bruno's suggestion --- robot_modeling/DQ_SerialManipulatorDH.m | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 973f58b2..1b3679ad 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -67,11 +67,17 @@ methods (Access = protected) function dq = dh2dq(obj,q,ith) - % For a given link's Extended DH parameters, calculate the correspondent dual + % For a given link's DH parameters, calculate the corresponding dual % quaternion + % % Usage: dq = dh2dq(q,ith), where % q: joint value % ith: link number + % + % Eq. (2.34) of Adorno, B. V. (2011). Two-arm Manipulation: From Manipulators + % to Enhanced Human-Robot Collaboration [Contribution à la manipulation à deux bras : + % des manipulateurs à la collaboration homme-robot]. + % https://tel.archives-ouvertes.fr/tel-00641678/ if nargin ~= 3 error('Wrong number of arguments. The parameters are joint value and the correspondent link') From 8b3a71a6ea812ae81a5e32dacee6b05388edf5f8 Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 16:27:41 +0900 Subject: [PATCH 37/74] [DQ_SerialManipulator.m] Added the dh2dq as an abstract method. --- robot_modeling/DQ_SerialManipulator.m | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 09f9b014..70a97e7a 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -96,7 +96,22 @@ % Jacobian function. pose = raw_fkm(obj,q, to_ith_link); end - + + methods (Abstract, Access = protected) + % DQ2DQ(q, ith) calculates the corresponding dual quaternion for + % a given link's DH parameters + % + % Usage: dq = dh2dq(q,ith), where + % q: joint value + % ith: link number + % + % Eq. (2.34) of Adorno, B. V. (2011). Two-arm Manipulation: From Manipulators + % to Enhanced Human-Robot Collaboration [Contribution à la manipulation à deux bras : + % des manipulateurs à la collaboration homme-robot]. + % https://tel.archives-ouvertes.fr/tel-00641678/ + dq = dh2dq(obj,q,ith); + end + methods function obj = DQ_SerialManipulator(dim_configuration_space) if nargin == 0 From c70d7c88e3f49364547f24b09e051b101eee2ede Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 16:30:22 +0900 Subject: [PATCH 38/74] [DQ_SerialManipulatorDH.m] Updated the documentation of dh2dq --- 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 1b3679ad..bcaaf294 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -67,8 +67,8 @@ methods (Access = protected) function dq = dh2dq(obj,q,ith) - % For a given link's DH parameters, calculate the corresponding dual - % quaternion + % DQ2DQ(q, ith) calculates the corresponding dual quaternion for + % a given link's DH parameters % % Usage: dq = dh2dq(q,ith), where % q: joint value From 7784648e5822669c6d6a1e8103c01de4af498cf7 Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 18:06:07 +0900 Subject: [PATCH 39/74] [JointType.m] Created a enumeration class containing type of joints. --- robot_modeling/JointType.m | 42 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 robot_modeling/JointType.m diff --git a/robot_modeling/JointType.m b/robot_modeling/JointType.m new file mode 100644 index 00000000..d9cba4f6 --- /dev/null +++ b/robot_modeling/JointType.m @@ -0,0 +1,42 @@ +% (C) Copyright 2022 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: +% +% Implementation of the original code: +% Juan Jose Quiroz Omana - (juanjqo@g.ecc.u-tokyo.ac.jp) +% +classdef JointType < double + + % This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). + % Dynamics of Mobile Manipulators Using Dual Quaternion Algebra + % https://doi.org/10.1115/1.4054320 + enumeration + REVOLUTE (1) + PRISMATIC (2) + SPHERICAL (3) + CYLINDRICAL (4) + PLANAR (5) + SIX_DOF (6) + HELICAL (7) + end +end + + + From 9e3d4ecdab87e117ef652986740c481fd384420e Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 19:13:01 +0900 Subject: [PATCH 40/74] [DQ_JointType] Implemented the Bruno's suggestions. --- robot_modeling/{JointType.m => DQ_JointType.m} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename robot_modeling/{JointType.m => DQ_JointType.m} (97%) diff --git a/robot_modeling/JointType.m b/robot_modeling/DQ_JointType.m similarity index 97% rename from robot_modeling/JointType.m rename to robot_modeling/DQ_JointType.m index d9cba4f6..af8554fc 100644 --- a/robot_modeling/JointType.m +++ b/robot_modeling/DQ_JointType.m @@ -22,7 +22,7 @@ % Implementation of the original code: % Juan Jose Quiroz Omana - (juanjqo@g.ecc.u-tokyo.ac.jp) % -classdef JointType < double +classdef DQ_JointType < uint32 % This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). % Dynamics of Mobile Manipulators Using Dual Quaternion Algebra From 08b4d65553396ac4462b8df61bd1ab9bfb3500a5 Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 19:32:02 +0900 Subject: [PATCH 41/74] [DQ_JointType.m] Updated the contributors section according to Murilo's suggestion --- robot_modeling/DQ_JointType.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot_modeling/DQ_JointType.m b/robot_modeling/DQ_JointType.m index af8554fc..db81f487 100644 --- a/robot_modeling/DQ_JointType.m +++ b/robot_modeling/DQ_JointType.m @@ -19,8 +19,8 @@ % % Contributors to this file: % -% Implementation of the original code: -% Juan Jose Quiroz Omana - (juanjqo@g.ecc.u-tokyo.ac.jp) +% 1. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +% Responsible for the original implementation. % classdef DQ_JointType < uint32 From f56bc550c3c902516971b5062163653c99705141 Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 6 Dec 2022 20:10:40 +0900 Subject: [PATCH 42/74] [DQ_SerialManipulatorDH.m] Updated the header as discussed in #75 --- robot_modeling/DQ_SerialManipulatorDH.m | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index bcaaf294..571cb50d 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -49,7 +49,20 @@ % DQ Robotics website: dqrobotics.github.io % % Contributors to this file: -% Murilo M. Marinho - murilo@nml.t.u-tokyo.ac.jp +% 1. Bruno Vihena Adorno (adorno@ieee.org) +% Responsible for the original implementation in file SerialManipulator.m +% (https://github.com/dqrobotics/matlab/blob/bc7a95f064b15046f43421d418946f60b1b33058/robot_modeling/DQ_SerialManipulator.m). +% +% 2. Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp) +% - Created this file by reorganizing the code in the original file to comply +% with SerialManipulator.m becoming an abstract class, according to the discussion +% at #56 (https://github.com/dqrobotics/matlab/pull/56). +% - Added support for prismatic joints. +% (f5aa70a) https://github.com/dqrobotics/matlab/commit/f5aa70ac6a0a676557543e2bf7c418ab05c47326 +% +% 3. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +% - Added some modifications discussed at #56 (https://github.com/dqrobotics/matlab/pull/75) +% to define DQ_SerialManipulator as an abstract class. classdef DQ_SerialManipulatorDH < DQ_SerialManipulator properties From 3ea97297cdae84742714577abd4f332a3d2401dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 10:24:43 +0900 Subject: [PATCH 43/74] [DQ_SerialManipulator.m] Removed the argument of the constructor, as suggested by Bruno. --- robot_modeling/DQ_SerialManipulator.m | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 70a97e7a..81ad19af 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -113,23 +113,17 @@ end methods - function obj = DQ_SerialManipulator(dim_configuration_space) - if nargin == 0 - error('Input: dimension of the configuration space') - end - + function obj = DQ_SerialManipulator() obj.reference_frame = DQ(1); %Default base's pose obj.base_frame = DQ(1); obj.effector = DQ(1); %Default effector's pose - obj.n_links = dim_configuration_space; - + % Define a unique robot name obj.name = sprintf('%f',rand(1)); %For visualisation obj.lineopt = {'Color', 'black', 'Linewidth', 2}; - obj.plotopt = {}; - + obj.plotopt = {}; end function ret = get_dim_configuration_space(obj) @@ -137,8 +131,7 @@ end function set_effector(obj,effector) - % SET_EFFECTOR(effector) sets the pose of the effector - + % SET_EFFECTOR(effector) sets the pose of the effector obj.effector = DQ(effector); end From 7d80e15dbaa8ff1834a5581ff6bfd2b4034fbb3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 10:26:19 +0900 Subject: [PATCH 44/74] [DQ_SerialManipulatorDH.m] Updated the constructor of the class taking into account the modifications in the superclass. --- robot_modeling/DQ_SerialManipulatorDH.m | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 571cb50d..8ffcd1b9 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -176,8 +176,10 @@ if(size(A,1) ~= 5) error('Input: Invalid DH matrix. It must have 5 rows.') end - - obj = obj@DQ_SerialManipulator(size(A,2)); + + % n_links + % TODO: change n_links to dim_configuration_space + obj.n_links = size(A,2); % Add theta, d, a, alpha and type obj.theta = A(1,:); From 3989e0918f68728ef802d02df8f66b65052f06b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 12:34:39 +0900 Subject: [PATCH 45/74] [DQ_SerialManipulatorDH.m] Added the date of some commits. --- robot_modeling/DQ_SerialManipulatorDH.m | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 8ffcd1b9..a28e3ef7 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -51,14 +51,17 @@ % Contributors to this file: % 1. Bruno Vihena Adorno (adorno@ieee.org) % Responsible for the original implementation in file SerialManipulator.m -% (https://github.com/dqrobotics/matlab/blob/bc7a95f064b15046f43421d418946f60b1b33058/robot_modeling/DQ_SerialManipulator.m). +% [bvadorno committed on Apr 10, 2019] (bc7a95f) +% (https://github.com/dqrobotics/matlab/blob/bc7a95f064b15046f43421d418946f60b1b33058/robot_modeling/DQ_SerialManipulator.m). % % 2. Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp) % - Created this file by reorganizing the code in the original file to comply % with SerialManipulator.m becoming an abstract class, according to the discussion % at #56 (https://github.com/dqrobotics/matlab/pull/56). +% % - Added support for prismatic joints. -% (f5aa70a) https://github.com/dqrobotics/matlab/commit/f5aa70ac6a0a676557543e2bf7c418ab05c47326 +% [mmmarinho committed on Apr 28, 2020] (f5aa70a) +% https://github.com/dqrobotics/matlab/commit/f5aa70ac6a0a676557543e2bf7c418ab05c47326 % % 3. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) % - Added some modifications discussed at #56 (https://github.com/dqrobotics/matlab/pull/75) @@ -176,7 +179,7 @@ if(size(A,1) ~= 5) error('Input: Invalid DH matrix. It must have 5 rows.') end - + % n_links % TODO: change n_links to dim_configuration_space obj.n_links = size(A,2); From ce7f3af98b51c3a427b76e03271258e6dfdb8b84 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 14:17:03 +0900 Subject: [PATCH 46/74] [DQ_SerialManipulatorDH.m] Modified the class to use DQ_JointType --- robot_modeling/DQ_SerialManipulatorDH.m | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index a28e3ef7..38108105 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -9,13 +9,13 @@ % a1 ... an; % alpha1 ... alphan; % type1 ... typen] -% where type is the actuation type, either DQ_SerialManipulatorDH.JOINT_ROTATIONAL -% or DQ_SerialManipulatorDH.JOINT_PRISMATIC +% where type is the actuation type, either DQ_JointType.REVOLUTE +% or DQ_JointType.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 +% If the joint is of type REVOLUTE, then the first row of A will +% have the joint offsets. If the joint is of type PRISMATIC, then the % second row of A will have the joints offsets. % % DQ_SerialManipulatorDH Methods (Concrete): @@ -76,9 +76,9 @@ properties (Constant) % Joints that can be actuated % Rotational joint - JOINT_ROTATIONAL = 1; + JOINT_ROTATIONAL = 1; % Deprecated % Prismatic joint - JOINT_PRISMATIC = 2; + JOINT_PRISMATIC = 2; % Deprecated end methods (Access = protected) @@ -104,10 +104,9 @@ 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 + if obj.type(ith) == DQ_JointType.REVOLUTE + % If joint is revolute half_theta = half_theta + (q/2.0); else % If joint is prismatic @@ -141,7 +140,7 @@ % Human-Robot Collaboration' by Bruno Adorno. % Usage: w = get_w(ith), where % ith: link number - if obj.type(ith) == obj.JOINT_ROTATIONAL + if obj.type(ith) == DQ_JointType.REVOLUTE w = DQ.k; else % see Table 1 of "Dynamics of Mobile Manipulators using Dual Quaternion Algebra." From c56b6a84429f6d20ddbc7ec27f125771e53632dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 14:50:06 +0900 Subject: [PATCH 47/74] [ComauSmartSixRobot.m] Updated the class to use DQ_JointType --- robots/ComauSmartSixRobot.m | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/robots/ComauSmartSixRobot.m b/robots/ComauSmartSixRobot.m index 39820bcc..4cb9aaff 100644 --- a/robots/ComauSmartSixRobot.m +++ b/robots/ComauSmartSixRobot.m @@ -32,13 +32,12 @@ comau_DH_d = [-0.45, 0, 0, -0.64707, 0, -0.095]; comau_DH_a = [0, 0.150, 0.590, 0.13, 0, 0]; comau_DH_alpha = [pi, pi/2, pi, -pi/2, -pi/2, pi/2]; - comau_DH_type = [1,1,1,1,1,1]; - + comau_DH_type = double(repmat(DQ_JointType.REVOLUTE,1,6)); comau_DH_matrix = [comau_DH_theta; - comau_DH_d; - comau_DH_a; - comau_DH_alpha - comau_DH_type]; + comau_DH_d; + comau_DH_a; + comau_DH_alpha + comau_DH_type]; comau = DQ_SerialManipulatorDH(comau_DH_matrix); From bc1e1d5cb70e9f5e61a9e32586d54b584ed34640 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 14:54:18 +0900 Subject: [PATCH 48/74] [ComauSmartSixRobot.m] Updated the website, Bruno's email and the copyright. --- robots/ComauSmartSixRobot.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robots/ComauSmartSixRobot.m b/robots/ComauSmartSixRobot.m index 4cb9aaff..b8232f67 100644 --- a/robots/ComauSmartSixRobot.m +++ b/robots/ComauSmartSixRobot.m @@ -1,7 +1,7 @@ % comau = DQ_COMAU returns a DQ_kinematics object using the modified % Denavit-Hartenberg parameters of the COMAU SmartSiX robot -% (C) Copyright 2015 DQ Robotics Developers +% (C) Copyright 2015-2022 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -18,10 +18,10 @@ % You should have received a copy of the GNU Lesser General Public License % along with DQ Robotics. If not, see . % -% DQ Robotics website: dqrobotics.sourceforge.net +% DQ Robotics website: dqrobotics.github.io % % Contributors to this file: -% Bruno Vihena Adorno - adorno@ufmg.br +% Bruno Vihena Adorno - adorno@ieee.org classdef ComauSmartSixRobot methods (Static) From 4d1e47f3127e9f5f9d34feac6323f4b1358235a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 14:55:54 +0900 Subject: [PATCH 49/74] [ComauSmartSixRobot.m] Fixed the robot definition using the MDH class for serial manipulators. --- robots/ComauSmartSixRobot.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robots/ComauSmartSixRobot.m b/robots/ComauSmartSixRobot.m index b8232f67..56cd302b 100644 --- a/robots/ComauSmartSixRobot.m +++ b/robots/ComauSmartSixRobot.m @@ -39,7 +39,7 @@ comau_DH_alpha comau_DH_type]; - comau = DQ_SerialManipulatorDH(comau_DH_matrix); + comau = DQ_SerialManipulatorMDH(comau_DH_matrix); % There is a final transformation for the end-effector given by % a rotation of pi around the local x-axis followed by a From 1996fa63a4f25fb22f2f9b00e561fa8280e8831e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 15:22:24 +0900 Subject: [PATCH 50/74] [DQ_SerialManiplator.m] Updated the comments of pose_jacobian and pose_jacobian_derivative according to Bruno's suggestion. --- robot_modeling/DQ_SerialManipulator.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 81ad19af..b9c140a8 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -177,7 +177,7 @@ function set_effector(obj,effector) if nargin == 3 && ith < obj.n_links % If the Jacobian is not related to the mapping between the % end-effector velocities and the joint velocities, it takes - % into account only the base displacement + % into account only the constant base displacement J = hamiplus8(obj.reference_frame)*obj.raw_pose_jacobian(... q, ith); else @@ -199,13 +199,13 @@ function set_effector(obj,effector) % end-effector displacements. if nargin == 4 && ith < obj.n_links - % If the Jacobian is not related to the mapping between the + % If the Jacobian derivative is not related to the mapping between the % end-effector velocities and the joint velocities, it takes % into account only the constant base displacement J_dot = hamiplus8(obj.reference_frame)*obj.raw_pose_jacobian_derivative(... q, q_dot, ith); else - % Otherwise, it the Jacobian is related to the + % Otherwise, it the Jacobian derivative is related to the % end-effector velocity, it takes into account both base % and end-effector (constant) displacements. J_dot = hamiplus8(obj.reference_frame)*haminus8(obj.effector)*... From f0d0f0c90eb08f657f9b90485553840069eecaaf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Wed, 7 Dec 2022 15:32:47 +0900 Subject: [PATCH 51/74] [DQ_SerialManipulator.m] Updated the condition of pose_jacobian_derivative. If ith==n_links the effector is not taken into account anymore --- robot_modeling/DQ_SerialManipulator.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index b9c140a8..0f3a47cc 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -198,7 +198,7 @@ function set_effector(obj,effector) % This function does not take into account any base or % end-effector displacements. - if nargin == 4 && ith < obj.n_links + if nargin == 4 % If the Jacobian derivative is not related to the mapping between the % end-effector velocities and the joint velocities, it takes % into account only the constant base displacement From 4ee9ebd127ecd1be7d89414dea8f5924c102fd04 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Thu, 8 Dec 2022 10:58:38 +0900 Subject: [PATCH 52/74] [DQ_SerialManipulator.m] Modified the if condition in pose_jacobian() Now, when ith==n_links the effector is not taken into account. --- robot_modeling/DQ_SerialManipulator.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 0f3a47cc..79601463 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -174,7 +174,7 @@ function set_effector(obj,effector) % both base and end-effector displacements (their default % values are 1). - if nargin == 3 && ith < obj.n_links + if nargin == 3 % If the Jacobian is not related to the mapping between the % end-effector velocities and the joint velocities, it takes % into account only the constant base displacement From 5f6befa4ce60eba5130703a53f29f80924b05694 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Thu, 8 Dec 2022 10:58:38 +0900 Subject: [PATCH 53/74] Revert "[DQ_SerialManipulator.m] Modified the if condition in pose_jacobian()" This reverts commit 4ee9ebd127ecd1be7d89414dea8f5924c102fd04. --- robot_modeling/DQ_SerialManipulator.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 79601463..0f3a47cc 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -174,7 +174,7 @@ function set_effector(obj,effector) % both base and end-effector displacements (their default % values are 1). - if nargin == 3 + if nargin == 3 && ith < obj.n_links % If the Jacobian is not related to the mapping between the % end-effector velocities and the joint velocities, it takes % into account only the constant base displacement From 116bbe5e3ee5f1e46c489eb9f95c05df67df1841 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 11 Dec 2022 16:07:39 +0900 Subject: [PATCH 54/74] [DQ_SerialManipulator.m] Removed the abstract method dq2dh(). --- robot_modeling/DQ_SerialManipulator.m | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 0f3a47cc..e516aae7 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -97,20 +97,6 @@ pose = raw_fkm(obj,q, to_ith_link); end - methods (Abstract, Access = protected) - % DQ2DQ(q, ith) calculates the corresponding dual quaternion for - % a given link's DH parameters - % - % Usage: dq = dh2dq(q,ith), where - % q: joint value - % ith: link number - % - % Eq. (2.34) of Adorno, B. V. (2011). Two-arm Manipulation: From Manipulators - % to Enhanced Human-Robot Collaboration [Contribution à la manipulation à deux bras : - % des manipulateurs à la collaboration homme-robot]. - % https://tel.archives-ouvertes.fr/tel-00641678/ - dq = dh2dq(obj,q,ith); - end methods function obj = DQ_SerialManipulator() From 4c1ed0eafe60220339cfb5fef637907df354280c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Sun, 11 Dec 2022 16:55:08 +0900 Subject: [PATCH 55/74] [CONTRIBUTING.md] Fixed type of font and the color of two words. --- CONTRIBUTING.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 1a866122..b8dfafc0 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -16,8 +16,7 @@ This is a set of guidelines for contributing to [DQ Robotics](https://dqrobotics - Your modifications will be tested automatically through Github actions. Github actions run the tests of [matlab-tests](https://github.com/dqrobotics/matlab-tests), which execute all the examples of [matlab-examples](https://github.com/dqrobotics/matlab-examples). - Once your draft pull request passes all the tests, you can switch the status to pull request. (More details [here](https://github.blog/2019-02-14-introducing-draft-pull-requests/)). -drawing - +drawing ## Case 1 (Common cases) From 91f00ca4896b044b6b9cf99ae7382f056a40aa7a Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 11 Dec 2022 18:45:56 +0900 Subject: [PATCH 56/74] [DQ_SerialManipulator.m] Removed some lines in the header as requested by Bruno --- robot_modeling/DQ_SerialManipulator.m | 3 --- 1 file changed, 3 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index e516aae7..7c233721 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -1,8 +1,5 @@ % Abstract class that defines serial manipulators. % -% Usage: robot = DQ_SerialManipulator(dim_configuration_space) -% - 'dim_configuration_space' is the dimension of the -% configuration space. % % DQ_SerialManipulator Methods (Concrete): % get_dim_configuration_space - Return the dimension of the configuration space. From 66a25b78fa580d0b980608b56a6b12c6e2297907 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 11 Dec 2022 18:51:00 +0900 Subject: [PATCH 57/74] [DQ_SerialManipulator.m] Updated the pose_jacobian_derivative method to match pose_jacobian, as discussed in #75 --- robot_modeling/DQ_SerialManipulator.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 7c233721..07023fdf 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -181,7 +181,7 @@ function set_effector(obj,effector) % This function does not take into account any base or % end-effector displacements. - if nargin == 4 + if nargin == 4 && ith < obj.n_links % If the Jacobian derivative is not related to the mapping between the % end-effector velocities and the joint velocities, it takes % into account only the constant base displacement From 6f15dccc2660a76205f3b15d8e90067a57e1304f Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 11 Dec 2022 19:30:45 +0900 Subject: [PATCH 58/74] [DQ_SerialManipulatorDH.m] Fixed the number of the PR to which I contributed. --- 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 38108105..988a27ce 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -64,7 +64,7 @@ % https://github.com/dqrobotics/matlab/commit/f5aa70ac6a0a676557543e2bf7c418ab05c47326 % % 3. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) -% - Added some modifications discussed at #56 (https://github.com/dqrobotics/matlab/pull/75) +% - Added some modifications discussed at #75 (https://github.com/dqrobotics/matlab/pull/75) % to define DQ_SerialManipulator as an abstract class. classdef DQ_SerialManipulatorDH < DQ_SerialManipulator From e375d0e7927d001c53caf0783bdb6d1aeab047e1 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 11 Dec 2022 20:50:04 +0900 Subject: [PATCH 59/74] [DQ_SerialManipulatorMDH.m] Added the class to model robots using the MDH parameters. --- robot_modeling/DQ_SerialManipulatorMDH.m | 304 +++++++++++++++++++++++ 1 file changed, 304 insertions(+) create mode 100644 robot_modeling/DQ_SerialManipulatorMDH.m diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m new file mode 100644 index 00000000..021b696f --- /dev/null +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -0,0 +1,304 @@ +% Concrete class that extends the DQ_SerialManipulator using the +% modified Denavit-Hartenberg parameters (MDH) +% +% Usage: robot = DQ_SerialManipulatorMDH(A) +% - 'A' is a 5 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_JointType.REVOLUTE +% or DQ_JointType.PRISMATIC +% - The only accepted convention in this subclass is the 'standard' DH +% convention. +% +% If the joint is of type REVOLUTE, then the first row of A will +% have the joint offsets. If the joint is of type PRISMATIC, then the +% second row of A will have the joints offsets. +% +% DQ_SerialManipulatorMDH 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-2022 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: +% 1. Bruno Vihena Adorno (adorno@ieee.org) +% Responsible for the original implementation in file SerialManipulator.m +% [bvadorno committed on Apr 10, 2019] (bc7a95f) +% (https://github.com/dqrobotics/matlab/blob/bc7a95f064b15046f43421d418946f60b1b33058/robot_modeling/DQ_SerialManipulator.m). +% +% 2. Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp) +% - Created the file DQ_SerialManipulatorDH at #56 +% (https://github.com/dqrobotics/matlab/pull/56), +% in which this file is based. +% +% 3. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +% - Created this file. + + + + + +classdef DQ_SerialManipulatorMDH < DQ_SerialManipulator + properties + theta,d,a,alpha; + type + end + + properties (Constant) + % Joints that can be actuated + % Rotational joint + JOINT_ROTATIONAL = 1; % Deprecated + % Prismatic joint + JOINT_PRISMATIC = 2; % Deprecated + end + + methods (Access = protected) + function dq = mdh2dq(obj,q,ith) + % MDQ2DQ(q, ith) calculates the corresponding dual quaternion for + % a given link's modified DH parameters + % + % Usage: dq = mdh2dq(q,ith), where + % q: joint value + % ith: link number + % + % Eq. (2.34) of Adorno, B. V. (2011). Two-arm Manipulation: From Manipulators + % to Enhanced Human-Robot Collaboration [Contribution à la manipulation à deux bras : + % des manipulateurs à la collaboration homme-robot]. + % https://tel.archives-ouvertes.fr/tel-00641678/ + + if nargin ~= 3 + error('Wrong number of arguments. The parameters are joint value and the correspondent link') + end + + % 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) == DQ_JointType.REVOLUTE + % If joint is revolute + 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); + + d2 = d/2; + a2 = a/2; + h(1) = cosine_of_half_alpha*cosine_of_half_theta; + h(2) = sine_of_half_alpha*cosine_of_half_theta; + h(3) = -sine_of_half_alpha*sine_of_half_theta; + h(4) = cosine_of_half_alpha*sine_of_half_theta; + h(5) = -a2*h(2) - d2*h(4); + h(6) = a2*h(1) - d2*-h(3); + h(7) = -a2*h(4) - d2*h(2); + h(8) = d2*h(1) - a2*-h(3); + dq = DQ(h); + end + + function w = get_w(obj,ith) + % This method returns the term 'w' related with the time derivative of + % the unit dual quaternion pose using the Modified DH convention. + % See. eq (2.32) of 'Two-arm Manipulation: From Manipulators to Enhanced + % Human-Robot Collaboration' by Bruno Adorno. + % Usage: w = get_w(ith), where + % ith: link number + + if obj.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 + + methods + function obj = DQ_SerialManipulatorMDH(A) + + str = ['DQ_SerialManipulatorMDH(A), where ' ... + 'A = [theta1 ... thetan; ' ... + ' d1 ... dn; ' ... + ' a1 ... an; ' ... + ' alpha1 ... alphan; ' ... + ' type1 ... typen]']; + + if nargin == 0 + error(['Input: matrix whose columns contain the modified DH parameters' ... + ' and type of joints. Example: ' str]) + end + + if(size(A,1) ~= 5) + error('Input: Invalid DH matrix. It must have 5 rows.') + end + + % n_links + % TODO: change n_links to dim_configuration_space + obj.n_links = size(A,2); + + % Add theta, d, a, alpha and type + obj.theta = A(1,:); + obj.d = A(2,:); + obj.a = A(3,:); + obj.alpha = A(4,:); + 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. + + %obj.check_q_vec(q); + + if nargin == 3 + n = to_ith_link; + else + n = obj.n_links; + end + + %obj.check_to_ith_link(n); + x = DQ(1); + + for i=1:n + x = x*mdh2dq(obj,q(i),i); + 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 + % obj.check_q_vec(q); + + if nargin == 3 + n = to_ith_link; + else + n = obj.n_links; + end + + %obj.check_to_ith_link(n); + x_effector = obj.raw_fkm(q,n); + + x = DQ(1); + J = zeros(8,n); + for i = 1:n + w = obj.get_w(i); + z = 0.5*Ad(x,w); + x = x*obj.mdh2dq(q(i),i); + j = z * x_effector; + J(:,i) = vec8(j); + end + + end + + % TODO: + % This method is not defined in the DQ_Kinematics superclass + % we need to fix it in the future. + function J_dot = raw_pose_jacobian_derivative(obj,q,q_dot, to_ith_link) + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian + % time derivative. + % + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first + % to_ith_link columns of the Jacobian time derivative. + % This function does not take into account any base or + % end-effector displacements. + %obj.check_q_vec(q); + %obj.check_q_vec(q_dot); + + if nargin == 4 + n = to_ith_link; + x_effector = obj.raw_fkm(q,to_ith_link); + J = obj.raw_pose_jacobian(q,to_ith_link); + vec_x_effector_dot = J*q_dot(1:to_ith_link); + else + n = obj.n_links; + %obj.check_to_ith_link(n); + x_effector = obj.raw_fkm(q); + J = obj.raw_pose_jacobian(q); + vec_x_effector_dot = J*q_dot; + end + + + x = DQ(1); + J_dot = zeros(8,n); + + for i = 0:n-1 + % Use the standard DH convention + w = obj.get_w(i+1); + z = 0.5*x*w*conj(x); + + % When i = 0 and length(theta) = 1, theta(1,i) returns + % a 1 x 0 vector, differently from the expected + % behavior, which is to return a 0 x 1 matrix. + % Therefore, we have to deal with the case i = 0 + % explictly. + if i ~= 0 + vec_zdot = 0.5*(haminus8(w*x') + ... + hamiplus8(x*w)*DQ.C8) * ... + obj.raw_pose_jacobian(q,i)*q_dot(1:i); + else + vec_zdot = zeros(8,1); + end + J_dot(:,i+1) = haminus8(x_effector)*vec_zdot +... + hamiplus8(z)*vec_x_effector_dot; + x = x*obj.mdh2dq(q(i+1),i+1); + end + end + + end +end \ No newline at end of file From b8a21fe48d0b0996ede44cb60b58534fa8a66be1 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 11 Dec 2022 20:55:17 +0900 Subject: [PATCH 60/74] [DQ_SerialManipulatorMDH.m] Updated the header. --- robot_modeling/DQ_SerialManipulatorMDH.m | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m index 021b696f..44dc307b 100644 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -11,7 +11,7 @@ % type1 ... typen] % where type is the actuation type, either DQ_JointType.REVOLUTE % or DQ_JointType.PRISMATIC -% - The only accepted convention in this subclass is the 'standard' DH +% - The only accepted convention in this subclass is the 'modified' DH % convention. % % If the joint is of type REVOLUTE, then the first row of A will @@ -60,7 +60,8 @@ % in which this file is based. % % 3. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) -% - Created this file. +% - Created this file. Implemented the case for prismatic joints +% in method get_w(). From 423e95fff8a38a0a0febf5a92a32a2c6c6fff8b6 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 11 Dec 2022 22:59:35 +0900 Subject: [PATCH 61/74] [DQ_SerialManipulatorMDH.m] Updated the header according to Bruno's suggestion. --- robot_modeling/DQ_SerialManipulatorMDH.m | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m index 44dc307b..aa747a67 100644 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -55,9 +55,10 @@ % (https://github.com/dqrobotics/matlab/blob/bc7a95f064b15046f43421d418946f60b1b33058/robot_modeling/DQ_SerialManipulator.m). % % 2. Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp) -% - Created the file DQ_SerialManipulatorDH at #56 +% - Reorganized the code by moving the implementation of SerialManipulator.m +% to the file DQ_SerialManipulatorDH at #56 % (https://github.com/dqrobotics/matlab/pull/56), -% in which this file is based. +% which is the starting point for this file. % % 3. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) % - Created this file. Implemented the case for prismatic joints From 45c574a67647e4bc1e94df19213f836ef856e4db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 10:19:23 +0900 Subject: [PATCH 62/74] [DQ_SerialManipulatorMDH.m] Improved the design of the class as requested by Bruno. --- robot_modeling/DQ_SerialManipulatorMDH.m | 173 +---------------------- 1 file changed, 5 insertions(+), 168 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m index aa747a67..b62cd6af 100644 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -68,26 +68,14 @@ -classdef DQ_SerialManipulatorMDH < DQ_SerialManipulator - properties - theta,d,a,alpha; - type - end - - properties (Constant) - % Joints that can be actuated - % Rotational joint - JOINT_ROTATIONAL = 1; % Deprecated - % Prismatic joint - JOINT_PRISMATIC = 2; % Deprecated - end - +classdef DQ_SerialManipulatorMDH < DQ_SerialManipulatorDH + methods (Access = protected) - function dq = mdh2dq(obj,q,ith) - % MDQ2DQ(q, ith) calculates the corresponding dual quaternion for + function dq = dh2dq(obj,q,ith) + % DQ2DQ(q, ith) calculates the corresponding dual quaternion for % a given link's modified DH parameters % - % Usage: dq = mdh2dq(q,ith), where + % Usage: dq = dh2dq(q,ith), where % q: joint value % ith: link number % @@ -152,155 +140,4 @@ end - methods - function obj = DQ_SerialManipulatorMDH(A) - - str = ['DQ_SerialManipulatorMDH(A), where ' ... - 'A = [theta1 ... thetan; ' ... - ' d1 ... dn; ' ... - ' a1 ... an; ' ... - ' alpha1 ... alphan; ' ... - ' type1 ... typen]']; - - if nargin == 0 - error(['Input: matrix whose columns contain the modified DH parameters' ... - ' and type of joints. Example: ' str]) - end - - if(size(A,1) ~= 5) - error('Input: Invalid DH matrix. It must have 5 rows.') - end - - % n_links - % TODO: change n_links to dim_configuration_space - obj.n_links = size(A,2); - - % Add theta, d, a, alpha and type - obj.theta = A(1,:); - obj.d = A(2,:); - obj.a = A(3,:); - obj.alpha = A(4,:); - 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. - - %obj.check_q_vec(q); - - if nargin == 3 - n = to_ith_link; - else - n = obj.n_links; - end - - %obj.check_to_ith_link(n); - x = DQ(1); - - for i=1:n - x = x*mdh2dq(obj,q(i),i); - 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 - % obj.check_q_vec(q); - - if nargin == 3 - n = to_ith_link; - else - n = obj.n_links; - end - - %obj.check_to_ith_link(n); - x_effector = obj.raw_fkm(q,n); - - x = DQ(1); - J = zeros(8,n); - for i = 1:n - w = obj.get_w(i); - z = 0.5*Ad(x,w); - x = x*obj.mdh2dq(q(i),i); - j = z * x_effector; - J(:,i) = vec8(j); - end - - end - - % TODO: - % This method is not defined in the DQ_Kinematics superclass - % we need to fix it in the future. - function J_dot = raw_pose_jacobian_derivative(obj,q,q_dot, to_ith_link) - % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian - % time derivative. - % - % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first - % to_ith_link columns of the Jacobian time derivative. - % This function does not take into account any base or - % end-effector displacements. - %obj.check_q_vec(q); - %obj.check_q_vec(q_dot); - - if nargin == 4 - n = to_ith_link; - x_effector = obj.raw_fkm(q,to_ith_link); - J = obj.raw_pose_jacobian(q,to_ith_link); - vec_x_effector_dot = J*q_dot(1:to_ith_link); - else - n = obj.n_links; - %obj.check_to_ith_link(n); - x_effector = obj.raw_fkm(q); - J = obj.raw_pose_jacobian(q); - vec_x_effector_dot = J*q_dot; - end - - - x = DQ(1); - J_dot = zeros(8,n); - - for i = 0:n-1 - % Use the standard DH convention - w = obj.get_w(i+1); - z = 0.5*x*w*conj(x); - - % When i = 0 and length(theta) = 1, theta(1,i) returns - % a 1 x 0 vector, differently from the expected - % behavior, which is to return a 0 x 1 matrix. - % Therefore, we have to deal with the case i = 0 - % explictly. - if i ~= 0 - vec_zdot = 0.5*(haminus8(w*x') + ... - hamiplus8(x*w)*DQ.C8) * ... - obj.raw_pose_jacobian(q,i)*q_dot(1:i); - else - vec_zdot = zeros(8,1); - end - J_dot(:,i+1) = haminus8(x_effector)*vec_zdot +... - hamiplus8(z)*vec_x_effector_dot; - x = x*obj.mdh2dq(q(i+1),i+1); - end - end - - end end \ No newline at end of file From 9f78c8cb4cfe2aee3a3d91c2b145e999b4473710 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 18:45:08 +0900 Subject: [PATCH 63/74] [DQ_SerialManipulator.m] Added get_w and get_link2dq as protected abstract methods. --- robot_modeling/DQ_SerialManipulator.m | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 07023fdf..f7dc06a9 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -56,6 +56,29 @@ n_links; end + methods (Abstract, Access = protected) + % GET_LINK2DQ(q, ith) calculates the corresponding dual quaternion for + % a given link's parameters + % + % Usage: dq = get_link2dq(q,ith), where + % q: joint value + % ith: link number + % + % Eq. (2.34) of Adorno, B. V. (2011). Two-arm Manipulation: From Manipulators + % to Enhanced Human-Robot Collaboration [Contribution à la manipulation à deux bras : + % des manipulateurs à la collaboration homme-robot]. + % https://tel.archives-ouvertes.fr/tel-00641678/ + dq = get_link2dq(obj,q,ith); + + % This method returns the term 'w' related with the time derivative of + % the unit dual quaternion pose. + % See. eq (2.32) of 'Two-arm Manipulation: From Manipulators to Enhanced + % Human-Robot Collaboration' by Bruno Adorno. + % Usage: w = get_w(ith), where + % ith: link number + w = get_w(obj,ith) ; + end + methods (Abstract) % RAW_POSE_JACOBIAN(q) returns the Jacobian that satisfies % vec(x_dot) = J * q_dot, where x = fkm(q) and q is the From 7bcbcea655f27ca5ad9312ae8c8e89659b8a30b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 18:50:54 +0900 Subject: [PATCH 64/74] [DQ_SerialManipulator.m] The raw methods are concrete methods instead of abstract ones. --- robot_modeling/DQ_SerialManipulator.m | 151 +++++++++++++++++++------- 1 file changed, 110 insertions(+), 41 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index f7dc06a9..c0712914 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -77,46 +77,7 @@ % Usage: w = get_w(ith), where % ith: link number w = get_w(obj,ith) ; - end - - methods (Abstract) - % 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 - J = raw_pose_jacobian(obj, q,to_ith_link); - - - % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian - % time derivative. - % - % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot, to_ith_link) returns the first - % to_ith_link columns of the Jacobian time derivative. - % This function does not take into account any base or - % end-effector displacements. - J_dot = raw_pose_jacobian_derivative(obj, q, q_dot, 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. - pose = raw_fkm(obj,q, to_ith_link); - end - + end methods function obj = DQ_SerialManipulator() @@ -171,7 +132,115 @@ function set_effector(obj,effector) p(6) = h(2)*h(8)+h(6)*h(4)+h(1)*h(7)+h(5)*h(3); p(7) = h(3)*h(8)+h(7)*h(4)-h(1)*h(6)-h(5)*h(2); p(8) = h(4)*h(8)-h(3)*h(7)-h(2)*h(6)+h(1)*h(5); - end + 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*get_link2dq(obj,q(i),i); + 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.get_link2dq(q(i+1),i+1); + j = z * x_effector; + J(:,i+1) = vec8(j); + end + end + + function J_dot = raw_pose_jacobian_derivative(obj,q,q_dot, to_ith_link) + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian + % time derivative. + % + % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first + % to_ith_link columns of the Jacobian time derivative. + % This function does not take into account any base or + % end-effector displacements. + % obj.check_q_vec(q); + % obj.check_q_vec(q_dot); + + if nargin == 4 + n = to_ith_link; + x_effector = obj.raw_fkm(q,to_ith_link); + J = obj.raw_pose_jacobian(q,to_ith_link); + vec_x_effector_dot = J*q_dot(1:to_ith_link); + else + n = obj.n_links; + % obj.check_to_ith_link(n); + x_effector = obj.raw_fkm(q); + J = obj.raw_pose_jacobian(q); + vec_x_effector_dot = J*q_dot; + end + + x = DQ(1); + J_dot = zeros(8,n); + + for i = 0:n-1 + % Use the standard DH convention + + w = obj.get_w(i+1); %w = DQ.k; + %z = DQ(obj.get_z(x.q)); + z = 0.5*x*w*conj(x); + + % When i = 0 and length(theta) = 1, theta(1,i) returns + % a 1 x 0 vector, differently from the expected + % behavior, which is to return a 0 x 1 matrix. + % Therefore, we have to deal with the case i = 0 + % explictly. + if i ~= 0 + vec_zdot = 0.5*(haminus8(w*x') + ... + hamiplus8(x*w)*DQ.C8) * ... + obj.raw_pose_jacobian(q,i)*q_dot(1:i); + else + vec_zdot = zeros(8,1); + end + J_dot(:,i+1) = haminus8(x_effector)*vec_zdot +... + hamiplus8(z)*vec_x_effector_dot; + x = x*obj.get_link2dq(q(i+1),i+1); + end + end function J = pose_jacobian(obj, q, ith) % POSE_JACOBIAN(q) returns the Jacobian that satisfies From b2fece8191f4a2fe0c2453a56b8e489014586fd4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 18:55:12 +0900 Subject: [PATCH 65/74] [DQ_SerialManipulatorDH.m] Removed the concrete methods raw_fkm, raw_pose_jacobian, and raw_pose_jacobian_derivative --- robot_modeling/DQ_SerialManipulatorDH.m | 108 +----------------------- 1 file changed, 1 insertion(+), 107 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 988a27ce..20ed8c51 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -191,113 +191,7 @@ 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 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 - - function J_dot = raw_pose_jacobian_derivative(obj,q,q_dot, to_ith_link) - % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot) returns the Jacobian - % time derivative. - % - % RAW_POSE_JACOBIAN_DERIVATIVE(q,q_dot,to_ith_link) returns the first - % to_ith_link columns of the Jacobian time derivative. - % This function does not take into account any base or - % end-effector displacements. - % obj.check_q_vec(q); - % obj.check_q_vec(q_dot); - - if nargin == 4 - n = to_ith_link; - x_effector = obj.raw_fkm(q,to_ith_link); - J = obj.raw_pose_jacobian(q,to_ith_link); - vec_x_effector_dot = J*q_dot(1:to_ith_link); - else - n = obj.n_links; - % obj.check_to_ith_link(n); - x_effector = obj.raw_fkm(q); - J = obj.raw_pose_jacobian(q); - vec_x_effector_dot = J*q_dot; - end - - x = DQ(1); - J_dot = zeros(8,n); - - for i = 0:n-1 - % Use the standard DH convention - - w = obj.get_w(i+1); %w = DQ.k; - %z = DQ(obj.get_z(x.q)); - z = 0.5*x*w*conj(x); - - % When i = 0 and length(theta) = 1, theta(1,i) returns - % a 1 x 0 vector, differently from the expected - % behavior, which is to return a 0 x 1 matrix. - % Therefore, we have to deal with the case i = 0 - % explictly. - if i ~= 0 - vec_zdot = 0.5*(haminus8(w*x') + ... - hamiplus8(x*w)*DQ.C8) * ... - obj.raw_pose_jacobian(q,i)*q_dot(1:i); - else - vec_zdot = zeros(8,1); - end - J_dot(:,i+1) = haminus8(x_effector)*vec_zdot +... - hamiplus8(z)*vec_x_effector_dot; - x = x*obj.dh2dq(q(i+1),i+1); - end - end + end end \ No newline at end of file From a8dd1ceddfc89e18c0ba0d0c1178532db8884817 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 19:01:20 +0900 Subject: [PATCH 66/74] [DQ_SerialManipulatorDH.m] Renamed the method dh2dq. --- robot_modeling/DQ_SerialManipulatorDH.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 20ed8c51..572dc7a9 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -82,11 +82,11 @@ end methods (Access = protected) - function dq = dh2dq(obj,q,ith) - % DQ2DQ(q, ith) calculates the corresponding dual quaternion for + function dq = get_link2dq(obj,q,ith) + % GET_LINK2DQ(q, ith) calculates the corresponding dual quaternion for % a given link's DH parameters % - % Usage: dq = dh2dq(q,ith), where + % Usage: dq = get_link2dq(q,ith), where % q: joint value % ith: link number % From 3c724abdb1477c0f12b55e24d4d06b74a44c4168 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 19:02:04 +0900 Subject: [PATCH 67/74] [DQ_SerialManipulatorMDH.m] Updated the class according to the new design proposed by Murilo. --- robot_modeling/DQ_SerialManipulatorMDH.m | 57 +++++++++++++++++++++--- 1 file changed, 52 insertions(+), 5 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m index b62cd6af..fb3d22f3 100644 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -68,14 +68,25 @@ -classdef DQ_SerialManipulatorMDH < DQ_SerialManipulatorDH - +classdef DQ_SerialManipulatorMDH < DQ_SerialManipulator + properties + theta,d,a,alpha; + type + end + + properties (Constant) + % Joints that can be actuated + % Rotational joint + JOINT_ROTATIONAL = 1; % Deprecated + % Prismatic joint + JOINT_PRISMATIC = 2; % Deprecated + end methods (Access = protected) - function dq = dh2dq(obj,q,ith) - % DQ2DQ(q, ith) calculates the corresponding dual quaternion for + function dq = get_link2dq(obj,q,ith) + % GET_LINK2DQ(q, ith) calculates the corresponding dual quaternion for % a given link's modified DH parameters % - % Usage: dq = dh2dq(q,ith), where + % Usage: dq = get_link2dq(q,ith), where % q: joint value % ith: link number % @@ -139,5 +150,41 @@ end end + methods + function obj = DQ_SerialManipulatorMDH(A) + % These are initialized in the constructor of + % DQ_SerialManipulator + % obj.dim_configuration_space = dim_configuration_space; + + str = ['DQ_SerialManipulatorMDH(A), where ' ... + 'A = [theta1 ... thetan; ' ... + ' d1 ... dn; ' ... + ' a1 ... an; ' ... + ' alpha1 ... alphan; ' ... + ' type1 ... typen]']; + + + if nargin == 0 + error(['Input: matrix whose columns contain the modified DH parameters' ... + ' and type of joints. Example: ' str]) + end + + if(size(A,1) ~= 5) + error('Input: Invalid modified DH matrix. It must have 5 rows.') + end + + % n_links + % TODO: change n_links to dim_configuration_space + obj.n_links = size(A,2); + + % Add theta, d, a, alpha and type + obj.theta = A(1,:); + obj.d = A(2,:); + obj.a = A(3,:); + obj.alpha = A(4,:); + obj.type = A(5,:); + end + + end end \ No newline at end of file From b3e1219b897638228a756d87764808ff7ec78b87 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 19:56:33 +0900 Subject: [PATCH 68/74] [DQ_SerialManipulator.m] Updated the header. --- robot_modeling/DQ_SerialManipulator.m | 3 --- 1 file changed, 3 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index c0712914..be208901 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -8,9 +8,6 @@ % 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. % set_effector - Set an arbitrary end-effector rigid transformation with respect to the last frame in the kinematic chain. -% -% -% DQ_SerialManipulator Methods (Abstract): % 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. % raw_pose_jacobian_derivative - Compute the pose Jacobian derivative without taking into account base's and end-effector's rigid transformations. From a4c071bef649e8eef035433c1102dc7ece7020c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 19:56:50 +0900 Subject: [PATCH 69/74] [DQ_SerialManipulatorDH.m] Updated the header. --- robot_modeling/DQ_SerialManipulatorDH.m | 1 + 1 file changed, 1 insertion(+) diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 572dc7a9..1c0246f7 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -26,6 +26,7 @@ % 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. +% raw_pose_jacobian_derivative - Compute the pose Jacobian derivative 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. From ff8bb4395f60ffd85280ac4d689135bd544936f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 12 Dec 2022 19:57:16 +0900 Subject: [PATCH 70/74] [DQ_SerialManipulatorMDH.m] Updated the header. --- robot_modeling/DQ_SerialManipulatorMDH.m | 1 + 1 file changed, 1 insertion(+) diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m index fb3d22f3..8f096006 100644 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -26,6 +26,7 @@ % 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. +% raw_pose_jacobian_derivative - Compute the pose Jacobian derivative 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. From bee728b9a09f90e7989f9f632b4e55d69cd2abfa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Thu, 15 Dec 2022 20:38:10 +0900 Subject: [PATCH 71/74] [DQ_SerialManipulator.m] Modified the documentation of raw_fkm as suggested by Bruno. --- robot_modeling/DQ_SerialManipulator.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index be208901..55832c70 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -138,8 +138,8 @@ function set_effector(obj,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. + % 'to_ith_link' defines the last link that will be used in + % calculations of the forward kinematics. % % This is an auxiliary function to be used mainly with the % Jacobian function. From cebaba2cb3e524e5ce09bd162aadfaf385678f65 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 26 Dec 2022 17:14:02 +0900 Subject: [PATCH 72/74] This commit adds methods to set and get the type of joints. [DQ_SerialManipulator] Added the protected property joint_types. Furthermore, I implemented the methods set_joint_types, set_joint_type, get_joint_types and get_joint_type. [DQ_SerialManipulatorDH, MDH] Updated the subclasses to comply the modifications made in DQ_SerialManipulator --- robot_modeling/DQ_SerialManipulator.m | 25 ++++++++++++++++++++++++ robot_modeling/DQ_SerialManipulatorDH.m | 7 +++---- robot_modeling/DQ_SerialManipulatorMDH.m | 7 +++---- 3 files changed, 31 insertions(+), 8 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index 55832c70..bd4a1453 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -51,6 +51,7 @@ % mainly in the plot function. handle n_links; + joint_types; end methods (Abstract, Access = protected) @@ -98,6 +99,30 @@ 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' the vector that contains the joint types. + obj.joint_types = 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; + end + + function ret = get_joint_types(obj) + % GET_JOINT_TYPES() returns the vector of 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..29336470 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -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." @@ -189,7 +188,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..2b394bff 100644 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -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,7 +141,7 @@ % 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 @@ -183,7 +182,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 From 436db7ae831f57d836a71cc2f4421978a8a90ec6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 26 Dec 2022 17:28:43 +0900 Subject: [PATCH 73/74] This commit adds the method get_suppported_joint_types() [DQ_SerialManipulator] Added the abstract and protected method get_supported_joint_types(). [DQ_SerialManipulatorDH, MDH] Implemented the method get_supported_joint_types() in both subclasses to comply the modification in DQ_SerialManipulator. --- robot_modeling/DQ_SerialManipulator.m | 4 ++++ robot_modeling/DQ_SerialManipulatorDH.m | 5 +++++ robot_modeling/DQ_SerialManipulatorMDH.m | 5 +++++ 3 files changed, 14 insertions(+) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index bd4a1453..ae70ae4d 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -75,7 +75,11 @@ % Usage: w = get_w(ith), where % ith: link number w = get_w(obj,ith) ; + + % This method returns the supported joint types. + st = get_supported_joint_types(~); end + methods function obj = DQ_SerialManipulator() diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m index 29336470..211abc14 100644 --- a/robot_modeling/DQ_SerialManipulatorDH.m +++ b/robot_modeling/DQ_SerialManipulatorDH.m @@ -149,6 +149,11 @@ w = DQ.E*DQ.k; end end + + function ret = get_supported_joint_types(~) + % This method returns the supported joint types. + ret = [DQ_JointType.REVOLUTE, DQ_JointType.PRISMATIC]; + end end methods diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m index 2b394bff..b668ad93 100644 --- a/robot_modeling/DQ_SerialManipulatorMDH.m +++ b/robot_modeling/DQ_SerialManipulatorMDH.m @@ -148,6 +148,11 @@ w = DQ.E*(cos(obj.alpha(ith))*DQ.k - sin(obj.alpha(ith))*DQ.j); end end + + function ret = get_supported_joint_types(~) + % This method returns the supported joint types. + ret = [DQ_JointType.REVOLUTE, DQ_JointType.PRISMATIC]; + end end methods From 2ce8e9cf6a517f63d042da475c175dc29bd5a559 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Mon, 26 Dec 2022 17:56:51 +0900 Subject: [PATCH 74/74] [DQ_SerialManipulator] Added the check_joint_types method. --- robot_modeling/DQ_SerialManipulator.m | 43 +++++++++++++++++++++++++-- 1 file changed, 40 insertions(+), 3 deletions(-) diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m index ae70ae4d..e399663d 100644 --- a/robot_modeling/DQ_SerialManipulator.m +++ b/robot_modeling/DQ_SerialManipulator.m @@ -79,9 +79,44 @@ % This method returns the supported joint types. st = get_supported_joint_types(~); end - - - methods + + methods + 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 obj.base_frame = DQ(1); @@ -108,6 +143,7 @@ function set_joint_types(obj, joint_types) % SET_JOINT_TYPES(joint_types) sets the joint types. % 'joint_types' the vector that contains the joint types. obj.joint_types = joint_types; + obj.check_joint_types(); end function set_joint_type(obj, joint_type, ith_joint) @@ -116,6 +152,7 @@ function set_joint_type(obj, joint_type, 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)