scaling meshes makes fk output the wrong poses

[GondotCl]

Behavior

When running

robot = load("f.urdf")
fk = robot.visual_trimesh_fk()
for tm in fk:
    pose = fk[tm]
    print(pose)

With this line in the URDF :

<mesh filename="meshes\0784-2_ife-environnement.dae" scale="0.001"/>

The output is

[[0.001 0.001 0.001 0.   ]
 [0.001 0.001 0.001 0.   ]
 [0.001 0.001 0.001 0.   ]
 [0.    0.    0.    1.   ]]

When it should be

[[1.    0.    0.    0.   ]
 [0.    1.    0.    0.   ]
 [0.    0.    1.    0.   ]
 [0.    0.    0.    1.   ]]

or at least

[[0.001 0.    0.    0.   ]
 [0.    0.001 0.    0.   ]
 [0.    0.    0.001 0.   ]
 [0.    0.    0.    1.   ]]

Depending on what the scale means.

Probable cause

In the function visual_trimesh_fk :

if visual.geometry.mesh.scale is not None:
    S = np.eye(4, dtype=np.float64)
    S[:3,:3] = np.diag(visual.geometry.mesh.scale)
    pose = pose.dot(S)

You mixed the dilatation and rotation operation.
You could replace S by something like that :

S = np.diag(3*[visual.geometry.mesh.scale[0]]+[1])

Note

I use Python3.10 an your fix descrided in the issue #4