Fix kinematic chain
Add change ref point in test