diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 00000000..b8dfafc0
--- /dev/null
+++ b/CONTRIBUTING.md
@@ -0,0 +1,96 @@
+# Welcome, contributors to DQ Robotics!
+
+We are happy about your interest in our project! DQ Robotics is a standalone, **fully moderated** open-source library, and contributions are always welcome!
+
+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 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 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/)).
+
+
+
+## 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 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.
+
+
+
+- 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
+
+## Fork the [dqrobotics/matlab](https://github.com/dqrobotics/matlab) in your Github account
+
+
+
+## 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`
+
+
+
+## Make your modifications
+
+Now, you can modify the code with your contributions.
+(In this specific example, as shown in the animated figure below, the CONTRIBUTING.md file is modified.)
+
+
+
+## Add, commit and push your changes
+
+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."`
+
+
+
+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.
+
+
+
+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'.
+
+
diff --git a/robot_modeling/DQ_JointType.m b/robot_modeling/DQ_JointType.m
new file mode 100644
index 00000000..db81f487
--- /dev/null
+++ b/robot_modeling/DQ_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:
+%
+% 1. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp)
+% Responsible for the original implementation.
+%
+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
+ % 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
+
+
+
diff --git a/robot_modeling/DQ_Kinematics.m b/robot_modeling/DQ_Kinematics.m
index d526b1aa..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
@@ -73,7 +73,7 @@
% Frame used to determine the robot physical location
base_frame;
% Robot configuration vector
- q
+ q;
end
methods
diff --git a/robot_modeling/DQ_SerialManipulator.m b/robot_modeling/DQ_SerialManipulator.m
index d7356871..55832c70 100644
--- a/robot_modeling/DQ_SerialManipulator.m
+++ b/robot_modeling/DQ_SerialManipulator.m
@@ -1,17 +1,5 @@
-% 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.
%
% DQ_SerialManipulator Methods (Concrete):
% get_dim_configuration_space - Return the dimension of the configuration space.
@@ -19,12 +7,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.
% 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-2020 DQ Robotics Developers
+% (C) Copyright 2011-2022 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
@@ -44,16 +35,10 @@
% 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 DQ_SerialManipulator < DQ_Kinematics
+classdef (Abstract) DQ_SerialManipulator < DQ_Kinematics
properties
- theta,d,a,alpha;
- convention;
effector;
% Properties for the plot function
@@ -67,36 +52,42 @@
handle
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
- function obj = DQ_SerialManipulator(A,convention)
- 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,:);
-
+ 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
-
+
% 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};
- obj.plotopt = {};
-
+ obj.plotopt = {};
end
function ret = get_dim_configuration_space(obj)
@@ -104,39 +95,10 @@
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
- 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
@@ -158,54 +120,6 @@ function set_effector(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
-
function p = get_z(~,h)
p(1) = 0;
p(2) = h(2)*h(4) + h(1)*h(3);
@@ -215,9 +129,35 @@ 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 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 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.
+ 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,ith)
+
+ 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.
@@ -230,31 +170,72 @@ function set_effector(obj,effector)
% 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);
+ 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,n);
+ 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
- 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);
+ 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
@@ -268,7 +249,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
@@ -289,48 +270,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 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
- 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 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)*...
+ obj.raw_pose_jacobian_derivative(q, q_dot);
end
end
@@ -769,6 +720,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 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
o.workspace = [-reach reach -reach reach -reach reach];
diff --git a/robot_modeling/DQ_SerialManipulatorDH.m b/robot_modeling/DQ_SerialManipulatorDH.m
index e906bcc5..1c0246f7 100644
--- a/robot_modeling/DQ_SerialManipulatorDH.m
+++ b/robot_modeling/DQ_SerialManipulatorDH.m
@@ -2,20 +2,20 @@
% 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;
% 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):
@@ -26,10 +26,11 @@
% 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.
-% (C) Copyright 2020 DQ Robotics Developers
+% (C) Copyright 2020-2022 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
@@ -49,125 +50,64 @@
% 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
+% [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.
+% [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 #75 (https://github.com/dqrobotics/matlab/pull/75)
+% to define DQ_SerialManipulator as an abstract class.
classdef DQ_SerialManipulatorDH < DQ_SerialManipulator
properties
+ theta,d,a,alpha;
type
end
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
- function obj = DQ_SerialManipulatorDH(A,convention)
- % These are initialized in the constructor of
- % DQ_SerialManipulator
- %obj.convention = convention;
- %obj.n_links = size(A,2);
- %obj.theta = A(1,:);
- %obj.d = A(2,:);
- %obj.a = A(3,:);
- %obj.alpha = A(4,:);
- obj = obj@DQ_SerialManipulator(A(1:4,:),convention);
-
- if nargin == 0
- error('Input: matrix whose columns contain the DH parameters')
- end
-
- if(size(A,1) ~= 5)
- error('Input: Invalid DH matrix. It should have 5 rows.')
- end
-
- % Add type
- obj.type = A(5,:);
- end
-
- function x = raw_fkm(obj,q,to_ith_link)
- % RAW_FKM(q) calculates the forward kinematic model and
- % returns the dual quaternion corresponding to the
- % last joint (the displacements due to the base and the effector
- % are not taken into account).
- %
- % 'q' is the vector of joint variables
- % 'to_ith_link' defines until which link the raw_fkm will be
- % calculated.
- %
- % This is an auxiliary function to be used mainly with the
- % Jacobian function.
- if nargin == 3
- n = to_ith_link;
- else
- n = obj.n_links;
- end
-
- x = DQ(1);
-
- for i=1:n
- x = x*dh2dq(obj,q(i),i);
- end
- end
-
- function x = fkm(obj,q,to_ith_link)
- % FKM(q) calculates the forward kinematic model and
- % returns the dual quaternion corresponding to the
- % end-effector pose. This function takes into account the
- % displacement due to the base's and effector's poses.
+
+ methods (Access = protected)
+ function dq = get_link2dq(obj,q,ith)
+ % GET_LINK2DQ(q, ith) calculates the corresponding dual quaternion for
+ % a given link's DH parameters
%
- % 'q' is the vector of joint variables
- % 'to_ith_link' defines up to which link the fkm will be
- % calculated. If to_ith_link corresponds to the last link,
- % the method DOES NOT take into account the transformation
- % given by set_effector. If you want to take into account
- % that transformation, use FKM(q) instead.
-
- if nargin == 3
- x = obj.reference_frame*obj.raw_fkm(q, to_ith_link); %Takes into account the base displacement
- else
- x = obj.reference_frame*obj.raw_fkm(q)*obj.effector;
- end
- end
-
- function dq = dh2dq(obj,q,ith)
- % For a given link's Extended DH parameters, calculate the correspondent dual
- % quaternion
- % Usage: dq = dh2dq(q,ith), where
+ % 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/
if nargin ~= 3
error('Wrong number of arguments. The parameters are joint value and the correspondent link')
end
- %The unoptimized standard dh2dq calculation is commented below
- %if obj.type(ith) == obj.JOINT_ROTATIONAL
- % % If joint is rotational
- % h1 = cos((obj.theta(ith)+q)/2.0)+DQ.k*sin((obj.theta(ith)+q)/2.0);
- % h2 = 1 + DQ.E*0.5*obj.d(ith)*DQ.k;
- %else
- % % If joint is prismatic
- % h1 = cos(obj.theta(ith)/2.0)+DQ.k*sin(obj.theta(ith)/2.0);
- % h2 = 1 + DQ.E*0.5*(obj.d(ith)+q)*DQ.k;
- %end
- %h3 = 1 + DQ.E*0.5*obj.a(ith)*DQ.i;
- %h4 = cos(obj.alpha(ith)/2.0)+DQ.i*sin(obj.alpha(ith)/2.0);
- %dq = h1*h2*h3*h4;
-
- % The optimized standard dh2dq calculation
% Store half angles and displacements
half_theta = obj.theta(ith)/2.0;
d = obj.d(ith);
a = obj.a(ith);
half_alpha = obj.alpha(ith)/2.0;
-
% Add the effect of the joint value
- if obj.type(ith) == obj.JOINT_ROTATIONAL
- % If joint is rotational
+ if obj.type(ith) == DQ_JointType.REVOLUTE
+ % If joint is revolute
half_theta = half_theta + (q/2.0);
else
% If joint is prismatic
@@ -180,67 +120,79 @@
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)
- if obj.type(ith) == obj.JOINT_ROTATIONAL
+ 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) == DQ_JointType.REVOLUTE
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
-
- 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
+ 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 < 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);
+ 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
- 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);
+ 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
+
+
end
end
\ No newline at end of file
diff --git a/robot_modeling/DQ_SerialManipulatorMDH.m b/robot_modeling/DQ_SerialManipulatorMDH.m
new file mode 100644
index 00000000..8f096006
--- /dev/null
+++ b/robot_modeling/DQ_SerialManipulatorMDH.m
@@ -0,0 +1,191 @@
+% 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 'modified' 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.
+% 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.
+
+% (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)
+% - Reorganized the code by moving the implementation of SerialManipulator.m
+% to the file DQ_SerialManipulatorDH at #56
+% (https://github.com/dqrobotics/matlab/pull/56),
+% 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
+% in method get_w().
+
+
+
+
+
+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 = get_link2dq(obj,q,ith)
+ % GET_LINK2DQ(q, ith) calculates the corresponding dual quaternion for
+ % a given link's modified DH 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/
+
+ 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)
+ % 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
diff --git a/robots/ComauSmartSixRobot.m b/robots/ComauSmartSixRobot.m
index 71935483..56cd302b 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)
@@ -32,13 +32,14 @@
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 = double(repmat(DQ_JointType.REVOLUTE,1,6));
comau_DH_matrix = [comau_DH_theta;
- comau_DH_d;
- comau_DH_a;
- comau_DH_alpha];
+ comau_DH_d;
+ comau_DH_a;
+ comau_DH_alpha
+ comau_DH_type];
- comau = DQ_SerialManipulator(comau_DH_matrix, 'modified');
+ 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