Merge remote-tracking branch 'origin/add_api_for_arm_tasks_in_g1'

unitreerobotics · Jan 13, 2025 · 34d3444 · 34d3444
2 parents ca1bcab + 4183922
commit 34d3444
Show file tree

Hide file tree

Showing 4 changed files with 57 additions and 0 deletions.
diff --git a/example/g1/high_level/g1_loco_client_example.cpp b/example/g1/high_level/g1_loco_client_example.cpp
@@ -230,6 +230,31 @@ int main(int argc, char const *argv[]) {
       client.Move(vx, vy, omega);
     }
 
+    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;
+    }
+
+    if (arg_pair.first == "wave_hand_with_turn") {
+      client.WaveHand(true);
+      std::cout << "wave hand with turn" << std::endl;
+    }
+
     std::cout << "Done!" << std::endl;
   }
 

diff --git a/include/unitree/robot/g1/loco/g1_loco_api.hpp b/include/unitree/robot/g1/loco/g1_loco_api.hpp
@@ -26,6 +26,7 @@ const int32_t ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 7102;
 const int32_t ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 7103;
 const int32_t ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104;
 const int32_t ROBOT_API_ID_LOCO_SET_VELOCITY = 7105;
+const int32_t ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106;
 
 using LocoCmd =
     std::map<std::string, std::variant<int, float, std::vector<float>>>;

diff --git a/include/unitree/robot/g1/loco/g1_loco_client.hpp b/include/unitree/robot/g1/loco/g1_loco_client.hpp
@@ -29,6 +29,7 @@ class LocoClient : public Client {
     UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT);
     UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT);
     UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_VELOCITY);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_ARM_TASK);
   };
 
   /*Low Level API Call*/
@@ -168,6 +169,16 @@ class LocoClient : public Client {
     return Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter, data);
   }
 
+  int32_t SetTaskId(int task_id) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = task_id;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_ARM_TASK, parameter, data);
+  }
+
   /*High Level API Call*/
   int32_t Damp() { return SetFsmId(1); }
 
@@ -202,8 +213,27 @@ class LocoClient : public Client {
     return 0;
   }
 
+  int32_t WaveHand(bool turn_flag = false) { return SetTaskId(turn_flag ? 1 : 0); }
+
+  int32_t ShakeHand(int stage = -1) {
+    switch (stage) {
+      case 0:
+        first_shake_hand_stage_ = false;
+        return SetTaskId(2);
+
+      case 1:
+        first_shake_hand_stage_ = true;
+        return SetTaskId(3);
+
+      default:
+        first_shake_hand_stage_ = !first_shake_hand_stage_;
+        return SetTaskId(first_shake_hand_stage_ ? 3 : 2);
+    }
+  }
+
  private:
   bool continous_move_ = false;
+  bool first_shake_hand_stage_ = true;
 };
 } // namespace g1
 

diff --git a/include/unitree/robot/g1/loco/g1_loco_error.hpp b/include/unitree/robot/g1/loco/g1_loco_error.hpp
@@ -9,6 +9,7 @@ namespace g1 {
 UT_DECL_ERR(UT_ROBOT_LOCO_ERR_LOCOSTATE_NOT_AVAILABLE, 7301,
             "LocoState not available.")
 UT_DECL_ERR(UT_ROBOT_LOCO_ERR_INVALID_FSM_ID, 7302, "Invalid fsm id.")
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_INVALID_TASK_ID, 7303, "Invalid task id.")
 } // namespace g1
 } // namespace robot
 } // namespace unitree