Added nullspace resolution and modified test_greedy_cartesian_planner to always start from its current configuration.