cpp-visualizer: getWorldLinkTransform returns zero position

[lrapetti]

The methods getWorldLinkTransform and getWorldModelTransform of the cpp visualizer returns zero-vector for the position of all the links.

A simple way to test it is to add the following lines to the visualization loop of the idyntree-model-view

// Visualization loop
    while( visualizer.run() )
    {
        visualizer.draw();

        //print model transform
        for (size_t i = 0; i < visualizer.modelViz("model").getLinkNames().size(); i++)
        {
            std::cerr << "link number" << i << std::endl;
            std::cerr << visualizer.modelViz("model").getWorldLinkTransform(i).toString();
        }
    }

The output is the following

$ idyntree-model-view -m model.urdf
...
link number25
-0.25 -0.0669873 0.965926
-0.258819 0.965926 2.26267e-08
-0.933013 -0.25 -0.258819
 0 0 0
link number26
-0.258819 2.26267e-08 0.965926
2.97885e-09 1 -2.26267e-08
-0.965926 -2.97885e-09 -0.258819
 0 0 0
link number27
-1 8.74228e-08 1.11022e-15
8.74228e-08 1 -8.74228e-08
-8.75296e-15 -8.74228e-08 -1
 0 0 0
link number28
-0.258819 2.26267e-08 0.965926
2.97885e-09 1 -2.26267e-08
-0.965926 -2.97885e-09 -0.258819
 0 0 0
...

[lrapetti]

@S-Dafarra did you ever noticed this problem, or do you see any possible cause?

[S-Dafarra]

@S-Dafarra did you ever noticed this problem, or do you see any possible cause?

I am able to reproduce it. Investigating.

[lrapetti]

I light of this comment #515 (comment), not sure if it is possible that I am misunderstanding the output of those methods

@traversaro @prashanthr05

[S-Dafarra]

I think there is also another issue. Both getWorldModelTransform and getWorldLinkTransform use the method irr2idyntree_trans that seem problematic. It copies directly the rotation, but that is a left handed transform, so the net result is that the rotation is transposed. I wonder whether it is indeed taking the correct column for the position. I will try to edit that method.

An additional note. The fact that both methods use the getRelativeTransform method seems correct, since the links are a child of the "world" link

idyntree/src/visualization/src/ModelVisualization.cpp

Line 102 in 2e5cf71

this->linkNodes[linkIdx] = this->m_irrSmgr->addEmptySceneNode(this->modelNode);

[traversaro]

Thanks @lrapetti and @S-Dafarra for handling this. I wanted just to add that probably a simple test for this methods (that I do not remember why it was implemented) could be to add a loop in #515 to check that the output of getWorldLinkTransform matches the one that you can compute with the KinDynComputations.

[lrapetti]

I light of this comment #515 (comment), not sure if it is possible that I am misunderstanding the output of those methods

@traversaro @prashanthr05

Looking also at the method description:

idyntree/src/visualization/include/iDynTree/Visualizer.h

Lines 590 to 595 in 2e5cf71

	/**
	* Get the transformation of given link with respect to visualizer world \f$ w_H_{link}\f$
	* The obtained transformation matrix can be used to map any homogeneous vector from the
	* given link frame to the visualizer world frame.
	*/
	virtual Transform getWorldLinkTransform(const LinkIndex& linkIndex) = 0;

this do not return the actual pose of the link, but it is the vector transformation matrix that express a vector (as homoegenous vector) to the world orientation frame. Indeed it basically represents just a rotation of reference frame.
Indeed it was somehow expected this behaviour (zero position), even if not 100% clear from the method name.

I guess I should then look at something else to get the position of the link. At a first look I didn't find any other method to get it.

[traversaro]

If that is the case, the documentation may need to be revised, as in your comment @lrapetti you wrote "to the world orientation frame", but that is not written there. Furthermore, if the function just returns a rotation, it should either return a rotation object or at least be callled getWorldLinkRotation .

[S-Dafarra]

30eddcf in https://github.com/robotology/idyntree/tree/fix/visualization/getWorldPose should fix the problem.

[S-Dafarra]

In any case, I agree that a test should be added. This was most likely broken since the beginning.

[traversaro]

If the fix is working fine we can also add the fix directly without any test, working library + test is the best, but even just working library with no test is better then not working library. : )

[lrapetti]

If the fix is working fine we can also add the fix directly without any test, working library + test is the best, but even just working library with no test is better then not working library. : )

I agree on fixing it.

Anyway, in view of #515 (comment) I am not sure if that was an error or was done on purpose. I don't see otherwise why WorldToLink was not a good candidate name.

[lrapetti]

If we fix it, I think we might also remove the latter part in the documentation of the two methods

idyntree/src/visualization/include/iDynTree/Visualizer.h

Lines 583 to 595 in 2e5cf71

	/**
	* Get the transformation of the model (root link) with respect to visualizer world \f$ w_H_{root}\f$
	* The obtained transformation matrix can be used to map any homogeneous vector from the
	* model's root link frame to the visualizer world frame.
	*/
	virtual Transform getWorldModelTransform() = 0;
	/**
	* Get the transformation of given link with respect to visualizer world \f$ w_H_{link}\f$
	* The obtained transformation matrix can be used to map any homogeneous vector from the
	* given link frame to the visualizer world frame.
	*/
	virtual Transform getWorldLinkTransform(const LinkIndex& linkIndex) = 0;

because I don't see the reason for that clarification regarding the vectors, and

* Get the transformation of given link with respect to visualizer world \f$ w_H_{link}\f$

should be enough

[prashanthr05]

If I remember well, I think I was trying to implement a visualizer process for base estimation back in time, the set and reset link colors were added to check the frames in contact. I do not remember why the getWorldLinkTransform() or getWorldModelTransform() were implemented though (My bad, sorry for not adding a relevant test.)

[S-Dafarra]

Actually, I don't know why the getWorldModel method exists. It always returns the identity.

[lrapetti]

I do not remember why the getWorldLinkTransform() or getWorldModelTransform() were implemented though (My bad, sorry for not adding a relevant test.)

So we can think the fix shouldn't brake any code.

Actually, I don't know why the getWorldModel method exists. It always returns the identity.

If that is the case, I guess it can be either changed in order to stream the base pose, or directly removed if not used anywhere.

[traversaro]

Actually, I don't know why the getWorldModel method exists. It always returns the identity.

If that is the case, I guess it can be either changed in order to stream the base pose, or directly removed if not used anywhere.

Let's not remove it altogether as it would break the ABI. Modifying it to have a sane semantics make sense to me, otherwise we can just deprecated it and remove it in iDynTree 4.0

[S-Dafarra]

Actually, I don't know why the getWorldModel method exists. It always returns the identity.

If that is the case, I guess it can be either changed in order to stream the base pose, or directly removed if not used anywhere.

Let's not remove it altogether as it would break the ABI. Modifying it to have a sane semantics make sense to me, otherwise we can just deprecated it and remove it in iDynTree 4.0

I would just deprecate it, also because it is not clear that "Model" refers to the base.

[lrapetti]

@S-Dafarra let me know if you are planning to open the PR, otherwise I can take care of it.

[S-Dafarra]

@S-Dafarra let me know if you are planning to open the PR, otherwise I can take care of it.

Feel free to push on that branch if you like 😉

[lrapetti]

PR opened: #838