92 lines
3.2 KiB
C++
92 lines
3.2 KiB
C++
/**
|
|
* @file arm_action.cpp
|
|
* @brief Trigger Unitree's built-in G1 upper-body actions (shake hand, etc.).
|
|
*
|
|
* Uses unitree_sdk2 G1ArmActionClient. The action server drives rt/arm_sdk for
|
|
* you (balance-aware, Unitree-designed motions) — do NOT run arm_raise at the
|
|
* same time (error 7400 = rt/arm_sdk occupied).
|
|
*
|
|
* ./arm_action list # list actions
|
|
* ./arm_action "shake hand" # by name
|
|
* ./arm_action 27 [iface] # by id (default iface: eth0)
|
|
* ./arm_action "release arm" # = 99, returns the arm to normal
|
|
*
|
|
* After most actions the arm HOLDS until you send "release arm" (99).
|
|
* Actions require the robot FSM id in {500,501,801} (check rt/sportmodestate).
|
|
*/
|
|
|
|
#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
|
|
#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
|
|
|
|
#include <iostream>
|
|
#include <memory>
|
|
#include <string>
|
|
|
|
using namespace unitree::robot::g1;
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
if (argc < 2) {
|
|
std::cout << "Usage: ./arm_action <list | id | \"name\"> [networkInterface]\n";
|
|
return 1;
|
|
}
|
|
std::string action = argv[1];
|
|
std::string iface = argc > 2 ? argv[2] : "eth0";
|
|
|
|
unitree::robot::ChannelFactory::Instance()->Init(0, iface);
|
|
auto client = std::make_shared<G1ArmActionClient>();
|
|
client->Init();
|
|
client->SetTimeout(10.f);
|
|
|
|
if (action == "list") {
|
|
std::cout << "Built-in actions (id name):\n";
|
|
for (const auto &kv : client->action_map)
|
|
std::cout << " " << kv.second << "\t" << kv.first << "\n";
|
|
std::string data;
|
|
if (client->GetActionList(data) == 0)
|
|
std::cout << "\nServer-reported list:\n" << data << std::endl;
|
|
return 0;
|
|
}
|
|
|
|
// Resolve action id from a number or a name.
|
|
int32_t id = 0;
|
|
try {
|
|
id = std::stoi(action);
|
|
} catch (const std::exception &) {
|
|
auto it = client->action_map.find(action);
|
|
if (it == client->action_map.end()) {
|
|
std::cerr << "Unknown action '" << action << "'. Try: ./arm_action list\n";
|
|
return 1;
|
|
}
|
|
id = it->second;
|
|
}
|
|
|
|
std::cout << "Executing action id " << id << " ..." << std::endl;
|
|
int32_t ret = client->ExecuteAction(id);
|
|
if (ret == 0) {
|
|
std::cout << "OK. (Arm holds until you send \"release arm\" / 99.)" << std::endl;
|
|
return 0;
|
|
}
|
|
|
|
switch (ret) {
|
|
case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
|
|
std::cerr << "Error 7400: " << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC
|
|
<< " (something else holds rt/arm_sdk — e.g. arm_raise)." << std::endl;
|
|
break;
|
|
case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
|
|
std::cerr << "Error 7401: " << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << std::endl;
|
|
break;
|
|
case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
|
|
std::cerr << "Error 7402: " << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << std::endl;
|
|
break;
|
|
case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
|
|
std::cerr << "Error 7404: invalid FSM id. Actions need fsm id {500,501,801} "
|
|
"(check rt/sportmodestate; in 801 only fsm mode {0,3})." << std::endl;
|
|
break;
|
|
default:
|
|
std::cerr << "Execute action failed, error code: " << ret << std::endl;
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|