function equalityConstraint input Orientation R1; input Orientation R2; output Real residue[3]; end equalityConstraint;