refactor(controller): Update TrajectoryFollow method signature to accept non-const reference

- Modified the TrajectoryFollow method in both the header and implementation files to accept a non-const reference to a vector of unitree::robot::go2::PathPoint, allowing for in-place modifications of the path.

include/controller.hpp

@@ -61,7 +61,7 @@ public:
     bool ClassicWalk(bool flag);
     bool WalkUpright(bool flag);
     bool CrossStep(bool flag);
-    bool TrajectoryFollow(const std::vector<unitree::robot::go2::PathPoint>& path);
+    bool TrajectoryFollow(std::vector<unitree::robot::go2::PathPoint>& path);
     
     // Obstacle 
     bool SwitchSet(bool enable);

src/controller.cpp

@@ -339,7 +339,7 @@ bool Controller::CrossStep(bool flag) {
     });
 }
 
-bool Controller::TrajectoryFollow(const std::vector<unitree::robot::go2::PathPoint>& path) {
+bool Controller::TrajectoryFollow(std::vector<unitree::robot::go2::PathPoint>& path) {
     return ExecuteSportCmd([&path](auto* sc) {
         return sc->TrajectoryFollow(path);
     });