Robot TCP Velocity calculation
Summary
Creating a manual test case with the panda robot I found that the TCP velocities calculated are not near what expected (> 50% off).
Steps to reproduce
from timor import Robot
import numpy as np
from timor.utilities.file_locations import robots
import pinocchio as pin
robot = Robot.PinRobot.from_urdf(robots['panda'].joinpath('urdf').joinpath('panda.urdf'), robots['panda'].parent)
q = np.zeros(7,)
dq = np.asarray((0., 1., 0., 0., 0., 0., 0.))
robot.update_configuration(q, dq)
robot.tcp_velocity # [-3.33000000e-01, 6.16297582e-33, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00, 2.22044605e-16] - seems off
pin.getFrameVelocity(robot.model, robot.data, robot.tcp) # [ 5.93000000e-01, -5.86197757e-17, 8.80000000e-02, 0.00000000e+00, -1.00000000e+00, -6.66133815e-16] - pretty exectly result of manual calculation
What is the current bug behavior?
(What actually happens)
What is the expected correct behavior?
(What you should see instead)
Relevant logs and/or screenshots
(Paste any relevant logs - please use code blocks (```) to format console output, logs, and code, as it's very hard to read otherwise.)
Possible fixes
(If you can, link to the line of code that might be responsible for the problem)
/cc @JonathanKuelz
Edited by Matthias Mayer