Debugged the planner. Introduced manipulation with both arms. Working version for camera looking down. Does not have push action yet.