Update RigidBodyPlanner.cpp