[BUG] Intermediate jacobian matrix from DQ_WholeBody class does not return correct number of columns.

[StUrb]

Bug description

• I serialized two kinematic chains using the class DQ_WholeBody. The first chain is reversed and the second is normal. When I call the method pose_jacobian for the i-th joint of the reversed chain, the method does not return the correct matrix size, i.e., 8xi.

To Reproduce
I used the two_leg_kinematic_control.m example from the DQ lybrary to start my minimal example.

Code

    clear all;
    close all;
    clc;
    
    % Include the DQ name space to use i_ instead of DQ.i, etc.
    include_namespace_dq
    
    % The legs are composed of two KUKA LWR4 robots coupled together    
    right_leg = KukaLwr4Robot.kinematics();
    right_leg.name = 'Right leg';
    left_leg = KukaLwr4Robot.kinematics();
    left_leg.name = 'Left leg';
           
    %% Let's assemble the robot.
    % This is just for the sake of example. Ideally, we would create a
    % dedicated class, such as DQ_LeggedRobot to do all of the low-level
    % assembly.
    
    % The left foot will be located 20 cm to the left, along the x axis,
    % but with the z-axis pointing downwards.
    left_foot = (1 + E_ * 0.5 * (-0.2*i_ - j_)) * i_;
    % The right foot will be located 20 cm to the right, along the x axis,
    % but with the z-axis pointing downwards.
    right_foot = (1 + E_ * 0.5 * (0.2*i_ - j_)) * i_;
    
    % Physically place the left leg in the workspace
    x_0_to_left_effector = left_leg.fkm(zeros(7,1));
    new_base_frame = left_foot .* T(x_0_to_left_effector);
    left_leg.set_base_frame(new_base_frame);
    % Physically place the right leg in the workspace
    x_0_to_right_effector = right_leg.fkm(zeros(7,1));
    new_base_frame = right_foot .* T(x_0_to_right_effector);
    right_leg.set_base_frame(new_base_frame);
    
    %% Initializes the serially coupled kinematic chains
    % From the left leg towards the right leg
    left_to_right = DQ_WholeBody(left_leg, 'reversed');
    % Determines the rigid transformation between the base frames of the
    % two legs
    base_left_to_base_right = left_leg.base_frame'*right_leg.base_frame;
    % Add this constant transformation to the chain
    left_to_right.add(base_left_to_base_right);    
    % Finally, add the right frame
    left_to_right.add(right_leg);
    
    % We do the same thing, but from the right leg towards the left leg
    right_to_left = DQ_WholeBody(right_leg, 'reversed');
    % Determines the rigid transformation between the base frames of the
    % two legs
    base_right_to_base_left = base_left_to_base_right';
    % Add this constant transformation to the chain
    right_to_left.add(base_right_to_base_left);    
    % Finally, add the left leg
    right_to_left.add(left_leg);
    
    % Configuration.
    q = zeros(1,14);
    
    J = left_to_right.pose_jacobian(q,1,1)
    
    s = size(J)

Output

J =

             0             0             0             0             0             0             0
             0             0             0             0             0             0             0
   -0.0000             0   -0.0000             0   -0.0000             0             0
             0   -0.0000             0     0.0000             0   -0.0000   -0.5000
   -0.5500   -0.0000   -0.5500   -0.0000   -0.5500   -0.0000             0
   -0.0000     0.5500  -0.0000   -0.5500   -0.0000     0.5500     0.0000
             0             0             0             0             0             0              0
             0             0             0             0             0             0              0

s =

     8     7

Expected behavior

• In the example I call ' left_to_right.pose_jacobian(q,1,1)' expecting the jacobian until the first joint, i.e., a matrix with eight rows and one column.

Expected output

s =

     8     1

Environment:

• OS: [Ubuntu 18.04]
• dqrobotics latest version (I just did a pull today)
• MATLAB version R2016b