How does Forward kinematik work in Robotics System Toolbox?

How does the Robot System Toolbox work internally?

I noticed some small offset in the results of the Forward Kinematics if I compare the result of getTransform with the plain geometric Forward Kinematics calculated with the parameters from the Datasheet (p. 61).

The offset also seems to be dynamic. So I wonder what the reason could be.

Is there some kind of dynamic simulation of the joints stiffnes?

Attached is some code to reproduce this:

% Compare DH forward Kinematics with Matlab Model of KinGen3

%% get robot model

close all

gen3 = loadrobot("kinovaGen3");

gen3.DataFormat = 'column';

eeName = 'EndEffector_Link';

% define q:

q=[ 1.18 -68.68 18.47 -69.09 94.36 112.93 46.00]';

%q=[ 1.18+180 -68.68 18.47 -69.09 94.36 112.93 46.00]';

%q=[0 0 0 0 0 0 0]';

%% calculate FK with DH and with Matlab modell:

% DH

H_DH = getSingleH(getHcomplete(q),0,8)

pos_DH = H_DH(1:3,4);

% Model

H_mod = getTransform(gen3, q/180*pi', eeName)

pos_mod=H_mod(1:3,4);

%calculate difference:

pos_dif=(pos_DH-pos_mod)*1000

%% Funktiondecalrations for forward kinematics

function H = getHcomplete(q)

% calculates H cell according to input angles

% wheras H{1}=H01, H{2}=H12, H{3}=H23, etc...

i=1;

R{i}=rotx(180)*rotz(q(i)); % Rotation

D{i}=[0 0 +156.4]'/1000; % Displacement

H{i}=RnD2H(R{i},D{i});

i=2;

R{i}=rotx(+90)*rotz(q(i));

D{i}=[0 5.4 -128.4]'/1000;

H{i}=RnD2H(R{i},D{i});

i=3;

R{i}=rotx(-90)*rotz(q(i));

D{i}=[0 -210.4 -6.4]'/1000;

H{i}=RnD2H(R{i},D{i});

i=4;

R{i}=rotx(+90)*rotz(q(i));

D{i}=[0 6.4 -210.4]'/1000;

H{i}=RnD2H(R{i},D{i});

i=5;

R{i}=rotx(-90)*rotz(q(i));

D{i}=[0 -208.4 -6.4]'/1000;

H{i}=RnD2H(R{i},D{i});

i=6;

R{i}=rotx(+90)*rotz(q(i));

D{i}=[0 0 -105.9]'/1000;

H{i}=RnD2H(R{i},D{i});

i=7;

R{i}=rotx(-90)*rotz(q(i));

D{i}=[0 -105.9 0]'/1000;

H{i}=RnD2H(R{i},D{i});

i=8;

R{i}=rotx(180)*rotz(0);

D{i}=[0 0 -61.5]'/1000;

H{i}=RnD2H(R{i},D{i});

end

function H = RnD2H(R,D)

% combines Rotation and Displacement into homogenous transform

H = horzcat(R,D);

H = vertcat(H,[0 0 0 1]);

end

function H = getSingleH(h,von,bis)

% returns homogenous transfrormation from to defined frames

H=eye(4);

for i=von+(bis-von>0):sign(bis-von):bis+(von-bis>0)

H=H*h{i}^sign(bis-von);

end

end

[Umar]

Hi @Peter Abt ,

When you loaded a robot model using `loadrobot`, it created a `rigidBodyTree` object which encapsulated the kinematic and dynamic properties of the robot which includes predefined joint limits, gravity and other characteristics that are not present when you manually defined the robot's kinematics using DH parameters. Also, your DH parameters are defined based on the robot's geometry and do not change with or without loading robot.

After going through loadrobot function defined in link below

https://www.mathworks.com/help/robotics/ref/loadrobot.html

I found out how gravity was defined in rigidBodyTree with properties of your robot, and it indicated the absence of gravity in your model ([0 0 0]) which implied that the calculations were performed in a zero-gravity environment, which really simplifies the kinematic analysis but may not reflect real-world conditions.

%% get robot model

close all

gen3 = loadrobot("kinovaGen3");

disp(gen3);

rigidBodyTree with properties:

NumBodies: 8

Bodies: {1x8 cell}

Base: [1x1 rigidBody]

BodyNames: {1x8 cell}

BaseName: 'base_link'

Gravity: [0 0 0]

DataFormat: 'struct'

Also, it show that dynamic effects are not being considered during calculations when the robot is loaded. So, based on attached results after this time experimenting with your code, it provided the following matrices as follows:

DH Matrix ( H_{DH} )

    0.9806   -0.0749    0.1814   -0.5673
    0.1771   -0.0603   -0.9823   -0.0830
    0.0845    0.9954   -0.0459    0.2077
         0         0         0    1.0000

Model Matrix ( H_{mod} )

    0.9806   -0.0749    0.1814   -0.5674
    0.1771   -0.0603   -0.9823   -0.0810
    0.0845    0.9954   -0.0459    0.2075
         0         0         0    1.0000

Here is my analysis of the differences mentioned below.

As you will notice that the first three columns of both matrices are identical, indicating that the rotation components are consistent across both methods. This suggests that the orientation of the end effector is accurately represented in both cases. However, the discrepancies found in the last column of the matrices shows H_DH has a value of (-0.5673) and (-0.0830) in the last column while H_mod has values of (-0.5674) and (-0.0810) which you noted and shared with us. After careful analysis, these differences, while seemingly minor, can have significant implications in applications requiring high precision, such as robotic manipulation tasks. Also, this shows that robot's physical constraints might be introducing slight offsets and the `loadrobot` function incorporates additional details about the robot’s geometry that are not captured in your manual DH parameter definitions. So, at this point, to further investigate these discrepancies, try modifying the gravity settings in your `rigidBodyTree` and observe how it affects your forward kinematics results and consider importing a different robot model using URDF or SDF formats to see if this yields different results compared to manually defined DH parameters. Also, utilizing visualization tools within MATLAB to compare positions graphically. This could provide intuitive insights into where and why offsets occur. By understanding these internal workings and exploring the various factors influencing kinematic computations, you can achieve more accurate results aligned with real-world scenarios in robotic applications.

I forgot to address your query about dynamic simulations of joint stiffness since you raised a question about it. While the toolbox does allow for dynamic modeling (e.g., simulating forces due to gravity or inertia), if your model is set to zero gravity, these effects won't be accounted for. If you want to analyze dynamics, consider enabling gravity and observing how it affects your calculations. So, I will suggest import your own robot model to see compare the results by using importrobot function in code below.

https://www.mathworks.com/help/robotics/ref/importrobot.html

I would like @Garmit Pant to share his opinion on this as well. Hope this helps. Please let me know if you have any further questions.

[Umar]

Hi @Peter Abt,

Forgot to attached plots of visualization of robot arm and experimentation with your code.

Hope, these plots will help you visualize the results better. Like I mentioned in my comments, “utilizing visualization tools within MATLAB to compare positions graphically” to help resolve issues especially when dealing with degree of freedom.

Answer 1

[Garmit Pant]

Hello Peter,

The potential reasons behind the offsets between the transforms calculated using your custom function and those calculated using the “getTransform” method can be attributed to the modeling of the rigid-body system in MATLAB.

In MATLAB, forward kinematics are calculated based on the geometric model of a rigid body, which describes the relative position of each joint on each rigid body. Using this system, “getTransform” can return the required transformation matrix by utilizing the transforms of each body relative to the base frame. Since the transformation matrices of both the ‘sourceBody’ and the ‘targetBody’ relative to the base frame are available, the required transformation matrix can be calculated by multiplying the inverse of the target transformation matrix relative to the base frame with the source transformation matrix relative to the base frame.

Your method, on the other hand, uses Denavit-Hartenberg (DH) Parameters to locate the body coordinate frames and subsequently calculate the transformation matrices.

The difference in these methods can be a reason for the minor offsets observed. Additionally, precision and angle conversions can also lead to discrepancies. MATLAB may be internally performing conversions between different positional representations, such as axis-angle rotation representation to homogeneous transform. During these conversions, small offsets can occur.

For further understanding, kindly refer to the following resources:

• Refer to Sections 4.2 and 4.3, which explain the differences between different modeling methods for rigid bodies and demonstrate algorithms to find transforms for the bodies of a system: Rigid Body Dynamics Algorithms

I hope you find the above explanation and suggestions useful!

[Peter Abt]

I do indeed find your explanation and suggestions very helpfull.

Thank you!

Answer 2

[Umar]

Hi @Peter Abt ,

Addressing your query regarding, “How does the Robot System Toolbox work internally? “

Begin with the “User’s Guide” for a holistic understanding followed by the “Reference” document for deeper insights into specific functions by clicking the link below. This approach will provide both a broad overview and detailed technical information necessary for grasping how the Robot System Toolbox operates internally in MATLAB.

https://www.mathworks.com/help/pdf_doc/robotics/index.html

Now, coming back to, “I noticed some small offset in the results of the Forward Kinematics if I compare the result of getTransform with the plain geometric Forward Kinematics calculated with the parameters from the Datasheet (p. 61). The offset also seems to be dynamic. So I wonder what the reason could be. Is there some kind of dynamic simulation of the joints stiffnes?”

The getTransform function expects the joint angles in radians, while the code snippet uses a division by 180 and multiplication by π to convert degrees to radians. However, the conversion is applied incorrectly in the context of the getTransform function call.

In the line:

H_mod = getTransform(gen3, q/180*pi', eeName)

The joint angles are being transposed ('), which is unnecessary and may lead to incorrect dimensions. The correct approach should be:

H_mod = getTransform(gen3, q * (pi / 180), eeName)

This change will make sure that the joint angles are correctly interpreted by the getTransform function, potentially eliminating the observed offset. Hope this helps. Please let me know if you have any further questions.

[Peter Abt]

Thanks for the link to the Reference Document and good call on spotting the unessesacy transposed operation.

However your proposed change does not alter the results.

It gave me the idea to add some Gravity to the model when loading it.

However this also has no effect on the output.

So I am still wondering what the origin off that offset is.

[Garmit Pant]

Hi Umar

The conversion of the angles from degrees to radians has been correctly implemented in the code snippet.

Additonally, the correction suggested by you has no affect on the calculated configuration as is demonstrated below.

q=[  1.18  -68.68 18.47  -69.09   94.36  112.93  46.00]';
q/180*pi'

ans = 7x1

0.0206 -1.1987 0.3224 -1.2058 1.6469 1.9710 0.8029

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

q * (pi / 180)

ans = 7x1

0.0206 -1.1987 0.3224 -1.2058 1.6469 1.9710 0.8029

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

The joint angles are not being transposed since the transpose operator (') has a higher precedence than the matrix operations being performed in the expression. The transpose operator is only acting on 'pi'. The code snippet below demonstrates the effect of different precedences on the same expression:

(q/180*pi)'

ans = 1x7

0.0206 -1.1987 0.3224 -1.2058 1.6469 1.9710 0.8029

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

(q/180)*pi'

ans = 7x1

0.0206 -1.1987 0.3224 -1.2058 1.6469 1.9710 0.8029

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

Please verify the suggested changes.

[Rik]

(Note: I have only read the flags and the flaged contributions. As far as I'm concerned the response below holds for all users of this forum.)

It is good practice to verify your suggestions work. If you use the desktop version of the website you can easily do this. If you use a mobile version of the site, you need to be careful with definitive statements.

Personally I think you should let your contributions speak for themselves (i.e. show why they do indeed work, preferably with the 'run' feature) instead of flagging (I fail to see why religion should ever be mentioned and questioning the quality/validity of a post is not a personal attack on its writer). Nobody is perfect. If you made a mistake, that is fine. If someone incorrectly tells you you've made a mistake, that is also fine. Either ignore the post, or explain/show why what you posted is indeed correct. (and if you did indeed made a mistake, thank them for the correction and try to learn from your mistake)

As to the assertion some posts were AI generated: the current stance on this forum is that such content is fine as long as it is attributed and tested. Low quality AI written posts are dealt with the same way as low quality human generated posts: utter garbage is deleted as spam, a decent attempt should be corrected/explained in a comment.

I think you have both made your point, so I will remove the flags. I will not respond on this thread. While I am an editor, I'm just a volunteer. If you disagree with me, feel free to contact John Kelly or another Mathworks employee.

Answer 3

[Umar]

Hi @Peter Abt ,

When you loaded a robot model using `loadrobot`, it created a `rigidBodyTree` object which encapsulated the kinematic and dynamic properties of the robot which includes predefined joint limits, gravity and other characteristics that are not present when you manually defined the robot's kinematics using DH parameters. Also, your DH parameters are defined based on the robot's geometry and do not change with or without loading robot.

After going through loadrobot function defined in link below

https://www.mathworks.com/help/robotics/ref/loadrobot.html

I found out how gravity was defined in rigidBodyTree with properties of your robot, and it indicated the absence of gravity in your model ([0 0 0]) which implied that the calculations were performed in a zero-gravity environment, which really simplifies the kinematic analysis but may not reflect real-world conditions.

%% get robot model

close all

gen3 = loadrobot("kinovaGen3");

disp(gen3);

rigidBodyTree with properties:

NumBodies: 8

Bodies: {1x8 cell}

Base: [1x1 rigidBody]

BodyNames: {1x8 cell}

BaseName: 'base_link'

Gravity: [0 0 0]

DataFormat: 'struct'

Also, it show that dynamic effects are not being considered during calculations when the robot is loaded. So, based on attached results after this time experimenting with your code, it provided the following matrices as follows:

DH Matrix ( H_{DH} )

    0.9806   -0.0749    0.1814   -0.5673
    0.1771   -0.0603   -0.9823   -0.0830
    0.0845    0.9954   -0.0459    0.2077
         0         0         0    1.0000

Model Matrix ( H_{mod} )

    0.9806   -0.0749    0.1814   -0.5674
    0.1771   -0.0603   -0.9823   -0.0810
    0.0845    0.9954   -0.0459    0.2075
         0         0         0    1.0000

Here is my analysis of the differences mentioned below.

As you will notice that the first three columns of both matrices are identical, indicating that the rotation components are consistent across both methods. This suggests that the orientation of the end effector is accurately represented in both cases. However, the discrepancies found in the last column of the matrices shows H_DH has a value of (-0.5673) and (-0.0830) in the last column while H_mod has values of (-0.5674) and (-0.0810) which you noted and shared with us. After careful analysis, these differences, while seemingly minor, can have significant implications in applications requiring high precision, such as robotic manipulation tasks. Also, this shows that robot's physical constraints might be introducing slight offsets and the `loadrobot` function incorporates additional details about the robot’s geometry that are not captured in your manual DH parameter definitions. So, at this point, to further investigate these discrepancies, try modifying the gravity settings in your `rigidBodyTree` and observe how it affects your forward kinematics results and consider importing a different robot model using URDF or SDF formats to see if this yields different results compared to manually defined DH parameters. Also, utilizing visualization tools within MATLAB to compare positions graphically. This could provide intuitive insights into where and why offsets occur. By understanding these internal workings and exploring the various factors influencing kinematic computations, you can achieve more accurate results aligned with real-world scenarios in robotic applications.

I forgot to address your query about dynamic simulations of joint stiffness since you raised a question about it. While the toolbox does allow for dynamic modeling (e.g., simulating forces due to gravity or inertia), if your model is set to zero gravity, these effects won't be accounted for. If you want to analyze dynamics, consider enabling gravity and observing how it affects your calculations. So, I will suggest import your own robot model to see compare the results by using importrobot function in code below.

https://www.mathworks.com/help/robotics/ref/importrobot.html

I would like @Garmit Pant to share his opinion on this as well. Hope this helps. Please let me know if you have any further questions.