implement the precise position constraint using Rotation matrix in MW.
implemented the precise position constratin using Jacobian method using AD.