function equalityConstraint input ReferenceAngle[2] theta_p; input ReferenceAngle[2] theta_n; output Real[0] residue; end equalityConstraint;