Skip to content

Commit 34d3444

Browse files
committed
Merge remote-tracking branch 'origin/add_api_for_arm_tasks_in_g1'
2 parents ca1bcab + 4183922 commit 34d3444

File tree

4 files changed

+57
-0
lines changed

4 files changed

+57
-0
lines changed

example/g1/high_level/g1_loco_client_example.cpp

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -230,6 +230,31 @@ int main(int argc, char const *argv[]) {
230230
client.Move(vx, vy, omega);
231231
}
232232

233+
if (arg_pair.first == "set_task_id") {
234+
int task_id = std::stoi(arg_pair.second);
235+
client.SetTaskId(task_id);
236+
std::cout << "set task_id to " << task_id << std::endl;
237+
}
238+
239+
if (arg_pair.first == "shake_hand") {
240+
client.ShakeHand(0);
241+
std::cout << "Shake hand starts! Waiting for 10 s for ending"
242+
<< std::endl;
243+
std::this_thread::sleep_for(std::chrono::seconds(10));
244+
std::cout << "Shake hand ends!" << std::endl;
245+
client.ShakeHand(1);
246+
}
247+
248+
if (arg_pair.first == "wave_hand") {
249+
client.WaveHand();
250+
std::cout << "wave hand" << std::endl;
251+
}
252+
253+
if (arg_pair.first == "wave_hand_with_turn") {
254+
client.WaveHand(true);
255+
std::cout << "wave hand with turn" << std::endl;
256+
}
257+
233258
std::cout << "Done!" << std::endl;
234259
}
235260

include/unitree/robot/g1/loco/g1_loco_api.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ const int32_t ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 7102;
2626
const int32_t ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 7103;
2727
const int32_t ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104;
2828
const int32_t ROBOT_API_ID_LOCO_SET_VELOCITY = 7105;
29+
const int32_t ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106;
2930

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

include/unitree/robot/g1/loco/g1_loco_client.hpp

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ class LocoClient : public Client {
2929
UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT);
3030
UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT);
3131
UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_VELOCITY);
32+
UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_ARM_TASK);
3233
};
3334

3435
/*Low Level API Call*/
@@ -168,6 +169,16 @@ class LocoClient : public Client {
168169
return Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter, data);
169170
}
170171

172+
int32_t SetTaskId(int task_id) {
173+
std::string parameter, data;
174+
175+
go2::JsonizeDataInt json;
176+
json.data = task_id;
177+
parameter = common::ToJsonString(json);
178+
179+
return Call(ROBOT_API_ID_LOCO_SET_ARM_TASK, parameter, data);
180+
}
181+
171182
/*High Level API Call*/
172183
int32_t Damp() { return SetFsmId(1); }
173184

@@ -202,8 +213,27 @@ class LocoClient : public Client {
202213
return 0;
203214
}
204215

216+
int32_t WaveHand(bool turn_flag = false) { return SetTaskId(turn_flag ? 1 : 0); }
217+
218+
int32_t ShakeHand(int stage = -1) {
219+
switch (stage) {
220+
case 0:
221+
first_shake_hand_stage_ = false;
222+
return SetTaskId(2);
223+
224+
case 1:
225+
first_shake_hand_stage_ = true;
226+
return SetTaskId(3);
227+
228+
default:
229+
first_shake_hand_stage_ = !first_shake_hand_stage_;
230+
return SetTaskId(first_shake_hand_stage_ ? 3 : 2);
231+
}
232+
}
233+
205234
private:
206235
bool continous_move_ = false;
236+
bool first_shake_hand_stage_ = true;
207237
};
208238
} // namespace g1
209239

include/unitree/robot/g1/loco/g1_loco_error.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ namespace g1 {
99
UT_DECL_ERR(UT_ROBOT_LOCO_ERR_LOCOSTATE_NOT_AVAILABLE, 7301,
1010
"LocoState not available.")
1111
UT_DECL_ERR(UT_ROBOT_LOCO_ERR_INVALID_FSM_ID, 7302, "Invalid fsm id.")
12+
UT_DECL_ERR(UT_ROBOT_LOCO_ERR_INVALID_TASK_ID, 7303, "Invalid task id.")
1213
} // namespace g1
1314
} // namespace robot
1415
} // namespace unitree

0 commit comments

Comments
 (0)