Issue when getting joint and cartesian positions

[Mathilde-LL]

Hi @mhubii,
after doing several tests and debugging with the pyfri examples (with LBR Med7, FRI 2.7), it appears that all functions asking for joint and cartesian position data (getIpoJointPosition, getMeasuredJointPosition, getMeasuredCartesianPose,...) are not working properly for me. In all the examples.
For the joints, I have always approx. 0 whatever the robot configuration and for the cartesian pose, I have the same number for the 7 dimensions of the vector (3D position and quaternions are all the same).
The communication per se is fine since the getTimestampSec function works properly. The problem must come from python (not Sunrise or the FRI configuration) since everything works well when I run the same C++ code with your LBRServer sunrise app. Did this issue appear somehow for someone else? Would you have any idea of how to solve it?
Thanks in advance,
Best,
Mathilde

[mhubii]

What FRI version is running on the robot?

Maybe @cmower any idea?

Please be aware the this repo does not support FRI 2.x and 3.x (2.7) API yet. Did you try add the functionality?

[cmower]

hi, sorry, last time i worked on this repo was ~Aug 2023 😆

If I understand correctly, @Mathilde-LL, you see 0.'s on all output from the functions like getIpoJointPosition. My guess is that the data retrieved from the C++-side is being incorrectly copied/returned to python - ie somewhere in the following for example

pyfri/pyfri/src/wrapper.cpp

Lines 376 to 384 in 302317a

	// Retrieve state
	memcpy(data, self.getIpoJointPosition(),
	KUKA::FRI::LBRState::NUMBER_OF_JOINTS * sizeof(double));
	// Parse: double -> float
	for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++)
	dataf[i] = (float)data[i];
	return py::array_t<float>({KUKA::FRI::LBRState::NUMBER_OF_JOINTS},

it is possible KUKA changed the message types and/or deserialization to use a different convention to previous FRI versions, and then the memory being copied is somehow wrong

eg maybe they updated the protobuf message types - this could potentially affect how the data is retrieved from packets sent over FRI

[mhubii]

there is another library: https://github.com/lbr-stack/pylbr/tree/dev-beta

just open-sourced this now (never had time to pursue), maybe you can get some inspiration. It is an alternative approach. It provides bindings at a higher level. It should circumvent any potential C++ / Python data transfer issues, but also provides no low level access from Python.

I'd use this code with care though, but maybe you can implement such a concept! It is currently using FRI 1.15 as submodule.