Use HandPositionCommandExecutor to find end configuration

[gilwoolee]

The current hand executor creates a trajectory to the given end configuration without collision checking. Use simulated hand position commend executors to find the end configuration immediately before collision and use it to create the hand trajectory.