bug fixed: index out of range
bug fixed: kinematic offset