Fixed old T_kinematics_to_planning transformation bug. Extended the test.