Add forward kinematics start code