amigo_kinematics_constraint_aware