Conversion of DQ_SerialManipulator to an abstract class

[juanjqo]

@dqrobotics/developers

Hi @bvadorno,

this PR modifies the classes DQ_SerialManipulator and DQ_SerialManipulatorDH. The following changes are proposed:

• DQ_SerialManipulator is an abstract class now.
• The methods raw_fkm(), raw_pose_jacobian() and raw_pose_jacobian_derivative() are defined as abstract methods in DQ_SerialManipulator and they are defined as concrete ones in DQ_SerialManipulatorDH.
• The methods fkm(), pose_jacobian() and pose_jacobian_derivative() are defined only in DQ_SerialManipulator.

Update

• Added the class DQ_SerialManipulatorMDH.

Requirements:

• This PR does not implement the class DQ_SerialManipulatorMDH() and therefore it is required this modification in matlab-examples.

UML diagram:
Blue background with bold text represents modifications or new things.

Final design (proposed by @mmmarinho)

Tasks:

• Add the contributing file.
• DQ_SerialManipulator is now an abstrac class.
• Update DQ_SerialManipulatorDH.
• Update the matlab-examples to ensure compatibility. (here)
• Include a UML diagram to show the proposed changes.
• Add the class DQ_SerialManipulatorMDH.

The main goal is to update/sincronize the current version of dqrobotics-matlab with respecto to the C++ version. A lot of things are not sincronized between them yet, but this is the first step.

Best regards,

Juancho

[juanjqo]

@dqrobotics/developers

Hi @mmmarinho and @bvadorno,

Assuming I'm not missing anything, this is the UML design proposed by Murilo.

Best regards,

Juancho

[juanjqo]

Hi @bvadorno and @mmmarinho

Following our previous discussion, I have a proposal to modify the constructor of the subclasses. Instead of using a cell, we could have two arguments: a 4xn DH matrix and the type of joints. For instance:

dh_matrix = [theta; d; a; alpha];
types = repmat(DQ_JointType.REVOLUTE, 1,5);  % No casting to double 
robot = DQ_SerialManipulatorDH(dh_matrix, types); 

The key justification is that while a cell might be simple to create in Matlab or (pure) Python, it might be challenging to do so in C++ without using a struct, and, looks like that somethings as structs or raw pointers (raw pointers are being replaced by smart pointers) are not being used in the current implementation of the library in C++, at least for public functions or methods. (@mmmarinho please correct me if I'm wrong).

To ensure compatibility with previous versions, the constructor could be (this is a very raw beta implementation)

function obj = DQ_SerialManipulatorDH(A, types)
        obj.n_links = size(A,2);
        str_new = ['DQ_SerialManipulatorDH(A, types), where ' ...
                   'A = [theta1 ... thetan; ' ...
                   ' d1  ...   dn; ' ...
                   ' a1  ...   an; ' ...
                   ' alpha1 ... alphan;] ' ...
                   '                     ' ...
                   'types = [DQ_JointType.REVOLUTE ... DQ_JointType.PRISMATIC]'];
        
        switch nargin
            case 0
                error(['Input: matrix whose columns contain the DH parameters' ...
                   ' and type of joints. Example: ' str_new])
            case 1
                 warning(['DQ_SerialManipulatorDH(A) is deprecated.' ...
                    ' Please use DQ_SerialManipulatorDH(A, types) instead. Example: ' str_new]);  
                 if(size(A,1) ~= 5)
                   error('Input: Invalid DH matrix. It must have 5 rows.')
                 end
                 obj.theta = A(1,:);
                 obj.d     = A(2,:);
                 obj.a     = A(3,:);
                 obj.alpha = A(4,:);
                 obj.type  = A(5,:);

            case 2
                if isa( types, 'char') % ex: types = 'standard'
                    warning(['DQ_SerialManipulatorDH(A, convention) is deprecated.' ...
                    ' Please use DQ_SerialManipulatorDH(A, types) instead. Example: ' str_new]);   
                    if(size(A,1) ~= 5)
                       error('Input: Invalid DH matrix. It must have 5 rows.')
                    end
                    obj.theta = A(1,:);
                    obj.d     = A(2,:);
                    obj.a     = A(3,:);
                    obj.alpha = A(4,:);
                    obj.type  = A(5,:);
                elseif strcmp(class(convention), 'DQ_JointType')
                    if(size(A,1) ~= 4)
                      error('Input: Invalid DH matrix. It must have 4 rows.')
                    end
                    obj.theta = A(1,:);
                    obj.d     = A(2,:);
                    obj.a     = A(3,:);
                    obj.alpha = A(4,:);
                    obj.type  = types;
                elseif isa(obj.type, 'double') 
                    warning(['bla bla bla. Use DQ_JoinType instead.'])
                    if(size(A,1) ~= 4)
                      error('Input: Invalid DH matrix. It must have 4 rows.')
                    end
                    obj.theta = A(1,:);
                    obj.d     = A(2,:);
                    obj.a     = A(3,:);
                    obj.alpha = A(4,:);
                    obj.type  = types;
                    
                end
        end
    end
end

Cases:

1. DQ_SerialManipulatorDH(A, 'standard'), where A is a 5xn matrix. This throws a warning of deprecation

types = double(repmat(DQ_JointType.REVOLUTE, 1,5));  
dh_matrix = [theta; d; a; alpha, types];
robot = DQ_SerialManipulatorDH(dh_matrix, 'standard'); 

2. DQ_SerialManipulatorDH(A), where A is a 5xn matrix. This throws a warning of deprecation

types = double(repmat(DQ_JointType.REVOLUTE, 1,5));  
dh_matrix = [theta; d; a; alpha, types];
robot = DQ_SerialManipulatorDH(dh_matrix); 

3. DQ_SerialManipulatorDH(A, types), where A is a 4xn matrix, and types is built using integers. This throws a warning because the user is not using the class DQ_JointTypes.

dh_matrix = [theta; d; a; alpha];
types = [1 ... 1];
robot = DQ_SerialManipulatorDH(dh_matrix, types); 

4. DQ_SerialManipulatorDH(A, types), where A is a 4xn, and types is built using the class DQ_JointType.

dh_matrix = [theta; d; a; alpha];
types = repmat(DQ_JointType.REVOLUTE, 1,5);  % No casting to double 
robot = DQ_SerialManipulatorDH(dh_matrix, types); 

I think this could be easily implemented in C++/Python. For instance, we could have something like

DQ_SerialManipulatorDH(const MatrixXd& dh_matrix, const std::vector<DQ_JointTypes>& types);

This proposal make sense or could be useful? what do you think?

Best regards,

Juancho

[bvadorno]

@juanjqo,
The calculations for get_w() are correct. However, this class is essentially the same as the DQ_SerialManipulatorDH except for the methods dh2dq() and get_w(). A better design would be to have DQ_SerialManipulatorMDH inheriting from DQ_SerialManipulatorDH, and then overloading the methods dh2dq() and get_w().
The implementation will become much cleaner, the distinction between the two classes much more evident, and any future maintenance will be much simpler.

@bvadorno @juanjqo

Sorry to intrude, but can we take this one step further?

The original work in @bvadorno's thesis describes, among other things, general fkm and Jacobian derivations for any serial-link manipulator regardless of parametrization. Hence, I'd like to suggest that we reflect that on the code by doing the following:

1. We move the current implementation of


* `raw_fkm()`

* `raw_pose_jacobian()`
  and possibly others to `DQ_SerialManipulator`.


2. In `DQ_SerialManipulator`, we can have two protected abstract methods, such as
   `get_w()` (Same as before because the naming is generic)
   `get_link2dq()` (A more general name instead of `dh2dq`), naming pending. (@bvadorno @juanjqo please help me with a better name here.)

3. `DQ_SerialManipulatorDH`, `DQ_SerialManipulatorMDH`, `DQ_SerialManipulatorDenso`, and possibly other sub-classes inherit only from `DQ_SerialManipulator` and, by consequence, only need to implement `get_w()` and `get_link_dq()`.

This way, all sub-classes will be much easier to implement and maintain. In addition, the hierarchy chain will be also cleaner. This is basically @bvadorno's suggestion one step up in the hierarchy.

@juanjqo could you please make a quick UML to help me describe this concept? Please only make any possible changes to the code related to my suggestion after we all agree on this point. Thanks in advance!

Hi @mmmarinho,

It's a sensible strategy. Let's proceed with it.

Best wishes,
Bruno

[bvadorno]

Hi @bvadorno and @mmmarinho

Following our previous discussion, I have a proposal to modify the constructor of the subclasses. Instead of using a cell, we could have two arguments: a 4xn DH matrix and the type of joints. For instance:

dh_matrix = [theta; d; a; alpha];
types = repmat(DQ_JointType.REVOLUTE, 1,5);  % No casting to double 
robot = DQ_SerialManipulatorDH(dh_matrix, types); 

The key justification is that while a cell might be simple to create in Matlab or (pure) Python, it might be challenging to do so in C++ without using a struct, and, looks like that somethings as structs or raw pointers (raw pointers are being replaced by smart pointers) are not being used in the current implementation of the library in C++, at least for public functions or methods. (@mmmarinho please correct me if I'm wrong).

To ensure compatibility with previous versions, the constructor could be (this is a very raw beta implementation)

function obj = DQ_SerialManipulatorDH(A, types)
        obj.n_links = size(A,2);
        str_new = ['DQ_SerialManipulatorDH(A, types), where ' ...
                   'A = [theta1 ... thetan; ' ...
                   ' d1  ...   dn; ' ...
                   ' a1  ...   an; ' ...
                   ' alpha1 ... alphan;] ' ...
                   '                     ' ...
                   'types = [DQ_JointType.REVOLUTE ... DQ_JointType.PRISMATIC]'];
        
        switch nargin
            case 0
                error(['Input: matrix whose columns contain the DH parameters' ...
                   ' and type of joints. Example: ' str_new])
            case 1
                 warning(['DQ_SerialManipulatorDH(A) is deprecated.' ...
                    ' Please use DQ_SerialManipulatorDH(A, types) instead. Example: ' str_new]);  
                 if(size(A,1) ~= 5)
                   error('Input: Invalid DH matrix. It must have 5 rows.')
                 end
                 obj.theta = A(1,:);
                 obj.d     = A(2,:);
                 obj.a     = A(3,:);
                 obj.alpha = A(4,:);
                 obj.type  = A(5,:);

            case 2
                if isa( types, 'char') % ex: types = 'standard'
                    warning(['DQ_SerialManipulatorDH(A, convention) is deprecated.' ...
                    ' Please use DQ_SerialManipulatorDH(A, types) instead. Example: ' str_new]);   
                    if(size(A,1) ~= 5)
                       error('Input: Invalid DH matrix. It must have 5 rows.')
                    end
                    obj.theta = A(1,:);
                    obj.d     = A(2,:);
                    obj.a     = A(3,:);
                    obj.alpha = A(4,:);
                    obj.type  = A(5,:);
                elseif strcmp(class(convention), 'DQ_JointType')
                    if(size(A,1) ~= 4)
                      error('Input: Invalid DH matrix. It must have 4 rows.')
                    end
                    obj.theta = A(1,:);
                    obj.d     = A(2,:);
                    obj.a     = A(3,:);
                    obj.alpha = A(4,:);
                    obj.type  = types;
                elseif isa(obj.type, 'double') 
                    warning(['bla bla bla. Use DQ_JoinType instead.'])
                    if(size(A,1) ~= 4)
                      error('Input: Invalid DH matrix. It must have 4 rows.')
                    end
                    obj.theta = A(1,:);
                    obj.d     = A(2,:);
                    obj.a     = A(3,:);
                    obj.alpha = A(4,:);
                    obj.type  = types;
                    
                end
        end
    end
end

Cases:

1. DQ_SerialManipulatorDH(A, 'standard'), where A is a 5xn matrix.  This throws a warning of deprecation

types = double(repmat(DQ_JointType.REVOLUTE, 1,5));  
dh_matrix = [theta; d; a; alpha, types];
robot = DQ_SerialManipulatorDH(dh_matrix, 'standard'); 

2. DQ_SerialManipulatorDH(A), where A is a 5xn matrix. This throws a warning of deprecation

types = double(repmat(DQ_JointType.REVOLUTE, 1,5));  
dh_matrix = [theta; d; a; alpha, types];
robot = DQ_SerialManipulatorDH(dh_matrix); 

3. DQ_SerialManipulatorDH(A, types), where A is a 4xn matrix, and types is built using integers. This throws a warning because the user is not using the class DQ_JointTypes.

dh_matrix = [theta; d; a; alpha];
types = [1 ... 1];
robot = DQ_SerialManipulatorDH(dh_matrix, types); 

4. DQ_SerialManipulatorDH(A, types), where A is a 4xn, and types is built using the class DQ_JointType.

dh_matrix = [theta; d; a; alpha];
types = repmat(DQ_JointType.REVOLUTE, 1,5);  % No casting to double 
robot = DQ_SerialManipulatorDH(dh_matrix, types); 

I think this could be easily implemented in C++/Python. For instance, we could have something like

DQ_SerialManipulatorDH(const MatrixXd& dh_matrix, const std::vector<DQ_JointTypes>& types);

This proposal make sense or could be useful? what do you think?

Best regards,

Juancho

Hi @juancho,

Let's defer this because we need to focus on @mmmarinho's suggestion.

Your solution works, but we need to see if it's necessary, given your motivations, because cells in MATLAB are just a mechanism to store data. Therefore, it suffices to use a container in C++, which is pretty standard, to provide the same functionality as cells. For example, see how to use lists.

[mmmarinho]

@juanjqo

No comment on the applicability of the code for now, but a little bit of advice in making it less verbose, if applicable.

function obj = DQ_SerialManipulatorDH(A, types)
        obj.n_links = size(A,2);

        % Maybe explain what this switch aims to achieve and some information about which is legacy
        switch nargin
            case 0 % No empty constructor
                error(['Input: matrix whose columns contain the DH parameters' ...
                   ' and type of joints.'])
            case 1 % One argument initialization is deprecated from 23.04
                 warning(['DQ_SerialManipulatorDH(A) is deprecated.' ...
                    ' Please use DQ_SerialManipulatorDH(A, types) instead.']);  
                 if(size(A,1) ~= 5)
                   error('Input: Invalid DH matrix. It must have 5 rows.')
                 end
                 obj.type  = A(5,:);
            case 2 % String convention is deprecated since (?) 
                if isa( types, 'char') % ex: types = 'standard'
                    warning(['DQ_SerialManipulatorDH(A, convention) is deprecated.' ...
                    ' Please use DQ_SerialManipulatorDH(A, types) instead. Example: ' str_new]);   
                    if(size(A,1) ~= 5)
                       error('Input: Invalid DH matrix. It must have 5 rows.')
                    end
                    obj.type  = A(5,:);
                elseif strcmp(class(types), 'DQ_JointType')
                    if(size(A,1) ~= 4)
                      error('Input: Invalid DH matrix. It must have 4 rows.')
                    end
                    obj.type  = types;
                elseif isa(types, 'double') 
                    warning(['TODO warning. Use DQ_JoinType instead.'])
                    if(size(A,1) ~= 4)
                      error('Input: Invalid DH matrix. It must have 4 rows.')
                    end
                    obj.type  = types;
                end
        end
        obj.theta = A(1,:);
        obj.d     = A(2,:);
        obj.a     = A(3,:);
        obj.alpha = A(4,:);
    end
end

[juanjqo]

Hi @bvadorno, thank you for your feedback. I implemented the new design proposed by @mmmarinho.

[juanjqo]

@juanjqo

No comment on the applicability of the code for now, but a little bit of advice in making it less verbose, if applicable.

function obj = DQ_SerialManipulatorDH(A, types)
        obj.n_links = size(A,2);

        % Maybe explain what this switch aims to achieve and some information about which is legacy
        switch nargin
            case 0 % No empty constructor
                error(['Input: matrix whose columns contain the DH parameters' ...
                   ' and type of joints.'])
            case 1 % One argument initialization is deprecated from 23.04
                 warning(['DQ_SerialManipulatorDH(A) is deprecated.' ...
                    ' Please use DQ_SerialManipulatorDH(A, types) instead.']);  
                 if(size(A,1) ~= 5)
                   error('Input: Invalid DH matrix. It must have 5 rows.')
                 end
                 obj.type  = A(5,:);
            case 2 % String convention is deprecated since (?) 
                if isa( types, 'char') % ex: types = 'standard'
                    warning(['DQ_SerialManipulatorDH(A, convention) is deprecated.' ...
                    ' Please use DQ_SerialManipulatorDH(A, types) instead. Example: ' str_new]);   
                    if(size(A,1) ~= 5)
                       error('Input: Invalid DH matrix. It must have 5 rows.')
                    end
                    obj.type  = A(5,:);
                elseif strcmp(class(types), 'DQ_JointType')
                    if(size(A,1) ~= 4)
                      error('Input: Invalid DH matrix. It must have 4 rows.')
                    end
                    obj.type  = types;
                elseif isa(types, 'double') 
                    warning(['TODO warning. Use DQ_JoinType instead.'])
                    if(size(A,1) ~= 4)
                      error('Input: Invalid DH matrix. It must have 4 rows.')
                    end
                    obj.type  = types;
                end
        end
        obj.theta = A(1,:);
        obj.d     = A(2,:);
        obj.a     = A(3,:);
        obj.alpha = A(4,:);
    end
end

Hi @mmmarinho, thank you for your feedback. Your suggestion is really welcome. The code is less verbose!

[juanjqo]

@bvadorno I realized that I forgot to update the header of DQ_SerialManipulator, specifically the part describing raw_fkm, raw_pose_jacobian, and raw_pose_jacobian_derivative as abstract methods.
Can I update it?

[bvadorno]

@bvadorno I realized that I forgot to update the header of DQ_SerialManipulator, specifically the part describing raw_fkm, raw_pose_jacobian, and raw_pose_jacobian_derivative as abstract methods. Can I update it?

Hi @juanjqo,

Yes, you can. I won't be able to work on this review in the next few hours.

Best wishes,
Bruno

[juanjqo]

@bvadorno I realized that I forgot to update the header of DQ_SerialManipulator, specifically the part describing raw_fkm, raw_pose_jacobian, and raw_pose_jacobian_derivative as abstract methods. Can I update it?

Hi @juanjqo,

Yes, you can. I won't be able to work on this review in the next few hours.

Best wishes, Bruno

@bvadorno thanks. I implemented the modifications. The PR is ready to your review.