Bug affecting tree structures

[Jamtalhutte]

Not sure whether this code is maintained, but for anyone who might find this useful:

I believe there is an error in the velocity/twist computation at line 60 of (src/kinematics_dynamics/Velocities.m). The line is:

tL(1:6,i)=Bij(1:6,1:6,i,i-1)*tL(1:6,i-1);

But should be something like:

tL(1:6,i)=Bij(1:6,1:6,i,robot.joints(i).parent_link)*tL(1:6,robot.joints(i).parent_link);

The issue with the existing line is that it assumes the parent of "i" is always "i-1". This won't cause any issue if your robot is a single serial chain or a tree where the only branching occurs at the base so this line isn't hit. BUT if you have some other branching (say bodies 2 and 3 both have body 1 as their parent), then the twist will be wrong (for body 3 you would be looking at the twist propagation from body 2 which is on a different branch, when you instead need the twist propagation form its parent, body 1)

All of the provided test cases conveniently satisfy the conditions to not be affected by the error (either single arms or dual arms with each connected to the base), hence why no errors are thrown.

If anyone disagrees with this, or finds this useful lmk.