Question about the calculation of joint local velocity(lvrt) #55
Description
Hello, I am confused about the calculation of joint local velocity(lvrt) when reading code:
lvrt = np.zeros_like(lpos)
lvrt[1:] = quat.to_helical(quat.abs(quat.mul(lrot[1:], quat.inv(lrot[:-1])))) / dt
lvrt[0] = lvrt[1] - (lvrt[3] - lvrt[2])
But in my understanding, something should like this (used in the fk_vel function):
lvrt[1:] = quat.to_helical(quat.abs(quat.inv(quat.mul(lrot[1:]), (lrot[:-1])))) / dt
Am I right? Thanks for your reply.