add kinematics constraint aware