Refactor V-REP plan_motion

[beverlylytle]

Feature Request

As a user of the V-REP backend, I want plan_motion to have the argument goal_constraints to be of type list of Constraints rather than Frame so that the world makes sense again.

Details

goal_constraints should be a list of Constraints and positional and orientation values should be checked to rebuild the frame. Further one should merge plan_motion with plan_motion_to_config with this change.