added planning modes, cleaned up code

fixed bugs related to when using the left arm as primary arm
split up code in path reconstruction
added the ability to specify which planning mode you want (though mprim removal
isn't in yet)