Skip to content

Commit

Permalink
update h1 high level api for ver. 4.0.0.0
Browse files Browse the repository at this point in the history
  • Loading branch information
@cyoahs
cyoahs committed Jan 15, 2025
1 parent 34d3444 commit 197b720
Show file tree
Hide file tree
Showing 4 changed files with 191 additions and 53 deletions.
68 changes: 57 additions & 11 deletions example/h1/high_level/h1_loco_client_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,17 +45,16 @@ int main(int argc, char const *argv[]) {
}
}

unitree::robot::ChannelFactory::Instance()->Init(0,
args["network_interface"]);
unitree::robot::ChannelFactory::Instance()->Init(0, args["network_interface"]);

unitree::robot::h1::LocoClient client;

client.Init();
client.SetTimeout(10.f);

for (const auto &arg_pair : args) {
std::cout << "Processing command: [" << arg_pair.first << "] with param: ["
<< arg_pair.second << "] ..." << std::endl;
std::cout << "Processing command: [" << arg_pair.first << "] with param: [" << arg_pair.second << "] ..."
<< std::endl;
if (arg_pair.first == "network_interface") {
continue;
}
Expand Down Expand Up @@ -100,6 +99,24 @@ int main(int argc, char const *argv[]) {
std::cout << ")" << std::endl;
}

if (arg_pair.first == "enable_odom") {
float x, y, yaw;
client.EnableOdom();
std::cout << "Send enable odom signal" << std::endl;
}

if (arg_pair.first == "disable_odom") {
float x, y, yaw;
client.DisableOdom();
std::cout << "Send disable odom signal" << std::endl;
}

if (arg_pair.first == "get_odom") {
float x, y, yaw;
client.GetOdom(x, y, yaw);
std::cout << "Get Odom: (" << x << ", " << y << ", " << yaw << ")" << std::endl;
}

if (arg_pair.first == "set_fsm_id") {
int fsm_id = std::stoi(arg_pair.second);
client.SetFsmId(fsm_id);
Expand Down Expand Up @@ -139,8 +156,7 @@ int main(int argc, char const *argv[]) {
omega = param.at(2);
duration = param.at(3);
} else {
std::cerr << "Invalid param size for method SetVelocity: " << param_size
<< std::endl;
std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
return 1;
}

Expand All @@ -155,12 +171,23 @@ int main(int argc, char const *argv[]) {
client.SetPhase(param);
std::cout << "set phase to " << arg_pair.second << std::endl;
} else {
std::cerr << "Invalid param size for method SetPhase: " << param_size
<< std::endl;
std::cerr << "Invalid param size for method SetPhase: " << param_size << std::endl;
return 1;
}
}

if (arg_pair.first == "set_target_pos") {
std::vector<float> param = stringToFloatVector(arg_pair.second);
client.SetTargetPos(param.at(0), param.at(1), param.at(2), false);
std::cout << "set_target_pos: " << arg_pair.second << std::endl;
}

if (arg_pair.first == "set_target_pos_relative") {
std::vector<float> param = stringToFloatVector(arg_pair.second);
client.SetTargetPos(param.at(0), param.at(1), param.at(2));
std::cout << "set_target_pos: " << arg_pair.second << std::endl;
}

if (arg_pair.first == "damp") {
client.Damp();
}
Expand Down Expand Up @@ -228,8 +255,7 @@ int main(int argc, char const *argv[]) {
vy = param.at(1);
omega = param.at(2);
} else {
std::cerr << "Invalid param size for method SetVelocity: " << param_size
<< std::endl;
std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
return 1;
}
client.Move(vx, vy, omega);
Expand All @@ -246,8 +272,28 @@ int main(int argc, char const *argv[]) {
return 1;
}
client.SetNextFoot(flag);
std::cout << "Done!" << std::endl;
}

if (arg_pair.first == "set_task_id") {
int task_id = std::stoi(arg_pair.second);
client.SetTaskId(task_id);
std::cout << "set task_id to " << task_id << std::endl;
}

if (arg_pair.first == "shake_hand") {
client.ShakeHand(0);
std::cout << "Shake hand starts! Waiting for 10 s for ending" << std::endl;
std::this_thread::sleep_for(std::chrono::seconds(10));
std::cout << "Shake hand ends!" << std::endl;
client.ShakeHand(1);
}

if (arg_pair.first == "wave_hand") {
client.WaveHand();
std::cout << "wave hand" << std::endl;
}

std::cout << "Done!" << std::endl;
}

return 0;
Expand Down
54 changes: 40 additions & 14 deletions include/unitree/robot/h1/loco/h1_loco_api.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,43 +19,46 @@ const int32_t ROBOT_API_ID_LOCO_GET_FSM_MODE = 8002;
const int32_t ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 8003;
const int32_t ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 8004;
const int32_t ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 8005;
const int32_t ROBOT_API_ID_LOCO_GET_PHASE = 8006; // deprecated
const int32_t ROBOT_API_ID_LOCO_GET_PHASE = 8006; // deprecated

const int32_t ROBOT_API_ID_LOCO_SET_FSM_ID = 8101;
const int32_t ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 8102;
const int32_t ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 8103;
const int32_t ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 8104;
const int32_t ROBOT_API_ID_LOCO_SET_VELOCITY = 8105;
const int32_t ROBOT_API_ID_LOCO_SET_PHASE = 8106;
const int32_t ROBOT_API_ID_LOCO_SET_ARM_TASK = 8107;

using LocoCmd =
std::map<std::string, std::variant<int, float, std::vector<float>>>;
const int32_t ROBOT_API_ID_LOCO_ENABLE_ODOM = 8201;
const int32_t ROBOT_API_ID_LOCO_DISABLE_ODOM = 8202;
const int32_t ROBOT_API_ID_LOCO_GET_ODOM = 8203;
const int32_t ROBOT_API_ID_LOCO_SET_TARGET_POSITION = 8204;

using LocoCmd = std::map<std::string, std::variant<int, float, std::vector<float>>>;

class JsonizeDataVecFloat : public common::Jsonize {
public:
public:
JsonizeDataVecFloat() {}
~JsonizeDataVecFloat() {}

void fromJson(common::JsonMap &json) { common::FromJson(json["data"], data); }
void fromJson(common::JsonMap& json) { common::FromJson(json["data"], data); }

void toJson(common::JsonMap &json) const {
common::ToJson(data, json["data"]);
}
void toJson(common::JsonMap& json) const { common::ToJson(data, json["data"]); }

std::vector<float> data;
};

class JsonizeVelocityCommand : public common::Jsonize {
public:
public:
JsonizeVelocityCommand() {}
~JsonizeVelocityCommand() {}

void fromJson(common::JsonMap &json) {
void fromJson(common::JsonMap& json) {
common::FromJson(json["velocity"], velocity);
common::FromJson(json["duration"], duration);
}

void toJson(common::JsonMap &json) const {
void toJson(common::JsonMap& json) const {
common::ToJson(velocity, json["velocity"]);
common::ToJson(duration, json["duration"]);
}
Expand All @@ -64,8 +67,31 @@ class JsonizeVelocityCommand : public common::Jsonize {
float duration;
};

} // namespace h1
} // namespace robot
} // namespace unitree
class JsonizeTargetPos : public common::Jsonize {
public:
JsonizeTargetPos() {}
~JsonizeTargetPos() {}

void fromJson(common::JsonMap& json) {
common::FromJson(json["x"], x);
common::FromJson(json["y"], y);
common::FromJson(json["yaw"], yaw);
common::FromJson(json["relative"], relative);
}

void toJson(common::JsonMap& json) const {
common::ToJson(x, json["x"]);
common::ToJson(y, json["y"]);
common::ToJson(yaw, json["yaw"]);
common::ToJson(relative, json["relative"]);
}

float x, y, yaw;
bool relative;
};

} // namespace h1
} // namespace robot
} // namespace unitree

#endif // __UT_ROBOT_H1_LOCO_API_HPP__
Loading

0 comments on commit 197b720

Please sign in to comment.