Merge pull request #21 from unitreerobotics/update-g1_arm_action-api

Agnel-Wang · web-flow · commit 211dad48a40f · 2025-10-14T15:45:35.000+08:00
add execute_custom_action api
diff --git a/example/g1/CMakeLists.txt b/example/g1/CMakeLists.txt
@@ -7,9 +7,6 @@ target_link_libraries(g1_arm5_sdk_dds_example unitree_sdk2)
 add_executable(g1_arm7_sdk_dds_example high_level/g1_arm7_sdk_dds_example.cpp)
 target_link_libraries(g1_arm7_sdk_dds_example unitree_sdk2)
 
-add_executable(g1_arm_action_example high_level/g1_arm_action_example.cpp)
-target_link_libraries(g1_arm_action_example unitree_sdk2)
-
 add_executable(g1_ankle_swing_example low_level/g1_ankle_swing_example.cpp)
 target_link_libraries(g1_ankle_swing_example unitree_sdk2)
 
@@ -23,6 +20,9 @@ find_package(Boost COMPONENTS program_options)
 if(Boost_FOUND)
     add_executable(g1_termination low_level/terminations.cpp)
     target_link_libraries(g1_termination unitree_sdk2 Boost::program_options)
+
+    add_executable(g1_arm_action_example high_level/g1_arm_action_example.cpp)
+    target_link_libraries(g1_arm_action_example unitree_sdk2 Boost::program_options)
 endif()
 
 find_package(yaml-cpp QUIET)
diff --git a/example/g1/high_level/g1_arm_action_example.cpp b/example/g1/high_level/g1_arm_action_example.cpp
@@ -4,76 +4,81 @@
  */
 #include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
 #include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
+#include <boost/program_options.hpp>
 
+namespace po = boost::program_options;
+using namespace unitree::robot;
 using namespace unitree::robot::g1;
 
 int main(int argc, const char** argv) 
 {
     std::cout << " --- Unitree Robotics --- \n";
     std::cout << "     G1 Arm Action Example      \n\n";
 
-    // Unitree DDS Initialization; arg[1] is the network interface
-    unitree::robot::ChannelFactory::Instance()->Init(0, argc > 1 ? argv[1] : "");
+    // Parse command line arguments
+    po::options_description desc("Unitree G1 Arm Action Example.");
+    desc.add_options()
+        ("help,h", "show help message")
+        ("network,n", po::value<std::string>()->default_value(""), "dds network interface")
+        ("list,l", "list all supported actions")
+        ("id,i", po::value<int>(), "action id to execute, 0 to list all supported actions")
+        ("name", po::value<std::string>(), "custom action name to execute")
+        ("stop", "stop the current custom action")
+        ;
+    po::variables_map vm;
+    po::store(po::parse_command_line(argc, argv, desc), vm);
+    po::notify(vm);
+    if(argc < 2 || vm.count("help")) {
+        std::cout << desc << std::endl;
+        return 0;
+    }
 
-    auto client = std::make_shared<unitree::robot::g1::G1ArmActionClient>();
-    client->Init();
-    client->SetTimeout(10.f); // All actions will last less than 10 seconds.
+    // DDS Init
+    ChannelFactory::Instance()->Init(0, vm["network"].as<std::string>());
 
-    std::cout << "Usage: \n";
-    std::cout << "  - 0: print supported actions.\n";
-    std::cout << "  - an id: execute an action.\n";
-    std::cout << "Attention: \n";
-    std::cout << "  Some actions will not be displayed on the APP, \n"; 
-    std::cout << "  but can be executed by the program.\n";
-    std::cout << "  These actions may cause the robot to fall,\n";
-    std::cout << "  so please execute them with caution.\n";
+    auto client = std::make_shared<G1ArmActionClient>();
+    client->Init();
+    // Attention: unitree actions are all less than 10s, 
+    //   but the custom actions may be longer.
+    client->SetTimeout(10.f);
 
-    int32_t action_id = 0;
-    std::string line;
-    while (true) {
-        std::cout << "\nEnter action ID: .\n";
-        std::getline(std::cin, line);
-        try {
-            action_id = std::stoi(line);
-        } catch (const std::exception&) {
-            std::cout << "Invalid input. Please enter an integer.\n";
-            continue;
+    if(vm.count("list") || (vm.count("id") && vm["id"].as<int>() == 0)){
+        std::string action_list_data;
+        int32_t ret = client->GetActionList(action_list_data);
+        if (ret != 0) {
+            std::cerr << "Failed to get action list, error code: " << ret << "\n";
         }
-
-        if (action_id == 0) {
-            std::string action_list_data;
-            int32_t ret = client->GetActionList(action_list_data);
-            if (ret != 0) {
-                std::cerr << "Failed to get action list, error code: " << ret << "\n";
-                continue;
-            }
-            std::cout << "Available actions:\n" << action_list_data << std::endl;
-        } else {
-            int32_t ret = client->ExecuteAction(action_id);
-            if(ret != 0) {
-                switch (ret)
-                {
-                case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
-                    std::cout << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC << std::endl;
-                    break;
-                case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
-                    std::cout << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << std::endl;
-                    break;
-                case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
-                    std::cout << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << std::endl;
-                    break;
-                case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
-                    std::cout << "The actions are only supported in fsm id {500, 501, 801}" << std::endl;
-                    std::cout << "You can subscribe the topic rt/sportmodestate to check the fsm id." << std::endl;
-                    std::cout << "And in the state 801, the actions are only supported in the fsm mode {0, 3}." << std::endl;
-                    std::cout << "If an error is still returned at this point, ignore this action.";
-                    break;
-                default:
-                    std::cerr << "Execute action failed, error code: " << ret << std::endl;
-                    break;
-                }
+        std::cout << "Available actions:\n" << action_list_data << std::endl;
+    } else if (vm.count("id")) {
+        int32_t ret = client->ExecuteAction(vm["id"].as<int>());
+        if(ret != 0) {
+            switch (ret)
+            {
+            case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
+                std::cout << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC << std::endl;
+                break;
+            case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
+                std::cout << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << std::endl;
+                break;
+            case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
+                std::cout << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << std::endl;
+                break;
+            case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
+                std::cout << "The actions are only supported in fsm id {500, 501, 801}" << std::endl;
+                std::cout << "You can subscribe the topic rt/sportmodestate to check the fsm id." << std::endl;
+                std::cout << "And in the state 801, the actions are only supported in the fsm mode {0, 3}." << std::endl;
+                std::cout << "If an error is still returned at this point, ignore this action.";
+                break;
+            default:
+                std::cerr << "Execute action failed, error code: " << ret << std::endl;
+                break;
             }
         }
+    } else if (vm.count("name")) {
+        int32_t ret = client->ExecuteAction(vm["name"].as<std::string>());
+        if(ret != 0) std::cout << "Execute custom action failed, error code: " << ret << std::endl;
+    } else if (vm.count("stop")) {
+        int32_t ret = client->StopCustomAction();
     }
 
     return 0;
diff --git a/include/unitree/robot/g1/arm/g1_arm_action_api.hpp b/include/unitree/robot/g1/arm/g1_arm_action_api.hpp
@@ -15,22 +15,8 @@ const std::string ARM_ACTION_API_VERSION = "1.0.0.14";
 /*api id*/
 const int32_t ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION = 7106;
 const int32_t ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST = 7107;
-
-class JsonizeArmActionCommand : public common::Jsonize {
- public:
-  JsonizeArmActionCommand() {}
-  ~JsonizeArmActionCommand() {}
-
-  void fromJson(common::JsonMap &json) {
-    common::FromJson(json["data"], action_id);
-  }
-
-  void toJson(common::JsonMap &json) const {
-    common::ToJson(action_id, json["data"]);
-  }
-
-  int32_t action_id;
-};
+const int32_t ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION = 7108;
+const int32_t ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION = 7113;
 
 }  // namespace g1
 }  // namespace robot
diff --git a/include/unitree/robot/g1/arm/g1_arm_action_client.hpp b/include/unitree/robot/g1/arm/g1_arm_action_client.hpp
@@ -13,6 +13,13 @@ namespace g1 {
    * 
    * The arm action server provides some upper body actions.
    * The controller is based on the `rt/arm_sdk` interface.
+   * 
+   * The current state of the arm will be published on the `rt/arm/action/state` topic:
+   * {
+   *   "holding": false,  # Whether to hold the position after the action ends; will release after a maximum of 20 seconds
+   *   "id": 99,          # Current action ID
+   *   "name": "release_arm" # Current action name
+   * }
    */
 class G1ArmActionClient : public Client {
   public:
@@ -24,19 +31,51 @@ class G1ArmActionClient : public Client {
       SetApiVersion(ARM_ACTION_API_VERSION);
       UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION);
       UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST);
+      UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION);
+      UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION);
   }
   
   /*API Call*/
+  /**
+   * @brief Execute Unitree's internal preset actions
+   * Indexed by ID, where 99 is to release the action.
+   * Some actions will hold the position at the last keyframe after completion; 
+   * you can send id = 99 or the same id to release.
+   * 
+   * Attention:
+   *   Some actions will not be displayed on the APP, 
+   *   but can be executed by the program.
+   *   These actions may cause the robot to fall,
+   *   so please execute them with caution.
+   */
   int32_t ExecuteAction(int32_t action_id) {
     std::string parameter, data;
-    JsonizeArmActionCommand json;
-  
-    json.action_id = action_id;
-    parameter = common::ToJsonString(json);
-  
+    parameter = R"({"action_id":)" + std::to_string(action_id) + R"(})";
     return Call(ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION, parameter, data);
   }
 
+  /**
+   * @brief Execute custom teach actions, indexed by name
+   * 
+   * # rt/arm/action/state
+   * {
+   *   "holding": false,  # teach actions do not include holding
+   *   "id": 100,         # Always 100, the action is determined by the name
+   *   "name": "sth"      # Current teach action name
+   * }
+   */
+  int32_t ExecuteAction(const std::string &action_name) {
+    std::string parameter, data;
+    parameter = R"({"action_name":")" + action_name + R"("})";
+    return Call(ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION, parameter, data);
+  }
+
+  /* Stop Custom Action */
+  int32_t StopCustomAction() {
+    std::string parameter, data;
+    return Call(ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION, parameter, data);
+  }
+
   int32_t GetActionList(std::string &data) {
     std::string parameter;
     return Call(ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST, parameter, data);
