Memory issue in getWorldTransform(frame_name).getPosition().toNumPy() #1263
Description
I am using the idyntree python bindings.
I have created a conda environment as:
conda install python idyntree=13.4.2 yarp ergocub-software numpy
Now look at the following script to reproduce the error:
import numpy as np
import os
import yarp
import idyntree.bindings as idyntree
if __name__ == "__main__":
# urdf file
os.environ["YARP_ROBOT_NAME"] = "ergoCubSN001"
urdf_file = yarp.ResourceFinder().findFileByName("model.urdf")
joints_list = [
"torso_pitch",
"torso_roll",
"torso_yaw",
"l_shoulder_pitch",
"l_shoulder_roll",
"l_shoulder_yaw",
"l_elbow",
"r_shoulder_pitch",
"r_shoulder_roll",
"r_shoulder_yaw",
"r_elbow",
"l_hip_pitch",
"l_hip_roll",
"l_hip_yaw",
"l_knee",
"l_ankle_pitch",
"l_ankle_roll",
"r_hip_pitch",
"r_hip_roll",
"r_hip_yaw",
"r_knee",
"r_ankle_pitch",
"r_ankle_roll",
]
# load model and create kindyn object
mdl_loader = idyntree.ModelLoader()
if not mdl_loader.loadReducedModelFromFile(urdf_file, joints_list):
raise RuntimeError(f"Could not load the model {urdf_file}")
model = mdl_loader.model()
kindyn = idyntree.KinDynComputations()
kindyn.loadRobotModel(model)
# set state
kindyn.setRobotState(
idyntree.Transform(
idyntree.Rotation.RPY(
0,
0,
0,
),
idyntree.Position.FromPython(np.array([0, 0, 0.75])),
),
np.zeros(len(joints_list)),
np.zeros(6),
np.zeros(len(joints_list)),
np.array([0, 0, -9.81]),
)
left_foot_frame = "l_sole"
w_H_lf = kindyn.getWorldTransform(left_foot_frame)
w_R_lf = (
kindyn.getWorldTransform(left_foot_frame).getRotation().toNumPy()
)
w_p_lf = (
kindyn.getWorldTransform(left_foot_frame).getPosition().toNumPy()
)
assert np.allclose(w_H_lf.getRotation().toNumPy(), w_R_lf)
assert np.allclose(w_H_lf.getPosition().toNumPy(), w_p_lf)
When running it, the second assert is failing.
The issue seems to be in kindyn.getWorldTransform(left_foot_frame).getPosition().toNumPy().
In fact, when I run the script multiple times with some printings I get:
w_H_lf.getPosition().toNumPy(): [ 0.04403 0.0744 -0.0293 ]
kindyn.getWorldTransform(left_foot_frame).getPosition().toNumPy(): [ 1.17600173e-313 1.35510088e+121 -2.93000000e-002]
w_H_lf.getPosition().toNumPy(): [ 0.04403 0.0744 -0.0293 ]
kindyn.getWorldTransform(left_foot_frame).getPosition().toNumPy(): [ 1.20278189e-313 -2.54762491e+099 -2.93000000e-002]
w_H_lf.getPosition().toNumPy(): [ 0.04403 0.0744 -0.0293 ]
kindyn.getWorldTransform(left_foot_frame).getPosition().toNumPy(): [ 1.22198293e-313 9.13798852e-167 -2.93000000e-002]
it seems there is some memory issue when using kindyn.getWorldTransform(left_foot_frame).getPosition().toNumPy().