tanjianbin/lzwc-terminal-unitreeGo2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(controller): Update TrajectoryFollow method signature and improve path handling

Browse Source 
- Changed the signature of TrajectoryFollow to accept a vector of unitree::robot::go2::PathPoint instead of std::array<float, 6>.
- Updated the implementation of TrajectoryFollow in controller.cpp to utilize the new path type.
- Removed unnecessary status message in CMakeLists.txt related to unitree_sdk2 detection.
- Added a new function in custom_robot.cpp to create example trajectories for different path types (line, square, circle) and integrated it into the command processing for TrajectoryFollow.
This commit is contained in:
Sucan126
2025-09-23 18:40:42 +08:00
parent e0e1b5f642
commit 6c8a4a3b38
4 changed files with 99 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/controller.cpp
View File

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

96
src/custom_robot.cpp
View File

@@ -8,10 +8,94 @@
#include <iomanip>
#include <nlohmann/json.hpp>
#include <unitree/robot/channel/channel_factory.hpp>
#include <unitree/robot/go2/sport/sport_client.hpp>
#include "recharge.hpp"
#include <cmath>
namespace custom {
std::vector<unitree::robot::go2::PathPoint> createExampleTrajectory(const std::string& type) {
std::vector<unitree::robot::go2::PathPoint> 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<float>(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<float>(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<float>(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<float>(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<float>(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<float>(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)
@@ -700,7 +784,17 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json&
} else if (cmd == "CrossStep") {
return controller_->CrossStep(message["param"]["flag"]);
} else if (cmd == "TrajectoryFollow") {
return controller_->TrajectoryFollow(message["param"]["path"]);
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 == "FootRaiseHeight") {
return controller_->FootRaiseHeight(message["param"]["height"]);
}