Update RigidBodyPlanner.cpp

-ConfigurationMove now takes index of a sub-robot as parameter