Move the commits from drc sandbox to drake. Almost finished the P step of the bilinear alternation