diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index 7523e30..47c823e 100644 --- a/include/custom_robot.hpp +++ b/include/custom_robot.hpp @@ -47,11 +47,11 @@ public: bool processRechargeCmd(const std::string& cmd, const nlohmann::json& message); void printServiceList(const std::vector& serviceList, int filterStatus = -1); - void trajControl(); bool startTraj(); void stopTraj(); private: + void trajControl(); std::string generateRandomClientId() const; std::unique_ptr controller_; // std::unique_ptr low_controller_; @@ -65,6 +65,8 @@ private: std::atomic initialized_; unitree::common::ThreadPtr traj_th_; std::atomic traj_running_; + float traj_dt_; + float traj_count_; }; } // namespace custom diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index 3026849..09be2ce 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -14,96 +14,16 @@ namespace custom { -std::vector createExampleTrajectory(const std::string& type) { - std::vector path; - const int num_points = 30; - const float total_time = 5.0f; // seconds - - if (type == "line") { - // Move forward 1 meter in a straight line - for (int i = 0; i < num_points; ++i) { - unitree::robot::go2::PathPoint p; - float t = (static_cast(i) / (num_points - 1)) * total_time; - p.timeFromStart = t; - p.x = 1.0f * (t / total_time); // x from 0 to 1 - p.y = 0.0f; - p.yaw = 0.0f; - p.vx = 1.0f / total_time; // constant velocity - p.vy = 0.0f; - p.vyaw = 0.0f; - path.push_back(p); - } - } else if (type == "square") { - // Trace a 1x1 meter square - const int points_per_side = num_points / 4; - float side_time = total_time / 4.0f; - float speed = 1.0f / side_time; - - // Side 1: move in +x - for (int i = 0; i < points_per_side; ++i) { - unitree::robot::go2::PathPoint p; - float t = (static_cast(i) / (points_per_side -1)) * side_time; - p.timeFromStart = t; - p.x = speed * t; p.y = 0; p.yaw = 0; - p.vx = speed; p.vy = 0; p.vyaw = 0; - path.push_back(p); - } - // Side 2: move in +y - for (int i = 0; i < points_per_side; ++i) { - unitree::robot::go2::PathPoint p; - float t = (static_cast(i) / (points_per_side - 1)) * side_time; - p.timeFromStart = side_time + t; - p.x = 1.0; p.y = speed * t; p.yaw = M_PI / 2.0f; - p.vx = 0; p.vy = speed; p.vyaw = 0; - path.push_back(p); - } - // Side 3: move in -x - for (int i = 0; i < points_per_side; ++i) { - unitree::robot::go2::PathPoint p; - float t = (static_cast(i) / (points_per_side - 1)) * side_time; - p.timeFromStart = 2 * side_time + t; - p.x = 1.0 - speed * t; p.y = 1.0; p.yaw = M_PI; - p.vx = -speed; p.vy = 0; p.vyaw = 0; - path.push_back(p); - } - // Side 4: move in -y - for (int i = 0; i < points_per_side; ++i) { - unitree::robot::go2::PathPoint p; - float t = (static_cast(i) / (points_per_side - 1)) * side_time; - p.timeFromStart = 3 * side_time + t; - p.x = 0; p.y = 1.0 - speed * t; p.yaw = -M_PI / 2.0f; - p.vx = 0; p.vy = -speed; p.vyaw = 0; - path.push_back(p); - } - } else if (type == "circle") { - // Move in a circle with radius 0.5m - float radius = 0.5f; - for (int i = 0; i < num_points; ++i) { - unitree::robot::go2::PathPoint p; - float t = (static_cast(i) / (num_points - 1)) * total_time; - float angle = 2.0f * M_PI * (t / total_time); - p.timeFromStart = t; - p.x = radius * std::cos(angle); - p.y = radius * std::sin(angle); - p.yaw = angle + M_PI / 2.0f; - p.vx = -radius * (2.0f * M_PI / total_time) * std::sin(angle); - p.vy = radius * (2.0f * M_PI / total_time) * std::cos(angle); - p.vyaw = 2.0f * M_PI / total_time; - path.push_back(p); - } - } - - return path; -} - CustomRobot::CustomRobot() : controller_(nullptr) , navigation_(nullptr) , rsc_(nullptr) , mqttClient_(nullptr) , running_(false) - , initialized_(false) - , traj_running_(false) { + , initialized_(false) + , traj_running_(false) + , traj_dt_(0.02f) + , traj_count_(0.0f) { config_.loadDefaults(); try { @@ -234,24 +154,20 @@ void CustomRobot::trajControl() return; } - const float dt = 0.02f; // Control frequency 50Hz - const float delta = 0.06f; // Time between path points - static float count = 0; - count += dt; - + float vx = 0.3f; + float delta = 0.06f; + traj_count_ += traj_dt_; std::vector path; for (int i=0; i<30; i++) { unitree::robot::go2::PathPoint p; - float var = (count + i * delta); - float vx = 0.3f; // Example constant velocity - + float var = (traj_count_ + i * delta); p.timeFromStart = i * delta; p.x = vx * var; - p.y = 0.6f * sin(M_PI * vx * var); - p.yaw = 2 * 0.6f * vx * M_PI * cos(M_PI * vx * var); + p.y = 0.6 * sin(M_PI * vx * var); + p.yaw = 2 * 0.6 * vx * M_PI * cos(M_PI * vx * var); p.vx = vx; - p.vy = M_PI * vx * (0.6f * cos(M_PI * vx * var)); - p.vyaw = -M_PI * vx * 2 * 0.6f * vx * M_PI * sin(M_PI * vx * var); + p.vy = M_PI * vx * (0.6 * cos(M_PI * vx * var)); + p.vyaw = - M_PI * vx * 2 * 0.6 * vx * M_PI * sin(M_PI * vx * var); path.push_back(p); } @@ -269,9 +185,9 @@ bool CustomRobot::startTraj() LOG_INFO("Starting dynamic trajectory control..."); traj_running_ = true; + traj_count_ = 0.0f; - const float dt = 0.02f; // 50Hz - traj_th_ = unitree::common::CreateRecurrentThread(dt * 1000000, std::bind(&CustomRobot::trajControl, this)); + traj_th_ = unitree::common::CreateRecurrentThread(static_cast(traj_dt_ * 1000000), std::bind(&CustomRobot::trajControl, this)); return true; } @@ -832,18 +748,6 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json& return controller_->WalkUpright(message["param"]["flag"]); } else if (cmd == "CrossStep") { return controller_->CrossStep(message["param"]["flag"]); - } else if (cmd == "TrajectoryFollow") { - if (!message.contains("param") || !message["param"].contains("path_type")) { - LOG_ERROR("TrajectoryFollow cmd missing 'path_type' parameter"); - return false; - } - std::string path_type = message["param"]["path_type"]; - auto path = createExampleTrajectory(path_type); - if (path.empty()) { - LOG_ERROR("Unknown or empty path_type for TrajectoryFollow: " + path_type); - return false; - } - return controller_->TrajectoryFollow(path); } else if (cmd == "start_traj") { return startTraj(); } else if (cmd == "stop_traj") {