changed code for inverting M when calculating the direction to pinvM as this may help with initialization issues when decreasing and having multiple constraints