[LANGUAGE INCOMPATIBILITY] Different behaviors from fkm() on MATLAB and Python

[anachristinaac]

Describe the missing/unexpected functionality

For DQ_SerialManipulatorDH objects, I'm getting different behaviors in MATLAB and Python when trying to obtain the poses of intermediate frames. The minimum example below uses the 2-DOF planar robot shown in the figure, but I've also tried with robots available in the library, such as the KukaLw4Robot.

The Matlab behavior is what I would expect to be the correct output, since with it is possible to obtain the pose of the first and last frames when they are different from the reference and the end effector, respectively.

This is the robot of the example:

Matlab behavior

MATLAB SCRIPT

classdef Planar2DOF
    methods (Static)
        function ret = kinematics()
            DH_theta = [0, 0];
            DH_d = [0, 0];
            DH_a = [1, 1];
            DH_alpha = [0, 0];
            DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,2);
            DH_matrix = [DH_theta; DH_d; DH_a; DH_alpha; DH_type];
            
            obj = DQ_SerialManipulatorDH(DH_matrix,'standard');

            x = 1 + DQ.E * (1/2) * DQ([0, 1, 0]);
            obj.set_base_frame(x);
            obj.set_reference_frame(x);

            x = 1 + DQ.E * (1/2) * DQ([1, 0, 0]);
            obj.set_effector(x)

            ret = obj;
        end
    end
end

robot = Planar2DOF.kinematics();
q = [0;0];

F_ee = robot.fkm(q).translation()
F_1 = robot.fkm(q,0).translation()
F_2 = robot.fkm(q,1).translation()
F_3 = robot.fkm(q,2).translation()

MATLAB OUTPUT

F_ee = 
         3i + 1j
F_1 = 
         1j
F_2 = 
         1i + 1j
F_3 = 
         2i + 1j

Python behavior

PYTHON SCRIPT

from dqrobotics import *
from dqrobotics.robot_modeling import DQ_SerialManipulatorDH

class Planar2DOF:
    def kinematics(self) -> DQ_SerialManipulatorDH:
        DH_theta = [0, 0]
        DH_d = [0, 0]
        DH_a = [1, 1]
        DH_alpha = [0, 0]
        DH_type = [0, 0]
        DH_matrix = [DH_theta, DH_d, DH_a, DH_alpha, DH_type]

        obj = DQ_SerialManipulatorDH(DH_matrix)

        x = 1 + DQ.E * (1 / 2) * DQ([0, 1, 0])
        obj.set_base_frame(x)
        obj.set_reference_frame(x)

        x = 1 + DQ.E * (1 / 2) * DQ([1, 0, 0])
        obj.set_effector(x)

        return obj


robot = Planar2DOF().kinematics()
q = [0, 0]
print("F_ee: ", robot.fkm(q).translation())
print("F_1: ", robot.fkm(q, 0).translation())
print("F_2: ", robot.fkm(q, 1).translation())
print("F_3: ", robot.fkm(q, 2).translation())

PYTHON OUTPUT

F_ee:  3i + 1j
F_1:  1i + 1j
F_2:  3i + 1j
Traceback (most recent call last):
  File "...", line 31, in <module>
    print("F_3: ", robot.fkm(q, 2).translation())
RuntimeError: Tried to access link index 2 which is unnavailable.

Environment:

• OS: macOS Sonoma 14.5
• dqrobotics python version 23.4.0a22
• dqrobotics matlab version 20.04.0.1
• MATLAB version R2023b
• Python version 3.9.6

Additional context
I have also tried in a machine running Ubuntu 22.04 LTS, with the dqrobotics for Python versions 20.04 and 23.4.0a22, and obtained the same results.

[mmmarinho]

Hi @anachristinaac,

This topic has appeared in many forms along the years.

I can’t link you to all of the appearances now, but the major point is that we chose to adjust the indexing of fkm to match the language.

From your example, in MATLAB, index 0 stands for the reference frame which can otherwise be obtained. The rest are shifted by 1 to match the 0-indexing in C++ and Python.

Please let me know if this explanation does not match your experience and if there are other situations in which the indexing is causing issues so that the discussion can progress.

[anachristinaac]

Hi, @mmmarinho,

Thanks for the reply!
I might have misunderstood something, but I don't think this is what I'm experiencing.

The index 0 in MATLAB does not give me the reference frame. Considering the figure I attached before, $\mathcal{F}_0$ is the reference frame, but robot.fkm(q,0) in MATLAB gives me the pose of $\mathcal{F}_1$, which is the pose of the first frame of the chain. This is exactly the behavior I would expect, since there is no need for an index to give me the pose of the reference frame relative to itself.

In my experience, when the first and last frames are not aligned, respectively, with the reference and end effector frames (both set through set_reference_frame and set_effector methods), their poses can be obtained using MATLAB (with 0 and N indexes, where N is the number of joints) but cannot be obtained with Python. In the robot from my example, robot.fkm(q, 0) in Python gives me the pose of $\mathcal{F}_2$, robot.fkm(q, 1) the pose of the end effector, and the index N = 2 is not accepted. With that, there is no way to access $\mathcal{F}_1$ and $\mathcal{F}_3$ through fkm() in Python.

Is this what I should expect considering the choice of indexing that you mentioned?

[mmmarinho]

@anachristinaac

Thanks, I think that your explanation makes sense. Could you help by delving a bit on the implementations and see exactly what is different? If the behaviour is this different we need to adjust it.

[anachristinaac]

Hi, @mmmarinho,

I'm not sure about what is the problem, but I checked the codes, and here is what I noticed so far:

When you call fkm(q) in Python/C++, you are actually calling fkm(q, N-1), where N is the number of joints, and this will return the pose of the end effector frame. There is no way to call fkm(q,N), corresponding to the last frame of the chain (as in MATLAB) because this index is "blocked" by the method _check_to_ith_link. This method raises an error for indexes outside an interval. In Python/C++, this method only accepts indexes from 0 to N-1. In MATLAB, the same method accepts indexes from 0 to N, i.e., one more possibility than with Python/C++. Even with the different indexing in Python/C++ and MATLAB, I would expect the same number of possibilities for the parameter, i.e., an interval of the same length in both languages. The length N+1 makes sense to me, since the behaviour in MATLAB is what I believe is the correct one.

If the index N is accepted, then we could have different results when calling fkm(q) and fkm(q,N). I believe fkm(q) (without an index), should return

reference_frame_ * raw_fkm(q_vec, get_dim_configuration_space()-1) * curr_effector_

while fkm(q, N) should result in

reference_frame_ * raw_fkm(q_vec, N-1)

I would like to point here that calling the raw_fkm subtracting 1 from the given index seems correct to me, assuming that the method works according with the indexing interval of each language (0 to N-1 in Python/C++ and 1 to N in MATLAB).

However, I don't undestand entirely the implementation of the method raw_fkm() in Python/C++. A variable int j = 0 is declared, but it does not make any difference in the FOR loop that it follows. Comparing the method with its corresponding implementation in MATLAB, I thought that maybe this variable was supposed to address the indexing differences. However, calling the raw_fkm with the -1 should not be enough? If not, should j be 1 then? Or maybe change its value inside the loop?

I'm afraid I don't have answers yet, but maybe you can think of something just by looking at the implementation again with these questions in mind. I'll let you know if I have any new ideas on where the problem might be and how to solve it.

[mmmarinho]

Hi @anachristinaac

What you said makes sense but the problem we have is that fkm is used everywhere in C++ and changing its behavior won’t be trivial.

Unless you and other members/users help in a deeper level, I’m afraid I can’t address this in a timely manner.

This help is by checking the behavior, what would be affected, and also a deeper testing after the changes are made.

[bvadorno]

Hi, @anachristinaac and @mmmarinho. Thanks for this productive discussion.

@anachristinaac, we (@mmmarinho and I) reached a point where we do not have the time to spend many hours on non-critical issues in DQ Robotics (well, I reached that point many years ago, but I'm stubborn and too passionate about the library to let it go!).

To ensure that this issue is adequately addressed, I suggest you follow Murilo's suggestion, namely:

1. Investigate how to fix the problem to make the C++ version match the MATLAB version.
2. Analyze what side-effects would result from such a fix.
3. If the side effects are easily fixable with no substantial change to the library, then:
   3.1 proceed with the fix yourself by following the procedure described here.
   3.2 I'll assign @juanjqo to double-check it.
4. If Step 3 generates more problems than we'd like to address now, we'll keep this bug in our list and deal with it when other more critical issues are addressed first.

Kind regards,
Bruno

[ffasilva]

Hi @bvadorno,

Since @juanjqo is already pulling enough weight fixing the website, can we relieve him from this task and have me double-checking it instead?

I'm even currently sitting by @anachristinaac in the office, which should make the whole process easier.

Kind regards,
Frederico

[bvadorno]

Hi @ffasilva,

@juanjqo already fixed the website! 😄 Your help is appreciated, though. Therefore, I suggest you take the lead on the double-checking procedure, but I believe @juanjqo's help will still be fundamental because the process is not easy, especially with all quality-control measures in place. He already passed the trial by fire and can guide you and @anachristinaac along the way.

Kind regards,
Bruno

[anachristinaac]

Hi all,

I'll follow the suggestions, and let you know of any update on this issue.

Kind regards,
Ana