Skip to content

Commit 197b720

Browse files
cyoahscyoahs
committed
update h1 high level api for ver. 4.0.0.0
1 parent 34d3444 commit 197b720

File tree

4 files changed

+191
-53
lines changed

4 files changed

+191
-53
lines changed

‎example/h1/high_level/h1_loco_client_example.cpp‎

Lines changed: 57 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -45,17 +45,16 @@ int main(int argc, char const *argv[]) {
4545
}
4646
}
4747

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

5150
unitree::robot::h1::LocoClient client;
5251

5352
client.Init();
5453
client.SetTimeout(10.f);
5554

5655
for (const auto &arg_pair : args) {
57-
std::cout << "Processing command: [" << arg_pair.first << "] with param: ["
58-
<< arg_pair.second << "] ..." << std::endl;
56+
std::cout << "Processing command: [" << arg_pair.first << "] with param: [" << arg_pair.second << "] ..."
57+
<< std::endl;
5958
if (arg_pair.first == "network_interface") {
6059
continue;
6160
}
@@ -100,6 +99,24 @@ int main(int argc, char const *argv[]) {
10099
std::cout << ")" << std::endl;
101100
}
102101

102+
if (arg_pair.first == "enable_odom") {
103+
float x, y, yaw;
104+
client.EnableOdom();
105+
std::cout << "Send enable odom signal" << std::endl;
106+
}
107+
108+
if (arg_pair.first == "disable_odom") {
109+
float x, y, yaw;
110+
client.DisableOdom();
111+
std::cout << "Send disable odom signal" << std::endl;
112+
}
113+
114+
if (arg_pair.first == "get_odom") {
115+
float x, y, yaw;
116+
client.GetOdom(x, y, yaw);
117+
std::cout << "Get Odom: (" << x << ", " << y << ", " << yaw << ")" << std::endl;
118+
}
119+
103120
if (arg_pair.first == "set_fsm_id") {
104121
int fsm_id = std::stoi(arg_pair.second);
105122
client.SetFsmId(fsm_id);
@@ -139,8 +156,7 @@ int main(int argc, char const *argv[]) {
139156
omega = param.at(2);
140157
duration = param.at(3);
141158
} else {
142-
std::cerr << "Invalid param size for method SetVelocity: " << param_size
143-
<< std::endl;
159+
std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
144160
return 1;
145161
}
146162

@@ -155,12 +171,23 @@ int main(int argc, char const *argv[]) {
155171
client.SetPhase(param);
156172
std::cout << "set phase to " << arg_pair.second << std::endl;
157173
} else {
158-
std::cerr << "Invalid param size for method SetPhase: " << param_size
159-
<< std::endl;
174+
std::cerr << "Invalid param size for method SetPhase: " << param_size << std::endl;
160175
return 1;
161176
}
162177
}
163178

179+
if (arg_pair.first == "set_target_pos") {
180+
std::vector<float> param = stringToFloatVector(arg_pair.second);
181+
client.SetTargetPos(param.at(0), param.at(1), param.at(2), false);
182+
std::cout << "set_target_pos: " << arg_pair.second << std::endl;
183+
}
184+
185+
if (arg_pair.first == "set_target_pos_relative") {
186+
std::vector<float> param = stringToFloatVector(arg_pair.second);
187+
client.SetTargetPos(param.at(0), param.at(1), param.at(2));
188+
std::cout << "set_target_pos: " << arg_pair.second << std::endl;
189+
}
190+
164191
if (arg_pair.first == "damp") {
165192
client.Damp();
166193
}
@@ -228,8 +255,7 @@ int main(int argc, char const *argv[]) {
228255
vy = param.at(1);
229256
omega = param.at(2);
230257
} else {
231-
std::cerr << "Invalid param size for method SetVelocity: " << param_size
232-
<< std::endl;
258+
std::cerr << "Invalid param size for method SetVelocity: " << param_size << std::endl;
233259
return 1;
234260
}
235261
client.Move(vx, vy, omega);
@@ -246,8 +272,28 @@ int main(int argc, char const *argv[]) {
246272
return 1;
247273
}
248274
client.SetNextFoot(flag);
249-
std::cout << "Done!" << std::endl;
250275
}
276+
277+
if (arg_pair.first == "set_task_id") {
278+
int task_id = std::stoi(arg_pair.second);
279+
client.SetTaskId(task_id);
280+
std::cout << "set task_id to " << task_id << std::endl;
281+
}
282+
283+
if (arg_pair.first == "shake_hand") {
284+
client.ShakeHand(0);
285+
std::cout << "Shake hand starts! Waiting for 10 s for ending" << std::endl;
286+
std::this_thread::sleep_for(std::chrono::seconds(10));
287+
std::cout << "Shake hand ends!" << std::endl;
288+
client.ShakeHand(1);
289+
}
290+
291+
if (arg_pair.first == "wave_hand") {
292+
client.WaveHand();
293+
std::cout << "wave hand" << std::endl;
294+
}
295+
296+
std::cout << "Done!" << std::endl;
251297
}
252298

253299
return 0;

‎include/unitree/robot/h1/loco/h1_loco_api.hpp‎

Lines changed: 40 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -19,43 +19,46 @@ const int32_t ROBOT_API_ID_LOCO_GET_FSM_MODE = 8002;
1919
const int32_t ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 8003;
2020
const int32_t ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 8004;
2121
const int32_t ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 8005;
22-
const int32_t ROBOT_API_ID_LOCO_GET_PHASE = 8006; // deprecated
22+
const int32_t ROBOT_API_ID_LOCO_GET_PHASE = 8006; // deprecated
2323

2424
const int32_t ROBOT_API_ID_LOCO_SET_FSM_ID = 8101;
2525
const int32_t ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 8102;
2626
const int32_t ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 8103;
2727
const int32_t ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 8104;
2828
const int32_t ROBOT_API_ID_LOCO_SET_VELOCITY = 8105;
2929
const int32_t ROBOT_API_ID_LOCO_SET_PHASE = 8106;
30+
const int32_t ROBOT_API_ID_LOCO_SET_ARM_TASK = 8107;
3031

31-
using LocoCmd =
32-
std::map<std::string, std::variant<int, float, std::vector<float>>>;
32+
const int32_t ROBOT_API_ID_LOCO_ENABLE_ODOM = 8201;
33+
const int32_t ROBOT_API_ID_LOCO_DISABLE_ODOM = 8202;
34+
const int32_t ROBOT_API_ID_LOCO_GET_ODOM = 8203;
35+
const int32_t ROBOT_API_ID_LOCO_SET_TARGET_POSITION = 8204;
36+
37+
using LocoCmd = std::map<std::string, std::variant<int, float, std::vector<float>>>;
3338

3439
class JsonizeDataVecFloat : public common::Jsonize {
35-
public:
40+
public:
3641
JsonizeDataVecFloat() {}
3742
~JsonizeDataVecFloat() {}
3843

39-
void fromJson(common::JsonMap &json) { common::FromJson(json["data"], data); }
44+
void fromJson(common::JsonMap& json) { common::FromJson(json["data"], data); }
4045

41-
void toJson(common::JsonMap &json) const {
42-
common::ToJson(data, json["data"]);
43-
}
46+
void toJson(common::JsonMap& json) const { common::ToJson(data, json["data"]); }
4447

4548
std::vector<float> data;
4649
};
4750

4851
class JsonizeVelocityCommand : public common::Jsonize {
49-
public:
52+
public:
5053
JsonizeVelocityCommand() {}
5154
~JsonizeVelocityCommand() {}
5255

53-
void fromJson(common::JsonMap &json) {
56+
void fromJson(common::JsonMap& json) {
5457
common::FromJson(json["velocity"], velocity);
5558
common::FromJson(json["duration"], duration);
5659
}
5760

58-
void toJson(common::JsonMap &json) const {
61+
void toJson(common::JsonMap& json) const {
5962
common::ToJson(velocity, json["velocity"]);
6063
common::ToJson(duration, json["duration"]);
6164
}
@@ -64,8 +67,31 @@ class JsonizeVelocityCommand : public common::Jsonize {
6467
float duration;
6568
};
6669

67-
} // namespace h1
68-
} // namespace robot
69-
} // namespace unitree
70+
class JsonizeTargetPos : public common::Jsonize {
71+
public:
72+
JsonizeTargetPos() {}
73+
~JsonizeTargetPos() {}
74+
75+
void fromJson(common::JsonMap& json) {
76+
common::FromJson(json["x"], x);
77+
common::FromJson(json["y"], y);
78+
common::FromJson(json["yaw"], yaw);
79+
common::FromJson(json["relative"], relative);
80+
}
81+
82+
void toJson(common::JsonMap& json) const {
83+
common::ToJson(x, json["x"]);
84+
common::ToJson(y, json["y"]);
85+
common::ToJson(yaw, json["yaw"]);
86+
common::ToJson(relative, json["relative"]);
87+
}
88+
89+
float x, y, yaw;
90+
bool relative;
91+
};
92+
93+
} // namespace h1
94+
} // namespace robot
95+
} // namespace unitree
7096

7197
#endif // __UT_ROBOT_H1_LOCO_API_HPP__

0 commit comments

Comments
 (0)